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
2023-05-12 04:58:17 -07:00
parent 777cad52b4
commit 770b4b363a
13 changed files with 180 additions and 34 deletions
@@ -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