Increment body node version after it has been removed from a new skeleton

.ci/travis/docs_versions.txt

@@ -1,5 +1,5 @@
 DART 6
-v6.9.2
+v6.9.3
 v6.8.5
 v6.7.3
 v6.6.2

CHANGELOG.md

@@ -83,6 +83,13 @@
 
   * Updated tutorial documentation and code to reflect new APIs: [#1481](https://github.com/dartsim/dart/pull/1481)
 
+### [DART 6.9.3 (2020-08-26)](https://github.com/dartsim/dart/milestone/61?closed=1)
+
+* Dynamics
+
+  * Changed to update the Properties version of a BodyNode when moved to a new Skeleton: [#1445](https://github.com/dartsim/dart/pull/1445)
+  * Fixed incorrect implicit joint damping/spring force computation in inverse dynamics: [#1451](https://github.com/dartsim/dart/pull/1451)
+
 ### [DART 6.9.2 (2019-08-16)](https://github.com/dartsim/dart/milestone/60?closed=1)
 
 * Dynamics

dart/collision/CollisionResult.cpp

@@ -129,7 +129,7 @@ void CollisionResult::addObject(CollisionObject* object)
   {
     dterr << "[CollisionResult::addObject] Attempting to add a collision with "
           << "a nullptr object to a CollisionResult instance. This is not "
-          << "allowed. Please report this as a bug!";
+          << "allowed. Please report this as a bug!\n";
     assert(false);
     return;
   }

dart/dynamics/Skeleton.cpp

@@ -2265,7 +2265,11 @@ void Skeleton::registerBodyNode(BodyNode* _newBodyNode)
   }
 #endif // ------- Debug mode
 
+  _newBodyNode->incrementVersion();
   _newBodyNode->mStructuralChangeSignal.raise(_newBodyNode);
+  // We don't need to explicitly increment the version of this Skeleton here
+  // because the BodyNode will increment the version of its dependent, which is
+  // this Skeleton.
 }
 
 //==============================================================================
@@ -2437,6 +2441,12 @@ void Skeleton::unregisterBodyNode(BodyNode* _oldBodyNode)
   }
 
   updateTotalMass();
+
+  _oldBodyNode->incrementVersion();
+  _oldBodyNode->mStructuralChangeSignal.raise(_oldBodyNode);
+  // We don't need to explicitly increment the version of this Skeleton here
+  // because the BodyNode will increment the version of its dependent, which is
+  // this Skeleton.
 }
 
 //==============================================================================

dart/dynamics/Skeleton.hpp

@@ -646,7 +646,32 @@ class Skeleton : public virtual common::VersionCounter,
   /// Compute forward dynamics
   void computeForwardDynamics();
 
-  /// Compute inverse dynamics
+  /// Computes inverse dynamics.
+  ///
+  /// The inverse dynamics is computed according to the following equations of
+  /// motion:
+  ///
+  /// \f$ M(q) \ddot{q} + C(q, \dot{q}) + N(q) - \tau_{\text{ext}} - \tau_d -
+  /// \tau_s = \tau \f$
+  ///
+  /// where \f$ \tau_{\text{ext}} \f$, \f$ \tau_d \f$, and \f$ \tau_s \f$ are
+  /// external forces, joint damping forces, and joint spring forces projected
+  /// on to the joint space, respectively. This function provides three flags
+  /// whether to take into account each forces in computing the joint forces,
+  /// \f$ \tau_d \f$. Not accounting each forces implies that the forces is
+  /// added to \f$ \tau \f$ in order to keep the equation equivalent. If there
+  /// forces are zero (by setting external forces, damping coeff, spring
+  /// stiffness zeros), these flags have no effect.
+  ///
+  /// Once this function is called, the joint forces, \f$ \tau \f$, can be
+  /// obtained by calling getForces().
+  ///
+  /// \param[in] _withExternalForces Set \c true to take external forces into
+  /// account.
+  /// \param[in] _withDampingForces  Set \c true to take damping forces into
+  /// account.
+  /// \param[in] _withSpringForces   Set \c true to take spring forces into
+  /// account.
   void computeInverseDynamics(
       bool _withExternalForces = false,
       bool _withDampingForces = false,

dart/dynamics/detail/GenericJoint.hpp

@@ -2324,22 +2324,25 @@ void GenericJoint<ConfigSpaceT>::updateForceID(
   this->mAspectState.mForces
       = getRelativeJacobianStatic().transpose() * bodyForce;
 
-  // Damping force
+  // Implicit damping force:
+  //   tau_d = -Kd * dq - Kd * h * ddq
   if (withDampingForces)
   {
     const typename ConfigSpaceT::Vector dampingForces
         = -Base::mAspectProperties.mDampingCoefficients.cwiseProduct(
-            getVelocitiesStatic());
+            getVelocitiesStatic() + getAccelerationsStatic() * timeStep);
     this->mAspectState.mForces -= dampingForces;
   }
 
-  // Spring force
+  // Implicit spring force:
+  //   tau_s = -Kp * (q - q0) - Kp * h * dq - Kp * h^2 * ddq
   if (withSpringForces)
   {
     const typename ConfigSpaceT::Vector springForces
         = -Base::mAspectProperties.mSpringStiffnesses.cwiseProduct(
             getPositionsStatic() - Base::mAspectProperties.mRestPositions
-            + getVelocitiesStatic() * timeStep);
+            + getVelocitiesStatic() * timeStep
+            + getAccelerationsStatic() * timeStep * timeStep);
     this->mAspectState.mForces -= springForces;
   }
 }

