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 contant 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

@@ -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  *
@@ -149,8 +149,52 @@ 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 */
+
+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 OSIMMOCO_API MeyerFregly2016Force
         : public StationPlaneContactForce {
 OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force,
@@ -160,8 +204,12 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force,
             "Spring stiffness in N/m (default: 1e4).");
     OpenSim_DECLARE_PROPERTY(dissipation, double,
             "Dissipation coefficient in s/m (default: 0.01).");
-    OpenSim_DECLARE_PROPERTY(tscale, double,
-            "TODO");
+    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).");
 
     MeyerFregly2016Force() {
         constructProperties();
@@ -177,17 +225,16 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force,
         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 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 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;
 
         /// Normal force.
         const SimTK::Real vp = (Kval + klow) / (Kval - klow);
@@ -198,20 +245,22 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force,
 
         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 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 mu_d = get_dynamic_friction();
+        const SimTK::Real mu_v = get_viscous_friction();
+        const SimTK::Real latchvel = get_latch_velocity();
 
-        const SimTK::Real mu = mu_d * tanh(velSliding / latchvel / 2);
-        force[0] = -force[1] * mu;
+        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;
 
         return force;
     }
@@ -220,7 +269,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MeyerFregly2016Force,
     void constructProperties() {
         constructProperty_stiffness(1e4);
         constructProperty_dissipation(1e-2);
-        constructProperty_tscale(1.0);
+        constructProperty_dynamic_friction(0);
+        constructProperty_viscous_friction(5);
+        constructProperty_latch_velocity(0.05);
     }
 
 };