Fix grouping of skeletons by constraints

dartsim · Dec 18, 2021 · 9637b02 · 9637b02
1 parent 6f9feed
commit 9637b02
Show file tree

Hide file tree

Showing 10 changed files with 17 additions and 13 deletions.
diff --git a/dart/constraint/BallJointConstraint.cpp b/dart/constraint/BallJointConstraint.cpp
@@ -334,13 +334,13 @@ void BallJointConstraint::applyImpulse(double* _lambda)
 dynamics::SkeletonPtr BallJointConstraint::getRootSkeleton() const
 {
   if (mBodyNode1->isReactive())
-    return mBodyNode1->getSkeleton()->mUnionRootSkeleton.lock();
+    return ConstraintBase::getRootSkeleton(mBodyNode1->getSkeleton());
 
   if (mBodyNode2)
   {
     if (mBodyNode2->isReactive())
     {
-      return mBodyNode2->getSkeleton()->mUnionRootSkeleton.lock();
+      return ConstraintBase::getRootSkeleton(mBodyNode2->getSkeleton());
     }
     else
     {

diff --git a/dart/constraint/JointConstraint.cpp b/dart/constraint/JointConstraint.cpp
@@ -537,7 +537,7 @@ void JointConstraint::applyImpulse(double* lambda)
 //==============================================================================
 dynamics::SkeletonPtr JointConstraint::getRootSkeleton() const
 {
-  return mJoint->getSkeleton()->mUnionRootSkeleton.lock();
+  return ConstraintBase::getRootSkeleton(mJoint->getSkeleton()->getSkeleton());
 }
 
 //==============================================================================

diff --git a/dart/constraint/JointCoulombFrictionConstraint.cpp b/dart/constraint/JointCoulombFrictionConstraint.cpp
@@ -276,7 +276,7 @@ void JointCoulombFrictionConstraint::applyImpulse(double* _lambda)
 //==============================================================================
 dynamics::SkeletonPtr JointCoulombFrictionConstraint::getRootSkeleton() const
 {
-  return mJoint->getSkeleton()->mUnionRootSkeleton.lock();
+  return ConstraintBase::getRootSkeleton(mJoint->getSkeleton()->getSkeleton());
 }
 
 //==============================================================================

diff --git a/dart/constraint/JointLimitConstraint.cpp b/dart/constraint/JointLimitConstraint.cpp
@@ -456,7 +456,7 @@ void JointLimitConstraint::applyImpulse(double* lambda)
 //==============================================================================
 dynamics::SkeletonPtr JointLimitConstraint::getRootSkeleton() const
 {
-  return mJoint->getSkeleton()->mUnionRootSkeleton.lock();
+  return ConstraintBase::getRootSkeleton(mJoint->getSkeleton()->getSkeleton());
 }
 
 //==============================================================================

diff --git a/dart/constraint/MimicMotorConstraint.cpp b/dart/constraint/MimicMotorConstraint.cpp
@@ -288,7 +288,7 @@ void MimicMotorConstraint::applyImpulse(double* lambda)
 //==============================================================================
 dynamics::SkeletonPtr MimicMotorConstraint::getRootSkeleton() const
 {
-  return mJoint->getSkeleton()->mUnionRootSkeleton.lock();
+  return ConstraintBase::getRootSkeleton(mJoint->getSkeleton()->getSkeleton());
 }
 
 //==============================================================================

diff --git a/dart/constraint/ServoMotorConstraint.cpp b/dart/constraint/ServoMotorConstraint.cpp
@@ -277,7 +277,7 @@ void ServoMotorConstraint::applyImpulse(double* lambda)
 //==============================================================================
 dynamics::SkeletonPtr ServoMotorConstraint::getRootSkeleton() const
 {
-  return mJoint->getSkeleton()->mUnionRootSkeleton.lock();
+  return ConstraintBase::getRootSkeleton(mJoint->getSkeleton()->getSkeleton());
 }
 
 //==============================================================================

diff --git a/dart/constraint/SoftContactConstraint.cpp b/dart/constraint/SoftContactConstraint.cpp
@@ -981,9 +981,11 @@ double SoftContactConstraint::computeRestitutionCoefficient(
 dynamics::SkeletonPtr SoftContactConstraint::getRootSkeleton() const
 {
   if (mSoftBodyNode1 || mBodyNode1->isReactive())
-    return mBodyNode1->getSkeleton()->mUnionRootSkeleton.lock();
+    return ConstraintBase::getRootSkeleton(
+        mBodyNode1->getSkeleton()->getSkeleton());
   else
-    return mBodyNode2->getSkeleton()->mUnionRootSkeleton.lock();
+    return ConstraintBase::getRootSkeleton(
+        mBodyNode2->getSkeleton()->getSkeleton());
 }
 
 //==============================================================================

diff --git a/dart/constraint/WeldJointConstraint.cpp b/dart/constraint/WeldJointConstraint.cpp
@@ -355,13 +355,15 @@ void WeldJointConstraint::applyImpulse(double* _lambda)
 dynamics::SkeletonPtr WeldJointConstraint::getRootSkeleton() const
 {
   if (mBodyNode1->isReactive())
-    return mBodyNode1->getSkeleton()->mUnionRootSkeleton.lock();
+    return ConstraintBase::getRootSkeleton(
+        mBodyNode1->getSkeleton()->getSkeleton());
 
   if (mBodyNode2)
   {
     if (mBodyNode2->isReactive())
     {
-      return mBodyNode2->getSkeleton()->mUnionRootSkeleton.lock();
+      return ConstraintBase::getRootSkeleton(
+          mBodyNode2->getSkeleton()->getSkeleton());
     }
     else
     {

diff --git a/dart/dynamics/detail/DegreeOfFreedomPtr.hpp b/dart/dynamics/detail/DegreeOfFreedomPtr.hpp
@@ -218,7 +218,7 @@ class TemplateDegreeOfFreedomPtr
   TemplateBodyNodePtr<BodyNodeT> mBodyNodePtr;
 
   /// Local index of this DegreeOfFreedom within its Joint
-  std::size_t mIndex;
+  std::size_t mIndex = 0;
 };
 
 /// TemplateWeakDegreeOfFreedomPtr is a templated class that enables users to

diff --git a/unittests/regression/test_Issue1231.cpp b/unittests/regression/test_Issue1231.cpp
@@ -45,7 +45,7 @@ TEST(Issue1231, NoContacts)
   dart::simulation::WorldPtr world = dart::simulation::World::create();
 
   double x = -0.25;
-  for (const std::string& name : {"1", "2"})
+  for (const auto& name : {"1", "2"})
   {
     const auto skeleton = dart::dynamics::Skeleton::create(name);
     skeleton->setMobile(false);