def put_left_arm_over_tray(robot, tray): """Move the robot's left arm so that it's about to grasp the right edge of the tray. Raises a generate_reaching_poses.GraspingPoseError if no IK solution can be found. Parameters: robot: a Robot instance tray: a KinBody instance """ manip = robot.GetManipulator('leftarm') ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot,iktype=openravepy.IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() min_x, max_x, min_y, max_y, z = utils.get_object_limits(tray) z -= 0.06 x = min_x + (max_x - min_x)/2 y = max_y - 0.02 gripper_angle = (np.pi, 0., 0) #pointing downward rot_mat = openravepy.rotationMatrixFromAxisAngle(gripper_angle) T = np.eye(4) T[:3,:3] = rot_mat T[:3,3] = [x,y,z] sol = generate_reaching_poses.check_reachable(env, tray, manip, [T], False) if sol is not None: robot.SetDOFValues(sol, manip.GetArmIndices()) #opening gripper robot.SetDOFValues([0.2], manip.GetGripperJoints()) else: raise generate_reaching_poses.GraspingPoseError("No IK solution for tray right side")
def get_occluding_objects(good_bodies, robot, object_to_grasp, max_trials = 0, just_one_attempt = False, return_pose = False, body_filter = None ): """Generates a list of all the objects that prevent the robot from reaching a target object. Several (up to max_trials) attempts are performed to grasp Parameters: robot: a openravepy.Robot instance object_to_grasp: a openravepy.KinBody instance representing the object to grasp Returns: a list of sets of objects """ if just_one_attempt != return_pose: raise ValueError("If asking for a return poes then set just_one_attempt to True") env = robot.GetEnv() robot_pose = robot.GetTransform() manip = robot.GetActiveManipulator() ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot,iktype=openravepy.IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() min_torso, max_torso = utils.get_pr2_torso_limit(robot) num_trial = 0 collisions_list = [] with robot: while num_trial < max_trials: num_trial +=1 # sample a base pose torso_angle = generate_reaching_poses.move_random_torso(robot, min_torso, max_torso) robot_pose = generate_reaching_poses.generate_random_pos(robot, object_to_grasp) robot.SetTransform(robot_pose) report = openravepy.CollisionReport() collision = env.CheckCollision(robot, report=report) if not collision: openravepy.raveLogInfo("Got a position not in collision") #sample a gripper pose grasping_poses = generate_reaching_poses.generate_grasping_pose(robot, object_to_grasp, use_general_grasps = True, checkik=False) openravepy.raveLogInfo("Got %d grasping poses" % len(grasping_poses)) #check if gripper pose is reachable from base pose #use robot's base pose to transform precomputed #gripper poses into the robot's frame of reference sol, collisions = generate_reaching_poses.check_reachable(good_bodies, env, object_to_grasp, manip, grasping_poses, only_reachable = True) if sol is None: print "Finding collisions: trial {0} No sol from base pose to gripper pose".\ format(num_trial) goodCollisions = False if body_filter is not None: goodCollisions = filter(body_filter, collisions) print "good collisions: " + repr(goodCollisions) print "all collisions: " + repr(collisions) if sol is not None and goodCollisions: print "Sol from base pose to gripper pose found in trial {0}".\ format(num_trial) openravepy.raveLogInfo("Getting the list of collisions") with robot: #robot.SetDOFValues(sol, robot.GetActiveManipulator().GetArmIndices()); #collisions_list.append(utils.get_all_collisions(robot, env)) collisions_list.append(collisions) if just_one_attempt: if return_pose: return (robot_pose, sol, torso_angle, collisions_list) else: return collisions_list if num_trial == max_trials: print "Getting obj names: No gripper pose reachable from collision free base pose found", print "after {0} trials".format(num_trial) if return_pose: return (robot_pose, None, torso_angle, collisions_list) else: return collisions_list