def _in_body_coord(self, p, q):
"""
q is the inverse quaternion of the rotation of the helicopter in world coordinates
"""
q_pos = np.zeros((4))
q_pos[1:] = p
q_p = trans.quaternion_multiply(trans.quaternion_multiply(q, q_pos),
trans.quaternion_conjugate(q))
return q_p[1:]
def step(self, s, a):
a = self.actions[a]
# make sure the actions are not beyond their limits
a = np.maximum(self._action_bounds[:, 0], np.minimum(a,
self._action_bounds[:, 1]))
pos, vel, ang_rate, ori_bases, q = self._state_in_world(s)
t = s[-1]
gust_noise = s[13:19]
gust_noise = (self.gust_memory * gust_noise
+ (1. - self.gust_memory) * np.random.randn(6) * self.noise_level * self.noise_std)
# update noise which simulates gusts
for i in range(10):
# Euler integration
# position
pos += self.dt * vel
# compute acceleration on the helicopter
vel_body = self._in_world_coord(vel, q)
wind_body = self._in_world_coord(self.wind, q)
wind_body[-1] = 0. # the java implementation
# has it this way
acc_body = -self.drag_vel_body * (vel_body + wind_body)
acc_body[-1] += self.u_coeffs[-1] * a[-1]
acc_body[1] += self.tail_rotor_side_thrust
acc_body += gust_noise[:3]
acc = self._in_body_coord(acc_body, q)
acc[-1] += 9.81 # gravity
# velocity
vel += self.dt * acc
# orientation
tmp = self.dt * ang_rate
qdt = trans.quaternion_about_axis(np.linalg.norm(tmp), tmp)
q = trans.quaternion_multiply(q, qdt)
#assert np.allclose(1., np.sum(q**2))
# angular accelerations
ang_acc = -ang_rate * self.drag_ang_rate + self.u_coeffs[:3] * a[:3]
ang_acc += gust_noise[3:]
ang_rate += self.dt * ang_acc
st = np.zeros_like(s)
st[:3] = -self._in_body_coord(pos, q)
st[3:6] = self._in_body_coord(vel, q)
st[6:9] = ang_rate
st[9:13] = q
st[13:19] = gust_noise
st[-1] = t + 1
return (self._get_reward(s=st), st, self.isTerminal(st))
