chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 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
python/mujoco/bindings_test.py
+165 -1
View File
@@ -1369,7 +1369,9 @@ Euler integrator, semi-implicit in velocity.
for i in range(0, 3):
self.assertEqual(
dist[i],
mujoco.mj_ray(self.model, self.data, pnt, vec[i], None, 1, -1, geom1, None),
mujoco.mj_ray(
self.model, self.data, pnt, vec[i], None, 1, -1, geom1, None
),
)
self.assertEqual(geomid[i], geom1)
self.assertEqual(geomid[i], geom_ex[i])
@@ -1717,6 +1719,168 @@ Euler integrator, semi-implicit in velocity.
self.assertIn(model_path, dependencies)
self.assertIn(msh_path, dependencies)
def test_mj_read_ctrl_and_init_ctrl_delay(self):
xml = r"""
<mujoco>
<worldbody>
<body>
<geom type="sphere" size="0.1"/>
<joint name="hinge" type="hinge"/>
</body>
</worldbody>
<actuator>
<position name="actuator" joint="hinge" delay="0.01" nsample="4"/>
</actuator>
</mujoco>
"""
model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)
mujoco.mj_forward(model, data)
# Initialize the delay buffer with known values
# actuator_history[i, 0] = nsample, actuator_history[i, 1] = interp
nhistory = model.actuator_history[0, 0]
self.assertEqual(nhistory, 4)
times = np.array([0.0, 0.01, 0.02, 0.03])
values = np.array([1.0, 2.0, 3.0, 4.0])
mujoco.mj_initCtrlHistory(model, data, 0, times, values)
# Read back a value using zero-order hold
# mj_readCtrl auto-subtracts delay: lookup_time = read_time - delay
# delay = 0.01, so:
# read_time=0.02 -> lookup at 0.01 -> value 2.0
# read_time=0.03 -> lookup at 0.02 -> value 3.0
result = mujoco.mj_readCtrl(model, data, 0, 0.02, interp=0)
self.assertEqual(result, 2.0) # ZOH returns value at t=0.01
# Test with times=None (uses existing timestamps)
new_values = np.array([5.0, 6.0, 7.0, 8.0])
mujoco.mj_initCtrlHistory(model, data, 0, None, new_values)
# read_time=0.02 -> lookup at 0.01 -> value 6.0
result = mujoco.mj_readCtrl(model, data, 0, 0.02, interp=0)
self.assertEqual(result, 6.0)
# Test dimension validation errors
with self.assertRaises(TypeError):
# wrong times
mujoco.mj_initCtrlHistory(model, data, 0, np.zeros(3), values)
with self.assertRaises(TypeError):
# wrong values
mujoco.mj_initCtrlHistory(model, data, 0, times, np.zeros(5))
def test_mj_read_sensor_and_init_sensor_delay(self):
xml = r"""
<mujoco>
<worldbody>
<body>
<geom type="sphere" size="0.1"/>
<joint name="hinge" type="hinge"/>
<site name="site"/>
</body>
</worldbody>
<sensor>
<accelerometer name="accel" site="site" delay="0.01" nsample="3"/>
</sensor>
</mujoco>
"""
model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)
mujoco.mj_forward(model, data)
# Initialize the delay buffer with known values
# sensor_history[i, 0] = nsample, sensor_history[i, 1] = interp
nhistory = model.sensor_history[0, 0]
dim = model.sensor_dim[0]
self.assertEqual(nhistory, 3)
self.assertEqual(dim, 3) # accelerometer has dim=3
times = np.array([0.0, 0.01, 0.02])
values = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]], dtype=np.float64)
mujoco.mj_initSensorHistory(model, data, 0, times, values, phase=0.0)
# Read back a value using zero-order hold
# mj_readSensor auto-subtracts delay: lookup_time = read_time - delay
# delay = 0.01, so:
# read_time=0.02 -> lookup at 0.01 -> value [4, 5, 6]
result = np.zeros(dim)
mujoco.mj_readSensor(model, data, 0, 0.02, result, interp=0)
# ZOH returns value at t=0.01
np.testing.assert_array_equal(result, [4, 5, 6])
# Test with times=None (uses existing timestamps)
new_values = np.array([
[10, 11, 12], [13, 14, 15], [16, 17, 18]], dtype=np.float64)
mujoco.mj_initSensorHistory(model, data, 0, None, new_values, phase=0.0)
# read_time=0.02 -> lookup at 0.01 -> value [13, 14, 15]
mujoco.mj_readSensor(model, data, 0, 0.02, result, interp=0)
np.testing.assert_array_equal(result, [13, 14, 15])
# Test dimension validation errors
with self.assertRaises(TypeError):
# wrong result size
mujoco.mj_readSensor(model, data, 0, 0.02, np.zeros(2), interp=0)
with self.assertRaises(TypeError):
# wrong times size
mujoco.mj_initSensorHistory(model, data, 0, np.zeros(2), values, 0.0)
with self.assertRaises(TypeError):
# wrong values rows
mujoco.mj_initSensorHistory(model, data, 0, times, np.zeros((4, 3)), 0.0)
with self.assertRaises(TypeError):
# wrong values cols
mujoco.mj_initSensorHistory(model, data, 0, times, np.zeros((3, 2)), 0.0)
def test_init_sensor_history_pedagogical(self):
# A framequat sensor reports body orientation as a unit quaternion.
# Quaternions are never zero: the identity quaternion is [1, 0, 0, 0].
# This test demonstrates why mj_initSensorHistory is needed: after
# mj_makeData, the history buffer is filled with zeros, which is invalid.
xml = r"""
<mujoco>
<worldbody>
<body name="body">
<freejoint/>
<geom type="sphere" size="0.1"/>
</body>
</worldbody>
<sensor>
<framequat name="quat" objtype="body" objname="body" delay="0.01" nsample="5"/>
</sensor>
</mujoco>
"""
model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)
dim = model.sensor_dim[0]
nsample = model.sensor_history[0, 0]
delay = model.sensor_delay[0]
self.assertEqual(dim, 4)
self.assertEqual(nsample, 5)
self.assertEqual(delay, 0.01)
# After mj_makeData, reading from the delay buffer gives all zeros.
# For a quaternion sensor, this is invalid data.
result = np.zeros(dim)
mujoco.mj_readSensor(model, data, 0, delay, result, interp=0)
np.testing.assert_array_equal(result, [0, 0, 0, 0])
# To get valid sensor values, we temporarily set delay to 0 so that
# mj_forward populates sensordata directly (without using the delay
# buffer), then restore the original delay value.
saved_delay = model.sensor_delay.copy()
model.sensor_delay[:] = 0
mujoco.mj_forward(model, data)
model.sensor_delay[:] = saved_delay
# Now sensordata contains the valid identity quaternion.
np.testing.assert_array_equal(data.sensordata, [1, 0, 0, 0])
# Use mj_initSensorHistory to fill the buffer with valid quaternion values.
# Passing None for times keeps the existing timestamps in the buffer.
values = np.tile(data.sensordata, (nsample, 1))
mujoco.mj_initSensorHistory(model, data, 0, None, values, phase=0.0)
# Now reading from the delay buffer gives the valid identity quaternion.
mujoco.mj_readSensor(model, data, 0, delay, result, interp=0)
np.testing.assert_array_equal(result, [1, 0, 0, 0])
def _assert_attributes_equal(self, actual_obj, expected_obj, attr_to_compare):
for name in attr_to_compare:
actual_value = getattr(actual_obj, name)
python/mujoco/functions.cc
+55 -1
View File
@@ -91,7 +91,7 @@ PYBIND11_MODULE(_functions, pymodule) {
DEF_WITH_OMITTED_PY_ARGS(traits::mj_printSchema,
"filename", "buffer", "buffer_sz")(
pymodule, [](bool flg_html, bool flg_pad) {
constexpr int kBufferSize = 40000;
constexpr int kBufferSize = 60000;
auto buffer = std::unique_ptr<char[]>(new char[kBufferSize]);
const int out_length = InterceptMjErrors(::mj_printSchema)(
nullptr, buffer.get(), kBufferSize, flg_html, flg_pad);
@@ -357,6 +357,60 @@ PYBIND11_MODULE(_functions, pymodule) {
return InterceptMjErrors(::mj_setState)(m, d, state.data(), sig);
});
Def<traits::mj_copyState>(pymodule);
Def<traits::mj_readCtrl>(pymodule);
Def<traits::mj_readSensor>(
pymodule,
[](const raw::MjModel* m, const raw::MjData* d, int id, mjtNum time,
Eigen::Ref<EigenVectorX> result, int order) {
int dim = m->sensor_dim[id];
if (result.size() != dim) {
throw py::type_error("result should have length sensor_dim[id]");
}
const mjtNum* ptr = InterceptMjErrors(::mj_readSensor)(
m, d, id, time, result.data(), order);
if (ptr && ptr != result.data()) {
for (int i = 0; i < dim; ++i) {
result[i] = ptr[i];
}
}
return result;
});
Def<traits::mj_initCtrlHistory>(
pymodule,
[](const raw::MjModel* m, raw::MjData* d, int id,
std::optional<Eigen::Ref<const EigenVectorX>> times,
Eigen::Ref<const EigenVectorX> values) {
int nhistory = m->actuator_history[2*id];
if (times.has_value() && times->size() != nhistory) {
throw py::type_error(
"times should have length actuator_history[2*id]");
}
if (values.size() != nhistory) {
throw py::type_error(
"values should have length actuator_history[2*id]");
}
return InterceptMjErrors(::mj_initCtrlHistory)(
m, d, id,
times.has_value() ? times->data() : nullptr, values.data());
});
Def<traits::mj_initSensorHistory>(
pymodule, [](const raw::MjModel* m, raw::MjData* d, int id,
std::optional<Eigen::Ref<const EigenVectorX>> times,
Eigen::Ref<const EigenArrayXX> values, mjtNum phase) {
int nhistory = m->sensor_history[2 * id];
int dim = m->sensor_dim[id];
if (times.has_value() && times->size() != nhistory) {
throw py::type_error("times should have length sensor_history[2*id]");
}
if (values.rows() != nhistory || values.cols() != dim) {
throw py::type_error(
"values should have shape (sensor_history[2*id], "
"sensor_dim[id])");
}
return InterceptMjErrors(::mj_initSensorHistory)(
m, d, id, times.has_value() ? times->data() : nullptr,
values.data(), phase);
});
Def<traits::mj_setKeyframe>(pymodule);
Def<traits::mj_addContact>(pymodule);
Def<traits::mj_isPyramidal>(pymodule);
python/mujoco/introspect/enums.py
+15 -14
View File
@@ -522,20 +522,21 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjSTATE_QPOS', 2),
('mjSTATE_QVEL', 4),
('mjSTATE_ACT', 8),
('mjSTATE_WARMSTART', 16),
('mjSTATE_CTRL', 32),
('mjSTATE_QFRC_APPLIED', 64),
('mjSTATE_XFRC_APPLIED', 128),
('mjSTATE_EQ_ACTIVE', 256),
('mjSTATE_MOCAP_POS', 512),
('mjSTATE_MOCAP_QUAT', 1024),
('mjSTATE_USERDATA', 2048),
('mjSTATE_PLUGIN', 4096),
('mjNSTATE', 13),
('mjSTATE_PHYSICS', 14),
('mjSTATE_FULLPHYSICS', 4111),
('mjSTATE_USER', 4064),
('mjSTATE_INTEGRATION', 8191),
('mjSTATE_HISTORY', 16),
('mjSTATE_WARMSTART', 32),
('mjSTATE_CTRL', 64),
('mjSTATE_QFRC_APPLIED', 128),
('mjSTATE_XFRC_APPLIED', 256),
('mjSTATE_EQ_ACTIVE', 512),
('mjSTATE_MOCAP_POS', 1024),
('mjSTATE_MOCAP_QUAT', 2048),
('mjSTATE_USERDATA', 4096),
('mjSTATE_PLUGIN', 8192),
('mjNSTATE', 14),
('mjSTATE_PHYSICS', 30),
('mjSTATE_FULLPHYSICS', 8223),
('mjSTATE_USER', 8128),
('mjSTATE_INTEGRATION', 16383),
]),
)),
('mjtConstraint',
python/mujoco/introspect/functions.py
+150
View File
@@ -2636,6 +2636,156 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
),
doc='Copy state from src to dst.',
)),
('mj_readCtrl',
FunctionDecl(
name='mj_readCtrl',
return_type=ValueType(name='mjtNum'),
parameters=(
FunctionParameterDecl(
name='m',
type=PointerType(
inner_type=ValueType(name='mjModel', is_const=True),
),
),
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData', is_const=True),
),
),
FunctionParameterDecl(
name='id',
type=ValueType(name='int'),
),
FunctionParameterDecl(
name='time',
type=ValueType(name='mjtNum'),
),
FunctionParameterDecl(
name='interp',
type=ValueType(name='int'),
),
),
doc='Read ctrl value for actuator at given time. Returns d->ctrl[id] if no history, otherwise reads from history buffer. interp: 0=zero-order-hold, 1=linear, 2=cubic spline.', # pylint: disable=line-too-long
)),
('mj_readSensor',
FunctionDecl(
name='mj_readSensor',
return_type=PointerType(
inner_type=ValueType(name='mjtNum', is_const=True),
),
parameters=(
FunctionParameterDecl(
name='m',
type=PointerType(
inner_type=ValueType(name='mjModel', is_const=True),
),
),
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData', is_const=True),
),
),
FunctionParameterDecl(
name='id',
type=ValueType(name='int'),
),
FunctionParameterDecl(
name='time',
type=ValueType(name='mjtNum'),
),
FunctionParameterDecl(
name='result',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
),
FunctionParameterDecl(
name='interp',
type=ValueType(name='int'),
),
),
doc='Read sensor value from history buffer at given time. Returns pointer to sensordata (no history) or history buffer (exact match), or NULL if interpolation performed (writes to result). interp: 0=zero-order-hold, 1=linear, 2=cubic spline.', # pylint: disable=line-too-long
)),
('mj_initCtrlHistory',
FunctionDecl(
name='mj_initCtrlHistory',
return_type=ValueType(name='void'),
parameters=(
FunctionParameterDecl(
name='m',
type=PointerType(
inner_type=ValueType(name='mjModel', is_const=True),
),
),
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData'),
),
),
FunctionParameterDecl(
name='id',
type=ValueType(name='int'),
),
FunctionParameterDecl(
name='times',
type=PointerType(
inner_type=ValueType(name='mjtNum', is_const=True),
),
nullable=True,
),
FunctionParameterDecl(
name='values',
type=PointerType(
inner_type=ValueType(name='mjtNum', is_const=True),
),
),
),
doc='Initialize history buffer for actuator; if times is NULL, uses existing buffer timestamps.', # pylint: disable=line-too-long
)),
('mj_initSensorHistory',
FunctionDecl(
name='mj_initSensorHistory',
return_type=ValueType(name='void'),
parameters=(
FunctionParameterDecl(
name='m',
type=PointerType(
inner_type=ValueType(name='mjModel', is_const=True),
),
),
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData'),
),
),
FunctionParameterDecl(
name='id',
type=ValueType(name='int'),
),
FunctionParameterDecl(
name='times',
type=PointerType(
inner_type=ValueType(name='mjtNum', is_const=True),
),
nullable=True,
),
FunctionParameterDecl(
name='values',
type=PointerType(
inner_type=ValueType(name='mjtNum', is_const=True),
),
),
FunctionParameterDecl(
name='phase',
type=ValueType(name='mjtNum'),
),
),
doc='Initialize history buffer for sensor; if times is NULL, uses existing buffer timestamps. phase sets the user slot (last computation time for interval sensors).', # pylint: disable=line-too-long
)),
('mj_setKeyframe',
FunctionDecl(
name='mj_setKeyframe',
python/mujoco/introspect/structs.py
+107
View File
@@ -1277,6 +1277,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='mjtSize'),
doc='number of mjtNums in plugin state vector',
),
StructFieldDecl(
name='nhistory',
type=ValueType(name='mjtSize'),
doc='number of mjtNums in history buffer',
),
StructFieldDecl(
name='narena',
type=ValueType(name='mjtSize'),
@@ -4157,6 +4162,30 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='group for visibility',
array_extent=('nu',),
),
StructFieldDecl(
name='actuator_history',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='history buffer: [nsample, interp]',
array_extent=('nu', 2),
),
StructFieldDecl(
name='actuator_historyadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='address in history buffer; -1: none',
array_extent=('nu',),
),
StructFieldDecl(
name='actuator_delay',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='delay time in seconds; 0: no delay',
array_extent=('nu',),
),
StructFieldDecl(
name='actuator_ctrllimited',
type=PointerType(
@@ -4389,6 +4418,38 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='noise standard deviation',
array_extent=('nsensor',),
),
StructFieldDecl(
name='sensor_history',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='history buffer: [nsample, interp]',
array_extent=('nsensor', 2),
),
StructFieldDecl(
name='sensor_historyadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='address in history buffer; -1: none',
array_extent=('nsensor',),
),
StructFieldDecl(
name='sensor_delay',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='delay time in seconds; 0: no delay',
array_extent=('nsensor',),
),
StructFieldDecl(
name='sensor_interval',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='interval: [period, phase] in seconds',
array_extent=('nsensor', 2),
),
StructFieldDecl(
name='sensor_user',
type=PointerType(
@@ -5402,6 +5463,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='actuator activation',
array_extent=('na',),
),
StructFieldDecl(
name='history',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='history buffer',
array_extent=('nhistory',),
),
StructFieldDecl(
name='qacc_warmstart',
type=PointerType(
@@ -9173,6 +9242,21 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='int'),
doc='group',
),
StructFieldDecl(
name='nsample',
type=ValueType(name='int'),
doc='number of samples in history buffer',
),
StructFieldDecl(
name='interp',
type=ValueType(name='int'),
doc='interpolation order (0=ZOH, 1=linear, 2=cubic)',
),
StructFieldDecl(
name='delay',
type=ValueType(name='double'),
doc='delay time in seconds; 0: no delay',
),
StructFieldDecl(
name='userdata',
type=PointerType(
@@ -9268,6 +9352,29 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='double'),
doc='noise stdev',
),
StructFieldDecl(
name='nsample',
type=ValueType(name='int'),
doc='number of samples in history buffer',
),
StructFieldDecl(
name='interp',
type=ValueType(name='int'),
doc='interpolation order (0=ZOH, 1=linear, 2=cubic)',
),
StructFieldDecl(
name='delay',
type=ValueType(name='double'),
doc='delay time in seconds',
),
StructFieldDecl(
name='interval',
type=ArrayType(
inner_type=ValueType(name='double'),
extents=(2,),
),
doc='[period, time_prev] in seconds',
),
StructFieldDecl(
name='userdata',
type=PointerType(