From 6d01627c0b9dab6fb1595608b745c2c1cc17ea60 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 5 Jul 2022 01:38:01 -0700 Subject: [PATCH] No longer modify control vector in-place, add `mju_clip`. - Never modify `d->ctrl`: - `ctrl` values which are outside their respective `ctrlrange` are clamped internally, but `d->ctrl` itself is unmodified. - Similarly, if `mjWARN_BADCTRL` is raised (NaNs or huge values), `ctrl` is cleared internally, but `d->ctrl` itself is unmodified. - `mjWARN_BADCTRL` is no longer raised for `d->ctrl` values that are larger in absolute value than `mjMAXVAL`, if clamping makes them smaller than that. - Added `mju_clip()` utility function. - Added missing warning string for `case mjWARN_BADCTRL`. PiperOrigin-RevId: 459025950 Change-Id: Ifbf974f9c02c3d70afdaa3114018fc81dfd746d3 --- include/mujoco/mujoco.h | 3 ++ introspect/functions.py | 20 ++++++++ src/engine/engine_forward.c | 74 +++++++++++++++--------------- src/engine/engine_util_misc.c | 24 ++++++++-- src/engine/engine_util_misc.h | 3 ++ src/engine/engine_vis_visualize.c | 4 +- test/engine/engine_forward_test.cc | 73 +++++++++++++++++++++++++++++ 7 files changed, 158 insertions(+), 43 deletions(-) diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 23c39642..226f4be2 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1028,6 +1028,9 @@ MJAPI mjtNum mju_min(mjtNum a, mjtNum b); // Return max(a,b) with single evaluation of a and b. MJAPI mjtNum mju_max(mjtNum a, mjtNum b); +// Clip x to the range [min, max]. +MJAPI mjtNum mju_clip(mjtNum x, mjtNum min, mjtNum max); + // Return sign of x: +1, -1 or 0. MJAPI mjtNum mju_sign(mjtNum x); diff --git a/introspect/functions.py b/introspect/functions.py index 3291ddf6..b78d68f3 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -6573,6 +6573,26 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Return max(a,b) with single evaluation of a and b.', )), + ('mju_clip', + FunctionDecl( + name='mju_clip', + return_type=ValueType(name='mjtNum'), + parameters=( + FunctionParameterDecl( + name='x', + type=ValueType(name='mjtNum'), + ), + FunctionParameterDecl( + name='min', + type=ValueType(name='mjtNum'), + ), + FunctionParameterDecl( + name='max', + type=ValueType(name='mjtNum'), + ), + ), + doc='Clip x to the range [min, max].', + )), ('mju_sign', FunctionDecl( name='mju_sign', diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 1bfbaf2a..2d606ef7 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -157,37 +157,43 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { mjtNum gain, bias, tau; mjtNum *prm, *moment = d->actuator_moment, *force = d->actuator_force; - // clear results + // clear outputs mju_zero(d->qfrc_actuator, nv); - if (nu) { - mju_zero(d->actuator_force, nu); - } - - // check controls, set to 0 if any are bad - for (int i=0; ictrl[i])) { - mj_warning(d, mjWARN_BADCTRL, i); - mju_zero(d->ctrl, nu); - break; - } - } + mju_zero(d->actuator_force, nu); // disabled or no actuation: return if (nu==0 || mjDISABLED(mjDSBL_ACTUATION)) { return; } - // force = gain .* [ctrl/act] + bias - for (int i=0; iactuator_ctrllimited[i] && !mjDISABLED(mjDSBL_CLAMPCTRL)) { - if (d->ctrl[i] < m->actuator_ctrlrange[2*i]) { - d->ctrl[i] = m->actuator_ctrlrange[2*i]; - } else if (d->ctrl[i] > m->actuator_ctrlrange[2*i+1]) { - d->ctrl[i] = m->actuator_ctrlrange[2*i+1]; + // local, clamped copy of ctrl + mjMARKSTACK; + mjtNum *ctrl = mj_stackAlloc(d, nu); + if (mjDISABLED(mjDSBL_CLAMPCTRL)) { + mju_copy(ctrl, d->ctrl, nu); + } else { + for (int i=0; iactuator_ctrllimited[i]) { + mjtNum *ctrlrange = m->actuator_ctrlrange + 2*i; + ctrl[i] = mju_clip(d->ctrl[i], ctrlrange[0], ctrlrange[1]); + } else { + ctrl[i] = d->ctrl[i]; } } + } + // check controls, set all to 0 if any are bad + for (int i=0; iactuator_gainprm + mjNGAIN*i; @@ -219,7 +225,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // set force = gain .* [ctrl/act] if (m->actuator_dyntype[i]==mjDYN_NONE) { - force[i] = gain * d->ctrl[i]; + force[i] = gain * ctrl[i]; } else { force[i] = gain * d->act[i-(nu-na)]; } @@ -259,11 +265,8 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // clamp actuator_force for (int i=0; iactuator_forcelimited[i]) { - if (force[i]actuator_forcerange[2*i]) { - force[i] = m->actuator_forcerange[2*i]; - } else if (force[i]>m->actuator_forcerange[2*i+1]) { - force[i] = m->actuator_forcerange[2*i+1]; - } + mjtNum *forcerange = m->actuator_forcerange + 2*i; + force[i] = mju_clip(force[i], forcerange[0], forcerange[1]); } } @@ -279,16 +282,16 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // compute act_dot according to dynamics type switch (m->actuator_dyntype[i]) { case mjDYN_INTEGRATOR: // simple integrator - d->act_dot[j] = d->ctrl[i]; + d->act_dot[j] = ctrl[i]; break; case mjDYN_FILTER: // linear filter: prm = tau tau = mju_max(mjMINVAL, prm[0]); - d->act_dot[j] = (d->ctrl[i] - d->act[j]) / tau; + d->act_dot[j] = (ctrl[i] - d->act[j]) / tau; break; case mjDYN_MUSCLE: // muscle model: prm = (tau_act, tau_deact) - d->act_dot[j] = mju_muscleDynamics(d->ctrl[i], d->act[j], prm); + d->act_dot[j] = mju_muscleDynamics(ctrl[i], d->act[j], prm); break; default: // user dynamics @@ -299,7 +302,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } } } - + mjFREESTACK; TM_END(mjTIMER_ACTUATION); } @@ -464,13 +467,8 @@ static void mj_advance(const mjModel* m, mjData* d, for (int i=0; ina; i++) { int iu = i + m->nu - m->na; if (m->actuator_actlimited[iu]) { - mjtNum min = m->actuator_actrange[2*iu]; - mjtNum max = m->actuator_actrange[2*iu+1]; - if (d->act[i]act[i] = min; - } else if (d->act[i]>max) { - d->act[i] = max; - } + mjtNum* actrange = m->actuator_actrange + 2*iu; + d->act[i] = mju_clip(d->act[i], actrange[0], actrange[1]); } } } diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index d1c41512..e67fb758 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -524,10 +524,10 @@ 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]) { // clamp control - mjtNum ctrlclamp = mjMIN(1, mjMAX(0, ctrl)); + mjtNum ctrlclamp = mju_clip(ctrl, 0, 1); // clamp activation - mjtNum actclamp = mjMIN(1, mjMAX(0, act)); + 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; @@ -688,6 +688,19 @@ mjtNum mju_max(mjtNum a, mjtNum b) { +// clip x to the range [min, max] +mjtNum mju_clip(mjtNum x, mjtNum min, mjtNum max) { + if (xmax) { + return max; + } else { + return x; + } +} + + + // sign function mjtNum mju_sign(mjtNum x) { if (x<0) { @@ -928,8 +941,13 @@ const char* mju_warningText(int warning, int info) { mjSNPRINTF(str, "Nan, Inf or huge value in QACC at DOF %d. The simulation is unstable.", info); break; + case mjWARN_BADCTRL: + mjSNPRINTF(str, "Nan, Inf or huge value in CTRL at ACTUATOR %d. The simulation is unstable.", + info); + break; + default: - mjSTRNCPY(str, "Unknown warning type"); + mjSNPRINTF(str, "Unknown warning type %d.", warning); } return str; diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 46b3ad5f..b498d0ba 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -69,6 +69,9 @@ MJAPI mjtNum mju_min(mjtNum a, mjtNum b); // max function, single evaluation of a and b MJAPI mjtNum mju_max(mjtNum a, mjtNum b); +// clip x to the range [min, max] +MJAPI mjtNum mju_clip(mjtNum x, mjtNum min, mjtNum max); + // sign function MJAPI mjtNum mju_sign(mjtNum x); diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 82cb441c..d05eb805 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -806,9 +806,9 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, // clamp act to extended range if (vopt->flags[mjVIS_ACTIVATION] && m->actuator_dyntype[i]) { - act = mjMIN(rng[2], mjMAX(rng[0], d->act[i-(m->nu-m->na)])); + act = mju_clip(d->act[i-(m->nu-m->na)], rng[0], rng[2]); } else { - act = mjMIN(rng[2], mjMAX(rng[0], d->ctrl[i])); + act = mju_clip(d->ctrl[i], rng[0], rng[2]); } // compute interpolants diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index b2d7cca1..0ad9317b 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -19,7 +19,9 @@ #include #include #include +#include #include +#include "src/cc/array_safety.h" #include "src/engine/engine_io.h" #include "test/fixture.h" @@ -38,6 +40,7 @@ static const char* const kDampedActuatorsPath = using ::testing::Pointwise; using ::testing::DoubleNear; using ::testing::Ne; +using ::testing::HasSubstr; // --------------------------- activation limits ------------------------------- @@ -271,5 +274,75 @@ TEST_F(ImplicitIntegratorTest, EnergyConservation) { mj_deleteModel(model); } +// --------------------------- control clamping -------------------------------- + +TEST_F(ForwardTest, ControlClamping) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + mjData* data = mj_makeData(model); + + // for the unclamped actuator, ctrl={1, 2} produce different accelerations + data->ctrl[0] = 1; + mj_forward(model, data); + mjtNum qacc1 = data->qacc[0]; + data->ctrl[0] = 2; + mj_forward(model, data); + mjtNum qacc2 = data->qacc[0]; + EXPECT_NE(qacc1, qacc2); + + // for the clamped actuator, ctrl={1, 2} produce identical accelerations + data->ctrl[1] = 1; + mj_forward(model, data); + qacc1 = data->qacc[0]; + data->ctrl[1] = 2; + mj_forward(model, data); + qacc2 = data->qacc[0]; + EXPECT_EQ(qacc1, qacc2); + + // data->ctrl[1] remains pristine + EXPECT_EQ(data->ctrl[1], 2); + + // install warning handler + static char warning[1024]; + warning[0] = '\0'; + mju_user_warning = [](const char* msg) { + util::strcpy_arr(warning, msg); + }; + + // for the unclamped actuator, huge raises warning + data->ctrl[0] = 10*mjMAXVAL; + mj_forward(model, data); + EXPECT_THAT(warning, HasSubstr("Nan, Inf or huge value in CTRL at ACTUATOR 0")); + + // for the clamped actuator, huge does not raise warning + mj_resetData(model, data); + warning[0] = '\0'; + data->ctrl[1] = 10*mjMAXVAL; + mj_forward(model, data); + EXPECT_EQ(warning[0], '\0'); + + // for the clamped actuator, NaN raises warning + mj_resetData(model, data); + data->ctrl[1] = std::numeric_limits::quiet_NaN(); + mj_forward(model, data); + EXPECT_THAT(warning, HasSubstr("Nan, Inf or huge value in CTRL at ACTUATOR 1")); + + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco