chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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
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