def find_grasp_pose(self, target, collision_object_name='', collision_support_surface_name=''): """ target = GraspableObject collision_object_name = name of target in collision map collision_support_surface_name = name of surface target is touching """ req = GraspPlanningRequest() req.arm_name = ARM_GROUP_NAME req.target = target req.collision_object_name = collision_object_name req.collision_support_surface_name = collision_support_surface_name rospy.loginfo( '%s: trying to find a good grasp for graspable object %s' % (ACTION_NAME, collision_object_name)) grasping_result = self.grasp_planning_srv(req) if grasping_result.error_code.value != GraspPlanningErrorCode.SUCCESS: rospy.logerr('%s: unable to find a feasable grasp, aborting' % ACTION_NAME) return None pose_stamped = PoseStamped() pose_stamped.header.frame_id = grasping_result.grasps[ 0].grasp_posture.header.frame_id pose_stamped.pose = grasping_result.grasps[0].grasp_pose rospy.loginfo('%s: found good grasp, looking for corresponding IK' % ACTION_NAME) return self.find_ik_for_grasping_pose(pose_stamped)
def find_grasp_pose(self, target, collision_object_name='', collision_support_surface_name=''): """ target = GraspableObject collision_object_name = name of target in collision map collision_support_surface_name = name of surface target is touching """ req = GraspPlanningRequest() req.arm_name = ARM_GROUP_NAME req.target = target req.collision_object_name = collision_object_name req.collision_support_surface_name = collision_support_surface_name rospy.loginfo('%s: trying to find a good grasp for graspable object %s' % (ACTION_NAME, collision_object_name)) grasping_result = self.grasp_planning_srv(req) if grasping_result.error_code.value != GraspPlanningErrorCode.SUCCESS: rospy.logerr('%s: unable to find a feasable grasp, aborting' % ACTION_NAME) return None pose_stamped = PoseStamped() pose_stamped.header.frame_id = grasping_result.grasps[0].grasp_posture.header.frame_id pose_stamped.pose = grasping_result.grasps[0].grasp_pose rospy.loginfo('%s: found good grasp, looking for corresponding IK' % ACTION_NAME) return self.find_ik_for_grasping_pose(pose_stamped)
def plan_grasp(self, graspable, which_arm, graspable_name = "", table_name = "", ): """Picks up a previously detected object. Parameters: graspable: an object_manipulation_msgs/GraspableObject msg instance. This usually comes from a Detector.call_collision_map_processing call. graspable_name: the name of the object to graps. It is provided by Detector.call_collision_map_processing. table_name: the name of the table. Again provided by Detector.call_collision_map_processing. which_arm: left_arm or right_arm Return: a object_manipulation_msgs.GraspPlanningResponse msg """ if self.grap_planning_srv is None: srv_name = "/plan_point_cluster_grasp" rospy.loginfo("Waiting for service %s", srv_name) rospy.wait_for_service(srv_name) self.grap_planning_srv = rospy.ServiceProxy(srv_name, GraspPlanning) rospy.loginfo("Calling the grasp planning service") gp = GraspPlanningRequest() gp.arm_name = which_arm gp.target = graspable gp.collision_object_name = graspable_name gp.collision_support_surface_name = table_name res = self.grap_planning_srv(gp) isinstance(res, GraspPlanningResponse) if res.error_code.value != res.error_code.SUCCESS: rospy.logerr("Could not find valid grasps!") return None else: grasps = sorted(res.grasps, key = lambda g:g.success_probability) res.grasps = grasps return res
def generate_scored_grasps(self, pc = None, whicharm = "leftarm", pullup = False): if pc is None: rospy.loginfo("Waiting for BOLT pointcloud") pc = rospy.wait_for_message("/bolt/vision/pcl_robot", PointCloud2) rospy.loginfo("Ok got it!") xyz, rgb = utils.pc2xyzrgb(pc) weights = rgb[0,:,0] sorted_indexes = np.argsort(weights)[::-1] weights = weights[sorted_indexes] xyz = xyz[0,:,:] xyz = xyz[sorted_indexes,:] rospy.loginfo("Initially The maximum weight is %f", weights.max()) rospy.loginfo("Initially The min weight is %f", weights.min()) trim_weights = 5 rospy.loginfo("Taking only the maximum %d weights", trim_weights) weights = weights[:trim_weights] xyz = xyz[:trim_weights,:] rospy.loginfo("XYZ shape %s, W shape: %s", xyz.shape, weights.shape) rospy.loginfo("After trimming The maximum weight is %f", weights.max()) rospy.loginfo("After trimming The min weight is %f", weights.min()) rospy.wait_for_service('evaluate_point_cluster_grasps') evaluate_grasps = rospy.ServiceProxy('evaluate_point_cluster_grasps', GraspPlanning) grasp_planning = GraspPlanningRequest() grasps = [] for coords in xyz: _g = utils.create_spaced_downward_grasps(coords, pc,40) grasps.extend(_g) rospy.loginfo("Testing a total of %d grasps", len(grasps)) graspable = utils.pc2graspable(pc) grasp_planning.target = graspable grasp_planning.grasps_to_evaluate = grasps res = evaluate_grasps(grasp_planning) grasps = res.grasps non_zero_grasps = sorted([g for g in grasps if g.success_probability > 0], key = lambda g:g.success_probability, reverse=True) if len(non_zero_grasps) == 0: rospy.logerr("Error: no valid grasps found!") return False probs = [g.success_probability for g in non_zero_grasps] rospy.loginfo("probabilities are: %s", probs) rospy.loginfo("Number of non-zero %d grasps", len(non_zero_grasps)) #self.publish_grasps(res) for g in non_zero_grasps: rospy.loginfo("Testing a grasp") ps = PoseStamped() ps.header.frame_id = pc.header.frame_id ps.pose = g.grasp_pose self.publish_gripper_pose(ps) approach = copy.deepcopy(ps) approach.pose.position.z += 0.1 if whicharm == "leftarm": self.robot.move_left_arm(approach) self.robot.controller.open_left_gripper(True) success = self.robot.move_left_arm(ps) if success: self.robot.controller.close_left_gripper(True) if pullup: self.robot.move_left_arm(approach) else: self.robot.move_right_arm(approach) self.robot.controller.open_right_gripper(True) success = self.robot.move_right_arm(ps) if success: self.robot.controller.close_right_gripper(True) if pullup: self.robot.move_right_arm(approach) if success: break
def pick(self,pickup_goal): #prepare result pickresult = PickupResult() #get grasps for the object # fill up a grasp planning request grasp_planning_req = GraspPlanningRequest() grasp_planning_req.arm_name = pickup_goal.arm_name grasp_planning_req.target = pickup_goal.target object_to_attach = pickup_goal.collision_object_name # call grasp planning service grasp_planning_res = self.grasp_planning_service_.call(grasp_planning_req) #print grasp_planning_res # process grasp planning result if (grasp_planning_res.error_code.value != grasp_planning_res.error_code.SUCCESS): rospy.logerr("No grasp found for this object, we will generate some, but only when the node is ready for that !") pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE return pickresult else: rospy.loginfo("Got "+ str(len(grasp_planning_res.grasps)) +" grasps for this object") # for each grasp, generate rotational symmetric grasps around the object (this is already in the DB for the CokeCan but should be removed and done online) #for each grasp, check path from pre-grasp pose to grasp pose first and then check motion to pre-grasp pose motion_plan_res=GetMotionPlanResponse() grasp_to_execute_=Grasp() for index, grasp in enumerate(grasp_planning_res.grasps): # extract grasp_pose grasp_pose_ = PoseStamped() grasp_pose_.header.frame_id = "/world"; grasp_pose_.pose = grasp.grasp_pose grasp_pose_.pose.position.y=grasp_pose_.pose.position.y-0.01 #-0.01 in sim, +0.03 in real #cheating here # copy the grasp_pose as a pre-grasp_pose pre_grasp_pose_ = copy.deepcopy(grasp_pose_) # add desired_approach_distance along the approach vector. above the object to plan pre-grasp pose # currently add this to Z because approach vector needs to be computed somehow first (TODO) pre_grasp_pose_.pose.position.z = pre_grasp_pose_.pose.position.z + 0.05 # for distance from 0 (grasp_pose) to desired_approach distance (pre_grasp_pose) test IK/Collision and save result # decompose this in X steps depending on distance to do and max speed interpolated_motion_plan_res = self.plan.get_interpolated_ik_motion_plan(pre_grasp_pose_, grasp_pose_, False) # check the result (depending on number of steps etc...) if (interpolated_motion_plan_res.error_code.val == interpolated_motion_plan_res.error_code.SUCCESS): number_of_interpolated_steps=0 # check if one approach trajectory is feasible for interpolation_index, traj_error_code in enumerate(interpolated_motion_plan_res.trajectory_error_codes): if traj_error_code.val!=1: rospy.logerr("One unfeasible approach-phase step found at "+str(interpolation_index)+ "with val " + str(traj_error_code.val)) break else: number_of_interpolated_steps=interpolation_index # if trajectory is feasible then plan reach motion to pre-grasp pose if number_of_interpolated_steps+1==len(interpolated_motion_plan_res.trajectory.joint_trajectory.points): rospy.loginfo("Grasp number "+str(index)+" approach is possible, checking motion plan to pre-grasp") #print interpolated_motion_plan_res # check and plan motion to this pre_grasp_pose motion_plan_res = self.plan.plan_arm_motion( pickup_goal.arm_name, "jointspace", pre_grasp_pose_ ) #if this pre-grasp pose is successful do not test others if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS): rospy.loginfo("Grasp number "+str(index)+" is possible, executing it") # copy the grasp to execute for the following steps grasp_to_execute_ = copy.deepcopy(grasp) break else: rospy.logerr("Grasp number "+str(index)+" approach is impossible") #print interpolated_motion_plan_res else: rospy.logerr("Grasp number "+str(index)+" approach is impossible") #print interpolated_motion_plan_res # execution part if (motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS): #put hand in pre-grasp posture if self.pre_grasp_exec(grasp_to_execute_)<0: #QMessageBox.warning(self, "Warning", # "Pre-grasp action failed: ") pickresult.manipulation_result.value = ManipulationResult.FAILED return pickresult #go there # filter the trajectory filtered_traj = self.filter_traj_(motion_plan_res) self.display_traj_( filtered_traj ) # reach pregrasp pose if self.send_traj_( filtered_traj )<0: #QMessageBox.warning(self, "Warning", # "Reach trajectory execution failed: ") pickresult.manipulation_result.value = ManipulationResult.FAILED return pickresult #time.sleep(20) # TODO use actionlib here time.sleep(self.simdelay) # TODO use actionlib here # approach if self.send_traj_( interpolated_motion_plan_res.trajectory.joint_trajectory )<0: #QMessageBox.warning(self, "Warning", # "Approach trajectory execution failed: ") pickresult.manipulation_result.value = ManipulationResult.FAILED return pickresult time.sleep(self.simdelay) # TODO use actionlib here #grasp if self.grasp_exec(grasp_to_execute_)<0: #QMessageBox.warning(self, "Warning", # "Grasp action failed: ") pickresult.manipulation_result.value = ManipulationResult.FAILED return pickresult time.sleep(self.simdelay) # TODO use actionlib here #attach the collision object to the hand (should be cleaned-up) rospy.loginfo("Now we attach the object") att_object = AttachedCollisionObject() att_object.link_name = "palm" att_object.object.id = object_to_attach att_object.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT att_object.object.header.frame_id = "palm" att_object.object.header.stamp = rospy.Time.now() object = Shape() object.type = Shape.CYLINDER object.dimensions.append(.03) object.dimensions.append(0.1) pose = Pose() pose.position.x = 0.0 pose.position.y = -0.06 pose.position.z = 0.06 pose.orientation.x = 0 pose.orientation.y = 0 pose.orientation.z = 0 pose.orientation.w = 1 att_object.object.shapes.append(object) att_object.object.poses.append(pose); att_object.touch_links= ["ffdistal","mfdistal","rfdistal","lfdistal","thdistal","ffmiddle","mfmiddle","rfmiddle","lfmiddle","thmiddle","ffproximal","mfproximal","rfproximal","lfproximal","thproximal","palm","lfmetacarpal","thbase"] self.att_object_in_map_pub_.publish(att_object) rospy.loginfo("Attach object published") else: rospy.logerr("None of the grasps tested is possible") pickresult.manipulation_result.value = ManipulationResult.UNFEASIBLE return pickresult pickresult.manipulation_result.value = ManipulationResult.SUCCESS pickresult.grasp= grasp_to_execute_ return pickresult