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