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 call_plan_point_cluster_grasp(self, cluster, arm_name, reference_frame_id, side=False): req = GraspPlanningRequest() req.target.reference_frame_id = reference_frame_id req.target.region.cloud = cluster req.arm_name = arm_name #not that cluster planner cares right now which arm try: res = self._cluster_grasp_srv(req) except rospy.ServiceException, e: rospy.logerr("error when calling plan_point_cluster_grasp: %s" % e) return 0
def call_plan_point_cluster_grasp(cluster): req = GraspPlanningRequest() req.target.cluster = cluster req.arm_name = "arm" req.collision_support_surface_name = "table" service_name = "openrave_grasp_planner" rospy.loginfo("waiting for /openrave_grasp_planner service") rospy.wait_for_service(service_name) rospy.loginfo("service found") serv = rospy.ServiceProxy(service_name, GraspPlanning) try: res = serv(req) except rospy.ServiceException, e: rospy.logerr("error when calling plan_point_cluster_grasp: %s" % e) return 0
def call_plan_point_cluster_grasp(cluster): req = GraspPlanningRequest() req.target.reference_frame_id = "/base_link" req.target.cluster = cluster req.arm_name = "right_arm" req.collision_support_surface_name = "table" service_name = "plan_point_cluster_grasp" rospy.loginfo("waiting for plan_point_cluster_grasp service") rospy.wait_for_service(service_name) rospy.loginfo("service found") serv = rospy.ServiceProxy(service_name, GraspPlanning) try: res = serv(req) except rospy.ServiceException, e: rospy.logerr("error when calling plan_point_cluster_grasp: %s" % e) return 0
def call_plan_segmented_clutter_grasps(point_cloud, box_pose, box_dims, arm_name): req = GraspPlanningRequest() req.arm_name = arm_name req.target.reference_frame_id = box_pose.header.frame_id req.target.region.cloud = point_cloud req.target.region.roi_box_pose = box_pose req.target.region.roi_box_dims = box_dims service_name = "plan_segmented_clutter_grasps" rospy.loginfo("waiting for %s" % service_name) rospy.wait_for_service(service_name) rospy.loginfo("service found") serv = rospy.ServiceProxy(service_name, GraspPlanning) try: res = serv(req) except rospy.ServiceException, e: rospy.logerr("error when calling plan_segmented_clutter_grasps: %s" % e) return 0
def get_grasps(self, model_id, model_pose): ''' Get the set of grasps for a model, given a particular model pose **Args:** **model_id:** The model id corresponding to the database being used *model_pose:* The pose of the model ''' request = GraspPlanningRequest() request.arm_name = 'right_arm' db_pose = DatabaseModelPose() db_pose.pose = model_pose db_pose.model_id = model_id db_pose.confidence = 0.5 request.target.potential_models.append(db_pose) request.target.reference_frame_id = model_pose.header.frame_id response = self.database_grasp_planner(request) rospy.loginfo('Found = ' + str(len(response.grasps)) + ' grasps') self._visualize_grasps(response.grasps, model_pose.header.frame_id) return response.grasps
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
req = TabletopCollisionMapProcessingRequest() req.detection_result = tdr req.reset_collision_models = True req.reset_attached_models = True req.reset_static_map = False req.take_static_collision_map = False req.desired_frame = 'base_link' coll_map_res = collision_map_processing_srv(req) rospy.loginfo('collision_map update is done') rospy.loginfo('there are %d graspable objects %s, collision support surface name is "%s"' % (len(coll_map_res.graspable_objects), coll_map_res.collision_object_names, coll_map_res.collision_support_surface_name)) req = GraspPlanningRequest() req.arm_name = 'left_arm' req.target = coll_map_res.graspable_objects[0] req.collision_object_name = coll_map_res.collision_object_names[0] req.collision_support_surface_name = coll_map_res.collision_support_surface_name rospy.loginfo('trying to find a good grasp for graspable object %s' % coll_map_res.collision_object_names[0]) grasping_result = grasp_planning_srv(req) if grasping_result.error_code.value != GraspPlanningErrorCode.SUCCESS: rospy.logerr('unable to find a feasable grasp, aborting') exit(1) 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