def getGraspableBody(id):
_wait_for_service(GraspitCommander.GRASPIT_NODE_NAME + 'getGraspableBody')
serviceProxy = rospy.ServiceProxy(GraspitCommander.GRASPIT_NODE_NAME + 'getGraspableBody', GetGraspableBody)
graspableBody = serviceProxy(id)
if graspableBody.result is GetGraspableBody._response_class.RESULT_SUCCESS:
return graspableBody
elif graspableBody.result is GetGraspableBody._response_class.RESULT_INVALID_ID:
raise InvalidGraspableBodyIDException(id)
def computeEnergy(energy_type, hand_id=0, body_id=0):
_wait_for_service(GraspitCommander.GRASPIT_NODE_NAME + 'computeEnergy')
serviceProxy = rospy.ServiceProxy(GraspitCommander.GRASPIT_NODE_NAME + 'computeEnergy', ComputeEnergy)
result = serviceProxy(hand_id, body_id, energy_type)
if result.result is ComputeEnergy._response_class.RESULT_SUCCESS:
return result
elif result.result is ComputeEnergy._response_class.RESULT_INVALID_HAND_ID:
raise InvalidRobotIDException(hand_id)
elif result.result is ComputeEnergy._response_class.RESULT_INVALID_BODY_ID:
raise InvalidGraspableBodyIDException(body_id)
elif result.result is ComputeEnergy._response_class.RESULT_INVALID_ENERGY_TYPE:
raise InvalidEnergyTypeException(energy_type)
def setGraspableBodyPose(id, pose):
_wait_for_service(GraspitCommander.GRASPIT_NODE_NAME + 'setGraspableBodyPose')
serviceProxy = rospy.ServiceProxy(GraspitCommander.GRASPIT_NODE_NAME + 'setGraspableBodyPose', SetGraspableBodyPose)
result = serviceProxy(id, pose)
if result.result is SetGraspableBodyPose._response_class.RESULT_SUCCESS:
return
elif result.result is SetGraspableBodyPose._response_class.RESULT_INVALID_ID:
raise InvalidGraspableBodyIDException(id)
elif result.result is SetGraspableBodyPose._response_class.RESULT_INVALID_POSE:
raise InvalidGraspableBodyPoseException()
elif result.result is SetGraspableBodyPose._response_class.RESULT_ROBOT_IN_COLLISION:
raise GraspableBodyCollisionException()