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

Add <dcmotor> actuator and related docs and tests.

Browse Source 
PiperOrigin-RevId: 892927987
Change-Id: I38ed6412801341ba03ddf5fe7b93a6081df24d37
This commit is contained in:
Yuval Tassa
2026-04-01 07:49:53 -07:00
committed by Copybara-Service
parent 6da210c794
commit 70a7647ad9
31 changed files with 3994 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_support.c
+53 -6
View File
@@ -709,22 +709,69 @@ int mj_actuatorDisabled(const mjModel* m, int i) {
mjtNum mj_nextActivation(const mjModel* m, const mjData* d,
int actuator_id, int act_adr, mjtNum act_dot) {
mjtNum act = d->act[act_adr];
int dyntype = m->actuator_dyntype[actuator_id];
if (m->actuator_dyntype[actuator_id] == mjDYN_FILTEREXACT) {
if (dyntype == 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
} else if (dyntype == mjDYN_DCMOTOR) {
const mjtNum* dynprm = m->actuator_dynprm + actuator_id * mjNDYN;
const mjtNum* gainprm = m->actuator_gainprm + actuator_id * mjNGAIN;
mjDCMotorSlots slots = mj_dcmotorSlots(dynprm, gainprm);
int offset = act_adr - m->actuator_actadr[actuator_id];
// current filter: exact integration
if (offset == slots.current) {
mjtNum te = mju_max(mjMINVAL, dynprm[0]);
act = act + act_dot * te * (1 - mju_exp(-m->opt.timestep / te));
}
// LuGre bristle: dz/dt = a*z + v where a = -sigma0*|v|/g(v)
else if (offset == slots.bristle) {
const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*actuator_id;
mjtNum F_C = biasprm[3]; // Coulomb friction
mjtNum F_S = biasprm[4]; // static friction
mjtNum v_S = biasprm[5]; // Stribeck velocity
mjtNum sigma0 = dynprm[5]; // bristle stiffness
mjtNum velocity = d->actuator_velocity[actuator_id];
mjtNum g = mj_lugreStribeck(velocity, F_C, F_S, v_S);
// ZOH exact ZOH integration: z(h) = exp(ah)*z(0) + ((exp(ah)-1)/a)*v
mjtNum a = -sigma0 * mju_abs(velocity) / mju_max(mjMINVAL, g); // decay rate
mjtNum h = m->opt.timestep;
mjtNum exp_ah = mju_exp(a * h); // state transition
mjtNum int_h = mju_abs(a) > mjMINVAL ? (exp_ah - 1) / a : h; // input integral
act = exp_ah * act + int_h * velocity;
}
// integral state: Euler integration with anti-windup clamp
else if (offset == slots.integral) {
act = act + act_dot * m->opt.timestep;
mjtNum Imax = dynprm[8];
if (Imax > 0) {
act = mju_clip(act, -Imax, Imax);
}
}
// temperature and slew: Euler integration
else {
act = act + act_dot * m->opt.timestep;
}
}
// otherwise Euler integration
else {
act = act + act_dot * m->opt.timestep;
}
// clamp to actrange
if (m->actuator_actlimited[actuator_id]) {
mjtNum* actrange = m->actuator_actrange + 2*actuator_id;
// clamp to actrange unless DC motor
if (dyntype != mjDYN_DCMOTOR && m->actuator_actlimited[actuator_id]) {
const mjtNum* actrange = m->actuator_actrange + 2*actuator_id;
act = mju_clip(act, actrange[0], actrange[1]);
}