From 18d9f1abb0d18220cc75322fcddf3edf6be91dcb Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 9 Oct 2023 11:13:59 -0700 Subject: [PATCH] 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 --- sample/testspeed.cc | 121 +++++++++++++++------------ src/engine/engine_collision_driver.c | 3 + src/engine/engine_core_smooth.c | 5 ++ src/engine/engine_forward.c | 12 +-- src/engine/engine_inverse.c | 15 ++-- src/engine/engine_macro.h | 1 + 6 files changed, 86 insertions(+), 71 deletions(-) diff --git a/sample/testspeed.cc b/sample/testspeed.cc index 8f7932ce..09d80a3d 100644 --- a/sample/testspeed.cc +++ b/sample/testspeed.cc @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -39,9 +40,10 @@ double simtime[maxthread]; // timer -std::chrono::system_clock::time_point tm_start; +std::chrono::steady_clock::time_point tm_start; mjtNum gettm(void) { - std::chrono::duration elapsed = std::chrono::system_clock::now() - tm_start; + std::chrono::duration elapsed; + elapsed = std::chrono::steady_clock::now() - tm_start; return elapsed.count(); } @@ -90,8 +92,8 @@ void simulate(int id, int nstep, mjtNum* ctrl) { // run and time double start = gettm(); - for (int i=0; ictrl, ctrl + i*m->nu, m->nu); // advance simulation @@ -111,7 +113,7 @@ void simulate(int id, int nstep, mjtNum* ctrl) { accuracy_mid[id] += 100; } } - simtime[id] = gettm() - start; + simtime[id] = 1e-6 * (gettm() - start); } @@ -119,12 +121,24 @@ void simulate(int id, int nstep, mjtNum* ctrl) { int main(int argc, char** argv) { // print help if arguments are missing - if (argc < 2 || argc > 7) { - return finish("\n Usage: testspeed modelfile [nstep nthread ctrlnoise profile npoolthread]\n"); + if (argc < 2 || argc > 6) { + return finish( + "\n" + "Usage: testspeed modelfile [nstep nthread ctrlnoise npoolthread]\n" + "\n" + " argument default semantic\n" + " -------- ------- --------\n" + " modelfile path to model (required)\n" + " nstep 10000 number of steps per rollout\n" + " nthread 1 number of threads for which to run parallel rollouts\n" + " ctrlnoise 0.01 scale of pseudo-random noise injected into actuators\n" + " npoolthread 1 number of threads in engine-internal threadpool\n" + "\n" + "Note: If the model has a keyframe named \"test\", it will be loaded prior to simulation\n"); } // read arguments - int nstep = 10000, nthread = 0, profile = 1, npoolthread = 0; + int nstep = 10000, nthread = 0, npoolthread = 0; // inject small noise by default, to avoid fixed contact state mjtNum ctrlnoise = 0.01; if (argc > 2 && (std::sscanf(argv[2], "%d", &nstep) != 1 || nstep <= 0)) { @@ -136,10 +150,7 @@ int main(int argc, char** argv) { if (argc > 4 && std::sscanf(argv[4], "%lf", &ctrlnoise) != 1) { return finish("Invalid ctrlnoise argument"); } - if (argc > 5 && std::sscanf(argv[5], "%d", &profile) != 1) { - return finish("Invalid profile argument"); - } - if (argc > 6 && std::sscanf(argv[6], "%d", &npoolthread) != 1) { + if (argc > 5 && std::sscanf(argv[5], "%d", &npoolthread) != 1) { return finish("Invalid npoolthread argument"); } @@ -148,7 +159,7 @@ int main(int argc, char** argv) { // clamp nthread to [1, maxthread] nthread = mjMAX(1, mjMIN(maxthread, nthread)); - npoolthread = mjMAX(0, mjMIN(maxthread, npoolthread)); + npoolthread = mjMAX(1, mjMIN(maxthread, npoolthread)); // get filename, determine file type std::string filename(argv[1]); @@ -167,36 +178,37 @@ int main(int argc, char** argv) { // make per-thread data int testkey = mj_name2id(m, mjOBJ_KEY, "test"); - for (int id=0; id 0) { + // reset to keyframe + if (testkey >= 0) { + mj_resetDataKeyframe(m, d[id], testkey); + } + + // make and bind threadpool + if (npoolthread > 1) { mjThreadPool* threadpool = mju_threadPoolCreate(npoolthread); mju_bindThreadPool(d[id], threadpool); } - // init to keyframe "test" if present - if (testkey>=0) { - mju_copy(d[id]->qpos, m->key_qpos + testkey*m->nq, m->nq); - mju_copy(d[id]->qvel, m->key_qvel + testkey*m->nv, m->nv); - mju_copy(d[id]->act, m->key_act + testkey*m->na, m->na); - } } - // install timer callback for profiling if requested - tm_start = std::chrono::system_clock::now(); - if (profile) { - mjcb_time = gettm; - } + // install timer callback for profiling + mjcb_time = gettm; // print start - if (nthread>1) { - std::printf("\nRunning %d steps per thread at dt = %g ...\n\n", nstep, m->opt.timestep); - } else { - std::printf("\nRunning %d steps at dt = %g ...\n\n", nstep, m->opt.timestep); + std::printf("\nRolling out %d steps%s, at dt = %g", + nstep, + nthread > 1 ? " per thread" : "", + m->opt.timestep); + if (npoolthread > 1) { + std::printf(", using %d threads for engine-internal threadpool", npoolthread); } + std::printf("...\n\n"); // create pseudo-random control sequence std::vector ctrl = CtrlNoise(m, nstep, ctrlnoise); @@ -204,17 +216,17 @@ int main(int argc, char** argv) { // run simulation, record total time std::thread th[maxthread]; double starttime = gettm(); - for (int id=0; id1) { + if (nthread > 1) { std::printf("Summary for all %d threads\n\n", nthread); std::printf(" Total simulation time : %.2f s\n", tottime); std::printf(" Total steps per second : %.0f\n", nthread*nstep/tottime); @@ -235,33 +247,32 @@ int main(int argc, char** argv) { std::printf(" Constraints per step : %.2f\n", static_cast(constraints[0])/nstep); std::printf(" Degrees of freedom : %d\n\n", m->nv); - // profiler results for thread 0 - if (profile) { - printf(" Internal profiler for thread 0 (%ss per step)\n", mu_str); - mjtNum tstep = d[0]->timer[mjTIMER_STEP].duration/d[0]->timer[mjTIMER_STEP].number; - mjtNum components = 0, total = 0; - for (int i=0; itimer[i].number > 0) { - mjtNum istep = d[0]->timer[i].duration/d[0]->timer[i].number; - std::printf(" %16s : %6.1f (%6.2f %%)\n", mjTIMERSTRING[i], 1e6*istep, 100*istep/tstep); + // profiler results + printf(" Internal profiler%s, %ss per step\n", nthread > 1 ? " for thread 0" : "", mu_str); + mjtNum tstep = d[0]->timer[mjTIMER_STEP].duration/d[0]->timer[mjTIMER_STEP].number; + mjtNum components = 0, total = 0; + for (int i=0; i < mjNTIMER; i++) { + if (d[0]->timer[i].number > 0) { + mjtNum istep = d[0]->timer[i].duration/d[0]->timer[i].number; + std::printf(" %16s : %6.1f (%6.2f %%)\n", mjTIMERSTRING[i], istep, 100*istep/tstep); - // save step time, add up timing of components - if (i == 0) total = istep; - if (i >= mjTIMER_POSITION && i <= mjTIMER_ADVANCE) { - components += istep; - } + // save step time, add up timing of components + if (i == 0) total = istep; + if (i >= mjTIMER_POSITION && i <= mjTIMER_ADVANCE) { + components += istep; } } - - // compute "other" (computation not covered by timers) - if (tstep > 0) { - mjtNum other = total - components; - std::printf(" %16s : %6.1f (%6.2f %%)\n", "other", 1e6*other, 100*other/tstep); - } } + // compute "other" (computation not covered by timers) + if (tstep > 0) { + mjtNum other = total - components; + std::printf(" %16s : %6.1f (%6.2f %%)\n", "other", other, 100*other/tstep); + } + + // free per-thread data - for (int id=0; idthreadpool; mj_deleteData(d[id]); if (threadpool) { diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 6bfd3ab2..f656efe2 100644 --- a/src/engine/engine_collision_driver.c +++ b/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); } diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 6e8a4008..d672d664 100644 --- a/src/engine/engine_core_smooth.c +++ b/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); } diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index a099dd6f..5746d865 100644 --- a/src/engine/engine_forward.c +++ b/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); diff --git a/src/engine/engine_inverse.c b/src/engine/engine_inverse.c index d81b8746..961d46b1 100644 --- a/src/engine/engine_inverse.c +++ b/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); } diff --git a/src/engine/engine_macro.h b/src/engine/engine_macro.h index 5238000d..31b7a4b2 100644 --- a/src/engine/engine_macro.h +++ b/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++;}