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