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

PiperOrigin-RevId: 957597256
Change-Id: I10a1c85fb49f00d60aa65d0cbdedc110423adb95

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

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"(
+  <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>