Debian Codegraph @

unittests/integration/test_Frames.cpp

raw
1 /*
2 * Copyright (c) 2011-2022, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <gtest/gtest.h>
34
35 #include "dart/dynamics/SimpleFrame.hpp"
36 #include "dart/math/Helpers.hpp"
37 #include "dart/math/Random.hpp"
38
39 #include "GTestUtils.hpp"
40 #include "TestHelpers.hpp"
41
42 using namespace dart;
43 using namespace dynamics;
44
45 template <int N>
46 Eigen::Matrix<double, N, 1> random_vec(double limit = 100)
47 {
48 return Random::uniform<Eigen::Matrix<double, N, 1>>(-limit, limit);
49 }
50
51 void randomize_transform(
52 Eigen::Isometry3d& tf,
53 double translation_limit = 100,
54 double rotation_limit = 100)
55 {
56 Eigen::Vector3d r = random_vec<3>(translation_limit);
57 Eigen::Vector3d theta = random_vec<3>(rotation_limit);
58
59 tf.setIdentity();
60 tf.translate(r);
61
62 if (theta.norm() > 0)
63 tf.rotate(Eigen::AngleAxisd(theta.norm(), theta.normalized()));
64 }
65
66 void randomize_transforms(common::aligned_vector<Eigen::Isometry3d>& tfs)
67 {
68 for (std::size_t i = 0; i < tfs.size(); ++i)
69 {
70 randomize_transform(tfs[i]);
71 }
72 }
73
74 void compute_spatial_velocity(
75 const Eigen::Vector6d& v_parent,
76 const Eigen::Vector6d& v_relative,
77 const Eigen::Isometry3d& tf_rel,
78 Eigen::Vector6d& v_child)
79 {
80 v_child = math::AdInvT(tf_rel, v_parent) + v_relative;
81 }
82
83 void compute_spatial_acceleration(
84 const Eigen::Vector6d& a_parent,
85 const Eigen::Vector6d& a_relative,
86 const Eigen::Vector6d& v_child,
87 const Eigen::Vector6d& v_rel,
88 const Eigen::Isometry3d& tf_rel,
89 Eigen::Vector6d& a_child)
90 {
91 a_child
92 = math::AdInvT(tf_rel, a_parent) + a_relative + math::ad(v_child, v_rel);
93 }
94
95 void compute_velocity(
96 const Eigen::Vector3d& v_parent,
97 const Eigen::Vector3d& w_parent,
98 const Eigen::Vector3d& v_rel,
99 const Eigen::Vector3d& w_rel,
100 const Eigen::Vector3d& offset,
101 const Eigen::Isometry3d& tf_parent,
102 Eigen::Vector3d& v_child,
103 Eigen::Vector3d& w_child)
104 {
105 const Eigen::Matrix3d& R = tf_parent.rotation();
106
107 v_child = v_parent + R * v_rel + w_parent.cross(R * offset);
108
109 w_child = w_parent + R * w_rel;
110 }
111
112 void compute_acceleration(
113 const Eigen::Vector3d& a_parent,
114 const Eigen::Vector3d& alpha_parent,
115 const Eigen::Vector3d& w_parent,
116 const Eigen::Vector3d& a_rel,
117 const Eigen::Vector3d& alpha_rel,
118 const Eigen::Vector3d& v_rel,
119 const Eigen::Vector3d& w_rel,
120 const Eigen::Vector3d& offset,
121 const Eigen::Isometry3d& tf_parent,
122 Eigen::Vector3d& a_child,
123 Eigen::Vector3d& alpha_child)
124 {
125 const Eigen::Matrix3d& R = tf_parent.rotation();
126
127 a_child = a_parent + R * a_rel + alpha_parent.cross(R * offset)
128 + 2 * w_parent.cross(R * v_rel)
129 + w_parent.cross(w_parent.cross(R * offset));
130
131 alpha_child = alpha_parent + R * alpha_rel + w_parent.cross(R * w_rel);
132 }
133
134 TEST(FRAMES, FORWARD_KINEMATICS_CHAIN)
135 {
136 std::vector<SimpleFrame*> frames;
137
138 double tolerance = 1e-6;
139
140 SimpleFrame A(Frame::World(), "A");
141 frames.push_back(&A);
142 SimpleFrame B(&A, "B");
143 frames.push_back(&B);
144 SimpleFrame C(&B, "C");
145 frames.push_back(&C);
146 SimpleFrame D(&C, "D");
147 frames.push_back(&D);
148
149 // -- Test Position --------------------------------------------------------
150 EXPECT_TRANSFORM_NEAR(
151 D.getTransform(), Eigen::Isometry3d::Identity(), tolerance);
152
153 common::aligned_vector<Eigen::Isometry3d> tfs;
154 tfs.resize(frames.size(), Eigen::Isometry3d::Identity());
155
156 randomize_transforms(tfs);
157
158 for (std::size_t i = 0; i < frames.size(); ++i)
159 {
160 SimpleFrame* F = frames[i];
161 F->setRelativeTransform(tfs[i]);
162 }
163
164 for (std::size_t i = 0; i < frames.size(); ++i)
165 {
166 Frame* F = frames[i];
167 Eigen::Isometry3d expectation(Eigen::Isometry3d::Identity());
168 for (std::size_t j = 0; j <= i; ++j)
169 {
170 expectation = expectation * tfs[j];
171 }
172
173 Eigen::Isometry3d actual = F->getTransform();
174 EXPECT_TRANSFORM_NEAR(actual, expectation, tolerance);
175 }
176
177 randomize_transforms(tfs);
178 for (std::size_t i = 0; i < frames.size(); ++i)
179 {
180 SimpleFrame* F = frames[i];
181 F->setRelativeTransform(tfs[i]);
182 }
183
184 Eigen::Isometry3d expectation(Eigen::Isometry3d::Identity());
185 for (std::size_t j = 0; j < frames.size(); ++j)
186 expectation = expectation * tfs[j];
187
188 EXPECT_TRANSFORM_NEAR(frames.back()->getTransform(), expectation, tolerance);
189
190 // -- Test Velocity --------------------------------------------------------
191
192 // Basic forward spatial velocity propagation
193 { // The brackets are to allow reusing variable names
194 common::aligned_vector<Eigen::Vector6d> v_rels(frames.size());
195 common::aligned_vector<Eigen::Vector6d> v_total(frames.size());
196
197 for (std::size_t i = 0; i < frames.size(); ++i)
198 {
199 v_rels[i] = random_vec<6>();
200
201 SimpleFrame* F = frames[i];
202 F->setRelativeSpatialVelocity(v_rels[i]);
203
204 if (i > 0)
205 compute_spatial_velocity(
206 v_total[i - 1], v_rels[i], F->getRelativeTransform(), v_total[i]);
207 else
208 compute_spatial_velocity(
209 Eigen::Vector6d::Zero(),
210 v_rels[i],
211 F->getRelativeTransform(),
212 v_total[i]);
213 }
214
215 for (std::size_t i = 0; i < frames.size(); ++i)
216 {
217 SimpleFrame* F = frames[i];
218
219 Eigen::Vector6d v_actual = F->getSpatialVelocity();
220
221 EXPECT_VECTOR_DOUBLE_EQ(v_total[i], v_actual);
222 }
223 }
224
225 // Testing conversion beteween spatial and classical velocities
226 {
227 std::vector<Eigen::Vector3d> v_rels(frames.size());
228 std::vector<Eigen::Vector3d> w_rels(frames.size());
229
230 std::vector<Eigen::Vector3d> v_total(frames.size());
231 std::vector<Eigen::Vector3d> w_total(frames.size());
232
233 for (std::size_t i = 0; i < frames.size(); ++i)
234 {
235 v_rels[i] = random_vec<3>();
236 w_rels[i] = random_vec<3>();
237
238 SimpleFrame* F = frames[i];
239 F->setClassicDerivatives(v_rels[i], w_rels[i]);
240
241 Eigen::Vector3d offset = F->getRelativeTransform().translation();
242
243 Eigen::Isometry3d tf = i > 0 ? frames[i - 1]->getTransform()
244 : Eigen::Isometry3d::Identity();
245
246 if (i > 0)
247 compute_velocity(
248 v_total[i - 1],
249 w_total[i - 1],
250 v_rels[i],
251 w_rels[i],
252 offset,
253 tf,
254 v_total[i],
255 w_total[i]);
256 else
257 compute_velocity(
258 Eigen::Vector3d::Zero(),
259 Eigen::Vector3d::Zero(),
260 v_rels[i],
261 w_rels[i],
262 offset,
263 tf,
264 v_total[i],
265 w_total[i]);
266 }
267
268 for (std::size_t i = 0; i < frames.size(); ++i)
269 {
270 SimpleFrame* F = frames[i];
271 Eigen::Vector3d v_actual = F->getLinearVelocity();
272 Eigen::Vector3d w_actual = F->getAngularVelocity();
273
274 EXPECT_VECTOR_NEAR(v_total[i], v_actual, tolerance);
275 EXPECT_VECTOR_NEAR(w_total[i], w_actual, tolerance);
276 }
277 }
278
279 // -- Acceleration ---------------------------------------------------------
280
281 // Basic forward spatial acceleration propagation
282 {
283 common::aligned_vector<Eigen::Vector6d> v_rels(frames.size());
284 common::aligned_vector<Eigen::Vector6d> a_rels(frames.size());
285
286 common::aligned_vector<Eigen::Vector6d> v_total(frames.size());
287 common::aligned_vector<Eigen::Vector6d> a_total(frames.size());
288
289 for (std::size_t i = 0; i < frames.size(); ++i)
290 {
291 v_rels[i] = random_vec<6>();
292 a_rels[i] = random_vec<6>();
293
294 SimpleFrame* F = frames[i];
295 F->setRelativeSpatialVelocity(v_rels[i]);
296 F->setRelativeSpatialAcceleration(a_rels[i]);
297 Eigen::Isometry3d tf = F->getRelativeTransform();
298
299 if (i > 0)
300 {
301 compute_spatial_velocity(v_total[i - 1], v_rels[i], tf, v_total[i]);
302 compute_spatial_acceleration(
303 a_total[i - 1], a_rels[i], v_total[i], v_rels[i], tf, a_total[i]);
304 }
305 else
306 {
307 compute_spatial_velocity(
308 Eigen::Vector6d::Zero(), v_rels[i], tf, v_total[i]);
309 compute_spatial_acceleration(
310 Eigen::Vector6d::Zero(),
311 a_rels[i],
312 v_total[i],
313 v_rels[i],
314 tf,
315 a_total[i]);
316 }
317 }
318
319 for (std::size_t i = 0; i < frames.size(); ++i)
320 {
321 SimpleFrame* F = frames[i];
322
323 Eigen::Vector6d a_actual = F->getSpatialAcceleration();
324
325 EXPECT_VECTOR_DOUBLE_EQ(a_total[i], a_actual);
326 }
327
328 // Test relative computations
329 for (std::size_t i = 0; i < frames.size(); ++i)
330 {
331 Frame* F = frames[i];
332 Frame* P = F->getParentFrame();
333
334 Eigen::Vector6d v_rel = F->getSpatialVelocity(P, F);
335 Eigen::Vector6d a_rel = F->getSpatialAcceleration(P, F);
336
337 EXPECT_VECTOR_NEAR(v_rels[i], v_rel, tolerance);
338 EXPECT_VECTOR_NEAR(a_rels[i], a_rel, tolerance);
339 }
340
341 // Test offset computations
342 for (std::size_t i = 0; i < frames.size(); ++i)
343 {
344 Eigen::Vector3d offset = random_vec<3>();
345 Frame* F = frames[i];
346
347 Eigen::Vector3d v_actual = F->getLinearVelocity(offset);
348 Eigen::Vector3d w_actual = F->getAngularVelocity();
349 Eigen::Vector3d v_expect, w_expect;
350 compute_velocity(
351 F->getLinearVelocity(),
352 F->getAngularVelocity(),
353 Eigen::Vector3d::Zero(),
354 Eigen::Vector3d::Zero(),
355 offset,
356 F->getWorldTransform(),
357 v_expect,
358 w_expect);
359
360 EXPECT_VECTOR_DOUBLE_EQ(v_expect, v_actual);
361 EXPECT_VECTOR_DOUBLE_EQ(w_expect, w_actual);
362
363 Eigen::Vector3d a_actual = F->getLinearAcceleration(offset);
364 Eigen::Vector3d alpha_actual = F->getAngularAcceleration();
365 Eigen::Vector3d a_expect, alpha_expect;
366 compute_acceleration(
367 F->getLinearAcceleration(),
368 F->getAngularAcceleration(),
369 F->getAngularVelocity(),
370 Eigen::Vector3d::Zero(),
371 Eigen::Vector3d::Zero(),
372 Eigen::Vector3d::Zero(),
373 Eigen::Vector3d::Zero(),
374 offset,
375 F->getWorldTransform(),
376 a_expect,
377 alpha_expect);
378
379 EXPECT_VECTOR_DOUBLE_EQ(a_expect, a_actual);
380 EXPECT_VECTOR_DOUBLE_EQ(alpha_expect, alpha_actual);
381 }
382 }
383
384 // Testing conversion between spatial and classical accelerations
385 {
386 std::vector<Eigen::Vector3d> v_rels(frames.size());
387 std::vector<Eigen::Vector3d> w_rels(frames.size());
388 std::vector<Eigen::Vector3d> a_rels(frames.size());
389 std::vector<Eigen::Vector3d> alpha_rels(frames.size());
390
391 std::vector<Eigen::Vector3d> v_total(frames.size());
392 std::vector<Eigen::Vector3d> w_total(frames.size());
393 std::vector<Eigen::Vector3d> a_total(frames.size());
394 std::vector<Eigen::Vector3d> alpha_total(frames.size());
395
396 for (std::size_t i = 0; i < frames.size(); ++i)
397 {
398 v_rels[i] = random_vec<3>();
399 w_rels[i] = random_vec<3>();
400 a_rels[i] = random_vec<3>();
401 alpha_rels[i] = random_vec<3>();
402
403 SimpleFrame* F = frames[i];
404 Eigen::Isometry3d rel_tf;
405 randomize_transform(rel_tf);
406 F->setRelativeTransform(rel_tf);
407 F->setClassicDerivatives(v_rels[i], w_rels[i], a_rels[i], alpha_rels[i]);
408
409 Eigen::Vector3d offset = F->getRelativeTransform().translation();
410
411 Eigen::Isometry3d tf = i > 0 ? frames[i - 1]->getTransform()
412 : Eigen::Isometry3d::Identity();
413
414 if (i > 0)
415 {
416 compute_velocity(
417 v_total[i - 1],
418 w_total[i - 1],
419 v_rels[i],
420 w_rels[i],
421 offset,
422 tf,
423 v_total[i],
424 w_total[i]);
425 compute_acceleration(
426 a_total[i - 1],
427 alpha_total[i - 1],
428 w_total[i - 1],
429 a_rels[i],
430 alpha_rels[i],
431 v_rels[i],
432 w_rels[i],
433 offset,
434 tf,
435 a_total[i],
436 alpha_total[i]);
437 }
438 else
439 {
440 compute_velocity(
441 Eigen::Vector3d::Zero(),
442 Eigen::Vector3d::Zero(),
443 v_rels[i],
444 w_rels[i],
445 offset,
446 tf,
447 v_total[i],
448 w_total[i]);
449 compute_acceleration(
450 Eigen::Vector3d::Zero(),
451 Eigen::Vector3d::Zero(),
452 Eigen::Vector3d::Zero(),
453 a_rels[i],
454 alpha_rels[i],
455 v_rels[i],
456 w_rels[i],
457 offset,
458 tf,
459 a_total[i],
460 alpha_total[i]);
461 }
462 }
463
464 for (std::size_t i = 0; i < frames.size(); ++i)
465 {
466 SimpleFrame* F = frames[i];
467 Eigen::Vector3d v_actual = F->getLinearVelocity();
468 Eigen::Vector3d w_actual = F->getAngularVelocity();
469 Eigen::Vector3d a_actual = F->getLinearAcceleration();
470 Eigen::Vector3d alpha_actual = F->getAngularAcceleration();
471
472 EXPECT_VECTOR_NEAR(v_total[i], v_actual, tolerance);
473 EXPECT_VECTOR_NEAR(w_total[i], w_actual, tolerance);
474 EXPECT_VECTOR_NEAR(a_total[i], a_actual, tolerance);
475 EXPECT_VECTOR_NEAR(alpha_total[i], alpha_actual, tolerance);
476 }
477
478 // Test relative computations
479 for (std::size_t i = 0; i < frames.size(); ++i)
480 {
481 SimpleFrame* F = frames[i];
482 Frame* P = F->getParentFrame();
483 Eigen::Vector3d v_rel = F->getLinearVelocity(P, P);
484 Eigen::Vector3d w_rel = F->getAngularVelocity(P, P);
485 Eigen::Vector3d a_rel = F->getLinearAcceleration(P, P);
486 Eigen::Vector3d alpha_rel = F->getAngularAcceleration(P, P);
487
488 EXPECT_VECTOR_NEAR(v_rels[i], v_rel, tolerance);
489 EXPECT_VECTOR_NEAR(w_rels[i], w_rel, tolerance);
490 EXPECT_VECTOR_NEAR(a_rels[i], a_rel, tolerance);
491 EXPECT_VECTOR_NEAR(alpha_rels[i], alpha_rel, tolerance);
492 }
493 }
494 }
495
496 void randomize_target_values(
497 const std::vector<SimpleFrame*>& targets, bool spatial)
498 {
499 for (std::size_t i = 0; i < targets.size(); ++i)
500 {
501 SimpleFrame* T = targets[i];
502
503 Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
504 randomize_transform(tf, 1, 2 * dart::math::constantsd::pi());
505
506 if (spatial)
507 {
508 T->setRelativeTransform(tf);
509 T->setRelativeSpatialVelocity(random_vec<6>(1));
510 T->setRelativeSpatialAcceleration(random_vec<6>(1));
511 }
512 else
513 {
514 T->setRelativeTransform(tf);
515 T->setClassicDerivatives(
516 random_vec<3>(1),
517 random_vec<3>(1),
518 random_vec<3>(1),
519 random_vec<3>(1));
520 }
521 }
522 }
523
524 void set_relative_values(
525 const std::vector<SimpleFrame*>& targets,
526 const std::vector<SimpleFrame*>& followers,
527 bool spatial)
528 {
529 for (std::size_t i = 0; i < targets.size(); ++i)
530 {
531 Frame* T = targets[i];
532 SimpleFrame* F = followers[i];
533 Frame* P = F->getParentFrame();
534
535 F->setRelativeTransform(T->getTransform(P));
536 if (spatial)
537 {
538 F->setRelativeSpatialVelocity(T->getSpatialVelocity(P, F));
539 F->setRelativeSpatialAcceleration(T->getSpatialAcceleration(P, F));
540 }
541 else
542 {
543 F->setClassicDerivatives(
544 T->getLinearVelocity(P, P),
545 T->getAngularVelocity(P, P),
546 T->getLinearAcceleration(P, P),
547 T->getAngularAcceleration(P, P));
548 }
549 }
550 }
551
552 void check_world_values(
553 const std::vector<SimpleFrame*>& targets,
554 const std::vector<SimpleFrame*>& followers,
555 double tolerance)
556 {
557 for (std::size_t i = 0; i < targets.size(); ++i)
558 {
559 Frame* T = targets[i];
560 Frame* F = followers[i];
561
562 const Eigen::Isometry3d& tf_error
563 = T->getWorldTransform() * F->getWorldTransform().inverse();
564 Eigen::Vector6d error;
565 Eigen::AngleAxisd rot_error(tf_error.rotation());
566 error.block<3, 1>(0, 0) = rot_error.angle() * rot_error.axis();
567 error.block<3, 1>(3, 0) = tf_error.translation();
568
569 EXPECT_TRUE(error.norm() < tolerance);
570
571 error.block<3, 1>(0, 0) = rot_error.angle() * rot_error.axis();
572 error.block<3, 1>(3, 0) = tf_error.translation();
573
574 EXPECT_TRUE(error.norm() < tolerance);
575
576 EXPECT_VECTOR_NEAR(
577 T->getSpatialVelocity(), F->getSpatialVelocity(), tolerance);
578
579 EXPECT_VECTOR_NEAR(
580 T->getLinearVelocity(), F->getLinearVelocity(), tolerance);
581
582 EXPECT_VECTOR_NEAR(
583 T->getAngularVelocity(), F->getAngularVelocity(), tolerance);
584
585 EXPECT_VECTOR_NEAR(
586 T->getSpatialAcceleration(), F->getSpatialAcceleration(), tolerance);
587
588 EXPECT_VECTOR_NEAR(
589 T->getLinearAcceleration(), F->getLinearAcceleration(), tolerance);
590 }
591 }
592
593 void check_values(
594 const std::vector<SimpleFrame*>& targets,
595 const std::vector<SimpleFrame*>& followers,
596 const Frame* relativeTo,
597 const Frame* inCoordinatesOf,
598 double tolerance)
599 {
600 for (std::size_t i = 0; i < targets.size(); ++i)
601 {
602 Frame* T = targets[i];
603 Frame* F = followers[i];
604
605 const Eigen::Isometry3d& tf_error
606 = T->getTransform(relativeTo) * F->getTransform(relativeTo).inverse();
607 Eigen::Vector6d error;
608 Eigen::AngleAxisd rot_error(tf_error.rotation());
609 error.block<3, 1>(0, 0) = rot_error.angle() * rot_error.axis();
610 error.block<3, 1>(3, 0) = tf_error.translation();
611
612 EXPECT_TRUE(error.norm() < tolerance);
613
614 EXPECT_VECTOR_NEAR(
615 T->getSpatialVelocity(relativeTo, inCoordinatesOf),
616 F->getSpatialVelocity(relativeTo, inCoordinatesOf),
617 tolerance);
618
619 EXPECT_VECTOR_NEAR(
620 T->getLinearVelocity(relativeTo, inCoordinatesOf),
621 F->getLinearVelocity(relativeTo, inCoordinatesOf),
622 tolerance);
623
624 EXPECT_VECTOR_NEAR(
625 T->getAngularVelocity(relativeTo, inCoordinatesOf),
626 F->getAngularVelocity(relativeTo, inCoordinatesOf),
627 tolerance);
628
629 EXPECT_VECTOR_NEAR(
630 T->getSpatialAcceleration(relativeTo, inCoordinatesOf),
631 F->getSpatialAcceleration(relativeTo, inCoordinatesOf),
632 tolerance);
633
634 EXPECT_VECTOR_NEAR(
635 T->getLinearAcceleration(relativeTo, inCoordinatesOf),
636 F->getLinearAcceleration(relativeTo, inCoordinatesOf),
637 tolerance);
638
639 EXPECT_VECTOR_NEAR(
640 T->getAngularAcceleration(relativeTo, inCoordinatesOf),
641 F->getAngularAcceleration(relativeTo, inCoordinatesOf),
642 tolerance);
643 }
644 }
645
646 void check_offset_computations(
647 const std::vector<SimpleFrame*>& targets,
648 const std::vector<SimpleFrame*>& followers,
649 const Frame* relativeTo,
650 const Frame* inCoordinatesOf,
651 double tolerance)
652 {
653 for (std::size_t i = 0; i < targets.size(); ++i)
654 {
655 Frame* T = targets[i];
656 Frame* F = followers[i];
657
658 Eigen::Isometry3d coordTf = relativeTo->getTransform(inCoordinatesOf);
659
660 Vector3d offset_T = random_vec<3>();
661 Vector3d offset_F = T->getTransform(F) * offset_T;
662
663 Vector3d v_TO, w_TO, v_FO, w_FO, a_TO, alpha_TO, a_FO, alpha_FO;
664
665 // Compute velocity of the offfset in the relative frame
666 compute_velocity(
667 T->getLinearVelocity(relativeTo, relativeTo),
668 T->getAngularVelocity(relativeTo, relativeTo),
669 Eigen::Vector3d::Zero(),
670 Eigen::Vector3d::Zero(),
671 offset_T,
672 T->getTransform(relativeTo),
673 v_TO,
674 w_TO);
675 // Convert to the desired coordinate system
676 v_TO = coordTf.linear() * v_TO;
677 w_TO = coordTf.linear() * w_TO;
678
679 // Compute acceleration of the offset in the relative frame
680 compute_acceleration(
681 T->getLinearAcceleration(relativeTo, relativeTo),
682 T->getAngularAcceleration(relativeTo, relativeTo),
683 T->getAngularVelocity(relativeTo, relativeTo),
684 Eigen::Vector3d::Zero(),
685 Eigen::Vector3d::Zero(),
686 Eigen::Vector3d::Zero(),
687 Eigen::Vector3d::Zero(),
688 offset_T,
689 T->getTransform(relativeTo),
690 a_TO,
691 alpha_TO);
692 // Convert to the desired coordinate system
693 a_TO = coordTf.linear() * a_TO;
694 alpha_TO = coordTf.linear() * alpha_TO;
695
696 compute_velocity(
697 F->getLinearVelocity(relativeTo, relativeTo),
698 F->getAngularVelocity(relativeTo, relativeTo),
699 Eigen::Vector3d::Zero(),
700 Eigen::Vector3d::Zero(),
701 offset_F,
702 F->getTransform(relativeTo),
703 v_FO,
704 w_FO);
705 v_FO = coordTf.linear() * v_FO;
706 w_FO = coordTf.linear() * w_FO;
707
708 compute_acceleration(
709 F->getLinearAcceleration(relativeTo, relativeTo),
710 F->getAngularAcceleration(relativeTo, relativeTo),
711 F->getAngularVelocity(relativeTo, relativeTo),
712 Eigen::Vector3d::Zero(),
713 Eigen::Vector3d::Zero(),
714 Eigen::Vector3d::Zero(),
715 Eigen::Vector3d::Zero(),
716 offset_F,
717 F->getTransform(relativeTo),
718 a_FO,
719 alpha_FO);
720 a_FO = coordTf.linear() * a_FO;
721 alpha_FO = coordTf.linear() * alpha_FO;
722
723 Eigen::Vector3d v_TO_actual
724 = T->getLinearVelocity(offset_T, relativeTo, inCoordinatesOf);
725 Eigen::Vector3d v_FO_actual
726 = F->getLinearVelocity(offset_F, relativeTo, inCoordinatesOf);
727
728 EXPECT_VECTOR_NEAR(v_TO, v_FO, tolerance);
729 EXPECT_VECTOR_NEAR(v_TO, v_TO_actual, tolerance);
730 EXPECT_VECTOR_NEAR(v_FO, v_FO_actual, tolerance);
731
732 Eigen::Vector3d a_TO_actual
733 = T->getLinearAcceleration(offset_T, relativeTo, inCoordinatesOf);
734 Eigen::Vector3d a_FO_actual
735 = F->getLinearAcceleration(offset_F, relativeTo, inCoordinatesOf);
736
737 EXPECT_VECTOR_NEAR(a_TO, a_FO, tolerance);
738 EXPECT_VECTOR_NEAR(a_TO, a_TO_actual, tolerance);
739 EXPECT_VECTOR_NEAR(a_FO, a_FO_actual, tolerance);
740 }
741 }
742
743 void test_relative_values(bool spatial_targets, bool spatial_followers)
744 {
745 const double tolerance = 1e-8;
746
747 // These frames will form a chain
748 SimpleFrame A(Frame::World(), "A");
749 SimpleFrame B(&A, "B");
750 SimpleFrame C(&B, "C");
751 SimpleFrame D(&C, "D");
752
753 std::vector<SimpleFrame*> targets;
754 targets.push_back(&A);
755 targets.push_back(&B);
756 targets.push_back(&C);
757 targets.push_back(&D);
758
759 // R will be an arbitrary reference frame
760 SimpleFrame R(Frame::World(), "R");
761 targets.push_back(&R);
762
763 // Each of these frames will attempt to track one of the frames in the chain
764 // with respect to the frame R.
765 SimpleFrame AR(&R, "AR");
766 SimpleFrame BR(&R, "BR");
767 SimpleFrame CR(&R, "CR");
768 SimpleFrame DR(&R, "DR");
769 SimpleFrame RR(&R, "RR");
770
771 std::vector<SimpleFrame*> followers;
772 followers.push_back(&AR);
773 followers.push_back(&BR);
774 followers.push_back(&CR);
775 followers.push_back(&DR);
776 followers.push_back(&RR);
777
778 assert(targets.size() == followers.size());
779
780 randomize_target_values(targets, spatial_targets);
781 set_relative_values(targets, followers, spatial_followers);
782 check_world_values(targets, followers, tolerance);
783
784 // Check every combination of relative values
785 for (std::size_t i = 0; i < targets.size(); ++i)
786 {
787 Frame* T = targets[i];
788 for (std::size_t j = 0; j < followers.size(); ++j)
789 {
790 Frame* F = followers[j];
791
792 check_values(targets, followers, T, F, tolerance);
793 check_values(targets, followers, F, T, tolerance);
794 check_offset_computations(targets, followers, T, F, tolerance);
795 }
796 }
797
798 // Test SimpleFrame::setTransform()
799 for (std::size_t i = 0; i < followers.size(); ++i)
800 {
801 for (std::size_t j = 0; j < followers.size(); ++j)
802 {
803 SimpleFrame* F = followers[i];
804 SimpleFrame* T = followers[j];
805 Eigen::Isometry3d tf;
806 randomize_transform(tf, 1, 2 * dart::math::constantsd::pi());
807 T->setTransform(tf, F);
808 if (i != j)
809 {
810 EXPECT_TRANSFORM_NEAR(T->getTransform(F), tf, 1e-10);
811 }
812 }
813 }
814 }
815
816 // Test different combinations of using spatial and classical derivative terms
817
818 TEST(RELATIVE_FRAMES, SPATIAL_SPATIAL)
819 {
820 test_relative_values(true, true);
821 }
822
823 TEST(RELATIVE_FRAMES, CLASSIC_CLASSIC)
824 {
825 test_relative_values(false, false);
826 }
827
828 TEST(RELATIVE_FRAMES, SPATIAL_CLASSIC)
829 {
830 test_relative_values(true, false);
831 }
832
833 TEST(RELATIVE_FRAMES, CLASSIC_SPATIAL)
834 {
835 test_relative_values(false, true);
836 }
837
838 TEST(FRAMES, CHILDHOOD)
839 {
840 SimpleFrame F1(Frame::World(), "F1");
841 SimpleFrame F2(&F1, "F2");
842
843 EXPECT_TRUE(F1.getNumChildFrames() == 1);
844 }