target_obj_pose.orientation.z = 0 target_obj_pose.orientation.w = 1 workspace_info = getWorkspaceOnMap() parent_obj_geometry = SRSSpatialInfo() parent_obj_geometry.pose.position.x = workspace_info.objectsInfo[0].pose.position.x parent_obj_geometry.pose.position.y = workspace_info.objectsInfo[0].pose.position.y parent_obj_geometry.pose.position.z = workspace_info.objectsInfo[0].pose.position.z parent_obj_geometry.pose.orientation.x = workspace_info.objectsInfo[0].pose.orientation.x parent_obj_geometry.pose.orientation.y = workspace_info.objectsInfo[0].pose.orientation.y parent_obj_geometry.pose.orientation.z = workspace_info.objectsInfo[0].pose.orientation.z parent_obj_geometry.pose.orientation.w = workspace_info.objectsInfo[0].pose.orientation.w parent_obj_geometry.l = workspace_info.objectsInfo[0].l parent_obj_geometry.w = workspace_info.objectsInfo[0].w parent_obj_geometry.h = workspace_info.objectsInfo[0].h #parent_obj_geometry.pose.position.x = -3.3 #parent_obj_geometry.pose.position.y = 0.5 furniture_geometry_list = list() furniture_geometry_list = workspace_info.objectsInfo #rospy.loginfo(workspace_info.objectsInfo[6]) print "Requesting reachability and grasp base pose." result = symbol_grounding_grasp_base_pose_experimental_client(target_obj_pose, parent_obj_geometry, furniture_geometry_list) print result
index_of_the_target_workspace = all_workspaces_on_map.objects.index(userdata.target_workspace_name)
#get the pose of the workspace from knowledge service
target_object_pose = all_workspaces_on_map.objectsInfo[index_of_the_target_workspace].pose
#get the houseHoldID of the workspace
object_id = all_workspaces_on_map.houseHoldId[index_of_the_target_workspace]
#rospy.loginfo ("target name: %s", userdata.target_workspace_name)
#rospy.loginfo ("target pose: %s", target_object_pose)
#rospy.loginfo ("target id: %s", object_id)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
return 'failed'
parent_obj_geometry = SRSSpatialInfo()
parent_obj_geometry.pose = target_object_pose
parent_obj_geometry.l = all_workspaces_on_map.objectsInfo[index_of_the_target_workspace].l
parent_obj_geometry.w = all_workspaces_on_map.objectsInfo[index_of_the_target_workspace].w
parent_obj_geometry.h = all_workspaces_on_map.objectsInfo[index_of_the_target_workspace].h
rospy.wait_for_service('symbol_grounding_grasp_base_pose_experimental')
symbol_grounding_grasp_base_pose_experimental = rospy.ServiceProxy('symbol_grounding_grasp_base_pose_experimental', SymbolGroundingGraspBasePoseExperimental)
try:
result = symbol_grounding_grasp_base_pose_experimental(object_pose_map.pose, parent_obj_geometry, all_workspaces_on_map.objectsInfo)
userdata.base_pose = [result.grasp_base_pose.x, result.grasp_base_pose.y, result.grasp_base_pose.theta]
return 'retry'
except rospy.ServiceException, e:
print "Service call failed: %s" %e
return 'failed'
index_of_the_target_workspace].pose
#get the houseHoldID of the workspace
object_id = all_workspaces_on_map.houseHoldId[
index_of_the_target_workspace]
#rospy.loginfo ("target name: %s", userdata.target_workspace_name)
#rospy.loginfo ("target pose: %s", target_object_pose)
#rospy.loginfo ("target id: %s", object_id)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
return 'failed'
parent_obj_geometry = SRSSpatialInfo()
parent_obj_geometry.pose = target_object_pose
parent_obj_geometry.l = all_workspaces_on_map.objectsInfo[
index_of_the_target_workspace].l
parent_obj_geometry.w = all_workspaces_on_map.objectsInfo[
index_of_the_target_workspace].w
parent_obj_geometry.h = all_workspaces_on_map.objectsInfo[
index_of_the_target_workspace].h
rospy.wait_for_service('symbol_grounding_grasp_base_pose_experimental')
symbol_grounding_grasp_base_pose_experimental = rospy.ServiceProxy(
'symbol_grounding_grasp_base_pose_experimental',
SymbolGroundingGraspBasePoseExperimental)
try:
result = symbol_grounding_grasp_base_pose_experimental(
object_pose_map.pose, parent_obj_geometry,
all_workspaces_on_map.objectsInfo)
userdata.base_pose = [
result.grasp_base_pose.x, result.grasp_base_pose.y,
parent_obj_geometry.pose.position.x = workspace_info.objectsInfo[
2].pose.position.x
parent_obj_geometry.pose.position.y = workspace_info.objectsInfo[
2].pose.position.y
parent_obj_geometry.pose.position.z = workspace_info.objectsInfo[
2].pose.position.z
parent_obj_geometry.pose.orientation.x = workspace_info.objectsInfo[
2].pose.orientation.x
parent_obj_geometry.pose.orientation.y = workspace_info.objectsInfo[
2].pose.orientation.y
parent_obj_geometry.pose.orientation.z = workspace_info.objectsInfo[
2].pose.orientation.z
parent_obj_geometry.pose.orientation.w = workspace_info.objectsInfo[
2].pose.orientation.w
parent_obj_geometry.l = workspace_info.objectsInfo[2].l
parent_obj_geometry.w = workspace_info.objectsInfo[2].w
parent_obj_geometry.h = workspace_info.objectsInfo[2].h
rospy.loginfo(parent_obj_geometry.pose)
#parent_obj_geometry.pose.position.x = 8.6
#parent_obj_geometry.pose.position.y = 2.5
#parent_obj_geometry.pose.orientation.x = 0
#parent_obj_geometry.pose.orientation.y = 0
#parent_obj_geometry.pose.orientation.z = -0.999783754349
#parent_obj_geometry.pose.orientation.w = 0.0207948293537
#parent_obj_geometry.l = 1.5
#parent_obj_geometry.w = 0.4
furniture_geometry_list = list()
furniture_geometry_list = workspace_info.objectsInfo
parent_obj_geometry.pose.orientation.z = workspace_info.objectsInfo[0].pose.orientation.z parent_obj_geometry.pose.orientation.w = workspace_info.objectsInfo[0].pose.orientation.w parent_obj_geometry.l = workspace_info.objectsInfo[0].l parent_obj_geometry.w = workspace_info.objectsInfo[0].w parent_obj_geometry.h = workspace_info.objectsInfo[0].h ''' parent_obj_geometry.pose.position.x = -3.3 parent_obj_geometry.pose.position.y = 0.5 parent_obj_geometry.pose.position.z = 1.0 parent_obj_geometry.pose.orientation.x = 0 parent_obj_geometry.pose.orientation.y = 0 parent_obj_geometry.pose.orientation.z = 0 parent_obj_geometry.pose.orientation.w = 1.0 parent_obj_geometry.l = 2.0 parent_obj_geometry.w = 1.0 parent_obj_geometry.h = 0.85 furniture_geometry_list = list() #furniture_geometry_list = workspace_info.objectsInfo #rospy.loginfo(workspace_info.objectsInfo[6]) print "Requesting reachability and grasp base pose." result = symbol_grounding_grasp_base_pose_experimental_client(target_obj_pose, parent_obj_geometry, furniture_geometry_list) print result #print math.atan(0.26 / 0.7)
