def create_spaced_downward_grasps(xyz, cloud_in, num_grasps=20, num_axis_attempts = 5): x_angle = 0 y_angle = np.pi / 2 grasps = [] orig_x, orig_y, orig_z = xyz for z_angle in np.linspace(-np.pi, np.pi, num_grasps): #for x in np.linspace(orig_x - 0.01, orig_x + 0.01, num_axis_attempts): #for y in np.linspace(orig_y - 0.01, orig_y + 0.01, num_axis_attempts): #for z in np.linspace(orig_z - 0.01, orig_z + 0.01, num_axis_attempts): x = orig_x y = orig_y z = orig_z g = Grasp() p = Pose() p.position.x = x p.position.y = y p.position.z = z + 0.17 q = transformations.quaternion_from_euler(x_angle, y_angle, z_angle) p.orientation.x = q[0] p.orientation.y = q[1] p.orientation.z = q[2] p.orientation.w = q[3] g.grasp_pose = p grasps.append(g) return grasps
def refine_and_grasp(self, name, arms_to_try, table_frame_x_offset=0.0, table_frame_y_offset=0.0, desired_grasp_pose=None): # table_frame_y_offset is positive if the robot moved to its right (meaning the table y position is greater in the robot's base frame) object_to_grab = self.refine_and_and_get_graspable_object( name, table_frame_x_offset, table_frame_y_offset) if object_to_grab == None: rospy.logwarn( 'no object with name was found in broad detection list: ' + str(name)) return (0, -1, None) for whicharm in arms_to_try: self.pick_and_place_manager.check_preempted() if desired_grasp_pose is not None: desired_grasp = Grasp() desired_grasp.grasp_pose = desired_grasp_pose desired_grasps = [desired_grasp] else: desired_grasps = None grasplogger = EventLoggerClient('grasp') (result, arm_used, grasp_pose ) = self.pick_and_place_manager.grasp_object_and_check_success( object_to_grab, whicharm, desired_grasps) if result == 'succeeded': grasplogger.stops() rospy.loginfo('BTO: grasp success.') return (whicharm, result, grasp_pose) grasplogger.stopf() rospy.logwarn('BTO: grasp failed') return (whicharm, result, grasp_pose)
def place_click(self): if (self.pickup_result): grasp = self.pickup_result.grasp self.pickupservice.pre_grasp_exec(grasp, 10.0) else: self.pickupservice.grasp_release_exec(10.0) initial_pose = PoseStamped() initial_pose.header.stamp = rospy.get_rostime() initial_pose.header.frame_id = "/world" #"/fixed" #fake hand to world pose initial_pose.pose.position.x = 0.4 #self.box_pose.pose.position.x+0.0 initial_pose.pose.position.y = 0.091 # self.box_pose.pose.position.y-0.2 initial_pose.pose.position.z = 1.25 #self.box_pose.pose.position.z+0.05 q = transformations.quaternion_about_axis(-0.05, (0, 0, 1)) initial_pose.pose.orientation.x = 0.41 #self.box_pose.pose.orientation.x#q[0] initial_pose.pose.orientation.y = 0.695 #self.box_pose.pose.orientation.y#q[1] initial_pose.pose.orientation.z = 0.52 #self.box_pose.pose.orientation.z#q[2] initial_pose.pose.orientation.w = 0.284 #self.box_pose.pose.orientation.w#q[3] executed_grasp = Grasp() executed_grasp.grasp_pose = copy.deepcopy(initial_pose.pose) #fake obj world origin pose initial_pose.pose.position.x = 0.43 initial_pose.pose.position.y = 0.149 initial_pose.pose.position.z = 1.088 initial_pose.pose.orientation.x = 0.0 #self.box_pose.pose.orientation.x#q[0] initial_pose.pose.orientation.y = 0.0 #self.box_pose.pose.orientation.y#q[1] initial_pose.pose.orientation.z = 0.567 #self.box_pose.pose.orientation.z#q[2] initial_pose.pose.orientation.w = 0.824 #self.box_pose.pose.orientation.w#q[3] #fake graspable object graspable_object = GraspableObject() graspable_object.reference_frame_id = "/world" mypotentialmodels = DatabaseModelPose() mypotentialmodels.model_id = 18744 mypotentialmodels.confidence = 1.0 mypotentialmodels.pose = copy.deepcopy(initial_pose) graspable_object.potential_models.append(mypotentialmodels) #fake obj world destination pose initial_pose.pose.position.x = 0.43 initial_pose.pose.position.y = 0.0 initial_pose.pose.position.z = 1.088 initial_pose.pose.orientation.x = 0.0 #self.box_pose.pose.orientation.x#q[0] initial_pose.pose.orientation.y = 0.0 #self.box_pose.pose.orientation.y#q[1] initial_pose.pose.orientation.z = 0.567 #self.box_pose.pose.orientation.z#q[2] initial_pose.pose.orientation.w = 0.824 #self.box_pose.pose.orientation.w#q[3] list_of_poses = self.compute_list_of_poses(initial_pose, graspable_object, executed_grasp)
def build_simple_pickup_goal(grasp_joint_state, pre_grasp_joint_state, fingers_config): # Adjust finger config un param server rospy.set_param('/estirabot/skills/grasp/fingers_configuration', fingers_config) g = Grasp() g.grasp_posture = grasp_joint_state g.pre_grasp_posture = pre_grasp_joint_state r = PickupGoal() r.desired_grasps.append(g) return r
def grasp_release_exec(self,timeout_sec=10.0): hand_posture_goal_ = GraspHandPostureExecutionGoal() hand_posture_goal_.grasp = Grasp() hand_posture_goal_.grasp.pre_grasp_posture.name= [ "FFJ0", "FFJ3", "FFJ4", "LFJ0", "LFJ3", "LFJ4", "LFJ5", "MFJ0", "MFJ3", "MFJ4", "RFJ0", "RFJ3", "RFJ4", "THJ1", "THJ2", "THJ3", "THJ4", "THJ5", "WRJ1", "WRJ2"] hand_posture_goal_.grasp.pre_grasp_posture.position = [0]*(18) # do release action hand_posture_goal_.goal = GraspHandPostureExecutionGoal.RELEASE # call hand_posture_exec action lib rospy.loginfo("Trying to release...") return self.hand_posture_exec(hand_posture_goal_,timeout_sec)
def plan_point_cluster_grasps(self, target, arm_name): error_code = GraspPlanningErrorCode() grasps = [] #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 not arm_name: 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 "+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(target.cluster.points) > 0: self.pcgp.init_cluster_grasper(target.cluster) cluster_frame = target.cluster.header.frame_id else: self.pcgp.init_cluster_grasper(target.region.cloud) cluster_frame = target.region.cloud.header.frame_id if len(cluster_frame) == 0: rospy.logerr("region.cloud.header.frame_id was empty!") error_code.value = error_code.OTHER_ERROR return (grasps, error_code) 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 error_code.value = error_code.SUCCESS grasp_list = [] if pregrasp_poses == None: error_code.value = error_code.OTHER_ERROR return (grasps, error_code) #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 == 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), 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:] grasps = shuffled_grasp_list else: grasps = grasp_list return (grasps, error_code)
def generate_grasps(box, num_grasps=4, desired_approach_distance = 0.10, min_approach_distance = 0.05, effort = 50): """ Generates grasps Parameters: box: bounding box to genereate grasps for num_grasps: number of grasps to generate, spaced equally around the box desired_approach_distance: how far the pre-grasp should ideally be away from the grasp min_approach_distance: how much distance between pre-grasp and grasp must actually be feasible for the grasp not to be rejected Return: a list of grasps """ rospy.loginfo("Generating grasps") grasps = [] for i in range(num_grasps): g = Grasp() # align z-axis rotation to box euler = transformations.euler_from_quaternion([ box.pose.pose.orientation.x, box.pose.pose.orientation.y, box.pose.pose.orientation.z, box.pose.pose.orientation.w]) rot_ang = euler[2] # apply transformations based on rotations rot_ang += i * (2 * math.pi) / num_grasps t1 = transformations.translation_matrix([ box.pose.pose.position.x, box.pose.pose.position.y, box.pose.pose.position.z + \ box.box_dims.z * 0.05]) r1 = transformations.rotation_matrix(rot_ang, [0, 0, 1]) box_width = max(box.box_dims.x, box.box_dims.y) t2 = transformations.translation_matrix([-(box_width/2 + 0.12), 0, 0]) mat = transformations.concatenate_matrices(t1, r1, t2) translation = transformations.translation_from_matrix(mat) g.grasp_pose.position.x = translation[0] g.grasp_pose.position.y = translation[1] g.grasp_pose.position.z = translation[2] q = transformations.quaternion_from_matrix(mat) g.grasp_pose.orientation.x = q[0] g.grasp_pose.orientation.y = q[1] g.grasp_pose.orientation.z = q[2] g.grasp_pose.orientation.w = q[3] g.desired_approach_distance = desired_approach_distance g.min_approach_distance = min_approach_distance pre_grasp_posture = JointState() pre_grasp_posture.header.stamp = rospy.Time.now() pre_grasp_posture.header.frame_id = 'base_link' pre_grasp_posture.name = ['r_wrist_flex_link'] pre_grasp_posture.position = [0.8] pre_grasp_posture.effort = [effort] g.pre_grasp_posture = pre_grasp_posture grasp_posture = JointState() grasp_posture.header.stamp = rospy.Time.now() grasp_posture.header.frame_id = 'base_link' grasp_posture.name = ['r_wrist_flex_link'] grasp_posture.position = [0.45] grasp_posture.effort = [effort] g.grasp_posture = grasp_posture grasps.append(g) np.random.shuffle(grasps) return 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
def go_to_mechanism_and_grasp_(self, suitcase): # compute the full trajectory semi_circle = self.compute_semi_circle_traj_(suitcase) # compute the pregrasp and grasp grasp = Grasp() grasp_pose_ = PoseStamped() # the grasp is the first item of the semi circle grasp_pose_ = semi_circle[0] # 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 # TODO: use the suitcase axis to approach from the perpendicular pre_grasp_pose_.pose.position.x = pre_grasp_pose_.pose.position.x - self.APPROACH_DISTANCE # TODO: find better postures grasp.pre_grasp_posture.name = [ "FFJ0", "FFJ3", "FFJ4", "LFJ0", "LFJ3", "LFJ4", "LFJ5", "MFJ0", "MFJ3", "MFJ4", "RFJ0", "RFJ3", "RFJ4", "THJ1", "THJ2", "THJ3", "THJ4", "THJ5", "WRJ1", "WRJ2", ] grasp.pre_grasp_posture.position = [0.0] * 18 grasp.pre_grasp_posture.position[grasp.pre_grasp_posture.name.index("THJ4")] = 58.0 grasp.pre_grasp_posture.position[grasp.pre_grasp_posture.name.index("THJ5")] = -50.0 grasp.grasp_posture.name = [ "FFJ0", "FFJ3", "FFJ4", "LFJ0", "LFJ3", "LFJ4", "LFJ5", "MFJ0", "MFJ3", "MFJ4", "RFJ0", "RFJ3", "RFJ4", "THJ1", "THJ2", "THJ3", "THJ4", "THJ5", "WRJ1", "WRJ2", ] grasp.grasp_posture.position = [0.0] * 18 grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ1")] = 57.0 grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ2")] = 30.0 grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ3")] = -15.0 grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ4")] = 58.0 grasp.grasp_posture.position[grasp.grasp_posture.name.index("THJ5")] = 40.0 grasp.grasp_pose = grasp_pose_.pose # 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 motion_plan_res = GetMotionPlanResponse() interpolated_motion_plan_res = self.execution.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 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.execution.plan.plan_arm_motion("right_arm", "jointspace", pre_grasp_pose_) # execution part if motion_plan_res.error_code.val == motion_plan_res.error_code.SUCCESS: # put hand in pre-grasp posture if self.execution.pre_grasp_exec(grasp) < 0: rospy.logerr("Failed to go in pregrasp.") sys.exit() # go there # filter the trajectory filtered_traj = self.execution.filter_traj_(motion_plan_res) self.execution.display_traj_(filtered_traj) # reach pregrasp pose if self.execution.send_traj_(filtered_traj) < 0: time.sleep(20) # TODO use actionlib here # approach if self.execution.send_traj_(interpolated_motion_plan_res.trajectory.joint_trajectory) < 0: rospy.logerr("Failed to approach.") sys.exit() time.sleep(20) # TODO use actionlib here # grasp if self.execution.grasp_exec(grasp) < 0: rospy.logerr("Failed to grasp.") sys.exit() time.sleep(20) # TODO use actionlib here else: # Failed, don't return the computed traj return None # return the full traj return semi_circle
def pick_object(self, object_to_pick, collision_support_surface_name): self.server.publish_feedback( PickObjectFeedback(1, "Finding object bounding box")) self.object_name = object_to_pick.model_description.name self.object = object_to_pick graspable_object = self.object.graspable_object # draw a bounding box around the selected object (box_pose, box_dims) = self.call_find_cluster_bounding_box( graspable_object.cluster) if box_pose == None: return (1, None, None) box_mat = pose_to_mat(box_pose.pose) rospy.logerr("box_pose %f %f %f q: %f %f %f %f", box_pose.pose.position.x, box_pose.pose.position.y, box_pose.pose.position.z, box_pose.pose.orientation.x, box_pose.pose.orientation.y, box_pose.pose.orientation.z, box_pose.pose.orientation.w) box_ranges = [[-box_dims.x / 2, -box_dims.y / 2, -box_dims.z / 2], [box_dims.x / 2, box_dims.y / 2, box_dims.z / 2]] self.box_pose = box_pose self.draw_functions.draw_rviz_box(box_mat, box_ranges, '/world', ns='bounding box', color=[0, 0, 1], opaque=0.25, duration=60) if self.server.is_preempt_requested(): self.server.publish_feedback( PickObjectFeedback(50, "Pick action cancelled")) self.server.set_preempted() return (1, None, None) self.server.publish_feedback( PickObjectFeedback(10, "Calling pick up service")) # call the pickup service res = self.pickup(graspable_object, self.object.graspable_object_name, self.object_name, collision_support_surface_name) initial_pose = PoseStamped() initial_pose.header.stamp = rospy.get_rostime() initial_pose.header.frame_id = "/world" initial_pose.pose.position.x = self.box_pose.pose.position.x initial_pose.pose.position.y = self.box_pose.pose.position.y initial_pose.pose.position.z = self.box_pose.pose.position.z - box_dims.z / 2 # graspable object is from bottom but bounding box is at center ! initial_pose.pose.orientation.x = self.box_pose.pose.orientation.x initial_pose.pose.orientation.y = self.box_pose.pose.orientation.y initial_pose.pose.orientation.z = self.box_pose.pose.orientation.z initial_pose.pose.orientation.w = self.box_pose.pose.orientation.w if res == 0: #correctly picked up executed_grasp = self.pickup_result.grasp else: executed_grasp = Grasp() return (res, initial_pose, executed_grasp)