From f95d50c12f0f55efa484fb901347157615855dae Mon Sep 17 00:00:00 2001 From: Giuseppe Sensolini Date: Wed, 29 Jul 2026 17:30:22 +0200 Subject: [PATCH 1/2] Add regression test for LuGre bristle velocity indexing. The DC motor's LuGre bristle state must integrate the velocity of its own transmission, so actuator ordering cannot affect it. The test places a multi-output SO3 actuator before the DC motor, making the motor's actuator id and output address diverge, and requires the bristle state to match the motor-first ordering. Currently fails: the exact ZOH update in mj_nextActivation reads actuator_velocity[actuator_id] instead of the motor's own actuator_velocity[outadr], so the bristle integrates the SO3 actuator's velocity. --- test/engine/engine_forward_test.cc | 70 ++++++++++++++++++++++++++++++ 1 file changed, 70 insertions(+) diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 43b790b8..a44e8e2b 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -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"( + + + + + + + + + + + + + + + + + )"; + static constexpr char xml_so3_first[] = R"( + + + + + + + + + + + + + + + + + )"; + 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"( From 4c2653273c0159ff6ee4b1c0c27206623e1d8d0a Mon Sep 17 00:00:00 2001 From: Giuseppe Sensolini Date: Wed, 29 Jul 2026 17:36:05 +0200 Subject: [PATCH 2/2] Fix actuator_velocity indexing in the LuGre bristle update. mj_nextActivation indexed actuator_velocity with the actuator id, but the array is laid out by output address. The two coincide only when all preceding actuators are single-output, so a DC motor placed after a multi-output SO3 actuator integrated its bristle state with another actuator's velocity, while act_dot (computed in mj_fwdActuation with the correct velocity) disagreed with the actual evolution of the state. Fixes DCMotorTest.LuGreBristleVelocityOrderInvariance. --- src/engine/engine_support.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index b95aad8a..a58934f2 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -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