Implement midpoint integrator for free bodies.

PiperOrigin-RevId: 899043541
Change-Id: I0bb38f6ad94e189b45ab16777a04ad6fefc6adf7

src/engine/engine_forward.h

@@ -18,6 +18,7 @@
 #include <mujoco/mjdata.h>
 #include <mujoco/mjexport.h>
 #include <mujoco/mjmodel.h>
+#include <mujoco/mjtnum.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -46,7 +47,6 @@ MJAPI void mj_forward(const mjModel* m, mjData* d);
 MJAPI void mj_forwardSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor);
 
 
-
 //-------------------------------- integrators -----------------------------------------------------
 
 // Runge Kutta explicit order-N integrator
@@ -64,6 +64,13 @@ MJAPI void mj_implicit(const mjModel *m, mjData *d);
 // fully implicit in velocity, possibly skipping factorization
 MJAPI void mj_implicitSkip(const mjModel *m, mjData *d, int skipfactor);
 
+// implicit midpoint integration for 6 DOFs (translation + rotation) of a single body
+// returns number of Newton iterations
+MJAPI int mj_midpoint(mjtNum mass, const mjtNum inertia[3], const mjtNum ipos[3],
+                      const mjtNum iquat[4], const mjtNum xquat[4], const mjtNum qvel[6],
+                      const mjtNum qfrc[6], const mjtNum gravity[3], mjtNum h,
+                      mjtNum qvel_new[6]);
+
 
 //-------------------------------- solver components -----------------------------------------------