def get_current_orientation(self):
"""
Gives the current robot orientation on the environment.
Returns:
orientation: Array with the euler angles (alpha, beta and gamma).
"""
res, orientation = vrep.simxGetObjectOrientation(self.clientID, self.robot_handle, -1, vrep.simx_opmode_streaming)
while(res != vrep.simx_return_ok):
res, orientation = vrep.simxGetObjectOrientation(self.clientID, self.robot_handle, -1, vrep.simx_opmode_streaming)
return orientation
def getEuler(self):
e, handle = vrep.simxGetObjectHandle(self.CLIENT_ID,
'MP_BODY_respondable',
vrep.simx_opmode_oneshot_wait)
print handle
_, euler = vrep.simxGetObjectOrientation(self.CLIENT_ID, handle, -1,
vrep.simx_opmode_oneshot_wait)
print euler
def get_object_orientation(clientID, handle, reference_handle=-1):
'''Return the object orientation in reference to the relative handle.'''
response, orientation = vrep.simxGetObjectOrientation(clientID,
handle,
reference_handle, vrep.simx_opmode_blocking)
if response != 0:
print("Error: Cannot query position for handle {} with reference to {}".
format(handle, reference_handle))
return orientation
def get_object_orientation(self, handle, reference_handle=-1):
'''Return the object orientation in reference to the relative handle.'''
response, orientation = vrep.simxGetObjectOrientation(
self.client_id, handle, reference_handle,
vrep.simx_opmode_blocking)
if response != 0:
logging.warning(
"Error: Cannot query position for handle {} with reference to {}"
.format(handle, reference_handle))
return np.array(orientation)
