diff --git a/src/herbpy/action/grasping.py b/src/herbpy/action/grasping.py index 449eb82..499190e 100755 --- a/src/herbpy/action/grasping.py +++ b/src/herbpy/action/grasping.py @@ -189,7 +189,7 @@ 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 @@ -197,6 +197,7 @@ def Place(robot, obj, on_obj, manip=None, render=True, **kw_args): @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() diff --git a/src/herbpy/herbrobot.py b/src/herbpy/herbrobot.py index f313091..2795a13 100644 --- a/src/herbpy/herbrobot.py +++ b/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