Fix bug in muscle length-gain curve. Fixes #1342

PiperOrigin-RevId: 599165049
Change-Id: Ic5ef77b4349a8a9c343eacfd781195bfebea9aca

doc/changelog.rst

@@ -21,6 +21,8 @@ Bug fixes
 ^^^^^^^^^
 6. Fixed a bug that prevented the use of pins with plugins if flexes are not in the worldbody. Fixes
    :github:issue:`1270`.
+7. Fixed a bug in the :ref:`muscle model<CMuscle>` that led to non-zero values outside the lower
+   bound of the length range. Fixes :github:issue:`1342`.
 
 
 Version 3.1.1 (December 18, 2023)

src/engine/engine_derivative.c

@@ -790,11 +790,6 @@ static mjtNum mjd_muscleGain_vel(mjtNum len, mjtNum vel, const mjtNum lengthrang
     force = scale / mju_max(mjMINVAL, acc0);
   }
 
-  // mid-ranges
-  mjtNum a = 0.5*(lmin+1);
-  mjtNum b = 0.5*(1+lmax);
-  mjtNum x;
-
   // optimum length
   mjtNum L0 = (lengthrange[1]-lengthrange[0]) / mju_max(mjMINVAL, range[1]-range[0]);
 
@@ -803,20 +798,7 @@ static mjtNum mjd_muscleGain_vel(mjtNum len, mjtNum vel, const mjtNum lengthrang
   mjtNum V = vel / mju_max(mjMINVAL, L0*vmax);
 
   // length curve
-  mjtNum FL = 0;
+  mjtNum FL = mju_muscleGainLength(L, lmin, lmax);
-  if (L >= lmin && L <= a) {
-    x = (L-lmin) / mju_max(mjMINVAL, a-lmin);
-    FL = 0.5*x*x;
-  } else if (L <= 1) {
-    x = (1-L) / mju_max(mjMINVAL, 1-a);
-    FL = 1 - 0.5*x*x;
-  } else if (L <= b) {
-    x = (L-1) / mju_max(mjMINVAL, b-1);
-    FL = 1 - 0.5*x*x;
-  } else if (L <= lmax) {
-    x = (lmax-L) / mju_max(mjMINVAL, lmax-b);
-    FL = 0.5*x*x;
-  }
 
   // velocity curve
   mjtNum dFV;

src/engine/engine_util_misc.c

@@ -455,6 +455,33 @@ void mju_geomSemiAxes(const mjModel* m, int geom_id, mjtNum semiaxes[3]) {
 
 //------------------------------ actuator models ---------------------------------------------------
 
+// normalized muscle length-gain curve
+mjtNum mju_muscleGainLength(mjtNum length, mjtNum lmin, mjtNum lmax) {
+  if (lmin <= length && length <= lmax) {
+    // mid-ranges (maximum is at 1.0)
+    mjtNum a = 0.5*(lmin+1);
+    mjtNum b = 0.5*(1+lmax);
+
+    if (length <= a) {
+      mjtNum x = (length-lmin) / mjMAX(mjMINVAL, a-lmin);
+      return 0.5*x*x;
+    } else if (length <= 1) {
+      mjtNum x = (1-length) / mjMAX(mjMINVAL, 1-a);
+      return 1 - 0.5*x*x;
+    } else if (length <= b) {
+      mjtNum x = (length-1) / mjMAX(mjMINVAL, b-1);
+      return 1 - 0.5*x*x;
+    } else {
+      mjtNum x = (lmax-length) / mjMAX(mjMINVAL, lmax-b);
+      return 0.5*x*x;
+    }
+  }
+
+  return 0.0;
+}
+
+
+
 // muscle active force, prm = (range[2], force, scale, lmin, lmax, vmax, fpmax, fvmax)
 mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2],
                       mjtNum acc0, const mjtNum prm[9]) {
@@ -472,11 +499,6 @@ mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2],
     force = scale / mjMAX(mjMINVAL, acc0);
   }
 
-  // mid-ranges
-  mjtNum a = 0.5*(lmin+1);
-  mjtNum b = 0.5*(1+lmax);
-  mjtNum x;
-
   // optimum length
   mjtNum L0 = (lengthrange[1]-lengthrange[0]) / mjMAX(mjMINVAL, range[1]-range[0]);
 
@@ -485,20 +507,7 @@ mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2],
   mjtNum V = vel / mjMAX(mjMINVAL, L0*vmax);
 
   // length curve
-  mjtNum FL = 0;
+  mjtNum FL = mju_muscleGainLength(L, lmin, lmax);
-  if (L >= lmin && L <= a) {
-    x = (L-lmin) / mjMAX(mjMINVAL, a-lmin);
-    FL = 0.5*x*x;
-  } else if (L <= 1) {
-    x = (1-L) / mjMAX(mjMINVAL, 1-a);
-    FL = 1 - 0.5*x*x;
-  } else if (L <= b) {
-    x = (L-1) / mjMAX(mjMINVAL, b-1);
-    FL = 1 - 0.5*x*x;
-  } else if (L <= lmax) {
-    x = (lmax-L) / mjMAX(mjMINVAL, lmax-b);
-    FL = 0.5*x*x;
-  }
 
   // velocity curve
   mjtNum FV;

src/engine/engine_util_misc.h

@@ -33,6 +33,9 @@ mjtNum mju_wrap(mjtNum* wpnt, const mjtNum* x0, const mjtNum* x1,
                 const mjtNum* xpos, const mjtNum* xmat, const mjtNum* size,
                 int type, const mjtNum* side);
 
+// normalized muscle length-gain curve
+MJAPI mjtNum mju_muscleGainLength(mjtNum length, mjtNum lmin, mjtNum lmax);
+
 // muscle active force, prm = (range[2], force, scale, lmin, lmax, vmax, fpmax, fvmax)
 MJAPI mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2],
                             mjtNum acc0, const mjtNum prm[9]);

test/engine/engine_util_misc_test.cc

@@ -135,6 +135,19 @@ TEST_F(MujocoTest, SmoothMuscleDynamics) {
   }
 }
 
+TEST_F(MujocoTest, MuscleGainLength) {
+  mjtNum lmin = 0.5;
+  mjtNum lmax = 1.5;
+
+  EXPECT_EQ(mju_muscleGainLength(0.0,  lmin, lmax), 0);
+  EXPECT_EQ(mju_muscleGainLength(0.5,  lmin, lmax), 0);
+  EXPECT_EQ(mju_muscleGainLength(0.75, lmin, lmax), 0.5);
+  EXPECT_EQ(mju_muscleGainLength(1.0,  lmin, lmax), 1);
+  EXPECT_EQ(mju_muscleGainLength(1.25, lmin, lmax), 0.5);
+  EXPECT_EQ(mju_muscleGainLength(1.5,  lmin, lmax), 0);
+  EXPECT_EQ(mju_muscleGainLength(2.0,  lmin, lmax), 0);
+}
+
 TEST_F(MujocoTest, mju_makefullname) {
   char buffer[1000];
   constexpr char path[] = "engine/testdata/";