rpy.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2020 INRIA
3 //
4 
5 #include <pinocchio/math/rpy.hpp>
6 #include <pinocchio/math/quaternion.hpp>
7 #include <pinocchio/spatial/skew.hpp>
8 
9 #include <boost/variant.hpp> // to avoid C99 warnings
10 
11 #include <boost/test/unit_test.hpp>
12 #include <boost/utility/binary.hpp>
13 
14 BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
15 
16 BOOST_AUTO_TEST_CASE(test_rpyToMatrix)
17 {
18  double r = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX / (2 * M_PI))) - M_PI;
19  double p = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX / M_PI)) - (M_PI / 2);
20  double y = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX / (2 * M_PI))) - M_PI;
21 
22  Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(r, p, y);
23 
24  Eigen::Matrix3d Raa =
25  (Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
26  * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()))
27  .toRotationMatrix();
28 
29  BOOST_CHECK(R.isApprox(Raa));
30 
31  Eigen::Vector3d v;
32  v << r, p, y;
33 
34  Eigen::Matrix3d Rv = pinocchio::rpy::rpyToMatrix(v);
35 
36  BOOST_CHECK(Rv.isApprox(Raa));
37  BOOST_CHECK(Rv.isApprox(R));
38 }
39 
40 BOOST_AUTO_TEST_CASE(test_matrixToRpy)
41 {
42 #ifdef NDEBUG
43  const int n = 1e5;
44 #else
45  const int n = 1e2;
46 #endif
47  for (int k = 0; k < n; ++k)
48  {
49  Eigen::Quaterniond quat;
50  pinocchio::quaternion::uniformRandom(quat);
51  const Eigen::Matrix3d R = quat.toRotationMatrix();
52 
53  const Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);
54  Eigen::Matrix3d Rprime = pinocchio::rpy::rpyToMatrix(v);
55 
56  BOOST_CHECK(Rprime.isApprox(R));
57  BOOST_CHECK(-M_PI <= v[0] && v[0] <= M_PI);
58  BOOST_CHECK(-M_PI / 2 <= v[1] && v[1] <= M_PI / 2);
59  BOOST_CHECK(-M_PI <= v[2] && v[2] <= M_PI);
60  }
61 
62 #ifdef NDEBUG
63  const int n2 = 1e3;
64 #else
65  const int n2 = 1e2;
66 #endif
67 
68  // Test singular case theta = pi/2
69  for (int k = 0; k < n2; ++k)
70  {
71  double r = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX / (2 * M_PI))) - M_PI;
72  double y = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX / (2 * M_PI))) - M_PI;
73  Eigen::Matrix3d Rp;
74  Rp << 0.0, 0.0, 1.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.0;
75  const Eigen::Matrix3d R = Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()).toRotationMatrix() * Rp
76  * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()).toRotationMatrix();
77 
78  const Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);
79  Eigen::Matrix3d Rprime = pinocchio::rpy::rpyToMatrix(v);
80 
81  BOOST_CHECK(Rprime.isApprox(R));
82  BOOST_CHECK(-M_PI <= v[0] && v[0] <= M_PI);
83  BOOST_CHECK(-M_PI / 2 <= v[1] && v[1] <= M_PI / 2);
84  BOOST_CHECK(-M_PI <= v[2] && v[2] <= M_PI);
85  }
86 
87  // Test singular case theta = -pi/2
88  for (int k = 0; k < n2; ++k)
89  {
90  double r = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX / (2 * M_PI))) - M_PI;
91  double y = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX / (2 * M_PI))) - M_PI;
92  Eigen::Matrix3d Rp;
93  Rp << 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0;
94  const Eigen::Matrix3d R = Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()).toRotationMatrix() * Rp
95  * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()).toRotationMatrix();
96 
97  const Eigen::Vector3d v = pinocchio::rpy::matrixToRpy(R);
98  Eigen::Matrix3d Rprime = pinocchio::rpy::rpyToMatrix(v);
99 
100  BOOST_CHECK(Rprime.isApprox(R));
101  BOOST_CHECK(-M_PI <= v[0] && v[0] <= M_PI);
102  BOOST_CHECK(-M_PI / 2 <= v[1] && v[1] <= M_PI / 2);
103  BOOST_CHECK(-M_PI <= v[2] && v[2] <= M_PI);
104  }
105 }
106 
107 BOOST_AUTO_TEST_CASE(test_computeRpyJacobian)
108 {
109  // Check identity at zero
110  Eigen::Vector3d rpy(Eigen::Vector3d::Zero());
111  Eigen::Matrix3d j0 = pinocchio::rpy::computeRpyJacobian(rpy);
112  BOOST_CHECK(j0.isIdentity());
113  Eigen::Matrix3d jL = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL);
114  BOOST_CHECK(jL.isIdentity());
115  Eigen::Matrix3d jW = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::WORLD);
116  BOOST_CHECK(jW.isIdentity());
117  Eigen::Matrix3d jA = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
118  BOOST_CHECK(jA.isIdentity());
119 
120  // Check correct identities between different versions
121  double r = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX / (2 * M_PI))) - M_PI;
122  double p = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX / M_PI)) - (M_PI / 2);
123  double y = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX / (2 * M_PI))) - M_PI;
124  rpy = Eigen::Vector3d(r, p, y);
125  Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(rpy);
126  j0 = pinocchio::rpy::computeRpyJacobian(rpy);
127  jL = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL);
128  jW = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::WORLD);
129  jA = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
130  BOOST_CHECK(j0 == jL);
131  BOOST_CHECK(jW == jA);
132  BOOST_CHECK(jW.isApprox(R * jL));
133 
134  // Check against analytical formulas
135  Eigen::Vector3d jL0Expected = Eigen::Vector3d::UnitX();
136  Eigen::Vector3d jL1Expected =
137  Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()).toRotationMatrix().transpose().col(1);
138  Eigen::Vector3d jL2Expected = (Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY())
139  * Eigen::AngleAxisd(r, Eigen::Vector3d::UnitX()))
140  .toRotationMatrix()
141  .transpose()
142  .col(2);
143  BOOST_CHECK(jL.col(0).isApprox(jL0Expected));
144  BOOST_CHECK(jL.col(1).isApprox(jL1Expected));
145  BOOST_CHECK(jL.col(2).isApprox(jL2Expected));
146 
147  Eigen::Vector3d jW0Expected = (Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ())
148  * Eigen::AngleAxisd(p, Eigen::Vector3d::UnitY()))
149  .toRotationMatrix()
150  .col(0);
151  Eigen::Vector3d jW1Expected =
152  Eigen::AngleAxisd(y, Eigen::Vector3d::UnitZ()).toRotationMatrix().col(1);
153  Eigen::Vector3d jW2Expected = Eigen::Vector3d::UnitZ();
154  BOOST_CHECK(jW.col(0).isApprox(jW0Expected));
155  BOOST_CHECK(jW.col(1).isApprox(jW1Expected));
156  BOOST_CHECK(jW.col(2).isApprox(jW2Expected));
157 
158  // Check against finite differences
159  Eigen::Vector3d rpydot = Eigen::Vector3d::Random();
160  double const eps = 1e-7;
161  double const tol = 1e-5;
162 
163  Eigen::Matrix3d dRdr = (pinocchio::rpy::rpyToMatrix(r + eps, p, y) - R) / eps;
164  Eigen::Matrix3d dRdp = (pinocchio::rpy::rpyToMatrix(r, p + eps, y) - R) / eps;
165  Eigen::Matrix3d dRdy = (pinocchio::rpy::rpyToMatrix(r, p, y + eps) - R) / eps;
166  Eigen::Matrix3d Rdot = dRdr * rpydot[0] + dRdp * rpydot[1] + dRdy * rpydot[2];
167 
168  Eigen::Vector3d omegaL = jL * rpydot;
169  BOOST_CHECK(Rdot.isApprox(R * pinocchio::skew(omegaL), tol));
170 
171  Eigen::Vector3d omegaW = jW * rpydot;
172  BOOST_CHECK(Rdot.isApprox(pinocchio::skew(omegaW) * R, tol));
173 }
174 
175 BOOST_AUTO_TEST_CASE(test_computeRpyJacobianInverse)
176 {
177  // Check correct identities between different versions
178  double r = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX / (2 * M_PI))) - M_PI;
179  double p = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX / M_PI)) - (M_PI / 2);
180  p *= 0.999; // ensure we are not too close to a singularity
181  double y = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX / (2 * M_PI))) - M_PI;
182  Eigen::Vector3d rpy(r, p, y);
183 
184  Eigen::Matrix3d j0 = pinocchio::rpy::computeRpyJacobian(rpy);
185  Eigen::Matrix3d j0inv = pinocchio::rpy::computeRpyJacobianInverse(rpy);
186  BOOST_CHECK(j0inv.isApprox(j0.inverse()));
187 
188  Eigen::Matrix3d jL = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL);
189  Eigen::Matrix3d jLinv = pinocchio::rpy::computeRpyJacobianInverse(rpy, pinocchio::LOCAL);
190  BOOST_CHECK(jLinv.isApprox(jL.inverse()));
191 
192  Eigen::Matrix3d jW = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::WORLD);
193  Eigen::Matrix3d jWinv = pinocchio::rpy::computeRpyJacobianInverse(rpy, pinocchio::WORLD);
194  BOOST_CHECK(jWinv.isApprox(jW.inverse()));
195 
196  Eigen::Matrix3d jA = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
197  Eigen::Matrix3d jAinv =
198  pinocchio::rpy::computeRpyJacobianInverse(rpy, pinocchio::LOCAL_WORLD_ALIGNED);
199  BOOST_CHECK(jAinv.isApprox(jA.inverse()));
200 }
201 
202 BOOST_AUTO_TEST_CASE(test_computeRpyJacobianTimeDerivative)
203 {
204  // Check zero at zero velocity
205  double r = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX / (2 * M_PI))) - M_PI;
206  double p = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX / M_PI)) - (M_PI / 2);
207  double y = static_cast<double>(rand()) / (static_cast<double>(RAND_MAX / (2 * M_PI))) - M_PI;
208  Eigen::Vector3d rpy(r, p, y);
209  Eigen::Vector3d rpydot(Eigen::Vector3d::Zero());
210  Eigen::Matrix3d dj0 = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot);
211  BOOST_CHECK(dj0.isZero());
212  Eigen::Matrix3d djL =
213  pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::LOCAL);
214  BOOST_CHECK(djL.isZero());
215  Eigen::Matrix3d djW =
216  pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::WORLD);
217  BOOST_CHECK(djW.isZero());
218  Eigen::Matrix3d djA =
219  pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::LOCAL_WORLD_ALIGNED);
220  BOOST_CHECK(djA.isZero());
221 
222  // Check correct identities between different versions
223  rpydot = Eigen::Vector3d::Random();
224  dj0 = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot);
225  djL = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::LOCAL);
226  djW = pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::WORLD);
227  djA =
228  pinocchio::rpy::computeRpyJacobianTimeDerivative(rpy, rpydot, pinocchio::LOCAL_WORLD_ALIGNED);
229  BOOST_CHECK(dj0 == djL);
230  BOOST_CHECK(djW == djA);
231 
232  Eigen::Matrix3d R = pinocchio::rpy::rpyToMatrix(rpy);
233  Eigen::Matrix3d jL = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::LOCAL);
234  Eigen::Matrix3d jW = pinocchio::rpy::computeRpyJacobian(rpy, pinocchio::WORLD);
235  Eigen::Vector3d omegaL = jL * rpydot;
236  Eigen::Vector3d omegaW = jW * rpydot;
237  BOOST_CHECK(omegaW.isApprox(R * omegaL));
238  BOOST_CHECK(djW.isApprox(pinocchio::skew(omegaW) * R * jL + R * djL));
239  BOOST_CHECK(djW.isApprox(R * pinocchio::skew(omegaL) * jL + R * djL));
240 
241  // Check against finite differences
242  double const eps = 1e-7;
243  double const tol = 1e-5;
244  Eigen::Vector3d rpyEps = rpy;
245 
246  rpyEps[0] += eps;
247  Eigen::Matrix3d djLdr = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::LOCAL) - jL) / eps;
248  rpyEps[0] = rpy[0];
249  rpyEps[1] += eps;
250  Eigen::Matrix3d djLdp = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::LOCAL) - jL) / eps;
251  rpyEps[1] = rpy[1];
252  rpyEps[2] += eps;
253  Eigen::Matrix3d djLdy = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::LOCAL) - jL) / eps;
254  rpyEps[2] = rpy[2];
255  Eigen::Matrix3d djLf = djLdr * rpydot[0] + djLdp * rpydot[1] + djLdy * rpydot[2];
256  BOOST_CHECK(djL.isApprox(djLf, tol));
257 
258  rpyEps[0] += eps;
259  Eigen::Matrix3d djWdr = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::WORLD) - jW) / eps;
260  rpyEps[0] = rpy[0];
261  rpyEps[1] += eps;
262  Eigen::Matrix3d djWdp = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::WORLD) - jW) / eps;
263  rpyEps[1] = rpy[1];
264  rpyEps[2] += eps;
265  Eigen::Matrix3d djWdy = (pinocchio::rpy::computeRpyJacobian(rpyEps, pinocchio::WORLD) - jW) / eps;
266  rpyEps[2] = rpy[2];
267  Eigen::Matrix3d djWf = djWdr * rpydot[0] + djWdp * rpydot[1] + djWdy * rpydot[2];
268  BOOST_CHECK(djW.isApprox(djWf, tol));
269 }
270 
271 BOOST_AUTO_TEST_SUITE_END()
rpy
Definition: rpy.py:1
pinocchio::WORLD
@ WORLD
Definition: multibody/fwd.hpp:48
quaternion.hpp
pinocchio::quaternion::uniformRandom
void uniformRandom(Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Definition: math/quaternion.hpp:114
quat
quat
y
y
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.
R
R
skew.hpp
pinocchio::LOCAL_WORLD_ALIGNED
@ LOCAL_WORLD_ALIGNED
Definition: multibody/fwd.hpp:52
r
FCL_REAL r
dpendulum.p
p
Definition: dpendulum.py:7
pinocchio::skew
void skew(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector, i.e. the antisymmetric matrix representation o...
Definition: skew.hpp:22
cartpole.v
v
Definition: cartpole.py:153
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.
BOOST_AUTO_TEST_CASE
BOOST_AUTO_TEST_CASE(test_rpyToMatrix)
Definition: rpy.cpp:16
pinocchio::toRotationMatrix
void toRotationMatrix(const Eigen::MatrixBase< Vector3 > &axis, const Scalar &cos_value, const Scalar &sin_value, const Eigen::MatrixBase< Matrix3 > &res)
Computes a rotation matrix from a vector and values of sin and cos orientations values.
Definition: rotation.hpp:26
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.
dcrba.eps
int eps
Definition: dcrba.py:458
pinocchio.utils.rand
def rand(n)
Definition: bindings/python/pinocchio/utils.py:41
rpy.hpp
n
Vec3f n
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::LOCAL
@ LOCAL
Definition: multibody/fwd.hpp:50

pinocchio
Author(s):
autogenerated on Thu Dec 19 2024 03:41:32