Added optional smoothing to muscle dynamics:

- `muscle` actuators take a new `tausmooth` attribute (defaults to 0) which when positive, smooths the transition between activation and deactivation timescales.
- `mju_muscleDynamics` takes 3 parameters, adding width of smoothing sigmoid.

PiperOrigin-RevId: 531473354
Change-Id: I8ab6c0289ff04437e798c3ca6edda132a30fcc31

doc/APIreference/functions.rst

@@ -2944,7 +2944,7 @@ mju_muscleDynamics
 
 .. mujoco-include:: mju_muscleDynamics
 
-Muscle activation dynamics, prm = (tau_act, tau_deact).
+Muscle activation dynamics, prm = (tau_act, tau_deact, smoothing_width).
 
 .. _mju_encodePyramid:
 

doc/XMLreference.rst

@@ -5535,7 +5535,7 @@ section. The underlying :el:`general` attributes are set as follows:
 ========= ======= ========= ======================================================
 Attribute Setting Attribute Setting
 ========= ======= ========= ======================================================
-dyntype   muscle  dynprm    timeconst(2)
+dyntype   muscle  dynprm    timeconst(2) tausmooth
 gaintype  muscle  gainprm   range(2), force, scale, lmin, lmax, vmax, fpmax, fvmax
 biastype  muscle  biasprm   same as gainprm
 ========= ======= ========= ======================================================
@@ -5589,6 +5589,12 @@ This element has nine custom attributes in addition to the common attributes:
 :at:`timeconst`: :at-val:`real(2), "0.01 0.04"`
    Time constants for activation and de-activation dynamics.
 
+.. _actuator-muscle-tausmooth:
+
+:at:`tausmooth`: :at-val:`real, "0"`
+   Width of smooth transition between activation and deactivation time constants. Units of ctrl, must be
+   nonegative.
+
 .. _actuator-muscle-range:
 
 :at:`range`: :at-val:`real(2), "0.75 1.05"`

doc/XMLschema.rst

@@ -1105,13 +1105,13 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`jointinparent<actuator-muscle-jointinparent>`             | :ref:`tendon<actuator-muscle-tendon>`                           | :ref:`slidersite<actuator-muscle-slidersite>`                   | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`cranksite<actuator-muscle-cranksite>`                     | :ref:`timeconst<actuator-muscle-timeconst>`                     | :ref:`range<actuator-muscle-range>`                             | |
+|                                    |    |    | :ref:`cranksite<actuator-muscle-cranksite>`                     | :ref:`timeconst<actuator-muscle-timeconst>`                     | :ref:`tausmooth<actuator-muscle-tausmooth>`                     | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`force<actuator-muscle-force>`                             | :ref:`scale<actuator-muscle-scale>`                             | :ref:`lmin<actuator-muscle-lmin>`                               | |
+|                                    |    |    | :ref:`range<actuator-muscle-range>`                             | :ref:`force<actuator-muscle-force>`                             | :ref:`scale<actuator-muscle-scale>`                             | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`lmax<actuator-muscle-lmax>`                               | :ref:`vmax<actuator-muscle-vmax>`                               | :ref:`fpmax<actuator-muscle-fpmax>`                             | |
+|                                    |    |    | :ref:`lmin<actuator-muscle-lmin>`                               | :ref:`lmax<actuator-muscle-lmax>`                               | :ref:`vmax<actuator-muscle-vmax>`                               | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`fvmax<actuator-muscle-fvmax>`                             |                                                                 |                                                                 | |
+|                                    |    |    | :ref:`fpmax<actuator-muscle-fpmax>`                             | :ref:`fvmax<actuator-muscle-fvmax>`                             |                                                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| actuator |br| |_| |L|          |    | .. table::                                                                                                                                                                                                 |

doc/changelog.rst

@@ -45,6 +45,11 @@ General
         6x^5 - 15x^4 + 10x^3, & 0 \lt & x \lt 1  \\
         1,                    & 1 \le & x \qquad
      \end{cases}
