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/user/user_model.cc
+43
View File
@@ -2249,6 +2249,20 @@ void mjCModel::SetSizes() {
nsensordata += sensors_[i]->dim;
}
// nhistory: layout is [user, cursor, times(n), values(n*dim)] = 2+2n per actuator (dim=1)
nhistory = 0;
for (int i=0; i < actuators_.size(); i++) {
if (actuators_[i]->nsample > 0) {
nhistory += 2 + 2 * actuators_[i]->nsample;
}
}
// sensor delay: layout is [user, cursor, times(n), values(n*dim)] = 2 + n + n*dim
for (int i=0; i < sensors_.size(); i++) {
if (sensors_[i]->nsample > 0) {
nhistory += 2 + sensors_[i]->nsample + sensors_[i]->nsample * sensors_[i]->dim;
}
}
// nnumericdata
for (int i=0; i < nnumeric; i++) {
nnumericdata += numerics_[i]->size;
@@ -3226,6 +3240,7 @@ void mjCModel::CopyObjects(mjModel* m) {
m->njmax = njmax;
m->nconmax = nconmax;
m->nsensordata = nsensordata;
m->nhistory = nhistory;
m->nuserdata = nuserdata;
m->na = na;
@@ -3724,6 +3739,7 @@ void mjCModel::CopyObjects(mjModel* m) {
// actuators
adr = 0;
int delay_adr = 0;
for (int i=0; i < nu; i++) {
// get pointer
mjCActuator* pac = actuators_[i];
@@ -3741,6 +3757,18 @@ void mjCModel::CopyObjects(mjModel* m) {
pac->actdim_ = m->actuator_actnum[i];
adr += m->actuator_actnum[i];
m->actuator_group[i] = pac->group;
// historyadr
m->actuator_delay[i] = (mjtNum)pac->delay;
m->actuator_history[2*i] = pac->nsample;
m->actuator_history[2*i+1] = pac->interp;
if (pac->nsample > 0) {
m->actuator_historyadr[i] = delay_adr;
delay_adr += 2 + 2 * pac->nsample; // [user, cursor, times, values]
} else {
m->actuator_historyadr[i] = -1;
}
m->actuator_ctrllimited[i] = (mjtByte)pac->is_ctrllimited();
m->actuator_forcelimited[i] = (mjtByte)pac->is_forcelimited();
m->actuator_actlimited[i] = (mjtByte)pac->is_actlimited();
@@ -3775,6 +3803,21 @@ void mjCModel::CopyObjects(mjModel* m) {
m->sensor_dim[i] = psen->dim;
m->sensor_cutoff[i] = (mjtNum)psen->cutoff;
m->sensor_noise[i] = (mjtNum)psen->noise;
// history buffer
m->sensor_delay[i] = (mjtNum)psen->delay;
m->sensor_history[2*i] = psen->nsample;
m->sensor_history[2*i+1] = psen->interp;
m->sensor_interval[2*i] = (mjtNum)psen->interval[0];
m->sensor_interval[2*i+1] = (mjtNum)psen->interval[1];
if (psen->nsample > 0) {
m->sensor_historyadr[i] = delay_adr;
int dim = psen->dim;
delay_adr += 2 + psen->nsample + psen->nsample * dim; // [user, cursor, times(n), values(n*dim)]
} else {
m->sensor_historyadr[i] = -1;
}
mjuu_copyvec(m->sensor_user+nuser_sensor*i, psen->get_userdata().data(), nuser_sensor);
// calculate address and advance