chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix implicit integrator derivatives for actearly actuators.

Browse Source 
The derivative calculation for actuator velocity in implicit integrators now correctly accounts for the `actearly` flag, using the next activation value when `actearly` is true.

PiperOrigin-RevId: 868598722
Change-Id: Ia180afb15b31a718170aeaf9d4ac514bb9e6073b
This commit is contained in:
Yuval Tassa
2026-02-11 04:02:43 -08:00
committed by Copybara-Service
parent 730d494b7f
commit 7f74487a26
6 changed files with 108 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_forward.c
+2 -28
View File
@@ -260,32 +260,6 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
}
// returns the next act given the current act_dot, after clamping
static mjtNum nextActivation(const mjModel* m, const mjData* d,
int actuator_id, int act_adr, mjtNum act_dot) {
mjtNum act = d->act[act_adr];
if (m->actuator_dyntype[actuator_id] == mjDYN_FILTEREXACT) {
// exact filter integration
// act_dot(0) = (ctrl-act(0)) / tau
// act(h) = act(0) + (ctrl-act(0)) (1 - exp(-h / tau))
// = act(0) + act_dot(0) * tau * (1 - exp(-h / tau))
mjtNum tau = mju_max(mjMINVAL, m->actuator_dynprm[actuator_id * mjNDYN]);
act = act + act_dot * tau * (1 - mju_exp(-m->opt.timestep / tau));
} else {
// Euler integration
act = act + act_dot * m->opt.timestep;
}
// clamp to actrange
if (m->actuator_actlimited[actuator_id]) {
mjtNum* actrange = m->actuator_actrange + 2 * actuator_id;
act = mju_clip(act, actrange[0], actrange[1]);
}
return act;
}
// clamp vector to range
static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, int n,
@@ -474,7 +448,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
mjtNum act;
if (m->actuator_actearly[i]) {
act = nextActivation(m, d, i, act_adr, d->act_dot[act_adr]);
act = mj_nextActivation(m, d, i, act_adr, d->act_dot[act_adr]);
} else {
act = d->act[act_adr];
}
@@ -916,7 +890,7 @@ static void mj_advance(const mjModel* m, mjData* d,
int actadr_end = actadr + m->actuator_actnum[i];
for (int j=actadr; j < actadr_end; j++) {
// if disabled, set act_dot to 0
d->act[j] = nextActivation(m, d, i, j, mj_actuatorDisabled(m, i) ? 0 : act_dot[j]);
d->act[j] = mj_nextActivation(m, d, i, j, mj_actuatorDisabled(m, i) ? 0 : act_dot[j]);
}
}
}