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 graspThis(self, object_name): dbmp = DatabaseModelPose() dbmp.model_id = self.ez_objects[object_name][0] dbmp.confidence = 1 dbmp.detector_name = "manual_detection" planning_req = GraspPlanning() target = GraspableObject() target.reference_frame_id = "1" target.potential_models = [dbmp] response = self.planning_srv(arm_name = self.gripper_name, target = target) 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 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