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_derivative_fd.c
+7
View File
@@ -295,6 +295,10 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) {
void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered,
mjtNum* DyDq, mjtNum* DyDv, mjtNum* DyDa, mjtNum* DyDu,
mjtNum* DsDq, mjtNum* DsDv, mjtNum* DsDa, mjtNum* DsDu) {
if (m->nhistory) {
mjERROR("delays are not supported");
}
int nq = m->nq, nv = m->nv, na = m->na, nu = m->nu, ns = m->nsensordata;
int ndx = 2*nv+na; // row length of Dy Jacobians
mj_markStack(d);
@@ -540,6 +544,9 @@ void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_cente
if (m->opt.integrator == mjINT_RK4) {
mjERROR("RK4 integrator is not supported");
}
if (m->nhistory) {
mjERROR("delays are not supported");
}
int nv = m->nv, na = m->na, nu = m->nu, ns = m->nsensordata;
int ndx = 2*nv+na; // row length of state Jacobians
src/engine/engine_forward.c
+61 -4
View File
@@ -320,10 +320,17 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// any tendon transmission targets with force limits
int tendon_frclimited = 0;
// local, clamped copy of ctrl
// local copy of ctrl
mj_markStack(d);
mjtNum *ctrl = mjSTACKALLOC(d, nu, mjtNum);
mju_copy(ctrl, d->ctrl, nu);
// read from ctrl or history buffer for delayed actuators
for (int i = 0; i < nu; i++) {
int interp = m->actuator_history[2*i+1];
ctrl[i] = m->actuator_delay[i] ? mj_readCtrl(m, d, i, d->time, interp) : d->ctrl[i];
}
// clamp local copy
if (!mjDISABLED(mjDSBL_CLAMPCTRL)) {
clampVec(ctrl, m->actuator_ctrlrange, m->actuator_ctrllimited, nu, NULL);
}
@@ -846,14 +853,64 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
}
//-------------------------- integrators ----------------------------------------------------------
//-------------------------- state advancement and integration ------------------------------------
// advance state and time given activation derivatives, acceleration, and optional velocity
static void mj_advance(const mjModel* m, mjData* d,
const mjtNum* act_dot, const mjtNum* qacc, const mjtNum* qvel) {
int nu = m->nu, nsensor = m->nsensor;
// advance history buffers
if (m->nhistory > 0) {
// advance ctrl history buffers
for (int i = 0; i < nu; i++) {
int nsample = m->actuator_history[2*i];
if (nsample == 0) continue;
// get history buffer pointer and insert ctrl at current time
mjtNum* buf = d->history + m->actuator_historyadr[i];
*mju_delayInsert(buf, nsample, /*dim=*/1, d->time) = d->ctrl[i];
}
// advance sensor history buffers
for (int i = 0; i < nsensor; i++) {
int nsample = m->sensor_history[2*i];
if (nsample == 0) continue;
// get history buffer parameters
int dim = m->sensor_dim[i];
mjtNum* buf = d->history + m->sensor_historyadr[i];
mjtNum delay = m->sensor_delay[i];
mjtNum interval = m->sensor_interval[2*i];
if (interval > 0) {
// interval mode: if condition is satisfied, compute; otherwise copy
mjtNum time_prev = buf[0]; // first slot stores previous sensor tick
if (time_prev + interval <= d->time) {
buf[0] += interval; // advance by exact interval (continuous time)
mjtNum* slot = mju_delayInsert(buf, nsample, dim, d->time);
if (delay > 0) {
// have delay, compute sensor
mj_computeSensor(m, d, i, slot);
} else {
// no delay, copy from sensordata (already computed)
mju_copy(slot, d->sensordata + m->sensor_adr[i], dim);
}
}
} else if (delay > 0) {
// delay-only mode: always compute and insert
mjtNum* slot = mju_delayInsert(buf, nsample, dim, d->time);
mj_computeSensor(m, d, i, slot);
} else {
// history-only mode: copy from sensordata (already computed)
mjtNum* slot = mju_delayInsert(buf, nsample, dim, d->time);
mju_copy(slot, d->sensordata + m->sensor_adr[i], dim);
}
}
}
// advance activations
if (m->na && !mjDISABLED(mjDSBL_ACTUATION)) {
int nu = m->nu;
for (int i=0; i < nu; i++) {
int actadr = m->actuator_actadr[i];
int actadr_end = actadr + m->actuator_actnum[i];
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);
src/engine/engine_print.c
+75
View File
@@ -120,6 +120,51 @@ static void printArray2dInt(const char* str, int nr, int nc, const int* data, FI
}
// print history buffer with semantic labels
static void printDelayBuffer(const char* name, const mjtNum* buf, int nhistory, int dim,
FILE* fp, const char* float_format) {
if (!buf || nhistory <= 0) {
return;
}
fprintf(fp, " %s:\n", name);
// user value (first slot)
fprintf(fp, " phase = ");
fprintf(fp, float_format, buf[0]);
fprintf(fp, "\n");
// cursor (second slot, stored as mjtNum but is an integer)
fprintf(fp, " cursor = %d\n", (int)buf[1]);
// timestamps
const mjtNum* times = buf + 2;
fprintf(fp, " times = ");
for (int i = 0; i < nhistory; i++) {
fprintf(fp, float_format, times[i]);
}
fprintf(fp, "\n");
// values
const mjtNum* values = times + nhistory;
if (dim == 1) {
fprintf(fp, " values = ");
for (int i = 0; i < nhistory; i++) {
fprintf(fp, float_format, values[i]);
}
fprintf(fp, "\n");
} else {
fprintf(fp, " values:\n");
for (int i = 0; i < nhistory; i++) {
fprintf(fp, " [%d] =", i);
for (int j = 0; j < dim; j++) {
fprintf(fp, float_format, values[i*dim + j]);
}
fprintf(fp, "\n");
}
}
}
// print sparse matrix
static void printSparse(const char* str, const mjtNum* mat, int nr,
const int* rownnz, const int* rowadr,
@@ -1241,6 +1286,36 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena
printArray2d("QPOS", m->nq, 1, d->qpos, fp, float_format);
printArray2d("QVEL", m->nv, 1, d->qvel, fp, float_format);
printArray2d("ACT", m->na, 1, d->act, fp, float_format);
// print history buffers with semantic structure
if (m->nhistory) {
fprintf(fp, "DELAY\n");
// actuator history buffers
for (int i = 0; i < m->nu; i++) {
int adr = m->actuator_historyadr[i];
if (adr >= 0) {
char name[100];
const char* actuator_name = mj_id2name(m, mjOBJ_ACTUATOR, i);
snprintf(name, sizeof(name), "actuator %d '%s'", i, actuator_name ? actuator_name : "");
printDelayBuffer(name, d->history + adr, m->actuator_history[2*i], 1, fp, float_format);
}
}
// sensor history buffers
for (int i = 0; i < m->nsensor; i++) {
int adr = m->sensor_historyadr[i];
if (adr >= 0) {
char name[100];
const char* sensor_name = mj_id2name(m, mjOBJ_SENSOR, i);
snprintf(name, sizeof(name), "sensor %d '%s'", i, sensor_name ? sensor_name : "");
printDelayBuffer(name, d->history + adr, m->sensor_history[2*i], m->sensor_dim[i],
fp, float_format);
}
}
fprintf(fp, "\n");
}
printArray2d("QACC_WARMSTART", m->nv, 1, d->qacc_warmstart, fp, float_format);
printArray2d("CTRL", m->nu, 1, d->ctrl, fp, float_format);
printArray2d("QFRC_APPLIED", m->nv, 1, d->qfrc_applied, fp, float_format);
src/engine/engine_sensor.c
+54 -6
View File
@@ -1343,6 +1343,51 @@ void mj_computeSensor(const mjModel* m, mjData* d, int i, mjtNum* sensordata) {
}
// compute sensor or read from history buffer (handles delay and interval logic)
static void compute_or_read_sensor(const mjModel* m, mjData* d, int i, mjtNum* sensordata) {
int nsample = m->sensor_history[2*i];
// no history: compute directly
if (nsample <= 0) {
mj_computeSensor(m, d, i, sensordata);
return;
}
mjtNum delay = m->sensor_delay[i];
int dim = m->sensor_dim[i];
// delay > 0: read delayed value from buffer
if (delay > 0) {
int interp = m->sensor_history[2*i+1];
const mjtNum* ptr = mj_readSensor(m, d, i, d->time, sensordata, interp);
if (ptr) mju_copy(sensordata, ptr, dim);
return;
}
// interval > 0: compute if interval condition satisfied, else read from buffer
mjtNum interval = m->sensor_interval[2*i];
if (interval > 0) {
int historyadr = m->sensor_historyadr[i];
mjtNum* buf = d->history + historyadr;
mjtNum time_prev = buf[0]; // first slot stores time_prev
if (time_prev + interval <= d->time) {
// interval condition satisfied: compute new sensor value
mj_computeSensor(m, d, i, sensordata);
} else {
// interval condition not satisfied: read from buffer
int interp = m->sensor_history[2*i+1];
const mjtNum* ptr = mj_readSensor(m, d, i, d->time, sensordata, interp);
if (ptr) mju_copy(sensordata, ptr, dim);
}
return;
}
// history only, no delay or interval: compute directly
mj_computeSensor(m, d, i, sensordata);
}
// compute user sensors: call user callback and apply cutoff
static void compute_user_sensors(const mjModel* m, mjData* d, mjtStage stage) {
if (mjcb_sensor) {
@@ -1438,13 +1483,14 @@ void mj_sensorPos(const mjModel* m, mjData* d) {
if (m->sensor_needstage[i] == mjSTAGE_POS) {
int adr = m->sensor_adr[i];
mjtNum* sensordata = d->sensordata + adr;
if (type == mjSENS_USER) {
// clear result, compute later
mju_zero(d->sensordata + adr, m->sensor_dim[i]);
mju_zero(sensordata, m->sensor_dim[i]);
nusersensor++;
} else {
mj_computeSensor(m, d, i, d->sensordata + adr);
compute_or_read_sensor(m, d, i, sensordata);
}
}
}
@@ -1486,6 +1532,7 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
if (m->sensor_needstage[i] == mjSTAGE_VEL) {
mjtSensor type = m->sensor_type[i];
int adr = m->sensor_adr[i];
mjtNum* sensordata = d->sensordata + adr;
if (type == mjSENS_USER) {
// call mj_subtreeVel for user sensors
@@ -1494,10 +1541,10 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
}
// clear result, compute later
mju_zero(d->sensordata + adr, m->sensor_dim[i]);
mju_zero(sensordata, m->sensor_dim[i]);
nusersensor++;
} else {
mj_computeSensor(m, d, i, d->sensordata + adr);
compute_or_read_sensor(m, d, i, sensordata);
}
}
}
@@ -1539,6 +1586,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
if (m->sensor_needstage[i] == mjSTAGE_ACC) {
mjtSensor type = m->sensor_type[i];
int adr = m->sensor_adr[i];
mjtNum* sensordata = d->sensordata + adr;
if (type == mjSENS_USER) {
// call mj_rnePostConstraint for user sensors
@@ -1547,10 +1595,10 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
}
// clear result, compute later
mju_zero(d->sensordata + adr, m->sensor_dim[i]);
mju_zero(sensordata, m->sensor_dim[i]);
nusersensor++;
} else {
mj_computeSensor(m, d, i, d->sensordata + adr);
compute_or_read_sensor(m, d, i, sensordata);
}
}
}
src/engine/engine_sensor.h
+3
View File
@@ -25,6 +25,9 @@ extern "C" {
//-------------------------------- sensors ---------------------------------------------------------
// compute value for one sensor, write to sensordata, apply cutoff
void mj_computeSensor(const mjModel* m, mjData* d, int i, mjtNum* sensordata);
// position-dependent sensors
MJAPI void mj_sensorPos(const mjModel* m, mjData* d);
src/engine/engine_support.c
+113 -1
View File
@@ -132,6 +132,7 @@ static inline int mj_stateElemSize(const mjModel* m, mjtState sig) {
case mjSTATE_QPOS: return m->nq;
case mjSTATE_QVEL: return m->nv;
case mjSTATE_ACT: return m->na;
case mjSTATE_HISTORY: return m->nhistory;
case mjSTATE_WARMSTART: return m->nv;
case mjSTATE_CTRL: return m->nu;
case mjSTATE_QFRC_APPLIED: return m->nv;
@@ -155,6 +156,7 @@ static inline mjtNum* mj_stateElemPtr(const mjModel* m, mjData* d, mjtState sig)
case mjSTATE_QPOS: return d->qpos;
case mjSTATE_QVEL: return d->qvel;
case mjSTATE_ACT: return d->act;
case mjSTATE_HISTORY: return d->history;
case mjSTATE_WARMSTART: return d->qacc_warmstart;
case mjSTATE_CTRL: return d->ctrl;
case mjSTATE_QFRC_APPLIED: return d->qfrc_applied;
@@ -619,7 +621,7 @@ void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt,
vadr += 3;
padr += 3;
// continute with rotations
// continue with rotations
mjFALLTHROUGH;
case mjJNT_BALL:
@@ -806,3 +808,113 @@ void mju_camIntrinsics(const mjModel* m, int camid,
// extent only used for orthographic cameras
*extent = m->cam_fovy[camid];
}
// read delayed ctrl value for actuator at given time
mjtNum mj_readCtrl(const mjModel* m, const mjData* d, int id, mjtNum time, int interp) {
// validate actuator id
if (id < 0 || id >= m->nu) {
mjERROR("invalid actuator id %d", id);
return 0;
}
// no delay: return current ctrl value
int nsample = m->actuator_history[2*id];
if (nsample == 0) {
return d->ctrl[id];
}
// resolve interpolation order: use model's interp if argument is -1
if (interp < 0) interp = m->actuator_history[2*id+1];
// get buffer pointer and read from history buffer
mjtNum delay = m->actuator_delay[id];
const mjtNum* buf = d->history + m->actuator_historyadr[id];
mjtNum res;
const mjtNum* ptr = mju_delayRead(buf, nsample, /*dim=*/1, &res, time - delay, interp);
return ptr ? *ptr : res;
}
// read sensor value from history buffer at given time
const mjtNum* mj_readSensor(const mjModel* m, const mjData* d, int id, mjtNum time,
mjtNum* result, int interp) {
// validate sensor id
if (id < 0 || id >= m->nsensor) {
mjERROR("invalid sensor id %d", id);
return NULL;
}
// no history: return current sensor value
int nsample = m->sensor_history[2*id];
if (nsample == 0) {
return d->sensordata + m->sensor_adr[id];
}
// resolve interpolation order: use model's interp if argument is -1
if (interp < 0) interp = m->sensor_history[2*id+1];
// get buffer pointer and read from history buffer
int dim = m->sensor_dim[id];
mjtNum delay = m->sensor_delay[id];
const mjtNum* buf = d->history + m->sensor_historyadr[id];
return mju_delayRead(buf, nsample, dim, result, time - delay, interp);
}
// initialize history buffer for actuator
void mj_initCtrlHistory(const mjModel* m, mjData* d, int id,
const mjtNum* times, const mjtNum* values) {
// validate actuator id
if (id < 0 || id >= m->nu) {
mjERROR("invalid actuator id %d", id);
return;
}
// check that actuator has a history buffer
int nsample = m->actuator_history[2*id];
if (nsample == 0) {
mjERROR("actuator %d has no history buffer", id);
return;
}
// get buffer pointer
mjtNum* buf = d->history + m->actuator_historyadr[id];
// if times is NULL, use existing buffer times
const mjtNum* buf_times = times ? times : buf + 2;
// get existing user value (preserve it)
mjtNum user = buf[0];
// initialize history buffer
mju_delayInit(buf, nsample, 1, buf_times, values, user);
}
// initialize history buffer for sensor
void mj_initSensorHistory(const mjModel* m, mjData* d, int id,
const mjtNum* times, const mjtNum* values, mjtNum phase) {
// validate sensor id
if (id < 0 || id >= m->nsensor) {
mjERROR("invalid sensor id %d", id);
return;
}
// check that sensor has a history buffer
int nsample = m->sensor_history[2*id];
if (nsample == 0) {
mjERROR("sensor %d has no history buffer", id);
return;
}
// get buffer pointer and dimension
mjtNum* buf = d->history + m->sensor_historyadr[id];
int dim = m->sensor_dim[id];
// if times is NULL, use existing buffer times
const mjtNum* buf_times = times ? times : buf + 2;
// initialize history buffer with provided phase
mju_delayInit(buf, nsample, dim, buf_times, values, phase);
}
src/engine/engine_support.h
+23
View File
@@ -130,6 +130,29 @@ void mju_camIntrinsics(const mjModel* m, int camid,
mjtNum* fx, mjtNum* fy, mjtNum* cx, mjtNum* cy,
mjtNum* ortho_extent);
// 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
MJAPI mjtNum mj_readCtrl(const mjModel* m, const mjData* d, int id, mjtNum time, int interp);
// 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
MJAPI const mjtNum* mj_readSensor(const mjModel* m, const mjData* d, int id, mjtNum time,
mjtNum* result, int interp);
// initialize history buffer for actuator with given values
// if times is NULL, uses existing buffer timestamps
MJAPI void mj_initCtrlHistory(const mjModel* m, mjData* d, int id,
const mjtNum* times, const mjtNum* values);
// initialize history buffer for sensor with given values
// if times is NULL, uses existing buffer timestamps
// phase sets the user slot (last computation time for interval sensors)
MJAPI void mj_initSensorHistory(const mjModel* m, mjData* d, int id,
const mjtNum* times, const mjtNum* values, mjtNum phase);
#ifdef __cplusplus
}
#endif
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
src/user/user_model.h
+1
View File
@@ -118,6 +118,7 @@ class mjCModel_ : public mjsElement {
mjtSize ntexdata; // number of texture bytes
mjtSize nwrap; // number of wrap objects in all tendon paths
mjtSize nsensordata; // number of mjtNums in sensor data vector
mjtSize nhistory; // number of mjtNums in history buffer
mjtSize nnumericdata; // number of mjtNums in all custom fields
mjtSize ntextdata; // number of chars in all text fields, including 0
mjtSize ntupledata; // number of objects in all tuple fields
src/user/user_objects.cc
+36
View File
@@ -6960,6 +6960,17 @@ void mjCActuator::Compile(void) {
throw mjCError(this, "plugin '%s' does not support actuators", pplugin->name);
}
}
// validate delay
if (delay > 0 && nsample <= 0) {
throw mjCError(this, "setting delay > 0 without a history buffer");
}
// nsample is limited to 2^24 because the cursor is stored as an mjtNum, which may be a float
// single-precision floats can represent all integers up to 2^24 exactly
if (nsample > 16777216) {
throw mjCError(this, "at most 2^24 samples in history buffer, got %d", nullptr, nsample);
}
}
@@ -7291,6 +7302,31 @@ void mjCSensor::Compile(void) {
throw mjCError(this, "negative cutoff in sensor");
}
// require non-negative interval
if (interval[0] < 0) {
throw mjCError(this, "negative interval in sensor");
}
// require non-positive phase
if (interval[1] > 0) {
throw mjCError(this, "positive phase in sensor");
}
// require phase > -period (values outside this are equivalent modulo period)
if (interval[0] > 0 && interval[1] <= -interval[0]) {
throw mjCError(this, "phase must be greater than -period in sensor");
}
// require nsample for delay
if (delay > 0 && nsample <= 0) {
throw mjCError(this, "setting delay > 0 without a history buffer");
}
// validate nsample size (max 2^24)
if (nsample > 16777216) {
throw mjCError(this, "at most 2^24 samples in sensor history buffer, got %d", nullptr, nsample);
}
// Find referenced object
ResolveReferences(model);
src/xml/xml_base.h
+2
View File
@@ -43,6 +43,7 @@ extern const int mark_sz;
extern const int dyn_sz;
extern const int gain_sz;
extern const int bias_sz;
extern const int interp_sz;
extern const int stage_sz;
extern const int datatype_sz;
extern const int camout_sz;
@@ -74,6 +75,7 @@ extern const mjMap mark_map[];
extern const mjMap dyn_map[];
extern const mjMap gain_map[];
extern const mjMap bias_map[];
extern const mjMap interp_map[];
extern const mjMap stage_map[];
extern const mjMap datatype_map[];
extern const mjMap condata_map[];
src/xml/xml_native_reader.cc
+82 -66
View File
@@ -182,29 +182,29 @@ std::vector<const char*> MJCF[nMJCF] = {
"frictionloss", "springlength", "width", "material",
"margin", "stiffness", "damping", "rgba", "user"},
{"general", "?", "ctrllimited", "forcelimited", "actlimited", "ctrlrange",
"forcerange", "actrange", "gear", "cranklength", "user", "group", "actdim",
"forcerange", "actrange", "gear", "cranklength", "user", "group", "nsample", "interp", "delay", "actdim",
"dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", "actearly"},
{"motor", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group"},
"gear", "cranklength", "user", "group", "nsample", "interp", "delay"},
{"position", "?", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange",
"forcerange", "gear", "cranklength", "user", "group", "kp", "kv", "dampratio", "timeconst"},
"forcerange", "gear", "cranklength", "user", "group", "nsample", "interp", "delay", "kp", "kv", "dampratio", "timeconst"},
{"velocity", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group", "kv"},
"gear", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"},
{"intvelocity", "?", "ctrllimited", "forcelimited",
"ctrlrange", "forcerange", "actrange", "inheritrange",
"gear", "cranklength", "user", "group",
"gear", "cranklength", "user", "group", "nsample", "interp", "delay",
"kp", "kv", "dampratio"},
{"damper", "?", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group", "kv"},
"gear", "cranklength", "user", "group", "nsample", "interp", "delay", "kv"},
{"cylinder", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group",
"gear", "cranklength", "user", "group", "nsample", "interp", "delay",
"timeconst", "area", "diameter", "bias"},
{"muscle", "?", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"gear", "cranklength", "user", "group",
"gear", "cranklength", "user", "group", "nsample", "interp", "delay",
"timeconst", "range", "force", "scale",
"lmin", "lmax", "vmax", "fpmax", "fvmax"},
{"adhesion", "?", "forcelimited", "ctrlrange", "forcerange",
"gain", "user", "group"},
"gain", "user", "group", "nsample", "interp", "delay"},
{">"},
{"extension", "*"},
@@ -389,51 +389,51 @@ std::vector<const char*> MJCF[nMJCF] = {
{"actuator", "*"},
{"<"},
{"general", "*", "name", "class", "group",
{"general", "*", "name", "class", "group", "nsample", "interp", "delay",
"ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"body", "actdim", "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm",
"actearly"},
{"motor", "*", "name", "class", "group",
{"motor", "*", "name", "class", "group", "nsample", "interp", "delay",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite"},
{"position", "*", "name", "class", "group",
{"position", "*", "name", "class", "group", "nsample", "interp", "delay",
"ctrllimited", "forcelimited", "ctrlrange", "inheritrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"kp", "kv", "dampratio", "timeconst"},
{"velocity", "*", "name", "class", "group",
{"velocity", "*", "name", "class", "group", "nsample", "interp", "delay",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"kv"},
{"intvelocity", "*", "name", "class", "group",
{"intvelocity", "*", "name", "class", "group", "nsample", "interp", "delay",
"ctrllimited", "forcelimited",
"ctrlrange", "forcerange", "actrange", "inheritrange", "lengthrange",
"gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"kp", "kv", "dampratio"},
{"damper", "*", "name", "class", "group",
{"damper", "*", "name", "class", "group", "nsample", "interp", "delay",
"forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"kv"},
{"cylinder", "*", "name", "class", "group",
{"cylinder", "*", "name", "class", "group", "nsample", "interp", "delay",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
"timeconst", "area", "diameter", "bias"},
{"muscle", "*", "name", "class", "group",
{"muscle", "*", "name", "class", "group", "nsample", "interp", "delay",
"ctrllimited", "forcelimited", "ctrlrange", "forcerange",
"lengthrange", "gear", "cranklength", "user",
"joint", "jointinparent", "tendon", "slidersite", "cranksite",
"timeconst", "tausmooth", "range", "force", "scale",
"lmin", "lmax", "vmax", "fpmax", "fvmax"},
{"adhesion", "*", "name", "class", "group",
{"adhesion", "*", "name", "class", "group", "nsample", "interp", "delay",
"forcelimited", "ctrlrange", "forcerange", "user", "body", "gain"},
{"plugin", "*", "name", "class", "plugin", "instance", "group",
{"plugin", "*", "name", "class", "plugin", "instance", "group", "nsample", "interp", "delay",
"ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange",
"lengthrange", "gear", "cranklength", "joint", "jointinparent",
"site", "actdim", "dyntype", "dynprm", "tendon", "cranksite", "slidersite", "user",
@@ -445,56 +445,56 @@ std::vector<const char*> MJCF[nMJCF] = {
{"sensor", "*"},
{"<"},
{"touch", "*", "name", "site", "cutoff", "noise", "user"},
{"accelerometer", "*", "name", "site", "cutoff", "noise", "user"},
{"velocimeter", "*", "name", "site", "cutoff", "noise", "user"},
{"gyro", "*", "name", "site", "cutoff", "noise", "user"},
{"force", "*", "name", "site", "cutoff", "noise", "user"},
{"torque", "*", "name", "site", "cutoff", "noise", "user"},
{"magnetometer", "*", "name", "site", "cutoff", "noise", "user"},
{"camprojection", "*", "name", "site", "camera", "cutoff", "noise", "user"},
{"rangefinder", "*", "name", "site", "camera", "data", "cutoff", "noise", "user"},
{"jointpos", "*", "name", "joint", "cutoff", "noise", "user"},
{"jointvel", "*", "name", "joint", "cutoff", "noise", "user"},
{"tendonpos", "*", "name", "tendon", "cutoff", "noise", "user"},
{"tendonvel", "*", "name", "tendon", "cutoff", "noise", "user"},
{"actuatorpos", "*", "name", "actuator", "cutoff", "noise", "user"},
{"actuatorvel", "*", "name", "actuator", "cutoff", "noise", "user"},
{"actuatorfrc", "*", "name", "actuator", "cutoff", "noise", "user"},
{"jointactuatorfrc", "*", "name", "joint", "cutoff", "noise", "user"},
{"tendonactuatorfrc", "*", "name", "tendon", "cutoff", "noise", "user"},
{"ballquat", "*", "name", "joint", "cutoff", "noise", "user"},
{"ballangvel", "*", "name", "joint", "cutoff", "noise", "user"},
{"jointlimitpos", "*", "name", "joint", "cutoff", "noise", "user"},
{"jointlimitvel", "*", "name", "joint", "cutoff", "noise", "user"},
{"jointlimitfrc", "*", "name", "joint", "cutoff", "noise", "user"},
{"tendonlimitpos", "*", "name", "tendon", "cutoff", "noise", "user"},
{"tendonlimitvel", "*", "name", "tendon", "cutoff", "noise", "user"},
{"tendonlimitfrc", "*", "name", "tendon", "cutoff", "noise", "user"},
{"framepos", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
{"framequat", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
{"framexaxis", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
{"frameyaxis", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
{"framezaxis", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
{"framelinvel", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
{"frameangvel", "*", "name", "objtype", "objname", "reftype", "refname", "cutoff", "noise", "user"},
{"framelinacc", "*", "name", "objtype", "objname", "cutoff", "noise", "user"},
{"frameangacc", "*", "name", "objtype", "objname", "cutoff", "noise", "user"},
{"subtreecom", "*", "name", "body", "cutoff", "noise", "user"},
{"subtreelinvel", "*", "name", "body", "cutoff", "noise", "user"},
{"subtreeangmom", "*", "name", "body", "cutoff", "noise", "user"},
{"insidesite", "*", "name", "site", "objtype", "objname", "cutoff", "noise", "user"},
{"distance", "*", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
{"normal", "*", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
{"fromto", "*", "name", "geom1", "geom2", "body1", "body2", "cutoff", "noise", "user"},
{"touch", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"accelerometer", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"velocimeter", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"gyro", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"force", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"torque", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"magnetometer", "*", "name", "site", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"camprojection", "*", "name", "site", "camera", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"rangefinder", "*", "name", "site", "camera", "data", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"jointpos", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"jointvel", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"tendonpos", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"tendonvel", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"actuatorpos", "*", "name", "actuator", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"actuatorvel", "*", "name", "actuator", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"actuatorfrc", "*", "name", "actuator", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"jointactuatorfrc", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"tendonactuatorfrc", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"ballquat", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"ballangvel", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"jointlimitpos", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"jointlimitvel", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"jointlimitfrc", "*", "name", "joint", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"tendonlimitpos", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"tendonlimitvel", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"tendonlimitfrc", "*", "name", "tendon", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"framepos", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"framequat", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"framexaxis", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"frameyaxis", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"framezaxis", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"framelinvel", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"frameangvel", "*", "name", "objtype", "objname", "reftype", "refname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"framelinacc", "*", "name", "objtype", "objname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"frameangacc", "*", "name", "objtype", "objname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"subtreecom", "*", "name", "body", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"subtreelinvel", "*", "name", "body", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"subtreeangmom", "*", "name", "body", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"insidesite", "*", "name", "site", "objtype", "objname", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"distance", "*", "name", "geom1", "geom2", "body1", "body2", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"normal", "*", "name", "geom1", "geom2", "body1", "body2", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"fromto", "*", "name", "geom1", "geom2", "body1", "body2", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"contact", "*", "name", "geom1", "geom2", "body1", "body2", "subtree1", "subtree2", "site",
"num", "data", "reduce", "cutoff", "noise", "user"},
{"e_potential", "*", "name", "cutoff", "noise", "user"},
{"e_kinetic", "*", "name", "cutoff", "noise", "user"},
{"clock", "*", "name", "cutoff", "noise", "user"},
"num", "data", "reduce", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"e_potential", "*", "name", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"e_kinetic", "*", "name", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"clock", "*", "name", "nsample", "interp", "delay", "interval", "cutoff", "noise", "user"},
{"tactile", "*", "name", "geom", "mesh", "nsample", "interp", "delay", "interval", "user"},
{"user", "*", "name", "objtype", "objname", "datatype", "needstage",
"dim", "cutoff", "noise", "user"},
{"tactile", "*", "name", "geom", "mesh", "user"},
{"plugin", "*", "name", "plugin", "instance", "cutoff", "objtype", "objname", "reftype", "refname",
"user"},
{"<"},
@@ -751,6 +751,15 @@ const mjMap bias_map[bias_sz] = {
};
// interpolation type
const int interp_sz = 3;
const mjMap interp_map[interp_sz] = {
{"zoh", 0},
{"linear", 1},
{"cubic", 2}
};
// stage type
const int stage_sz = 4;
const mjMap stage_map[stage_sz] = {
@@ -2285,6 +2294,9 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) {
}
}
ReadAttrInt(elem, "group", &actuator->group);
ReadAttrInt(elem, "nsample", &actuator->nsample);
MapValue(elem, "interp", &actuator->interp, interp_map, interp_sz);
ReadAttr(elem, "delay", 1, &actuator->delay, text);
MapValue(elem, "ctrllimited", &actuator->ctrllimited, TFAuto_map, 3);
MapValue(elem, "forcelimited", &actuator->forcelimited, TFAuto_map, 3);
MapValue(elem, "actlimited", &actuator->actlimited, TFAuto_map, 3);
@@ -4037,6 +4049,10 @@ void mjXReader::Sensor(XMLElement* section) {
}
ReadAttr(elem, "cutoff", 1, &sensor->cutoff, text);
ReadAttr(elem, "noise", 1, &sensor->noise, text);
ReadAttrInt(elem, "nsample", &sensor->nsample);
MapValue(elem, "interp", &sensor->interp, interp_map, interp_sz);
ReadAttr(elem, "delay", 1, &sensor->delay, text);
ReadAttr(elem, "interval", 2, sensor->interval, text, /*required=*/false, /*exact=*/false);
if (ReadVector(elem, "user", userdata, text)) {
mjs_setDouble(sensor->userdata, userdata.data(), userdata.size());
}
src/xml/xml_native_writer.cc
+8
View File
@@ -825,6 +825,9 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDe
// defaults and regular
WriteAttrInt(elem, "group", actuator->group, def->Actuator().group);
WriteAttrInt(elem, "nsample", actuator->nsample, def->Actuator().nsample);
WriteAttrKey(elem, "interp", interp_map, interp_sz, actuator->interp, def->Actuator().interp);
WriteAttr(elem, "delay", 1, &actuator->delay, &def->Actuator().delay);
WriteAttrKey(elem, "ctrllimited", TFAuto_map, 3, actuator->ctrllimited, def->Actuator().ctrllimited);
WriteAttr(elem, "ctrlrange", 2, actuator->ctrlrange, def->Actuator().ctrlrange);
WriteAttrKey(elem, "forcelimited", TFAuto_map, 3, actuator->forcelimited, def->Actuator().forcelimited);
@@ -2338,6 +2341,11 @@ void mjXWriter::Sensor(XMLElement* root) {
if (sensor->type != mjSENS_PLUGIN) {
WriteAttr(elem, "noise", 1, &sensor->noise, &zero);
}
WriteAttrInt(elem, "nsample", sensor->nsample, 0);
WriteAttrKey(elem, "interp", interp_map, interp_sz, sensor->interp, 0);
WriteAttr(elem, "delay", 1, &sensor->delay, &zero);
double zeros[2] = {0, 0};
WriteAttr(elem, "interval", 2, sensor->interval, zeros);
WriteVector(elem, "user", sensor->get_userdata());
}