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

Skip disabled actuators in mjd_actuator_vel.

Browse Source 
Fixes a bug where the derivative of the actuator force with respect to the generalized velocities (used in implicit and implicitfast integrators) was failing to take into account disabled actuators.

Fixes #1838

PiperOrigin-RevId: 657193960
Change-Id: Id8c0ab863a39e2a01cd2703774f460e9731b4807
This commit is contained in:
Yuval Tassa
2024-07-29 07:30:54 -07:00
committed by Copybara-Service
parent 3f0749a0f7
commit a4bd2bec0a
4 changed files with 84 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_derivative_test.cc
+64
View File
@@ -159,6 +159,70 @@ TEST_F(DerivativeTest, SmoothDvel) {
}
}
// disabled actuators do not contribute to d_qfrc_actuator/d_qvel
TEST_F(DerivativeTest, DisabledActuators) {
// model with only a position actuator
static constexpr char xml1[] = R"(
<mujoco>
<option integrator="implicitfast"/>
<worldbody>
<body>
<joint name="joint" type="slide"/>
<geom size=".1"/>
</body>
</worldbody>
<actuator>
<position joint="joint" group="1" kp="2000" kv="200"/>
</actuator>
</mujoco>
)";
mjModel* m1 = LoadModelFromString(xml1);
mjData* d1 = mj_makeData(m1);
d1->ctrl[0] = 6;
while (d1->time < 1)
mj_step(m1, d1);
// model with a position actuator and an intvelocity actuator
static constexpr char xml2[] = R"(
<mujoco>
<option integrator="implicitfast" actuatorgroupdisable="2"/>
<worldbody>
<body>
<joint name="joint" type="slide"/>
<geom size=".1"/>
</body>
</worldbody>
<actuator>
<position joint="joint" group="1" kp="2000" kv="200"/>
<intvelocity joint="joint" group="2" kp="2000" kv="200" actrange="-6 6"/>
</actuator>
</mujoco>
)";
mjModel* m2 = LoadModelFromString(xml2);
mjData* d2 = mj_makeData(m2);
d2->ctrl[0] = 6;
d2->ctrl[1] = 6;
while (d2->time < 1)
mj_step(m2, d2);
// expect same qvel in both models
EXPECT_EQ(d1->qvel[0], d2->qvel[0]);
mj_deleteData(d2);
mj_deleteModel(m2);
mj_deleteData(d1);
mj_deleteModel(m1);
}
// compare analytic and fin-diff d_qfrc_passive/d_qvel
TEST_F(DerivativeTest, PassiveDvel) {
for (const char* local_path : {kTumblingThinObjectPath,