def setRobotDesiredDOF(dofs, dof_velocities, id=0):
_wait_for_service(GraspitCommander.GRASPIT_NODE_NAME + 'setRobotDesiredDOF')
serviceProxy = rospy.ServiceProxy(GraspitCommander.GRASPIT_NODE_NAME + 'setRobotDesiredDOF', SetRobotDesiredDOF)
result = serviceProxy(id, dofs, dof_velocities)
if result.result is SetRobotDesiredDOF._response_class.RESULT_SUCCESS:
return
elif result.result is SetRobotDesiredDOF._response_class.RESULT_INVALID_ID:
raise InvalidRobotIDException(id)
elif result.result is SetRobotDesiredDOF._response_class.RESULT_DOF_OUT_OF_RANGE:
raise InvalidRobotDOFOutOfRangeException()
elif result.result is SetRobotDesiredDOF._response_class.RESULT_DOF_COUNT_MISMATCH:
raise InvalidRobotDOFCountMismatchException()
def moveDOFToContacts(dofs, desired_steps, stop_at_contact, id=0):
_wait_for_service(GraspitCommander.GRASPIT_NODE_NAME + 'moveDOFToContacts')
serviceProxy = rospy.ServiceProxy(GraspitCommander.GRASPIT_NODE_NAME + 'moveDOFToContacts', MoveDOFToContacts)
result = serviceProxy(id, dofs, desired_steps, stop_at_contact)
if result.result is MoveDOFToContacts._response_class.RESULT_SUCCESS:
return
elif result.result is MoveDOFToContacts._response_class.RESULT_INVALID_ID:
raise InvalidRobotIDException(id)
elif result.result is MoveDOFToContacts._response_class.RESULT_DOF_OUT_OF_RANGE:
raise InvalidRobotDOFOutOfRangeException()
elif result.result is MoveDOFToContacts._response_class.RESULT_DOF_COUNT_MISMATCH:
raise InvalidRobotDOFCountMismatchException()
elif result.result is MoveDOFToContacts._response_class.RESULT_DYNAMICS_MODE_ENABLED:
raise InvalidDynamicsModeException()
def forceRobotDof(dofs, id=0):
_wait_for_service(GraspitCommander.GRASPIT_NODE_NAME + 'forceRobotDof')
serviceProxy = rospy.ServiceProxy(GraspitCommander.GRASPIT_NODE_NAME + 'forceRobotDof', ForceRobotDOF)
result = serviceProxy(id, dofs)
if result.result is ForceRobotDOF._response_class.RESULT_SUCCESS:
return
elif result.result is ForceRobotDOF._response_class.RESULT_INVALID_ID:
raise InvalidRobotIDException(id)
elif result.result is ForceRobotDOF._response_class.RESULT_DOF_OUT_OF_RANGE:
raise InvalidRobotDOFOutOfRangeException()
elif result.result is ForceRobotDOF._response_class.RESULT_DOF_COUNT_MISMATCH:
raise InvalidRobotDOFCountMismatchException()
elif result.result is ForceRobotDOF._response_class.RESULT_DYNAMICS_MODE_ENABLED:
raise InvalidDynamicsModeException()