chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add midphase and broadphase statistics to mjData.

Browse Source 
PiperOrigin-RevId: 521387641
Change-Id: Id50f2549d119e364216491a23b107e04d9ebb5b6
This commit is contained in:
Alessio Quaglino
2023-04-03 00:59:18 -07:00
committed by Copybara-Service
parent c29f32657c
commit 5f132af625
8 changed files with 96 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/includes/references.h
+12 -4
View File
@@ -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
include/mujoco/mjdata.h
+12 -4
View File
@@ -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
include/mujoco/mjxmacro.h
+18 -14
View File
@@ -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
sample/testspeed.cc
+16
View File
@@ -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<float>(contacts[0])/nstep);
std::printf(" Constraints per step : %.2f\n", static_cast<float>(constraints[0])/nstep);
std::printf(" Degrees of freedom : %d\n\n", m->nv);
simulate/simulate.cc
+5 -2
View File
@@ -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),
src/engine/engine_collision_driver.c
+22
View File
@@ -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; pairadr<npair; pairadr++) {
int ngeompair_narrow_before = d->ngeompair_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");
src/engine/engine_io.c
+7 -1
View File
@@ -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;
unity/Runtime/Bindings/MjBindings.cs
+4
View File
@@ -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;