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
