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 _state_in_world(self, s): """ transforms state from body coordinates in world coordinates .. warning:: angular rate still in body frame! """ pos_body = s[:3] vel_body = s[3:6] ang_rate = s[6:9].copy() q = s[9:13].copy() pos = self._in_world_coord(-pos_body, q) vel = self._in_world_coord(vel_body, q) rot = trans.quaternion_matrix(trans.quaternion_conjugate(q))[:3, :3] return pos, vel, ang_rate, rot, q
def _in_world_coord(self, p, q): """ q is the inverse quaternion of the rotation of the helicopter in world coordinates """ return self._in_body_coord(p, trans.quaternion_conjugate(q))