def plan_point_cluster_grasp_callback(self, req): rospy.loginfo("planning grasps for a point cluster") resp = GraspPlanningResponse() (grasps, error_code) = self.plan_point_cluster_grasps(req.target, req.arm_name) resp.grasps = grasps resp.error_code = error_code return resp
def evaluate_point_cluster_grasps_callback(self, req): #rospy.loginfo("evaluating grasps for a point cluster") #find the cluster bounding box and relevant frames, and transform the cluster if len(req.target.cluster.points) > 0: self.pcgp.init_cluster_grasper(req.target.cluster) cluster_frame = req.target.cluster.header.frame_id else: self.pcgp.init_cluster_grasper(req.target.region.cloud) cluster_frame = req.target.region.cloud.header.frame_id #evaluate the grasps on the cluster probs = self.pcgp.evaluate_point_cluster_grasps( req.grasps_to_evaluate, cluster_frame) #return the same grasps with the qualities added for (grasp, prob) in zip(req.grasps_to_evaluate, probs): grasp.success_probability = prob #fill in the response resp = GraspPlanningResponse() resp.error_code.value = 0 resp.grasps = req.grasps_to_evaluate return resp
def plan_segmented_clutter_grasps_callback(self, req): rospy.loginfo("planning grasps for a point cloud") resp = GraspPlanningResponse() (grasps, error_code) = self.plan_grasps_for_target(req.target, req.arm_name) resp.grasps = grasps resp.error_code = error_code return resp
def evaluate_point_cluster_grasps_callback(self, req): #rospy.loginfo("evaluating grasps for a point cluster") #find the cluster bounding box and relevant frames, and transform the cluster if len(req.target.cluster.points) > 0: self.pcgp.init_cluster_grasper(req.target.cluster) cluster_frame = req.target.cluster.header.frame_id else: self.pcgp.init_cluster_grasper(req.target.region.cloud) cluster_frame = req.target.region.cloud.header.frame_id #evaluate the grasps on the cluster probs = self.pcgp.evaluate_point_cluster_grasps(req.grasps_to_evaluate, cluster_frame) #return the same grasps with the qualities added for (grasp, prob) in zip(req.grasps_to_evaluate, probs): grasp.success_probability = prob #fill in the response resp = GraspPlanningResponse() resp.error_code.value = 0 resp.grasps = req.grasps_to_evaluate return resp
def plan_point_cluster_grasp_callback(self, req): rospy.loginfo("planning grasps for a point cluster") resp = GraspPlanningResponse() #get the hand joint names from the param server (loaded from yaml config file) joint_names_dict = rospy.get_param('~joint_names') pregrasp_joint_angles_dict = rospy.get_param('~pregrasp_joint_angles') grasp_joint_angles_dict = rospy.get_param('~grasp_joint_angles') pregrasp_joint_efforts_dict = rospy.get_param('~pregrasp_joint_efforts') grasp_joint_efforts_dict = rospy.get_param('~grasp_joint_efforts') if req.arm_name: arm_name = req.arm_name else: arm_name = joint_names_dict.keys()[0] rospy.logerr("point cluster grasp planner: missing arm_name in request! Using "+arm_name) try: hand_joints = joint_names_dict[arm_name] except KeyError: arm_name = joint_names_dict.keys()[0] rospy.logerr("arm_name "+req.arm_name+" not found! Using joint names from "+arm_name) hand_joints = joint_names_dict[arm_name] pregrasp_joint_angles = pregrasp_joint_angles_dict[arm_name] grasp_joint_angles = grasp_joint_angles_dict[arm_name] pregrasp_joint_efforts = pregrasp_joint_efforts_dict[arm_name] grasp_joint_efforts = grasp_joint_efforts_dict[arm_name] #hand_joints = rospy.get_param('/hand_description/'+arm_name+'/hand_joints') rospy.loginfo("hand_joints:"+str(hand_joints)) #find the cluster bounding box and relevant frames, and transform the cluster init_start_time = time.time() if len(req.target.cluster.points) > 0: self.pcgp.init_cluster_grasper(req.target.cluster) cluster_frame = req.target.cluster.header.frame_id else: self.pcgp.init_cluster_grasper(req.target.region.cloud) cluster_frame = req.target.region.cloud.header.frame_id if len(cluster_frame) == 0: rospy.logerr("region.cloud.header.frame_id was empty!") resp.error_code.value = resp.error_code.OTHER_ERROR return resp init_end_time = time.time() #print "init time: %.3f"%(init_end_time - init_start_time) #plan grasps for the cluster (returned in the cluster frame) grasp_plan_start_time = time.time() (pregrasp_poses, grasp_poses, gripper_openings, qualities, pregrasp_dists) = self.pcgp.plan_point_cluster_grasps() grasp_plan_end_time = time.time() #print "total grasp planning time: %.3f"%(grasp_plan_end_time - grasp_plan_start_time) #add error code to service resp.error_code.value = resp.error_code.SUCCESS grasp_list = [] if pregrasp_poses == None: resp.error_code.value = resp.error_code.OTHER_ERROR return resp #fill in the list of grasps for (grasp_pose, quality, pregrasp_dist) in zip(grasp_poses, qualities, pregrasp_dists): pre_grasp_joint_state = self.create_joint_state(hand_joints, pregrasp_joint_angles, pregrasp_joint_efforts) grasp_joint_state = self.create_joint_state(hand_joints, grasp_joint_angles, grasp_joint_efforts) #if the cluster isn't in the same frame as the graspable object reference frame, #transform the grasps to be in the reference frame if cluster_frame == req.target.reference_frame_id: transformed_grasp_pose = grasp_pose else: transformed_grasp_pose = change_pose_stamped_frame(self.pcgp.tf_listener, stamp_pose(grasp_pose, cluster_frame), req.target.reference_frame_id).pose if self.pcgp.pregrasp_just_outside_box: min_approach_distance = pregrasp_dist else: min_approach_distance = max(pregrasp_dist-.05, .05) grasp_list.append(Grasp(pre_grasp_posture=pre_grasp_joint_state, grasp_posture=grasp_joint_state, grasp_pose=transformed_grasp_pose, success_probability=quality, desired_approach_distance = pregrasp_dist, min_approach_distance = min_approach_distance)) #if requested, randomize the first few grasps if self.randomize_grasps: first_grasps = grasp_list[:30] random.shuffle(first_grasps) shuffled_grasp_list = first_grasps + grasp_list[30:] resp.grasps = shuffled_grasp_list else: resp.grasps = grasp_list return resp
def plan_point_cluster_grasp_callback(self, req): rospy.loginfo("planning grasps for a point cluster") resp = GraspPlanningResponse() #get the hand joint names from the param server (loaded from yaml config file) joint_names_dict = rospy.get_param('~joint_names') pregrasp_joint_angles_dict = rospy.get_param('~pregrasp_joint_angles') grasp_joint_angles_dict = rospy.get_param('~grasp_joint_angles') pregrasp_joint_efforts_dict = rospy.get_param( '~pregrasp_joint_efforts') grasp_joint_efforts_dict = rospy.get_param('~grasp_joint_efforts') if req.arm_name: arm_name = req.arm_name else: arm_name = joint_names_dict.keys()[0] rospy.logerr( "point cluster grasp planner: missing arm_name in request! Using " + arm_name) try: hand_joints = joint_names_dict[arm_name] except KeyError: arm_name = joint_names_dict.keys()[0] rospy.logerr("arm_name " + req.arm_name + " not found! Using joint names from " + arm_name) hand_joints = joint_names_dict[arm_name] pregrasp_joint_angles = pregrasp_joint_angles_dict[arm_name] grasp_joint_angles = grasp_joint_angles_dict[arm_name] pregrasp_joint_efforts = pregrasp_joint_efforts_dict[arm_name] grasp_joint_efforts = grasp_joint_efforts_dict[arm_name] #hand_joints = rospy.get_param('/hand_description/'+arm_name+'/hand_joints') rospy.loginfo("hand_joints:" + str(hand_joints)) #find the cluster bounding box and relevant frames, and transform the cluster init_start_time = time.time() if len(req.target.cluster.points) > 0: self.pcgp.init_cluster_grasper(req.target.cluster) cluster_frame = req.target.cluster.header.frame_id else: self.pcgp.init_cluster_grasper(req.target.region.cloud) cluster_frame = req.target.region.cloud.header.frame_id if len(cluster_frame) == 0: rospy.logerr("region.cloud.header.frame_id was empty!") resp.error_code.value = resp.error_code.OTHER_ERROR return resp init_end_time = time.time() #print "init time: %.3f"%(init_end_time - init_start_time) #plan grasps for the cluster (returned in the cluster frame) grasp_plan_start_time = time.time() (pregrasp_poses, grasp_poses, gripper_openings, qualities, pregrasp_dists) = self.pcgp.plan_point_cluster_grasps() grasp_plan_end_time = time.time() #print "total grasp planning time: %.3f"%(grasp_plan_end_time - grasp_plan_start_time) #add error code to service resp.error_code.value = resp.error_code.SUCCESS grasp_list = [] if pregrasp_poses == None: resp.error_code.value = resp.error_code.OTHER_ERROR return resp #fill in the list of grasps for (grasp_pose, quality, pregrasp_dist) in zip(grasp_poses, qualities, pregrasp_dists): pre_grasp_joint_state = self.create_joint_state( hand_joints, pregrasp_joint_angles, pregrasp_joint_efforts) grasp_joint_state = self.create_joint_state( hand_joints, grasp_joint_angles, grasp_joint_efforts) #if the cluster isn't in the same frame as the graspable object reference frame, #transform the grasps to be in the reference frame if cluster_frame == req.target.reference_frame_id: transformed_grasp_pose = grasp_pose else: transformed_grasp_pose = change_pose_stamped_frame( self.pcgp.tf_listener, stamp_pose(grasp_pose, cluster_frame), req.target.reference_frame_id).pose if self.pcgp.pregrasp_just_outside_box: min_approach_distance = pregrasp_dist else: min_approach_distance = max(pregrasp_dist - .05, .05) grasp_list.append( Grasp(pre_grasp_posture=pre_grasp_joint_state, grasp_posture=grasp_joint_state, grasp_pose=transformed_grasp_pose, success_probability=quality, desired_approach_distance=pregrasp_dist, min_approach_distance=min_approach_distance)) #if requested, randomize the first few grasps if self.randomize_grasps: first_grasps = grasp_list[:30] random.shuffle(first_grasps) shuffled_grasp_list = first_grasps + grasp_list[30:] resp.grasps = shuffled_grasp_list else: resp.grasps = grasp_list return resp