Ejemplo n.º 1
0
 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:]
Ejemplo n.º 2
0
    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
Ejemplo n.º 3
0
 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))