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

Merge pull request #3445 from giusenso:fix/actuator-velocity-index

Browse Source 
PiperOrigin-RevId: 957597256
Change-Id: I10a1c85fb49f00d60aa65d0cbdedc110423adb95
This commit is contained in:
Copybara-Service
2026-08-01 05:10:55 -07:00
parent 279df98cd0 a670a6ac10
commit 2bb152b77b
2 changed files with 71 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_support.c
+1 -1
View File
@@ -735,7 +735,7 @@ mjtNum mj_nextActivation(const mjModel* m, const mjData* d,
mjtNum F_S = biasprm[4]; // static friction
mjtNum v_S = biasprm[5]; // Stribeck velocity
mjtNum sigma0 = dynprm[5]; // bristle stiffness
mjtNum velocity = d->actuator_velocity[actuator_id];
mjtNum velocity = d->actuator_velocity[m->actuator_outadr[actuator_id]];
mjtNum g = mj_lugreStribeck(velocity, F_C, F_S, v_S);
// ZOH exact ZOH integration: z(h) = exp(ah)*z(0) + ((exp(ah)-1)/a)*v
test/engine/engine_forward_test.cc
+70
View File
@@ -1726,6 +1726,76 @@ TEST_F(DCMotorTest, LuGreViscousFriction) {
MjTol(1e-12, 1e-5));
}
// the LuGre bristle must integrate the velocity of its own transmission:
// placing a multi-output (SO3) actuator before the DC motor, so that the
// motor's actuator id and output address diverge, must not change the
// bristle dynamics
TEST_F(DCMotorTest, LuGreBristleVelocityOrderInvariance) {
static constexpr char xml_dc_first[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="hinge"/>
<geom size="1"/>
</body>
<body pos="3 0 0">
<joint name="ball" type="ball"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<dcmotor name="dc" joint="hinge" motorconst="0.05" resistance="2.0"
lugre="100 0.5 0.5 0.8 0.5"/>
<orientation name="so3" joint="ball" kp="1"/>
</actuator>
</mujoco>
)";
static constexpr char xml_so3_first[] = R"(
<mujoco>
<worldbody>
<body>
<joint name="hinge"/>
<geom size="1"/>
</body>
<body pos="3 0 0">
<joint name="ball" type="ball"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<orientation name="so3" joint="ball" kp="1"/>
<dcmotor name="dc" joint="hinge" motorconst="0.05" resistance="2.0"
lugre="100 0.5 0.5 0.8 0.5"/>
</actuator>
</mujoco>
)";
char error[1024];
// reference: DC motor first, actuator id == output address
MjModelPtr model = LoadModelFromString(xml_dc_first, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
MjDataPtr data = MakeData(model);
int dc = mj_name2id(model.get(), mjOBJ_ACTUATOR, "dc");
data->qvel[model->jnt_dofadr[mj_name2id(model.get(), mjOBJ_JOINT, "hinge")]] = 1;
mj_step(model.get(), data.get());
double z_dc_first = data->act[model->actuator_actadr[dc]];
// reordered: the SO3 actuator has 3 outputs, so the DC motor now has
// actuator id 1 but output address 3
model = LoadModelFromString(xml_so3_first, error, sizeof(error));
ASSERT_THAT(model.get(), NotNull()) << error;
data = MakeData(model);
dc = mj_name2id(model.get(), mjOBJ_ACTUATOR, "dc");
ASSERT_EQ(model->actuator_outadr[dc], 3);
data->qvel[model->jnt_dofadr[mj_name2id(model.get(), mjOBJ_JOINT, "hinge")]] = 1;
mj_step(model.get(), data.get());
double z_so3_first = data->act[model->actuator_actadr[dc]];
// the bristle state saw the same spinning hinge in both models
EXPECT_NE(z_dc_first, 0);
EXPECT_MJTNUM_EQ(z_so3_first, z_dc_first);
}
TEST_F(DCMotorTest, ThermalRiseAndFall) {
static constexpr char xml[] = R"(
<mujoco>