Debian Codegraph @

unittests/integration/test_Skeleton.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 <iostream>
34
35 #include <gtest/gtest.h>
36
37 #include "dart/common/sub_ptr.hpp"
38 #include "dart/dynamics/BodyNode.hpp"
39 #include "dart/dynamics/RevoluteJoint.hpp"
40 #include "dart/dynamics/Skeleton.hpp"
41 #include "dart/math/Geometry.hpp"
42 #include "dart/simulation/World.hpp"
43 #include "dart/utils/SkelParser.hpp"
44
45 #include "TestHelpers.hpp"
46
47 using namespace dart;
48 using namespace math;
49 using namespace dynamics;
50 using namespace simulation;
51
52 std::vector<common::Uri> getFileList()
53 {
54 std::vector<common::Uri> fileList;
55 fileList.push_back("dart://sample/skel/test/chainwhipa.skel");
56 fileList.push_back("dart://sample/skel/test/single_pendulum.skel");
57 fileList.push_back(
58 "dart://sample/skel/test/single_pendulum_euler_joint.skel");
59 fileList.push_back("dart://sample/skel/test/single_pendulum_ball_joint.skel");
60 fileList.push_back("dart://sample/skel/test/double_pendulum.skel");
61 fileList.push_back(
62 "dart://sample/skel/test/double_pendulum_euler_joint.skel");
63 fileList.push_back("dart://sample/skel/test/double_pendulum_ball_joint.skel");
64 fileList.push_back(
65 "dart://sample/skel/test/serial_chain_revolute_joint.skel");
66 fileList.push_back(
67 "dart://sample/skel/test/serial_chain_eulerxyz_joint.skel");
68 fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint.skel");
69 fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_20.skel");
70 fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_40.skel");
71 fileList.push_back("dart://sample/skel/test/simple_tree_structure.skel");
72 fileList.push_back(
73 "dart://sample/skel/test/simple_tree_structure_euler_joint.skel");
74 fileList.push_back(
75 "dart://sample/skel/test/simple_tree_structure_ball_joint.skel");
76 fileList.push_back("dart://sample/skel/test/tree_structure.skel");
77 fileList.push_back("dart://sample/skel/test/tree_structure_euler_joint.skel");
78 fileList.push_back("dart://sample/skel/test/tree_structure_ball_joint.skel");
79 fileList.push_back("dart://sample/skel/fullbody1.skel");
80
81 return fileList;
82 }
83
84 std::vector<SkeletonPtr> getSkeletons()
85 {
86 const auto fileList = getFileList();
87
88 std::vector<WorldPtr> worlds;
89 for (std::size_t i = 0; i < fileList.size(); ++i)
90 worlds.push_back(utils::SkelParser::readWorld(fileList[i]));
91
92 std::vector<SkeletonPtr> skeletons;
93 for (std::size_t i = 0; i < worlds.size(); ++i)
94 {
95 WorldPtr world = worlds[i];
96 for (std::size_t j = 0; j < world->getNumSkeletons(); ++j)
97 skeletons.push_back(world->getSkeleton(j));
98 }
99
100 return skeletons;
101 }
102
103 void constructSubtree(std::vector<BodyNode*>& _tree, BodyNode* bn)
104 {
105 _tree.push_back(bn);
106 for (std::size_t i = 0; i < bn->getNumChildBodyNodes(); ++i)
107 constructSubtree(_tree, bn->getChildBodyNode(i));
108 }
109
110 TEST(Skeleton, Restructuring)
111 {
112 std::vector<SkeletonPtr> skeletons = getSkeletons();
113
114 #ifndef NDEBUG
115 std::size_t numIterations = 10;
116 #else
117 std::size_t numIterations = 2 * skeletons.size();
118 #endif
119
120 for (const auto& skeleton : skeletons)
121 EXPECT_TRUE(skeleton->checkIndexingConsistency());
122
123 // Test moves within the current Skeleton
124 for (std::size_t i = 0; i < numIterations; ++i)
125 {
126 std::size_t index
127 = math::Random::uniform<std::size_t>(0, skeletons.size() - 1);
128 index = std::min(index, skeletons.size() - 1);
129 SkeletonPtr skeleton = skeletons[index];
130 EXPECT_TRUE(skeleton->checkIndexingConsistency());
131 SkeletonPtr original = skeleton->cloneSkeleton();
132 EXPECT_TRUE(original->checkIndexingConsistency());
133
134 std::size_t maxNode = skeleton->getNumBodyNodes() - 1;
135 BodyNode* bn1
136 = skeleton->getBodyNode(math::Random::uniform<std::size_t>(0, maxNode));
137 BodyNode* bn2
138 = skeleton->getBodyNode(math::Random::uniform<std::size_t>(0, maxNode));
139
140 if (bn1 == bn2)
141 {
142 --i;
143 continue;
144 }
145
146 BodyNode* child = bn1->descendsFrom(bn2) ? bn1 : bn2;
147 BodyNode* parent = child == bn1 ? bn2 : bn1;
148
149 auto skelVer = skeleton->getVersion();
150 auto childVer = child->getVersion();
151 child->moveTo(parent);
152 EXPECT_NE(skeleton->getVersion(), skelVer);
153 EXPECT_NE(child->getVersion(), childVer);
154
155 EXPECT_TRUE(skeleton->getNumBodyNodes() == original->getNumBodyNodes());
156 if (skeleton->getNumBodyNodes() == original->getNumBodyNodes())
157 {
158 for (std::size_t j = 0; j < skeleton->getNumBodyNodes(); ++j)
159 {
160 // Make sure no BodyNodes have been lost or gained in translation
161 std::string name = original->getBodyNode(j)->getName();
162 BodyNode* bn = skeleton->getBodyNode(name);
163 EXPECT_FALSE(bn == nullptr);
164 if (bn)
165 {
166 EXPECT_TRUE(bn->getName() == name);
167 }
168
169 name = skeleton->getBodyNode(j)->getName();
170 bn = original->getBodyNode(name);
171 EXPECT_FALSE(bn == nullptr);
172 if (bn)
173 {
174 EXPECT_TRUE(bn->getName() == name);
175 }
176
177 // Make sure no Joints have been lost or gained in translation
178 name = original->getJoint(j)->getName();
179 Joint* joint = skeleton->getJoint(name);
180 EXPECT_FALSE(joint == nullptr);
181 if (joint)
182 {
183 EXPECT_TRUE(joint->getName() == name);
184 }
185
186 name = skeleton->getJoint(j)->getName();
187 joint = original->getJoint(name);
188 EXPECT_FALSE(joint == nullptr);
189 if (joint)
190 {
191 EXPECT_TRUE(joint->getName() == name);
192 }
193 }
194 }
195
196 EXPECT_TRUE(skeleton->getNumDofs() == original->getNumDofs());
197 for (std::size_t j = 0; j < skeleton->getNumDofs(); ++j)
198 {
199 std::string name = original->getDof(j)->getName();
200 DegreeOfFreedom* dof = skeleton->getDof(name);
201 EXPECT_FALSE(dof == nullptr);
202 if (dof)
203 {
204 EXPECT_TRUE(dof->getName() == name);
205 }
206
207 name = skeleton->getDof(j)->getName();
208 dof = original->getDof(name);
209 EXPECT_FALSE(dof == nullptr);
210 if (dof)
211 {
212 EXPECT_TRUE(dof->getName() == name);
213 }
214 }
215 }
216
217 // Test moves between Skeletons
218 for (std::size_t i = 0; i < numIterations; ++i)
219 {
220 std::size_t fromIndex
221 = math::Random::uniform<std::size_t>(0, skeletons.size() - 1);
222 fromIndex = std::min(fromIndex, skeletons.size() - 1);
223 SkeletonPtr fromSkel = skeletons[fromIndex];
224
225 if (fromSkel->getNumBodyNodes() == 0)
226 {
227 --i;
228 continue;
229 }
230
231 std::size_t toIndex
232 = math::Random::uniform<std::size_t>(0, skeletons.size() - 1);
233 toIndex = std::min(toIndex, skeletons.size() - 1);
234 SkeletonPtr toSkel = skeletons[toIndex];
235
236 if (toSkel->getNumBodyNodes() == 0)
237 {
238 --i;
239 continue;
240 }
241
242 BodyNode* childBn = fromSkel->getBodyNode(
243 math::Random::uniform<std::size_t>(0, fromSkel->getNumBodyNodes() - 1));
244 BodyNode* parentBn = toSkel->getBodyNode(
245 math::Random::uniform<std::size_t>(0, toSkel->getNumBodyNodes() - 1));
246
247 if (fromSkel == toSkel)
248 {
249 if (childBn == parentBn)
250 {
251 --i;
252 continue;
253 }
254
255 if (parentBn->descendsFrom(childBn))
256 {
257 BodyNode* tempBn = childBn;
258 childBn = parentBn;
259 parentBn = tempBn;
260
261 SkeletonPtr tempSkel = fromSkel;
262 fromSkel = toSkel;
263 toSkel = tempSkel;
264 }
265 }
266
267 BodyNode* originalParent = childBn->getParentBodyNode();
268 std::vector<BodyNode*> subtree;
269 constructSubtree(subtree, childBn);
270
271 // Move to a new Skeleton
272 auto fromSkelVer = fromSkel->getVersion();
273 auto toSkelVer = toSkel->getVersion();
274 auto child_ver = childBn->getVersion();
275 childBn->moveTo(parentBn);
276 EXPECT_NE(fromSkel->getVersion(), fromSkelVer);
277 EXPECT_NE(toSkel->getVersion(), toSkelVer);
278 EXPECT_NE(childBn->getVersion(), child_ver);
279 EXPECT_TRUE(childBn->getSkeleton()->checkIndexingConsistency());
280 EXPECT_TRUE(parentBn->getSkeleton()->checkIndexingConsistency());
281
282 // Make sure all the objects have moved
283 for (std::size_t j = 0; j < subtree.size(); ++j)
284 {
285 BodyNode* bn = subtree[j];
286 EXPECT_TRUE(bn->getSkeleton() == toSkel);
287 }
288
289 // Move to the Skeleton's root while producing a new Joint type
290 sub_ptr<Joint> originalJoint = childBn->getParentJoint();
291 childBn->moveTo<FreeJoint>(nullptr);
292
293 // The original parent joint should be deleted now
294 EXPECT_TRUE(originalJoint == nullptr);
295
296 // The BodyNode should now be a root node
297 EXPECT_TRUE(childBn->getParentBodyNode() == nullptr);
298
299 // The subtree should still be in the same Skeleton
300 for (std::size_t j = 0; j < subtree.size(); ++j)
301 {
302 BodyNode* bn = subtree[j];
303 EXPECT_TRUE(bn->getSkeleton() == toSkel);
304 }
305
306 // Create some new Skeletons and mangle them all up
307
308 childBn->copyTo<RevoluteJoint>(fromSkel, originalParent);
309
310 SkeletonPtr temporary = childBn->split("temporary");
311 SkeletonPtr other_temporary
312 = childBn->split<PrismaticJoint>("other temporary");
313 SkeletonPtr another_temporary = childBn->copyAs("another temporary");
314 SkeletonPtr last_temporary = childBn->copyAs<ScrewJoint>("last temporary");
315
316 childBn->copyTo(another_temporary->getBodyNode(
317 another_temporary->getNumBodyNodes() - 1));
318 childBn->copyTo<PlanarJoint>(another_temporary->getBodyNode(0));
319 childBn->copyTo<TranslationalJoint>(temporary, nullptr);
320 childBn->moveTo(
321 last_temporary,
322 last_temporary->getBodyNode(last_temporary->getNumBodyNodes() - 1));
323 childBn->moveTo<BallJoint>(last_temporary, nullptr);
324 childBn->moveTo<EulerJoint>(last_temporary, last_temporary->getBodyNode(0));
325 childBn->changeParentJointType<FreeJoint>();
326
327 // Test the non-recursive copying
328 if (toSkel->getNumBodyNodes() > 1)
329 {
330 SkeletonPtr singleBodyNode
331 = toSkel->getBodyNode(0)->copyAs("single", false);
332 EXPECT_TRUE(singleBodyNode->getNumBodyNodes() == 1);
333
334 std::pair<Joint*, BodyNode*> singlePair
335 = toSkel->getBodyNode(0)->copyTo(nullptr, false);
336 EXPECT_TRUE(singlePair.second->getNumChildBodyNodes() == 0);
337 }
338
339 // Check that the mangled Skeletons are all self-consistent
340 EXPECT_TRUE(fromSkel->checkIndexingConsistency());
341 EXPECT_TRUE(toSkel->checkIndexingConsistency());
342 EXPECT_TRUE(temporary->checkIndexingConsistency());
343 EXPECT_TRUE(other_temporary->checkIndexingConsistency());
344 EXPECT_TRUE(another_temporary->checkIndexingConsistency());
345 EXPECT_TRUE(last_temporary->checkIndexingConsistency());
346 }
347 }
348
349 TEST(Skeleton, Persistence)
350 {
351 WeakBodyNodePtr weakBnPtr;
352 SoftBodyNodePtr softBnPtr;
353 WeakSoftBodyNodePtr weakSoftBnPtr;
354 WeakSkeletonPtr weakSkelPtr;
355 {
356 BodyNodePtr strongPtr;
357 {
358 {
359 SkeletonPtr skeleton = createThreeLinkRobot(
360 Eigen::Vector3d(1.0, 1.0, 1.0),
361 DOF_X,
362 Eigen::Vector3d(1.0, 1.0, 1.0),
363 DOF_Y,
364 Eigen::Vector3d(1.0, 1.0, 1.0),
365 DOF_Z);
366 weakSkelPtr = skeleton;
367
368 // Test usability of the BodyNodePtr
369 strongPtr = skeleton->getBodyNode(0);
370 weakBnPtr = strongPtr;
371 ConstBodyNodePtr constPtr = strongPtr;
372
373 EXPECT_FALSE(strongPtr == nullptr);
374 EXPECT_FALSE(nullptr == strongPtr);
375
376 EXPECT_TRUE(strongPtr == skeleton->getBodyNode(0));
377 EXPECT_TRUE(skeleton->getBodyNode(0) == strongPtr);
378 EXPECT_TRUE(constPtr == strongPtr);
379 EXPECT_TRUE(weakBnPtr.lock() == strongPtr);
380
381 EXPECT_FALSE(strongPtr < constPtr);
382 EXPECT_FALSE(strongPtr < skeleton->getBodyNode(0));
383 EXPECT_FALSE(strongPtr < weakBnPtr.lock());
384 EXPECT_FALSE(skeleton->getBodyNode(0) < strongPtr);
385 EXPECT_FALSE(weakBnPtr.lock() < strongPtr);
386
387 EXPECT_FALSE(strongPtr > constPtr);
388 EXPECT_FALSE(strongPtr > skeleton->getBodyNode(0));
389 EXPECT_FALSE(strongPtr > weakBnPtr.lock());
390 EXPECT_FALSE(skeleton->getBodyNode(0) > strongPtr);
391 EXPECT_FALSE(weakBnPtr.lock() > strongPtr);
392
393 BodyNodePtr tail
394 = skeleton->getBodyNode(skeleton->getNumBodyNodes() - 1);
395 std::pair<Joint*, SoftBodyNode*> pair
396 = skeleton->createJointAndBodyNodePair<RevoluteJoint, SoftBodyNode>(
397 tail);
398
399 softBnPtr = pair.second;
400 weakSoftBnPtr = softBnPtr;
401 WeakBodyNodePtr otherWeakPtr = weakSoftBnPtr; // Test convertability
402
403 // Test usability of the DegreeOfFreedomPtr
404 DegreeOfFreedomPtr dof = skeleton->getDof(1);
405 WeakDegreeOfFreedomPtr weakdof = dof;
406 ConstDegreeOfFreedomPtr const_dof = dof;
407 WeakConstDegreeOfFreedomPtr const_weakdof = weakdof;
408 const_weakdof = const_dof;
409
410 EXPECT_TRUE(dof == skeleton->getDof(1));
411 EXPECT_TRUE(dof == const_dof);
412 EXPECT_TRUE(weakdof.lock() == const_weakdof.lock());
413 EXPECT_TRUE(const_weakdof.lock() == skeleton->getDof(1));
414 EXPECT_TRUE(skeleton->getDof(1) == const_weakdof.lock());
415
416 EXPECT_FALSE(dof < const_dof);
417 EXPECT_FALSE(dof < skeleton->getDof(1));
418 EXPECT_FALSE(dof < weakdof.lock());
419 EXPECT_FALSE(skeleton->getDof(1) < dof);
420 EXPECT_FALSE(weakdof.lock() < dof);
421
422 EXPECT_FALSE(dof > const_dof);
423 EXPECT_FALSE(dof > skeleton->getDof(1));
424 EXPECT_FALSE(dof > weakdof.lock());
425 EXPECT_FALSE(skeleton->getDof(1) > dof);
426 EXPECT_FALSE(weakdof.lock() > dof);
427
428 dof = nullptr;
429 weakdof = nullptr;
430 const_dof = nullptr;
431 const_weakdof = nullptr;
432
433 EXPECT_TRUE(dof == nullptr);
434 EXPECT_TRUE(nullptr == dof);
435 EXPECT_TRUE(weakdof.lock() == nullptr);
436 EXPECT_TRUE(nullptr == weakdof.lock());
437 EXPECT_TRUE(const_dof == nullptr);
438 EXPECT_TRUE(const_weakdof.lock() == nullptr);
439
440 EXPECT_FALSE(dof < const_dof);
441
442 // Test usability of the JointPtr
443 JointPtr joint = skeleton->getJoint(1);
444 WeakJointPtr weakjoint = joint;
445 ConstJointPtr const_joint = joint;
446 WeakConstJointPtr const_weakjoint = const_joint;
447
448 EXPECT_TRUE(joint == skeleton->getJoint(1));
449 EXPECT_TRUE(joint == const_joint);
450 EXPECT_TRUE(weakjoint.lock() == const_weakjoint.lock());
451 EXPECT_TRUE(const_weakjoint.lock() == skeleton->getJoint(1));
452
453 EXPECT_FALSE(joint < const_joint);
454 EXPECT_FALSE(joint < skeleton->getJoint(1));
455 EXPECT_FALSE(joint < weakjoint.lock());
456 EXPECT_FALSE(skeleton->getJoint(1) < joint);
457 EXPECT_FALSE(weakjoint.lock() < joint);
458
459 EXPECT_FALSE(joint > const_joint);
460 EXPECT_FALSE(joint > skeleton->getJoint(1));
461 EXPECT_FALSE(joint > weakjoint.lock());
462 EXPECT_FALSE(skeleton->getJoint(1) > joint);
463 EXPECT_FALSE(weakjoint.lock() > joint);
464
465 joint = nullptr;
466 weakjoint = nullptr;
467 const_joint = nullptr;
468 const_weakjoint = nullptr;
469
470 EXPECT_TRUE(joint == nullptr);
471 EXPECT_TRUE(weakjoint.lock() == nullptr);
472 EXPECT_TRUE(const_joint == nullptr);
473 EXPECT_TRUE(const_weakjoint.lock() == nullptr);
474 }
475
476 // The BodyNode should still be alive, because a BodyNodePtr still
477 // references it
478 EXPECT_FALSE(weakBnPtr.expired());
479
480 // The Skeleton should still be alive, because a BodyNodePtr still
481 // references one of its BodyNodes
482 EXPECT_FALSE(weakSkelPtr.lock() == nullptr);
483
484 // Take the BodyNode out of its Skeleton and put it into a temporary one
485 strongPtr->remove();
486
487 // The BodyNode should still be alive, because a BodyNodePtr still
488 // references it
489 EXPECT_FALSE(weakBnPtr.expired());
490
491 // The Skeleton should be destroyed, because it lost the only BodyNode
492 // that still had a reference
493 EXPECT_TRUE(weakSkelPtr.lock() == nullptr);
494
495 // Update the weakSkelPtr so it references the Skeleton that still exists
496 weakSkelPtr = strongPtr->getSkeleton();
497 EXPECT_FALSE(weakSkelPtr.lock() == nullptr);
498
499 // Change the BodyNode that this BodyNodePtr is referencing
500 strongPtr = strongPtr->getChildBodyNode(0);
501
502 // Make sure the Skeleton is still alive. If the SkeletonPtr being used
503 // by the BodyNodePtr is not swapped atomically, then this will fail,
504 // which means we cannot rely on BodyNodePtr to keep BodyNodes alive.
505 EXPECT_FALSE(weakSkelPtr.lock() == nullptr);
506 }
507
508 SkeletonPtr other_skeleton = createThreeLinkRobot(
509 Eigen::Vector3d(1.0, 1.0, 1.0),
510 DOF_X,
511 Eigen::Vector3d(1.0, 1.0, 1.0),
512 DOF_Y,
513 Eigen::Vector3d(1.0, 1.0, 1.0),
514 DOF_Z);
515 BodyNode* tail
516 = other_skeleton->getBodyNode(other_skeleton->getNumBodyNodes() - 1);
517
518 WeakConstBodyNodePtr weakParentPtr;
519 {
520 ConstBodyNodePtr parent = strongPtr;
521 parent = parent->getParentBodyNode();
522 weakParentPtr = parent;
523 strongPtr->moveTo(tail);
524
525 // The Skeleton should still be alive because 'parent' exists
526 EXPECT_FALSE(weakSkelPtr.lock() == nullptr);
527 }
528
529 // Now that 'parent' is out of scope, the Skeleton should be gone
530 EXPECT_TRUE(weakSkelPtr.lock() == nullptr);
531 EXPECT_TRUE(weakParentPtr.lock() == nullptr);
532
533 weakBnPtr = strongPtr;
534 weakSkelPtr = strongPtr->getSkeleton();
535 EXPECT_FALSE(weakBnPtr.expired());
536 EXPECT_FALSE(weakSkelPtr.expired());
537 }
538
539 // softBnPtr still exists, so it should be keeping the Skeleton active
540 EXPECT_FALSE(weakBnPtr.expired());
541
542 std::weak_ptr<Skeleton> weakSkel = softBnPtr->remove();
543
544 // Now that the SoftBodyNode which is holding the reference has been moved to
545 // another Skeleton, the weakBnPtr and weakSkelPtr should disappear
546 EXPECT_TRUE(weakBnPtr.expired());
547 EXPECT_TRUE(weakSkelPtr.expired());
548
549 // The WeakSoftBodyNodePtr should not have expired yet, because a strong
550 // reference to its SoftBodyNode still exists
551 EXPECT_FALSE(weakSoftBnPtr.expired());
552
553 // Test the user-defined copy constructor
554 SoftBodyNodePtr otherSoftBnPtr = softBnPtr;
555
556 softBnPtr = nullptr;
557
558 EXPECT_FALSE(weakSkel.lock() == nullptr);
559 EXPECT_FALSE(weakSoftBnPtr.lock() == nullptr);
560
561 BodyNodePtr strongPtr = otherSoftBnPtr;
562
563 otherSoftBnPtr = nullptr;
564
565 BodyNodePtr otherStrongPtr = strongPtr;
566
567 strongPtr = nullptr;
568
569 EXPECT_FALSE(weakSkel.lock() == nullptr);
570 EXPECT_FALSE(weakSoftBnPtr.lock() == nullptr);
571
572 otherStrongPtr = nullptr;
573
574 // Now that all the strong BodyNodePtrs have been cleared, the
575 // WeakSoftBodyNodePtr should also be cleared
576 EXPECT_TRUE(weakSoftBnPtr.lock() == nullptr);
577 EXPECT_TRUE(weakSkel.lock() == nullptr);
578 }
579
580 class GenericNode final : public dart::dynamics::Node,
581 public AccessoryNode<GenericNode>
582 {
583 public:
584 GenericNode(BodyNode* bn, const std::string& name) : Node(bn), mName(name) {}
585
586 const std::string& setName(const std::string& newName) override
587 {
588 mName = registerNameChange(newName);
589 return mName;
590 }
591
592 const std::string& getName() const override
593 {
594 return mName;
595 }
596
597 protected:
598 Node* cloneNode(BodyNode* bn) const override
599 {
600 return new GenericNode(bn, mName);
601 }
602
603 std::string mName;
604 };
605
606 TEST(Skeleton, NodePersistence)
607 {
608 SkeletonPtr skel = Skeleton::create();
609 skel->createJointAndBodyNodePair<FreeJoint>(nullptr);
610
611 //--------------------------------------------------------------------------
612 // Testing EndEffector, which is a specialized Node type
613 //--------------------------------------------------------------------------
614 {
615 EndEffector* manip = skel->getBodyNode(0)->createEndEffector("manip");
616
617 // Test both methods of adding a Support to an EndEffector
618 manip->createAspect<Support>();
619 manip->createSupport();
620
621 EXPECT_EQ(skel->getEndEffector("manip"), manip);
622 EXPECT_EQ(skel->getEndEffector(0), manip);
623 // EXPECT_EQ(skel->getBodyNode(0)->getEndEffector(0), manip);
624
625 WeakEndEffectorPtr weakManip = manip;
626
627 EXPECT_NE(weakManip.lock(), nullptr);
628
629 manip->remove();
630
631 // The Node has been removed, and no strong reference to it exists, so it
632 // should be gone from the Skeleton
633 EXPECT_EQ(skel->getEndEffector("manip"), nullptr);
634 EXPECT_EQ(skel->getNumEndEffectors(), 0u);
635 // EXPECT_EQ(skel->getBodyNode(0)->getNumEndEffectors(), 0u);
636
637 EXPECT_EQ(weakManip.lock(), nullptr);
638 }
639
640 {
641 EndEffector* manip = skel->getBodyNode(0)->createEndEffector("manip");
642
643 EXPECT_EQ(skel->getEndEffector("manip"), manip);
644 EXPECT_EQ(skel->getEndEffector(0), manip);
645 // EXPECT_EQ(skel->getBodyNode(0)->getEndEffector(0), manip);
646
647 EndEffectorPtr strongManip = manip;
648 WeakEndEffectorPtr weakManip = strongManip;
649
650 EXPECT_FALSE(weakManip.lock() == nullptr);
651
652 manip->remove();
653
654 // The Node has been removed, so no reference to it will exist in the
655 // Skeleton
656 #ifdef NDEBUG // Release Mode
657 EXPECT_NE(skel->getEndEffector("manip"), manip);
658 EXPECT_EQ(skel->getEndEffector("manip"), nullptr);
659
660 EXPECT_NE(skel->getEndEffector(0), manip);
661 EXPECT_EQ(skel->getEndEffector(0), nullptr);
662 #endif // Release Mode
663
664 #ifdef NDEBUG // Release Mode
665 // But it will not remain in the BodyNode's indexing.
666 // Note: We should only run this test in release mode, because otherwise it
667 // will trigger an assertion.
668 // EXPECT_NE(skel->getBodyNode(0)->getEndEffector(0), manip);
669 // EXPECT_EQ(skel->getBodyNode(0)->getEndEffector(0), nullptr);
670 #endif // Release Mode
671
672 EXPECT_NE(weakManip.lock(), nullptr);
673
674 strongManip = nullptr;
675
676 // The Node has been removed, and no strong reference to it exists any
677 // longer, so it should be gone from the Skeleton
678 EXPECT_EQ(skel->getEndEffector("manip"), nullptr);
679 EXPECT_EQ(skel->getNumEndEffectors(), 0u);
680 // EXPECT_EQ(skel->getBodyNode(0)->getNumEndEffectors(), 0u);
681
682 EXPECT_EQ(weakManip.lock(), nullptr);
683 }
684
685 using GenericNodePtr = TemplateNodePtr<GenericNode, BodyNode>;
686 using WeakGenericNodePtr = TemplateWeakNodePtr<GenericNode, BodyNode>;
687 //--------------------------------------------------------------------------
688 // Testing GenericNode, which is NOT a specialized Node type
689 //--------------------------------------------------------------------------
690 {
691 GenericNode* node = skel->getBodyNode(0)->createNode<GenericNode>("node");
692
693 EXPECT_EQ(skel->getNode<GenericNode>("node"), node);
694 EXPECT_EQ(skel->getNode<GenericNode>(0), node);
695 EXPECT_EQ(skel->getBodyNode(0)->getNode<GenericNode>(0), node);
696
697 WeakGenericNodePtr weakNode = node;
698
699 EXPECT_NE(weakNode.lock(), nullptr);
700
701 node->remove();
702
703 // The Node has been removed, and no strong reference to it exists, so it
704 // should be gone from the Skeleton
705 EXPECT_EQ(skel->getNode<GenericNode>("node"), nullptr);
706 EXPECT_EQ(skel->getNumNodes<GenericNode>(), 0u);
707 EXPECT_EQ(skel->getBodyNode(0)->getNumNodes<GenericNode>(), 0u);
708
709 EXPECT_EQ(weakNode.lock(), nullptr);
710 }
711
712 {
713 GenericNode* node = skel->getBodyNode(0)->createNode<GenericNode>("node");
714
715 EXPECT_EQ(skel->getNode<GenericNode>("node"), node);
716 EXPECT_EQ(skel->getNode<GenericNode>(0), node);
717 EXPECT_EQ(skel->getBodyNode(0)->getNode<GenericNode>(0), node);
718
719 GenericNodePtr strongNode = node;
720 WeakGenericNodePtr weakNode = strongNode;
721
722 EXPECT_FALSE(weakNode.lock() == nullptr);
723
724 node->remove();
725
726 // The Node has been removed, so no reference to it will exist in the
727 // Skeleton
728 #ifdef NDEBUG // Release Mode
729 EXPECT_NE(skel->getNode<GenericNode>("node"), node);
730 EXPECT_EQ(skel->getNode<GenericNode>("node"), nullptr);
731
732 EXPECT_NE(skel->getNode<GenericNode>(0), node);
733 EXPECT_EQ(skel->getNode<GenericNode>(0), nullptr);
734 #endif // Release Mode
735
736 #ifdef NDEBUG // Release Mode
737 // But it will not remain in the BodyNode's indexing.
738 // Note: We should only run this test in release mode, because otherwise it
739 // will trigger an assertion.
740 EXPECT_NE(skel->getBodyNode(0)->getNode<GenericNode>(0), node);
741 EXPECT_EQ(skel->getBodyNode(0)->getNode<GenericNode>(0), nullptr);
742 #endif // Release Mode
743
744 EXPECT_NE(weakNode.lock(), nullptr);
745
746 strongNode = nullptr;
747
748 // The Node has been removed, and no strong reference to it exists any
749 // longer, so it should be gone from the Skeleton
750 EXPECT_EQ(skel->getNode<GenericNode>("node"), nullptr);
751 EXPECT_EQ(skel->getNumNodes<GenericNode>(), 0u);
752 EXPECT_EQ(skel->getBodyNode(0)->getNumNodes<GenericNode>(), 0u);
753
754 EXPECT_EQ(weakNode.lock(), nullptr);
755 }
756 }
757
758 TEST(Skeleton, CloneNodeOrdering)
759 {
760 // This test checks that the ordering of Nodes in a cloned Skeleton will match
761 // the ordering of Nodes in the original that was copied.
762
763 SkeletonPtr skel = Skeleton::create();
764 skel->createJointAndBodyNodePair<FreeJoint>(nullptr);
765 skel->createJointAndBodyNodePair<FreeJoint>(nullptr);
766 skel->createJointAndBodyNodePair<FreeJoint>(nullptr);
767
768 // Add Nodes in the reverse order, so that their indexing is different from
769 // the BodyNodes they are attached to
770 for (int i = skel->getNumBodyNodes() - 1; i > 0; --i)
771 {
772 skel->getBodyNode(i)->createEndEffector("manip_" + std::to_string(i));
773 }
774
775 skel->getBodyNode(1)->createEndEffector("other_manip");
776 skel->getBodyNode(0)->createEndEffector("another_manip");
777 skel->getBodyNode(2)->createEndEffector("yet_another_manip");
778
779 SkeletonPtr clone = skel->cloneSkeleton();
780
781 for (std::size_t i = 0; i < skel->getNumEndEffectors(); ++i)
782 {
783 EXPECT_EQ(
784 skel->getEndEffector(i)->getName(),
785 clone->getEndEffector(i)->getName());
786 }
787 }
788
789 TEST(Skeleton, ZeroDofJointInReferential)
790 {
791 // This is a regression test which makes sure that the BodyNodes of
792 // ZeroDofJoints will be correctly included in linkages.
793 SkeletonPtr skel = Skeleton::create();
794
795 BodyNode* bn = skel->createJointAndBodyNodePair<RevoluteJoint>().second;
796 BodyNode* zeroDof1 = skel->createJointAndBodyNodePair<WeldJoint>(bn).second;
797 bn = skel->createJointAndBodyNodePair<PrismaticJoint>(zeroDof1).second;
798 BodyNode* zeroDof2 = skel->createJointAndBodyNodePair<WeldJoint>(bn).second;
799
800 BranchPtr branch = Branch::create(skel->getBodyNode(0));
801 EXPECT_EQ(branch->getNumBodyNodes(), skel->getNumBodyNodes());
802 EXPECT_FALSE(branch->getIndexOf(zeroDof1) == INVALID_INDEX);
803 EXPECT_FALSE(branch->getIndexOf(zeroDof2) == INVALID_INDEX);
804 }
805
806 TEST(Skeleton, ZeroDofJointConstraintForces)
807 {
808 // This is a regression test which makes sure that the BodyNodes of
809 // ZeroDofJoints will be correctly aggregate constraint forces.
810 SkeletonPtr skel = Skeleton::create();
811
812 BodyNode* bn = skel->createJointAndBodyNodePair<RevoluteJoint>().second;
813 BodyNode* zeroDof1 = skel->createJointAndBodyNodePair<WeldJoint>(bn).second;
814 bn = skel->createJointAndBodyNodePair<PrismaticJoint>(zeroDof1).second;
815 skel->createJointAndBodyNodePair<WeldJoint>(bn);
816
817 const auto numSkelDofs = skel->getNumDofs();
818 for (auto& bodyNode : skel->getBodyNodes())
819 bodyNode->setConstraintImpulse(Eigen::Vector6d::Random());
820
821 // Make sure this does not cause seg-fault
822 Eigen::VectorXd constraintForces = skel->getConstraintForces();
823 EXPECT_EQ(constraintForces.size(), static_cast<int>(numSkelDofs));
824 }
825
826 TEST(Skeleton, Configurations)
827 {
828 SkeletonPtr twoLink = createTwoLinkRobot(
829 Vector3d::Ones(), DOF_YAW, Vector3d::Ones(), DOF_ROLL);
830
831 SkeletonPtr threeLink = createThreeLinkRobot(
832 Vector3d::Ones(),
833 DOF_PITCH,
834 Vector3d::Ones(),
835 DOF_ROLL,
836 Vector3d::Ones(),
837 DOF_YAW);
838
839 Skeleton::Configuration c2 = twoLink->getConfiguration();
840 Skeleton::Configuration c3 = threeLink->getConfiguration();
841
842 EXPECT_FALSE(c2 == c3);
843 EXPECT_TRUE(c2 == c2);
844 EXPECT_TRUE(c3 == c3);
845 EXPECT_TRUE(c2 != c3);
846
847 twoLink->setPosition(0, 1.0);
848 EXPECT_FALSE(c2 == twoLink->getConfiguration());
849
850 threeLink->setPosition(1, 2.0);
851 EXPECT_FALSE(c3 == twoLink->getConfiguration());
852
853 c2 = twoLink->getConfiguration(Skeleton::CONFIG_VELOCITIES);
854 EXPECT_TRUE(c2.mPositions.size() == 0);
855 EXPECT_TRUE(c2.mVelocities.size() == static_cast<int>(twoLink->getNumDofs()));
856 EXPECT_TRUE(c2.mAccelerations.size() == 0);
857 }
858
859 TEST(Skeleton, LinearJacobianDerivOverload)
860 {
861 // Regression test for #626: Make sure that getLinearJacobianDeriv's overload
862 // is working appropriately.
863 SkeletonPtr skeleton = createThreeLinkRobot(
864 Vector3d::Ones(),
865 DOF_PITCH,
866 Vector3d::Ones(),
867 DOF_ROLL,
868 Vector3d::Ones(),
869 DOF_YAW);
870
871 skeleton->getLinearJacobianDeriv(skeleton->getBodyNode(0));
872
873 LinkagePtr linkage = Branch::create(skeleton->getBodyNode(0));
874 linkage->getLinearJacobianDeriv(linkage->getBodyNode(0));
875 }
876
877 TEST(Skeleton, Updating)
878 {
879 // Make sure that structural properties get automatically updated correctly
880
881 // RevoluteJoint
882 SkeletonPtr skeleton = createTwoLinkRobot(
883 Vector3d::Ones(), DOF_PITCH, Vector3d::Ones(), DOF_ROLL);
884
885 Joint* joint0 = skeleton->getJoint(0);
886 Joint* joint1 = skeleton->getJoint(1);
887
888 math::Jacobian J0i = joint0->getRelativeJacobian();
889 joint0->get<RevoluteJoint::Aspect>()->setProperties(
890 joint1->get<RevoluteJoint::Aspect>()->getProperties());
891
892 math::Jacobian J0f = joint0->getRelativeJacobian();
893 EXPECT_FALSE(equals(J0i, J0f));
894
895 // PrismaticJoint
896 skeleton
897 = createTwoLinkRobot(Vector3d::Ones(), DOF_X, Vector3d::Ones(), DOF_Y);
898 joint0 = skeleton->getJoint(0);
899 joint1 = skeleton->getJoint(1);
900
901 J0i = joint0->getRelativeJacobian();
902 joint0->get<PrismaticJoint::Aspect>()->setProperties(
903 joint1->get<PrismaticJoint::Aspect>()->getProperties());
904 J0f = joint0->getRelativeJacobian();
905 EXPECT_FALSE(equals(J0i, J0f));
906
907 skeleton = Skeleton::create();
908 ScrewJoint* screw = skeleton->createJointAndBodyNodePair<ScrewJoint>().first;
909
910 screw->setAxis(Eigen::Vector3d::UnitX());
911 screw->setPitch(2);
912
913 J0i = screw->getRelativeJacobian();
914 screw->setAxis(Eigen::Vector3d::UnitY());
915 J0f = screw->getRelativeJacobian();
916 EXPECT_FALSE(equals(J0i, J0f));
917
918 J0i = J0f;
919 screw->setPitch(3);
920 J0f = screw->getRelativeJacobian();
921 EXPECT_FALSE(equals(J0i, J0f));
922
923 // Regression test for Pull Request #731
924 const double originalMass = skeleton->getMass();
925 BodyNode* lastBn = skeleton->getBodyNode(skeleton->getNumBodyNodes() - 1);
926 const double removedMass = lastBn->getMass();
927 EXPECT_FALSE(removedMass == 0.0);
928 lastBn->remove();
929 const double newMass = skeleton->getMass();
930 EXPECT_FALSE(originalMass == newMass);
931 EXPECT_TRUE(newMass == originalMass - removedMass);
932 }