Enable activation clamping when using implicit integrator.

- Introduced private function `mj_advance()` as single point of state & time advancement.

PiperOrigin-RevId: 456525969
Change-Id: Iae17217e305c7baf07cb5ecd1e1b84b936421097

src/engine/engine_forward.c

@@ -453,6 +453,40 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
 
 //-------------------------- integrators  ----------------------------------------------------------
 
+// advance state and time given activation derivatives, acceleration, and optional velocity
+static void mj_advance(const mjModel* m, mjData* d,
+                       const mjtNum* act_dot, const mjtNum* qacc, const mjtNum* qvel) {
+  // advance activations and clamp
+  if (m->na) {
+    mju_addToScl(d->act, act_dot, m->opt.timestep, m->na);
+
+    // clamp activations
+    for (int i=0; i<m->na; i++) {
+      int iu = i + m->nu - m->na;
+      if (m->actuator_actlimited[iu]) {
+        mjtNum min = m->actuator_actrange[2*iu];
+        mjtNum max = m->actuator_actrange[2*iu+1];
+        if (d->act[i]<min) {
+          d->act[i] = min;
+        } else if (d->act[i]>max) {
+          d->act[i] = max;
+        }
+      }
+    }
+  }
+
+  // advance velocities
+  mju_addToScl(d->qvel, qacc, m->opt.timestep, m->nv);
+
+  // advance positions with qvel if given, d->qvel otherwise (semi-implicit)
+  mj_integratePos(m, d->qpos, qvel ? qvel : d->qvel, m->opt.timestep);
+
+  // advance time
+  d->time += m->opt.timestep;
+}
+
+
+
 // Euler integrator, semi-implicit in velocity
 void mj_Euler(const mjModel* m, mjData* d) {
   int i, nv = m->nv, nM = m->nM;
@@ -473,7 +507,7 @@ void mj_Euler(const mjModel* m, mjData* d) {
 
   // no damping: explicit velocity integration
   if (i>=nv) {
-    mju_addToScl(d->qvel, d->qacc, m->opt.timestep, nv);
+    mju_copy(qacc, d->qacc, nv);
   }
 
   // damping: integrate implicitly
@@ -496,9 +530,6 @@ void mj_Euler(const mjModel* m, mjData* d) {
     mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv);
     mj_solveM(m, d, qacc, qfrc, 1);
 
-    // integrate velocity
-    mju_addToScl(d->qvel, qacc, m->opt.timestep, nv);
-
     // restore M and factorization
     mju_copy(d->qM, saveM, nM);
     mju_copy(d->qLD, saveLD, nM);
@@ -506,30 +537,8 @@ void mj_Euler(const mjModel* m, mjData* d) {
     mju_copy(d->qLDiagSqrtInv, saveLDiagSqrtInv, nv);
   }
 
-  // update act
-  if (m->na) {
-    mju_addToScl(d->act, d->act_dot, m->opt.timestep, m->na);
-
-    // clamp activations
-    for (i=0; i<m->na; i++) {
-      int iu = i + m->nu - m->na;
-      if (m->actuator_actlimited[iu]) {
-        mjtNum min = m->actuator_actrange[2*iu];
-        mjtNum max = m->actuator_actrange[2*iu+1];
-        if (d->act[i]<min) {
-          d->act[i] = min;
-        } else if (d->act[i]>max) {
-          d->act[i] = max;
-        }
-      }
-    }
-  }
-
-  // update qpos using new qvel
-  mj_integratePos(m, d->qpos, d->qvel, m->opt.timestep);
-
-  // advance time
-  d->time += m->opt.timestep;
+  // advance state and time
+  mj_advance(m, d, d->act_dot, qacc, NULL);
 
   mjFREESTACK;
 }
@@ -628,30 +637,14 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
     mju_addToScl(dX+nv, F[j], B[j], nv+na);
   }
 
-  // compute Xfinal
-  d->time = time + h;
+  // reset state and time
+  d->time = time;
   mju_copy(d->qpos, X[0], nq);
   mju_copy(d->qvel, X[0]+nq, nv);
   mju_copy(d->act, X[0]+nq+nv, na);
-  mj_integratePos(m, d->qpos, dX, h);
-  mju_addToScl(d->qvel, dX+nv, h, nv);
-  if (na) {
-    mju_addToScl(d->act, dX+2*nv, h, na);
 
-    // clamp activations
-    for (int i=0; i<m->na; i++) {
-      int iu = i + m->nu - m->na;
-      if (m->actuator_actlimited[iu]) {
-        mjtNum min = m->actuator_actrange[2*iu];
-        mjtNum max = m->actuator_actrange[2*iu+1];
-        if (d->act[i]<min) {
-          d->act[i] = min;
-        } else if (d->act[i]>max) {
-          d->act[i] = max;
-        }
-      }
-    }
-  }
+  // advance state and time
+  mj_advance(m, d, dX+2*nv, dX+nv, dX);
 
   mjFREESTACK;
 }
