def find_bp(self, surface): """""Find a base pose for putting down """"" print "Getting base pose for putting down" robot_pose, _, _= generate_reaching_poses.get_collision_free_surface_pose(self.getGoodBodies(), self.robot, surface, ) return robot_pose
def find_bp(self, surface): """""Find a base pose for putting down """ "" print "Getting base pose for putting down" robot_pose, _, _ = generate_reaching_poses.get_collision_free_surface_pose( self.getGoodBodies(), self.robot, surface, ) return robot_pose
def putdown(self, obj_name, table_name, _unused1): print "Putting down object %s on %s" % (obj_name, table_name) obj = self.env.GetKinBody(obj_name) if obj is None: raise ValueError("Object %s does not exist" % obj_name) if table_name.startswith("dest_"): #this is a fixed location!!! T = getattr(tray_world, table_name, None) if T is None: raise ValueError( "The location %s is unknown! check spelling?" % table_name) T = T.copy() #put the gripper facing down gripper_angle = (np.pi, 0., 0) #just got this from trial and test rot_mat = openravepy.rotationMatrixFromAxisAngle(gripper_angle) T[:3, :3] = rot_mat T[2, 3] += 0.03 try: (pose, sol, torso_angle ) = generate_reaching_poses.get_collision_free_ik_pose( self.getGoodBodies(), self.env, obj, self.robot, T, only_reachable=False, max_trials=1000) except generate_reaching_poses.GraspingPoseError: raise ExecutingException( "Putting down on location has problems!") else: table = self.env.GetKinBody(table_name) if table is None: raise ValueError("Object %s does not exist" % table_name) try: pose, sol, torso_angle = generate_reaching_poses.get_collision_free_surface_pose( self.getGoodBodies(), self.robot, table, ) except generate_reaching_poses.GraspingPoseError, e: raise e
def putdown(self, obj_name, table_name, _unused1): print "Putting down object %s on %s" %(obj_name, table_name) obj = self.env.GetKinBody(obj_name) if obj is None: raise ValueError("Object %s does not exist" % obj_name) if table_name.startswith("dest_"): #this is a fixed location!!! T = getattr(tray_world, table_name, None) if T is None: raise ValueError("The location %s is unknown! check spelling?" % table_name) T = T.copy() #put the gripper facing down gripper_angle = (np.pi, 0., 0) #just got this from trial and test rot_mat = openravepy.rotationMatrixFromAxisAngle(gripper_angle) T[:3,:3] = rot_mat T[2, 3] += 0.03 try: (pose, sol, torso_angle) = generate_reaching_poses.get_collision_free_ik_pose( self.getGoodBodies(), self.env, obj, self.robot, T, only_reachable=False, max_trials =1000 ) except generate_reaching_poses.GraspingPoseError: raise ExecutingException("Putting down on location has problems!") else: table = self.env.GetKinBody(table_name) if table is None: raise ValueError("Object %s does not exist" % table_name) try: pose, sol, torso_angle = generate_reaching_poses.get_collision_free_surface_pose(self.getGoodBodies(), self.robot, table, ) except generate_reaching_poses.GraspingPoseError, e: raise e