GCC Code Coverage Report

Directory: ./
File: unittest/rpy.cpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 177 0.0%
Branches: 0 1518 0.0%
Line Branch Exec Source
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()
272