From 5f132af6256ab704dd318afa2475e092fd9f8c34 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 3 Apr 2023 00:59:18 -0700 Subject: [PATCH] Add midphase and broadphase statistics to `mjData`. PiperOrigin-RevId: 521387641 Change-Id: Id50f2549d119e364216491a23b107e04d9ebb5b6 --- doc/includes/references.h | 16 ++++++++++---- include/mujoco/mjdata.h | 16 ++++++++++---- include/mujoco/mjxmacro.h | 32 ++++++++++++++++------------ sample/testspeed.cc | 16 ++++++++++++++ simulate/simulate.cc | 7 ++++-- src/engine/engine_collision_driver.c | 22 +++++++++++++++++++ src/engine/engine_io.c | 8 ++++++- unity/Runtime/Bindings/MjBindings.cs | 4 ++++ 8 files changed, 96 insertions(+), 25 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index c4c97b62..faabc619 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -119,10 +119,18 @@ struct mjData_ { // diagnostics mjWarningStat warning[mjNWARNING]; // warning statistics mjTimerStat timer[mjNTIMER]; // timer statistics - mjSolverStat solver[mjNSOLVER]; // solver statistics per iteration - int solver_iter; // number of solver iterations - int solver_nnz; // number of non-zeros in Hessian or efc_AR - mjtNum solver_fwdinv[2]; // forward-inverse comparison: qfrc, efc + + // solver statistics + mjSolverStat solver[mjNSOLVER]; // solver statistics per iteration + int solver_iter; // number of solver iterations + int solver_nnz; // number of non-zeros in Hessian or efc_AR + mjtNum solver_fwdinv[2]; // forward-inverse comparison: qfrc, efc + + // collision statistics + int nbodypair_broad; // number of body pairs in collision according to the broad-phase + int nbodypair_narrow; // number of body pairs actually in collision in the narrow-phase + int ngeompair_mid; // number of geom pairs in collision according to the mid-phase + int ngeompair_narrow; // number of geom pairs actually in collision in the narrow-phase // variable sizes int ne; // number of equality constraints diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index df68035f..d908eaf1 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -144,10 +144,18 @@ struct mjData_ { // diagnostics mjWarningStat warning[mjNWARNING]; // warning statistics mjTimerStat timer[mjNTIMER]; // timer statistics - mjSolverStat solver[mjNSOLVER]; // solver statistics per iteration - int solver_iter; // number of solver iterations - int solver_nnz; // number of non-zeros in Hessian or efc_AR - mjtNum solver_fwdinv[2]; // forward-inverse comparison: qfrc, efc + + // solver statistics + mjSolverStat solver[mjNSOLVER]; // solver statistics per iteration + int solver_iter; // number of solver iterations + int solver_nnz; // number of non-zeros in Hessian or efc_AR + mjtNum solver_fwdinv[2]; // forward-inverse comparison: qfrc, efc + + // collision statistics + int nbodypair_broad; // number of body pairs in collision according to the broad-phase + int nbodypair_narrow; // number of body pairs actually in collision in the narrow-phase + int ngeompair_mid; // number of geom pairs in collision according to the mid-phase + int ngeompair_narrow; // number of geom pairs actually in collision in the narrow-phase // variable sizes int ne; // number of equality constraints diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 39a81830..94766a30 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -590,20 +590,24 @@ // scalar fields of mjData -#define MJDATA_SCALAR \ - X( int, nstack ) \ - X( int, nbuffer ) \ - X( int, pstack ) \ - X( int, maxuse_stack ) \ - X( int, maxuse_con ) \ - X( int, maxuse_efc ) \ - X( int, solver_iter ) \ - X( int, solver_nnz ) \ - X( int, ne ) \ - X( int, nf ) \ - X( int, nefc ) \ - X( int, ncon ) \ - X( mjtNum, time ) +#define MJDATA_SCALAR \ + X( int, nstack ) \ + X( int, nbuffer ) \ + X( int, pstack ) \ + X( int, maxuse_stack ) \ + X( int, maxuse_con ) \ + X( int, maxuse_efc ) \ + X( int, solver_iter ) \ + X( int, solver_nnz ) \ + X( int, nbodypair_broad ) \ + X( int, nbodypair_narrow ) \ + X( int, ngeompair_mid ) \ + X( int, ngeompair_narrow ) \ + X( int, ne ) \ + X( int, nf ) \ + X( int, nefc ) \ + X( int, ncon ) \ + X( mjtNum, time ) // vector fields of mjData diff --git a/sample/testspeed.cc b/sample/testspeed.cc index 5f51cae1..3294f761 100644 --- a/sample/testspeed.cc +++ b/sample/testspeed.cc @@ -31,6 +31,8 @@ mjData* d[maxthread]; // per-thread statistics +double accuracy_broad[maxthread]; +double accuracy_mid[maxthread]; int contacts[maxthread]; int constraints[maxthread]; double simtime[maxthread]; @@ -83,6 +85,8 @@ void simulate(int id, int nstep, mjtNum* ctrl) { // clear statistics contacts[id] = 0; constraints[id] = 0; + accuracy_broad[id] = 0; + accuracy_mid[id] = 0; // run and time double start = gettm(); @@ -96,6 +100,16 @@ void simulate(int id, int nstep, mjtNum* ctrl) { // accumulate statistics contacts[id] += d[id]->ncon; constraints[id] += d[id]->nefc; + if (d[id]->nbodypair_broad) { + accuracy_broad[id] += (100.0*d[id]->nbodypair_narrow)/d[id]->nbodypair_broad; + } else { + accuracy_broad[id] += 100; + } + if (d[id]->ngeompair_mid) { + accuracy_mid[id] += (100.0*d[id]->nbodypair_narrow)/d[id]->ngeompair_mid; + } else { + accuracy_mid[id] += 100; + } } simtime[id] = gettm() - start; } @@ -210,6 +224,8 @@ int main(int argc, char** argv) { std::printf(" Steps per second : %.0f\n", nstep/simtime[0]); std::printf(" Realtime factor : %.2f x\n", nstep*m->opt.timestep/simtime[0]); std::printf(" Time per step : %.4f ms\n\n", 1000*simtime[0]/nstep); + std::printf(" Broadphase accuracy : %.2f%%\n", accuracy_broad[0]/nstep); + std::printf(" Midphase accuracy : %.2f%%\n", accuracy_mid[0]/nstep); std::printf(" Contacts per step : %.2f\n", static_cast(contacts[0])/nstep); std::printf(" Constraints per step : %.2f\n", static_cast(constraints[0])/nstep); std::printf(" Degrees of freedom : %d\n\n", m->nv); diff --git a/simulate/simulate.cc b/simulate/simulate.cc index b6992a85..2aa71524 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -471,11 +471,14 @@ void infotext(mj::Simulate* sim, solerr = mju_log10(mju_max(mjMINVAL, solerr)); // prepare info text - mju::strcpy_arr(title, "Time\nSize\nCPU\nSolver \nFPS\nMemory"); + mju::strcpy_arr(title, "Time\nSize\nPruning\nCPU\nSolver \nFPS\nMemory"); + int broad_pruning = d->nbodypair_broad ? (100.0*d->nbodypair_narrow)/d->nbodypair_broad : 0; + int mid_pruning = d->ngeompair_mid ? (100.0*d->nbodypair_narrow)/d->ngeompair_mid : 0; mju::sprintf_arr(content, - "%-9.3f\n%d (%d con)\n%.3f\n%.1f (%d it)\n%.0f\n%.2g of %s", + "%-9.3f\n%d (%d con)\nb: %d%% m: %d%%\n%.3f\n%.1f (%d it)\n%.0f\n%.2g of %s", d->time, d->nefc, d->ncon, + broad_pruning, mid_pruning, sim->run ? d->timer[mjTIMER_STEP].duration / mjMAX(1, d->timer[mjTIMER_STEP].number) : d->timer[mjTIMER_FORWARD].duration / mjMAX(1, d->timer[mjTIMER_FORWARD].number), diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index cad80992..9a057177 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -369,6 +369,12 @@ void mj_collision(const mjModel* m, mjData* d) { // reset the size of the contact array d->ncon = 0; + // reset diagnostics + d->nbodypair_broad = 0; + d->nbodypair_narrow = 0; + d->ngeompair_mid = 0; + d->ngeompair_narrow = 0; + // reset the visualization flags memset(d->bvh_active, 0, m->nbvh); @@ -380,8 +386,13 @@ void mj_collision(const mjModel* m, mjData* d) { // predefined only; ignore exclude if (m->opt.collision==mjCOL_PAIR) { + d->nbodypair_broad = npair; for (pairadr=0; pairadrngeompair_narrow; + int ngeompair_mid_before = d->ngeompair_mid; mj_collideGeoms(m, d, pairadr, -1, 0, 0); + if (d->ngeompair_narrow > ngeompair_narrow_before) d->nbodypair_narrow++; + if (d->ngeompair_mid > ngeompair_mid_before) d->nbodypair_broad++; } } @@ -434,6 +445,9 @@ void mj_collision(const mjModel* m, mjData* d) { } } + int ngeompair_narrow_before = d->ngeompair_narrow; + int ngeompair_mid_before = d->ngeompair_mid; + // test all geom pairs within this body pair if (m->body_geomnum[b1] && m->body_geomnum[b2]) { if (!mjDISABLED(mjDSBL_MIDPHASE) && m->body_geomnum[b1]*m->body_geomnum[b2]>1) { @@ -451,6 +465,8 @@ void mj_collision(const mjModel* m, mjData* d) { } } } + if (d->ngeompair_narrow > ngeompair_narrow_before) d->nbodypair_narrow++; + if (d->ngeompair_mid > ngeompair_mid_before) d->nbodypair_broad++; } // finish merging predefined pairs @@ -905,6 +921,9 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, return; } + // increment counter of expected collisions + d->ngeompair_mid++; + // call collision detector to generate contacts num = mjCOLLISIONFUNC[type1][type2](m, d, con, g1, g2, margin); @@ -913,6 +932,9 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2, int flg_user, return; } + // increment counter of actual collisions + d->ngeompair_narrow++; + // check number of contacts, SHOULD NOT OCCUR if (num>mjMAXCONPAIR) { mju_error("Too many contacts returned by collision function"); diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 62ba39dd..d14647e5 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -1272,7 +1272,7 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { d->maxuse_con = 0; d->maxuse_efc = 0; - // clear diagnostics + // clear solver diagnostics memset(d->warning, 0, mjNWARNING*sizeof(mjWarningStat)); memset(d->timer, 0, mjNTIMER*sizeof(mjTimerStat)); memset(d->solver, 0, mjNSOLVER*sizeof(mjSolverStat)); @@ -1280,6 +1280,12 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) { d->solver_nnz = 0; mju_zero(d->solver_fwdinv, 2); + // clear collision diagnostics + d->nbodypair_broad = 0; + d->nbodypair_narrow = 0; + d->ngeompair_mid = 0; + d->ngeompair_narrow = 0; + // clear variable sizes d->ne = 0; d->nf = 0; diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 97730ff1..f371f737 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -1587,6 +1587,10 @@ public unsafe struct mjData_ { public int solver_iter; public int solver_nnz; public fixed double solver_fwdinv[2]; + public int nbodypair_broad; + public int nbodypair_narrow; + public int ngeompair_mid; + public int ngeompair_narrow; public int ne; public int nf; public int nefc;