doxygen/Doxyfile.in

@@ -1472,7 +1472,7 @@ FORMULA_TRANSPARENT    = YES
 # The default value is: NO.
 # This tag requires that the tag GENERATE_HTML is set to YES.
 
-USE_MATHJAX            = NO
+USE_MATHJAX            = YES
 
 # When MathJax is enabled you can set the default output format to be used for
 # the MathJax output. See the MathJax site (see:
@@ -1502,7 +1502,7 @@ MATHJAX_RELPATH        = http://cdn.mathjax.org/mathjax/latest
 # MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
 # This tag requires that the tag USE_MATHJAX is set to YES.
 
-MATHJAX_EXTENSIONS     =
+MATHJAX_EXTENSIONS     = TeX/AMSmath
 
 # The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
 # of code that will be used on startup of the MathJax code. See the MathJax site

unittests/comprehensive/test_Dynamics.cpp

@@ -91,6 +91,9 @@ class DynamicsTest : public ::testing::Test
   // transformations, spatial velocities, and spatial accelerations correctly.
   void testForwardKinematics(const common::Uri& uri);
 
+  // Test inverse dynamics with various joint forces.
+  void testInverseDynamics(const common::Uri& uri);
+
   // Compare dynamics terms in equations of motion such as mass matrix, mass
   // inverse matrix, Coriolis force vector, gravity force vector, and external
   // force vector.
@@ -1529,6 +1532,84 @@ void DynamicsTest::testForwardKinematics(const common::Uri& uri)
   }
 }
 
