Line |
Branch |
Exec |
Source |
1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2021 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#include <iostream> |
6 |
|
|
|
7 |
|
|
#include "pinocchio/spatial/force.hpp" |
8 |
|
|
#include "pinocchio/spatial/motion.hpp" |
9 |
|
|
#include "pinocchio/spatial/se3.hpp" |
10 |
|
|
#include "pinocchio/spatial/inertia.hpp" |
11 |
|
|
#include "pinocchio/spatial/act-on-set.hpp" |
12 |
|
|
#include "pinocchio/spatial/explog.hpp" |
13 |
|
|
#include "pinocchio/spatial/skew.hpp" |
14 |
|
|
#include "pinocchio/spatial/cartesian-axis.hpp" |
15 |
|
|
#include "pinocchio/spatial/spatial-axis.hpp" |
16 |
|
|
|
17 |
|
|
#include <boost/test/unit_test.hpp> |
18 |
|
|
#include <boost/utility/binary.hpp> |
19 |
|
|
|
20 |
|
|
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
21 |
|
|
|
22 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_SE3) |
23 |
|
|
{ |
24 |
|
|
using namespace pinocchio; |
25 |
|
|
typedef SE3::HomogeneousMatrixType HomogeneousMatrixType; |
26 |
|
|
typedef SE3::ActionMatrixType ActionMatrixType; |
27 |
|
|
typedef SE3::Vector3 Vector3; |
28 |
|
|
typedef Eigen::Matrix<double, 4, 1> Vector4; |
29 |
|
|
|
30 |
|
✗ |
const SE3 identity = SE3::Identity(); |
31 |
|
|
|
32 |
|
|
typedef SE3::Quaternion Quaternion; |
33 |
|
|
typedef SE3::Vector4 Vector4; |
34 |
|
✗ |
Quaternion quat(Vector4::Random().normalized()); |
35 |
|
|
|
36 |
|
✗ |
SE3 m_from_quat(quat, Vector3::Random()); |
37 |
|
|
|
38 |
|
✗ |
SE3 amb = SE3::Random(); |
39 |
|
✗ |
SE3 bmc = SE3::Random(); |
40 |
|
✗ |
SE3 amc = amb * bmc; |
41 |
|
|
|
42 |
|
✗ |
HomogeneousMatrixType aMb = amb; |
43 |
|
✗ |
HomogeneousMatrixType bMc = bmc; |
44 |
|
|
|
45 |
|
|
// Test internal product |
46 |
|
✗ |
HomogeneousMatrixType aMc = amc; |
47 |
|
✗ |
BOOST_CHECK(aMc.isApprox(aMb * bMc)); |
48 |
|
|
|
49 |
|
✗ |
HomogeneousMatrixType bMa = amb.inverse(); |
50 |
|
✗ |
BOOST_CHECK(bMa.isApprox(aMb.inverse())); |
51 |
|
|
|
52 |
|
|
// Test point action |
53 |
|
✗ |
Vector3 p = Vector3::Random(); |
54 |
|
✗ |
Vector4 p4; |
55 |
|
✗ |
p4.head(3) = p; |
56 |
|
✗ |
p4[3] = 1; |
57 |
|
|
|
58 |
|
✗ |
Vector3 Mp = (aMb * p4).head(3); |
59 |
|
✗ |
BOOST_CHECK(amb.act(p).isApprox(Mp)); |
60 |
|
|
|
61 |
|
✗ |
Vector3 Mip = (aMb.inverse() * p4).head(3); |
62 |
|
✗ |
BOOST_CHECK(amb.actInv(p).isApprox(Mip)); |
63 |
|
|
|
64 |
|
|
// Test action matrix |
65 |
|
✗ |
ActionMatrixType aXb = amb; |
66 |
|
✗ |
ActionMatrixType bXc = bmc; |
67 |
|
✗ |
ActionMatrixType aXc = amc; |
68 |
|
✗ |
BOOST_CHECK(aXc.isApprox(aXb * bXc)); |
69 |
|
|
|
70 |
|
✗ |
ActionMatrixType bXa = amb.inverse(); |
71 |
|
✗ |
BOOST_CHECK(bXa.isApprox(aXb.inverse())); |
72 |
|
|
|
73 |
|
✗ |
ActionMatrixType X_identity = identity.toActionMatrix(); |
74 |
|
✗ |
BOOST_CHECK(X_identity.isIdentity()); |
75 |
|
|
|
76 |
|
✗ |
ActionMatrixType X_identity_inverse = identity.toActionMatrixInverse(); |
77 |
|
✗ |
BOOST_CHECK(X_identity_inverse.isIdentity()); |
78 |
|
|
|
79 |
|
|
// Test dual action matrix |
80 |
|
✗ |
BOOST_CHECK(aXb.inverse().transpose().isApprox(amb.toDualActionMatrix())); |
81 |
|
|
|
82 |
|
|
// Test isIdentity |
83 |
|
✗ |
BOOST_CHECK(identity.isIdentity()); |
84 |
|
|
|
85 |
|
|
// Test isApprox |
86 |
|
✗ |
BOOST_CHECK(identity.isApprox(identity)); |
87 |
|
|
|
88 |
|
|
// Test cast |
89 |
|
|
typedef SE3Tpl<float> SE3f; |
90 |
|
✗ |
SE3f::Matrix3 rot_float(amb.rotation().cast<float>()); |
91 |
|
✗ |
SE3f amb_float = amb.cast<float>(); |
92 |
|
✗ |
BOOST_CHECK(amb_float.isApprox(amb.cast<float>())); |
93 |
|
|
|
94 |
|
✗ |
SE3f amb_float2(amb); |
95 |
|
✗ |
BOOST_CHECK(amb_float.isApprox(amb_float2)); |
96 |
|
|
|
97 |
|
|
// Test actInv |
98 |
|
✗ |
const SE3 M = SE3::Random(); |
99 |
|
✗ |
const SE3 Minv = M.inverse(); |
100 |
|
|
|
101 |
|
✗ |
BOOST_CHECK(Minv.actInv(Minv).isIdentity()); |
102 |
|
✗ |
BOOST_CHECK(M.actInv(identity).isApprox(Minv)); |
103 |
|
|
|
104 |
|
|
// Test normalization |
105 |
|
|
{ |
106 |
|
✗ |
const double prec = Eigen::NumTraits<double>::dummy_precision(); |
107 |
|
✗ |
SE3 M(SE3::Random()); |
108 |
|
✗ |
M.rotation() += prec * SE3::Matrix3::Random(); |
109 |
|
✗ |
BOOST_CHECK(!M.isNormalized()); |
110 |
|
|
|
111 |
|
✗ |
SE3 M_normalized = M.normalized(); |
112 |
|
✗ |
BOOST_CHECK(M_normalized.isNormalized()); |
113 |
|
|
|
114 |
|
✗ |
M.normalize(); |
115 |
|
✗ |
BOOST_CHECK(M.isNormalized()); |
116 |
|
|
} |
117 |
|
|
} |
118 |
|
|
|
119 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_Motion) |
120 |
|
|
{ |
121 |
|
|
using namespace pinocchio; |
122 |
|
|
typedef SE3::ActionMatrixType ActionMatrixType; |
123 |
|
|
typedef Motion::Vector6 Vector6; |
124 |
|
|
|
125 |
|
✗ |
SE3 amb = SE3::Random(); |
126 |
|
✗ |
SE3 bmc = SE3::Random(); |
127 |
|
✗ |
SE3 amc = amb * bmc; |
128 |
|
|
|
129 |
|
✗ |
Motion bv = Motion::Random(); |
130 |
|
✗ |
Motion bv2 = Motion::Random(); |
131 |
|
|
|
132 |
|
|
typedef MotionBase<Motion> Base; |
133 |
|
|
|
134 |
|
✗ |
Vector6 bv_vec = bv; |
135 |
|
✗ |
Vector6 bv2_vec = bv2; |
136 |
|
|
|
137 |
|
|
// std::stringstream |
138 |
|
✗ |
std::stringstream ss; |
139 |
|
✗ |
ss << bv << std::endl; |
140 |
|
✗ |
BOOST_CHECK(!ss.str().empty()); |
141 |
|
|
|
142 |
|
|
// Test .+. |
143 |
|
✗ |
Vector6 bvPbv2_vec = bv + bv2; |
144 |
|
✗ |
BOOST_CHECK(bvPbv2_vec.isApprox(bv_vec + bv2_vec)); |
145 |
|
|
|
146 |
|
✗ |
Motion bplus = static_cast<Base &>(bv) + static_cast<Base &>(bv2); |
147 |
|
✗ |
BOOST_CHECK((bv + bv2).isApprox(bplus)); |
148 |
|
|
|
149 |
|
✗ |
Motion v_not_zero(Vector6::Ones()); |
150 |
|
✗ |
BOOST_CHECK(!v_not_zero.isZero()); |
151 |
|
|
|
152 |
|
✗ |
Motion v_zero(Vector6::Zero()); |
153 |
|
✗ |
BOOST_CHECK(v_zero.isZero()); |
154 |
|
|
|
155 |
|
|
// Test == and != |
156 |
|
✗ |
BOOST_CHECK(bv == bv); |
157 |
|
✗ |
BOOST_CHECK(!(bv != bv)); |
158 |
|
|
|
159 |
|
|
// Test -. |
160 |
|
✗ |
Vector6 Mbv_vec = -bv; |
161 |
|
✗ |
BOOST_CHECK(Mbv_vec.isApprox(-bv_vec)); |
162 |
|
|
|
163 |
|
|
// Test .+=. |
164 |
|
✗ |
Motion bv3 = bv; |
165 |
|
✗ |
bv3 += bv2; |
166 |
|
✗ |
BOOST_CHECK(bv3.toVector().isApprox(bv_vec + bv2_vec)); |
167 |
|
|
|
168 |
|
|
// Test .=V6 |
169 |
|
✗ |
bv3 = bv2_vec; |
170 |
|
✗ |
BOOST_CHECK(bv3.toVector().isApprox(bv2_vec)); |
171 |
|
|
|
172 |
|
|
// Test scalar*M6 |
173 |
|
✗ |
Motion twicebv(2. * bv); |
174 |
|
✗ |
BOOST_CHECK(twicebv.isApprox(Motion(2. * bv.toVector()))); |
175 |
|
|
|
176 |
|
|
// Test M6*scalar |
177 |
|
✗ |
Motion bvtwice(bv * 2.); |
178 |
|
✗ |
BOOST_CHECK(bvtwice.isApprox(twicebv)); |
179 |
|
|
|
180 |
|
|
// Test M6/scalar |
181 |
|
✗ |
Motion bvdividedbytwo(bvtwice / 2.); |
182 |
|
✗ |
BOOST_CHECK(bvdividedbytwo.isApprox(bv)); |
183 |
|
|
|
184 |
|
|
// Test constructor from V6 |
185 |
|
✗ |
Motion bv4(bv2_vec); |
186 |
|
✗ |
BOOST_CHECK(bv4.toVector().isApprox(bv2_vec)); |
187 |
|
|
|
188 |
|
|
// Test action |
189 |
|
✗ |
ActionMatrixType aXb = amb; |
190 |
|
✗ |
BOOST_CHECK(amb.act(bv).toVector().isApprox(aXb * bv_vec)); |
191 |
|
|
|
192 |
|
|
// Test action inverse |
193 |
|
✗ |
ActionMatrixType bXc = bmc; |
194 |
|
✗ |
BOOST_CHECK(bmc.actInv(bv).toVector().isApprox(bXc.inverse() * bv_vec)); |
195 |
|
|
|
196 |
|
|
// Test double action |
197 |
|
✗ |
Motion cv = Motion::Random(); |
198 |
|
✗ |
bv = bmc.act(cv); |
199 |
|
✗ |
BOOST_CHECK(amb.act(bv).toVector().isApprox(amc.act(cv).toVector())); |
200 |
|
|
|
201 |
|
|
// Simple test for cross product vxv |
202 |
|
✗ |
Motion vxv = bv.cross(bv); |
203 |
|
✗ |
BOOST_CHECK_SMALL( |
204 |
|
|
vxv.toVector().tail(3).norm(), |
205 |
|
|
1e-3); // previously ensure that (vxv.toVector().tail(3).isMuchSmallerThan(1e-3)); |
206 |
|
|
|
207 |
|
|
// Test Action Matrix |
208 |
|
✗ |
Motion v2xv = bv2.cross(bv); |
209 |
|
✗ |
Motion::ActionMatrixType actv2 = bv2.toActionMatrix(); |
210 |
|
|
|
211 |
|
✗ |
BOOST_CHECK(v2xv.toVector().isApprox(actv2 * bv.toVector())); |
212 |
|
|
|
213 |
|
|
// Test Dual Action Matrix |
214 |
|
✗ |
Force f(bv.toVector()); |
215 |
|
✗ |
Force v2xf = bv2.cross(f); |
216 |
|
✗ |
Motion::ActionMatrixType dualactv2 = bv2.toDualActionMatrix(); |
217 |
|
|
|
218 |
|
✗ |
BOOST_CHECK(v2xf.toVector().isApprox(dualactv2 * f.toVector())); |
219 |
|
✗ |
BOOST_CHECK(dualactv2.isApprox(-actv2.transpose())); |
220 |
|
|
|
221 |
|
|
// Simple test for cross product vxf |
222 |
|
✗ |
Force vxf = bv.cross(f); |
223 |
|
✗ |
BOOST_CHECK(vxf.linear().isApprox(bv.angular().cross(f.linear()))); |
224 |
|
✗ |
BOOST_CHECK_SMALL( |
225 |
|
|
vxf.angular().norm(), |
226 |
|
|
1e-3); // previously ensure that ( vxf.angular().isMuchSmallerThan(1e-3)); |
227 |
|
|
|
228 |
|
|
// Test frame change for vxf |
229 |
|
✗ |
Motion av = Motion::Random(); |
230 |
|
✗ |
Force af = Force::Random(); |
231 |
|
✗ |
bv = amb.actInv(av); |
232 |
|
✗ |
Force bf = amb.actInv(af); |
233 |
|
✗ |
Force avxf = av.cross(af); |
234 |
|
✗ |
Force bvxf = bv.cross(bf); |
235 |
|
✗ |
BOOST_CHECK(avxf.toVector().isApprox(amb.act(bvxf).toVector())); |
236 |
|
|
|
237 |
|
|
// Test frame change for vxv |
238 |
|
✗ |
av = Motion::Random(); |
239 |
|
✗ |
Motion aw = Motion::Random(); |
240 |
|
✗ |
bv = amb.actInv(av); |
241 |
|
✗ |
Motion bw = amb.actInv(aw); |
242 |
|
✗ |
Motion avxw = av.cross(aw); |
243 |
|
✗ |
Motion bvxw = bv.cross(bw); |
244 |
|
✗ |
BOOST_CHECK(avxw.toVector().isApprox(amb.act(bvxw).toVector())); |
245 |
|
|
|
246 |
|
|
// Test isApprox |
247 |
|
✗ |
bv.toVector().setOnes(); |
248 |
|
✗ |
const double eps = 1e-6; |
249 |
|
✗ |
BOOST_CHECK(bv == bv); |
250 |
|
✗ |
BOOST_CHECK(bv.isApprox(bv)); |
251 |
|
✗ |
Motion bv_approx(bv); |
252 |
|
✗ |
bv_approx.linear()[0] += eps; |
253 |
|
✗ |
BOOST_CHECK(bv_approx.isApprox(bv, eps)); |
254 |
|
|
|
255 |
|
|
// Test ref() method |
256 |
|
|
{ |
257 |
|
✗ |
Motion a(Motion::Random()); |
258 |
|
✗ |
BOOST_CHECK(a.ref().isApprox(a)); |
259 |
|
|
|
260 |
|
✗ |
const Motion b(a); |
261 |
|
✗ |
BOOST_CHECK(b.isApprox(a.ref())); |
262 |
|
|
} |
263 |
|
|
|
264 |
|
|
// Test cast |
265 |
|
|
{ |
266 |
|
|
typedef MotionTpl<float> Motionf; |
267 |
|
✗ |
Motion a(Motion::Random()); |
268 |
|
✗ |
Motionf a_float = a.cast<float>(); |
269 |
|
✗ |
BOOST_CHECK(a_float.isApprox(a.cast<float>())); |
270 |
|
|
|
271 |
|
✗ |
Motionf a_float2(a); |
272 |
|
✗ |
BOOST_CHECK(a_float.isApprox(a_float2)); |
273 |
|
|
} |
274 |
|
|
} |
275 |
|
|
|
276 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_motion_ref) |
277 |
|
|
{ |
278 |
|
|
using namespace pinocchio; |
279 |
|
|
typedef Motion::Vector6 Vector6; |
280 |
|
|
|
281 |
|
|
typedef MotionRef<Vector6> MotionV6; |
282 |
|
|
|
283 |
|
✗ |
Motion v_ref(Motion::Random()); |
284 |
|
✗ |
MotionV6 v(v_ref.toVector()); |
285 |
|
|
|
286 |
|
✗ |
BOOST_CHECK(v_ref.isApprox(v)); |
287 |
|
|
|
288 |
|
✗ |
MotionV6::MotionPlain v2(v * 2.); |
289 |
|
✗ |
Motion v2_ref(v_ref * 2.); |
290 |
|
|
|
291 |
|
✗ |
BOOST_CHECK(v2_ref.isApprox(v2)); |
292 |
|
|
|
293 |
|
✗ |
v2 = v_ref + v; |
294 |
|
✗ |
BOOST_CHECK(v2_ref.isApprox(v2)); |
295 |
|
|
|
296 |
|
✗ |
v = v2; |
297 |
|
✗ |
BOOST_CHECK(v2.isApprox(v)); |
298 |
|
|
|
299 |
|
✗ |
v2 = v - v; |
300 |
|
✗ |
BOOST_CHECK(v2.isApprox(Motion::Zero())); |
301 |
|
|
|
302 |
|
✗ |
SE3 M(SE3::Identity()); |
303 |
|
✗ |
v2 = M.act(v); |
304 |
|
✗ |
BOOST_CHECK(v2.isApprox(v)); |
305 |
|
|
|
306 |
|
✗ |
v2 = M.actInv(v); |
307 |
|
✗ |
BOOST_CHECK(v2.isApprox(v)); |
308 |
|
|
|
309 |
|
✗ |
Motion v3(Motion::Random()); |
310 |
|
✗ |
v_ref.setRandom(); |
311 |
|
✗ |
v = v_ref; |
312 |
|
✗ |
v2 = v.cross(v3); |
313 |
|
✗ |
v2_ref = v_ref.cross(v3); |
314 |
|
|
|
315 |
|
✗ |
BOOST_CHECK(v2.isApprox(v2_ref)); |
316 |
|
|
|
317 |
|
✗ |
v.setRandom(); |
318 |
|
✗ |
v.setZero(); |
319 |
|
✗ |
BOOST_CHECK(v.isApprox(Motion::Zero())); |
320 |
|
|
|
321 |
|
|
// Test ref() method |
322 |
|
|
{ |
323 |
|
✗ |
Vector6 v6(Vector6::Random()); |
324 |
|
✗ |
MotionV6 a(v6); |
325 |
|
✗ |
BOOST_CHECK(a.ref().isApprox(a)); |
326 |
|
|
|
327 |
|
✗ |
const Motion b(a); |
328 |
|
✗ |
BOOST_CHECK(b.isApprox(a.ref())); |
329 |
|
|
} |
330 |
|
|
} |
331 |
|
|
|
332 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_motion_zero) |
333 |
|
|
{ |
334 |
|
|
using namespace pinocchio; |
335 |
|
✗ |
Motion v((MotionZero())); |
336 |
|
|
|
337 |
|
✗ |
BOOST_CHECK(v.toVector().isZero()); |
338 |
|
✗ |
BOOST_CHECK(MotionZero() == Motion::Zero()); |
339 |
|
|
|
340 |
|
|
// SE3.act |
341 |
|
✗ |
SE3 m(SE3::Random()); |
342 |
|
✗ |
BOOST_CHECK(m.act(MotionZero()) == Motion::Zero()); |
343 |
|
✗ |
BOOST_CHECK(m.actInv(MotionZero()) == Motion::Zero()); |
344 |
|
|
|
345 |
|
|
// Motion.cross |
346 |
|
✗ |
Motion v2(Motion::Random()); |
347 |
|
✗ |
BOOST_CHECK(v2.cross(MotionZero()) == Motion::Zero()); |
348 |
|
|
} |
349 |
|
|
|
350 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_Force) |
351 |
|
|
{ |
352 |
|
|
using namespace pinocchio; |
353 |
|
|
typedef SE3::ActionMatrixType ActionMatrixType; |
354 |
|
|
typedef Force::Vector6 Vector6; |
355 |
|
|
|
356 |
|
✗ |
SE3 amb = SE3::Random(); |
357 |
|
✗ |
SE3 bmc = SE3::Random(); |
358 |
|
✗ |
SE3 amc = amb * bmc; |
359 |
|
|
|
360 |
|
✗ |
Force bf = Force::Random(); |
361 |
|
✗ |
Force bf2 = Force::Random(); |
362 |
|
|
|
363 |
|
✗ |
Vector6 bf_vec = bf; |
364 |
|
✗ |
Vector6 bf2_vec = bf2; |
365 |
|
|
|
366 |
|
|
// std::stringstream |
367 |
|
✗ |
std::stringstream ss; |
368 |
|
✗ |
ss << bf << std::endl; |
369 |
|
✗ |
BOOST_CHECK(!ss.str().empty()); |
370 |
|
|
|
371 |
|
|
// Test .+. |
372 |
|
✗ |
Vector6 bfPbf2_vec = bf + bf2; |
373 |
|
✗ |
BOOST_CHECK(bfPbf2_vec.isApprox(bf_vec + bf2_vec)); |
374 |
|
|
|
375 |
|
|
// Test -. |
376 |
|
✗ |
Vector6 Mbf_vec = -bf; |
377 |
|
✗ |
BOOST_CHECK(Mbf_vec.isApprox(-bf_vec)); |
378 |
|
|
|
379 |
|
|
// Test .+=. |
380 |
|
✗ |
Force bf3 = bf; |
381 |
|
✗ |
bf3 += bf2; |
382 |
|
✗ |
BOOST_CHECK(bf3.toVector().isApprox(bf_vec + bf2_vec)); |
383 |
|
|
|
384 |
|
|
// Test .= V6 |
385 |
|
✗ |
bf3 = bf2_vec; |
386 |
|
✗ |
BOOST_CHECK(bf3.toVector().isApprox(bf2_vec)); |
387 |
|
|
|
388 |
|
|
// Test constructor from V6 |
389 |
|
✗ |
Force bf4(bf2_vec); |
390 |
|
✗ |
BOOST_CHECK(bf4.toVector().isApprox(bf2_vec)); |
391 |
|
|
|
392 |
|
|
// Test action |
393 |
|
✗ |
ActionMatrixType aXb = amb; |
394 |
|
✗ |
BOOST_CHECK(amb.act(bf).toVector().isApprox(aXb.inverse().transpose() * bf_vec)); |
395 |
|
|
|
396 |
|
|
// Test action inverse |
397 |
|
✗ |
ActionMatrixType bXc = bmc; |
398 |
|
✗ |
BOOST_CHECK(bmc.actInv(bf).toVector().isApprox(bXc.transpose() * bf_vec)); |
399 |
|
|
|
400 |
|
|
// Test double action |
401 |
|
✗ |
Force cf = Force::Random(); |
402 |
|
✗ |
bf = bmc.act(cf); |
403 |
|
✗ |
BOOST_CHECK(amb.act(bf).toVector().isApprox(amc.act(cf).toVector())); |
404 |
|
|
|
405 |
|
|
// Simple test for cross product |
406 |
|
|
// Force vxv = bf.cross(bf); |
407 |
|
|
// ensure that (vxv.toVector().isMuchSmallerThan(bf.toVector())); |
408 |
|
|
|
409 |
|
✗ |
Force f_not_zero(Vector6::Ones()); |
410 |
|
✗ |
BOOST_CHECK(!f_not_zero.isZero()); |
411 |
|
|
|
412 |
|
✗ |
Force f_zero(Vector6::Zero()); |
413 |
|
✗ |
BOOST_CHECK(f_zero.isZero()); |
414 |
|
|
|
415 |
|
|
// Test isApprox |
416 |
|
|
|
417 |
|
✗ |
BOOST_CHECK(bf == bf); |
418 |
|
✗ |
bf.setRandom(); |
419 |
|
✗ |
bf2.setZero(); |
420 |
|
✗ |
BOOST_CHECK(bf == bf); |
421 |
|
✗ |
BOOST_CHECK(bf != bf2); |
422 |
|
✗ |
BOOST_CHECK(bf.isApprox(bf)); |
423 |
|
✗ |
BOOST_CHECK(!bf.isApprox(bf2)); |
424 |
|
|
|
425 |
|
✗ |
const double eps = 1e-6; |
426 |
|
✗ |
Force bf_approx(bf); |
427 |
|
✗ |
bf_approx.linear()[0] += 2. * eps; |
428 |
|
✗ |
BOOST_CHECK(!bf_approx.isApprox(bf, eps)); |
429 |
|
|
|
430 |
|
|
// Test ref() method |
431 |
|
|
{ |
432 |
|
✗ |
Force a(Force::Random()); |
433 |
|
✗ |
BOOST_CHECK(a.ref().isApprox(a)); |
434 |
|
|
|
435 |
|
✗ |
const Force b(a); |
436 |
|
✗ |
BOOST_CHECK(b.isApprox(a.ref())); |
437 |
|
|
} |
438 |
|
|
|
439 |
|
|
// Test cast |
440 |
|
|
{ |
441 |
|
|
typedef ForceTpl<float> Forcef; |
442 |
|
✗ |
Force a(Force::Random()); |
443 |
|
✗ |
Forcef a_float = a.cast<float>(); |
444 |
|
✗ |
BOOST_CHECK(a_float.isApprox(a.cast<float>())); |
445 |
|
|
|
446 |
|
✗ |
Forcef a_float2(a); |
447 |
|
✗ |
BOOST_CHECK(a_float.isApprox(a_float2)); |
448 |
|
|
} |
449 |
|
|
|
450 |
|
|
// Test scalar multiplication |
451 |
|
✗ |
const double alpha = 1.5; |
452 |
|
✗ |
Force b(Force::Random()); |
453 |
|
✗ |
Force alpha_f = alpha * b; |
454 |
|
✗ |
Force f_alpha = b * alpha; |
455 |
|
|
|
456 |
|
✗ |
BOOST_CHECK(alpha_f == f_alpha); |
457 |
|
|
} |
458 |
|
|
|
459 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_force_ref) |
460 |
|
|
{ |
461 |
|
|
using namespace pinocchio; |
462 |
|
|
typedef Force::Vector6 Vector6; |
463 |
|
|
|
464 |
|
|
typedef ForceRef<Vector6> ForceV6; |
465 |
|
|
|
466 |
|
✗ |
Force f_ref(Force::Random()); |
467 |
|
✗ |
ForceV6 f(f_ref.toVector()); |
468 |
|
|
|
469 |
|
✗ |
BOOST_CHECK(f_ref.isApprox(f)); |
470 |
|
|
|
471 |
|
✗ |
ForceV6::ForcePlain f2(f * 2.); |
472 |
|
✗ |
Force f2_ref(f_ref * 2.); |
473 |
|
|
|
474 |
|
✗ |
BOOST_CHECK(f2_ref.isApprox(f2)); |
475 |
|
|
|
476 |
|
✗ |
f2 = f_ref + f; |
477 |
|
✗ |
BOOST_CHECK(f2_ref.isApprox(f2)); |
478 |
|
|
|
479 |
|
✗ |
f = f2; |
480 |
|
✗ |
BOOST_CHECK(f2.isApprox(f)); |
481 |
|
|
|
482 |
|
✗ |
f2 = f - f; |
483 |
|
✗ |
BOOST_CHECK(f2.isApprox(Force::Zero())); |
484 |
|
|
|
485 |
|
✗ |
SE3 M(SE3::Identity()); |
486 |
|
✗ |
f2 = M.act(f); |
487 |
|
✗ |
BOOST_CHECK(f2.isApprox(f)); |
488 |
|
|
|
489 |
|
✗ |
f2 = M.actInv(f); |
490 |
|
✗ |
BOOST_CHECK(f2.isApprox(f)); |
491 |
|
|
|
492 |
|
✗ |
Motion v(Motion::Random()); |
493 |
|
✗ |
f_ref.setRandom(); |
494 |
|
✗ |
f = f_ref; |
495 |
|
✗ |
f2 = v.cross(f); |
496 |
|
✗ |
f2_ref = v.cross(f_ref); |
497 |
|
|
|
498 |
|
✗ |
BOOST_CHECK(f2.isApprox(f2_ref)); |
499 |
|
|
|
500 |
|
✗ |
f.setRandom(); |
501 |
|
✗ |
f.setZero(); |
502 |
|
✗ |
BOOST_CHECK(f.isApprox(Force::Zero())); |
503 |
|
|
|
504 |
|
|
// Test ref() method |
505 |
|
|
{ |
506 |
|
✗ |
Vector6 v6(Vector6::Random()); |
507 |
|
✗ |
ForceV6 a(v6); |
508 |
|
✗ |
BOOST_CHECK(a.ref().isApprox(a)); |
509 |
|
|
|
510 |
|
✗ |
const Force b(a); |
511 |
|
✗ |
BOOST_CHECK(b.isApprox(a.ref())); |
512 |
|
|
} |
513 |
|
|
} |
514 |
|
|
|
515 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_Inertia) |
516 |
|
|
{ |
517 |
|
|
using namespace pinocchio; |
518 |
|
|
typedef Inertia::Matrix6 Matrix6; |
519 |
|
|
|
520 |
|
✗ |
Inertia aI = Inertia::Random(); |
521 |
|
✗ |
Matrix6 matI = aI; |
522 |
|
✗ |
BOOST_CHECK_EQUAL(matI(0, 0), aI.mass()); |
523 |
|
✗ |
BOOST_CHECK_EQUAL(matI(1, 1), aI.mass()); |
524 |
|
✗ |
BOOST_CHECK_EQUAL(matI(2, 2), aI.mass()); // 1,1 before unifying |
525 |
|
|
|
526 |
|
✗ |
BOOST_CHECK_SMALL( |
527 |
|
|
(matI - matI.transpose()).norm(), |
528 |
|
|
matI.norm()); // previously ensure that( (matI-matI.transpose()).isMuchSmallerThan(matI) ); |
529 |
|
✗ |
BOOST_CHECK_SMALL( |
530 |
|
|
(matI.topRightCorner<3, 3>() * aI.lever()).norm(), |
531 |
|
|
aI.lever().norm()); // previously ensure that( |
532 |
|
|
// (matI.topRightCorner<3,3>()*aI.lever()).isMuchSmallerThan(aI.lever()) ); |
533 |
|
|
|
534 |
|
✗ |
Inertia I1 = Inertia::Identity(); |
535 |
|
✗ |
BOOST_CHECK(I1.matrix().isApprox(Matrix6::Identity())); |
536 |
|
|
|
537 |
|
|
// Test motion-to-force map |
538 |
|
✗ |
Motion v = Motion::Random(); |
539 |
|
✗ |
Force f = I1 * v; |
540 |
|
✗ |
BOOST_CHECK(f.toVector().isApprox(v.toVector())); |
541 |
|
|
|
542 |
|
|
// Test Inertia group application |
543 |
|
✗ |
SE3 bma = SE3::Random(); |
544 |
|
✗ |
Inertia bI = bma.act(aI); |
545 |
|
✗ |
Matrix6 bXa = bma; |
546 |
|
✗ |
BOOST_CHECK((bma.rotation() * aI.inertia().matrix() * bma.rotation().transpose()) |
547 |
|
|
.isApprox(bI.inertia().matrix())); |
548 |
|
✗ |
BOOST_CHECK((bXa.transpose().inverse() * aI.matrix() * bXa.inverse()).isApprox(bI.matrix())); |
549 |
|
|
|
550 |
|
|
// Test inverse action |
551 |
|
✗ |
BOOST_CHECK((bXa.transpose() * bI.matrix() * bXa).isApprox(bma.actInv(bI).matrix())); |
552 |
|
|
|
553 |
|
|
// Test vxIv cross product |
554 |
|
✗ |
v = Motion::Random(); |
555 |
|
✗ |
f = aI * v; |
556 |
|
✗ |
Force vxf = v.cross(f); |
557 |
|
✗ |
Force vxIv = aI.vxiv(v); |
558 |
|
✗ |
BOOST_CHECK(vxf.toVector().isApprox(vxIv.toVector())); |
559 |
|
|
|
560 |
|
|
// Test operator+ |
561 |
|
✗ |
I1 = Inertia::Random(); |
562 |
|
✗ |
Inertia I2 = Inertia::Random(); |
563 |
|
✗ |
BOOST_CHECK((I1.matrix() + I2.matrix()).isApprox((I1 + I2).matrix())); |
564 |
|
|
|
565 |
|
|
// Test operator- |
566 |
|
|
{ |
567 |
|
✗ |
I1 = Inertia::Random(); |
568 |
|
✗ |
Inertia I2 = Inertia::Random(); |
569 |
|
✗ |
Inertia Isum = I1 + I2; |
570 |
|
✗ |
Inertia I1_other = Isum - I2; |
571 |
|
✗ |
BOOST_CHECK(I1_other.matrix().isApprox(I1.matrix())); |
572 |
|
|
} |
573 |
|
|
|
574 |
|
|
// Test operator += |
575 |
|
✗ |
Inertia I12 = I1; |
576 |
|
✗ |
I12 += I2; |
577 |
|
✗ |
BOOST_CHECK((I1.matrix() + I2.matrix()).isApprox(I12.matrix())); |
578 |
|
|
|
579 |
|
|
// Test operator -= |
580 |
|
|
{ |
581 |
|
✗ |
Inertia I2 = Inertia::Random(); |
582 |
|
✗ |
Inertia Isum = I1 + I2; |
583 |
|
|
|
584 |
|
✗ |
Inertia I1_other = Isum; |
585 |
|
✗ |
I1_other -= I2; |
586 |
|
✗ |
BOOST_CHECK((I1_other.matrix()).isApprox(I1.matrix())); |
587 |
|
|
} |
588 |
|
|
|
589 |
|
|
// Test operator vtiv |
590 |
|
✗ |
double kinetic_ref = v.toVector().transpose() * aI.matrix() * v.toVector(); |
591 |
|
✗ |
double kinetic = aI.vtiv(v); |
592 |
|
✗ |
BOOST_CHECK_SMALL(kinetic_ref - kinetic, 1e-12); |
593 |
|
|
|
594 |
|
|
// Test constructor (Matrix6) |
595 |
|
✗ |
Inertia I1_bis(I1.matrix()); |
596 |
|
✗ |
BOOST_CHECK(I1.matrix().isApprox(I1_bis.matrix())); |
597 |
|
|
|
598 |
|
|
// Test Inertia from ellipsoid |
599 |
|
✗ |
const double sphere_mass = 5.; |
600 |
|
✗ |
const double sphere_radius = 2.; |
601 |
|
✗ |
I1 = Inertia::FromSphere(sphere_mass, sphere_radius); |
602 |
|
✗ |
const double L_sphere = 2. / 5. * sphere_mass * sphere_radius * sphere_radius; |
603 |
|
✗ |
BOOST_CHECK_SMALL(I1.mass() - sphere_mass, 1e-12); |
604 |
|
✗ |
BOOST_CHECK(I1.lever().isZero()); |
605 |
|
✗ |
BOOST_CHECK( |
606 |
|
|
I1.inertia().matrix().isApprox(Symmetric3(L_sphere, 0., L_sphere, 0., 0., L_sphere).matrix())); |
607 |
|
|
|
608 |
|
|
// Test Inertia from ellipsoid |
609 |
|
✗ |
I1 = Inertia::FromEllipsoid(2., 3., 4., 5.); |
610 |
|
✗ |
BOOST_CHECK_SMALL(I1.mass() - 2, 1e-12); |
611 |
|
✗ |
BOOST_CHECK_SMALL(I1.lever().norm(), 1e-12); |
612 |
|
✗ |
BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3(16.4, 0., 13.6, 0., 0., 10.).matrix())); |
613 |
|
|
|
614 |
|
|
// Test Inertia from Cylinder |
615 |
|
✗ |
I1 = Inertia::FromCylinder(2., 4., 6.); |
616 |
|
✗ |
BOOST_CHECK_SMALL(I1.mass() - 2, 1e-12); |
617 |
|
✗ |
BOOST_CHECK_SMALL(I1.lever().norm(), 1e-12); |
618 |
|
✗ |
BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3(14., 0., 14., 0., 0., 16.).matrix())); |
619 |
|
|
|
620 |
|
|
// Test Inertia from Box |
621 |
|
✗ |
I1 = Inertia::FromBox(2., 6., 12., 18.); |
622 |
|
✗ |
BOOST_CHECK_SMALL(I1.mass() - 2, 1e-12); |
623 |
|
✗ |
BOOST_CHECK_SMALL(I1.lever().norm(), 1e-12); |
624 |
|
✗ |
BOOST_CHECK(I1.inertia().matrix().isApprox(Symmetric3(78., 0., 60., 0., 0., 30.).matrix())); |
625 |
|
|
|
626 |
|
|
// Test Inertia From Capsule |
627 |
|
✗ |
I1 = Inertia::FromCapsule(1., 2., 3); |
628 |
|
✗ |
BOOST_CHECK_SMALL(I1.mass() - 1, 1e-12); |
629 |
|
✗ |
BOOST_CHECK_SMALL(I1.lever().norm(), 1e-12); |
630 |
|
✗ |
BOOST_CHECK(I1.inertia().matrix().isApprox( |
631 |
|
|
Symmetric3(3.79705882, 0., 3.79705882, 0., 0., 1.81176471).matrix(), |
632 |
|
|
1e-5)); // Computed with hppfcl::Capsule |
633 |
|
|
|
634 |
|
|
// Copy operator |
635 |
|
✗ |
Inertia aI_copy(aI); |
636 |
|
✗ |
BOOST_CHECK(aI_copy == aI); |
637 |
|
|
|
638 |
|
|
// Test isZero |
639 |
|
✗ |
Inertia I_not_zero = Inertia::Identity(); |
640 |
|
✗ |
BOOST_CHECK(!I_not_zero.isZero()); |
641 |
|
|
|
642 |
|
✗ |
Inertia I_zero = Inertia::Zero(); |
643 |
|
✗ |
BOOST_CHECK(I_zero.isZero()); |
644 |
|
|
|
645 |
|
|
// Test isApprox |
646 |
|
✗ |
const double eps = 1e-6; |
647 |
|
✗ |
BOOST_CHECK(aI == aI); |
648 |
|
✗ |
BOOST_CHECK(aI.isApprox(aI)); |
649 |
|
✗ |
Inertia aI_approx(aI); |
650 |
|
✗ |
aI_approx.mass() += eps / 2.; |
651 |
|
✗ |
BOOST_CHECK(aI_approx.isApprox(aI, eps)); |
652 |
|
|
|
653 |
|
|
// Test Variation |
654 |
|
✗ |
Inertia::Matrix6 aIvariation = aI.variation(v); |
655 |
|
|
|
656 |
|
✗ |
Motion::ActionMatrixType vAction = v.toActionMatrix(); |
657 |
|
✗ |
Motion::ActionMatrixType vDualAction = v.toDualActionMatrix(); |
658 |
|
|
|
659 |
|
✗ |
Inertia::Matrix6 aImatrix = aI.matrix(); |
660 |
|
✗ |
Inertia::Matrix6 aIvariation_ref = vDualAction * aImatrix - aImatrix * vAction; |
661 |
|
|
|
662 |
|
✗ |
BOOST_CHECK(aIvariation.isApprox(aIvariation_ref)); |
663 |
|
✗ |
BOOST_CHECK(vxIv.isApprox(Force(aIvariation * v.toVector()))); |
664 |
|
|
|
665 |
|
|
// Test vxI operator |
666 |
|
|
{ |
667 |
|
|
typedef Inertia::Matrix6 Matrix6; |
668 |
|
✗ |
Inertia I(Inertia::Random()); |
669 |
|
✗ |
Motion v(Motion::Random()); |
670 |
|
|
|
671 |
|
✗ |
const Matrix6 M_ref(v.toDualActionMatrix() * I.matrix()); |
672 |
|
✗ |
Matrix6 M; |
673 |
|
✗ |
Inertia::vxi(v, I, M); |
674 |
|
|
|
675 |
|
✗ |
BOOST_CHECK(M.isApprox(M_ref)); |
676 |
|
✗ |
BOOST_CHECK(I.vxi(v).isApprox(M_ref)); |
677 |
|
|
} |
678 |
|
|
|
679 |
|
|
// Test Ivx operator |
680 |
|
|
{ |
681 |
|
|
typedef Inertia::Matrix6 Matrix6; |
682 |
|
✗ |
Inertia I(Inertia::Random()); |
683 |
|
✗ |
Motion v(Motion::Random()); |
684 |
|
|
|
685 |
|
✗ |
const Matrix6 M_ref(I.matrix() * v.toActionMatrix()); |
686 |
|
✗ |
Matrix6 M; |
687 |
|
✗ |
Inertia::ivx(v, I, M); |
688 |
|
|
|
689 |
|
✗ |
BOOST_CHECK(M.isApprox(M_ref)); |
690 |
|
✗ |
BOOST_CHECK(I.ivx(v).isApprox(M_ref)); |
691 |
|
|
} |
692 |
|
|
|
693 |
|
|
// Test variation against vxI - Ivx operator |
694 |
|
|
{ |
695 |
|
|
typedef Inertia::Matrix6 Matrix6; |
696 |
|
✗ |
Inertia I(Inertia::Random()); |
697 |
|
✗ |
Motion v(Motion::Random()); |
698 |
|
|
|
699 |
|
✗ |
Matrix6 Ivariation = I.variation(v); |
700 |
|
|
|
701 |
|
✗ |
Matrix6 M1; |
702 |
|
✗ |
Inertia::vxi(v, I, M1); |
703 |
|
✗ |
Matrix6 M2; |
704 |
|
✗ |
Inertia::ivx(v, I, M2); |
705 |
|
✗ |
Matrix6 M3(M1 - M2); |
706 |
|
|
|
707 |
|
✗ |
BOOST_CHECK(M3.isApprox(Ivariation)); |
708 |
|
|
} |
709 |
|
|
|
710 |
|
|
// Test inverse |
711 |
|
|
{ |
712 |
|
✗ |
Inertia I(Inertia::Random()); |
713 |
|
✗ |
Inertia::Matrix6 I_inv = I.inverse(); |
714 |
|
|
|
715 |
|
✗ |
BOOST_CHECK(I_inv.isApprox(I.matrix().inverse())); |
716 |
|
|
} |
717 |
|
|
|
718 |
|
|
// Test dynamic parameters |
719 |
|
|
{ |
720 |
|
✗ |
Inertia I(Inertia::Random()); |
721 |
|
|
|
722 |
|
✗ |
Inertia::Vector10 v = I.toDynamicParameters(); |
723 |
|
|
|
724 |
|
✗ |
BOOST_CHECK_CLOSE(v[0], I.mass(), 1e-12); |
725 |
|
|
|
726 |
|
✗ |
BOOST_CHECK(v.segment<3>(1).isApprox(I.mass() * I.lever())); |
727 |
|
|
|
728 |
|
✗ |
Eigen::Matrix3d I_o = I.inertia() + I.mass() * skew(I.lever()).transpose() * skew(I.lever()); |
729 |
|
✗ |
Eigen::Matrix3d I_ov; |
730 |
|
✗ |
I_ov << v[4], v[5], v[7], v[5], v[6], v[8], v[7], v[8], v[9]; |
731 |
|
|
|
732 |
|
✗ |
BOOST_CHECK(I_o.isApprox(I_ov)); |
733 |
|
|
|
734 |
|
✗ |
Inertia I2 = Inertia::FromDynamicParameters(v); |
735 |
|
✗ |
BOOST_CHECK(I2.isApprox(I)); |
736 |
|
|
} |
737 |
|
|
|
738 |
|
|
// Test disp |
739 |
|
✗ |
std::cout << aI << std::endl; |
740 |
|
|
} |
741 |
|
|
|
742 |
|
✗ |
BOOST_AUTO_TEST_CASE(cast_inertia) |
743 |
|
|
{ |
744 |
|
|
using namespace pinocchio; |
745 |
|
✗ |
Inertia Y(Inertia::Random()); |
746 |
|
|
|
747 |
|
|
typedef InertiaTpl<long double> Inertiald; |
748 |
|
|
|
749 |
|
✗ |
BOOST_CHECK(Y.cast<double>() == Y); |
750 |
|
✗ |
BOOST_CHECK(Y.cast<long double>().cast<double>() == Y); |
751 |
|
|
|
752 |
|
✗ |
Inertiald Y2(Y); |
753 |
|
✗ |
BOOST_CHECK(Y2.isApprox(Y.cast<long double>())); |
754 |
|
|
} |
755 |
|
|
|
756 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_ActOnSet) |
757 |
|
|
{ |
758 |
|
|
using namespace pinocchio; |
759 |
|
✗ |
const int N = 20; |
760 |
|
|
typedef Eigen::Matrix<double, 6, N> Matrix6N; |
761 |
|
✗ |
SE3 jMi = SE3::Random(); |
762 |
|
✗ |
Motion v = Motion::Random(); |
763 |
|
|
|
764 |
|
|
// Forcet SET |
765 |
|
|
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
766 |
|
|
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
767 |
|
✗ |
Matrix6N iF = Matrix6N::Random(), jF, jFinv, jF_ref, jFinv_ref; |
768 |
|
|
|
769 |
|
|
// forceSet::se3Action |
770 |
|
✗ |
forceSet::se3Action(jMi, iF, jF); |
771 |
|
✗ |
for (int k = 0; k < N; ++k) |
772 |
|
✗ |
BOOST_CHECK(jMi.act(Force(iF.col(k))).toVector().isApprox(jF.col(k))); |
773 |
|
|
|
774 |
|
✗ |
jF_ref = jMi.toDualActionMatrix() * iF; |
775 |
|
✗ |
BOOST_CHECK(jF_ref.isApprox(jF)); |
776 |
|
|
|
777 |
|
✗ |
forceSet::se3ActionInverse(jMi.inverse(), iF, jFinv); |
778 |
|
✗ |
BOOST_CHECK(jFinv.isApprox(jF)); |
779 |
|
|
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
780 |
|
|
|
781 |
|
✗ |
Matrix6N iF2 = Matrix6N::Random(); |
782 |
|
✗ |
jF_ref += jMi.toDualActionMatrix() * iF2; |
783 |
|
|
|
784 |
|
✗ |
forceSet::se3Action<ADDTO>(jMi, iF2, jF); |
785 |
|
✗ |
BOOST_CHECK(jF.isApprox(jF_ref)); |
786 |
|
|
|
787 |
|
✗ |
Matrix6N iF3 = Matrix6N::Random(); |
788 |
|
✗ |
jF_ref -= jMi.toDualActionMatrix() * iF3; |
789 |
|
|
|
790 |
|
✗ |
forceSet::se3Action<RMTO>(jMi, iF3, jF); |
791 |
|
✗ |
BOOST_CHECK(jF.isApprox(jF_ref)); |
792 |
|
|
|
793 |
|
|
// forceSet::se3ActionInverse |
794 |
|
✗ |
forceSet::se3ActionInverse(jMi, iF, jFinv); |
795 |
|
✗ |
jFinv_ref = jMi.inverse().toDualActionMatrix() * iF; |
796 |
|
✗ |
BOOST_CHECK(jFinv_ref.isApprox(jFinv)); |
797 |
|
|
|
798 |
|
✗ |
jFinv_ref += jMi.inverse().toDualActionMatrix() * iF2; |
799 |
|
✗ |
forceSet::se3ActionInverse<ADDTO>(jMi, iF2, jFinv); |
800 |
|
✗ |
BOOST_CHECK(jFinv.isApprox(jFinv_ref)); |
801 |
|
|
|
802 |
|
✗ |
jFinv_ref -= jMi.inverse().toDualActionMatrix() * iF3; |
803 |
|
✗ |
forceSet::se3ActionInverse<RMTO>(jMi, iF3, jFinv); |
804 |
|
✗ |
BOOST_CHECK(jFinv.isApprox(jFinv_ref)); |
805 |
|
|
|
806 |
|
|
// forceSet::motionAction |
807 |
|
✗ |
forceSet::motionAction(v, iF, jF); |
808 |
|
✗ |
for (int k = 0; k < N; ++k) |
809 |
|
✗ |
BOOST_CHECK(v.cross(Force(iF.col(k))).toVector().isApprox(jF.col(k))); |
810 |
|
|
|
811 |
|
✗ |
jF_ref = v.toDualActionMatrix() * iF; |
812 |
|
✗ |
BOOST_CHECK(jF.isApprox(jF_ref)); |
813 |
|
|
|
814 |
|
✗ |
jF_ref += v.toDualActionMatrix() * iF2; |
815 |
|
✗ |
forceSet::motionAction<ADDTO>(v, iF2, jF); |
816 |
|
✗ |
BOOST_CHECK(jF.isApprox(jF_ref)); |
817 |
|
|
|
818 |
|
✗ |
jF_ref -= v.toDualActionMatrix() * iF3; |
819 |
|
✗ |
forceSet::motionAction<RMTO>(v, iF3, jF); |
820 |
|
✗ |
BOOST_CHECK(jF.isApprox(jF_ref)); |
821 |
|
|
|
822 |
|
|
// Motion SET |
823 |
|
|
PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH |
824 |
|
|
PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED |
825 |
|
✗ |
Matrix6N iV = Matrix6N::Random(), jV, jV_ref, jVinv, jVinv_ref; |
826 |
|
|
|
827 |
|
|
// motionSet::se3Action |
828 |
|
✗ |
motionSet::se3Action(jMi, iV, jV); |
829 |
|
✗ |
for (int k = 0; k < N; ++k) |
830 |
|
✗ |
BOOST_CHECK(jMi.act(Motion(iV.col(k))).toVector().isApprox(jV.col(k))); |
831 |
|
|
|
832 |
|
✗ |
jV_ref = jMi.toActionMatrix() * iV; |
833 |
|
✗ |
BOOST_CHECK(jV.isApprox(jV_ref)); |
834 |
|
|
|
835 |
|
✗ |
motionSet::se3ActionInverse(jMi.inverse(), iV, jVinv); |
836 |
|
✗ |
BOOST_CHECK(jVinv.isApprox(jV)); |
837 |
|
|
PINOCCHIO_COMPILER_DIAGNOSTIC_POP |
838 |
|
|
|
839 |
|
✗ |
Matrix6N iV2 = Matrix6N::Random(); |
840 |
|
✗ |
jV_ref += jMi.toActionMatrix() * iV2; |
841 |
|
✗ |
motionSet::se3Action<ADDTO>(jMi, iV2, jV); |
842 |
|
✗ |
BOOST_CHECK(jV.isApprox(jV_ref)); |
843 |
|
|
|
844 |
|
✗ |
Matrix6N iV3 = Matrix6N::Random(); |
845 |
|
✗ |
jV_ref -= jMi.toActionMatrix() * iV3; |
846 |
|
✗ |
motionSet::se3Action<RMTO>(jMi, iV3, jV); |
847 |
|
✗ |
BOOST_CHECK(jV.isApprox(jV_ref)); |
848 |
|
|
|
849 |
|
|
// motionSet::se3ActionInverse |
850 |
|
✗ |
motionSet::se3ActionInverse(jMi, iV, jVinv); |
851 |
|
✗ |
jVinv_ref = jMi.inverse().toActionMatrix() * iV; |
852 |
|
✗ |
BOOST_CHECK(jVinv.isApprox(jVinv_ref)); |
853 |
|
|
|
854 |
|
✗ |
jVinv_ref += jMi.inverse().toActionMatrix() * iV2; |
855 |
|
✗ |
motionSet::se3ActionInverse<ADDTO>(jMi, iV2, jVinv); |
856 |
|
✗ |
BOOST_CHECK(jVinv.isApprox(jVinv_ref)); |
857 |
|
|
|
858 |
|
✗ |
jVinv_ref -= jMi.inverse().toActionMatrix() * iV3; |
859 |
|
✗ |
motionSet::se3ActionInverse<RMTO>(jMi, iV3, jVinv); |
860 |
|
✗ |
BOOST_CHECK(jVinv.isApprox(jVinv_ref)); |
861 |
|
|
|
862 |
|
|
// motionSet::motionAction |
863 |
|
✗ |
motionSet::motionAction(v, iV, jV); |
864 |
|
✗ |
for (int k = 0; k < N; ++k) |
865 |
|
✗ |
BOOST_CHECK(v.cross(Motion(iV.col(k))).toVector().isApprox(jV.col(k))); |
866 |
|
|
|
867 |
|
✗ |
jV_ref = v.toActionMatrix() * iV; |
868 |
|
✗ |
BOOST_CHECK(jV.isApprox(jV_ref)); |
869 |
|
|
|
870 |
|
✗ |
jV_ref += v.toActionMatrix() * iV2; |
871 |
|
✗ |
motionSet::motionAction<ADDTO>(v, iV2, jV); |
872 |
|
✗ |
BOOST_CHECK(jV.isApprox(jV_ref)); |
873 |
|
|
|
874 |
|
✗ |
jV_ref -= v.toActionMatrix() * iV3; |
875 |
|
✗ |
motionSet::motionAction<RMTO>(v, iV3, jV); |
876 |
|
✗ |
BOOST_CHECK(jV.isApprox(jV_ref)); |
877 |
|
|
|
878 |
|
|
// motionSet::inertiaAction |
879 |
|
✗ |
const Inertia I(Inertia::Random()); |
880 |
|
✗ |
motionSet::inertiaAction(I, iV, jV); |
881 |
|
✗ |
for (int k = 0; k < N; ++k) |
882 |
|
✗ |
BOOST_CHECK((I * (Motion(iV.col(k)))).toVector().isApprox(jV.col(k))); |
883 |
|
|
|
884 |
|
✗ |
jV_ref = I.matrix() * iV; |
885 |
|
✗ |
BOOST_CHECK(jV.isApprox(jV_ref)); |
886 |
|
|
|
887 |
|
✗ |
jV_ref += I.matrix() * iV2; |
888 |
|
✗ |
motionSet::inertiaAction<ADDTO>(I, iV2, jV); |
889 |
|
✗ |
BOOST_CHECK(jV.isApprox(jV_ref)); |
890 |
|
|
|
891 |
|
✗ |
jV_ref -= I.matrix() * iV3; |
892 |
|
✗ |
motionSet::inertiaAction<RMTO>(I, iV3, jV); |
893 |
|
✗ |
BOOST_CHECK(jV.isApprox(jV_ref)); |
894 |
|
|
|
895 |
|
|
// motionSet::act |
896 |
|
✗ |
Force f = Force::Random(); |
897 |
|
✗ |
motionSet::act(iV, f, jF); |
898 |
|
✗ |
for (int k = 0; k < N; ++k) |
899 |
|
✗ |
BOOST_CHECK(Motion(iV.col(k)).cross(f).toVector().isApprox(jF.col(k))); |
900 |
|
|
|
901 |
|
✗ |
for (int k = 0; k < N; ++k) |
902 |
|
✗ |
jF_ref.col(k) = Force(Motion(iV.col(k)).cross(f)).toVector(); |
903 |
|
✗ |
BOOST_CHECK(jF.isApprox(jF_ref)); |
904 |
|
|
|
905 |
|
✗ |
for (int k = 0; k < N; ++k) |
906 |
|
✗ |
jF_ref.col(k) += Force(Motion(iV2.col(k)).cross(f)).toVector(); |
907 |
|
✗ |
motionSet::act<ADDTO>(iV2, f, jF); |
908 |
|
✗ |
BOOST_CHECK(jF.isApprox(jF_ref)); |
909 |
|
|
|
910 |
|
✗ |
for (int k = 0; k < N; ++k) |
911 |
|
✗ |
jF_ref.col(k) -= Force(Motion(iV3.col(k)).cross(f)).toVector(); |
912 |
|
✗ |
motionSet::act<RMTO>(iV3, f, jF); |
913 |
|
✗ |
BOOST_CHECK(jF.isApprox(jF_ref)); |
914 |
|
|
} |
915 |
|
|
|
916 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_skew) |
917 |
|
|
{ |
918 |
|
|
using namespace pinocchio; |
919 |
|
|
typedef SE3::Vector3 Vector3; |
920 |
|
|
typedef Motion::Vector6 Vector6; |
921 |
|
|
|
922 |
|
✗ |
Vector3 v3(Vector3::Random()); |
923 |
|
✗ |
Vector6 v6(Vector6::Random()); |
924 |
|
|
|
925 |
|
✗ |
Vector3 res1 = unSkew(skew(v3)); |
926 |
|
✗ |
BOOST_CHECK(res1.isApprox(v3)); |
927 |
|
|
|
928 |
|
✗ |
Vector3 res2 = unSkew(skew(v6.head<3>())); |
929 |
|
✗ |
BOOST_CHECK(res2.isApprox(v6.head<3>())); |
930 |
|
|
|
931 |
|
✗ |
Vector3 res3 = skew(v3) * v3; |
932 |
|
✗ |
BOOST_CHECK(res3.isZero()); |
933 |
|
|
|
934 |
|
✗ |
Vector3 rhs(Vector3::Random()); |
935 |
|
✗ |
Vector3 res41 = skew(v3) * rhs; |
936 |
|
✗ |
Vector3 res42 = v3.cross(rhs); |
937 |
|
|
|
938 |
|
✗ |
BOOST_CHECK(res41.isApprox(res42)); |
939 |
|
|
} |
940 |
|
|
|
941 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_addSkew) |
942 |
|
|
{ |
943 |
|
|
using namespace pinocchio; |
944 |
|
|
typedef SE3::Vector3 Vector3; |
945 |
|
|
typedef SE3::Matrix3 Matrix3; |
946 |
|
|
|
947 |
|
✗ |
Vector3 v(Vector3::Random()); |
948 |
|
✗ |
Matrix3 M(Matrix3::Random()); |
949 |
|
✗ |
Matrix3 Mcopy(M); |
950 |
|
|
|
951 |
|
✗ |
addSkew(v, M); |
952 |
|
✗ |
Matrix3 Mref = Mcopy + skew(v); |
953 |
|
✗ |
BOOST_CHECK(M.isApprox(Mref)); |
954 |
|
|
|
955 |
|
✗ |
Mref += skew(-v); |
956 |
|
✗ |
addSkew(-v, M); |
957 |
|
✗ |
BOOST_CHECK(M.isApprox(Mcopy)); |
958 |
|
|
|
959 |
|
✗ |
M.setZero(); |
960 |
|
✗ |
addSkew(v, M); |
961 |
|
✗ |
BOOST_CHECK(M.isApprox(skew(v))); |
962 |
|
|
} |
963 |
|
|
|
964 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_skew_square) |
965 |
|
|
{ |
966 |
|
|
using namespace pinocchio; |
967 |
|
|
typedef SE3::Vector3 Vector3; |
968 |
|
|
typedef SE3::Matrix3 Matrix3; |
969 |
|
|
|
970 |
|
✗ |
Vector3 u(Vector3::Random()); |
971 |
|
✗ |
Vector3 v(Vector3::Random()); |
972 |
|
|
|
973 |
|
✗ |
Matrix3 ref = skew(u) * skew(v); |
974 |
|
|
|
975 |
|
✗ |
Matrix3 res = skewSquare(u, v); |
976 |
|
|
|
977 |
|
✗ |
BOOST_CHECK(res.isApprox(ref)); |
978 |
|
|
} |
979 |
|
|
|
980 |
|
|
template<int axis> |
981 |
|
|
struct test_scalar_multiplication_cartesian_axis |
982 |
|
|
{ |
983 |
|
|
typedef pinocchio::CartesianAxis<axis> Axis; |
984 |
|
|
typedef double Scalar; |
985 |
|
|
typedef Eigen::Matrix<Scalar, 3, 1> Vector3; |
986 |
|
|
|
987 |
|
✗ |
static void run() |
988 |
|
|
{ |
989 |
|
✗ |
const Scalar alpha = static_cast<Scalar>(rand()) / static_cast<Scalar>(RAND_MAX); |
990 |
|
✗ |
const Vector3 r1 = Axis() * alpha; |
991 |
|
✗ |
const Vector3 r2 = alpha * Axis(); |
992 |
|
|
|
993 |
|
✗ |
BOOST_CHECK(r1.isApprox(r2)); |
994 |
|
|
|
995 |
|
✗ |
for (int k = 0; k < Axis::dim; ++k) |
996 |
|
|
{ |
997 |
|
✗ |
if (k == axis) |
998 |
|
|
{ |
999 |
|
✗ |
BOOST_CHECK(r1[k] == alpha); |
1000 |
|
✗ |
BOOST_CHECK(r2[k] == alpha); |
1001 |
|
|
} |
1002 |
|
|
else |
1003 |
|
|
{ |
1004 |
|
✗ |
BOOST_CHECK(r1[k] == Scalar(0)); |
1005 |
|
✗ |
BOOST_CHECK(r2[k] == Scalar(0)); |
1006 |
|
|
} |
1007 |
|
|
} |
1008 |
|
|
} |
1009 |
|
|
}; |
1010 |
|
|
|
1011 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_cartesian_axis) |
1012 |
|
|
{ |
1013 |
|
|
using namespace Eigen; |
1014 |
|
|
using namespace pinocchio; |
1015 |
|
✗ |
Vector3d v(Vector3d::Random()); |
1016 |
|
✗ |
const double alpha = 3; |
1017 |
|
✗ |
Vector3d v2(alpha * v); |
1018 |
|
|
|
1019 |
|
✗ |
BOOST_CHECK(XAxis::cross(v).isApprox(Vector3d::Unit(0).cross(v))); |
1020 |
|
✗ |
BOOST_CHECK(YAxis::cross(v).isApprox(Vector3d::Unit(1).cross(v))); |
1021 |
|
✗ |
BOOST_CHECK(ZAxis::cross(v).isApprox(Vector3d::Unit(2).cross(v))); |
1022 |
|
✗ |
BOOST_CHECK(XAxis::alphaCross(alpha, v).isApprox(Vector3d::Unit(0).cross(v2))); |
1023 |
|
✗ |
BOOST_CHECK(YAxis::alphaCross(alpha, v).isApprox(Vector3d::Unit(1).cross(v2))); |
1024 |
|
✗ |
BOOST_CHECK(ZAxis::alphaCross(alpha, v).isApprox(Vector3d::Unit(2).cross(v2))); |
1025 |
|
|
|
1026 |
|
✗ |
test_scalar_multiplication_cartesian_axis<0>::run(); |
1027 |
|
✗ |
test_scalar_multiplication_cartesian_axis<1>::run(); |
1028 |
|
✗ |
test_scalar_multiplication_cartesian_axis<2>::run(); |
1029 |
|
|
|
1030 |
|
|
// test Vector value |
1031 |
|
✗ |
BOOST_CHECK(XAxis::vector() == Vector3d::UnitX()); |
1032 |
|
✗ |
BOOST_CHECK(YAxis::vector() == Vector3d::UnitY()); |
1033 |
|
✗ |
BOOST_CHECK(ZAxis::vector() == Vector3d::UnitZ()); |
1034 |
|
|
} |
1035 |
|
|
|
1036 |
|
|
template<int axis> |
1037 |
|
|
struct test_scalar_multiplication |
1038 |
|
|
{ |
1039 |
|
|
typedef pinocchio::SpatialAxis<axis> Axis; |
1040 |
|
|
typedef double Scalar; |
1041 |
|
|
typedef pinocchio::MotionTpl<Scalar> Motion; |
1042 |
|
|
|
1043 |
|
✗ |
static void run() |
1044 |
|
|
{ |
1045 |
|
✗ |
const Scalar alpha = static_cast<Scalar>(rand()) / static_cast<Scalar>(RAND_MAX); |
1046 |
|
✗ |
const Motion r1 = Axis() * alpha; |
1047 |
|
✗ |
const Motion r2 = alpha * Axis(); |
1048 |
|
|
|
1049 |
|
✗ |
BOOST_CHECK(r1.isApprox(r2)); |
1050 |
|
|
|
1051 |
|
✗ |
for (int k = 0; k < Axis::dim; ++k) |
1052 |
|
|
{ |
1053 |
|
✗ |
if (k == axis) |
1054 |
|
|
{ |
1055 |
|
✗ |
BOOST_CHECK(r1.toVector()[k] == alpha); |
1056 |
|
✗ |
BOOST_CHECK(r2.toVector()[k] == alpha); |
1057 |
|
|
} |
1058 |
|
|
else |
1059 |
|
|
{ |
1060 |
|
✗ |
BOOST_CHECK(r1.toVector()[k] == Scalar(0)); |
1061 |
|
✗ |
BOOST_CHECK(r2.toVector()[k] == Scalar(0)); |
1062 |
|
|
} |
1063 |
|
|
} |
1064 |
|
|
} |
1065 |
|
|
}; |
1066 |
|
|
|
1067 |
|
✗ |
BOOST_AUTO_TEST_CASE(test_spatial_axis) |
1068 |
|
|
{ |
1069 |
|
|
using namespace pinocchio; |
1070 |
|
|
|
1071 |
|
✗ |
Motion v(Motion::Random()); |
1072 |
|
✗ |
Force f(Force::Random()); |
1073 |
|
|
|
1074 |
|
✗ |
Motion vaxis; |
1075 |
|
✗ |
vaxis << AxisVX(); |
1076 |
|
✗ |
BOOST_CHECK(AxisVX::cross(v).isApprox(vaxis.cross(v))); |
1077 |
|
✗ |
BOOST_CHECK(v.cross(AxisVX()).isApprox(v.cross(vaxis))); |
1078 |
|
✗ |
BOOST_CHECK(AxisVX::cross(f).isApprox(vaxis.cross(f))); |
1079 |
|
|
|
1080 |
|
✗ |
vaxis << AxisVY(); |
1081 |
|
✗ |
BOOST_CHECK(AxisVY::cross(v).isApprox(vaxis.cross(v))); |
1082 |
|
✗ |
BOOST_CHECK(v.cross(AxisVY()).isApprox(v.cross(vaxis))); |
1083 |
|
✗ |
BOOST_CHECK(AxisVY::cross(f).isApprox(vaxis.cross(f))); |
1084 |
|
|
|
1085 |
|
✗ |
vaxis << AxisVZ(); |
1086 |
|
✗ |
BOOST_CHECK(AxisVZ::cross(v).isApprox(vaxis.cross(v))); |
1087 |
|
✗ |
BOOST_CHECK(v.cross(AxisVZ()).isApprox(v.cross(vaxis))); |
1088 |
|
✗ |
BOOST_CHECK(AxisVZ::cross(f).isApprox(vaxis.cross(f))); |
1089 |
|
|
|
1090 |
|
✗ |
vaxis << AxisWX(); |
1091 |
|
✗ |
BOOST_CHECK(AxisWX::cross(v).isApprox(vaxis.cross(v))); |
1092 |
|
✗ |
BOOST_CHECK(v.cross(AxisWX()).isApprox(v.cross(vaxis))); |
1093 |
|
✗ |
BOOST_CHECK(AxisWX::cross(f).isApprox(vaxis.cross(f))); |
1094 |
|
|
|
1095 |
|
✗ |
vaxis << AxisWY(); |
1096 |
|
✗ |
BOOST_CHECK(AxisWY::cross(v).isApprox(vaxis.cross(v))); |
1097 |
|
✗ |
BOOST_CHECK(v.cross(AxisWY()).isApprox(v.cross(vaxis))); |
1098 |
|
✗ |
BOOST_CHECK(AxisWY::cross(f).isApprox(vaxis.cross(f))); |
1099 |
|
|
|
1100 |
|
✗ |
vaxis << AxisWZ(); |
1101 |
|
✗ |
BOOST_CHECK(AxisWZ::cross(v).isApprox(vaxis.cross(v))); |
1102 |
|
✗ |
BOOST_CHECK(v.cross(AxisWZ()).isApprox(v.cross(vaxis))); |
1103 |
|
✗ |
BOOST_CHECK(AxisWZ::cross(f).isApprox(vaxis.cross(f))); |
1104 |
|
|
|
1105 |
|
|
// Test operation Axis * Scalar |
1106 |
|
✗ |
test_scalar_multiplication<0>::run(); |
1107 |
|
✗ |
test_scalar_multiplication<1>::run(); |
1108 |
|
✗ |
test_scalar_multiplication<2>::run(); |
1109 |
|
✗ |
test_scalar_multiplication<3>::run(); |
1110 |
|
✗ |
test_scalar_multiplication<4>::run(); |
1111 |
|
✗ |
test_scalar_multiplication<5>::run(); |
1112 |
|
|
|
1113 |
|
|
// Operations of Constraint on forces Sxf |
1114 |
|
|
typedef Motion::ActionMatrixType ActionMatrixType; |
1115 |
|
|
typedef ActionMatrixType::ColXpr ColType; |
1116 |
|
|
typedef ForceRef<ColType> ForceRefOnColType; |
1117 |
|
|
typedef MotionRef<ColType> MotionRefOnColType; |
1118 |
|
✗ |
ActionMatrixType Sxf, Sxf_ref; |
1119 |
|
✗ |
ActionMatrixType S(ActionMatrixType::Identity()); |
1120 |
|
|
|
1121 |
|
✗ |
SpatialAxis<0>::cross(f, ForceRefOnColType(Sxf.col(0))); |
1122 |
|
✗ |
SpatialAxis<1>::cross(f, ForceRefOnColType(Sxf.col(1))); |
1123 |
|
✗ |
SpatialAxis<2>::cross(f, ForceRefOnColType(Sxf.col(2))); |
1124 |
|
✗ |
SpatialAxis<3>::cross(f, ForceRefOnColType(Sxf.col(3))); |
1125 |
|
✗ |
SpatialAxis<4>::cross(f, ForceRefOnColType(Sxf.col(4))); |
1126 |
|
✗ |
SpatialAxis<5>::cross(f, ForceRefOnColType(Sxf.col(5))); |
1127 |
|
|
|
1128 |
|
✗ |
for (int k = 0; k < 6; ++k) |
1129 |
|
|
{ |
1130 |
|
✗ |
MotionRefOnColType Scol(S.col(k)); |
1131 |
|
✗ |
ForceRefOnColType(Sxf_ref.col(k)) = Scol.cross(f); |
1132 |
|
|
} |
1133 |
|
|
|
1134 |
|
✗ |
BOOST_CHECK(Sxf.isApprox(Sxf_ref)); |
1135 |
|
|
} |
1136 |
|
|
|
1137 |
|
|
BOOST_AUTO_TEST_SUITE_END() |
1138 |
|
|
|