Improvements to timing.

- Fixed bug in timing of kinematics, was under-reported.
- Moved timing of inertia and collision into respective functions, in preparation for multi-threading.
- Detailed profiling in `testspeed` is always on, removed command-line flag.
- Improved `testspeed` self-documentation.

PiperOrigin-RevId: 571989548
Change-Id: If0e3cc17f6f3511b98fc36339f406a19968a9361

src/engine/engine_collision_driver.c

@@ -28,6 +28,7 @@
 #include "engine/engine_core_constraint.h"
 #include "engine/engine_crossplatform.h"
 #include "engine/engine_io.h"
+#include "engine/engine_macro.h"
 #include "engine/engine_support.h"
 #include "engine/engine_util_blas.h"
 #include "engine/engine_util_errmem.h"
@@ -387,6 +388,7 @@ quicksortfunc(contactcompare, context, el1, el2) {
 }
 
 void mj_collision(const mjModel* m, mjData* d) {
+  TM_START;
   int g1, g2, merged, b1 = 0, b2 = 0, exadr = 0, pairadr = 0, startadr;
   int nexclude = m->nexclude, npair = m->npair;
   int *broadphasepair = 0;
@@ -492,6 +494,7 @@ void mj_collision(const mjModel* m, mjData* d) {
   }
 
   mj_freeStack(d);
+  TM_END(mjTIMER_POS_COLLISION);
 }
 
 

src/engine/engine_core_smooth.c

@@ -23,6 +23,7 @@
 #include "engine/engine_core_constraint.h"
 #include "engine/engine_crossplatform.h"
 #include "engine/engine_io.h"
+#include "engine/engine_macro.h"
 #include "engine/engine_support.h"
 #include "engine/engine_util_blas.h"
 #include "engine/engine_util_errmem.h"
@@ -968,6 +969,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
 
 // 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;
@@ -1013,6 +1015,7 @@ void mj_crb(const mjModel* m, mjData* d) {
       d->qM[Madr_ij++] += mju_dot(d->cdof+6*j, buf, 6);
     }
   }
+  TM_END(mjTIMER_POS_INERTIA);
 }
 
 
@@ -1086,7 +1089,9 @@ void mj_factorI(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD, mjtNu
 
 // sparse L'*D*L factorizaton of the inertia matrix M, assumed spd
 void mj_factorM(const mjModel* m, mjData* d) {
+  TM_START;
   mj_factorI(m, d, d->qM, d->qLD, d->qLDiagInv, d->qLDiagSqrtInv);
+  TM_ADD(mjTIMER_POS_INERTIA);
 }
 
 

src/engine/engine_forward.c

@@ -106,14 +106,10 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
   mj_tendon(m, d);
   TM_END(mjTIMER_POS_KINEMATICS);
 
-  TM_RESTART;
-  mj_crb(m, d);
-  mj_factorM(m, d);
-  TM_END(mjTIMER_POS_INERTIA);
+  mj_crb(m, d);        // timed internally (POS_INERTIA)
+  mj_factorM(m, d);    // timed internally (POS_INERTIA)
 
-  TM_RESTART;
-  mj_collision(m, d);
-  TM_END(mjTIMER_POS_COLLISION);
+  mj_collision(m, d);  // timed internally (POS_COLLISION)
 
   TM_RESTART;
   mj_makeConstraint(m, d);
@@ -124,7 +120,7 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
 
   TM_RESTART;
   mj_transmission(m, d);
-  TM_END(mjTIMER_POS_KINEMATICS);
+  TM_ADD(mjTIMER_POS_KINEMATICS);
 
   TM_RESTART;
   mj_projectConstraint(m, d);

src/engine/engine_inverse.c

@@ -41,22 +41,21 @@ void mj_invPosition(const mjModel* m, mjData* d) {
   mj_comPos(m, d);
   mj_camlight(m, d);
   mj_tendon(m, d);
-  mj_transmission(m, d);
   TM_END(mjTIMER_POS_KINEMATICS);
 
-  TM_RESTART;
-  mj_crb(m, d);
-  mj_factorM(m, d);
-  TM_END(mjTIMER_POS_INERTIA);
+  mj_crb(m, d);        // timed internally (POS_INERTIA)
+  mj_factorM(m, d);    // timed internally (POS_INERTIA)
 
-  TM_RESTART;
-  mj_collision(m, d);
-  TM_END(mjTIMER_POS_COLLISION);
+  mj_collision(m, d);  // timed internally (POS_COLLISION)
 
   TM_RESTART;
   mj_makeConstraint(m, d);
   TM_END(mjTIMER_POS_MAKE);
 
+  TM_RESTART;
+  mj_transmission(m, d);
+  TM_ADD(mjTIMER_POS_KINEMATICS);
+
   TM_END1(mjTIMER_POSITION);
 }
 

src/engine/engine_macro.h

@@ -35,6 +35,7 @@
 #define TM_START mjtNum _tm = (mjcb_time ? mjcb_time() : 0);
 #define TM_RESTART _tm = (mjcb_time ? mjcb_time() : 0);
 #define TM_END(i) {d->timer[i].duration += ((mjcb_time ? mjcb_time() : 0) - _tm); d->timer[i].number++;}
+#define TM_ADD(i) {d->timer[i].duration += ((mjcb_time ? mjcb_time() : 0) - _tm);}
 #define TM_START1 mjtNum _tm1 = (mjcb_time ? mjcb_time() : 0);
 #define TM_END1(i) {d->timer[i].duration += ((mjcb_time ? mjcb_time() : 0) - _tm1); d->timer[i].number++;}