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

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)
-mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[2]) {
+// muscle time constant with optional smoothing
+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
-  mjtNum tau;
-  if (ctrlclamp>act) {
-    tau = prm[0] * (0.5 + 1.5*actclamp);
-  } else {
-    tau = prm[1] / (0.5 + 1.5*actclamp);
-  }
+  // compute timescales as in Millard et al. (2013) https://doi.org/10.1115/1.4023390
+  mjtNum tau_act = prm[0] * (0.5 + 1.5*actclamp);    // activation timscale
+  mjtNum tau_deact = prm[1] / (0.5 + 1.5*actclamp);  // deactivation timscale
+  mjtNum smoothing_width = prm[2];                   // width of smoothing sigmoid
+  mjtNum dctrl = ctrlclamp - act;                    // excess excitation
+
+  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)
-MJAPI mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[2]);
+// muscle time constant with optional smoothing
+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);