+//==============================================================================
+void DynamicsTest::testInverseDynamics(const common::Uri& uri)
+{
+  //---------------------------- Settings --------------------------------------
+  // Number of random state tests for each skeleton
+#ifndef NDEBUG // Debug mode
+  const std::size_t nRandomItr = 2;
+#else
+  const std::size_t nRandomItr = 100;
+#endif
+
+  // Lower and upper bound of configuration for system
+  const double lb = -1.0 * math::constantsd::pi();
+  const double ub = 1.0 * math::constantsd::pi();
+
+  // Lower and upper bound of joint damping and stiffness
+  const double lbD = 0.0;
+  const double ubD = 10.0;
+  const double lbK = 0.0;
+  const double ubK = 10.0;
+
+  // Lower and upper bound of joint forces
+  const double lbF = -10.0;
+  const double ubF = 10.0;
+
+  simulation::WorldPtr world = utils::SkelParser::readWorld(uri);
+  ASSERT_TRUE(world != nullptr);
+
+  for (std::size_t i = 0; i < world->getNumSkeletons(); ++i)
+  {
+    dynamics::SkeletonPtr skel = world->getSkeleton(i);
+    const std::size_t dofs = skel->getNumDofs();
+
+    for (std::size_t j = 0; j < nRandomItr; ++j)
+    {
+      // Random joint stiffness and damping coefficients
+      for (std::size_t l = 0; l < dofs; ++l)
+      {
+        dynamics::DegreeOfFreedom* dof = skel->getDof(l);
+        dof->setDampingCoefficient(math::Random::uniform(lbD, ubD));
+        dof->setSpringStiffness(math::Random::uniform(lbK, ubK));
+
+        double lbRP = dof->getPositionLowerLimit();
+        double ubRP = dof->getPositionUpperLimit();
+        lbRP = std::max(lbRP, -math::constantsd::pi());
+        ubRP = std::min(ubRP, +math::constantsd::pi());
+        dof->setRestPosition(math::Random::uniform(lbRP, ubRP));
+      }
+
+      // Set random states and forces
+      const VectorXd q0 = math::Random::uniform<VectorXd>(dofs, lb, ub);
+      const VectorXd dq0 = math::Random::uniform<VectorXd>(dofs, lb, ub);
+      const VectorXd f0 = math::Random::uniform<VectorXd>(dofs, lbF, ubF);
+
+      // Forward dynamics to compute accelerations
+      skel->setPositions(q0);
+      skel->setVelocities(dq0);
+      skel->setForces(f0);
+      skel->computeForwardDynamics();
+      const VectorXd ddq = skel->getAccelerations();
+
+      // Test inverse dynamics to ensure the result joint forces are identical
+      // to the forces used to compute the input accelerations
+      skel->setAccelerations(ddq);
+      skel->computeInverseDynamics(false, true, true);
+      const VectorXd f = skel->getForces();
+      EXPECT_TRUE(equals(f, f0));
+
+      // Test forward dynamics to ensure the result joint accelerations are
+      // identical to the accelerations used to compute the input forces
+      skel->setForces(f);
+      skel->computeForwardDynamics();
+      const VectorXd ddq2 = skel->getAccelerations();
+      EXPECT_TRUE(equals(ddq, ddq2));
+    }
+  }
+}
+
 //==============================================================================
 void DynamicsTest::compareEquationsOfMotion(const common::Uri& uri)
 {
@@ -2329,6 +2410,18 @@ TEST_F(DynamicsTest, testForwardKinematics)
   }
 }
 
+//==============================================================================
+TEST_F(DynamicsTest, testInverseDynamics)
+{
+  for (std::size_t i = 0; i < getList().size(); ++i)
+  {
+#ifndef NDEBUG
+    dtdbg << getList()[i].toString() << std::endl;
+#endif
+    testInverseDynamics(getList()[i]);
+  }
+}
+
 //==============================================================================
 TEST_F(DynamicsTest, compareEquationsOfMotion)
 {

unittests/comprehensive/test_Skeleton.cpp

@@ -144,7 +144,11 @@ TEST(Skeleton, Restructuring)
     BodyNode* child = bn1->descendsFrom(bn2) ? bn1 : bn2;
     BodyNode* parent = child == bn1 ? bn2 : bn1;
 
+    auto skelVer = skeleton->getVersion();
+    auto childVer = child->getVersion();
     child->moveTo(parent);
+    EXPECT_NE(skeleton->getVersion(), skelVer);
+    EXPECT_NE(child->getVersion(), childVer);
 
     EXPECT_TRUE(skeleton->getNumBodyNodes() == original->getNumBodyNodes());
     if (skeleton->getNumBodyNodes() == original->getNumBodyNodes())
@@ -263,7 +267,13 @@ TEST(Skeleton, Restructuring)
     constructSubtree(subtree, childBn);
 
     // Move to a new Skeleton
+    auto fromSkelVer = fromSkel->getVersion();
+    auto toSkelVer = toSkel->getVersion();
+    auto child_ver = childBn->getVersion();
     childBn->moveTo(parentBn);
+    EXPECT_NE(fromSkel->getVersion(), fromSkelVer);
+    EXPECT_NE(toSkel->getVersion(), toSkelVer);
+    EXPECT_NE(childBn->getVersion(), child_ver);
     EXPECT_TRUE(childBn->getSkeleton()->checkIndexingConsistency());
     EXPECT_TRUE(parentBn->getSkeleton()->checkIndexingConsistency());
 

unittests/regression/CMakeLists.txt

@@ -15,6 +15,8 @@ if(TARGET dart-utils-urdf)
 
   dart_add_test("regression" test_Issue1231)
 
+  dart_add_test("regression" test_Issue1445)
+
 endif()
 
 if(TARGET dart-collision-bullet)