Debian Codegraph @

unittests/integration/test_MetaSkeleton.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 //==============================================================================
53 std::vector<common::Uri> getFileList()
54 {
55 std::vector<common::Uri> fileList;
56 fileList.push_back("dart://sample/skel/test/chainwhipa.skel");
57 fileList.push_back("dart://sample/skel/test/single_pendulum.skel");
58 fileList.push_back(
59 "dart://sample/skel/test/single_pendulum_euler_joint.skel");
60 fileList.push_back("dart://sample/skel/test/single_pendulum_ball_joint.skel");
61 fileList.push_back("dart://sample/skel/test/double_pendulum.skel");
62 fileList.push_back(
63 "dart://sample/skel/test/double_pendulum_euler_joint.skel");
64 fileList.push_back("dart://sample/skel/test/double_pendulum_ball_joint.skel");
65 fileList.push_back(
66 "dart://sample/skel/test/serial_chain_revolute_joint.skel");
67 fileList.push_back(
68 "dart://sample/skel/test/serial_chain_eulerxyz_joint.skel");
69 fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint.skel");
70 fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_20.skel");
71 fileList.push_back("dart://sample/skel/test/serial_chain_ball_joint_40.skel");
72 fileList.push_back("dart://sample/skel/test/simple_tree_structure.skel");
73 fileList.push_back(
74 "dart://sample/skel/test/simple_tree_structure_euler_joint.skel");
75 fileList.push_back(
76 "dart://sample/skel/test/simple_tree_structure_ball_joint.skel");
77 fileList.push_back("dart://sample/skel/test/tree_structure.skel");
78 fileList.push_back("dart://sample/skel/test/tree_structure_euler_joint.skel");
79 fileList.push_back("dart://sample/skel/test/tree_structure_ball_joint.skel");
80 fileList.push_back("dart://sample/skel/fullbody1.skel");
81
82 return fileList;
83 }
84
85 //==============================================================================
86 std::vector<SkeletonPtr> getSkeletons()
87 {
88 const auto fileList = getFileList();
89
90 std::vector<WorldPtr> worlds;
91 for (std::size_t i = 0; i < fileList.size(); ++i)
92 worlds.push_back(utils::SkelParser::readWorld(fileList[i]));
93
94 std::vector<SkeletonPtr> skeletons;
95 for (std::size_t i = 0; i < worlds.size(); ++i)
96 {
97 WorldPtr world = worlds[i];
98 for (std::size_t j = 0; j < world->getNumSkeletons(); ++j)
99 skeletons.push_back(world->getSkeleton(j));
100 }
101
102 return skeletons;
103 }
104
105 //==============================================================================
106 TEST(MetaSkeleton, Referential)
107 {
108 std::vector<SkeletonPtr> skeletons = getSkeletons();
109
110 #ifndef NDEBUG // Debug mode
111 std::size_t numIterations = 1;
112 #else // Release mode
113 std::size_t numIterations = 20;
114 #endif
115
116 for (std::size_t i = 0; i < skeletons.size(); ++i)
117 {
118 SkeletonPtr skeleton = skeletons[i];
119
120 const auto& skelJoints = skeleton->getJoints();
121 EXPECT_TRUE(skeleton->getNumJoints() == skelJoints.size());
122 for (auto* joint : skelJoints)
123 EXPECT_TRUE(skeleton->hasJoint(joint));
124
125 for (std::size_t j = 0; j < skeleton->getNumTrees(); ++j)
126 {
127 BranchPtr tree = Branch::create(skeleton->getRootBodyNode(j));
128
129 const std::vector<BodyNode*>& skelBns = skeleton->getTreeBodyNodes(j);
130 EXPECT_TRUE(tree->getNumBodyNodes() == skelBns.size());
131 for (BodyNode* bn : skelBns)
132 {
133 EXPECT_FALSE(tree->getIndexOf(bn) == INVALID_INDEX);
134 EXPECT_TRUE(tree->getBodyNode(tree->getIndexOf(bn)) == bn);
135 EXPECT_TRUE(skeleton->hasBodyNode(bn));
136 }
137
138 const std::vector<DegreeOfFreedom*>& skelDofs = skeleton->getTreeDofs(j);
139 EXPECT_TRUE(tree->getNumDofs() == skelDofs.size());
140 for (DegreeOfFreedom* dof : skelDofs)
141 {
142 EXPECT_FALSE(tree->getIndexOf(dof) == INVALID_INDEX);
143 EXPECT_TRUE(tree->getDof(tree->getIndexOf(dof)) == dof);
144 }
145
146 Eigen::VectorXd q = tree->getPositions();
147 Eigen::VectorXd dq = tree->getVelocities();
148 Eigen::VectorXd ddq = tree->getAccelerations();
149
150 for (std::size_t k = 0; k < numIterations; ++k)
151 {
152 for (int r = 0; r < q.size(); ++r)
153 {
154 q[r] = math::Random::uniform<double>(-10, 10);
155 dq[r] = math::Random::uniform<double>(-10, 10);
156 ddq[r] = math::Random::uniform<double>(-10, 10);
157 }
158
159 tree->setPositions(q);
160 tree->setVelocities(dq);
161 tree->setAccelerations(ddq);
162
163 EXPECT_TRUE(equals(q, tree->getPositions(), 0.0));
164 EXPECT_TRUE(equals(dq, tree->getVelocities(), 0.0));
165 EXPECT_TRUE(equals(ddq, tree->getAccelerations(), 0.0));
166
167 const Eigen::MatrixXd& skelMassMatrix = skeleton->getMassMatrix();
168 const Eigen::MatrixXd& treeMassMatrix = tree->getMassMatrix();
169
170 const Eigen::MatrixXd& skelAugM = skeleton->getAugMassMatrix();
171 const Eigen::MatrixXd& treeAugM = tree->getAugMassMatrix();
172
173 const Eigen::MatrixXd& skelInvM = skeleton->getInvMassMatrix();
174 const Eigen::MatrixXd& treeInvM = tree->getInvMassMatrix();
175
176 const Eigen::MatrixXd& skelInvAugM = skeleton->getInvAugMassMatrix();
177 const Eigen::MatrixXd& treeInvAugM = tree->getInvAugMassMatrix();
178
179 const Eigen::VectorXd& skelCvec = skeleton->getCoriolisForces();
180 const Eigen::VectorXd& treeCvec = tree->getCoriolisForces();
181
182 const Eigen::VectorXd& skelFg = skeleton->getGravityForces();
183 const Eigen::VectorXd& treeFg = tree->getGravityForces();
184
185 const Eigen::VectorXd& skelCg = skeleton->getCoriolisAndGravityForces();
186 const Eigen::VectorXd& treeCg = tree->getCoriolisAndGravityForces();
187
188 const Eigen::VectorXd& skelFext = skeleton->getExternalForces();
189 const Eigen::VectorXd& treeFext = tree->getExternalForces();
190
191 const Eigen::VectorXd& skelFc = skeleton->getConstraintForces();
192 const Eigen::VectorXd& treeFc = tree->getConstraintForces();
193
194 const std::size_t nDofs = tree->getNumDofs();
195 for (std::size_t r1 = 0; r1 < nDofs; ++r1)
196 {
197 const std::size_t sr1 = tree->getDof(r1)->getIndexInSkeleton();
198 for (std::size_t r2 = 0; r2 < nDofs; ++r2)
199 {
200 const std::size_t sr2 = tree->getDof(r2)->getIndexInSkeleton();
201
202 EXPECT_TRUE(skelMassMatrix(sr1, sr2) == treeMassMatrix(r1, r2));
203 EXPECT_TRUE(skelAugM(sr1, sr2) == treeAugM(r1, r2));
204 EXPECT_TRUE(skelInvM(sr1, sr2) == treeInvM(r1, r2));
205 EXPECT_TRUE(skelInvAugM(sr1, sr2) == treeInvAugM(r1, r2));
206 }
207
208 EXPECT_TRUE(skelCvec[sr1] == treeCvec[r1]);
209 EXPECT_TRUE(skelFg[sr1] == treeFg[r1]);
210 EXPECT_TRUE(skelCg[sr1] == treeCg[r1]);
211 EXPECT_TRUE(skelFext[sr1] == treeFext[r1]);
212 EXPECT_TRUE(skelFext[sr1] == treeFext[r1]);
213 EXPECT_TRUE(skelFc[sr1] == treeFc[r1]);
214 }
215
216 const std::size_t numBodyNodes = tree->getNumBodyNodes();
217 for (std::size_t m = 0; m < numBodyNodes; ++m)
218 {
219 const BodyNode* bn = tree->getBodyNode(m);
220 const Eigen::MatrixXd Jtree = tree->getJacobian(bn);
221 const Eigen::MatrixXd Jskel = skeleton->getJacobian(bn);
222
223 for (std::size_t r2 = 0; r2 < nDofs; ++r2)
224 {
225 const std::size_t sr2 = tree->getDof(r2)->getIndexInSkeleton();
226 for (std::size_t r1 = 0; r1 < 6; ++r1)
227 {
228 EXPECT_TRUE(Jtree(r1, r2) == Jskel(r1, sr2));
229 }
230 }
231 }
232 }
233 }
234 }
235 }
236
237 //==============================================================================
238 template <class JointType = RevoluteJoint>
239 BodyNode* addBodyNode(BodyNode* bn, const std::string& name)
240 {
241 BodyNode* result = bn->createChildJointAndBodyNodePair<JointType>().second;
242 result->setName(name);
243 return result;
244 }
245
246 //==============================================================================
247 SkeletonPtr constructLinkageTestSkeleton()
248 {
249 // Skeleton structure:
250 //
251 // c1b1 (root)
252 // |
253 // c1b2
254 // |
255 // c1b3
256 // +-------+--------+
257 // c2b1 c3b1 c5b1
258 // | | |
259 // c2b2 c3b2 c5b2
260 // | |
261 // c2b3 c3b3
262 // +---+---+
263 // c4b1 c3b4
264 // |
265 // c4b2
266 // |
267 // c4b3
268 //
269
270 SkeletonPtr skel = Skeleton::create();
271 BodyNode* bn = skel->createJointAndBodyNodePair<RevoluteJoint>().second;
272 bn->setName("c1b1");
273 bn = addBodyNode<FreeJoint>(bn, "c1b2");
274
275 BodyNode* c1b3 = addBodyNode(bn, "c1b3");
276 bn = addBodyNode(c1b3, "c2b1");
277 bn = addBodyNode(bn, "c2b2");
278 addBodyNode(bn, "c2b3");
279
280 bn = addBodyNode(c1b3, "c3b1");
281 bn = addBodyNode(bn, "c3b2");
282 BodyNode* c3b3 = addBodyNode(bn, "c3b3");
283 bn = addBodyNode(c3b3, "c4b1");
284 bn = addBodyNode(bn, "c4b2");
285 addBodyNode(bn, "c4b3");
286 addBodyNode(c3b3, "c3b4");
287
288 bn = addBodyNode(c1b3, "c5b1");
289 addBodyNode(bn, "c5b2");
290
291 return skel;
292 }
293
294 //==============================================================================
295 void checkForBodyNodes(
296 std::size_t& /*count*/,
297 const ReferentialSkeletonPtr& /*refSkel*/,
298 const SkeletonPtr& /*skel*/)
299 {
300 // Do nothing
301 }
302
303 //==============================================================================
304 // Variadic function for testing a ReferentialSkeleton for a series of BodyNode
305 // names
306 template <typename... Args>
307 void checkForBodyNodes(
308 std::size_t& count,
309 const ReferentialSkeletonPtr& refSkel,
310 const SkeletonPtr& skel,
311 const std::string& name,
312 Args... args)
313 {
314 bool contains = refSkel->getIndexOf(skel->getBodyNode(name)) != INVALID_INDEX;
315 EXPECT_TRUE(contains);
316 if (!contains)
317 {
318 dtmsg << "The ReferentialSkeleton [" << refSkel->getName() << "] does NOT "
319 << "contain the BodyNode [" << name << "] of the Skeleton ["
320 << skel->getName() << "]\n";
321 }
322
323 ++count;
324 checkForBodyNodes(count, refSkel, skel, args...);
325 }
326
327 //==============================================================================
328 template <typename... Args>
329 std::size_t checkForBodyNodes(
330 const ReferentialSkeletonPtr& refSkel,
331 const SkeletonPtr& skel,
332 bool checkCount,
333 Args... args)
334 {
335 std::size_t count = 0;
336 checkForBodyNodes(count, refSkel, skel, args...);
337
338 if (checkCount)
339 {
340 bool countValid = (count == refSkel->getNumBodyNodes());
341 EXPECT_TRUE(countValid);
342 if (!countValid)
343 {
344 dtmsg << "The number of BodyNodes checked for [" << count << "] "
345 << "does not equal the number [" << refSkel->getNumBodyNodes()
346 << "] in the ReferentialSkeleton [" << refSkel->getName() << "]\n";
347 }
348 }
349
350 return count;
351 }
352
353 //==============================================================================
354 void checkLinkageJointConsistency(const ReferentialSkeletonPtr& refSkel)
355 {
356 EXPECT_TRUE(refSkel->getNumBodyNodes() == refSkel->getNumJoints());
357
358 // Linkages should have the property:
359 // getJoint(i) == getBodyNode(i)->getParentJoint()
360 for (std::size_t i = 0; i < refSkel->getNumJoints(); ++i)
361 {
362 EXPECT_EQ(refSkel->getJoint(i), refSkel->getBodyNode(i)->getParentJoint());
363 EXPECT_EQ(refSkel->getIndexOf(refSkel->getJoint(i)), i);
364 }
365 }
366
367 //==============================================================================
368 TEST(MetaSkeleton, Linkage)
369 {
370 // Test a variety of uses of Linkage::Criteria
371 SkeletonPtr skel = constructLinkageTestSkeleton();
372
373 BranchPtr subtree = Branch::create(skel->getBodyNode("c3b3"), "subtree");
374 checkForBodyNodes(
375 subtree, skel, true, "c3b3", "c3b4", "c4b1", "c4b2", "c4b3");
376
377 ChainPtr midchain = Chain::create(
378 skel->getBodyNode("c1b3"), skel->getBodyNode("c3b4"), "midchain");
379 checkForBodyNodes(midchain, skel, true, "c3b1", "c3b2", "c3b3");
380 checkLinkageJointConsistency(midchain);
381
382 Linkage::Criteria criteria;
383 criteria.mStart = skel->getBodyNode("c5b2");
384 criteria.mTargets.push_back(
385 Linkage::Criteria::Target(skel->getBodyNode("c4b3")));
386 LinkagePtr path = Linkage::create(criteria, "path");
387 checkForBodyNodes(
388 path,
389 skel,
390 true,
391 "c5b2",
392 "c5b1",
393 "c1b3",
394 "c3b1",
395 "c3b2",
396 "c3b3",
397 "c4b1",
398 "c4b2",
399 "c4b3");
400 checkLinkageJointConsistency(path);
401
402 skel->getBodyNode(0)->copyTo(nullptr);
403 criteria.mTargets.clear();
404 criteria.mStart = skel->getBodyNode("c3b1");
405 criteria.mStart.mPolicy = Linkage::Criteria::UPSTREAM;
406 criteria.mTargets.push_back(Linkage::Criteria::Target(
407 skel->getBodyNode("c3b1(1)"), Linkage::Criteria::UPSTREAM));
408
409 LinkagePtr combinedTreeBases = Linkage::create(criteria, "combinedTreeBases");
410 checkForBodyNodes(
411 combinedTreeBases,
412 skel,
413 true,
414 "c3b1",
415 "c1b3",
416 "c2b1",
417 "c2b2",
418 "c2b3",
419 "c3b1(1)",
420 "c1b3(1)",
421 "c2b1(1)",
422 "c2b2(1)",
423 "c2b3(1)",
424 "c5b1",
425 "c5b2",
426 "c1b2",
427 "c1b1",
428 "c5b1(1)",
429 "c5b2(1)",
430 "c1b2(1)",
431 "c1b1(1)");
432 checkLinkageJointConsistency(combinedTreeBases);
433
434 SkeletonPtr skel2 = skel->getBodyNode(0)->copyAs("skel2");
435 criteria.mTargets.clear();
436 criteria.mTargets.push_back(Linkage::Criteria::Target(
437 skel2->getBodyNode("c3b1"), Linkage::Criteria::UPSTREAM));
438 LinkagePtr combinedSkelBases = Linkage::create(criteria, "combinedSkelBases");
439 std::size_t count = 0;
440 count += checkForBodyNodes(
441 combinedSkelBases,
442 skel,
443 false,
444 "c3b1",
445 "c1b3",
446 "c2b1",
447 "c2b2",
448 "c2b3",
449 "c5b1",
450 "c5b2",
451 "c1b2",
452 "c1b1");
453 count += checkForBodyNodes(
454 combinedSkelBases,
455 skel2,
456 false,
457 "c3b1",
458 "c1b3",
459 "c2b1",
460 "c2b2",
461 "c2b3",
462 "c5b1",
463 "c5b2",
464 "c1b2",
465 "c1b1");
466 EXPECT_TRUE(count == combinedSkelBases->getNumBodyNodes());
467
468 ChainPtr downstreamFreeJoint = Chain::create(
469 skel->getBodyNode("c1b1"),
470 skel->getBodyNode("c1b3"),
471 Chain::IncludeUpstreamParentJoint,
472 "downstreamFreeJoint");
473 checkForBodyNodes(downstreamFreeJoint, skel, true, "c1b1");
474 checkLinkageJointConsistency(downstreamFreeJoint);
475
476 ChainPtr emptyChain = Chain::create(
477 skel->getBodyNode("c1b1"), skel->getBodyNode("c1b3"), "emptyChain");
478 checkForBodyNodes(emptyChain, skel, true);
479 checkLinkageJointConsistency(emptyChain);
480
481 ChainPtr chainFromNull
482 = Chain::create(nullptr, skel->getBodyNode("c1b2"), "chainFromNull");
483 checkForBodyNodes(chainFromNull, skel, true, "c1b1");
484 checkLinkageJointConsistency(chainFromNull);
485
486 ChainPtr upstreamFreeJoint = Chain::create(
487 skel->getBodyNode("c1b3"),
488 skel->getBodyNode("c1b1"),
489 "upstreamFreeJoint");
490 checkForBodyNodes(upstreamFreeJoint, skel, true, "c1b3", "c1b2");
491 checkLinkageJointConsistency(upstreamFreeJoint);
492
493 // Using nullptr as the target should bring us towards the root of the tree
494 ChainPtr upTowardsRoot
495 = Chain::create(skel->getBodyNode("c1b3"), nullptr, "upTowardsRoot");
496 checkForBodyNodes(upTowardsRoot, skel, true, "c1b3", "c1b2");
497 checkLinkageJointConsistency(upTowardsRoot);
498
499 criteria.mTargets.clear();
500 criteria.mTargets.push_back(skel->getBodyNode("c4b3"));
501 criteria.mStart = skel->getBodyNode("c1b3");
502 criteria.mTerminals.push_back(skel->getBodyNode("c3b2"));
503 LinkagePtr terminatedLinkage = Linkage::create(criteria, "terminatedLinkage");
504 checkForBodyNodes(terminatedLinkage, skel, true, "c1b3", "c3b1", "c3b2");
505 checkLinkageJointConsistency(terminatedLinkage);
506
507 criteria.mStart = skel->getBodyNode("c1b1");
508 criteria.mStart.mPolicy = Linkage::Criteria::DOWNSTREAM;
509 criteria.mTargets.clear();
510 criteria.mTerminals.clear();
511 criteria.mTerminals.push_back(
512 Linkage::Criteria::Terminal(skel->getBodyNode("c2b1"), false));
513 criteria.mTerminals.push_back(skel->getBodyNode("c3b3"));
514 LinkagePtr terminatedSubtree = Linkage::create(criteria, "terminatedSubtree");
515 checkForBodyNodes(
516 terminatedSubtree,
517 skel,
518 true,
519 "c1b1",
520 "c1b2",
521 "c1b3",
522 "c5b1",
523 "c5b2",
524 "c3b1",
525 "c3b2",
526 "c3b3");
527 checkLinkageJointConsistency(terminatedSubtree);
528
529 criteria.mStart.mPolicy = Linkage::Criteria::UPSTREAM;
530 criteria.mStart.mNode = skel->getBodyNode("c3b1");
531 LinkagePtr terminatedUpstream
532 = Linkage::create(criteria, "terminatedUpstream");
533 checkForBodyNodes(
534 terminatedUpstream,
535 skel,
536 true,
537 "c3b1",
538 "c1b3",
539 "c5b1",
540 "c5b2",
541 "c1b2",
542 "c1b1");
543 checkLinkageJointConsistency(terminatedUpstream);
544
545 // Sequence linkage 1
546 Linkage::Criteria sequenceCriteria1(
547 skel->getBodyNode("c1b3"), skel->getBodyNode("c4b1"), true);
548 LinkagePtr sequenceLinkage1 = Linkage::create(sequenceCriteria1, "sequence");
549 checkForBodyNodes(
550 sequenceLinkage1, skel, true, "c1b3", "c3b1", "c3b2", "c3b3", "c4b1");
551 checkLinkageJointConsistency(sequenceLinkage1);
552
553 // Sequence linkage 2: nullptr start
554 Linkage::Criteria sequenceCriteria2(nullptr, skel->getBodyNode("c4b1"), true);
555 LinkagePtr sequenceLinkage2 = Linkage::create(sequenceCriteria2, "sequence");
556 checkForBodyNodes(
557 sequenceLinkage2,
558 skel,
559 true,
560 "c1b1",
561 "c1b2",
562 "c1b3",
563 "c3b1",
564 "c3b2",
565 "c3b3",
566 "c4b1");
567 checkLinkageJointConsistency(sequenceLinkage2);
568
569 // Sequence linkage 3: nullptr target
570 Linkage::Criteria sequenceCriteria3(skel->getBodyNode("c1b3"), nullptr, true);
571 LinkagePtr sequenceLinkage3 = Linkage::create(sequenceCriteria3, "sequence");
572 checkForBodyNodes(sequenceLinkage3, skel, true, "c1b1", "c1b2", "c1b3");
573 checkLinkageJointConsistency(sequenceLinkage3);
574
575 // Sequence linkage 4: reversed
576 Linkage::Criteria sequenceCriteria4(
577 skel->getBodyNode("c4b1"), skel->getBodyNode("c1b3"), true);
578 LinkagePtr sequenceLinkage4 = Linkage::create(sequenceCriteria4, "sequence");
579 checkForBodyNodes(
580 sequenceLinkage4, skel, true, "c1b3", "c3b1", "c3b2", "c3b3", "c4b1");
581 checkLinkageJointConsistency(sequenceLinkage4);
582
583 // Sequence linkage 5: empty
584 Linkage::Criteria sequenceCriteria5(nullptr, nullptr, true);
585 LinkagePtr sequenceLinkage5 = Linkage::create(sequenceCriteria5, "sequence");
586 checkForBodyNodes(sequenceLinkage5, skel, true);
587 checkLinkageJointConsistency(sequenceLinkage5);
588
589 // Sequence linkage 6: not include both
590 Linkage::Criteria sequenceCriteria6(
591 skel->getBodyNode("c1b3"), skel->getBodyNode("c4b1"), false);
592 LinkagePtr sequenceLinkage6 = Linkage::create(sequenceCriteria6, "sequence");
593 checkForBodyNodes(
594 sequenceLinkage6, skel, true, "c3b1", "c3b2", "c3b3", "c4b1");
595 checkLinkageJointConsistency(sequenceLinkage6);
596 }
597
598 //==============================================================================
599 TEST(MetaSkeleton, Group)
600 {
601 SkeletonPtr skel = constructLinkageTestSkeleton();
602
603 // Make twice as many BodyNodes in the Skeleton
604 SkeletonPtr skel2 = constructLinkageTestSkeleton();
605 skel2->getRootBodyNode()->moveTo(skel, nullptr);
606
607 // Test nullptr construction
608 GroupPtr nullGroup = Group::create("null_group", nullptr);
609 EXPECT_EQ(nullGroup->getNumBodyNodes(), 0u);
610 EXPECT_EQ(nullGroup->getNumJoints(), 0u);
611 EXPECT_EQ(nullGroup->getNumDofs(), 0u);
612
613 // Test conversion from Skeleton
614 GroupPtr skel1Group = Group::create("skel1_group", skel);
615 EXPECT_EQ(skel1Group->getNumBodyNodes(), skel->getNumBodyNodes());
616 EXPECT_EQ(skel1Group->getNumJoints(), skel->getNumJoints());
617 EXPECT_EQ(skel1Group->getNumDofs(), skel->getNumDofs());
618
619 for (std::size_t i = 0; i < skel1Group->getNumBodyNodes(); ++i)
620 EXPECT_EQ(skel1Group->getBodyNode(i), skel->getBodyNode(i));
621
622 for (std::size_t i = 0; i < skel1Group->getNumJoints(); ++i)
623 EXPECT_EQ(skel1Group->getJoint(i), skel->getJoint(i));
624
625 for (std::size_t i = 0; i < skel1Group->getNumDofs(); ++i)
626 EXPECT_EQ(skel1Group->getDof(i), skel->getDof(i));
627
628 // Test arbitrary Groups by plucking random BodyNodes, Joints, and
629 // DegreesOfFreedom from a Skeleton.
630 GroupPtr group = Group::create();
631 std::vector<BodyNode*> bodyNodes;
632 std::vector<Joint*> joints;
633 std::vector<DegreeOfFreedom*> dofs;
634 for (std::size_t i = 0; i < 2 * skel->getNumBodyNodes(); ++i)
635 {
636 std::size_t randomIndex
637 = Random::uniform<std::size_t>(0, skel->getNumBodyNodes() - 1);
638 BodyNode* bn = skel->getBodyNode(randomIndex);
639 if (group->addBodyNode(bn, false))
640 bodyNodes.push_back(bn);
641
642 randomIndex = Random::uniform<std::size_t>(0, skel->getNumJoints() - 1);
643 Joint* joint = skel->getJoint(randomIndex);
644 if (group->addJoint(joint, false, false))
645 joints.push_back(joint);
646
647 randomIndex = Random::uniform<std::size_t>(0, skel->getNumDofs() - 1);
648 DegreeOfFreedom* dof = skel->getDof(randomIndex);
649 if (group->addDof(dof, false, false))
650 dofs.push_back(dof);
651 }
652
653 EXPECT_EQ(group->getNumBodyNodes(), bodyNodes.size());
654 EXPECT_EQ(group->getNumJoints(), joints.size());
655 EXPECT_EQ(group->getNumDofs(), dofs.size());
656
657 for (std::size_t i = 0; i < group->getNumBodyNodes(); ++i)
658 EXPECT_EQ(group->getBodyNode(i), bodyNodes[i]);
659
660 for (std::size_t i = 0; i < group->getNumJoints(); ++i)
661 EXPECT_EQ(group->getJoint(i), joints[i]);
662
663 for (std::size_t i = 0; i < group->getNumDofs(); ++i)
664 EXPECT_EQ(group->getDof(i), dofs[i]);
665 }
666
667 //==============================================================================
668 TEST(MetaSkeleton, LockSkeletonMutexesWithLockGuard)
669 {
670 // Test a variety of uses of Linkage::Criteria
671 SkeletonPtr skel = constructLinkageTestSkeleton();
672
673 BranchPtr subtree = Branch::create(skel->getBodyNode("c3b3"), "subtree");
674 checkForBodyNodes(
675 subtree, skel, true, "c3b3", "c3b4", "c4b1", "c4b2", "c4b3");
676
677 ChainPtr midchain = Chain::create(
678 skel->getBodyNode("c1b3"), skel->getBodyNode("c3b4"), "midchain");
679 checkForBodyNodes(midchain, skel, true, "c3b1", "c3b2", "c3b3");
680 checkLinkageJointConsistency(midchain);
681
682 {
683 auto mutex = subtree->getLockableReference();
684 std::lock_guard<common::LockableReference> lock(*mutex);
685 }
686
687 {
688 auto mutex = midchain->getLockableReference();
689 std::lock_guard<common::LockableReference> lock(*mutex);
690 }
691 }
692
693 //==============================================================================
694 TEST(MetaSkeleton, GetJointsAndBodyNodes)
695 {
696 auto skelA = Skeleton::create();
697 auto skelB = Skeleton::create();
698
699 BodyNode* bodyNodeA0;
700 BodyNode* bodyNodeA1;
701 BodyNode* bodyNodeA2;
702
703 BodyNode* bodyNodeB0;
704 BodyNode* bodyNodeB1;
705 BodyNode* bodyNodeB2;
706
707 Joint* jointA0;
708 Joint* jointA1;
709 Joint* jointA2;
710
711 Joint* jointB0;
712 Joint* jointB1;
713 Joint* jointB2;
714
715 std::tie(jointA0, bodyNodeA0)
716 = skelA->createJointAndBodyNodePair<FreeJoint>();
717 std::tie(jointA1, bodyNodeA1)
718 = skelA->createJointAndBodyNodePair<FreeJoint>(bodyNodeA0);
719 std::tie(jointA2, bodyNodeA2)
720 = skelA->createJointAndBodyNodePair<FreeJoint>(bodyNodeA1);
721
722 std::tie(jointB0, bodyNodeB0)
723 = skelB->createJointAndBodyNodePair<FreeJoint>();
724 std::tie(jointB1, bodyNodeB1)
725 = skelB->createJointAndBodyNodePair<FreeJoint>(bodyNodeB0);
726 std::tie(jointB2, bodyNodeB2)
727 = skelB->createJointAndBodyNodePair<FreeJoint>(bodyNodeB1);
728
729 EXPECT_TRUE(skelA->getNumBodyNodes() == 3u);
730 EXPECT_TRUE(skelB->getNumBodyNodes() == 3u);
731
732 EXPECT_TRUE(skelA->getJoints().size() == 3u);
733 EXPECT_TRUE(skelB->getJoints().size() == 3u);
734
735 bodyNodeA0->setName("bodyNode0");
736 bodyNodeA1->setName("bodyNode1");
737 bodyNodeA2->setName("bodyNode2");
738
739 bodyNodeB0->setName("bodyNode0");
740 bodyNodeB1->setName("bodyNode1");
741 bodyNodeB2->setName("bodyNode2");
742
743 jointA0->setName("joint0");
744 jointA1->setName("joint1");
745 jointA2->setName("joint2");
746
747 jointB0->setName("joint0");
748 jointB1->setName("joint1");
749 jointB2->setName("joint2");
750
751 EXPECT_TRUE(bodyNodeA0 == skelA->getBodyNode(bodyNodeA0->getName()));
752 EXPECT_TRUE(bodyNodeA1 == skelA->getBodyNode(bodyNodeA1->getName()));
753 EXPECT_TRUE(bodyNodeA2 == skelA->getBodyNode(bodyNodeA2->getName()));
754
755 EXPECT_TRUE(bodyNodeB0 == skelB->getBodyNode(bodyNodeB0->getName()));
756 EXPECT_TRUE(bodyNodeB1 == skelB->getBodyNode(bodyNodeB1->getName()));
757 EXPECT_TRUE(bodyNodeB2 == skelB->getBodyNode(bodyNodeB2->getName()));
758
759 EXPECT_TRUE(skelA->getBodyNodes("wrong name").empty());
760 EXPECT_TRUE(skelB->getBodyNodes("wrong name").empty());
761
762 EXPECT_TRUE(skelA->getBodyNodes(bodyNodeA0->getName()).size() == 1u);
763 EXPECT_TRUE(skelA->getBodyNodes(bodyNodeA1->getName()).size() == 1u);
764 EXPECT_TRUE(skelA->getBodyNodes(bodyNodeA2->getName()).size() == 1u);
765
766 EXPECT_TRUE(skelB->getBodyNodes(bodyNodeB0->getName()).size() == 1u);
767 EXPECT_TRUE(skelB->getBodyNodes(bodyNodeB1->getName()).size() == 1u);
768 EXPECT_TRUE(skelB->getBodyNodes(bodyNodeB2->getName()).size() == 1u);
769
770 EXPECT_TRUE(jointA0 == skelA->getJoint(jointA0->getName()));
771 EXPECT_TRUE(jointA1 == skelA->getJoint(jointA1->getName()));
772 EXPECT_TRUE(jointA2 == skelA->getJoint(jointA2->getName()));
773
774 EXPECT_TRUE(jointB0 == skelB->getJoint(jointB0->getName()));
775 EXPECT_TRUE(jointB1 == skelB->getJoint(jointB1->getName()));
776 EXPECT_TRUE(jointB2 == skelB->getJoint(jointB2->getName()));
777
778 EXPECT_TRUE(skelA->getJoints("wrong name").empty());
779 EXPECT_TRUE(skelB->getJoints("wrong name").empty());
780
781 EXPECT_TRUE(skelA->getJoints(jointA0->getName()).size() == 1u);
782 EXPECT_TRUE(skelA->getJoints(jointA1->getName()).size() == 1u);
783 EXPECT_TRUE(skelA->getJoints(jointA2->getName()).size() == 1u);
784
785 EXPECT_TRUE(skelB->getJoints(jointB0->getName()).size() == 1u);
786 EXPECT_TRUE(skelB->getJoints(jointB1->getName()).size() == 1u);
787 EXPECT_TRUE(skelB->getJoints(jointB2->getName()).size() == 1u);
788
789 auto group = Group::create();
790 group->addBodyNode(bodyNodeA0);
791 group->addBodyNode(bodyNodeB0);
792 group->addJoint(jointA0);
793 group->addJoint(jointB0);
794 EXPECT_TRUE(group->getJoints("wrong name").empty());
795 EXPECT_TRUE(group->getBodyNodes("wrong name").empty());
796 EXPECT_TRUE(
797 group->getBodyNode("bodyNode0") == bodyNodeA0
798 || group->getBodyNode("bodyNode0") == bodyNodeB0);
799 EXPECT_TRUE(
800 group->getJoint("joint0") == jointA0
801 || group->getJoint("joint0") == jointB0);
802 EXPECT_EQ(group->getBodyNodes("bodyNode0").size(), 2u);
803 EXPECT_EQ(group->getBodyNodes("bodyNode1").size(), 0u);
804 EXPECT_EQ(group->getBodyNodes("bodyNode2").size(), 0u);
805 EXPECT_EQ(group->getJoints("joint0").size(), 2u);
806 EXPECT_EQ(group->getJoints("joint1").size(), 0u);
807 EXPECT_EQ(group->getJoints("joint2").size(), 0u);
808 }
809
810 TEST(MetaSkeleton, ReferentialSkeletonHoldsStrongReferencesOfBodyNodes)
811 {
812 // Create skeletons
813 SkeletonPtr skel1 = constructLinkageTestSkeleton();
814 SkeletonPtr skel2 = constructLinkageTestSkeleton();
815 EXPECT_TRUE(skel1->getNumBodyNodes() > 1);
816 EXPECT_TRUE(skel2->getNumBodyNodes() > 2);
817
818 // Create group from the skeletons
819 GroupPtr group = Group::create();
820 group->addBodyNode(skel1->getBodyNode(0));
821 group->addBodyNode(skel2->getBodyNode(1));
822 EXPECT_EQ(group->getNumBodyNodes(), 2);
823
824 const std::string name1 = group->getBodyNode(0)->getName();
825 const std::string name2 = group->getBodyNode(1)->getName();
826 EXPECT_EQ(group->getBodyNode(0)->getName(), name1);
827 EXPECT_EQ(group->getBodyNode(1)->getName(), name2);
828
829 // Release the ownership of skel1 and skel2
830 skel1.reset();
831 skel2.reset();
832
833 // However, bodyNodes (and the Skeletons) are still alive because the group is
834 // alive
835 EXPECT_EQ(group->getBodyNode(0)->getName(), name1);
836 EXPECT_EQ(group->getBodyNode(1)->getName(), name2);
837 }
838
839 //==============================================================================
840 template <typename RefSkelType>
841 void testReferentialSkeletonClone(
842 const RefSkelType& skel, const RefSkelType& skelClone)
843 {
844 EXPECT_NE(skel, nullptr);
845 EXPECT_NE(skelClone, nullptr);
846 EXPECT_EQ(skel->getName(), skelClone->getName());
847 EXPECT_EQ(skel->getNumBodyNodes(), skelClone->getNumBodyNodes());
848 EXPECT_EQ(skel->getNumJoints(), skelClone->getNumJoints());
849 EXPECT_EQ(skel->getNumDofs(), skelClone->getNumDofs());
850
851 // The skeleton instance of group clone should be different from the original,
852 // but the properties should be the same.
853 for (std::size_t i = 0u; i < skel->getNumBodyNodes(); ++i)
854 {
855 const auto& bodyNode = skel->getBodyNode(i);
856 const auto& skel = bodyNode->getSkeleton();
857
858 const auto& bodyNodeClone = skelClone->getBodyNode(i);
859 const auto& skelClone = bodyNodeClone->getSkeleton();
860
861 EXPECT_NE(skel, skelClone);
862 EXPECT_EQ(skel->getNumBodyNodes(), skelClone->getNumBodyNodes());
863 EXPECT_EQ(skel->getNumJoints(), skelClone->getNumJoints());
864 EXPECT_EQ(skel->getNumDofs(), skelClone->getNumDofs());
865 }
866 for (std::size_t i = 0u; i < skel->getNumJoints(); ++i)
867 {
868 const auto& joint = skel->getJoint(i);
869 const auto& skel = joint->getSkeleton();
870
871 const auto& jointClone = skelClone->getJoint(i);
872 const auto& skelClone = jointClone->getSkeleton();
873
874 EXPECT_NE(skel, skelClone);
875 EXPECT_EQ(skel->getNumBodyNodes(), skelClone->getNumBodyNodes());
876 EXPECT_EQ(skel->getNumJoints(), skelClone->getNumJoints());
877 EXPECT_EQ(skel->getNumDofs(), skelClone->getNumDofs());
878 }
879 }
880
881 //==============================================================================
882 TEST(MetaSkeleton, CloneReferentialSkeletons)
883 {
884 SkeletonPtr skel1 = constructLinkageTestSkeleton();
885 SkeletonPtr skel2 = constructLinkageTestSkeleton();
886
887 // Group
888 GroupPtr group = Group::create();
889 group->addBodyNode(skel1->getBodyNode(0));
890 group->addBodyNode(skel2->getBodyNode(0));
891 group->addJoint(skel1->getJoint(1));
892 group->addJoint(skel2->getJoint(1));
893 GroupPtr groupClone
894 = std::dynamic_pointer_cast<Group>(group->cloneMetaSkeleton());
895 testReferentialSkeletonClone(group, groupClone);
896
897 // Linkage
898 Linkage::Criteria criteria;
899 criteria.mStart = skel1->getBodyNode("c5b2");
900 criteria.mTargets.push_back(
901 Linkage::Criteria::Target(skel1->getBodyNode("c4b3")));
902 LinkagePtr linkage = Linkage::create(criteria);
903 LinkagePtr linkageClone
904 = std::dynamic_pointer_cast<Linkage>(linkage->cloneMetaSkeleton());
905 testReferentialSkeletonClone(linkage, linkageClone);
906
907 // Chain
908 ChainPtr chain = Chain::create(
909 skel1->getBodyNode("c1b3"), skel1->getBodyNode("c3b4"), "midchain");
910 ChainPtr chainClone
911 = std::dynamic_pointer_cast<Chain>(chain->cloneMetaSkeleton());
912 testReferentialSkeletonClone(chain, chainClone);
913
914 // Branch
915 BranchPtr branch = Branch::create(skel1->getBodyNode("c3b3"), "subtree");
916 checkForBodyNodes(
917 branch, skel1, true, "c3b3", "c3b4", "c4b1", "c4b2", "c4b3");
918 BranchPtr branchClone
919 = std::dynamic_pointer_cast<Branch>(branch->cloneMetaSkeleton());
920 testReferentialSkeletonClone(branch, branchClone);
921 }