Add private function mj_makeM.

PiperOrigin-RevId: 757689756
Change-Id: Ie8bb49d9f18a95da2e3fd8622e30f9266a1ced98

src/engine/engine_core_smooth.c

@@ -1471,7 +1471,6 @@ void mj_transmission(const mjModel* m, mjData* d) {
 
 // add tendon armature to qM
 void mj_tendonArmature(const mjModel* m, mjData* d) {
-  TM_START;
   int nv = m->nv, ntendon = m->ntendon, issparse = mj_isSparse(m);
 
   for (int k=0; k < ntendon; k++) {
@@ -1521,14 +1520,12 @@ void mj_tendonArmature(const mjModel* m, mjData* d) {
       }
     }
   }
-  TM_END(mjTIMER_POS_INERTIA);
 }
 
 
 
 // composite rigid body inertia algorithm
 void mj_crb(const mjModel* m, mjData* d) {
-  TM_START;
   mjtNum buf[6];
   mjtNum* crb = d->crb;
   int last_body = m->nbody - 1, nv = m->nv;
@@ -1574,6 +1571,14 @@ void mj_crb(const mjModel* m, mjData* d) {
       d->qM[Madr_ij++] += mju_dot(d->cdof+6*j, buf, 6);
     }
   }
+}
+
+
+
+void mj_makeM(const mjModel* m, mjData* d) {
+  TM_START;
+  mj_crb(m, d);
+  mj_tendonArmature(m, d);
   TM_END(mjTIMER_POS_INERTIA);
 }
 

src/engine/engine_core_smooth.h

@@ -54,6 +54,9 @@ MJAPI void mj_crb(const mjModel* m, mjData* d);
 // add tendon armature to qM
 MJAPI void mj_tendonArmature(const mjModel* m, mjData* d);
 
+// make inertia matrix
+void mj_makeM(const mjModel* m, mjData* d);
+
 // sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd  (legacy implementation)
 MJAPI void mj_factorI_legacy(const mjModel* m, mjData* d, const mjtNum* M,
                              mjtNum* qLD, mjtNum* qLDiagInv);

src/engine/engine_forward.c

@@ -114,16 +114,15 @@ typedef struct mjFwdPositionArgs_ mjFwdPositionArgs;
 // wrapper for mj_crb and mj_factorM
 void* mj_inertialThreaded(void* args) {
   mjFwdPositionArgs* forward_args = (mjFwdPositionArgs*) args;
-  mj_crb(forward_args->m, forward_args->d);             // timed internally (POS_INERTIA)
+  mj_makeM(forward_args->m, forward_args->d);
-  mj_tendonArmature(forward_args->m, forward_args->d);  // timed internally (POS_INERTIA)
+  mj_factorM(forward_args->m, forward_args->d);
-  mj_factorM(forward_args->m, forward_args->d);         // timed internally (POS_INERTIA)
   return NULL;
 }
 
 // wrapper for mj_collision
 void* mj_collisionThreaded(void* args) {
   mjFwdPositionArgs* forward_args = (mjFwdPositionArgs*) args;
-  mj_collision(forward_args->m, forward_args->d);   // timed internally (POS_COLLISION)
+  mj_collision(forward_args->m, forward_args->d);
   return NULL;
 }
 
@@ -143,10 +142,12 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
 
   // no threadpool: inertia and collision on main thread
   if (!d->threadpool) {
-    mj_crb(m, d);             // timed internally (POS_INERTIA)
+    // inertia, timed internally (POS_INERTIA)
-    mj_tendonArmature(m, d);  // timed internally (POS_INERTIA)
+    mj_makeM(m, d);
-    mj_factorM(m, d);         // timed internally (POS_INERTIA)
+    mj_factorM(m, d);
-    mj_collision(m, d);       // timed internally (POS_COLLISION)
+
+    // collision, timed internally (POS_COLLISION)
+    mj_collision(m, d);
   }
 
   // have threadpool: inertia and collision on separate threads

src/engine/engine_inverse.c

@@ -46,8 +46,7 @@ void mj_invPosition(const mjModel* m, mjData* d) {
   mj_tendon(m, d);
   TM_END(mjTIMER_POS_KINEMATICS);
 
-  mj_crb(m, d);             // timed internally (POS_INERTIA)
+  mj_makeM(m, d);           // timed internally (POS_INERTIA)
-  mj_tendonArmature(m, d);  // timed internally (POS_INERTIA)
   mj_factorM(m, d);         // timed internally (POS_INERTIA)
 
   mj_collision(m, d);  // timed internally (POS_COLLISION)

src/engine/engine_setconst.c

@@ -103,8 +103,7 @@ static void set0(mjModel* m, mjData* d) {
 
   // run remaining computations
   mj_tendon(m, d);
-  mj_crb(m, d);
+  mj_makeM(m, d);
-  mj_tendonArmature(m, d);
   mj_factorM(m, d);
   mj_flex(m, d);
   mj_transmission(m, d);