def get_current_position(self):
"""
Gives the current robot position on the environment.
Returns:
position: Array with the (x,y,z) coordinates.
"""
res, position = vrep.simxGetObjectPosition(self.clientID, self.robot_handle, -1, vrep.simx_opmode_streaming)
while(res != vrep.simx_return_ok):
res, position = vrep.simxGetObjectPosition(self.clientID, self.robot_handle, -1, vrep.simx_opmode_streaming)
return position
def getObjPos(self):
e, handle = vrep.simxGetObjectHandle(self.CLIENT_ID,
'MP_BODY_respondable',
vrep.simx_opmode_oneshot_wait)
_, pos = vrep.simxGetObjectPosition(self.CLIENT_ID, handle, -1,
vrep.simx_opmode_oneshot_wait)
return handle, pos
def get_object_position(clientID, handle, relative_to_handle=-1):
'''Return the object position in reference to the relative handle.'''
response, position = vrep.simxGetObjectPosition(clientID,
handle,
relative_to_handle, vrep.simx_opmode_blocking)
if response != 0:
print("Error: Cannot query position for handle {} with reference to {}".
format(handle, relative_to_handle))
return position
def get_object_position(self, handle, relative_to_handle=-1):
'''Return the object position in reference to the relative handle.'''
response, position = vrep.simxGetObjectPosition(
self.client_id, handle, relative_to_handle,
vrep.simx_opmode_blocking)
if response != vrep.simx_return_ok:
print("Error: Cannot query position for handle {} "
"with reference to {}".format(handle, relative_to_handle))
return np.array(position)