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
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) {