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

Rejigged solver statistics in preparation for islanding.

Browse Source 
- See public description below.
- Stopped incrementing the iteration count in saveStats().
- `sizeof(mjSolverStat) == 40`, so this ends up costing 160KB, up from 40KB.

BEGIN_PUBLIC

Changed the size of `mjData.solver`, the structure used to collect solver diagnostic information. The array is now of length `mjNISLAND * mjNSOLVER`, where each row of length `mjNSOLVER` contains separate solver statistics for each constraint island. Until solver islanding is implemented, only row 0 is used.

- The new constant `mjNISLAND` is set to 20.
- `mjNSOLVER` is reduced from 1000 to 200.
- Added `mjData.solver_nisland`, the number of islands for which the solver ran.
- `mjData.solver_niter` (renamed from mjData.solver_iter) and `mjData.solver_nnz` are now integer vectors of length `mjNISLAND`.

END_PUBLIC

PiperOrigin-RevId: 565030093
Change-Id: I773e918805c6ced79f0dab5f19ea23956760c8c5
This commit is contained in:
Yuval Tassa
2023-09-13 06:17:45 -07:00
committed by Copybara-Service
parent 8faf47dd16
commit 86d8b912f4
15 changed files with 3303 additions and 155 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/APIglobals.rst
+9 -3
View File
@@ -450,9 +450,15 @@ shown in the table below. Their names are in the format ``mjKEY_XXX``. They corr
- The maximal number of real-valued parameters used to define the impedance of each scalar constraint.
Determines the size of all ``mjModel.XXX_solimp`` fields.
* - ``mjNSOLVER``
- 1000
- The size of the preallocated array ``mjData.solver``. This is used to store diagnostic information about each
iteration of the constraint solver. The actual number of iterations is given by ``mjData.solver_iter``.
- 200
- The number of iterations where solver statistics can be stored in ``mjData.solver``. This array is used
to store diagnostic information about each iteration of the constraint solver.
The actual number of iterations is given by ``mjData.solver_iter``.
* - ``mjNISLAND``
- 20
- The number of islands for which solver statistics can be stored in ``mjData.solver``. This array is
used to store diagnostic information about each iteration of the constraint solver.
The actual number of islands for which the solver was run is given by ``mjData.nsolver_island``.
* - ``mjNGROUP``
- 6
- The number of geom, site, joint, tendon and actuator groups whose rendering can be enabled and disabled via
doc/changelog.rst
+23 -12
View File
@@ -51,34 +51,45 @@ General
6. Renamed the ``nstack`` field in :ref:`mjModel` and :ref:`mjData` to ``narena``. Changed ``narena``, ``pstack``,
and ``maxuse_stack`` to count number of bytes rather than number of :ref:`mjtNum` |-| s.
7. Added a new :ref:`dyntype<actuator-general-dyntype>`, ``filterexact``, which updates first-order filter states with
7. Changed :ref:`mjData.solver<mjData>`, the array used to collect solver diagnostic information.
This array of :ref:`mjSolverStat` structs is now of length ``mjNISLAND * mjNSOLVER``, interpreted as as a matrix.
Each row of length ``mjNSOLVER`` contains separate solver statistics for each constraint island.
If the solver does not use islands, only row 0 is filled.
- The new constant :ref:`mjNISLAND<glNumeric>` was set to 20.
- :ref:`mjNSOLVER<glNumeric>` was reduced from 1000 to 200.
- Added :ref:`mjData.solver_nisland<mjData>`: the number of islands for which the solver ran.
- Renamed ``mjData.solver_iter`` to ``solver_niter``. Both this member and ``mjData.solver_nnz`` are now integer
vectors of length ``mjNISLAND``.
8. Added a new :ref:`dyntype<actuator-general-dyntype>`, ``filterexact``, which updates first-order filter states with
the exact formula rather than with Euler integration.
8. Added an actuator attribute, :ref:`actearly<actuator-general-actearly>`, which uses semi-implicit integration for
9. Added an actuator attribute, :ref:`actearly<actuator-general-actearly>`, which uses semi-implicit integration for
actuator forces: using the next step's actuator state to compute the current actuator forces at the current timestep.
9. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to
:ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` and
:ref:`actuatorfrclimited<body-joint-actuatorfrclimited>`, respectively.
10. Added the flag :ref:`eulerdamp<option-flag-eulerdamp>`, which disables implicit integration of joint damping in the
10. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to
:ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` and
:ref:`actuatorfrclimited<body-joint-actuatorfrclimited>`, respectively.
11. Added the flag :ref:`eulerdamp<option-flag-eulerdamp>`, which disables implicit integration of joint damping in the
Euler integrator. See the :ref:`Numerical Integration<geIntegration>` section for more details.
11. Added the flag :ref:`invdiscrete<option-flag-invdiscrete>`, which enables discrete-time inverse dynamics for all
12. Added the flag :ref:`invdiscrete<option-flag-invdiscrete>`, which enables discrete-time inverse dynamics for all
:ref:`integrators<option-integrator>` other than ``RK4``. See the flag documentation for more details.
12. Added :ref:`ls_iterations<option-ls_iterations>` and :ref:`ls_tolerance<option-ls_tolerance>` options for adjusting
13. Added :ref:`ls_iterations<option-ls_iterations>` and :ref:`ls_tolerance<option-ls_tolerance>` options for adjusting
linesearch stopping criteria in CG and Newton solvers. This can be useful for performance tuning.
13. Added ``mesh_pos`` and ``mesh_quat`` fields to :ref:`mjModel` to store normalizing transformation.
14. Added camera :ref:`resolution<body-camera-resolution>` attribute and :ref:`camprojection<sensor-camprojection>`
14. Added ``mesh_pos`` and ``mesh_quat`` fields to :ref:`mjModel` to store normalizing transformation.
15. Added camera :ref:`resolution<body-camera-resolution>` attribute and :ref:`camprojection<sensor-camprojection>`
sensor. If camera resolution is set to positive values, the camera projection sensor will report the location of a
target site, projected onto the camera image, in pixel coordinates.
Python bindings
^^^^^^^^^^^^^^^
15. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
16. Fixed `#870 <https://github.com/google-deepmind/mujoco/issues/870>`__ where calling ``update_scene`` with an invalid
camera name used the default camera.
Bug fixes
^^^^^^^^^
16. Fixed a bug that was causing the geom margins to be ignored during the midphase.
17. Fixed a bug that was causing the geom margins to be ignored during the midphase.
Version 2.3.7 (July 20, 2023)
doc/includes/references.h
+4 -3
View File
@@ -146,9 +146,10 @@ struct mjData_ {
mjTimerStat timer[mjNTIMER]; // timer statistics
// 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
mjSolverStat solver[mjNISLAND*mjNSOLVER]; // solver statistics per island, per iteration
int solver_nisland; // number of islands processed by solver
int solver_niter[mjNISLAND]; // number of solver iterations, per island
int solver_nnz[mjNISLAND]; // number of non-zeros in Hessian or efc_AR, per island
mjtNum solver_fwdinv[2]; // forward-inverse comparison: qfrc, efc
// collision statistics
include/mujoco/mjdata.h
+4 -3
View File
@@ -173,9 +173,10 @@ struct mjData_ {
mjTimerStat timer[mjNTIMER]; // timer statistics
// 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
mjSolverStat solver[mjNISLAND*mjNSOLVER]; // solver statistics per island, per iteration
int solver_nisland; // number of islands processed by solver
int solver_niter[mjNISLAND]; // number of solver iterations, per island
int solver_nnz[mjNISLAND]; // number of non-zeros in Hessian or efc_AR, per island
mjtNum solver_fwdinv[2]; // forward-inverse comparison: qfrc, efc
// collision statistics
include/mujoco/mjmodel.h
+2 -1
View File
@@ -41,7 +41,8 @@
#define mjNFLUID 12 // number of fluid interaction parameters
#define mjNREF 2 // number of solver reference parameters
#define mjNIMP 5 // number of solver impedance parameters
#define mjNSOLVER 1000 // size of mjData.solver_XXX arrays
#define mjNSOLVER 200 // size of one mjData.solver array
#define mjNISLAND 20 // number of mjData.solver arrays
//---------------------------------- enum types (mjt) ----------------------------------------------
include/mujoco/mjxmacro.h
+9 -8
View File
@@ -636,8 +636,7 @@
X( size_t, maxuse_arena ) \
X( int, maxuse_con ) \
X( int, maxuse_efc ) \
X( int, solver_iter ) \
X( int, solver_nnz ) \
X( int, solver_nisland ) \
X( int, nbodypair_broad ) \
X( int, nbodypair_narrow ) \
X( int, ngeompair_mid ) \
@@ -654,12 +653,14 @@
// vector fields of mjData
#define MJDATA_VECTOR \
X( mjWarningStat, warning, mjNWARNING, 1 ) \
X( mjTimerStat, timer, mjNTIMER, 1 ) \
X( mjSolverStat, solver, mjNSOLVER, 1 ) \
X( mjtNum, solver_fwdinv, 2, 1 ) \
X( mjtNum, energy, 2, 1 )
#define MJDATA_VECTOR \
X( mjWarningStat, warning, mjNWARNING, 1 ) \
X( mjTimerStat, timer, mjNTIMER, 1 ) \
X( mjSolverStat, solver, mjNILSAND, mjNSOLVER ) \
X( int, solver_niter, mjNISLAND, 1 ) \
X( int, solver_nnz, mjNISLAND, 1 ) \
X( mjtNum, solver_fwdinv, 2, 1 ) \
X( mjtNum, energy, 2, 1 )
// alias XMJV to be the same as X
introspect/structs.py
+17 -6
View File
@@ -3583,19 +3583,30 @@ STRUCTS: Mapping[str, StructDecl] = dict([
name='solver',
type=ArrayType(
inner_type=ValueType(name='mjSolverStat'),
extents=(1000,),
extents=(4000,),
),
doc='solver statistics per iteration',
doc='solver statistics per island, per iteration',
),
StructFieldDecl(
name='solver_iter',
name='solver_nisland',
type=ValueType(name='int'),
doc='number of solver iterations',
doc='number of islands processed by solver',
),
StructFieldDecl(
name='solver_niter',
type=ArrayType(
inner_type=ValueType(name='int'),
extents=(20,),
),
doc='number of solver iterations, per island',
),
StructFieldDecl(
name='solver_nnz',
type=ValueType(name='int'),
doc='number of non-zeros in Hessian or efc_AR',
type=ArrayType(
inner_type=ValueType(name='int'),
extents=(20,),
),
doc='number of non-zeros in Hessian or efc_AR, per island',
),
StructFieldDecl(
name='solver_fwdinv',
python/mujoco/structs.h
+2
View File
@@ -582,6 +582,8 @@ class MjWrapper<raw::MjData>: public WrapperBase<raw::MjData> {
py_array_or_tuple_t<raw::MjWarningStat> warning;
py_array_or_tuple_t<raw::MjTimerStat> timer;
py_array_or_tuple_t<raw::MjSolverStat> solver;
py_array_or_tuple_t<int> solver_niter;
py_array_or_tuple_t<int> solver_nnz;
py_array_or_tuple_t<mjtNum> solver_fwdinv;
py_array_or_tuple_t<mjtNum> energy;
simulate/simulate.cc
+115 -53
View File
@@ -176,6 +176,10 @@ const char help_title[] =
//-------------------------------- profiler, sensor, info, watch -----------------------------------
// number of lines in the Constraint ("Counts") and Cost ("Convergence") figures
static constexpr int kConstraintNum = 5;
static constexpr int kCostNum = 3;
// init profiler figures
void InitializeProfiler(mj::Simulate* sim) {
// set figures to default
@@ -212,6 +216,20 @@ void InitializeProfiler(mj::Simulate* sim) {
sim->figsize.figurergba[3] = 0.5f;
sim->figtimer.figurergba[3] = 0.5f;
// repeat line colors for constraint and cost figures
mjvFigure* fig = &sim->figcost;
for (int i=kCostNum; i<mjMAXLINE; i++) {
fig->linergb[i][0] = fig->linergb[i - kCostNum][0];
fig->linergb[i][1] = fig->linergb[i - kCostNum][1];
fig->linergb[i][2] = fig->linergb[i - kCostNum][2];
}
fig = &sim->figconstraint;
for (int i=kConstraintNum; i<mjMAXLINE; i++) {
fig->linergb[i][0] = fig->linergb[i - kConstraintNum][0];
fig->linergb[i][1] = fig->linergb[i - kConstraintNum][1];
fig->linergb[i][2] = fig->linergb[i - kConstraintNum][2];
}
// legends
mju::strcpy_arr(sim->figconstraint.linename[0], "total");
mju::strcpy_arr(sim->figconstraint.linename[1], "active");
@@ -272,54 +290,76 @@ void InitializeProfiler(mj::Simulate* sim) {
// update profiler figures
void UpdateProfiler(mj::Simulate* sim, const mjModel* m, const mjData* d) {
// update constraint figure
sim->figconstraint.linepnt[0] = mjMIN(mjMIN(d->solver_iter, mjNSOLVER), mjMAXLINEPNT);
for (int i=1; i<5; i++) {
sim->figconstraint.linepnt[i] = sim->figconstraint.linepnt[0];
}
if (m->opt.solver==mjSOL_PGS) {
sim->figconstraint.linepnt[3] = 0;
sim->figconstraint.linepnt[4] = 0;
}
if (m->opt.solver==mjSOL_CG) {
sim->figconstraint.linepnt[4] = 0;
}
for (int i=0; i<sim->figconstraint.linepnt[0]; i++) {
// x
sim->figconstraint.linedata[0][2*i] = i;
sim->figconstraint.linedata[1][2*i] = i;
sim->figconstraint.linedata[2][2*i] = i;
sim->figconstraint.linedata[3][2*i] = i;
sim->figconstraint.linedata[4][2*i] = i;
// reset lines in Constraint and Cost figures
memset(sim->figconstraint.linepnt, 0, mjMAXLINE*sizeof(int));
memset(sim->figcost.linepnt, 0, mjMAXLINE*sizeof(int));
// y
sim->figconstraint.linedata[0][2*i+1] = d->nefc;
sim->figconstraint.linedata[1][2*i+1] = d->solver[i].nactive;
sim->figconstraint.linedata[2][2*i+1] = d->solver[i].nchange;
sim->figconstraint.linedata[3][2*i+1] = d->solver[i].neval;
sim->figconstraint.linedata[4][2*i+1] = d->solver[i].nupdate;
}
// number of islands that have diagnostics
int nisland = mjMIN(d->solver_nisland, mjNISLAND);
// update cost figure
sim->figcost.linepnt[0] = mjMIN(mjMIN(d->solver_iter, mjNSOLVER), mjMAXLINEPNT);
for (int i=1; i<3; i++) {
sim->figcost.linepnt[i] = sim->figcost.linepnt[0];
}
if (m->opt.solver==mjSOL_PGS) {
sim->figcost.linepnt[1] = 0;
sim->figcost.linepnt[2] = 0;
}
// iterate over islands
for (int k=0; k < nisland; k++) {
// ==== update Constraint ("Counts") figure
for (int i=0; i<sim->figcost.linepnt[0]; i++) {
// x
sim->figcost.linedata[0][2*i] = i;
sim->figcost.linedata[1][2*i] = i;
sim->figcost.linedata[2][2*i] = i;
// number of points to plot, starting line
int npoints = mjMIN(mjMIN(d->solver_niter[k], mjNSOLVER), mjMAXLINEPNT);
int start = kConstraintNum * k;
// y
sim->figcost.linedata[0][2*i+1] = mju_log10(mju_max(mjMINVAL, d->solver[i].improvement));
sim->figcost.linedata[1][2*i+1] = mju_log10(mju_max(mjMINVAL, d->solver[i].gradient));
sim->figcost.linedata[2][2*i+1] = mju_log10(mju_max(mjMINVAL, d->solver[i].lineslope));
sim->figconstraint.linepnt[start + 0] = npoints;
for (int i=1; i < kConstraintNum; i++) {
sim->figconstraint.linepnt[start + i] = npoints;
}
if (m->opt.solver == mjSOL_PGS) {
sim->figconstraint.linepnt[start + 3] = 0;
sim->figconstraint.linepnt[start + 4] = 0;
}
if (m->opt.solver == mjSOL_CG) {
sim->figconstraint.linepnt[start + 4] = 0;
}
for (int i=0; i<npoints; i++) {
// x
sim->figconstraint.linedata[start + 0][2*i] = i;
sim->figconstraint.linedata[start + 1][2*i] = i;
sim->figconstraint.linedata[start + 2][2*i] = i;
sim->figconstraint.linedata[start + 3][2*i] = i;
sim->figconstraint.linedata[start + 4][2*i] = i;
// y
int nefc = nisland == 1 ? d->nefc : d->island_efcnum[k];
sim->figconstraint.linedata[start + 0][2*i+1] = nefc;
const mjSolverStat* stat = d->solver + k*mjNSOLVER + i;
sim->figconstraint.linedata[start + 1][2*i+1] = stat->nactive;
sim->figconstraint.linedata[start + 2][2*i+1] = stat->nchange;
sim->figconstraint.linedata[start + 3][2*i+1] = stat->neval;
sim->figconstraint.linedata[start + 4][2*i+1] = stat->nupdate;
}
// update cost figure
start = kCostNum * k;
sim->figcost.linepnt[start + 0] = npoints;
for (int i=1; i<kCostNum; i++) {
sim->figcost.linepnt[start + i] = npoints;
}
if (m->opt.solver==mjSOL_PGS) {
sim->figcost.linepnt[start + 1] = 0;
sim->figcost.linepnt[start + 2] = 0;
}
for (int i=0; i<sim->figcost.linepnt[0]; i++) {
// x
sim->figcost.linedata[start + 0][2*i] = i;
sim->figcost.linedata[start + 1][2*i] = i;
sim->figcost.linedata[start + 2][2*i] = i;
// y
const mjSolverStat* stat = d->solver + k*mjNSOLVER + i;
sim->figcost.linedata[start + 0][2*i + 1] =
mju_log10(mju_max(mjMINVAL, stat->improvement));
sim->figcost.linedata[start + 1][2*i + 1] =
mju_log10(mju_max(mjMINVAL, stat->gradient));
sim->figcost.linedata[start + 2][2*i + 1] =
mju_log10(mju_max(mjMINVAL, stat->lineslope));
}
}
// get timers: total, collision, prepare, solve, other
@@ -355,14 +395,22 @@ void UpdateProfiler(mj::Simulate* sim, const mjModel* m, const mjData* d) {
}
}
// get total number of iterations and nonzeros
mjtNum sqrt_nnz = 0;
int solver_niter = 0;
for (int island=0; island < nisland; island++) {
sqrt_nnz += mju_sqrt(d->solver_nnz[island]);
solver_niter += d->solver_niter[island];
}
// get sizes: nv, nbody, nefc, sqrt(nnz), ncont, iter
float sdata[6] = {
static_cast<float>(m->nv),
static_cast<float>(m->nbody),
static_cast<float>(d->nefc),
static_cast<float>(mju_sqrt(d->solver_nnz)),
static_cast<float>(sqrt_nnz),
static_cast<float>(d->ncon),
static_cast<float>(d->solver_iter)
static_cast<float>(solver_niter)
};
// update figsize
@@ -497,14 +545,22 @@ void UpdateInfoText(mj::Simulate* sim, const mjModel* m, const mjData* d,
char (&content)[mj::Simulate::kMaxFilenameLength]) {
char tmp[20];
// compute solver error
// number of islands with statistics
int nisland = mjMIN(d->solver_nisland, mjNISLAND);
// compute solver error (maximum over islands)
mjtNum solerr = 0;
if (d->solver_iter) {
int ind = mjMIN(d->solver_iter-1, mjNSOLVER-1);
solerr = mju_min(d->solver[ind].improvement, d->solver[ind].gradient);
if (solerr==0) {
solerr = mju_max(d->solver[ind].improvement, d->solver[ind].gradient);
for (int i=0; i < nisland; i++) {
mjtNum solerr_i = 0;
if (d->solver_niter[i]) {
int ind = mjMIN(d->solver_niter[i], mjNSOLVER) - 1;
const mjSolverStat* stat = d->solver + i*mjNSOLVER + ind;
solerr_i = mju_min(stat->improvement, stat->gradient);
if (solerr_i==0) {
solerr_i = mju_max(stat->improvement, stat->gradient);
}
}
solerr = mju_max(solerr, solerr_i);
}
solerr = mju_log10(mju_max(mjMINVAL, solerr));
@@ -516,6 +572,12 @@ void UpdateInfoText(mj::Simulate* sim, const mjModel* m, const mjData* d,
mju::sprintf_arr(fps, "%.0f ", sim->fps_);
}
// total iterations of all islands with statistics
int solver_niter = 0;
for (int i=0; i < nisland; i++) {
solver_niter += d->solver_niter[i];
}
// prepare info text
mju::strcpy_arr(title, "Time\nSize\nCPU\nSolver \nFPS\nMemory");
mju::sprintf_arr(content,
@@ -525,7 +587,7 @@ void UpdateInfoText(mj::Simulate* sim, const mjModel* m, const mjData* d,
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),
solerr, d->solver_iter,
solerr, solver_niter,
fps,
d->maxuse_arena/(double)(d->narena),
mju_writeNumBytes(d->narena));
src/engine/engine_core_smooth.c
+8 -2
View File
@@ -1212,11 +1212,17 @@ void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) {
// in-place sparse backsubstitution for one island: x = inv(L'*D*L)*x
// L is in lower triangle of qLD; D is on diagonal of qLD
void mj_solveM_island(const mjModel* m, const mjData* d, mjtNum* restrict x, int island) {
// if no islands, call mj_solveLD
const mjtNum* qLD = d->qLD;
const mjtNum* qLDiagInv = d->qLDiagInv;
if (island < 0) {
mj_solveLD(m, x, 1, qLD, qLDiagInv);
return;
}
// local constants: general
const int* Madr = m->dof_Madr;
const int* parentid = m->dof_parentid;
const mjtNum* qLD = d->qLD;
const mjtNum* qLDiagInv = d->qLDiagInv;
const int* simplenum = m->dof_simplenum;
// local constants: island specific
src/engine/engine_forward.c
+5 -2
View File
@@ -501,7 +501,7 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
mju_copy(d->qacc, d->qacc_smooth, nv);
mju_copy(d->qacc_warmstart, d->qacc_smooth, nv);
mju_zero(d->qfrc_constraint, nv);
d->solver_iter = 0;
mju_zeroInt(d->solver_niter, mjNISLAND);
TM_END(mjTIMER_CONSTRAINT);
return;
}
@@ -512,7 +512,7 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
// warmstart solver
warmstart(m, d);
d->solver_iter = 0;
mju_zeroInt(d->solver_niter, mjNISLAND);
// run main solver
switch ((mjtSolver) m->opt.solver) {
@@ -532,6 +532,9 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
mjERROR("unknown solver type %d", m->opt.solver);
}
// one (monolithic) island
d->solver_nisland = 1;
// save result for next step warmstart
mju_copy(d->qacc_warmstart, d->qacc, nv);
src/engine/engine_io.c
+4 -3
View File
@@ -1428,9 +1428,10 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
// clear solver diagnostics
memset(d->warning, 0, mjNWARNING*sizeof(mjWarningStat));
memset(d->timer, 0, mjNTIMER*sizeof(mjTimerStat));
memset(d->solver, 0, mjNSOLVER*sizeof(mjSolverStat));
d->solver_iter = 0;
d->solver_nnz = 0;
memset(d->solver, 0, mjNSOLVER*mjNISLAND*sizeof(mjSolverStat));
d->solver_nisland = 0;
mju_zeroInt(d->solver_niter, mjNISLAND);
mju_zeroInt(d->solver_nnz, mjNISLAND);
mju_zero(d->solver_fwdinv, 2);
// clear collision diagnostics
src/engine/engine_print.c
+25 -16
View File
@@ -837,24 +837,33 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
}
// SOLVER STAT
if (d->solver_iter) {
if (d->nefc) {
fprintf(fp, "SOLVER STAT\n");
fprintf(fp, " solver_iter = %d\n", d->solver_iter);
fprintf(fp, " solver_nnz = %d\n", d->solver_nnz);
for (int i=0; i < mjMIN(mjNSOLVER, d->solver_iter); i++) {
fprintf(fp, " %d: improvement = ", i);
fprintf(fp, float_format, d->solver[i].improvement);
fprintf(fp, " gradient = ");
fprintf(fp, float_format, d->solver[i].gradient);
fprintf(fp, " lineslope = ");
fprintf(fp, float_format, d->solver[i].lineslope);
fprintf(fp, "\n");
fprintf(fp, " nactive = %d nchange = %d neval = %d nupdate = %d\n",
d->solver[i].nactive, d->solver[i].nchange,
d->solver[i].neval, d->solver[i].nupdate);
fprintf(fp, " solver_nisland = %d\n", d->solver_nisland);
printVector(" solver_fwdinv = ", d->solver_fwdinv, 2, fp, float_format);
int nisland_stat = mjMIN(d->solver_nisland, mjNISLAND);
for (int island=0; island < nisland_stat; island++) {
int niter_stat = mjMIN(mjNSOLVER, d->solver_niter[island]);
if (niter_stat) {
fprintf(fp, " ISLAND %d\n", island);
fprintf(fp, " solver_niter = %d\n", d->solver_niter[island]);
fprintf(fp, " solver_nnz = %d\n", d->solver_nnz[island]);
for (int i=0; i < niter_stat; i++) {
mjSolverStat* stat = d->solver + island*mjNSOLVER + i;
fprintf(fp, " %d: improvement = ", i);
fprintf(fp, float_format, stat->improvement);
fprintf(fp, " gradient = ");
fprintf(fp, float_format, stat->gradient);
fprintf(fp, " lineslope = ");
fprintf(fp, float_format, stat->lineslope);
fprintf(fp, "\n");
fprintf(fp, " nactive = %d nchange = %d neval = %d nupdate = %d\n",
stat->nactive, stat->nchange,
stat->neval, stat->nupdate);
}
fprintf(fp, "\n");
}
}
printVector("solver_fwdinv = ", d->solver_fwdinv, 2, fp, float_format);
fprintf(fp, "\n");
}
printVector("ENERGY = ", d->energy, 2, fp, float_format);
src/engine/engine_solver.c
+71 -40
View File
@@ -39,24 +39,30 @@ static mjtNum rescale(const mjModel* m, mjtNum x) {
// save solver statistics, count
static void saveStats(const mjModel* m, mjData* d, int* piter,
// save solver statistics
static void saveStats(const mjModel* m, mjData* d, int island, int iter,
mjtNum improvement, mjtNum gradient, mjtNum lineslope,
int nactive, int nchange, int neval, int nupdate) {
// compute position, increase iter
int i = d->solver_iter + (*piter);
(*piter)++;
// save if within range
if (i < mjNSOLVER) {
d->solver[i].improvement = improvement;
d->solver[i].gradient = gradient;
d->solver[i].lineslope = lineslope;
d->solver[i].nactive = nactive;
d->solver[i].nchange = nchange;
d->solver[i].neval = neval;
d->solver[i].nupdate = nupdate;
// if out of range, return
if (island >= mjNISLAND) {
return;
}
// if no islands, use first island
island = mjMAX(0, island);
// get mjSolverStat pointer
iter += d->solver_niter[island]; // add current niter (in case of noslip)
mjSolverStat* stat = d->solver + island*mjNSOLVER + iter;
// save stats
stat->improvement = improvement;
stat->gradient = gradient;
stat->lineslope = lineslope;
stat->nactive = nactive;
stat->nchange = nchange;
stat->neval = neval;
stat->nupdate = nupdate;
}
@@ -313,6 +319,9 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
mjtNum* ARinv = mj_stackAllocNum(d, nefc);
int* oldstate = mj_stackAllocInt(d, nefc);
// TODO: b/295296178 - Use island index (currently hardcoded to 0)
int island = 0;
// precompute inverse diagonal of AR
ARdiaginv(m, d, ARinv, 0);
@@ -471,9 +480,13 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
nchange += (oldstate[i] != d->efc_state[i]);
}
// scale improvement, save stats, count
// scale improvement, save stats
improvement = rescale(m, improvement);
saveStats(m, d, &iter, improvement, 0, 0, nactive, nchange, 0, 0);
saveStats(m, d, island, iter, improvement, 0, 0, nactive, nchange, 0, 0);
// increment iteration count
iter++;
// terminate
if (improvement < m->opt.tolerance) {
@@ -481,17 +494,20 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
}
}
// update solver iterations
d->solver_iter += iter;
// finalize statistics
if (island < mjNISLAND) {
// update solver iterations
d->solver_niter[island] += iter;
// set nnz
if (mj_isSparse(m)) {
d->solver_nnz = 0;
for (int i=0; i < nefc; i++) {
d->solver_nnz += d->efc_AR_rownnz[i];
// set nnz
if (mj_isSparse(m)) {
d->solver_nnz[island] = 0;
for (int i=0; i < nefc; i++) {
d->solver_nnz[island] += d->efc_AR_rownnz[i];
}
} else {
d->solver_nnz[island] = nefc*nefc;
}
} else {
d->solver_nnz = nefc*nefc;
}
// map to joint space
@@ -515,6 +531,9 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
mjtNum* ARinv = mj_stackAllocNum(d, nefc);
int* oldstate = mj_stackAllocInt(d, nefc);
// TODO: b/295296178 - Use island index (currently hardcoded to 0)
int island = 0;
// precompute inverse diagonal of A
ARdiaginv(m, d, ARinv, 1);
@@ -687,9 +706,12 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
nchange += (oldstate[i] != d->efc_state[i]);
}
// scale improvement, save stats, count
// scale improvement, save stats
improvement = rescale(m, improvement);
saveStats(m, d, &iter, improvement, 0, 0, nactive, nchange, 0, 0);
saveStats(m, d, island, iter, improvement, 0, 0, nactive, nchange, 0, 0);
// increment iteration count
iter++;
// terminate
if (improvement < m->opt.noslip_tolerance) {
@@ -698,7 +720,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
}
// update solver iterations
d->solver_iter += iter;
d->solver_niter[island] += iter;
// map to joint space
dualFinish(m, d);
@@ -1514,6 +1536,9 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
mjCGContext ctx;
mj_markStack(d);
// TODO: b/295296178 - Use island index (currently hardcoded to 0)
int island = 0;
// allocate context
CGallocate(m, d, &ctx, flg_Newton);
@@ -1576,12 +1601,15 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
nchange += (d->efc_state[i] != oldstate[i]);
}
// scale improvement, save stats, count
// scale improvement, save stats
mjtNum improvement = rescale(m, oldcost-ctx.cost);
mjtNum gradient = rescale(m, mju_norm(ctx.grad, nv));
saveStats(m, d, &iter, improvement, gradient, ctx.LSslope,
saveStats(m, d, island, iter, improvement, gradient, ctx.LSslope,
ctx.nactive, nchange, ctx.LSiter, ctx.nupdate);
// increment iteration count
iter++;
// termination
if (improvement < m->opt.tolerance || gradient < m->opt.tolerance) {
break;
@@ -1608,18 +1636,21 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
}
}
// update solver iterations
d->solver_iter += iter;
// finalize statistics
if (island < mjNISLAND) {
// update solver iterations
d->solver_niter[island] += iter;
// set solver_nnz
if (flg_Newton) {
if (mj_isSparse(m)) {
d->solver_nnz = 2*ctx.nnz - nv;
// set solver_nnz
if (flg_Newton) {
if (mj_isSparse(m)) {
d->solver_nnz[island] = 2*ctx.nnz - nv;
} else {
d->solver_nnz[island] = nv*nv;
}
} else {
d->solver_nnz = nv*nv;
d->solver_nnz[island] = 0;
}
} else {
d->solver_nnz = 0;
}
mj_freeStack(d);
unity/Runtime/Bindings/MjBindings.cs
+3005 -3
View File
File diff suppressed because it is too large Load Diff