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

No longer modify control vector in-place, add mju_clip.

Browse Source 
- 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
This commit is contained in:
Yuval Tassa
2022-07-05 01:38:01 -07:00
committed by Copybara-Service
parent 09a5efc09e
commit 6d01627c0b
7 changed files with 158 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/mujoco/mujoco.h
+3
View File
@@ -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);
introspect/functions.py
+20
View File
@@ -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',
src/engine/engine_forward.c
+36 -38
View File
@@ -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; i<nu; i++) {
if (mju_isBad(d->ctrl[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; i<nu; i++) {
// clamp ctrl
if (m->actuator_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; i<nu; i++) {
// clamp ctrl
if (m->actuator_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; i<nu; i++) {
if (mju_isBad(ctrl[i])) {
mj_warning(d, mjWARN_BADCTRL, i);
mju_zero(ctrl, nu);
break;
}
}
// force = gain .* [ctrl/act] + bias
for (int i=0; i<nu; i++) {
// extract gain info
prm = m->actuator_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; i<nu; i++) {
if (m->actuator_forcelimited[i]) {
if (force[i]<m->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; i<m->na; 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]<min) {
d->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]);
}
}
}
src/engine/engine_util_misc.c
+21 -3
View File
@@ -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 (x<min) {
return min;
} else if (x>max) {
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;
src/engine/engine_util_misc.h
+3
View File
@@ -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);
src/engine/engine_vis_visualize.c
+2 -2
View File
@@ -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
test/engine/engine_forward_test.cc
+73
View File
@@ -19,7 +19,9 @@
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#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"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="slide" type="slide" axis="1 0 0"/>
</body>
</worldbody>
<actuator>
<motor name="unclamped" joint="slide"/>
<motor name="clamped" joint="slide" ctrllimited="true" ctrlrange="-1 1"/>
</actuator>
</mujoco>
)";
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<double>::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