def check_reachable(good_bodies, env, obj_to_grasp, manip, grasping_poses, only_reachable = False): """Check if the robot can reach at least one pose Parameters: manip: a Manipulator instance grasping_poses: a list of (4x4 matrix). only_reachable: do not check for collisions """ # opening gripper pr2 = env.GetRobots()[0] v = pr2.GetActiveDOFValues() v[pr2.GetJoint('r_gripper_l_finger_joint').GetDOFIndex()]=0.54 pr2.SetDOFValues(v) env.Remove(obj_to_grasp) if len(grasping_poses) == 0: return None, [] if only_reachable: options = (openravepy.IkFilterOptions.IgnoreEndEffectorCollisions) else: # options = (openravepy.IkFilterOptions.IgnoreEndEffectorCollisions | # openravepy.IkFilterOptions.CheckEnvCollisions) options = (openravepy.IkFilterOptions.CheckEnvCollisions) for pose in grasping_poses: sol = manip.FindIKSolution(pose, options) if sol is None: continue # if sol has collisions with unmovable objects, continue pr2.SetDOFValues(sol, pr2.GetActiveManipulator().GetArmIndices()); collisions = utils.get_all_collisions(pr2, env) # unmovable = lambda b: not (b.GetName().startswith("random") or\ # b.GetName().startswith('object')) # unmovable_bodies = filter(unmovable, collisions) # if len(unmovable_bodies) == 0: # pass # #continue bad_body_exists = False for body in collisions: if body not in good_bodies: bad_body_exists = True break if bad_body_exists: continue env.AddKinBody(obj_to_grasp) return sol, collisions env.AddKinBody(obj_to_grasp) return None, []
def get_collisions(self, traj, n=100): orig_values = self.robot.GetDOFValues(self.manip.GetArmIndices()); collisions = set() traj_up = mu.interp2d(np.linspace(0,1,n), np.linspace(0,1,len(traj)), traj) with self.env: for joint_values in traj_up: self.robot.SetDOFValues(joint_values, self.manip.GetArmIndices()) for obj in utils.get_all_collisions(self.robot, self.env): collisions.add(obj) self.robot.SetDOFValues(orig_values, self.manip.GetArmIndices()) return collisions
def get_occluding_objects(robot, object_to_grasp, max_trials = 100, just_one_attempt = False, return_pose = False ): """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 = generate_reaching_poses.check_reachable(manip, grasping_poses, only_reachable = True) if sol is None: print "Trial {0} No sol from base pose to gripper pose".\ format(num_trial) if sol is not None: 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)) 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 "No gripper pose reachable from collision free base pose found", print "after {0} trials".format(num_trial) return collisions_list