From bf3255b5e0f462086476b4596d5a5486f47d50c9 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 5 Nov 2016 08:35:07 -0400 Subject: [PATCH 1/4] Add test to ensure ZeroDofJoint::getIndexInTree() is not called --- unittests/comprehensive/test_Skeleton.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/unittests/comprehensive/test_Skeleton.cpp b/unittests/comprehensive/test_Skeleton.cpp index 4ebdea0c852af..cec4e973221ab 100644 --- a/unittests/comprehensive/test_Skeleton.cpp +++ b/unittests/comprehensive/test_Skeleton.cpp @@ -765,6 +765,26 @@ TEST(Skeleton, ZeroDofJointInReferential) EXPECT_FALSE(branch->getIndexOf(zeroDof2) == INVALID_INDEX); } +TEST(Skeleton, ZeroDofJointConstraintForces) +{ + // This is a regression test which makes sure that the BodyNodes of + // ZeroDofJoints will be correctly aggregate constraint forces. + SkeletonPtr skel = Skeleton::create(); + + BodyNode* bn = skel->createJointAndBodyNodePair().second; + BodyNode* zeroDof1 = skel->createJointAndBodyNodePair(bn).second; + bn = skel->createJointAndBodyNodePair(zeroDof1).second; + BodyNode* zeroDof2 = skel->createJointAndBodyNodePair(bn).second; + + const auto numSkelDofs = skel->getNumDofs(); + for (auto& bodyNode : skel->getBodyNodes()) + bodyNode->setConstraintImpulse(Eigen::Vector6d::Random()); + + // Make sure this does not cause seg-fault + Eigen::VectorXd constraintForces = skel->getConstraintForces(); + EXPECT_EQ(constraintForces.size(), static_cast(numSkelDofs)); +} + TEST(Skeleton, Referential) { std::vector skeletons = getSkeletons(); From 7f69002d07ba264d0c36c850e8310b256039e3e6 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 5 Nov 2016 08:38:30 -0400 Subject: [PATCH 2/4] Fix that ZeroDofJoint::getIndexInTree() was called --- dart/dynamics/BodyNode.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 99a6876a0c734..4c2b755a2118e 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -2092,9 +2092,13 @@ void BodyNode::aggregateSpatialToGeneralized(Eigen::VectorXd& _generalized, } // Project the spatial quantity to generalized coordinates - std::size_t iStart = mParentJoint->getIndexInTree(0); - _generalized.segment(iStart, mParentJoint->getNumDofs()) - = mParentJoint->getSpatialToGeneralized(mArbitrarySpatial); + const auto numDofs = mParentJoint->getNumDofs(); + if (numDofs > 0u) + { + const std::size_t iStart = mParentJoint->getIndexInTree(0); + _generalized.segment(iStart, numDofs) + = mParentJoint->getSpatialToGeneralized(mArbitrarySpatial); + } } //============================================================================== From e4f310d663d5d2f1810cff703b3ada379de20173 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 5 Nov 2016 08:52:42 -0400 Subject: [PATCH 3/4] Update changelog --- CHANGELOG.md | 4 ++++ unittests/comprehensive/test_Skeleton.cpp | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 646c0a9ab5ea8..e4e0a07ce462c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,6 +6,10 @@ * Fixed bug of ContactConstraint with kinematic joints: [#809](https://github.com/dartsim/dart/pull/809) +* Misc + + * Fixed that ZeroDofJoint::getIndexInTree was called: [#818](https://github.com/dartsim/dart/pull/818) + ### DART 6.1.1 (2016-10-14) * Build diff --git a/unittests/comprehensive/test_Skeleton.cpp b/unittests/comprehensive/test_Skeleton.cpp index cec4e973221ab..f4be269812675 100644 --- a/unittests/comprehensive/test_Skeleton.cpp +++ b/unittests/comprehensive/test_Skeleton.cpp @@ -774,7 +774,7 @@ TEST(Skeleton, ZeroDofJointConstraintForces) BodyNode* bn = skel->createJointAndBodyNodePair().second; BodyNode* zeroDof1 = skel->createJointAndBodyNodePair(bn).second; bn = skel->createJointAndBodyNodePair(zeroDof1).second; - BodyNode* zeroDof2 = skel->createJointAndBodyNodePair(bn).second; + skel->createJointAndBodyNodePair(bn).second; const auto numSkelDofs = skel->getNumDofs(); for (auto& bodyNode : skel->getBodyNodes()) From 08b8f21f545debeab4c89e3f5815201caa91dee6 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 5 Nov 2016 10:30:10 -0400 Subject: [PATCH 4/4] Fix compilation warning --- unittests/comprehensive/test_Skeleton.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unittests/comprehensive/test_Skeleton.cpp b/unittests/comprehensive/test_Skeleton.cpp index f4be269812675..a780fbad72ce8 100644 --- a/unittests/comprehensive/test_Skeleton.cpp +++ b/unittests/comprehensive/test_Skeleton.cpp @@ -774,7 +774,7 @@ TEST(Skeleton, ZeroDofJointConstraintForces) BodyNode* bn = skel->createJointAndBodyNodePair().second; BodyNode* zeroDof1 = skel->createJointAndBodyNodePair(bn).second; bn = skel->createJointAndBodyNodePair(zeroDof1).second; - skel->createJointAndBodyNodePair(bn).second; + skel->createJointAndBodyNodePair(bn); const auto numSkelDofs = skel->getNumDofs(); for (auto& bodyNode : skel->getBodyNodes())