Commit a448f174 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

[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)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment