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

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