Fix grouping of skeletons by constraints (#1628)

Author: jslee02

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
     {

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());
 }
 
 //==============================================================================

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());
 }
 
 //==============================================================================

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());
 }
 
 //==============================================================================

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());
 }
 
 //==============================================================================

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());
 }
 
 //==============================================================================

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());
 }
 
 //==============================================================================

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
     {

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

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);