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

Minor improvements to dcmotor

Browse Source 
PiperOrigin-RevId: 895907301
Change-Id: Ia50a6d06c1ede9894cc71f375db837d104f0211e
This commit is contained in:
Yuval Tassa
2026-04-07 08:10:42 -07:00
committed by Copybara-Service
parent b61d304126
commit 382474bb9d
8 changed files with 114 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_api.cc
+25 -25
View File
@@ -1125,18 +1125,18 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double
double nominal[3], double saturation[4], double inductance[2],
double cogging[3], double controller[5], double thermal[6],
double lugre[6], int input_mode) {
double Kt = motorconst[0]; // torque constant
double Ke = motorconst[1]; // back-EMF constant
double R = resistance; // electrical resistance
double vn = nominal[0]; // nominal voltage
double tau0 = nominal[1]; // stall torque
double omega0 = nominal[2]; // no-load speed
double R = resistance; // electrical resistance
double Kt = motorconst ? motorconst[0] : 0; // torque constant
double Ke = motorconst ? motorconst[1] : 0; // back-EMF constant
double vn = nominal ? nominal[0] : 0; // nominal voltage
double tau0 = nominal ? nominal[1] : 0; // stall torque
double omega0 = nominal ? nominal[2] : 0; // no-load speed
// derive Ke from nominal: omega0 = vn*Ke / (Ke^2 + R*B)
if (vn > 0 && Ke <= 0 && omega0 > 0) {
// viscous damping (linear), add lugre sigma2 contribution if any
double B = actuator->damping[0];
if (lugre[0] > 0) B += lugre[2];
if (lugre && lugre[0] > 0) B += lugre[2];
if (B > 0 && R > 0) {
// R known: solve quadratic Ke^2*omega0 - Ke*vn + R*B*omega0 = 0
@@ -1176,24 +1176,24 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double
actuator->gainprm[1] = K;
// controller parameters: gainprm[4:6] for kp, ki, kd
actuator->gainprm[4] = controller[0]; // kp
actuator->gainprm[5] = controller[1]; // ki
actuator->gainprm[6] = controller[2]; // kd
actuator->gainprm[4] = controller ? controller[0] : 0; // kp
actuator->gainprm[5] = controller ? controller[1] : 0; // ki
actuator->gainprm[6] = controller ? controller[2] : 0; // kd
// controller parameters: dynprm[7,8] for slewmax, Imax
actuator->dynprm[7] = controller[3]; // slewmax
actuator->dynprm[8] = controller[4]; // Imax
actuator->dynprm[7] = controller ? controller[3] : 0; // slewmax
actuator->dynprm[8] = controller ? controller[4] : 0; // Imax
// saturation: [tau_max, i_max, (di/dt)_max, v_max]
if (saturation[2] > 0) {
if (saturation && saturation[2] > 0) {
actuator->dynprm[1] = saturation[2]; // (di/dt)_max
}
if (saturation[3] > 0) {
if (saturation && saturation[3] > 0) {
actuator->gainprm[7] = saturation[3]; // v_max
}
// saturation -> forcerange
if (saturation[0] > 0 || saturation[1] > 0) {
if (saturation && (saturation[0] > 0 || saturation[1] > 0)) {
double tau_max = saturation[0];
if (tau_max == 0 && saturation[1] > 0) {
tau_max = K * saturation[1]; // tau_max = K * i_max
@@ -1204,34 +1204,34 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double
}
// cogging: [amplitude, periodicity, phase] -> biasprm[0:3]
actuator->biasprm[0] = cogging[0]; // amplitude
actuator->biasprm[1] = cogging[1]; // periodicity
actuator->biasprm[2] = cogging[2]; // phase
actuator->biasprm[0] = cogging ? cogging[0] : 0; // amplitude
actuator->biasprm[1] = cogging ? cogging[1] : 0; // periodicity
actuator->biasprm[2] = cogging ? cogging[2] : 0; // phase
// count activation variables: slot order is slew, integral, temperature, bristle, current
int actdim = 0;
// inductance: [L, te]
if (inductance[0] < 0) return "DC motor: inductance must be non-negative";
if (inductance[1] < 0) return "DC motor: electrical time constant must be non-negative";
double te = inductance[0] > 0 ? inductance[0] / R : inductance[1];
if (inductance && inductance[0] < 0) return "DC motor: inductance must be non-negative";
if (inductance && inductance[1] < 0) return "DC motor: electrical time constant must be non-negative";
double te = (inductance && inductance[0] > 0) ? inductance[0] / R : (inductance ? inductance[1] : 0);
actuator->dynprm[0] = te;
if (te > 0) {
actdim++;
}
// controller states: slew rate limiting
if (controller[3] > 0) { // slewmax
if (controller && controller[3] > 0) { // slewmax
actdim++;
}
// controller states: integral
if (controller[1] > 0) { // ki
if (controller && controller[1] > 0) { // ki
actdim++;
}
// thermal -> temperature activation
if (thermal[0] > 0 || thermal[1] > 0 || thermal[2] > 0) {
if (thermal && (thermal[0] > 0 || thermal[1] > 0 || thermal[2] > 0)) {
double RT = thermal[0]; // thermal resistance
double C = thermal[1]; // thermal capacitance
double tth = thermal[2]; // thermal time constant
@@ -1259,7 +1259,7 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double
}
// lugre: {stiffness, damping, viscous, coulomb, static, stribeck}
if (lugre[0] > 0) {
if (lugre && lugre[0] > 0) {
actuator->dynprm[5] = lugre[0]; // stiffness -> sigma0
actuator->dynprm[6] = lugre[1]; // damping -> sigma1
actuator->damping[0] += lugre[2]; // viscous -> sigma2