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