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
src/engine/engine_io.c
+63 -4
View File
@@ -224,8 +224,8 @@ void mj_makeModel(mjModel** dest,
// CHECK SIZE PARAMETERS
{
// dummy variables for MJMODEL_SIZES set after mjModel construction
int nnames_map=0, nJmom=0, ngravcomp=0, nemax=0, njmax=0;
int nconmax=0, nuserdata=0, nsensordata=0, npluginstate=0, narena=0, nbuffer=0;
int nnames_map=0, nJmom=0, ngravcomp=0, nemax=0, njmax=0, nconmax=0;
int nuserdata=0, nsensordata=0, npluginstate=0, nhistory=0, narena=0, nbuffer=0;
// sizes must be non-negative and fit in int, except for the byte arrays texdata and textdata
#define X(name) \
@@ -243,8 +243,9 @@ void mj_makeModel(mjModel** dest,
#undef X
// suppress unused variable warnings
(void)nnames_map; (void)nJmom; (void)ngravcomp; (void)nemax; (void)njmax;
(void)nconmax; (void)nuserdata; (void)nsensordata; (void)npluginstate; (void)narena; (void)nbuffer;
(void)nnames_map; (void)nJmom; (void)ngravcomp; (void)nemax; (void)njmax; (void)nconmax;
(void)nuserdata; (void)nsensordata; (void)npluginstate; (void)nhistory; (void)narena;
(void)nbuffer;
}
// nbody should always be positive
@@ -1262,6 +1263,12 @@ mjData* mjv_copyData(mjData* dest, const mjModel* m, const mjData* src) {
// clear data, set defaults
static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
// error early if history buffers cannot be initialized
mjtNum dt = m->opt.timestep;
if (m->nhistory && dt <= 0) {
mjERROR("history buffers require positive timestep, got %g", dt);
}
//------------------------------ save plugin state and data
mjtNum* plugin_state;
uintptr_t* plugindata;
@@ -1367,6 +1374,58 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
mju_zero(d->mocap_pos, 3*m->nmocap);
mju_zero(d->mocap_quat, 4*m->nmocap);
// initialize ctrl history buffers: timestamps at [-n*dt, ..., -dt]
for (int i = 0; i < m->nu; i++) {
int n = m->actuator_history[2*i];
if (n > 0) {
mjtNum* buf = d->history + m->actuator_historyadr[i];
buf[0] = 0; // user slot
buf[1] = n - 1; // cursor: newest at logical index n-1
mjtNum* times = buf + 2;
for (int j = 0; j < n; j++) {
times[j] = -(n-j)*dt;
}
// clear values
mjtNum* values = buf + 2 + n;
mju_zero(values, n);
}
}
// initialize sensor history buffers
for (int i = 0; i < m->nsensor; i++) {
int n = m->sensor_history[2*i];
if (n > 0) {
int dim = m->sensor_dim[i];
mjtNum period = m->sensor_interval[2*i];
mjtNum phase = m->sensor_interval[2*i+1];
mjtNum* buf = d->history + m->sensor_historyadr[i];
// user slot: last compute time (phase=0 means -period, i.e. first compute at t=0)
buf[0] = (period > 0) ? (phase == 0 ? -period : phase) : -dt;
buf[1] = n - 1; // cursor: newest at logical index n-1
mjtNum* times = buf + 2;
if (period > 0) {
// samples spaced at period intervals, rounded up to dt grid
mjtNum t0 = (phase == 0) ? -period : phase;
for (int j = 0; j < n; j++) {
mjtNum continuous_t = t0 - (n-1-j)*period;
times[j] = mju_ceil(continuous_t / dt) * dt;
}
} else {
// no period: timestamps at [-n*dt, ..., -dt]
for (int j = 0; j < n; j++) {
times[j] = -(n-j)*dt;
}
}
// clear values
mjtNum* values = buf + 2 + n;
mju_zero(values, n*dim);
}
}
// zero out qM, special case because scattering from M skips simple body off-diagonals
mju_zero(d->qM, m->nM);