rpy.py
Go to the documentation of this file.
1 import unittest
2 from math import pi
3 
4 import numpy as np
5 from numpy.linalg import inv
6 from random import random
7 
8 from eigenpy import AngleAxis
9 import pinocchio as pin
10 from pinocchio.utils import npToTuple
11 from pinocchio.rpy import matrixToRpy, rpyToMatrix, rotate, computeRpyJacobian, computeRpyJacobianInverse, computeRpyJacobianTimeDerivative
12 
13 from test_case import PinocchioTestCase as TestCase
14 
15 
16 class TestRPY(TestCase):
17  def test_npToTuple(self):
18  m = np.array(list(range(9)))
19  self.assertEqual(npToTuple(m), tuple(range(9)))
20  self.assertEqual(npToTuple(m.T), tuple(range(9)))
21  self.assertEqual(npToTuple(np.reshape(m, (3, 3))), ((0, 1, 2), (3, 4, 5), (6, 7, 8)))
22 
23  def test_rotate(self):
24  self.assertApprox(rotate('x', pi / 2), np.array([[1., 0., 0.],[0., 0., -1.],[0., 1., 0.]]))
25  self.assertApprox(rotate('x', pi).dot(rotate('y', pi)), rotate('z', pi))
26  m = rotate('x', pi / 3).dot(rotate('y', pi / 5)).dot(rotate('y', pi / 7))
27  self.assertApprox(rpyToMatrix(matrixToRpy(m)), m)
28  rpy = np.array(list(range(3))) * pi / 2
29  self.assertApprox(matrixToRpy(rpyToMatrix(rpy)), rpy)
30  self.assertApprox(rpyToMatrix(rpy), rpyToMatrix(float(rpy[0]), float(rpy[1]), float(rpy[2])))
31 
32  try:
33  rotate('toto',10.)
34  except ValueError:
35  self.assertTrue(True)
36  else:
37  self.assertTrue(False)
38 
39  try:
40  rotate('w',10.)
41  except ValueError:
42  self.assertTrue(True)
43  else:
44  self.assertTrue(False)
45 
46 
47  def test_rpyToMatrix(self):
48  r = random()*2*pi - pi
49  p = random()*pi - pi/2
50  y = random()*2*pi - pi
51 
52  R = rpyToMatrix(r, p, y)
53 
54  Raa = AngleAxis(y, np.array([0., 0., 1.])).matrix() \
55  .dot(AngleAxis(p, np.array([0., 1., 0.])).matrix()) \
56  .dot(AngleAxis(r, np.array([1., 0., 0.])).matrix())
57 
58  self.assertApprox(R, Raa)
59 
60  v = np.array([r, p, y])
61 
62  Rv = rpyToMatrix(v)
63 
64  self.assertApprox(Rv, Raa)
65  self.assertApprox(Rv, R)
66 
67 
68  def test_matrixToRpy(self):
69  n = 100
70  for _ in range(n):
71  quat = pin.Quaternion(np.random.rand(4,1)).normalized()
72  R = quat.toRotationMatrix()
73 
74  v = matrixToRpy(R)
75  Rprime = rpyToMatrix(v)
76 
77  self.assertApprox(Rprime, R)
78  self.assertTrue(-pi <= v[0] and v[0] <= pi)
79  self.assertTrue(-pi/2 <= v[1] and v[1] <= pi/2)
80  self.assertTrue(-pi <= v[2] and v[2] <= pi)
81 
82  n2 = 100
83 
84  # Test singular case theta = pi/2
85  Rp = np.array([[ 0.0, 0.0, 1.0],
86  [ 0.0, 1.0, 0.0],
87  [-1.0, 0.0, 0.0]])
88  for _ in range(n2):
89  r = random()*2*pi - pi
90  y = random()*2*pi - pi
91 
92  R = AngleAxis(y, np.array([0., 0., 1.])).matrix() \
93  .dot(Rp) \
94  .dot(AngleAxis(r, np.array([1., 0., 0.])).matrix())
95 
96  v = matrixToRpy(R)
97  Rprime = rpyToMatrix(v)
98 
99  self.assertApprox(Rprime, R)
100  self.assertTrue(-pi <= v[0] and v[0] <= pi)
101  self.assertTrue(-pi/2 <= v[1] and v[1] <= pi/2)
102  self.assertTrue(-pi <= v[2] and v[2] <= pi)
103 
104  # Test singular case theta = -pi/2
105  Rp = np.array([[0.0, 0.0, -1.0],
106  [0.0, 1.0, 0.0],
107  [1.0, 0.0, 0.0]])
108  for _ in range(n2):
109  r = random()*2*pi - pi
110  y = random()*2*pi - pi
111 
112  R = AngleAxis(y, np.array([0., 0., 1.])).matrix() \
113  .dot(Rp) \
114  .dot(AngleAxis(r, np.array([1., 0., 0.])).matrix())
115 
116  v = matrixToRpy(R)
117  Rprime = rpyToMatrix(v)
118 
119  self.assertApprox(Rprime, R)
120  self.assertTrue(-pi <= v[0] and v[0] <= pi)
121  self.assertTrue(-pi/2 <= v[1] and v[1] <= pi/2)
122  self.assertTrue(-pi <= v[2] and v[2] <= pi)
123 
124 
125 
126  def test_computeRpyJacobian(self):
127  # Check identity at zero
128  rpy = np.zeros(3)
129  j0 = computeRpyJacobian(rpy)
130  self.assertTrue((j0 == np.eye(3)).all())
131  jL = computeRpyJacobian(rpy, pin.LOCAL)
132  self.assertTrue((jL == np.eye(3)).all())
133  jW = computeRpyJacobian(rpy, pin.WORLD)
134  self.assertTrue((jW == np.eye(3)).all())
135  jA = computeRpyJacobian(rpy, pin.LOCAL_WORLD_ALIGNED)
136  self.assertTrue((jA == np.eye(3)).all())
137 
138  # Check correct identities between different versions
139  r = random()*2*pi - pi
140  p = random()*pi - pi/2
141  y = random()*2*pi - pi
142  rpy = np.array([r, p, y])
143  R = rpyToMatrix(rpy)
144  j0 = computeRpyJacobian(rpy)
145  jL = computeRpyJacobian(rpy, pin.LOCAL)
146  jW = computeRpyJacobian(rpy, pin.WORLD)
147  jA = computeRpyJacobian(rpy, pin.LOCAL_WORLD_ALIGNED)
148  self.assertTrue((j0 == jL).all())
149  self.assertTrue((jW == jA).all())
150  self.assertApprox(jW, R.dot(jL))
151 
152  # Check against finite differences
153  rpydot = np.random.rand(3)
154  eps = 1e-7
155  tol = 1e-4
156 
157  dRdr = (rpyToMatrix(r + eps, p, y) - R) / eps
158  dRdp = (rpyToMatrix(r, p + eps, y) - R) / eps
159  dRdy = (rpyToMatrix(r, p, y + eps) - R) / eps
160  Rdot = dRdr * rpydot[0] + dRdp * rpydot[1] + dRdy * rpydot[2]
161 
162  omegaL = jL.dot(rpydot)
163  self.assertTrue(np.allclose(Rdot, R.dot(pin.skew(omegaL)), atol=tol))
164 
165  omegaW = jW.dot(rpydot)
166  self.assertTrue(np.allclose(Rdot, pin.skew(omegaW).dot(R), atol=tol))
167 
168  def test_computeRpyJacobianInverse(self):
169  # Check correct identities between different versions
170  r = random()*2*pi - pi
171  p = random()*pi - pi/2
172  p *= 0.999 # ensure we are not too close to a singularity
173  y = random()*2*pi - pi
174  rpy = np.array([r, p, y])
175 
176  j0 = computeRpyJacobian(rpy)
177  j0inv = computeRpyJacobianInverse(rpy)
178  self.assertApprox(j0inv, inv(j0))
179 
180  jL = computeRpyJacobian(rpy, pin.LOCAL)
181  jLinv = computeRpyJacobianInverse(rpy, pin.LOCAL)
182  self.assertApprox(jLinv, inv(jL))
183 
184  jW = computeRpyJacobian(rpy, pin.WORLD)
185  jWinv = computeRpyJacobianInverse(rpy, pin.WORLD)
186  self.assertApprox(jWinv, inv(jW))
187 
188  jA = computeRpyJacobian(rpy, pin.LOCAL_WORLD_ALIGNED)
189  jAinv = computeRpyJacobianInverse(rpy, pin.LOCAL_WORLD_ALIGNED)
190  self.assertApprox(jAinv, inv(jA))
191 
192  def test_computeRpyJacobianTimeDerivative(self):
193  # Check zero at zero velocity
194  r = random()*2*pi - pi
195  p = random()*pi - pi/2
196  y = random()*2*pi - pi
197  rpy = np.array([r, p, y])
198  rpydot = np.zeros(3)
199  dj0 = computeRpyJacobianTimeDerivative(rpy, rpydot)
200  self.assertTrue((dj0 == np.zeros((3,3))).all())
201  djL = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.LOCAL)
202  self.assertTrue((djL == np.zeros((3,3))).all())
203  djW = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.WORLD)
204  self.assertTrue((djW == np.zeros((3,3))).all())
205  djA = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.LOCAL_WORLD_ALIGNED)
206  self.assertTrue((djA == np.zeros((3,3))).all())
207 
208  # Check correct identities between different versions
209  rpydot = np.random.rand(3)
210  dj0 = computeRpyJacobianTimeDerivative(rpy, rpydot)
211  djL = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.LOCAL)
212  djW = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.WORLD)
213  djA = computeRpyJacobianTimeDerivative(rpy, rpydot, pin.LOCAL_WORLD_ALIGNED)
214  self.assertTrue((dj0 == djL).all())
215  self.assertTrue((djW == djA).all())
216 
217  R = rpyToMatrix(rpy)
218  jL = computeRpyJacobian(rpy, pin.LOCAL)
219  jW = computeRpyJacobian(rpy, pin.WORLD)
220  omegaL = jL.dot(rpydot)
221  omegaW = jW.dot(rpydot)
222  self.assertApprox(omegaW, R.dot(omegaL))
223  self.assertApprox(djW, pin.skew(omegaW).dot(R).dot(jL) + R.dot(djL))
224  self.assertApprox(djW, R.dot(pin.skew(omegaL)).dot(jL) + R.dot(djL))
225 
226 if __name__ == '__main__':
227  unittest.main()
pinocchio::rpy::computeRpyJacobian
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > computeRpyJacobian(const Eigen::MatrixBase< Vector3Like > &rpy, const ReferenceFrame rf=LOCAL)
Compute the Jacobian of the Roll-Pitch-Yaw conversion.
rpy.TestRPY.test_computeRpyJacobianTimeDerivative
def test_computeRpyJacobianTimeDerivative(self)
Definition: rpy.py:192
rpy.TestRPY.test_computeRpyJacobian
def test_computeRpyJacobian(self)
Definition: rpy.py:126
rpy.TestRPY.test_matrixToRpy
def test_matrixToRpy(self)
Definition: rpy.py:68
pinocchio::rpy::matrixToRpy
Eigen::Matrix< typename Matrix3Like::Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options > matrixToRpy(const Eigen::MatrixBase< Matrix3Like > &R)
Convert from Transformation Matrix to Roll, Pitch, Yaw.
rpy.TestRPY.test_computeRpyJacobianInverse
def test_computeRpyJacobianInverse(self)
Definition: rpy.py:168
rpy.TestRPY.test_npToTuple
def test_npToTuple(self)
Definition: rpy.py:17
rpy.TestRPY.test_rotate
def test_rotate(self)
Definition: rpy.py:23
rpy.TestRPY.test_rpyToMatrix
def test_rpyToMatrix(self)
Definition: rpy.py:47
pinocchio::random
void random(const LieGroupGenericTpl< LieGroupCollection > &lg, const Eigen::MatrixBase< Config_t > &qout)
pinocchio::rpy::computeRpyJacobianInverse
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > computeRpyJacobianInverse(const Eigen::MatrixBase< Vector3Like > &rpy, const ReferenceFrame rf=LOCAL)
Compute the inverse Jacobian of the Roll-Pitch-Yaw conversion.
pinocchio::rpy::rpyToMatrix
Eigen::Matrix< Scalar, 3, 3 > rpyToMatrix(const Scalar &r, const Scalar &p, const Scalar &y)
Convert from Roll, Pitch, Yaw to rotation Matrix.
pinocchio::rpy::computeRpyJacobianTimeDerivative
Eigen::Matrix< typename Vector3Like0::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options > computeRpyJacobianTimeDerivative(const Eigen::MatrixBase< Vector3Like0 > &rpy, const Eigen::MatrixBase< Vector3Like1 > &rpydot, const ReferenceFrame rf=LOCAL)
Compute the time derivative Jacobian of the Roll-Pitch-Yaw conversion.
pinocchio::rpy
Roll-pitch-yaw operations.

pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:32