Add implicit integrator.

Added analytic derivatives of smooth (unconstrained) dynamics forces, with respect to velocities:
  - Centripetal and Coriolis forces computed by the Recursive Newton-Euler algorithm.
  - Damping and fluid-drag passive forces.
  - Actuation forces.

A new implicit-in-velocity integrator is implemented using the analytic derivatives. This integrator lies between the Euler and Runge Kutta integrators in terms of both stability and computational cost.

PiperOrigin-RevId: 450377010
Change-Id: Ie192b441876c22e732fb749333926f296e0a09cc

test/engine/engine_forward_test.cc

@@ -26,8 +26,22 @@
 namespace mujoco {
 namespace {
 
+std::vector<mjtNum> AsVector(const mjtNum* array, int n) {
+  return std::vector<mjtNum>(array, array + n);
+}
+
+static const char* const kEnergyConservingPendulumPath =
+    "engine/testdata/derivative/energy_conserving_pendulum.xml";
+static const char* const kDampedActuatorsPath =
+    "engine/testdata/derivative/damped_actuators.xml";
+
+using ::testing::Pointwise;
+using ::testing::DoubleNear;
+using ::testing::Ne;
 using ForwardTest = MujocoTest;
 
+// --------------------------- activation limits -------------------------------
+
 TEST_F(ForwardTest, ActLimited) {
   static constexpr char xml[] = R"(
   <mujoco>
@@ -75,6 +89,129 @@ TEST_F(ForwardTest, ActLimited) {
   mj_deleteModel(model);
 }
 
+// --------------------------- implicit integrator -----------------------------
+
+using ImplicitIntegratorTest = MujocoTest;
+
+// Euler and implicit should be equivalent if there is only joint damping
+TEST_F(ImplicitIntegratorTest, EulerImplicitEqivalent) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <joint axis="1 0 0" damping="2"/>
+        <geom type="capsule" size=".01" fromto="0 0 0 0 .1 0"/>
+        <body pos="0 .1 0">
+          <joint axis="0 1 0" damping="1"/>
+          <geom type="capsule" size=".01" fromto="0 0 0 .1 0 0"/>
+        </body>
+      </body>
+    </worldbody>
+  </mujoco>
+  )";
+
+  mjModel* model = LoadModelFromString(xml);
+  mjData* data = mj_makeData(model);
+
+  // step 10 times with Euler, save copy of qpos as vector
+  for (int i=0; i<10; i++) {
+    mj_step(model, data);
+  }
+  std::vector<mjtNum> qposEuler = AsVector(data->qpos, model->nq);
+
+  // reset, step 10 times with implicit
+  mj_resetData(model, data);
+  model->opt.integrator = mjINT_IMPLICIT;
+  for (int i=0; i<10; i++) {
+    mj_step(model, data);
+  }
+
+  // expect qpos vectors to be numerically different
+  EXPECT_THAT(AsVector(data->qpos, model->nq), Pointwise(Ne(), qposEuler));
+
+  // expect qpos vectors to be similar to high precision
+  EXPECT_THAT(AsVector(data->qpos, model->nq),
+              Pointwise(DoubleNear(1e-14), qposEuler));
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+// Joint and actuator damping should integrate identically under implicit
+TEST_F(ImplicitIntegratorTest, JointActuatorEqivalent) {
+  const std::string xml_path = GetTestDataFilePath(kDampedActuatorsPath);
+  mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
+  mjData* data = mj_makeData(model);
+
+  // take 1000 steps with Euler
+  for (int i=0; i<1000; i++) {
+    mj_step(model, data);
+  }
+  // expect corresponding joint values to be significantly different
+  EXPECT_GT(fabs(data->qpos[0]-data->qpos[2]), 1e-4);
+  EXPECT_GT(fabs(data->qpos[1]-data->qpos[3]), 1e-4);
+
+  // reset, take 1000 steps with implicit
+  mj_resetData(model, data);
+  model->opt.integrator = mjINT_IMPLICIT;
+  for (int i=0; i<10; i++) {
+    mj_step(model, data);
+  }
+
+  // expect corresponding joint values to be insignificantly different
+  EXPECT_LT(fabs(data->qpos[0]-data->qpos[2]), 1e-16);
+  EXPECT_LT(fabs(data->qpos[1]-data->qpos[3]), 1e-16);
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+// Energy conservation: RungeKutta > implicit > Euler
+TEST_F(ImplicitIntegratorTest, EnergyConservation) {
+  const std::string xml_path =
+      GetTestDataFilePath(kEnergyConservingPendulumPath);
+  mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
+  mjData* data = mj_makeData(model);
+
+  const int nstep = 500;  // number of steps to take
+
+  // take nstep steps with Euler, measure energy (potential + kinetic)
+  model->opt.integrator = mjINT_EULER;
+  for (int i=0; i<nstep; i++) {
+    mj_step(model, data);
+  }
+  mjtNum energyEuler = data->energy[0] + data->energy[1];
+
+  // take nstep steps with implicit, measure energy
+  model->opt.integrator = mjINT_IMPLICIT;
+  mj_resetData(model, data);
+  for (int i=0; i<nstep; i++) {
+    mj_step(model, data);
+  }
+  mjtNum energyImplicit = data->energy[0] + data->energy[1];
+
+  // take nstep steps with 4th order Runge-Kutta, measure energy
+  model->opt.integrator = mjINT_RK4;
+  mj_resetData(model, data);
+  for (int i=0; i<nstep; i++) {
+    mj_step(model, data);
+  }
+  mjtNum energyRK4 = data->energy[0] + data->energy[1];
+
+  // energy was measured: expect all energies to be nonzero
+  EXPECT_NE(energyEuler, 0);
+  EXPECT_NE(energyImplicit, 0);
+  EXPECT_NE(energyRK4, 0);
+
+  // test conservation: perfectly conserved energy would remain 0.0
+  // expect RK4 to be better than implicit
+  EXPECT_LT(fabs(energyRK4), fabs(energyImplicit));
+  // expect implicit to be better than Euler
+  EXPECT_LT(fabs(energyImplicit), fabs(energyEuler));
+
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
 
 }  // namespace
 }  // namespace mujoco