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))