Allow passing object sampling tsr to place function

src/herbpy/action/grasping.py

@@ -189,14 +189,15 @@ def Lift(robot, obj, distance=0.05, manip=None, render=True, **kw_args):
                                           **kw_args)
 
 @ActionMethod
-def Place(robot, obj, on_obj, manip=None, render=True, **kw_args):
+def Place(robot, obj, on_obj, obj_top_tsr = None, manip=None, render=True, **kw_args):
     """
     Place an object onto another object
     This assumes the 'point_on' tsr is defined for the on_obj and
     the 'place' tsr is defined for obj
     @param robot The robot performing the push grasp
     @param obj The object to place
     @param on_obj The object to place obj on
+    @param obj_top_tsr TSR describes that can sample the poses
     @param manip The manipulator to perform the grasp with 
        (if None active manipulator is used)
     @param render Render tsr samples and push direction vectors during planning
@@ -207,17 +208,19 @@ def Place(robot, obj, on_obj, manip=None, render=True, **kw_args):
             manip = robot.GetActiveManipulator()
 
     with robot.GetEnv():
-        # Get a tsr to sample places to put the glass
-        obj_extents = obj.ComputeAABB().extents()
-        obj_radius = max(obj_extents[0], obj_extents[1])
-        obj_top_tsr = robot.tsrlibrary(on_obj, 'point_on', padding=obj_radius, manip=manip)
-
-        #  Now use this to get a tsr for sampling ee_poses
-        place_tsr = robot.tsrlibrary(obj, 'place', pose_tsr_chain=obj_top_tsr[0], manip=manip)
+        if obj_top_tsr is None:
+	   # Get a tsr to sample places to put the glass
+           obj_extents = obj.ComputeAABB().extents()
+           obj_radius = max(obj_extents[0], obj_extents[1])
+           obj_top_tsr = robot.tsrlibrary(on_obj, 'point_on', padding=obj_radius, manip=manip)
+           #  Now use this to get a tsr for sampling ee_poses
+	   place_tsr = robot.tsrlibrary(obj, 'place', pose_tsr_chain=obj_top_tsr[0], manip=manip)
+	else:
+           place_tsr = robot.tsrlibrary(obj, 'place', pose_tsr_chain = obj_top_tsr, manip=manip)
 
     # Plan to the grasp
     with prpy.viz.RenderTSRList(place_tsr, robot.GetEnv(), render=render):
-        manip.PlanToTSR(place_tsr, execute=True)
+         manip.PlanToTSR(place_tsr, execute=True)
 
     # Open the hand
     manip.hand.OpenHand()

src/herbpy/herbrobot.py

@@ -225,7 +225,7 @@ def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
                      TSRPlanner(delegate_planner=actual_planner),
                      self.cbirrt_planner),
             # Special purpose meta-planner.
-            NamedPlanner(delegate_planner=actual_planner),
+            NamedPlanner(delegate_planner=Sequence(actual_planner, self.cbirrt_planner)),
         )
 
         from prpy.planning.retimer import HauserParabolicSmoother