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