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