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_derivative.c
+33
View File
@@ -1107,6 +1107,17 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
bias_vel = (m->actuator_biasprm + mjNBIAS*i)[2];
}
// DC motor bias (back-EMF)
else if (m->actuator_biastype[i] == mjBIAS_DCMOTOR) {
const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i;
const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i;
if (dynprm[0] <= 0) {
mjtNum R = mju_max(mjMINVAL, gainprm[0]);
mjtNum K = gainprm[1];
bias_vel -= K * K / R;
}
}
// affine gain
if (m->actuator_gaintype[i] == mjGAIN_AFFINE) {
// extract bias info: prm = [const, kp, kv]
@@ -1122,6 +1133,28 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
m->actuator_gainprm + mjNGAIN*i);
}
// DC motor controller damping and LuGre micro-damping
else if (m->actuator_gaintype[i] == mjGAIN_DCMOTOR) {
const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i;
const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i;
int input_mode = (int)gainprm[8];
if (input_mode > 0) {
mjtNum R = gainprm[0];
mjtNum K = gainprm[1];
mjtNum gain = (dynprm[0] > 0) ? K : K / mju_max(mjMINVAL, R);
mjtNum kp = gainprm[4];
mjtNum kd = gainprm[6];
bias_vel -= gain * (input_mode == 1 ? kd : kp);
}
// LuGre: force includes -sigma1*z_dot, z_dot = a*z + v
// d(sigma1*z_dot)/dv = sigma1*(da/dv*z + 1), ignoring higher-order da/dv*z
mjtNum sigma1 = dynprm[6];
if (sigma1 > 0) {
bias_vel -= sigma1;
}
}
// force = gain .* [ctrl/act]
if (gain_vel != 0) {
if (m->actuator_dyntype[i] == mjDYN_NONE) {