+- Added optional :ref:`tausmooth<actuator-muscle-tausmooth>` attribute to muscle actuators. When positive, the
+  time-constant :math:`\tau` of muscle activation/deactivation uses :ref:`mju_sigmoid` to transition smoothly
+  between the two extremal values given by the `Millard et al. (2013) <https://doi.org/10.1115/1.4023390>`__ muscle
+  model, within a range of width tausmooth.  See :ref:`Muscle actuators<CMuscle>` for more details.
+  Relatedly, :ref:`mju_muscleDynamics` now takes 3 parameters instead of 2, adding the new smoothing-width parameter.
 
 
 .. youtube:: ZppeDArq6AU

doc/includes/references.h

@@ -2434,7 +2434,7 @@ mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2],
                       mjtNum acc0, const mjtNum prm[9]);
 mjtNum mju_muscleBias(mjtNum len, const mjtNum lengthrange[2],
                       mjtNum acc0, const mjtNum prm[9]);
-mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[2]);
+mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]);
 void mju_encodePyramid(mjtNum* pyramid, const mjtNum* force, const mjtNum* mu, int dim);
 void mju_decodePyramid(mjtNum* force, const mjtNum* pyramid, const mjtNum* mu, int dim);
 mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum Kp, mjtNum Kv, mjtNum dt);

doc/modeling.rst

@@ -754,13 +754,12 @@ ascending portion of the :math:`\text{FL}` curve, or the descending portion, or
 model with 50 muscles. Do you believe that someone did careful experiments and measured the operating range for every
 muscle in your model, taking into account all the joints that the muscle spans? If not, then it is better to think of
 musculo-skeletal models as having the same general behavior as the biological system, while being different in various
-details - including details that are of great interest to some research community. For most muscle properties which
+details -- including details that are of great interest to some research community. For most muscle properties which
 modelers consider constant and known, there is an experimental paper showing that they vary under some conditions. This
 is not to discourage people from building accurate models, but rather to discourage people from believing too strongly
-in their models. Modeling in biology is quite different from modeling in physics and engineering... which is why we find
+in their models.
-it ironic when people in Robotics complain that building accurate robot models is hard.
 
-Coming back to our muscle model, there is the muscle activation act. This is the state of a first-order nonlinear
+Coming back to our muscle model, there is the muscle activation ``act``. This is the state of a first-order nonlinear
 filter whose input is the control signal. The filter dynamics are:
 
 
@@ -773,12 +772,17 @@ are two time constants specified with the attribute timeconst, namely :math:`\te
 <https://doi.org/10.1115/1.4023390>`__, the effective time constant :math:`\tau` is then computed at runtime as:
 
 .. math::
-   \tau(\texttt{ctrl}, \texttt{act}) =
+   \tau(\texttt{ctrl}-\texttt{act}) =
    \begin{cases}
-      \tau_\text{act} \cdot (0.5 + 1.5\cdot\texttt{act}) & \texttt{ctrl} \gt \texttt{act} \\
+      \tau_\text{act} \cdot (0.5 + 1.5\cdot\texttt{act}) & \texttt{ctrl}-\texttt{act} \gt 0 \\
-      \tau_\text{deact} / (0.5 + 1.5\cdot\texttt{act}) & \texttt{ctrl} \leq \texttt{act}
+      \tau_\text{deact} / (0.5 + 1.5\cdot\texttt{act}) & \texttt{ctrl} - \texttt{act} \leq 0
    \end{cases}
 
+Since the above equation describes discontinuous switching, which can be undesirable when using derivative-based
+optimization, we introduce the optional smoothing parameter :ref:`tausmooth<actuator-muscle-tausmooth>`. When greater
+than 0, the switching is replaced by :ref:`mju_sigmoid`, which will smoothly interpolate between the two values within
+the range :math:`(\texttt{ctrl}-\texttt{act}) \pm \text{tausmooth}/2`.
+
 Now we summarize the attributes of element :ref:`muscle <actuator-muscle>` which users may want to adjust,
 depending on their familiarity with the biomechanics literature and availability of detailed measurements with regard
 to a particular model:
