chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

Added optional smoothing to muscle dynamics:

Browse Source 
- `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
This commit is contained in:
Yuval Tassa
2023-05-12 04:58:17 -07:00
committed by Copybara-Service
parent 777cad52b4
commit 770b4b363a
13 changed files with 180 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/functions.rst
+1 -1
View File
@@ -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:
doc/XMLreference.rst
+7 -1
View File
@@ -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"`
doc/XMLschema.rst
+4 -4
View File
@@ -1105,13 +1105,13 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`jointinparent<actuator-muscle-jointinparent>` | :ref:`tendon<actuator-muscle-tendon>` | :ref:`slidersite<actuator-muscle-slidersite>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`cranksite<actuator-muscle-cranksite>` | :ref:`timeconst<actuator-muscle-timeconst>` | :ref:`range<actuator-muscle-range>` | |
| | | | :ref:`cranksite<actuator-muscle-cranksite>` | :ref:`timeconst<actuator-muscle-timeconst>` | :ref:`tausmooth<actuator-muscle-tausmooth>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`force<actuator-muscle-force>` | :ref:`scale<actuator-muscle-scale>` | :ref:`lmin<actuator-muscle-lmin>` | |
| | | | :ref:`range<actuator-muscle-range>` | :ref:`force<actuator-muscle-force>` | :ref:`scale<actuator-muscle-scale>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`lmax<actuator-muscle-lmax>` | :ref:`vmax<actuator-muscle-vmax>` | :ref:`fpmax<actuator-muscle-fpmax>` | |
| | | | :ref:`lmin<actuator-muscle-lmin>` | :ref:`lmax<actuator-muscle-lmax>` | :ref:`vmax<actuator-muscle-vmax>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`fvmax<actuator-muscle-fvmax>` | | | |
| | | | :ref:`fpmax<actuator-muscle-fpmax>` | :ref:`fvmax<actuator-muscle-fvmax>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| actuator |br| |_| |L| | | .. table:: |
doc/changelog.rst
+5
View File
@@ -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<actuator-muscle-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) <https://doi.org/10.1115/1.4023390>`__ muscle
model, within a range of width tausmooth. See :ref:`Muscle actuators<CMuscle>` for more details.
Relatedly, :ref:`mju_muscleDynamics` now takes 3 parameters instead of 2, adding the new smoothing-width parameter.
.. youtube:: ZppeDArq6AU
doc/includes/references.h
+1 -1
View File
@@ -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);
doc/modeling.rst
+15 -7
View File
@@ -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
<https://doi.org/10.1115/1.4023390>`__, 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<actuator-muscle-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 <actuator-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 <https://en.wikipedia.org/wiki/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
include/mujoco/mujoco.h
+2 -2
View File
@@ -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);
introspect/functions.py
+2 -2
View File
@@ -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(
src/engine/engine_util_misc.c
+30 -10
View File
@@ -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);
}
src/engine/engine_util_misc.h
+6 -2
View File
@@ -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]);
src/xml/xml_native_reader.cc
+6 -3
View File
@@ -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);
test/engine/engine_util_misc_test.cc
+58
View File
@@ -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
+43 -1
View File
@@ -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