Fix inverse dynamics with damping and stiffness (#1451)

Author: jslee02

CHANGELOG.md

@@ -5,6 +5,7 @@
 * Dynamics
 
   * 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)
 

dart/dynamics/Skeleton.hpp

@@ -668,7 +668,32 @@ class Skeleton :
   /// 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,
                               bool _withSpringForces = false);

dart/dynamics/detail/GenericJoint.hpp

@@ -2367,23 +2367,26 @@ 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.
@@ -1385,6 +1388,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)
 {
@@ -2163,6 +2244,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)
 {