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