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

Correct DC motor derivative calculation and enforce actearly.

Browse Source 
PiperOrigin-RevId: 898983657
Change-Id: I008529eacf3e400696d26bd3a52a4cf4c1c12225
This commit is contained in:
Yuval Tassa
2026-04-13 07:18:28 -07:00
committed by Copybara-Service
parent bc83c24a4d
commit c004d144d1
6 changed files with 181 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_derivative.c
+18 -6
View File
@@ -1137,14 +1137,26 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) {
else if (m->actuator_gaintype[i] == mjGAIN_DCMOTOR) {
const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i;
const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i;
mjtNum te = dynprm[0];
// controller velocity derivative: dV/dω
int input_mode = (int)gainprm[8];
if (input_mode > 0) {
mjtNum R = gainprm[0];
mjtNum dVdw = 0;
if (input_mode == 1) dVdw = -gainprm[6]; // position: -kd
else if (input_mode == 2) dVdw = -gainprm[4]; // velocity: -kp
if (te > 0) {
// stateful current with actearly: d(K*next_act)/dω
// includes both back-EMF (-K) and controller (dVdw) through act_dot
mjtNum R = mju_max(mjMINVAL, 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);
mjtNum s = 1 - mju_exp(-m->opt.timestep / te);
bias_vel += K * (dVdw - K) * s / R;
} else if (dVdw != 0) {
// stateless: controller terms only (back-EMF handled in bias block)
mjtNum R = mju_max(mjMINVAL, gainprm[0]);
mjtNum K = gainprm[1];
bias_vel += K * dVdw / R;
}
// LuGre: force includes -sigma1*z_dot, z_dot = a*z + v
src/user/user_api.cc
+3
View File
@@ -1280,6 +1280,9 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double
// enforce actlimited = 0; homogeneous bounds are invalid across DC motor states
actuator->actlimited = 0;
// DC motor always uses actearly
actuator->actearly = 1;
return "";
}
src/user/user_objects.cc
+6
View File
@@ -7238,6 +7238,12 @@ void mjCActuator::Compile(void) {
actdim = (dyntype != mjDYN_NONE && dyntype != mjDYN_DCMOTOR);
}
// DC motor always uses actearly
if (dyntype == mjDYN_DCMOTOR && !actearly) {
throw mjCError(this, "actearly cannot be false for DC motor actuator '%s' (id = %d)",
name.c_str(), id);
}
// check muscle parameters
for (int i=0; i < 2; i++) {
// select gain or bias
test/engine/engine_derivative_test.cc
+117
View File
@@ -1308,6 +1308,123 @@ TEST_F(DerivativeTest, ActearlyDerivative) {
mj_deleteModel(m);
}
// verify stateful DC motor derivative matches analytical formula
TEST_F(DerivativeTest, DCMotorStatefulDerivative) {
static constexpr char xml[] = R"(
<mujoco>
<option timestep="0.002"/>
<worldbody>
<body>
<joint name="j" type="slide"/>
<geom type="sphere" size="0.1" mass="1"/>
</body>
</worldbody>
<actuator>
<dcmotor name="dc" joint="j" motorconst="2.0" resistance="0.5"
inductance="0 0.001" input="position" controller="10 0 5"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
mjData* d = mj_makeData(m);
// set nonzero velocity and ctrl
d->qvel[0] = 1.0;
d->ctrl[0] = 0.5;
// forward to compute act_dot, etc.
mj_forward(m, d);
// compute analytical derivatives
mjd_smooth_vel(m, d, /* flg_bias = */ 1);
// extract diagonal of qDeriv
mjtNum qDeriv_diag = d->qDeriv[m->D_rowadr[0] + m->D_rownnz[0] - 1];
// expected: K*(dVdw - K)*(1 - exp(-h/te))/R
// with K=2, R=0.5, te=0.001, h=0.002, kd=5, dVdw=-5
mjtNum K = 2.0, R = 0.5, te = 0.001, h = 0.002, kd = 5.0;
mjtNum expected = K * (-kd - K) * (1 - mju_exp(-h / te)) / R;
EXPECT_NEAR(qDeriv_diag, expected, 1e-10)
<< "stateful DC motor derivative should match analytical formula";
mj_deleteData(d);
mj_deleteModel(m);
}
// verify that stateful DC motor derivative converges to stateless as te -> 0
TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) {
// stateless DC motor with position controller
static constexpr char xml_stateless[] = R"(
<mujoco>
<option timestep="0.002"/>
<worldbody>
<body>
<joint name="j" type="slide"/>
<geom type="sphere" size="0.1" mass="1"/>
</body>
</worldbody>
<actuator>
<dcmotor name="dc" joint="j" motorconst="1.0" resistance="1.0"
input="position" controller="10 0 5"/>
</actuator>
</mujoco>
)";
// stateful DC motor with very small te
static constexpr char xml_stateful[] = R"(
<mujoco>
<option timestep="0.002"/>
<worldbody>
<body>
<joint name="j" type="slide"/>
<geom type="sphere" size="0.1" mass="1"/>
</body>
</worldbody>
<actuator>
<dcmotor name="dc" joint="j" motorconst="1.0" resistance="1.0"
inductance="0 1e-8" input="position" controller="10 0 5"/>
</actuator>
</mujoco>
)";
char error[1024];
mjModel* m_sl = LoadModelFromString(xml_stateless, error, sizeof(error));
ASSERT_THAT(m_sl, NotNull()) << error;
mjData* d_sl = mj_makeData(m_sl);
mjModel* m_sf = LoadModelFromString(xml_stateful, error, sizeof(error));
ASSERT_THAT(m_sf, NotNull()) << error;
mjData* d_sf = mj_makeData(m_sf);
// set identical state
d_sl->qvel[0] = d_sf->qvel[0] = 1.0;
d_sl->ctrl[0] = d_sf->ctrl[0] = 0.5;
// forward and compute derivatives
mj_forward(m_sl, d_sl);
mj_forward(m_sf, d_sf);
mjd_smooth_vel(m_sl, d_sl, 1);
mjd_smooth_vel(m_sf, d_sf, 1);
// extract diagonals
mjtNum diag_sl = d_sl->qDeriv[m_sl->D_rowadr[0] + m_sl->D_rownnz[0] - 1];
mjtNum diag_sf = d_sf->qDeriv[m_sf->D_rowadr[0] + m_sf->D_rownnz[0] - 1];
EXPECT_NEAR(diag_sf, diag_sl, 1e-6)
<< "stateful derivative should converge to stateless as te -> 0";
mj_deleteData(d_sf);
mj_deleteModel(m_sf);
mj_deleteData(d_sl);
mj_deleteModel(m_sl);
}
// Utility: Rotate flex grid
void RotateFlexGrid(mjModel* model, mjData* data, const char* flex_name,
double angle) {
test/engine/engine_forward_test.cc
+11 -3
View File
@@ -1805,8 +1805,12 @@ TEST_F(DCMotorTest, StatefulPositionWithCurrentMode) {
// di/dt = (1.485 - 0.0125 - 0.5) / 0.5 = 0.9725 / 0.5 = 1.945
EXPECT_NEAR(data->act_dot[adr+2], 1.945, MjTol(1e-12, 1e-5));
// Force is just K * current since current is stateful
EXPECT_NEAR(data->actuator_force[0], 0.05 * 0.5, MjTol(1e-12, 1e-5));
// Force is K * next_activation (actearly is always on for DC motors)
// Inline mj_nextActivation for te = 0.5
mjtNum te = 0.5;
mjtNum h = model->opt.timestep;
mjtNum next_i = 0.5 + data->act_dot[adr+2] * te * (1 - mju_exp(-h / te));
EXPECT_NEAR(data->actuator_force[0], 0.05 * next_i, MjTol(1e-12, 1e-5));
mj_deleteData(data);
mj_deleteModel(model);
@@ -1902,7 +1906,11 @@ TEST_F(DCMotorTest, CurrentPlusThermal) {
data->ctrl[0] = V;
mj_forward(model, data);
EXPECT_NEAR(data->actuator_force[0], K * current, MjTol(1e-12, 1e-5));
// Force uses next_activation (actearly is always on for DC motors)
// Inline mj_nextActivation for te = 0.01 / R = 0.005
mjtNum h = model->opt.timestep;
mjtNum next_i = current + data->act_dot[adr+1] * te * (1 - mju_exp(-h / te));
EXPECT_NEAR(data->actuator_force[0], K * next_i, MjTol(1e-12, 1e-5));
double R_hot = R * (1 + 0.004 * dT);
double T_dot = (R_hot * current * current - dT / RT) / C;
test/engine/testdata/derivative/dcmotor.xml
Vendored
+26
View File
@@ -16,6 +16,18 @@
<joint name="joint4"/>
<geom size=".03"/>
</body>
<body name="motor5" pos="0 0.5 0">
<joint name="slide5" type="slide" axis="0 0 1"/>
<geom size=".03"/>
</body>
<body name="motor6" pos="0 0.6 0">
<joint name="slide6" type="slide" axis="0 0 1"/>
<geom size=".03"/>
</body>
<body name="motor7" pos="0 0.7 0">
<joint name="slide7" type="slide" axis="0 0 1"/>
<geom size=".03"/>
</body>
</worldbody>
<actuator>
@@ -31,5 +43,19 @@
<!-- DC motor with LuGre friction (sigma1 micro-damping) -->
<dcmotor name="dc_lugre" joint="joint4" motorconst="0.05" resistance="2.0"
damping="0.001" lugre="1e4 100 0.005 0.008 0.1"/>
<!-- Stateful current, voltage mode (back-EMF only through act_dot) -->
<dcmotor name="dc_stateful_v" joint="slide5"
motorconst="2.0" resistance="0.5" inductance="0 0.001"/>
<!-- Stateful current, position mode (controller + back-EMF through act_dot) -->
<dcmotor name="dc_stateful_pos" joint="slide6"
motorconst="1.0" resistance="1.0" inductance="0 0.001"
input="position" controller="10 0 5"/>
<!-- Stateful current, velocity mode -->
<dcmotor name="dc_stateful_vel" joint="slide7"
motorconst="1.0" resistance="1.0" inductance="0 0.001"
input="velocity" controller="5 0"/>
</actuator>
</mujoco>