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_forward_test.cc
+104
View File
@@ -1513,5 +1513,109 @@ TEST_F(ActuatorTest, TendonActuatorForceRange) {
mj_deleteModel(model);
}
// ----------------------------- actuator delays -------------------------------
TEST_F(ForwardTest, ActuatorDelay) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1" mass="1"/>
</body>
</worldbody>
<actuator>
<motor joint="slide" delay="0.02" nsample="2"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// delay = 0.02 seconds, timestep = 0.01, so ndelay = ceil(0.02/0.01) = 2
EXPECT_EQ(model->actuator_history[0], 2);
// set ctrl to a nonzero value
data->ctrl[0] = 10.0;
// step once: the new ctrl is appended but won't be read for 2 timesteps
mj_step(model, data);
// actuator_force should still be 0 (delayed value from buffer init)
EXPECT_NEAR(data->actuator_force[0], 0.0, 1e-10);
// step again
mj_step(model, data);
// still reading old values
EXPECT_NEAR(data->actuator_force[0], 0.0, 1e-10);
// step a third time - now the delayed ctrl should arrive
mj_step(model, data);
// actuator_force should now be 10.0
EXPECT_NEAR(data->actuator_force[0], 10.0, 1e-10);
mj_deleteData(data);
mj_deleteModel(model);
}
// Test actuator delay with linear interpolation (interp=1)
// Uses delay = 1.5*timestep so interpolation is meaningful
TEST_F(ForwardTest, ActuatorDelayLinearInterp) {
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.015" nsample="3" interp="linear"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// delay = 0.015 seconds = 1.5*timestep, nsample=3, interp=1 (linear)
EXPECT_EQ(model->actuator_history[0], 3);
EXPECT_EQ(model->actuator_history[1], 1); // interp=1 (linear)
EXPECT_NEAR(model->actuator_delay[0], 0.015, 1e-10);
// Set increasing ctrl values
// Buffer has samples at times: -0.02, -0.01, 0 with values 0, 0, 0
// After step 0 at time=0.01: buffer has times -0.01, 0, 0.01 with values 0, 0, ctrl[0]
// Read at time 0.01 - 0.015 = -0.005: interpolate between t=-0.01 and t=0
// Since both values are 0, expected actuator_force = 0
data->ctrl[0] = 10.0;
mj_step(model, data);
EXPECT_NEAR(data->actuator_force[0], 0.0, 1e-10) << "step 0";
// After step 1 at time=0.02: buffer has times 0, 0.01, 0.02 with values 0, 10, 20
// Read at time 0.02 - 0.015 = 0.005: interpolate between t=0 (val=0) and t=0.01 (val=10)
// Expected: 0 * 0.5 + 10 * 0.5 = 5
data->ctrl[0] = 20.0;
mj_step(model, data);
EXPECT_NEAR(data->actuator_force[0], 5.0, 1e-10) << "step 1";
// After step 2 at time=0.03: buffer has times 0.01, 0.02, 0.03 with values 10, 20, 30
// Read at 0.03 - 0.015 = 0.015: interpolate between t=0.01 (val=10) and t=0.02 (val=20)
// Expected: 10 * 0.5 + 20 * 0.5 = 15
data->ctrl[0] = 30.0;
mj_step(model, data);
EXPECT_NEAR(data->actuator_force[0], 15.0, 1e-10) << "step 2";
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/engine_sensor_test.cc
+398
View File
@@ -24,6 +24,7 @@
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mujoco.h>
#include "src/engine/engine_support.h"
#include "src/engine/engine_util_blas.h"
#include "src/engine/engine_util_spatial.h"
#include "test/fixture.h"
@@ -1193,5 +1194,402 @@ TEST_F(SensorTest, RFCamera) {
mj_deleteModel(model);
}
// ------------------------------- sensor delays -------------------------------
TEST_F(SensorTest, SensorDelay) {
constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01" gravity="0 0 0"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.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);
// delay = 0.02 seconds, timestep = 0.01
// history = 3 (more than delay/timestep=2) to ensure buffer coverage
EXPECT_EQ(model->sensor_history[0], 3);
EXPECT_NEAR(model->sensor_delay[0], 0.02, 1e-10);
// Use different values to verify exact delay timing.
// With delay=0.02 and timestep=0.01, we expect 2-step delay:
// - At step N, sensordata should reflect qpos from step N-2.
// step 0: qpos=10, read from initial buffer
data->qpos[0] = 10.0;
mj_step(model, data);
EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10) << "step 0";
// step 1: qpos=20, still reading initial buffer
data->qpos[0] = 20.0;
mj_step(model, data);
EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10) << "step 1";
// step 2: qpos=30, read value from step 0 (delay=2 steps)
data->qpos[0] = 30.0;
mj_step(model, data);
EXPECT_NEAR(data->sensordata[0], 10.0, 1e-10) << "step 2";
// step 3: qpos=40, read value from step 1 (delay=2 steps)
data->qpos[0] = 40.0;
mj_step(model, data);
EXPECT_NEAR(data->sensordata[0], 20.0, 1e-10) << "step 3";
// step 4: qpos=50, read value from step 2 (delay=2 steps)
data->qpos[0] = 50.0;
mj_step(model, data);
EXPECT_NEAR(data->sensordata[0], 30.0, 1e-10) << "step 4";
mj_deleteData(data);
mj_deleteModel(model);
}
// Test sensor delay with linear interpolation (interp=1)
// Uses delay = 1.5*timestep so interpolation is meaningful
TEST_F(SensorTest, SensorDelayLinearInterp) {
constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01" gravity="0 0 0"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide" delay="0.015" nsample="3" interp="linear"/>
</sensor>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// delay = 0.015 seconds = 1.5*timestep, nsample=3, interp=1 (linear)
// With linear interpolation and 1.5*timestep delay, the read time falls
// exactly between two buffer samples, so we should get the average.
EXPECT_EQ(model->sensor_history[0], 3);
EXPECT_EQ(model->sensor_history[1], 1); // interp=1 (linear)
EXPECT_NEAR(model->sensor_delay[0], 0.015, 1e-10);
// Set increasing qpos values: step i -> qpos = (i+1)*10
// Buffer has samples at times: -0.02, -0.01, 0 (initialized)
// After step 0 at time=0.01: buffer has times -0.01, 0, 0.01 with values 0, 0, 10
// Read at time 0.01 - 0.015 = -0.005: interpolate between t=-0.01 (val=0) and t=0 (val=0)
// Expected: 0 * 0.5 + 0 * 0.5 = 0
data->qpos[0] = 10.0;
mj_step(model, data);
EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10) << "step 0";
// After step 1 at time=0.02: buffer has times 0, 0.01, 0.02 with values 0, 10, 20
// Read at time 0.02 - 0.015 = 0.005: interpolate between t=0 (val=0) and t=0.01 (val=10)
// Expected: 0 * 0.5 + 10 * 0.5 = 5
data->qpos[0] = 20.0;
mj_step(model, data);
EXPECT_NEAR(data->sensordata[0], 5.0, 1e-10) << "step 1";
// After step 2 at time=0.03: buffer has times 0.01, 0.02, 0.03 with values 10, 20, 30
// Read at 0.03 - 0.015 = 0.015: interpolate between t=0.01 (val=10) and t=0.02 (val=20)
// Expected: 10 * 0.5 + 20 * 0.5 = 15
data->qpos[0] = 30.0;
mj_step(model, data);
EXPECT_NEAR(data->sensordata[0], 15.0, 1e-10) << "step 2";
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SensorTest, SensorInterval) {
// This test uses the exact values from the documentation for interval:
// timestep=1, interval=2.5, producing times 0, 3, 5, 8, 10, 13, ...
// with interval="2.5 -1.5", producing times 1, 4, 6, 9, 11, 14, ...
constexpr char xml[] = R"(
<mujoco>
<option timestep="1" gravity="0 0 0"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos name="default_phase" joint="slide" interval="2.5 0" nsample="10"/>
<jointpos name="offset_phase" joint="slide" interval="2.5 -1.5" nsample="10"/>
</sensor>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
int sensor0 = mj_name2id(model, mjOBJ_SENSOR, "default_phase");
int sensor1 = mj_name2id(model, mjOBJ_SENSOR, "offset_phase");
int adr0 = model->sensor_adr[sensor0];
int adr1 = model->sensor_adr[sensor1];
// Verify initial buffer timestamps (after mj_makeData/mj_resetData)
// With period=2.5, dt=1.0, nsample=10:
// sensor0 (phase = -period = -2.5): continuous times are -2.5, -5, -7.5, ...
// rounded up to dt: -2, -5, -7, -10, -12, -15, -17, -20, -22, -25
// sensor1 (phase = -1.5): continuous times are -1.5, -4, -6.5, ...
// rounded up to dt: -1, -4, -6, -9, -11, -14, -16, -19, -21, -24
int n0 = model->sensor_history[2*sensor0];
int n1 = model->sensor_history[2*sensor1];
mjtNum* buf0 = data->history + model->sensor_historyadr[sensor0];
mjtNum* buf1 = data->history + model->sensor_historyadr[sensor1];
mjtNum* times0 = buf0 + 2;
mjtNum* times1 = buf1 + 2;
mjtNum expected_times0[] = {-25, -22, -20, -17, -15, -12, -10, -7, -5, -2};
mjtNum expected_times1[] = {-24, -21, -19, -16, -14, -11, -9, -6, -4, -1};
for (int i = 0; i < n0; i++) {
EXPECT_NEAR(times0[i], expected_times0[i], 1e-10);
}
for (int i = 0; i < n1; i++) {
EXPECT_NEAR(times1[i], expected_times1[i], 1e-10);
}
// sensor0: interval="2.5 0" -> time_prev starts at -2.5
// triggers at: 0, 3, 5, 8, 10, 13, ... (gaps: 3,2,3,2,3,...)
// sensor1: interval="2.5 -1.5" -> time_prev starts at -1.5
// triggers at: 1, 4, 6, 9, 11, 14, ... (gaps: 3,2,3,2,3,...)
// Arrays tracking when each sensor triggers (1=triggers, 0=holds)
// Times: 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14
int triggers0[] = {1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0};
int triggers1[] = {0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1};
mjtNum value0 = 0, value1 = 0;
for (int t = 0; t < 15; t++) {
// set position to current time (so we can track when sensor was computed)
data->qpos[0] = t;
mj_step(model, data);
// update expected values based on trigger pattern
if (triggers0[t]) value0 = t;
if (triggers1[t]) value1 = t;
EXPECT_NEAR(data->sensordata[adr0], value0, 1e-10)
<< "sensor0 at t=" << t;
EXPECT_NEAR(data->sensordata[adr1], value1, 1e-10)
<< "sensor1 at t=" << t;
}
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SensorTest, SensorDelayInterval) {
constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01" gravity="0 0 0"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide" delay="0.02" interval="0.03 0" nsample="5"/>
</sensor>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// Combined delay and interval
EXPECT_EQ(model->sensor_history[0], 5);
EXPECT_NEAR(model->sensor_delay[0], 0.02, 1e-10);
EXPECT_NEAR(model->sensor_interval[2*0], 0.03, 1e-10);
// Verify initial buffer timestamps (after mj_makeData/mj_resetData)
// With period=0.03, dt=0.01, nsample=5, phase=0 (means -period=-0.03):
// continuous times: -0.03, -0.06, -0.09, -0.12, -0.15
// rounded up to dt: -0.03, -0.06, -0.09, -0.12, -0.15 (multiples of dt)
int n = model->sensor_history[0];
mjtNum* buf = data->history + model->sensor_historyadr[0];
mjtNum* times = buf + 2;
mjtNum expected_times[] = {-0.15, -0.12, -0.09, -0.06, -0.03};
for (int i = 0; i < n; i++) {
EXPECT_NEAR(times[i], expected_times[i], 1e-10);
}
// set position
data->qpos[0] = 5.0;
// initial steps: reading from buffer (initially 0)
// With delay=0.02, interval=0.03:
// - At t=0, interval satisfied: compute 5.0, insert at t=0 (current time)
// - Reading happens at d->time - delay; at t=0.02, reads at t=0.00 (5.0)
for (int i = 0; i < 2; i++) {
mj_step(model, data);
// sensor reads delayed value (0.0 from initial buffer)
EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10) << "step " << i;
}
// step 3 (i=2): reading at t=0.00 now returns the inserted value 5.0
mj_step(model, data);
EXPECT_NEAR(data->sensordata[0], 5.0, 1e-10);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SensorTest, SensorHistoryOnly) {
constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide" nsample="5"/>
</sensor>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// history only, no delay or interval
EXPECT_EQ(model->sensor_history[0], 5);
EXPECT_NEAR(model->sensor_delay[0], 0.0, 1e-10);
EXPECT_NEAR(model->sensor_interval[0], 0.0, 1e-10);
// set position
data->qpos[0] = 3.0;
// without delay, sensordata reflects current value immediately
mj_step(model, data);
EXPECT_NEAR(data->sensordata[0], 3.0, 1e-10);
// change position, check again
data->qpos[0] = 7.0;
mj_step(model, data);
EXPECT_NEAR(data->sensordata[0], 7.0, 1e-10);
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SensorTest, SensorDelayMultiDim) {
constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01"/>
<worldbody>
<body>
<joint name="ball" type="ball"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<ballangvel joint="ball" delay="0.02" nsample="2"/>
</sensor>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// ballangvel is 3D
EXPECT_EQ(model->sensor_dim[0], 3);
EXPECT_EQ(model->sensor_history[0], 2);
// set angular velocity
data->qvel[0] = 1.0;
data->qvel[1] = 2.0;
data->qvel[2] = 3.0;
// step: reading delayed value (initially 0)
mj_step(model, data);
EXPECT_NEAR(data->sensordata[0], 0.0, 1e-10);
EXPECT_NEAR(data->sensordata[1], 0.0, 1e-10);
EXPECT_NEAR(data->sensordata[2], 0.0, 1e-10);
// after delay, values should propagate
mj_step(model, data);
mj_step(model, data);
mj_step(model, data);
// angular velocity is affected by dynamics, just check the buffer works
EXPECT_THAT(AsVector(data->sensordata, 3), Not(ElementsAre(0.0, 0.0, 0.0)));
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SensorTest, ReadSensor) {
constexpr char xml[] = R"(
<mujoco>
<option timestep="0.01" gravity="0 0 0"/>
<worldbody>
<body>
<joint name="slide" type="slide"/>
<geom size="0.1"/>
</body>
</worldbody>
<sensor>
<jointpos joint="slide" nsample="5"/>
</sensor>
</mujoco>
)";
char error[1024];
mjModel* model = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
mjData* data = mj_makeData(model);
// step with different qpos values to populate buffer
// mj_advance inserts at current time, then time advances
data->qpos[0] = 1.0;
mj_step(model, data); // inserts 1.0 at t=0, time -> 0.01
data->qpos[0] = 2.0;
mj_step(model, data); // inserts 2.0 at t=0.01, time -> 0.02
data->qpos[0] = 3.0;
mj_step(model, data); // inserts 3.0 at t=0.02, time -> 0.03
// now time=0.03, buffer has: [t=0: 1.0, t=0.01: 2.0, t=0.02: 3.0]
// read at different times from history
mjtNum result[1];
const mjtNum* ptr;
// read at t=0 -> returns 1.0
ptr = mj_readSensor(model, data, 0, 0.0, result, /*order=*/0);
EXPECT_NEAR(*ptr, 1.0, 1e-10);
// read at t=0.01 -> returns 2.0 (ZOH: exactly at insertion time)
ptr = mj_readSensor(model, data, 0, 0.01, result, /*order=*/0);
EXPECT_NEAR(*ptr, 2.0, 1e-10);
// read at t=0.02 -> returns 3.0
ptr = mj_readSensor(model, data, 0, 0.02, result, /*order=*/0);
EXPECT_NEAR(*ptr, 3.0, 1e-10);
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
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