From a14a584f1d506c8636342af6f39e5e7157966a1a Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 17 Jan 2024 07:19:11 -0800 Subject: [PATCH] Fix bug in muscle length-gain curve. Fixes #1342 PiperOrigin-RevId: 599165049 Change-Id: Ic5ef77b4349a8a9c343eacfd781195bfebea9aca --- doc/changelog.rst | 2 ++ src/engine/engine_derivative.c | 20 +----------- src/engine/engine_util_misc.c | 47 +++++++++++++++++----------- src/engine/engine_util_misc.h | 3 ++ test/engine/engine_util_misc_test.cc | 13 ++++++++ 5 files changed, 47 insertions(+), 38 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index eb9aeab0..1f97f21f 100644 --- a/doc/changelog.rst +++ b/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` 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) diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index c0c0d5fe..643c9b4f 100644 --- a/src/engine/engine_derivative.c +++ b/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; - 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; - } + mjtNum FL = mju_muscleGainLength(L, lmin, lmax); // velocity curve mjtNum dFV; diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 37eca9e4..876fe52a 100644 --- a/src/engine/engine_util_misc.c +++ b/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; - 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; - } + mjtNum FL = mju_muscleGainLength(L, lmin, lmax); // velocity curve mjtNum FV; diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index cb317258..21bc3f83 100644 --- a/src/engine/engine_util_misc.h +++ b/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]); diff --git a/test/engine/engine_util_misc_test.cc b/test/engine/engine_util_misc_test.cc index 71702b8c..97557914 100644 --- a/test/engine/engine_util_misc_test.cc +++ b/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/";