@@ -801,6 +805,10 @@ timeconst
    Muscles are composed of slow-twitch and fast-twitch fibers. The typical muscle is mixed, but some muscles have a
    higher proportion of one or the other fiber type, making them faster or slower. This can be modeled by adjusting the
    time constants. The vmax parameter of the :math:`\text{\small FLV}` function should also be adjusted accordingly.
+tausmooth
+   When positive, smooths the transition between activation and de-activation time-constants. While a single
+   `motor unit <https://en.wikipedia.org/wiki/Motor_unit>`__ is either activating or de-activating, an entire muscle
+   will have a mixture of many units, leading to a corresponding mixture of timescales.
 lmin, lmax, vmax, fpmax, fvmax
    These are the parameters controlling the shape of the :math:`\text{\small FLV}` function. Advanced users can
    experiment with them; see MATLAB function `FLV.m <_static/FLV.m>`__. Similar to the scale setting, if you want to

include/mujoco/mujoco.h

@@ -1144,8 +1144,8 @@ MJAPI mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2],
 MJAPI mjtNum mju_muscleBias(mjtNum len, const mjtNum lengthrange[2],
                             mjtNum acc0, const mjtNum prm[9]);
 
-// Muscle activation dynamics, prm = (tau_act, tau_deact).
+// Muscle activation dynamics, prm = (tau_act, tau_deact, smoothing_width).
-MJAPI mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[2]);
+MJAPI mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]);
 
 // Convert contact force to pyramid representation.
 MJAPI void mju_encodePyramid(mjtNum* pyramid, const mjtNum* force, const mjtNum* mu, int dim);

introspect/functions.py

@@ -7294,11 +7294,11 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
                  name='prm',
                  type=ArrayType(
                      inner_type=ValueType(name='mjtNum', is_const=True),
-                     extents=(2,),
+                     extents=(3,),
                  ),
              ),
          ),
-         doc='Muscle activation dynamics, prm = (tau_act, tau_deact).',
+         doc='Muscle activation dynamics, prm = (tau_act, tau_deact, smoothing_width).',  # pylint: disable=line-too-long
      )),
     ('mju_encodePyramid',
      FunctionDecl(

src/engine/engine_util_misc.c

@@ -552,24 +552,44 @@ mjtNum mju_muscleBias(mjtNum len, const mjtNum lengthrange[2],
 
 
 
-// muscle activation dynamics, prm = (tau_act, tau_deact)
+// muscle time constant with optional smoothing
-mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[2]) {
+mjtNum mju_muscleDynamicsTimescale(mjtNum dctrl, mjtNum tau_act, mjtNum tau_deact,
+                                   mjtNum smoothing_width) {
+  mjtNum tau;
+
+  // hard switching
+  if (smoothing_width < mjMINVAL) {
+    tau = dctrl > 0 ? tau_act : tau_deact;
+  }
+
+  // smooth switching
+  else {
+    // scale by width, center around 0.5 midpoint, rescale to bounds
+    tau = tau_deact + (tau_act-tau_deact)*mju_sigmoid(dctrl/smoothing_width + 0.5);
+  }
+  return tau;
+}
+
+
+
+// muscle activation dynamics, prm = (tau_act, tau_deact, smoothing_width)
+mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]) {
   // clamp control
   mjtNum ctrlclamp = mju_clip(ctrl, 0, 1);
 
   // clamp activation
   mjtNum actclamp = mju_clip(act, 0, 1);
 
-  // compute time constant as in Millard et al. (2013) https://doi.org/10.1115/1.4023390
+  // compute timescales as in Millard et al. (2013) https://doi.org/10.1115/1.4023390
-  mjtNum tau;
+  mjtNum tau_act = prm[0] * (0.5 + 1.5*actclamp);    // activation timscale
-  if (ctrlclamp>act) {
+  mjtNum tau_deact = prm[1] / (0.5 + 1.5*actclamp);  // deactivation timscale
-    tau = prm[0] * (0.5 + 1.5*actclamp);
+  mjtNum smoothing_width = prm[2];                   // width of smoothing sigmoid
-  } else {
+  mjtNum dctrl = ctrlclamp - act;                    // excess excitation
-    tau = prm[1] / (0.5 + 1.5*actclamp);
+
-  }
+  mjtNum tau = mju_muscleDynamicsTimescale(dctrl, tau_act, tau_deact, smoothing_width);
 
   // filter output
-  return (ctrlclamp-act) / mjMAX(mjMINVAL, tau);
+  return dctrl / mjMAX(mjMINVAL, tau);
 }
 
 

src/engine/engine_util_misc.h

@@ -39,8 +39,12 @@ MJAPI mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2],
 MJAPI mjtNum mju_muscleBias(mjtNum len, const mjtNum lengthrange[2],
                             mjtNum acc0, const mjtNum prm[9]);
 
