def get_collision_free_surface_pose(good_bodies, robot, obj, max_trials = 500, use_general_grasps = True): env = robot.GetEnv() robot_pose = robot.GetTransform() torso_angle = robot.GetJoint("torso_lift_joint").GetValues()[0] manip = robot.GetActiveManipulator() ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot,iktype=openravepy.IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() grasping_poses = generate_manip_above_surface(obj) env.GetCollisionChecker().SetCollisionOptions(openravepy.CollisionOptions.Contacts) collision = env.CheckCollision(robot) sol, _ = check_reachable(good_bodies, env, obj, manip, grasping_poses) isreachable = sol is not None min_torso, max_torso = utils.get_pr2_torso_limit(robot) num_trial = 0 with robot: while ((collision) or (not isreachable)) and (num_trial < max_trials): num_trial +=1 torso_angle = move_random_torso(robot, min_torso, max_torso) robot_pose = generate_random_pos(robot, obj) robot.SetTransform(robot_pose) report = openravepy.CollisionReport() collision = env.CheckCollision(robot, report=report) if not collision: grasping_poses = generate_manip_above_surface(obj) sol, _ = check_reachable(good_bodies, env, obj, manip, grasping_poses) isreachable = sol is not None else: continue if (sol is None) or collision: raise GraspingPoseError("No collision free grasping pose found within %d steps" % max_trials) else: return (robot_pose, sol, torso_angle)
def get_collision_free_ik_pose(good_bodies, robot, object_to_grasp, ik_pose, max_trials = 100, only_reachable=False ): """Returns the position from where the robot can reach a position (in cartesian coordinates). The active manipulator is used. Parameters: robot: an OpenRave robot instance ik_pose: a 4x4 matrix with the desired 6D pose max_trials: how many attempts before giving up use_general_grasps: f True, don't calculate actual grasp points, but use a pre-generated list. It is much faster if a grasping model has not been generated. Returns: (robot_pose, sol, torso_angle): the robot position (as a transformation matrix), the active manipulator angles and the torso joint angle from where the robot can grasp an object. Raises GraspingPoseError if no valid solution is found. """ env = robot.GetEnv() robot_pose = robot.GetTransform() torso_angle = robot.GetJoint("torso_lift_joint").GetValues()[0] manip = robot.GetActiveManipulator() ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot,iktype=openravepy.IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() env.GetCollisionChecker().SetCollisionOptions(openravepy.CollisionOptions.Contacts) collision = env.CheckCollision(robot) sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, [ik_pose], only_reachable) isreachable = sol is not None min_torso, max_torso = utils.get_pr2_torso_limit(robot) num_trial = 0 xyz = ik_pose[:3, 3] with robot: while ((collision) or (not isreachable)) and (num_trial < max_trials): num_trial +=1 torso_angle = move_random_torso(robot, min_torso, max_torso) robot_pose = generate_random_pos(robot, xyz) robot.SetTransform(robot_pose) report = openravepy.CollisionReport() collision = env.CheckCollision(robot, report=report) if not collision: sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, [ik_pose], only_reachable) isreachable = sol is not None else: continue if (sol is None) or collision: e = GraspingPoseError("No collision free IK pose found within %d steps" % max_trials) raise e else: return (robot_pose, sol, torso_angle)
def get_torso_grasping_pose(good_bodies, robot, object_to_grasp, max_trials = 100, use_general_grasps = True, ): """Returns the torso height from where the robot can grasp an object. Parameters: robot: an OpenRave robot instance object_to_grasp: a KinBody that the robot should grasp max_trials: how many attempts before giving up use_general_grasps: f True, don't calculate actual grasp points, but use a pre-generated list. It is much faster if a grasping model has not been generated. Returns: (sol, torso_angle): , the active manipulator angles and the torso joint angle from where the robot can grasp an object. Raises GraspingPoseError if no valid solution is found. """ env = robot.GetEnv() robot_pose = robot.GetTransform() torso_angle = robot.GetJoint("torso_lift_joint").GetValues()[0] manip = robot.GetActiveManipulator() ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot,iktype=openravepy.IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() grasping_poses = generate_grasping_pose(robot, object_to_grasp, use_general_grasps) openravepy.raveLogInfo("I've got %d grasping poses" % len(grasping_poses)) env.GetCollisionChecker().SetCollisionOptions(openravepy.CollisionOptions.Contacts) collision = env.CheckCollision(robot) sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, grasping_poses) isreachable = sol is not None min_torso, max_torso = utils.get_pr2_torso_limit(robot) num_trial = 0 with robot: while ((collision) or (not isreachable)) and (num_trial < max_trials): num_trial +=1 torso_angle = move_random_torso(robot, min_torso, max_torso) report = openravepy.CollisionReport() collision = env.CheckCollision(robot, report=report) if not collision: grasping_poses = generate_grasping_pose(robot, object_to_grasp, use_general_grasps) sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, grasping_poses) isreachable = sol is not None else: continue if (sol is None) or collision: e = GraspingPoseError("No collision free grasping pose found within %d steps" % max_trials) raise e else: return (sol, torso_angle)
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