def StackCups(robot, table, cup, stack, cups_stacked, manip=None): """ @param robot The robot performing the stacking @param cup The cup that is being stacked @param stack The stack of cups the cup is going to be placed on @param cups_stacked A list of the cups that have been stacked @param manip The arm being used to stack cups """ if manip == None: manip = robot.GetActiveManipulator() env = robot.GetEnv() with env: aabb_cup = cup.ComputeAABB() aabb_stack = stack.ComputeAABB() stack_height = 2*aabb_stack.extents()[2] + (len(cups_stacked)+1)*0.03 herb_in_world = robot.GetTransform() cup_in_world = cup.GetTransform() cup_in_herb = numpy.dot(numpy.linalg.inv(herb_in_world), cup_in_world) theta = numpy.arctan2(cup_in_herb[1,0], cup_in_herb[0,0]) if manip == robot.left_arm: yaw_in_herb = numpy.array([-numpy.pi, 0]) else: yaw_in_herb = numpy.array([0, numpy.pi]) yaw_in_cup = yaw_in_herb - theta robot.PushGrasp(cup, yaw_range=yaw_in_cup) from prpy.rave import Disabled with Disabled(table): #Lift up cup manip.PlanToEndEffectorOffset(direction=[0, 0, 1], distance = stack_height, position_tolerance=0.1, execute=True) with env: aabb_cup = cup.ComputeAABB() aabb_stack = stack.ComputeAABB() direc = numpy.array([0., 0., 0.]) direc[:2] = aabb_stack.pos()[:2] - aabb_cup.pos()[:2] dist = numpy.linalg.norm(direc) manip.PlanToEndEffectorOffset(direction=direc, distance = dist, position_tolerance=0.05, execute=True) #Move cup down with Disabled(stack): manip.PlanToEndEffectorOffset(direction=[0, 0, -1], distance = 0.02, position_tolerance=0.1, execute=True) #Release cup manip.hand.OpenHand() with env: robot.Release(cup) cup.SetTransform(stack.GetTransform()) #Add cup to list of cups stacked cups_stacked.append(cup) return cups_stacked
def MoveCupAndPour(robot, table, manip_pitcher, manip_cup, cup, pitcher): """ @param robot The robot performing the pouring @param table The table the objects are on @param manip_pitcher The arm that is manipulating the pitcher @param manip_cup The arm that is manipulating the cup @param cup The cup to pour into @param pitcher The pitcher to pour """ env = robot.GetEnv() with env: pitcher_start = pitcher.GetTransform() pitcher_aabb = pitcher.ComputeAABB() cup_pose = cup.GetTransform() cup_aabb = cup.ComputeAABB() manip_cup_pose = manip_cup.GetEndEffectorTransform() manip_pitcher_pose = manip_pitcher.GetEndEffectorTransform() p_tsr, min_tilt, max_tilt = robot.tsrlibrary(pitcher, 'pour') traj = manip_pitcher.PlanToTSR(p_tsr, execute=False) cspec = traj.GetConfigurationSpecification() wpt = traj.GetWaypoint(traj.GetNumWaypoints() - 1) config = cspec.ExtractJointValues(wpt, robot, manip_pitcher.GetArmIndices()) with env: with robot.CreateRobotStateSaver( openravepy.KinBody.SaveParameters.LinkTransformation): manip_pitcher.SetDOFValues(config) desired_cup_in_world_x = pitcher_start[0, 3] + ( 2 * pitcher_aabb.extents()[0]) desired_cup_in_world_y = pitcher_start[1, 3] - ( 2 * pitcher_aabb.extents()[1]) cup_in_world_x = cup_pose[0, 3] cup_in_world_y = cup_pose[1, 3] move_x = desired_cup_in_world_x - cup_in_world_x + cup_aabb.extents( )[0] / 2 move_y = desired_cup_in_world_y - cup_in_world_y - cup_aabb.extents( )[1] move_cup = math.sqrt(move_x**2 + move_y**2) from prpy.rave import Disabled with Disabled(table): manip_cup.PlanToEndEffectorOffset(direction=[move_x, move_y, 0], distance=move_cup, position_tolerance=0.1, execute=True, timelimit=10) manip_cup.PlanToEndEffectorOffset(direction=[0, 0, -1], distance=0.007, position_tolerance=0.1, execute=True, timelimit=10) manip_cup.hand.OpenHand() with env: robot.Release(cup) manip_cup.PlanToEndEffectorOffset(direction=-1 * manip_cup_pose[:3, 2], distance=0.1, position_tolerance=0.1, execute=True, timelimit=10) manip_cup.PlanToNamedConfiguration('home', execute=True) # Pour # slow the arm down v = numpy.array([0.75, 0.75, 2., 2., 2.5, 2.5, 2.5]) with robot.CreateRobotStateSaver( openravepy.KinBody.SaveParameters.JointMaxVelocityAndAcceleration): manip_pitcher.SetVelocityLimits(0.5 * v, 0.3) #Gets position of the pitcher and pours traj = robot.PostProcessPath(traj) robot.ExecuteTrajectory(traj) time.sleep(1) #To let the water pour from pitcher into cup # Tilts pitcher back to original position robot.ExecuteTrajectory(openravepy.planningutils.ReverseTrajectory(traj)) manip_pitcher.PlanToEndEffectorOffset(direction=-1 * manip_pitcher_pose[:3, 0], distance=0.1, position_tolerance=0.1, execute=True)
def _GrabBlock(robot, blocks, table, manip=None, preshape=None, **kw_args): """ @param robot The robot performing the grasp @param block The block to grab @param table The table, or object, the block is resting on @param manip The manipulator to use to grab the block (if None active manipulator is used) @param preshape The shape (dof_values) to move the hand to before executing the grab """ from prpy.rave import AllDisabled, Disabled from prpy.viz import RenderTSRList, RenderVector env = robot.GetEnv() block = None if manip is None: with env: manip = robot.GetActiveManipulator() if preshape is None: preshape = [1.7, 1.7, 0.2, 2.45] # First move the hand to the right preshape manip.hand.MoveHand(*preshape) block_tsr_list = [] with env: for b in blocks: # Get a TSR to move near the block. tsr_list = robot.tsrlibrary(b, 'grasp', manip=manip) block_tsr_list += tsr_list # Plan to a pose above the block with RenderTSRList(block_tsr_list, robot.GetEnv()): with Disabled(table, padding_only=True): manip.PlanToTSR(block_tsr_list, execute=True) with manip.GetRobot().GetEnv(): ee_pose = manip.GetEndEffectorTransform() block_idxs = [ idx for idx, tsr_chain in enumerate(block_tsr_list) if tsr_chain.contains(ee_pose) ] if len(block_idxs) == 0: raise NoTSRException("Failed to find the TSR PlanToTSR planned to") block = blocks[block_idxs[0]] try: with AllDisabled(env, [table] + blocks, padding_only=True): # Move down until touching the table with env: start_point = manip.GetEndEffectorTransform()[0:3, 3] table_aabb = ComputeEnabledAABB(table) #table_height = table_aabb.pos()[2] + table_aabb.extents()[2] # 0.14 is the distance from finger tip to end-effector frame #current_finger_height = manip.GetEndEffectorTransform()[2, 3] - 0.14 min_distance = 0.10 # current_finger_height - table_height down_direction = [0., 0., -1.] with RenderVector(start_point, down_direction, min_distance, env): manip.MoveUntilTouch(direction=down_direction, timelimit=5, distance=min_distance, max_distance=min_distance + 0.05, ignore_collisions=blocks + [table]) # Move parallel to the table to funnel the block into the fingers # by projecting the -x direction of end-effector onto the xy-plane. with env: start_point = manip.GetEndEffectorTransform()[0:3, 3] funnel_direction = -1. * manip.GetEndEffectorTransform()[:3, 0] funnel_direction[2] = 0. # TODO: We should only have to disable the block for this. Why does # this fail if we do not disable the table? with AllDisabled(env, blocks + [table]): with RenderVector(start_point, funnel_direction, 0.1, env): manip.PlanToEndEffectorOffset(direction=funnel_direction, distance=0.08, max_distance=0.12, timelimit=5., execute=True) # Close the finger to grab the block manip.hand.MoveHand(f3=1.7) # Compute the pose of the block in the hand frame with env: local_p = [0.01, 0, 0.24, 1.0] hand_pose = manip.GetEndEffectorTransform() world_p = numpy.dot(hand_pose, local_p) block_pose = block.GetTransform() block_pose[:, 3] = world_p block_relative = numpy.dot(numpy.linalg.inv(hand_pose), block_pose) # Now lift the block up off the table with AllDisabled(env, blocks + [table]): manip.PlanToEndEffectorOffset(direction=[0, 0, 1], distance=0.05, timelimit=5, execute=True) # OpenRAVE trick to hallucinate the block into the correct # pose relative to the hand with env: hand_pose = manip.GetEndEffectorTransform() block_pose = numpy.dot(hand_pose, block_relative) block.SetTransform(block_pose) manip.GetRobot().Grab(block) return block except PlanningError as e: logger.error('Failed to complete block grasp') raise
glass = env.ReadKinBodyXMLFile(glass_path) env.Add(glass) glass_transform = numpy.eye(4) glass_transform[:3, 3] = [0.6239455840637041, -0.4013916328109689, 0.75] glass.SetTransform(glass_transform) # Goal pose for the object import numpy goal_in_table = [0.15, 0., -0.03, 1.] goal_in_world = numpy.dot(table.GetTransform(), goal_in_table) goal_pose = goal_in_world[:2] goal_radius = 0.1 from prpy.rave import Disabled from prpy.util import ComputeEnabledAABB with Disabled(table, padding_only=True): table_aabb = ComputeEnabledAABB(table) table_height = 0.01 + table_aabb.pos()[2] + table_aabb.extents()[2] pts = [] for a in numpy.arange(0, 2.*numpy.pi, 0.1): pts.append([goal_pose[0] + goal_radius*numpy.sin(a), goal_pose[1] + goal_radius*numpy.cos(a), table_height]) h = env.drawlinestrip(points=numpy.array(pts), linewidth=5.0, colors=numpy.array([0.0, 1., 0.])) # Plan to the start configuration try: push_arm.SetActive() start_pose = [4.49119545, -1.59899798, -0.6, 1.65274406, -1.7742985, -0.63854765, -1.23051631]
def PushToPoseOnTable(robot, obj, table, goal_position, goal_radius, manip=None, max_plan_duration=30.0, shortcut_time=3., render=True, search=True, tracker=None, **kw_args): """ @param robot The robot performing the push @param obj The object to push @param table The table kinbody the object is resting on @param goal_position The desired (x,y) position of the object in world coordinates @param goal_radius The max distance from goal_position for the object to be to still consider the goal achieved @param manip The manipulator to use for the push - if None the active manipulator is used @param max_plan_duration The max time to run the planner @param shortcut_time The amount of time to spend shortcutting, if 0. no shortcutting is performed @param render If true, render the trajectory while executing @param tracker The perception module to use to track the object during pushing """ # Get a push planner try: if search: from or_pushing.discrete_search_push_planner import DiscreteSearchPushPlanner planner = DiscreteSearchPushPlanner(robot.GetEnv()) else: from or_pushing.push_planner import PushPlanner planner = PushPlanner(robot.GetEnv()) except ImportError: raise ActionError( "Unable to create PushPlanner. Is the randomized_rearrangement_planning" "repository checked out in your workspace?") # Get the manipulator if manip is None: with robot.GetEnv(): manip = robot.GetActiveManipulator() with robot.GetEnv(): from prpy.rave import Disabled from prpy.util import ComputeEnabledAABB with Disabled(table, padding_only=True): table_aabb = ComputeEnabledAABB(table) ee_pushing_transform = manip.GetEndEffectorTransform() # Make the state bounds be at the edges of the table table_pos = table_aabb.pos() table_extents = table_aabb.extents() sbounds = { 'high': [ table_pos[0] + table_extents[0], table_pos[1] + table_extents[1], 2. * numpy.pi ], 'low': [table_pos[0] - table_extents[0], table_pos[1] - table_extents[1], 0] } # Assume we want to keep the current orientation and height of the # manipulator throughout the push ee_pushing_transform[:2, 3] = [0., 0.] #ignore x,y pose # Compute the goal pose #table_height = table_pos[2] + table_extents[2] goal_pose = [goal_position[0], goal_position[1]] with robot.CreateRobotStateSaver(): traj = planner.PushToPose( robot, obj, goal_pose, state_bounds=sbounds, pushing_manifold=ee_pushing_transform.flatten().tolist(), max_plan_duration=max_plan_duration, goal_radius=goal_radius, **kw_args) if traj is None: raise PlanningError('Failed to find pushing plan') # Execute from prpy.viz import RenderTrajectory with RenderTrajectory(robot, traj, color=[1, 0, 0, 1], render=render): if shortcut_time > 0: traj = planner.ShortcutPath(timelimit=shortcut_time) with RenderTrajectory(robot, traj, color=[0, 0, 1, 1], render=render): if tracker is not None: tracker.StartTrackObject(robot, obj.GetName()) try: if manip.simulated: # Use the push planner code to simulate path execution. # This simulates the pushing of the objects during # execution of the trajectory. planner.ExecutePlannedPath() else: # Execute the trajectory robot.ExecuteTrajectory(traj) # During execution, object pushes won't be simulated. # In the OpenRAVE world, the robot will instead move # through the objects and probably be in collision at # the end. This call sets all the objects to their # expected poses at the end of the trajectory. # If execution was successful, this should resolve # collisions. planner.SetFinalObjectPoses() finally: if tracker is not None: tracker.StopTrackObject(robot, obj.GetName()) return traj