Add NMSM Pipeline contact elements for use in Moco

CHANGELOG.md

@@ -23,6 +23,7 @@ v4.6
   components. The `ForceProducer` API was also rolled out to a variety of existing `Force` components, which
   means that API users can now now ask many `Force` components what forces they produce (see #3891 for a
   comprehensive overview).
+- Completed the implementation of the `MeyerFregly2016Force` included in the `StationPlaneContactForce` class to support NMSM Pipeline-equivalent contact models in Moco. (#3877)
 
 v4.5.1
 ======
@@ -88,6 +89,7 @@ pointer to avoid crashes in scripting due to invalid pointer ownership (#3781).
 - Improved exception handling for internal errors in `MocoCasADiSolver`. Problems will now abort and print a descriptive error message (rather than fail due to an empty trajectory). (#3834)
 - Upgraded the Ipopt dependency Metis to version 5.1.0 on Unix and macOS to enable building on `osx-arm64` (#3874).
 
+
 v4.5
 ====
 - Added `AbstractGeometryPath` which is a base class for `GeometryPath` and other path types (#3388). All path-based

OpenSim/Moco/Components/StationPlaneContactForce.h → OpenSim/Simulation/Model/StationPlaneContactForce.h

@@ -3,9 +3,9 @@
 /* -------------------------------------------------------------------------- *
  * OpenSim: StationPlaneContactForce.h                                        *
  * -------------------------------------------------------------------------- *
- * Copyright (c) 2017 Stanford University and the Authors                     *
+ * Copyright (c) 2024 Stanford University and the Authors                     *
  *                                                                            *
- * Author(s): Nicholas Bianco, Chris Dembia                                   *
+ * Author(s): Nicholas Bianco, Chris Dembia, Spencer Williams                 *
  *                                                                            *
  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
  * not use this file except in compliance with the License. You may obtain a  *
@@ -27,12 +27,18 @@ namespace OpenSim {
 
 /** This class models compliant point contact with a ground plane y=0.
 
+Vertical force is calculated using stiffness and dissipation coefficients, 
+while horizontal force is calculated using a friction model. This class
+includes multiple contact models, though currently only MeyerFregly2016Force
+has a complete implementation. Details for how this model calculates forces
+are included in its documentation below. 
+
 @underdevelopment */
-class OSIMMOCO_API StationPlaneContactForce : public ForceProducer {
+class OSIMSIMULATION_API StationPlaneContactForce : public ForceProducer {
 OpenSim_DECLARE_ABSTRACT_OBJECT(StationPlaneContactForce, ForceProducer);
 public:
     OpenSim_DECLARE_OUTPUT(force_on_station, SimTK::Vec3,
-            calcContactForceOnStation, SimTK::Stage::Velocity);
+            computeContactForceOnStation, SimTK::Stage::Velocity);
 
     OpenSim_DECLARE_SOCKET(station, Station,
             "The body-fixed point that can contact the plane.");
@@ -49,7 +55,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(StationPlaneContactForce, ForceProducer);
     const override {
         OpenSim::Array<double> values;
         // TODO cache.
-        const SimTK::Vec3 force = calcContactForceOnStation(s);
+        const SimTK::Vec3 force = computeContactForceOnStation(s);
         values.append(force[0]);
         values.append(force[1]);
         values.append(force[2]);
@@ -59,16 +65,15 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(StationPlaneContactForce, ForceProducer);
             const SimTK::State& s,
             SimTK::Array_<SimTK::DecorativeGeometry>& geoms) const override;
 
-    // TODO rename to computeContactForceOnStation
-    virtual SimTK::Vec3 calcContactForceOnStation(
+    virtual SimTK::Vec3 computeContactForceOnStation(
             const SimTK::State& s) const = 0;
 
 private:
     void implProduceForces(
         const SimTK::State& s,
         ForceConsumer& forceConsumer) const override {
 
-        const SimTK::Vec3 force = calcContactForceOnStation(s);
+        const SimTK::Vec3 force = computeContactForceOnStation(s);
         const auto& pt = getConnectee<Station>("station");
         const auto& pos = pt.getLocationInGround(s);
         const auto& frame = pt.getParentFrame();
@@ -78,98 +83,160 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(StationPlaneContactForce, ForceProducer);
     }
 };
 
-/** This class is still under development. */
-class OSIMMOCO_API AckermannVanDenBogert2010Force
+/** This contact model is from the following paper:
+Meyer A. J., Eskinazi, I., Jackson, J. N., Rao, A. V., Patten, C., & Fregly,
+B. J. (2016). Muscle Synergies Facilitate Computational Prediction of
+Subject-Specific Walking Motions. Frontiers in Bioengineering and
+Biotechnology, 4, 1055–27. http://doi.org/10.3389/fbioe.2016.00077 
+
+Following OpenSim convention, this contact element assumes that the y-direction
+is vertical. Vertical contact force is calculated based on vertical position 
+and velocity relative to a floor at y=0 using these equations:
+
+\f[
+v = \frac{k_{val} + k_{low}}{k_{val} - k_{low}}
+\f]
+\f[
+s = \frac{k_{val} - k_{low}}{2}
+\f]
+\f[
+R = -s * (v * y_{max} - c * log(\frac{cosh(y_{max} + h)}{c})
+\f]
+\f[
+F = -s * (v * y - c * log(\frac{cosh(y + h)}{c}) - R
+\f]
+
+With the following values:
+- \f$ k_{val} \f$: stiffness coefficient of contact element
+- \f$ k_{low} \f$: small out-of-contact stiffness to assist optimizations
+- \f$ y_{max} \f$: y value were out-of-contact force becomes zero
+- \f$ c \f$: transition curvature of transition between linear regions
+- \f$ h \f$: horizontal offset defining slope transition location
+- \f$ y \f$: current y (vertical) position of contact element
+
+Velocity is then used to incorporate non-linear damping:
+
+\f[
+F_{damped} = F * (1 + C_{val} * y_{vel})
+\f]
+
+With the following values:
+- \f$ C_{val} \f$: damping coefficient of contact element
+- \f$ y_{vel} \f$: current y (vertical) velocity of contact element, negated
+
+The force equation produces a force-penetration curve similar to a leaky 
+rectified linear function with a smooth transition region near zero. This 
+produces a smooth curve with a slight out-of-contact slope to assist 
+gradient-based optimizations when an element is inappropriately out of contact.
+
+Horizontal forces are then calculated based on this vertical force and the 
+horizontal velocity components of the contact element. Both dynamic (modeled 
+with a tanh function) and viscous (modeled with a linear function) friction 
+models may be used. */
+class OSIMSIMULATION_API MeyerFregly2016Force
         : public StationPlaneContactForce {
-OpenSim_DECLARE_CONCRETE_OBJECT(AckermannVanDenBogert2010Force,
+OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force,
         StationPlaneContactForce);
 public:
     OpenSim_DECLARE_PROPERTY(stiffness, double,
-            "Spring stiffness in N/m^3 (default: 5e7).");
+            "Spring stiffness in N/m (default: 1e4).");
     OpenSim_DECLARE_PROPERTY(dissipation, double,
-            "Dissipation coefficient in s/m (default: 1.0).");
-    OpenSim_DECLARE_PROPERTY(friction_coefficient, double,
-            "Friction coefficient");
-    // TODO rename to transition_velocity
-    OpenSim_DECLARE_PROPERTY(tangent_velocity_scaling_factor, double,
-            "Governs how rapidly friction develops (default: 0.05).");
+            "Dissipation coefficient in s/m (default: 0.01).");
+    OpenSim_DECLARE_PROPERTY(dynamic_friction, double,
+            "Dynamic friction coefficient (default: 0).");
+    OpenSim_DECLARE_PROPERTY(viscous_friction, double,
+            "Viscous friction coefficient (default: 5).");
+    OpenSim_DECLARE_PROPERTY(latch_velocity, double,
+            "Latching velocity in m/s (default: 0.05).");
 
-    AckermannVanDenBogert2010Force() {
+    MeyerFregly2016Force() {
         constructProperties();
     }
 
     /// Compute the force applied to body to which the station is attached, at
     /// the station, expressed in ground.
-    SimTK::Vec3 calcContactForceOnStation(const SimTK::State& s)
+    SimTK::Vec3 computeContactForceOnStation(const SimTK::State& s)
     const override {
         SimTK::Vec3 force(0);
         const auto& pt = getConnectee<Station>("station");
         const auto& pos = pt.getLocationInGround(s);
         const auto& vel = pt.getVelocityInGround(s);
         const SimTK::Real y = pos[1];
         const SimTK::Real velNormal = vel[1];
-        // TODO should project vel into ground.
-        const SimTK::Real velSliding = vel[0];
-        const SimTK::Real depth = 0 - y;
+        SimTK::Real velSliding = sqrt(vel[0] * vel[0] + vel[2] * vel[2]);
         const SimTK::Real depthRate = 0 - velNormal;
-        const SimTK::Real& a = get_stiffness();
-        const SimTK::Real& b = get_dissipation();
-        if (depth > 0) {
-            force[1] = fmax(0, a * pow(depth, 3) * (1 + b * depthRate));
-        }
-        const SimTK::Real voidStiffness = 1.0; // N/m
-        force[1] += voidStiffness * depth;
+        const SimTK::Real Kval = get_stiffness();
+        const SimTK::Real Cval = get_dissipation();
+        const SimTK::Real klow = 1e-1;
+        const SimTK::Real h = 1e-3;
+        const SimTK::Real c = 5e-4;
+        const SimTK::Real ymax = 1e-2;
+        // Prevents dividing by zero when sliding velocity is zero.
+        const SimTK::Real slipOffset = 1e-4;
 
-        const SimTK::Real velSlidingScaling =
-                get_tangent_velocity_scaling_factor();
-        // The paper used (1 - exp(-x)) / (1 + exp(-x)) = tanh(2x).
-        // tanh() has a wider domain than using exp().
-        const SimTK::Real transition = tanh(velSliding / velSlidingScaling / 2);
+        /// Normal force.
+        const SimTK::Real vp = (Kval + klow) / (Kval - klow);
+        const SimTK::Real sp = (Kval - klow) / 2;
 
-        const SimTK::Real frictionForce =
-                -transition * get_friction_coefficient() * force[1];
+        const SimTK::Real constant =
+                -sp * (vp * ymax - c * log(cosh((ymax + h) / c)));
+
+        SimTK::Real Fspring =
+                -sp * (vp * y - c * log(cosh((y + h) / c))) - constant;
+
+        const SimTK::Real Fy = Fspring * (1 + Cval * depthRate);
+
+        force[1] = Fy;
+
+        /// Friction force.
+        const SimTK::Real mu_d = get_dynamic_friction();
+        const SimTK::Real mu_v = get_viscous_friction();
+        const SimTK::Real latchvel = get_latch_velocity();
+
+        SimTK::Real horizontalForce = force[1] * (
+                mu_d * tanh(velSliding / latchvel) + mu_v * velSliding
+        );
+
+        force[0] = -vel[0] / (velSliding + slipOffset) * horizontalForce;
+        force[2] = -vel[2] / (velSliding + slipOffset) * horizontalForce;
 
-        force[0] = frictionForce;
         return force;
     }
 
 private:
     void constructProperties() {
-        constructProperty_friction_coefficient(1.0);
-        constructProperty_stiffness(5e7);
-        constructProperty_dissipation(1.0);
-        constructProperty_tangent_velocity_scaling_factor(0.05);
+        constructProperty_stiffness(1e4);
+        constructProperty_dissipation(1e-2);
+        constructProperty_dynamic_friction(0);
+        constructProperty_viscous_friction(5);
+        constructProperty_latch_velocity(0.05);
     }
-};
-
 
+};
 
-/** This contact model is from the following paper:
-Meyer A. J., Eskinazi, I., Jackson, J. N., Rao, A. V., Patten, C., & Fregly,
-B. J. (2016). Muscle Synergies Facilitate Computational Prediction of
-Subject-Specific Walking Motions. Frontiers in Bioengineering and
-Biotechnology, 4, 1055–27. http://doi.org/10.3389/fbioe.2016.00077 
-
-@underdevelopment */
-class OSIMMOCO_API MeyerFregly2016Force
+/** This class is still under development. */
+class OSIMSIMULATION_API AckermannVanDenBogert2010Force
         : public StationPlaneContactForce {
-OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force,
+OpenSim_DECLARE_CONCRETE_OBJECT(AckermannVanDenBogert2010Force,
         StationPlaneContactForce);
 public:
     OpenSim_DECLARE_PROPERTY(stiffness, double,
-            "Spring stiffness in N/m (default: 1e4).");
+            "Spring stiffness in N/m^3 (default: 5e7).");
     OpenSim_DECLARE_PROPERTY(dissipation, double,
-            "Dissipation coefficient in s/m (default: 0.01).");
-    OpenSim_DECLARE_PROPERTY(tscale, double,
-            "TODO");
+            "Dissipation coefficient in s/m (default: 1.0).");
+    OpenSim_DECLARE_PROPERTY(friction_coefficient, double,
+            "Friction coefficient");
+    // TODO rename to transition_velocity
+    OpenSim_DECLARE_PROPERTY(tangent_velocity_scaling_factor, double,
+            "Governs how rapidly friction develops (default: 0.05).");
 
-    MeyerFregly2016Force() {
+    AckermannVanDenBogert2010Force() {
         constructProperties();
     }
 
     /// Compute the force applied to body to which the station is attached, at
     /// the station, expressed in ground.
-    SimTK::Vec3 calcContactForceOnStation(const SimTK::State& s)
+    SimTK::Vec3 computeContactForceOnStation(const SimTK::State& s)
     const override {
         SimTK::Vec3 force(0);
         const auto& pt = getConnectee<Station>("station");
@@ -179,50 +246,36 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force,
         const SimTK::Real velNormal = vel[1];
         // TODO should project vel into ground.
         const SimTK::Real velSliding = vel[0];
-        // const SimTK::Real depth = 0 - y;
+        const SimTK::Real depth = 0 - y;
         const SimTK::Real depthRate = 0 - velNormal;
-        const SimTK::Real Kval = get_stiffness();
-        const SimTK::Real Cval = get_dissipation();
-        const SimTK::Real tscale = get_tscale();
-        const SimTK::Real klow = 1e-1 / (tscale * tscale);
-        const SimTK::Real h = 1e-3;
-        const SimTK::Real c = 5e-4;
-        const SimTK::Real ymax = 1e-2;
-
-        /// Normal force.
-        const SimTK::Real vp = (Kval + klow) / (Kval - klow);
-        const SimTK::Real sp = (Kval - klow) / 2;
-
-        const SimTK::Real constant =
-                -sp * (vp * ymax - c * log(cosh((ymax + h) / c)));
-
-        SimTK::Real Fspring =
-                -sp * (vp * y - c * log(cosh((y + h) / c))) - constant;
-        if (SimTK::isNaN(Fspring) || SimTK::isInf(Fspring)) {
-            Fspring = 0;
+        const SimTK::Real& a = get_stiffness();
+        const SimTK::Real& b = get_dissipation();
+        if (depth > 0) {
+            force[1] = fmax(0, a * pow(depth, 3) * (1 + b * depthRate));
         }
+        const SimTK::Real voidStiffness = 1.0; // N/m
+        force[1] += voidStiffness * depth;
 
-        const SimTK::Real Fy = Fspring * (1 + Cval * depthRate);
-
-        force[1] = Fy;
-
-        /// Friction force.
-        const SimTK::Real mu_d = 1;
-        const SimTK::Real latchvel = 0.05; // m/s
+        const SimTK::Real velSlidingScaling =
+                get_tangent_velocity_scaling_factor();
+        // The paper used (1 - exp(-x)) / (1 + exp(-x)) = tanh(2x).
+        // tanh() has a wider domain than using exp().
+        const SimTK::Real transition = tanh(velSliding / velSlidingScaling / 2);
 
-        const SimTK::Real mu = mu_d * tanh(velSliding / latchvel / 2);
-        force[0] = -force[1] * mu;
+        const SimTK::Real frictionForce =
+                -transition * get_friction_coefficient() * force[1];
 
+        force[0] = frictionForce;
         return force;
     }
 
 private:
     void constructProperties() {
-        constructProperty_stiffness(1e4);
-        constructProperty_dissipation(1e-2);
-        constructProperty_tscale(1.0);
+        constructProperty_friction_coefficient(1.0);
+        constructProperty_stiffness(5e7);
+        constructProperty_dissipation(1.0);
+        constructProperty_tangent_velocity_scaling_factor(0.05);
     }
-
 };
 
 /** This contact model uses a continuous equation to transition between in and
@@ -239,7 +292,7 @@ retains a normal metabolic cost in simulated walking after transtibial limb
 loss. PLoS ONE, 13(1), e0191310. http://doi.org/10.1371/journal.pone.0191310
 
 @underdevelopment */
-class OSIMMOCO_API EspositoMiller2018Force
+class OSIMSIMULATION_API EspositoMiller2018Force
         : public StationPlaneContactForce {
 OpenSim_DECLARE_CONCRETE_OBJECT(EspositoMiller2018Force,
         StationPlaneContactForce);
@@ -266,7 +319,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(EspositoMiller2018Force,
 
     /// Compute the force applied to body to which the station is attached, at
     /// the station, expressed in ground.
-    SimTK::Vec3 calcContactForceOnStation(const SimTK::State& s)
+    SimTK::Vec3 computeContactForceOnStation(const SimTK::State& s)
     const override {
         using SimTK::square;
         SimTK::Vec3 force(0);