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_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 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 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 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 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
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