-// muscle activation dynamics, prm = (tau_act, tau_deact)
+// muscle time constant with optional smoothing
-MJAPI mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[2]);
+MJAPI mjtNum mju_muscleDynamicsTimescale(mjtNum dctrl, mjtNum tau_act, mjtNum tau_deact,
+                                         mjtNum smoothing_width);
+
+// muscle activation dynamics, prm = (tau_act, tau_deact, smoothing_width)
+MJAPI mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]);
 
 // all 3 semi-axes of a geom
 MJAPI void mju_geomSemiAxes(const mjModel* m, int geom_id, mjtNum semiaxes[3]);

src/xml/xml_native_reader.cc

@@ -356,11 +356,11 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
             "lengthrange", "gear", "cranklength", "user",
             "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
             "timeconst", "area", "diameter", "bias"},
-        {"muscle", "*", "25",  "name", "class", "group",
+        {"muscle", "*", "26",  "name", "class", "group",
             "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
             "lengthrange", "gear", "cranklength", "user",
             "joint", "jointinparent", "tendon", "slidersite", "cranksite",
-            "timeconst", "range", "force", "scale",
+            "timeconst", "tausmooth", "range", "force", "scale",
             "lmin", "lmax", "vmax", "fpmax", "fvmax"},
         {"adhesion", "*", "9", "name", "class", "group",
             "forcelimited", "ctrlrange", "forcerange", "user", "body", "gain"},
@@ -1606,7 +1606,6 @@ void mjXReader::OneTendon(XMLElement* elem, mjCTendon* pten) {
 // actuator element parser
 void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
   string text, type;
-  double diameter;
 
   // common attributes
   ReadAttrTxt(elem, "name", pact->name);
@@ -1774,6 +1773,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
     ReadAttr(elem, "timeconst", 1, pact->dynprm, text);
     ReadAttr(elem, "bias", 3, pact->biasprm, text);
     ReadAttr(elem, "area", 1, pact->gainprm, text);
+    double diameter;
     if (ReadAttr(elem, "diameter", 1, &diameter, text)) {
       pact->gainprm[0] = mjPI / 4 * diameter*diameter;
     }
@@ -1801,6 +1801,9 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
 
     // explicit attributes
     ReadAttr(elem, "timeconst", 2, pact->dynprm, text);
+    ReadAttr(elem, "tausmooth", 1, pact->dynprm+2, text);
+    if (pact->dynprm[2]<0)
+      throw mjXError(elem, "muscle tausmooth cannot be negative");
     ReadAttr(elem, "range", 2, pact->gainprm, text);
     ReadAttr(elem, "force", 1, pact->gainprm+2, text);
     ReadAttr(elem, "scale", 1, pact->gainprm+3, text);

test/engine/engine_util_misc_test.cc

@@ -70,5 +70,63 @@ TEST_F(MujocoTest, Sigmoid) {
   EXPECT_THAT(dy_dx_0p5, DoubleNear(expected, dx));
 }
 
+// compute time constant as in Millard et al. (2013) https://doi.org/10.1115/1.4023390
+mjtNum muscleDynamicsMillard(mjtNum ctrl, mjtNum act, const mjtNum prm[2]) {
+  // clamp control
+  mjtNum ctrlclamp = mju_clip(ctrl, 0, 1);
+
+  // clamp activation
+  mjtNum actclamp = mju_clip(act, 0, 1);
+
+  mjtNum tau;
+  if (ctrlclamp > act) {
+    tau = prm[0] * (0.5 + 1.5*actclamp);
+  } else {
+    tau = prm[1] / (0.5 + 1.5*actclamp);
+  }
+
+  // filter output
+  return (ctrlclamp-act) / mjMAX(mjMINVAL, tau);
+}
+
+TEST_F(MujocoTest, SmoothMuscleDynamics) {
+  mjtNum prm[3] = {0.01, 0.04, 0.0};
+
+  // exact equality if tau_smooth = 0
+  for (mjtNum ctrl : {-0.1, 0.0, 0.4, 0.5, 1.0, 1.1}) {
+    for (mjtNum act : {-0.1, 0.0, 0.4, 0.5, 1.0, 1.1}) {
+      mjtNum actdot_old = muscleDynamicsMillard(ctrl, act, prm);
+      mjtNum actdot_new = mju_muscleDynamics(ctrl, act, prm);
+      EXPECT_EQ(actdot_new, actdot_old);
+    }
+  }
+
+  // positive tau_smooth
+  mjtNum tau_smooth = 0.2;
+  prm[2] = tau_smooth;
+  mjtNum act = 0.5;
+  mjtNum eps = 1e-6;
+
+  mjtNum ctrl = 0.4 - eps;  // smaller than act by just over 0.5*tau_smooth
+  EXPECT_EQ(muscleDynamicsMillard(ctrl, act, prm),
+            mju_muscleDynamics(ctrl, act, prm));
+
+  ctrl = 0.6 + eps;         // larger than act by just over 0.5*tau_smooth
+  EXPECT_EQ(muscleDynamicsMillard(ctrl, act, prm),
+            mju_muscleDynamics(ctrl, act, prm));
+
+  // right in the middle should give average of time constants
+  mjtNum tau_act = 0.2;
+  mjtNum tau_deact = 0.3;
+  for (mjtNum dctrl : {0.0, 0.1, 0.2, 1.0, 1.1}) {
+    mjtNum lower = mju_muscleDynamicsTimescale(-dctrl,
+                                               tau_act, tau_deact, tau_smooth);
+    mjtNum upper = mju_muscleDynamicsTimescale(dctrl,
+                                               tau_act, tau_deact, tau_smooth);
+    EXPECT_EQ(0.5*(upper + lower), 0.5*(tau_act + tau_deact));
+  }
+}
+
+
 }  // namespace
 }  // namespace mujoco

