[unittest/python] Better rpy tests

unittest/python/rpy.py

@@ -5,6 +5,7 @@ import numpy as np
 from numpy.linalg import inv
 from random import random
 
+from eigenpy import AngleAxis
 import pinocchio as pin
 from pinocchio.utils import npToTuple
 from pinocchio.rpy import matrixToRpy, rpyToMatrix, rotate, rpyToJac, rpyToJacInv, rpyToJacDerivative
@@ -42,6 +43,86 @@ class TestRPY(TestCase):
         else:
           self.assertTrue(False)
 
+
+    def test_rpyToMatrix(self):
+        r = random()*2*pi - pi
+        p = random()*pi - pi/2
+        y = random()*2*pi - pi
+        
+        R = rpyToMatrix(r, p, y)
+        
+        Raa = AngleAxis(y, np.array([0., 0., 1.])).matrix() \
+            .dot(AngleAxis(p, np.array([0., 1., 0.])).matrix()) \
+            .dot(AngleAxis(r, np.array([1., 0., 0.])).matrix())
+        
+        self.assertApprox(R, Raa)
+        
+        v = np.array([r, p, y])
+        
+        Rv = rpyToMatrix(v)
+        
+        self.assertApprox(Rv, Raa)
+        self.assertApprox(Rv, R)
+
+
+    def test_matrixToRpy(self):
+        n = 100
+        for k in range(n):
+            quat = pin.Quaternion(np.random.rand(4,1)).normalized()
+            R = quat.toRotationMatrix()
+
+            v = matrixToRpy(R)
+            Rprime = rpyToMatrix(v)
+
+            self.assertApprox(Rprime, R)
+            self.assertTrue(-pi <= v[0] and v[0] <= pi)
+            self.assertTrue(-pi/2 <= v[1] and v[1] <= pi/2)
+            self.assertTrue(-pi <= v[2] and v[2] <= pi)
+
+        n2 = 100
+
+        # Test singular case theta = pi/2
+        Rp = np.array([[ 0.0, 0.0, 1.0],
+                        [ 0.0, 1.0, 0.0],
+                        [-1.0, 0.0, 0.0]])
+        for k in range(n2):
+            r = random()*2*pi - pi
+            y = random()*2*pi - pi
+                    
+            R = AngleAxis(y, np.array([0., 0., 1.])).matrix() \
+                .dot(Rp) \
+                .dot(AngleAxis(r, np.array([1., 0., 0.])).matrix())
+
+            v = matrixToRpy(R)
+            Rprime = rpyToMatrix(v)
+
+            self.assertApprox(Rprime, R)
+            self.assertTrue(-pi <= v[0] and v[0] <= pi)
+            self.assertTrue(-pi/2 <= v[1] and v[1] <= pi/2)
+            self.assertTrue(-pi <= v[2] and v[2] <= pi)
+
+        # Test singular case theta = -pi/2
+        Rp = np.array([[0.0, 0.0, -1.0],
+                        [0.0, 1.0,  0.0],
+                        [1.0, 0.0,  0.0]])
+        for k in range(n2):
+            r = random()*2*pi - pi
+            y = random()*2*pi - pi
+            
+            R = AngleAxis(y, np.array([0., 0., 1.])).matrix() \
+                    .dot(Rp) \
+                    .dot(AngleAxis(r, np.array([1., 0., 0.])).matrix())
+
+            v = matrixToRpy(R)
+            Rprime = rpyToMatrix(v)
+
+            self.assertApprox(Rprime, R)
+            self.assertTrue(-pi <= v[0] and v[0] <= pi)
+            self.assertTrue(-pi/2 <= v[1] and v[1] <= pi/2)
+            self.assertTrue(-pi <= v[2] and v[2] <= pi)
+
+
+
     def test_rpyToJac(self):
         # Check identity at zero
         rpy = np.zeros(3)