Ejemplo n.º 1
0
 def get_orientations_euler(self, axes="sxyz"):
     if hasattr(self, "_poses_se3"):
         return np.array(
             [tr.euler_from_matrix(p, axes=axes) for p in self._poses_se3])
     elif hasattr(self, "_orientations_quat_wxyz"):
         return np.array([
             tr.euler_from_quaternion(q, axes=axes)
             for q in self._orientations_quat_wxyz
         ])
Ejemplo n.º 2
0
 def orientations_euler(self):
     if not hasattr(self, "_orientations_euler"):
         if hasattr(self, "_poses_se3"):
             self._orientations_euler \
                 = np.array([tr.euler_from_matrix(p, axes="sxyz") for p in self._poses_se3])
         elif hasattr(self, "_orientations_quat_wxyz"):
             self._orientations_euler \
                 = np.array([tr.euler_from_quaternion(q, axes="sxyz")
                             for q in self._orientations_quat_wxyz])
     return self._orientations_euler
Ejemplo n.º 3
0
 def orientations_euler(self):
     if not hasattr(self, "_orientations_euler"):
         if hasattr(self, "_poses_se3"):
             self._orientations_euler \
                 = np.array([tr.euler_from_matrix(p, axes="sxyz") for p in self._poses_se3])
         elif hasattr(self, "_orientations_quat_wxyz"):
             self._orientations_euler \
                 = np.array([tr.euler_from_quaternion(q, axes="sxyz")
                             for q in self._orientations_quat_wxyz])
     return self._orientations_euler
Ejemplo n.º 4
0
 def q_to_A(q):
     A = tr.euler_from_quaternion(q)
     A = np.array(A) * 180.0/np.pi
     return A