omimo · onyalcin · Mar 7, 2019 · Aug 22, 2020
diff --git a/pymo/preprocessing.py b/pymo/preprocessing.py
@@ -24,12 +24,15 @@ def fit(self, X, y=None):
 
     def transform(self, X, y=None):
         if self.param_type == 'euler':
+            print('euler')
             return X
         elif self.param_type == 'expmap':
             return self._to_expmap(X)
         elif self.param_type == 'quat':
-            return X
+            print('Quats')
+            return self._to_pos(X)
         elif self.param_type == 'position':
+            print('pos')
             return self._to_pos(X)
         else:
             raise UnsupportedParamError('Unsupported param: %s. Valid param types are: euler, quat, expmap, position' % self.param_type)
@@ -110,10 +113,17 @@ def _to_pos(self, X):
                 #euler_values = [[0,0,0] for f in rc.iterrows()] #for deugging
                 #pos_values = [[0,0,0] for f in pc.iterrows()] #for deugging
 
-                # Convert the eulers to rotation matrices
-                ############################ input rotation order as Rotation class's argument #########################
-                rotmats = np.asarray([Rotation([f[0], f[1], f[2]], 'euler', rotation_order, from_deg=True).rotmat for f in euler_values])
-                ########################################################################################################
+
+                # Convert the eulers or quats to rotation matrices
+                if self.param_type == 'euler':
+                    # Convert the eulers to rotation matrices
+                    rotmats = np.asarray([Rotation([f[0], f[1], f[2]], 'euler', rotation_order, from_deg=True).rotmat for f in euler_values])
+                elif self.param_type == 'quat':
+                    # Rotation order for quats currently not supported
+                    rotmats = np.asarray([Rotation([f[0], f[1], f[2]], 'quat', rotation_order, from_deg=False).rotmat for f in euler_values])
+                else:
+                    raise 'Type not supported'
+
                 tree_data[joint]=[
                                     [], # to store the rotation matrix
                                     []  # to store the calculated position

diff --git a/pymo/rotation_tools.py b/pymo/rotation_tools.py
@@ -25,6 +25,8 @@ def __init__(self,rot, param_type, rotation_order, **params):
             self._from_euler(rot[0],rot[1],rot[2], params)
         elif param_type == 'expmap':
             self._from_expmap(rot[0], rot[1], rot[2], params)
+        elif param_type == 'quat':
+            self._from_quat(rot[0], rot[1], rot[2], params)
 
     def _from_euler(self, alpha, beta, gamma, params):
         '''Expecting degress'''
@@ -90,7 +92,31 @@ def _from_expmap(self, alpha, beta, gamma, params):
             [2*x*z*s**2-2*y*c*s, 2*y*z*s**2+2*x*c*s , 2*(z**2-1)*s**2+1]
         ])
 
-
+    def _from_quat(self, alpha, beta, gamma, params):
+        if params['from_deg']==True:
+            alpha = deg2rad(alpha)
+            beta = deg2rad(beta)
+            gamma = deg2rad(gamma)
+
+        axis = [alpha, beta, gamma]
+        theta = np.linalg.norm(axis)
+
+        if not np.isclose(np.linalg.norm(axis), 1.):
+            axis = axis/np.linalg.norm(axis)
+
+        sinThetaOver2 = np.sin(theta * 0.5)
+
+        q1 = sinThetaOver2 * axis[0]
+        q2 = sinThetaOver2 * axis[1]
+        q3 = sinThetaOver2 * axis[2]
+        q4 = np.cos(theta * 0.5)
+
+        self.rotmat = np.asarray([
+            [1-2*q2*q2-2*q3*q3,  2*(q1*q2-q3*q4),    2*(q1*q3+q2*q4)],
+            [2*(q1*q2+q3*q4),    1-2*q1*q1-2*q3*q3,  2*(q2*q3-q1*q4)],
+            [2*(q1*q3-q2*q4),    2*(q1*q4+q2*q3),    1-2*q1*q1-2*q2*q2]
+        ])	
+
 
     def get_euler_axis(self):
         R = self.rotmat