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. - 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. Determines the size of all ``mjModel.XXX_solimp`` fields.
* - ``mjNSOLVER`` * - ``mjNSOLVER``
- 1000 - 200
- The size of the preallocated array ``mjData.solver``. This is used to store diagnostic information about each - The number of iterations where solver statistics can be stored in ``mjData.solver``. This array is used
iteration of the constraint solver. The actual number of iterations is given by ``mjData.solver_iter``. 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`` * - ``mjNGROUP``
- 6 - 6
- The number of geom, site, joint, tendon and actuator groups whose rendering can be enabled and disabled via - 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``, 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. 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. 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. 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 10. Renamed ``actuatorforcerange`` and ``actuatorforcelimited``, introduced in the previous version to
:ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` and :ref:`actuatorfrcrange<body-joint-actuatorfrcrange>` and
:ref:`actuatorfrclimited<body-joint-actuatorfrclimited>`, respectively. :ref:`actuatorfrclimited<body-joint-actuatorfrclimited>`, respectively.
10. Added the flag :ref:`eulerdamp<option-flag-eulerdamp>`, which disables implicit integration of joint damping in the 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. 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. :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. 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 ``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>` 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 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. target site, projected onto the camera image, in pixel coordinates.
Python bindings 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. camera name used the default camera.
Bug fixes 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) 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 mjTimerStat timer[mjNTIMER]; // timer statistics
// solver statistics // solver statistics
mjSolverStat solver[mjNSOLVER]; // solver statistics per iteration mjSolverStat solver[mjNISLAND*mjNSOLVER]; // solver statistics per island, per iteration
int solver_iter; // number of solver iterations int solver_nisland; // number of islands processed by solver
int solver_nnz; // number of non-zeros in Hessian or efc_AR 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 mjtNum solver_fwdinv[2]; // forward-inverse comparison: qfrc, efc
// collision statistics // collision statistics
include/mujoco/mjdata.h
+4 -3
View File
@@ -173,9 +173,10 @@ struct mjData_ {
mjTimerStat timer[mjNTIMER]; // timer statistics mjTimerStat timer[mjNTIMER]; // timer statistics
// solver statistics // solver statistics
mjSolverStat solver[mjNSOLVER]; // solver statistics per iteration mjSolverStat solver[mjNISLAND*mjNSOLVER]; // solver statistics per island, per iteration
int solver_iter; // number of solver iterations int solver_nisland; // number of islands processed by solver
int solver_nnz; // number of non-zeros in Hessian or efc_AR 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 mjtNum solver_fwdinv[2]; // forward-inverse comparison: qfrc, efc
// collision statistics // collision statistics
include/mujoco/mjmodel.h
+2 -1
View File
@@ -41,7 +41,8 @@
#define mjNFLUID 12 // number of fluid interaction parameters #define mjNFLUID 12 // number of fluid interaction parameters
#define mjNREF 2 // number of solver reference parameters #define mjNREF 2 // number of solver reference parameters
#define mjNIMP 5 // number of solver impedance 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) ---------------------------------------------- //---------------------------------- enum types (mjt) ----------------------------------------------
include/mujoco/mjxmacro.h
+9 -8
View File
@@ -636,8 +636,7 @@
X( size_t, maxuse_arena ) \ X( size_t, maxuse_arena ) \
X( int, maxuse_con ) \ X( int, maxuse_con ) \
X( int, maxuse_efc ) \ X( int, maxuse_efc ) \
X( int, solver_iter ) \ X( int, solver_nisland ) \
X( int, solver_nnz ) \
X( int, nbodypair_broad ) \ X( int, nbodypair_broad ) \
X( int, nbodypair_narrow ) \ X( int, nbodypair_narrow ) \
X( int, ngeompair_mid ) \ X( int, ngeompair_mid ) \
@@ -654,12 +653,14 @@
// vector fields of mjData // vector fields of mjData
#define MJDATA_VECTOR \ #define MJDATA_VECTOR \
X( mjWarningStat, warning, mjNWARNING, 1 ) \ X( mjWarningStat, warning, mjNWARNING, 1 ) \
X( mjTimerStat, timer, mjNTIMER, 1 ) \ X( mjTimerStat, timer, mjNTIMER, 1 ) \
X( mjSolverStat, solver, mjNSOLVER, 1 ) \ X( mjSolverStat, solver, mjNILSAND, mjNSOLVER ) \
X( mjtNum, solver_fwdinv, 2, 1 ) \ X( int, solver_niter, mjNISLAND, 1 ) \
X( mjtNum, energy, 2, 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 // 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', name='solver',
type=ArrayType( type=ArrayType(
inner_type=ValueType(name='mjSolverStat'), inner_type=ValueType(name='mjSolverStat'),
extents=(1000,), extents=(4000,),
), ),
doc='solver statistics per iteration', doc='solver statistics per island, per iteration',
), ),
StructFieldDecl( StructFieldDecl(
name='solver_iter', name='solver_nisland',
type=ValueType(name='int'), 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( StructFieldDecl(
name='solver_nnz', name='solver_nnz',
type=ValueType(name='int'), type=ArrayType(
doc='number of non-zeros in Hessian or efc_AR', inner_type=ValueType(name='int'),
extents=(20,),
),
doc='number of non-zeros in Hessian or efc_AR, per island',
), ),
StructFieldDecl( StructFieldDecl(
name='solver_fwdinv', 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::MjWarningStat> warning;
py_array_or_tuple_t<raw::MjTimerStat> timer; py_array_or_tuple_t<raw::MjTimerStat> timer;
py_array_or_tuple_t<raw::MjSolverStat> solver; 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> solver_fwdinv;
py_array_or_tuple_t<mjtNum> energy; 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 ----------------------------------- //-------------------------------- 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 // init profiler figures
void InitializeProfiler(mj::Simulate* sim) { void InitializeProfiler(mj::Simulate* sim) {
// set figures to default // set figures to default
@@ -212,6 +216,20 @@ void InitializeProfiler(mj::Simulate* sim) {
sim->figsize.figurergba[3] = 0.5f; sim->figsize.figurergba[3] = 0.5f;
sim->figtimer.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 // legends
mju::strcpy_arr(sim->figconstraint.linename[0], "total"); mju::strcpy_arr(sim->figconstraint.linename[0], "total");
mju::strcpy_arr(sim->figconstraint.linename[1], "active"); mju::strcpy_arr(sim->figconstraint.linename[1], "active");
@@ -272,54 +290,76 @@ void InitializeProfiler(mj::Simulate* sim) {
// update profiler figures // update profiler figures
void UpdateProfiler(mj::Simulate* sim, const mjModel* m, const mjData* d) { void UpdateProfiler(mj::Simulate* sim, const mjModel* m, const mjData* d) {
// update constraint figure // reset lines in Constraint and Cost figures
sim->figconstraint.linepnt[0] = mjMIN(mjMIN(d->solver_iter, mjNSOLVER), mjMAXLINEPNT); memset(sim->figconstraint.linepnt, 0, mjMAXLINE*sizeof(int));
for (int i=1; i<5; i++) { memset(sim->figcost.linepnt, 0, mjMAXLINE*sizeof(int));
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;
// y // number of islands that have diagnostics
sim->figconstraint.linedata[0][2*i+1] = d->nefc; int nisland = mjMIN(d->solver_nisland, mjNISLAND);
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;
}
// update cost figure // iterate over islands
sim->figcost.linepnt[0] = mjMIN(mjMIN(d->solver_iter, mjNSOLVER), mjMAXLINEPNT); for (int k=0; k < nisland; k++) {
for (int i=1; i<3; i++) { // ==== update Constraint ("Counts") figure
sim->figcost.linepnt[i] = sim->figcost.linepnt[0];
}
if (m->opt.solver==mjSOL_PGS) {
sim->figcost.linepnt[1] = 0;
sim->figcost.linepnt[2] = 0;
}
for (int i=0; i<sim->figcost.linepnt[0]; i++) { // number of points to plot, starting line
// x int npoints = mjMIN(mjMIN(d->solver_niter[k], mjNSOLVER), mjMAXLINEPNT);
sim->figcost.linedata[0][2*i] = i; int start = kConstraintNum * k;
sim->figcost.linedata[1][2*i] = i;
sim->figcost.linedata[2][2*i] = i;
// y sim->figconstraint.linepnt[start + 0] = npoints;
sim->figcost.linedata[0][2*i+1] = mju_log10(mju_max(mjMINVAL, d->solver[i].improvement)); for (int i=1; i < kConstraintNum; i++) {
sim->figcost.linedata[1][2*i+1] = mju_log10(mju_max(mjMINVAL, d->solver[i].gradient)); sim->figconstraint.linepnt[start + i] = npoints;
sim->figcost.linedata[2][2*i+1] = mju_log10(mju_max(mjMINVAL, d->solver[i].lineslope)); }
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 // 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 // get sizes: nv, nbody, nefc, sqrt(nnz), ncont, iter
float sdata[6] = { float sdata[6] = {
static_cast<float>(m->nv), static_cast<float>(m->nv),
static_cast<float>(m->nbody), static_cast<float>(m->nbody),
static_cast<float>(d->nefc), 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->ncon),
static_cast<float>(d->solver_iter) static_cast<float>(solver_niter)
}; };
// update figsize // update figsize
@@ -497,14 +545,22 @@ void UpdateInfoText(mj::Simulate* sim, const mjModel* m, const mjData* d,
char (&content)[mj::Simulate::kMaxFilenameLength]) { char (&content)[mj::Simulate::kMaxFilenameLength]) {
char tmp[20]; 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; mjtNum solerr = 0;
if (d->solver_iter) { for (int i=0; i < nisland; i++) {
int ind = mjMIN(d->solver_iter-1, mjNSOLVER-1); mjtNum solerr_i = 0;
solerr = mju_min(d->solver[ind].improvement, d->solver[ind].gradient); if (d->solver_niter[i]) {
if (solerr==0) { int ind = mjMIN(d->solver_niter[i], mjNSOLVER) - 1;
solerr = mju_max(d->solver[ind].improvement, d->solver[ind].gradient); 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)); 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_); 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 // prepare info text
mju::strcpy_arr(title, "Time\nSize\nCPU\nSolver \nFPS\nMemory"); mju::strcpy_arr(title, "Time\nSize\nCPU\nSolver \nFPS\nMemory");
mju::sprintf_arr(content, mju::sprintf_arr(content,
@@ -525,7 +587,7 @@ void UpdateInfoText(mj::Simulate* sim, const mjModel* m, const mjData* d,
sim->run ? sim->run ?
d->timer[mjTIMER_STEP].duration / mjMAX(1, d->timer[mjTIMER_STEP].number) : d->timer[mjTIMER_STEP].duration / mjMAX(1, d->timer[mjTIMER_STEP].number) :
d->timer[mjTIMER_FORWARD].duration / mjMAX(1, d->timer[mjTIMER_FORWARD].number), d->timer[mjTIMER_FORWARD].duration / mjMAX(1, d->timer[mjTIMER_FORWARD].number),
solerr, d->solver_iter, solerr, solver_niter,
fps, fps,
d->maxuse_arena/(double)(d->narena), d->maxuse_arena/(double)(d->narena),
mju_writeNumBytes(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 // 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 // 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) { 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 // local constants: general
const int* Madr = m->dof_Madr; const int* Madr = m->dof_Madr;
const int* parentid = m->dof_parentid; const int* parentid = m->dof_parentid;
const mjtNum* qLD = d->qLD;
const mjtNum* qLDiagInv = d->qLDiagInv;
const int* simplenum = m->dof_simplenum; const int* simplenum = m->dof_simplenum;
// local constants: island specific // 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, d->qacc_smooth, nv);
mju_copy(d->qacc_warmstart, d->qacc_smooth, nv); mju_copy(d->qacc_warmstart, d->qacc_smooth, nv);
mju_zero(d->qfrc_constraint, nv); mju_zero(d->qfrc_constraint, nv);
d->solver_iter = 0; mju_zeroInt(d->solver_niter, mjNISLAND);
TM_END(mjTIMER_CONSTRAINT); TM_END(mjTIMER_CONSTRAINT);
return; return;
} }
@@ -512,7 +512,7 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) {
// warmstart solver // warmstart solver
warmstart(m, d); warmstart(m, d);
d->solver_iter = 0; mju_zeroInt(d->solver_niter, mjNISLAND);
// run main solver // run main solver
switch ((mjtSolver) m->opt.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); mjERROR("unknown solver type %d", m->opt.solver);
} }
// one (monolithic) island
d->solver_nisland = 1;
// save result for next step warmstart // save result for next step warmstart
mju_copy(d->qacc_warmstart, d->qacc, nv); 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 // clear solver diagnostics
memset(d->warning, 0, mjNWARNING*sizeof(mjWarningStat)); memset(d->warning, 0, mjNWARNING*sizeof(mjWarningStat));
memset(d->timer, 0, mjNTIMER*sizeof(mjTimerStat)); memset(d->timer, 0, mjNTIMER*sizeof(mjTimerStat));
memset(d->solver, 0, mjNSOLVER*sizeof(mjSolverStat)); memset(d->solver, 0, mjNSOLVER*mjNISLAND*sizeof(mjSolverStat));
d->solver_iter = 0; d->solver_nisland = 0;
d->solver_nnz = 0; mju_zeroInt(d->solver_niter, mjNISLAND);
mju_zeroInt(d->solver_nnz, mjNISLAND);
mju_zero(d->solver_fwdinv, 2); mju_zero(d->solver_fwdinv, 2);
// clear collision diagnostics // 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 // SOLVER STAT
if (d->solver_iter) { if (d->nefc) {
fprintf(fp, "SOLVER STAT\n"); fprintf(fp, "SOLVER STAT\n");
fprintf(fp, " solver_iter = %d\n", d->solver_iter); fprintf(fp, " solver_nisland = %d\n", d->solver_nisland);
fprintf(fp, " solver_nnz = %d\n", d->solver_nnz); printVector(" solver_fwdinv = ", d->solver_fwdinv, 2, fp, float_format);
for (int i=0; i < mjMIN(mjNSOLVER, d->solver_iter); i++) { int nisland_stat = mjMIN(d->solver_nisland, mjNISLAND);
fprintf(fp, " %d: improvement = ", i); for (int island=0; island < nisland_stat; island++) {
fprintf(fp, float_format, d->solver[i].improvement); int niter_stat = mjMIN(mjNSOLVER, d->solver_niter[island]);
fprintf(fp, " gradient = "); if (niter_stat) {
fprintf(fp, float_format, d->solver[i].gradient); fprintf(fp, " ISLAND %d\n", island);
fprintf(fp, " lineslope = "); fprintf(fp, " solver_niter = %d\n", d->solver_niter[island]);
fprintf(fp, float_format, d->solver[i].lineslope); fprintf(fp, " solver_nnz = %d\n", d->solver_nnz[island]);
fprintf(fp, "\n"); for (int i=0; i < niter_stat; i++) {
fprintf(fp, " nactive = %d nchange = %d neval = %d nupdate = %d\n", mjSolverStat* stat = d->solver + island*mjNSOLVER + i;
d->solver[i].nactive, d->solver[i].nchange, fprintf(fp, " %d: improvement = ", i);
d->solver[i].neval, d->solver[i].nupdate); 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); 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 // save solver statistics
static void saveStats(const mjModel* m, mjData* d, int* piter, static void saveStats(const mjModel* m, mjData* d, int island, int iter,
mjtNum improvement, mjtNum gradient, mjtNum lineslope, mjtNum improvement, mjtNum gradient, mjtNum lineslope,
int nactive, int nchange, int neval, int nupdate) { int nactive, int nchange, int neval, int nupdate) {
// compute position, increase iter // if out of range, return
int i = d->solver_iter + (*piter); if (island >= mjNISLAND) {
(*piter)++; return;
// 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 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); mjtNum* ARinv = mj_stackAllocNum(d, nefc);
int* oldstate = mj_stackAllocInt(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 // precompute inverse diagonal of AR
ARdiaginv(m, d, ARinv, 0); 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]); nchange += (oldstate[i] != d->efc_state[i]);
} }
// scale improvement, save stats, count // scale improvement, save stats
improvement = rescale(m, improvement); 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 // terminate
if (improvement < m->opt.tolerance) { if (improvement < m->opt.tolerance) {
@@ -481,17 +494,20 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
} }
} }
// update solver iterations // finalize statistics
d->solver_iter += iter; if (island < mjNISLAND) {
// update solver iterations
d->solver_niter[island] += iter;
// set nnz // set nnz
if (mj_isSparse(m)) { if (mj_isSparse(m)) {
d->solver_nnz = 0; d->solver_nnz[island] = 0;
for (int i=0; i < nefc; i++) { for (int i=0; i < nefc; i++) {
d->solver_nnz += d->efc_AR_rownnz[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 // map to joint space
@@ -515,6 +531,9 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
mjtNum* ARinv = mj_stackAllocNum(d, nefc); mjtNum* ARinv = mj_stackAllocNum(d, nefc);
int* oldstate = mj_stackAllocInt(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 // precompute inverse diagonal of A
ARdiaginv(m, d, ARinv, 1); 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]); nchange += (oldstate[i] != d->efc_state[i]);
} }
// scale improvement, save stats, count // scale improvement, save stats
improvement = rescale(m, improvement); 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 // terminate
if (improvement < m->opt.noslip_tolerance) { if (improvement < m->opt.noslip_tolerance) {
@@ -698,7 +720,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
} }
// update solver iterations // update solver iterations
d->solver_iter += iter; d->solver_niter[island] += iter;
// map to joint space // map to joint space
dualFinish(m, d); dualFinish(m, d);
@@ -1514,6 +1536,9 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
mjCGContext ctx; mjCGContext ctx;
mj_markStack(d); mj_markStack(d);
// TODO: b/295296178 - Use island index (currently hardcoded to 0)
int island = 0;
// allocate context // allocate context
CGallocate(m, d, &ctx, flg_Newton); 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]); nchange += (d->efc_state[i] != oldstate[i]);
} }
// scale improvement, save stats, count // scale improvement, save stats
mjtNum improvement = rescale(m, oldcost-ctx.cost); mjtNum improvement = rescale(m, oldcost-ctx.cost);
mjtNum gradient = rescale(m, mju_norm(ctx.grad, nv)); 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); ctx.nactive, nchange, ctx.LSiter, ctx.nupdate);
// increment iteration count
iter++;
// termination // termination
if (improvement < m->opt.tolerance || gradient < m->opt.tolerance) { if (improvement < m->opt.tolerance || gradient < m->opt.tolerance) {
break; break;
@@ -1608,18 +1636,21 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
} }
} }
// update solver iterations // finalize statistics
d->solver_iter += iter; if (island < mjNISLAND) {
// update solver iterations
d->solver_niter[island] += iter;
// set solver_nnz // set solver_nnz
if (flg_Newton) { if (flg_Newton) {
if (mj_isSparse(m)) { if (mj_isSparse(m)) {
d->solver_nnz = 2*ctx.nnz - nv; d->solver_nnz[island] = 2*ctx.nnz - nv;
} else {
d->solver_nnz[island] = nv*nv;
}
} else { } else {
d->solver_nnz = nv*nv; d->solver_nnz[island] = 0;
} }
} else {
d->solver_nnz = 0;
} }
mj_freeStack(d); mj_freeStack(d);
unity/Runtime/Bindings/MjBindings.cs
+3005 -3
View File
File diff suppressed because it is too large Load Diff