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

Add actuator and sensor delays. Fixes #1004

Browse Source 
PiperOrigin-RevId: 866478839
Change-Id: Id21a6da0f98454c8fa39ea5af8a5e213d6eae497
This commit is contained in:
Yuval Tassa
2026-02-06 08:46:45 -08:00
committed by Copybara-Service
parent 84fa527723
commit 6419534bad
48 changed files with 6282 additions and 219 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_support_test.cc
+269 -4
View File
@@ -692,7 +692,9 @@ TEST_F(SupportTest, DifferentiatePosSubQuat) {
static const char* const kDefaultModel = "testdata/model.xml";
TEST_F(SupportTest, GetSetStateStepEqual) {
using StateTest = MujocoTest;
TEST_F(StateTest, GetSetStateStepEqual) {
const std::string xml_path = GetTestDataFilePath(kDefaultModel);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
@@ -745,7 +747,58 @@ TEST_F(SupportTest, GetSetStateStepEqual) {
mj_deleteModel(model);
}
TEST_F(SupportTest, CopyState) {
TEST_F(StateTest, GetSetStateDelay) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide" delay="0.05" nsample="5"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// verify history buffer exists: nhistory = 2 + 2*5 = 12
EXPECT_EQ(model->nhistory, 12); // [user, cursor, times(5), values(5)]
// state size should include history buffer
int size = mj_stateSize(model, mjSTATE_HISTORY);
EXPECT_EQ(size, model->nhistory);
// step to populate history buffer
data->ctrl[0] = 1.0;
mj_step(model, data);
data->ctrl[0] = 2.0;
mj_step(model, data);
// get history state
vector<mjtNum> history_state(size);
mj_getState(model, data, history_state.data(), mjSTATE_HISTORY);
// modify the history buffer manually (value at index 7 = 2+5 = after times)
data->history[7] = 99.0; // first value
// set history state back - should restore original
mj_setState(model, data, history_state.data(), mjSTATE_HISTORY);
// verify restoration
EXPECT_NE(data->history[7], 99.0);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(StateTest, CopyState) {
const std::string xml_path = GetTestDataFilePath(kDefaultModel);
mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
@@ -762,6 +815,7 @@ TEST_F(SupportTest, CopyState) {
for (int i=0; i < m->nv; ++i) src->qvel[i] = i*0.2;
for (int i=0; i < m->na; ++i) src->act[i] = i*0.3;
for (int i=0; i < m->nu; ++i) src->ctrl[i] = i*0.4;
for (int i=0; i < m->nhistory; ++i) src->history[i] = i*0.5;
for (int i=0; i < m->neq; ++i) src->eq_active[i] = 1 - m->eq_active0[i];
@@ -779,6 +833,8 @@ TEST_F(SupportTest, CopyState) {
EXPECT_EQ(AsVector(dst->qpos, m->nq), AsVector(src->qpos, m->nq));
EXPECT_EQ(AsVector(dst->qvel, m->nv), AsVector(src->qvel, m->nv));
EXPECT_EQ(AsVector(dst->act, m->na), AsVector(src->act, m->na));
EXPECT_EQ(AsVector(dst->history, m->nhistory),
AsVector(src->history, m->nhistory));
EXPECT_EQ(AsVector(dst->eq_active, m->neq), AsVector(src->eq_active, m->neq));
// check non-copied components (CTRL not in signature)
@@ -790,7 +846,7 @@ TEST_F(SupportTest, CopyState) {
mj_deleteModel(m);
}
TEST_F(SupportTest, ExtractState) {
TEST_F(StateTest, ExtractState) {
const std::string xml_path = GetTestDataFilePath(kDefaultModel);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
@@ -809,7 +865,8 @@ TEST_F(SupportTest, ExtractState) {
mj_step(model, data);
// take a state that will be used as src
int srcsig = mjSTATE_TIME | mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_CTRL;
int srcsig = mjSTATE_TIME | mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_CTRL |
mjSTATE_HISTORY;
int srcsize = mj_stateSize(model, srcsig);
vector<mjtNum> srcstate(srcsize);
mj_getState(model, data, srcstate.data(), srcsig);
@@ -835,6 +892,14 @@ TEST_F(SupportTest, ExtractState) {
EXPECT_EQ(AsVector(dststate2.data() + model->nq, model->nu),
AsVector(data->ctrl, model->nu));
// extract history state
int dstsig3 = mjSTATE_HISTORY;
int dstsize3 = mj_stateSize(model, dstsig3);
EXPECT_EQ(dstsize3, model->nhistory);
vector<mjtNum> dststate3(dstsize3);
mj_extractState(model, srcstate.data(), srcsig, dststate3.data(), dstsig3);
EXPECT_EQ(dststate3, AsVector(data->history, model->nhistory));
// test that an error is correctly raised if dstsig is not a subset of srcsig
static int error_count;
static char last_error_msg[128];
@@ -1202,5 +1267,205 @@ TEST_F(SupportTest, ContactSensorDim) {
EXPECT_EQ(mju_condataSize(dataSpec), 1+3+1+3+3);
}
// ------------------------------ ctrl delays --------------------------------
TEST_F(SupportTest, ReadCtrlNoDelay) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide"/>
</actuator>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
ASSERT_THAT(model, NotNull());
mjData* data = mj_makeData(model);
// no delay: should return current ctrl value
data->ctrl[0] = 42.0;
EXPECT_EQ(mj_readCtrl(model, data, 0, data->time, /*order=*/0), 42.0);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SupportTest, ReadCtrlWithDelay) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide" delay="0.03" nsample="3"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// model should have delay configured
// delay = 0.03 seconds, timestep = 0.01, so ndelay = ceil(0.03/0.01) = 3
EXPECT_EQ(model->actuator_history[0], 3);
EXPECT_NEAR(model->actuator_delay[0], 0.03, 1e-10);
EXPECT_GE(model->actuator_historyadr[0], 0);
// initially, buffer should be filled with constant value (from init)
// reading at current time should return the init value
mjtNum val = mj_readCtrl(model, data, 0, data->time, /*order=*/0);
EXPECT_EQ(val, data->ctrl[0]);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SupportTest, InitCtrlDelay) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide" delay="0.02" nsample="3"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// verify nhistory
EXPECT_EQ(model->actuator_history[0], 3);
// initialize with custom times and values
// buffer stores: time 0.0 -> value 1.0, time 0.01 -> value 2.0, time 0.02 -> value 3.0
mjtNum times[3] = {0.0, 0.01, 0.02};
mjtNum values[3] = {1.0, 2.0, 3.0};
mj_initCtrlHistory(model, data, 0, times, values);
// mj_readCtrl now auto-subtracts delay: lookup_time = time - delay
// delay = 0.02, so:
// time=0.04 -> lookup at 0.02 -> value 3.0
// time=0.03 -> lookup at 0.01 -> value 2.0
// time=0.02 -> lookup at 0.00 -> value 1.0
mjtNum val = mj_readCtrl(model, data, 0, 0.04, /*order=*/0);
EXPECT_EQ(val, 3.0);
val = mj_readCtrl(model, data, 0, 0.03, /*order=*/0);
EXPECT_EQ(val, 2.0);
val = mj_readCtrl(model, data, 0, 0.02, /*order=*/0);
EXPECT_EQ(val, 1.0);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SupportTest, InitCtrlDelayNullTimes) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide" delay="0.02" nsample="3"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// get existing times from buffer
int adr = model->actuator_historyadr[0];
mjtNum* buf = data->history + adr;
mjtNum existing_times[3] = {buf[2], buf[3], buf[4]};
// initialize with NULL times (use existing) and new values
mjtNum values[3] = {10.0, 20.0, 30.0};
mj_initCtrlHistory(model, data, 0, nullptr, values);
// verify times are unchanged
EXPECT_EQ(buf[2], existing_times[0]);
EXPECT_EQ(buf[3], existing_times[1]);
EXPECT_EQ(buf[4], existing_times[2]);
// verify values are updated
EXPECT_EQ(buf[5], 10.0);
EXPECT_EQ(buf[6], 20.0);
EXPECT_EQ(buf[7], 30.0);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SupportTest, InitSensorDelay) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide" delay="0.02" nsample="3"/>
</sensor>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// verify nsample for sensor
EXPECT_EQ(model->sensor_history[0], 3);
// initialize with custom times and values, phase=0
// buffer stores: time 0.0 -> value 0.5, time 0.01 -> value 0.6, time 0.02 -> value 0.7
mjtNum times[3] = {0.0, 0.01, 0.02};
mjtNum values[3] = {0.5, 0.6, 0.7};
mj_initSensorHistory(model, data, 0, times, values, /*phase=*/0.0);
// mj_readSensor now auto-subtracts delay: lookup_time = time - delay
// delay = 0.02, so:
// time=0.04 -> lookup at 0.02 -> value 0.7
// time=0.03 -> lookup at 0.01 -> value 0.6
mjtNum result = 0;
const mjtNum* ptr = mj_readSensor(model, data, 0, 0.04, &result, /*order=*/0);
mjtNum val = ptr ? *ptr : result;
EXPECT_EQ(val, 0.7);
ptr = mj_readSensor(model, data, 0, 0.03, &result, /*order=*/0);
val = ptr ? *ptr : result;
EXPECT_EQ(val, 0.6);
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco