Commit a448f174 by Gabriele Buondonno

### [unittest/python] Better rpy tests

parent 12132aac
Pipeline #12431 passed with stage
in 117 minutes and 24 seconds
 ... ... @@ -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) ... ...
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!