def call_db():
rospy.wait_for_service('/objects_database_node/database_grasp_planning')
try:
grasp = rospy.ServiceProxy(
'/objects_database_node/database_grasp_planning', GraspPlanning)
req = GraspPlanningRequest()
req.arm_name = 'right_arm'
req.target.reference_frame_id = 'head_mount_kinect_rgb_optical_frame'
model = DatabaseModelPose()
model.model_id = 18699
pose = PoseStamped()
pose.header.frame_id = 'head_mount_kinect_rgb_optical_frame'
pose.pose.position.x = 0.320944428444
pose.pose.position.y = 0.0485505461693
pose.pose.position.z = 0.959076166153
pose.pose.orientation.x = 0.992253601551
pose.pose.orientation.y = 0.000377745833248
pose.pose.orientation.z = 0.0030180788599
pose.pose.orientation.w = 0.124191477895
model.pose = pose
req.target.potential_models.append(model)
resp = grasp(req)
for i in range(resp.grasps.__len__()):
print "resp: ", resp.grasps[i].grasp_pose
return resp
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def get_grasps(self, arm_name, model_id):
target = GraspableObject()
modelpose = DatabaseModelPose()
modelpose.model_id = model_id
modelpose.pose.pose.orientation.w = 1
#target.model_pose = modelpose
target.potential_models = [modelpose]
return ht.database_grasp_planning_srv(arm_name=arm_name, target=target)
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 segment_objects(self, req):
segmentation_result = self.tabletop_segmentation_srv()
if segmentation_result.result != TabletopSegmentationResponse.SUCCESS or not segmentation_result.clusters:
rospy.logerr('TabletopSegmentation did not find any clusters')
return None
rospy.loginfo('TabletopSegmentation found %d clusters' %
len(segmentation_result.clusters))
tdr = TabletopDetectionResult()
tdr.table = segmentation_result.table
tdr.clusters = segmentation_result.clusters
tdr.result = segmentation_result.result
req = GetClusterLabelsRequest()
req.clusters = segmentation_result.clusters
res = self.classify_srv(req)
labels = res.labels
index_to_label = {
0: 'Lego',
1: 'Box',
2: 'Toy',
3: 'Ball',
4: 'Cup',
}
label_to_index = {
'Lego': 0,
'Box': 1,
'Toy': 2,
'Ball': 3,
'Cup': 4,
}
for idx, cluster in enumerate(segmentation_result.clusters):
bbox_response = self.find_bounding_box_srv(cluster)
dmp = DatabaseModelPose()
dmp.model_id = label_to_index[labels[idx]]
dmp.pose = bbox_response.pose
dmpl = DatabaseModelPoseList()
dmpl.model_list = [dmp]
tdr.models.append(dmpl)
res = TabletopDetectionResponse()
res.detection = tdr
rospy.loginfo(res)
return res
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 segment_objects(self, req):
segmentation_result = self.tabletop_segmentation_srv()
if segmentation_result.result != TabletopSegmentationResponse.SUCCESS or not segmentation_result.clusters:
rospy.logerr('TabletopSegmentation did not find any clusters')
return None
rospy.loginfo('TabletopSegmentation found %d clusters' % len(segmentation_result.clusters))
tdr = TabletopDetectionResult()
tdr.table = segmentation_result.table
tdr.clusters = segmentation_result.clusters
tdr.result = segmentation_result.result
req = GetClusterLabelsRequest()
req.clusters = segmentation_result.clusters
res = self.classify_srv(req)
labels = res.labels
index_to_label = {
0: 'Lego',
1: 'Box',
2: 'Toy',
3: 'Ball',
4: 'Cup',
}
label_to_index = {
'Lego': 0,
'Box': 1,
'Toy': 2,
'Ball': 3,
'Cup': 4,
}
for idx,cluster in enumerate(segmentation_result.clusters):
bbox_response = self.find_bounding_box_srv(cluster)
dmp = DatabaseModelPose()
dmp.model_id = label_to_index[labels[idx]]
dmp.pose = bbox_response.pose
dmpl = DatabaseModelPoseList()
dmpl.model_list = [dmp]
tdr.models.append(dmpl)
res = TabletopDetectionResponse()
res.detection = tdr
rospy.loginfo(res)
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 get_grasps(self,arm_name,model_id):
target = GraspableObject()
modelpose = DatabaseModelPose()
modelpose.model_id = model_id
target.model_pose = modelpose
return ht.database_grasp_planning_srv(arm_name=arm_name,target=target)