@@ -687,19 +680,8 @@ void mj_implicit(const mjModel *m, mjData *d) {
   // solve for qacc: (qM - dt*qDeriv) * qacc = qfrc
   mju_solveLUSparse(qacc, d->qLU, qfrc, nv, d->D_rownnz, d->D_rowadr, d->D_colind);
 
-  // update qvel
-  mju_addToScl(d->qvel, qacc, m->opt.timestep, nv);
-
-  // update act
-  if (m->na) {
-    mju_addToScl(d->act, d->act_dot, m->opt.timestep, m->na);
-  }
-
-  // update qpos using new qvel
-  mj_integratePos(m, d->qpos, d->qvel, m->opt.timestep);
-
-  // advance time
-  d->time += m->opt.timestep;
+  // advance state and time
+  mj_advance(m, d, d->act_dot, qacc, NULL);
 
   mjFREESTACK
 }

test/engine/engine_forward_test.cc

@@ -38,11 +38,17 @@ static const char* const kDampedActuatorsPath =
 using ::testing::Pointwise;
 using ::testing::DoubleNear;
 using ::testing::Ne;
-using ForwardTest = MujocoTest;
 
 // --------------------------- activation limits -------------------------------
 
-TEST_F(ForwardTest, ActLimited) {
+struct ActLimitedTestCase {
+  std::string test_name;
+  mjtIntegrator integrator;
+};
+
+using ParametrizedForwardTest = ::testing::TestWithParam<ActLimitedTestCase>;
+
+TEST_P(ParametrizedForwardTest, ActLimited) {
   static constexpr char xml[] = R"(
   <mujoco>
     <option timestep="0.01"/>
@@ -63,15 +69,17 @@ TEST_F(ForwardTest, ActLimited) {
   mjModel* model = LoadModelFromString(xml);
   mjData* data = mj_makeData(model);
 
+  model->opt.integrator = GetParam().integrator;
+
   data->ctrl[0] = 1.0;
   // integrating up from 0, we will hit the clamp after 99 steps
   for (int i=0; i<200; i++) {
     mj_step(model, data);
     // always greater than lower bound
-    ASSERT_GT(data->act[0], -1);
+    EXPECT_GT(data->act[0], -1);
     // after 99 steps we hit the upper bound
-    if (i < 99) ASSERT_LT(data->act[0], 1);
-    if (i >= 99) ASSERT_EQ(data->act[0], 1);
+    if (i < 99) EXPECT_LT(data->act[0], 1);
+    if (i >= 99) EXPECT_EQ(data->act[0], 1);
   }
 
   data->ctrl[0] = -1.0;
@@ -79,18 +87,31 @@ TEST_F(ForwardTest, ActLimited) {
   for (int i=0; i<300; i++) {
     mj_step(model, data);
     // always smaller than upper bound
-    ASSERT_LT(data->act[0], model->actuator_actrange[1]);
+    EXPECT_LT(data->act[0], model->actuator_actrange[1]);
     // after 199 steps we hit the lower bound
-    if (i < 199) ASSERT_GT(data->act[0], model->actuator_actrange[0]);
-    if (i >= 199) ASSERT_EQ(data->act[0], model->actuator_actrange[0]);
+    if (i < 199) EXPECT_GT(data->act[0], model->actuator_actrange[0]);
+    if (i >= 199) EXPECT_EQ(data->act[0], model->actuator_actrange[0]);
   }
 
   mj_deleteData(data);
   mj_deleteModel(model);
 }
 
+INSTANTIATE_TEST_SUITE_P(
+    ParametrizedForwardTest, ParametrizedForwardTest,
+    testing::ValuesIn<ActLimitedTestCase>({
+        {"Euler", mjINT_EULER},
+        {"Implicit", mjINT_IMPLICIT},
+        {"RK4", mjINT_RK4},
+    }),
+    [](const testing::TestParamInfo<ParametrizedForwardTest::ParamType>& info) {
+      return info.param.test_name;
+    });
+
 // --------------------------- damping actuator --------------------------------
 
+using ForwardTest = MujocoTest;
+
 TEST_F(ForwardTest, DamperDampens) {
   static constexpr char xml[] = R"(
   <mujoco>

test/testdata/model.xml

@@ -113,10 +113,10 @@
     <motor tendon="fixed" gear="100"/>
     <motor tendon="spatial" gear="10"/>
     <motor joint="hipy_2" gear="100"/>
-    <position joint="hipz_0" kp="100"/>
     <position joint="hipz_1" kp="100"/>
     <position joint="hipz_2" kp="100"/>
     <velocity joint="wheel_2" kv="1"/>
+    <intvelocity joint="hipz_0" kp="100" actrange="-1 1"/>
     <general site="wheel_0" gear="0 0 0 0 10 0" dyntype="filter" dynprm="1"/>
     <general joint="wheel_1" biastype="affine" dyntype="integrator" dynprm="1" biasprm="0 -1"/>
   </actuator>