diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index c81f9cb8..dff71a37 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -2944,7 +2944,7 @@ mju_muscleDynamics .. mujoco-include:: mju_muscleDynamics -Muscle activation dynamics, prm = (tau_act, tau_deact). +Muscle activation dynamics, prm = (tau_act, tau_deact, smoothing_width). .. _mju_encodePyramid: diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 02d9c4df..ee849901 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -5535,7 +5535,7 @@ section. The underlying :el:`general` attributes are set as follows: ========= ======= ========= ====================================================== Attribute Setting Attribute Setting ========= ======= ========= ====================================================== -dyntype muscle dynprm timeconst(2) +dyntype muscle dynprm timeconst(2) tausmooth gaintype muscle gainprm range(2), force, scale, lmin, lmax, vmax, fpmax, fvmax biastype muscle biasprm same as gainprm ========= ======= ========= ====================================================== @@ -5589,6 +5589,12 @@ This element has nine custom attributes in addition to the common attributes: :at:`timeconst`: :at-val:`real(2), "0.01 0.04"` Time constants for activation and de-activation dynamics. +.. _actuator-muscle-tausmooth: + +:at:`tausmooth`: :at-val:`real, "0"` + Width of smooth transition between activation and deactivation time constants. Units of ctrl, must be + nonegative. + .. _actuator-muscle-range: :at:`range`: :at-val:`real(2), "0.75 1.05"` diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 7ede7c9f..4be0f69a 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -1105,13 +1105,13 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`jointinparent` | :ref:`tendon` | :ref:`slidersite` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`cranksite` | :ref:`timeconst` | :ref:`range` | | +| | | | :ref:`cranksite` | :ref:`timeconst` | :ref:`tausmooth` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`force` | :ref:`scale` | :ref:`lmin` | | +| | | | :ref:`range` | :ref:`force` | :ref:`scale` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`lmax` | :ref:`vmax` | :ref:`fpmax` | | +| | | | :ref:`lmin` | :ref:`lmax` | :ref:`vmax` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`fvmax` | | | | +| | | | :ref:`fpmax` | :ref:`fvmax` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| actuator |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 29f27d03..f383025b 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -45,6 +45,11 @@ General 6x^5 - 15x^4 + 10x^3, & 0 \lt & x \lt 1 \\ 1, & 1 \le & x \qquad \end{cases} +- Added optional :ref:`tausmooth` attribute to muscle actuators. When positive, the + time-constant :math:`\tau` of muscle activation/deactivation uses :ref:`mju_sigmoid` to transition smoothly + between the two extremal values given by the `Millard et al. (2013) `__ muscle + model, within a range of width tausmooth. See :ref:`Muscle actuators` for more details. + Relatedly, :ref:`mju_muscleDynamics` now takes 3 parameters instead of 2, adding the new smoothing-width parameter. .. youtube:: ZppeDArq6AU diff --git a/doc/includes/references.h b/doc/includes/references.h index 8e3a2ef1..119bffbe 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -2434,7 +2434,7 @@ mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2], mjtNum acc0, const mjtNum prm[9]); mjtNum mju_muscleBias(mjtNum len, const mjtNum lengthrange[2], mjtNum acc0, const mjtNum prm[9]); -mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[2]); +mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]); void mju_encodePyramid(mjtNum* pyramid, const mjtNum* force, const mjtNum* mu, int dim); void mju_decodePyramid(mjtNum* force, const mjtNum* pyramid, const mjtNum* mu, int dim); mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum Kp, mjtNum Kv, mjtNum dt); diff --git a/doc/modeling.rst b/doc/modeling.rst index f1a72f65..832d80aa 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -754,13 +754,12 @@ ascending portion of the :math:`\text{FL}` curve, or the descending portion, or model with 50 muscles. Do you believe that someone did careful experiments and measured the operating range for every muscle in your model, taking into account all the joints that the muscle spans? If not, then it is better to think of musculo-skeletal models as having the same general behavior as the biological system, while being different in various -details - including details that are of great interest to some research community. For most muscle properties which +details -- including details that are of great interest to some research community. For most muscle properties which modelers consider constant and known, there is an experimental paper showing that they vary under some conditions. This is not to discourage people from building accurate models, but rather to discourage people from believing too strongly -in their models. Modeling in biology is quite different from modeling in physics and engineering... which is why we find -it ironic when people in Robotics complain that building accurate robot models is hard. +in their models. -Coming back to our muscle model, there is the muscle activation act. This is the state of a first-order nonlinear +Coming back to our muscle model, there is the muscle activation ``act``. This is the state of a first-order nonlinear filter whose input is the control signal. The filter dynamics are: @@ -773,12 +772,17 @@ are two time constants specified with the attribute timeconst, namely :math:`\te `__, the effective time constant :math:`\tau` is then computed at runtime as: .. math:: - \tau(\texttt{ctrl}, \texttt{act}) = + \tau(\texttt{ctrl}-\texttt{act}) = \begin{cases} - \tau_\text{act} \cdot (0.5 + 1.5\cdot\texttt{act}) & \texttt{ctrl} \gt \texttt{act} \\ - \tau_\text{deact} / (0.5 + 1.5\cdot\texttt{act}) & \texttt{ctrl} \leq \texttt{act} + \tau_\text{act} \cdot (0.5 + 1.5\cdot\texttt{act}) & \texttt{ctrl}-\texttt{act} \gt 0 \\ + \tau_\text{deact} / (0.5 + 1.5\cdot\texttt{act}) & \texttt{ctrl} - \texttt{act} \leq 0 \end{cases} +Since the above equation describes discontinuous switching, which can be undesirable when using derivative-based +optimization, we introduce the optional smoothing parameter :ref:`tausmooth`. When greater +than 0, the switching is replaced by :ref:`mju_sigmoid`, which will smoothly interpolate between the two values within +the range :math:`(\texttt{ctrl}-\texttt{act}) \pm \text{tausmooth}/2`. + Now we summarize the attributes of element :ref:`muscle ` which users may want to adjust, depending on their familiarity with the biomechanics literature and availability of detailed measurements with regard to a particular model: @@ -801,6 +805,10 @@ timeconst Muscles are composed of slow-twitch and fast-twitch fibers. The typical muscle is mixed, but some muscles have a higher proportion of one or the other fiber type, making them faster or slower. This can be modeled by adjusting the time constants. The vmax parameter of the :math:`\text{\small FLV}` function should also be adjusted accordingly. +tausmooth + When positive, smooths the transition between activation and de-activation time-constants. While a single + `motor unit `__ is either activating or de-activating, an entire muscle + will have a mixture of many units, leading to a corresponding mixture of timescales. lmin, lmax, vmax, fpmax, fvmax These are the parameters controlling the shape of the :math:`\text{\small FLV}` function. Advanced users can experiment with them; see MATLAB function `FLV.m <_static/FLV.m>`__. Similar to the scale setting, if you want to diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index d1bad7da..a2e7cbea 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1144,8 +1144,8 @@ 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 activation dynamics, prm = (tau_act, tau_deact, smoothing_width). +MJAPI mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]); // Convert contact force to pyramid representation. MJAPI void mju_encodePyramid(mjtNum* pyramid, const mjtNum* force, const mjtNum* mu, int dim); diff --git a/introspect/functions.py b/introspect/functions.py index 77e641ca..86e4045f 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -7294,11 +7294,11 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ name='prm', type=ArrayType( inner_type=ValueType(name='mjtNum', is_const=True), - extents=(2,), + extents=(3,), ), ), ), - doc='Muscle activation dynamics, prm = (tau_act, tau_deact).', + doc='Muscle activation dynamics, prm = (tau_act, tau_deact, smoothing_width).', # pylint: disable=line-too-long )), ('mju_encodePyramid', FunctionDecl( diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 0d8d0f75..a8b9dfcd 100644 --- a/src/engine/engine_util_misc.c +++ b/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); } diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 392123ae..a2b24372 100644 --- a/src/engine/engine_util_misc.h +++ b/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]); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 4ddb3208..ed0b77f9 100644 --- a/src/xml/xml_native_reader.cc +++ b/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); diff --git a/test/engine/engine_util_misc_test.cc b/test/engine/engine_util_misc_test.cc index 357e4c2d..cc85335d 100644 --- a/test/engine/engine_util_misc_test.cc +++ b/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 diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index d8228ca0..3fe91435 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/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"( + + + + + + + + + + + + + )"; + std::array 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"( + + + + + + + + + + + + )"; + std::array 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