def _get_observation_state(self, compute_acelleration = True):
_, position = vrep.simxGetObjectPosition(self.clientID, self.quad_handler, -1, vrep.simx_opmode_oneshot_wait)
_, orientation = vrep.simxGetObjectOrientation(self.clientID, self.quad_handler, -1, vrep.simx_opmode_oneshot_wait)
_, lin_vel, ang_vel = vrep.simxGetObjectVelocity(self.clientID, self.quad_handler, vrep.simx_opmode_oneshot_wait)
_, quaternion = vrep.simxGetObjectQuaternion(self.clientID, self.quad_handler, -1, vrep.simx_opmode_oneshot_wait)
position = np.asarray(position, dtype=np.float32)
orientation = np.asarray(orientation, dtype=np.float32)
lin_vel, ang_vel = np.asarray(lin_vel, dtype=np.float32), np.asarray(ang_vel, dtype=np.float32)
quaternion = np.asarray(quaternion, dtype=np.float32)
if compute_acelleration == True: lin_acel, ang_acel = self.compute_aceleration(lin_vel, ang_vel)
else: lin_acel, ang_acel = np.zeros(3, dtype=np.float32), np.zeros(3, dtype=np.float32)
position = position - self.targetpos
rotation_matrix = GetFlatRotationMatrix(orientation)
observation = OrderedDict(
position=position,
orientation=orientation,
lin_vel=lin_vel,
ang_vel=ang_vel,
rotation_matrix=rotation_matrix,
lin_acel=lin_acel,
ang_acel=ang_acel,
quaternion=quaternion
)
self.last_observation = observation
return observation
def _get_observation_state(self, compute_acelleration = True):
_, position = vrep.simxGetObjectPosition(self.clientID, self.quad_handler, -1, vrep.simx_opmode_oneshot_wait)
_, orientation = vrep.simxGetObjectOrientation(self.clientID, self.quad_handler, -1, vrep.simx_opmode_oneshot_wait)
_, lin_vel, ang_vel = vrep.simxGetObjectVelocity(self.clientID, self.quad_handler, vrep.simx_opmode_oneshot_wait)
position = np.asarray(position, dtype=np.float32)
orientation = np.asarray(orientation, dtype=np.float32)
lin_vel, ang_vel = np.asarray(lin_vel, dtype=np.float32), np.asarray(ang_vel, dtype=np.float32)
if compute_acelleration == True: lin_acel, ang_acel = self.compute_aceleration(lin_vel, ang_vel)
else: lin_acel, ang_acel = np.zeros(3, dtype=np.float32), np.zeros(3, dtype=np.float32)
position = position - self.targetpos
return position, orientation, lin_vel, ang_vel, lin_acel, ang_acel
def _get_observation_state(self):
_, position = vrep.simxGetObjectPosition(self.clientID,
self.quad_handler, -1,
vrep.simx_opmode_oneshot_wait)
_, orientation = vrep.simxGetObjectOrientation(
self.clientID, self.quad_handler, -1,
vrep.simx_opmode_oneshot_wait)
_, lin_vel, ang_vel = vrep.simxGetObjectVelocity(
self.clientID, self.quad_handler, vrep.simx_opmode_oneshot_wait)
# Flat and join states!
RotMat = GetFlatRotationMatrix(orientation)
""" Test relative position """
position = position - self.targetpos
observation = OrderedDict(position=position,
orientation=orientation,
lin_vel=lin_vel,
ang_vel=ang_vel,
rotation_matrix=RotMat)
self.last_observation = observation
return observation