Fix mimic/coupler constraints to mirror Gazebo mimic repro

dart/constraint/CouplerConstraint.cpp

@@ -38,12 +38,21 @@
 #include "dart/dynamics/Joint.hpp"
 #include "dart/dynamics/Skeleton.hpp"
 
-#define DART_CFM 1e-9
+#include <cmath>
+
+namespace {
+
+constexpr inline double kConstraintForceMixing = 1e-6;
+constexpr inline double kDefaultForceLimit = 800.0;
+constexpr inline double kDefaultVelocityLimit = 50.0;
+constexpr inline double kDefaultErp = 0.4;
+
+} // namespace
 
 namespace dart {
 namespace constraint {
 
-double CouplerConstraint::mConstraintForceMixing = DART_CFM;
+double CouplerConstraint::mConstraintForceMixing = kConstraintForceMixing;
 
 //==============================================================================
 CouplerConstraint::CouplerConstraint(
@@ -114,20 +123,32 @@ void CouplerConstraint::update()
     const auto& mimicProp = mMimicProps[i];
 
     double timeStep = mJoint->getSkeleton()->getTimeStep();
+    double velLower = mJoint->getVelocityLowerLimit(i);
+    double velUpper = mJoint->getVelocityUpperLimit(i);
+    if (!std::isfinite(velLower))
+      velLower = -kDefaultVelocityLimit;
+    if (!std::isfinite(velUpper))
+      velUpper = kDefaultVelocityLimit;
     double qError
         = mimicProp.mReferenceJoint->getPosition(mimicProp.mReferenceDofIndex)
               * mimicProp.mMultiplier
           + mimicProp.mOffset - mJoint->getPosition(i);
-    double desiredVelocity = math::clip(
-        qError / timeStep,
-        mJoint->getVelocityLowerLimit(i),
-        mJoint->getVelocityUpperLimit(i));
+    const double erp = kDefaultErp;
+    double desiredVelocity
+        = math::clip((erp * qError) / timeStep, velLower, velUpper);
 
     mNegativeVelocityError[i] = desiredVelocity - mJoint->getVelocity(i);
 
     if (mNegativeVelocityError[i] != 0.0) {
-      mUpperBound[i] = mJoint->getForceUpperLimit(i) * timeStep;
-      mLowerBound[i] = mJoint->getForceLowerLimit(i) * timeStep;
+      double upper = mJoint->getForceUpperLimit(i);
+      double lower = mJoint->getForceLowerLimit(i);
+      if (!std::isfinite(upper))
+        upper = kDefaultForceLimit;
+      if (!std::isfinite(lower))
+        lower = -kDefaultForceLimit;
+
+      mUpperBound[i] = upper * timeStep;
+      mLowerBound[i] = lower * timeStep;
 
       if (mActive[i]) {
         ++(mLifeTime[i]);

dart/constraint/CouplerConstraint.hpp

@@ -35,6 +35,7 @@
 
 #include <dart/constraint/ConstraintBase.hpp>
 
+#include <dart/dynamics/Fwd.hpp>
 #include <dart/dynamics/MimicDofProperties.hpp>
 
 #include <dart/Export.hpp>
@@ -144,7 +145,7 @@ class DART_API CouplerConstraint : public ConstraintBase
   double mLowerBound[6];
 
   /// Global constraint force mixing parameter in the range of [1e-9, 1]. The
-  /// default is 1e-5
+  /// default is 1e-6
   /// @sa http://www.ode.org/ode-latest-userguide.html#sec_3_8_0
   static double mConstraintForceMixing;
 };

dart/constraint/MimicMotorConstraint.cpp

@@ -41,12 +41,22 @@
 
 #include <iostream>
 
-#define DART_CFM 1e-9
+#include <cmath>
+
+namespace {
+
+constexpr inline double kConstraintForceMixing = 1e-6;
+constexpr inline double kDefaultForceLimit = 800.0;
+constexpr inline double kDefaultVelocityLimit = 50.0;
+constexpr inline double kDefaultErp = 0.4;
+
+} // namespace
 
 namespace dart {
 namespace constraint {
 
-double MimicMotorConstraint::mConstraintForceMixing = DART_CFM;
+double MimicMotorConstraint::mConstraintForceMixing = kConstraintForceMixing;
+double MimicMotorConstraint::mErrorReductionParameter = kDefaultErp;
 
 //==============================================================================
 MimicMotorConstraint::MimicMotorConstraint(
@@ -117,6 +127,31 @@ double MimicMotorConstraint::getConstraintForceMixing()
   return mConstraintForceMixing;
 }
 
+//==============================================================================
+void MimicMotorConstraint::setErrorReductionParameter(double erp)
+{
+  double clamped = erp;
+  if (clamped < 0.0) {
+    DART_WARN(
+        "Error reduction parameter [{}] is lower than 0.0. It is set to 0.0.",
+        erp);
+    clamped = 0.0;
+  } else if (clamped > 1.0) {
+    DART_WARN(
+        "Error reduction parameter [{}] is greater than 1.0. It is set to 1.0.",
+        erp);
+    clamped = 1.0;
+  }
+
+  mErrorReductionParameter = clamped;
+}
+
+//==============================================================================
+double MimicMotorConstraint::getErrorReductionParameter()
+{
+  return mErrorReductionParameter;
+}
+
 //==============================================================================
 void MimicMotorConstraint::update()
 {
@@ -128,21 +163,33 @@ void MimicMotorConstraint::update()
     const auto& mimicProp = mMimicProps[i];
 
     double timeStep = mJoint->getSkeleton()->getTimeStep();
+    double velLower = mJoint->getVelocityLowerLimit(i);
+    double velUpper = mJoint->getVelocityUpperLimit(i);
+    if (!std::isfinite(velLower))
+      velLower = -kDefaultVelocityLimit;
+    if (!std::isfinite(velUpper))
+      velUpper = kDefaultVelocityLimit;
     double qError
         = mimicProp.mReferenceJoint->getPosition(mimicProp.mReferenceDofIndex)
               * mimicProp.mMultiplier
           + mimicProp.mOffset - mJoint->getPosition(i);
-    double desiredVelocity = math::clip(
-        qError / timeStep,
-        mJoint->getVelocityLowerLimit(i),
-        mJoint->getVelocityUpperLimit(i));
+    const double erp = mErrorReductionParameter;
+    double desiredVelocity
+        = math::clip((erp * qError) / timeStep, velLower, velUpper);
 
     mNegativeVelocityError[i] = desiredVelocity - mJoint->getVelocity(i);
 
     if (mNegativeVelocityError[i] != 0.0) {
       // Note that we are computing impulse not force
-      mUpperBound[i] = mJoint->getForceUpperLimit(i) * timeStep;
-      mLowerBound[i] = mJoint->getForceLowerLimit(i) * timeStep;
+      double upper = mJoint->getForceUpperLimit(i);
+      double lower = mJoint->getForceLowerLimit(i);
+      if (!std::isfinite(upper))
+        upper = kDefaultForceLimit;
+      if (!std::isfinite(lower))
+        lower = -kDefaultForceLimit;
+
+      mUpperBound[i] = upper * timeStep;
+      mLowerBound[i] = lower * timeStep;
 
       if (mActive[i]) {
         ++(mLifeTime[i]);

dart/constraint/MimicMotorConstraint.hpp

@@ -35,17 +35,12 @@
 
 #include <dart/constraint/ConstraintBase.hpp>
 
+#include <dart/dynamics/Fwd.hpp>
 #include <dart/dynamics/MimicDofProperties.hpp>
 
 #include <vector>
 
 namespace dart {
-
-namespace dynamics {
-class BodyNode;
-class Joint;
-} // namespace dynamics
-
 namespace constraint {
 
 /// MimicMotorConstraint behaves like a servo motor: it drives only the
@@ -84,6 +79,12 @@ class MimicMotorConstraint : public ConstraintBase
   /// Get global constraint force mixing parameter
   static double getConstraintForceMixing();
 
+  /// Set global error reduction parameter applied to mimic motors.
+  static void setErrorReductionParameter(double erp);
+
+  /// Get global error reduction parameter applied to mimic motors.
+  static double getErrorReductionParameter();
+
   //----------------------------------------------------------------------------
   // Friendship
   //----------------------------------------------------------------------------
@@ -157,9 +158,12 @@ class MimicMotorConstraint : public ConstraintBase
   double mLowerBound[6];
 
   /// Global constraint force mixing parameter in the range of [1e-9, 1]. The
-  /// default is 1e-5
+  /// default is 1e-6
   /// @sa http://www.ode.org/ode-latest-userguide.html#sec_3_8_0
   static double mConstraintForceMixing;
+
+  /// Global error reduction parameter that scales mimic motor position error.
+  static double mErrorReductionParameter;
 };
 
 } // namespace constraint