test/xml/xml_native_reader_test.cc

@@ -934,6 +934,49 @@ TEST_F(ActuatorParseTest, ActdimDefaultsPropagate) {
   mj_deleteModel(model);
 }
 
+TEST_F(ActuatorParseTest, MusclesParseSmoothdyn) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <geom size="1"/>
+        <joint name="hinge" limited="true" range="-1 1"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <muscle joint="hinge"/>
+      <muscle joint="hinge" tausmooth="0.4"/>
+    </actuator>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull()) << error.data();
+  EXPECT_EQ(model->actuator_dynprm[2], 0.0);
+  EXPECT_EQ(model->actuator_dynprm[mjNDYN + 2], 0.4);
+  mj_deleteModel(model);
+}
+
+TEST_F(ActuatorParseTest, MusclesSmoothdynNegative) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <geom size="1"/>
+        <joint name="hinge" limited="true" range="-1 1"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <muscle joint="hinge" tausmooth="-0.4"/>
+    </actuator>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, IsNull());
+  EXPECT_THAT(error.data(), HasSubstr("muscle tausmooth cannot be negative"));
+}
+
 // ------------- test sensor parsing -------------------------------------------
 
 using SensorParseTest = MujocoTest;
@@ -1039,6 +1082,5 @@ TEST_F(XMLReaderTest, ExtentNegativeNotAllowed) {
   EXPECT_THAT(error.data(), HasSubstr("extent must be strictly positive"));
 }
 
-
 }  // namespace
 }  // namespace mujoco