Skip to content

Commit c836275

Browse files
yuvaltassacopybara-github
authored andcommitted
Scale solver termination by the trace of the current inertia matrix.
PiperOrigin-RevId: 1002652659 Change-Id: I70b3ab0f2a25b90dccb1aa15b5a1ba087af21be9
1 parent fded2da commit c836275

28 files changed

Lines changed: 173 additions & 218 deletions

‎doc/XMLreference.rst‎

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -477,7 +477,8 @@ adjust it properly through the XML.
477477
gradient norm. For Newton, it is additionally applied to the Newton decrement :math:`\tfrac{1}{2} g^T H^{-1} g`, the
478478
predicted cost improvement of the next iteration. Before the first iteration, CG and Newton also apply it to a
479479
:ref:`convergence certificate<soAlgorithms>` of the warmstarted solution, possibly terminating with zero iterations.
480-
Set the tolerance to 0 to disable early termination.
480+
The cost improvement and the gradient are scaled by the inverse trace of the inertia matrix, of the whole model or
481+
of a constraint :ref:`island<soIsland>`. Set the tolerance to 0 to disable early termination.
481482

482483
.. _option-ls_iterations:
483484

@@ -1253,9 +1254,11 @@ parameters.
12531254
.. _statistic-meaninertia:
12541255

12551256
:at:`meaninertia`: :at-val:`real, optional`
1256-
If this attribute is specified, it replaces the value of mjModel.stat.meaninertia computed by the compiler. The
1257-
computed value is the average diagonal element of the joint-space inertia matrix when the model is in qpos0. At
1258-
runtime this value scales the solver cost and gradient used for early termination.
1257+
This attribute is deprecated and will be removed in a future release; compiling a model which sets it gives a
1258+
warning. If it is specified, it replaces the value of mjModel.stat.meaninertia computed by the compiler. The
1259+
computed value is the average diagonal element of the joint-space inertia matrix when the model is in qpos0. It
1260+
does not affect simulation: the solvers scale their termination criteria by the trace of the current inertia
1261+
matrix, see :ref:`tolerance<option-tolerance>`.
12591262

12601263
.. _statistic-meansize:
12611264

‎doc/changelog.rst‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,12 @@ Engine
1010
- Trees marked :ref:`sleep="init"<body-sleep>` which cannot be put to sleep now cause a warning rather than an error,
1111
and start awake. See :ref:`Sleeping<siSleepSleeping>` for details.
1212
- Added single-shot :ref:`multicontact<coMultiCCD>` for geoms with positive contact :ref:`margins<body-geom-margin>`.
13+
- The constraint solvers scale their termination criteria by the trace of the current inertia matrix, of the model or
14+
of a constraint island, instead of ``mjModel.stat.meaninertia`` times ``nv``, which was computed in ``qpos0`` and used
15+
by CG and Newton only without islands. Models solved without islands, or with PGS or noslip, can stop at slightly
16+
different iterations. The :ref:`statistic<statistic>` attribute :ref:`meaninertia<statistic-meaninertia>`, which no
17+
longer affects simulation, is deprecated and will be removed in a future release (compiling a model that sets it now
18+
gives a warning; remove the attribute to migrate).
1319

1420
Compiler
1521
^^^^^^^^

‎doc/includes/references.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -64,7 +64,7 @@ typedef struct mjTimerStat_ { // timer statistics
6464
int number; // how many times was timer called
6565
} mjTimerStat;
6666
typedef struct mjSolverStat_ { // per-iteration solver statistics
67-
mjtNum improvement; // cost reduction, scaled by 1/trace(M(qpos0))
67+
mjtNum improvement; // cost reduction, scaled by 1/trace(M)
6868
mjtNum gradient; // gradient norm (primal only, scaled)
6969
mjtNum lineslope; // slope in linesearch
7070
int nactive; // number of active constraints

‎include/mujoco/mjdata.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -84,7 +84,7 @@ typedef struct mjTimerStat_ { // timer statistics
8484

8585

8686
typedef struct mjSolverStat_ { // per-iteration solver statistics
87-
mjtNum improvement; // cost reduction, scaled by 1/trace(M(qpos0))
87+
mjtNum improvement; // cost reduction, scaled by 1/trace(M)
8888
mjtNum gradient; // gradient norm (primal only, scaled)
8989
mjtNum lineslope; // slope in linesearch
9090
int nactive; // number of active constraints

‎mjx/mujoco/mjx/_src/io.py‎

Lines changed: 4 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -322,16 +322,8 @@ def _put_option(
322322
raise NotImplementedError(f'Unsupported implementation: {impl}')
323323

324324

325-
def _put_statistic(
326-
s: mujoco.MjStatistic, impl: types.Impl
327-
) -> Union[types.Statistic, types.StatisticWarp]:
325+
def _put_statistic(s: mujoco.MjStatistic) -> types.Statistic:
328326
"""Puts mujoco.MjStatistic onto a device, resulting in mjx.Statistic."""
329-
if impl == types.Impl.WARP:
330-
fields = {
331-
f.name: _wp_to_np_type(getattr(s, f.name, None), f.name)
332-
for f in types.StatisticWarp.fields()
333-
}
334-
return types.StatisticWarp(**fields)
335327
return types.Statistic(
336328
meaninertia=s.meaninertia,
337329
meanmass=s.meanmass,
@@ -424,7 +416,7 @@ def _put_model_jax(
424416
fields = {f: getattr(m, f) for f in mj_field_names}
425417
fields['cam_mat0'] = fields['cam_mat0'].reshape((-1, 3, 3))
426418
fields['opt'] = _put_option(m.opt, types.Impl.JAX)
427-
fields['stat'] = _put_statistic(m.stat, types.Impl.JAX)
419+
fields['stat'] = _put_statistic(m.stat)
428420

429421
fields_jax = {}
430422
fields_jax['dof_hasfrictionloss'] = fields['dof_frictionloss'] > 0
@@ -495,7 +487,7 @@ def _put_model_warp(
495487
private_options = {k: getattr(mw.opt, k) for k in option_keys}
496488
private_options['graph_mode'] = graph_mode
497489
fields['opt'] = _put_option(m.opt, types.Impl.WARP, private_options)
498-
fields['stat'] = _put_statistic(m.stat, types.Impl.WARP)
490+
fields['stat'] = _put_statistic(m.stat)
499491

500492
# Use MJW fields directly instead of MjModel, so that shape and dtype are
501493
# always compatible with MJXW (e.g. cam_mat0/geom_aabb).
@@ -544,7 +536,7 @@ def _put_model_cpp(
544536
fields = {f: getattr(m, f) for f in mj_field_names}
545537
fields['cam_mat0'] = fields['cam_mat0'].reshape((-1, 3, 3))
546538
fields['opt'] = _put_option(m.opt, impl=types.Impl.JAX)
547-
fields['stat'] = _put_statistic(m.stat, impl=types.Impl.JAX)
539+
fields['stat'] = _put_statistic(m.stat)
548540

549541
# get the pointer address
550542
# we use a 0-d array

‎mjx/mujoco/mjx/_src/solver.py‎

Lines changed: 18 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -418,8 +418,21 @@ def _update_gradient(m: Model, d: Data, ctx: Context) -> Context:
418418
return ctx
419419

420420

421-
def _rescale(m: Model, value: jax.Array) -> jax.Array:
422-
return value / (m.stat.meaninertia * max(1, m.nv))
421+
def _trace_m(m: Model, d: Data) -> jax.Array:
422+
if not isinstance(d._impl, DataJAX):
423+
raise ValueError('_trace_m requires JAX backend implementation.')
424+
if not m.nv:
425+
return jp.array(mujoco.mjMINVAL)
426+
trace = (
427+
d._impl.M[m.M_rowadr + m.M_rownnz - 1].sum()
428+
if support.is_sparse(m)
429+
else jp.trace(d._impl.M)
430+
)
431+
return jp.maximum(mujoco.mjMINVAL, trace)
432+
433+
434+
def _rescale(m: Model, d: Data, value: jax.Array) -> jax.Array:
435+
return value / _trace_m(m, d)
423436

424437

425438
def _linesearch(m: Model, d: Data, ctx: Context) -> Context:
@@ -440,7 +453,7 @@ def _linesearch(m: Model, d: Data, ctx: Context) -> Context:
440453
):
441454
raise ValueError('_lineasearch requires JAX backend implementation.')
442455

443-
smag = math.norm(ctx.search) * m.stat.meaninertia * max(1, m.nv)
456+
smag = math.norm(ctx.search) * _trace_m(m, d)
444457
gtol = m.opt.tolerance * m.opt.ls_tolerance * smag
445458

446459
# compute Mv, Jv
@@ -582,8 +595,8 @@ def solve(m: Model, d: Data) -> Data:
582595
raise ValueError('solve requires JAX backend implementation.')
583596

584597
def cond(ctx: Context) -> jax.Array:
585-
improvement = _rescale(m, ctx.prev_cost - ctx.cost)
586-
gradient = _rescale(m, math.norm(ctx.grad))
598+
improvement = _rescale(m, d, ctx.prev_cost - ctx.cost)
599+
gradient = _rescale(m, d, math.norm(ctx.grad))
587600

588601
done = ctx.solver_niter >= m.opt.iterations
589602
done |= improvement < m.opt.tolerance

‎mjx/mujoco/mjx/_src/types.py‎

Lines changed: 1 addition & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -475,13 +475,6 @@ class Statistic(PyTreeNode):
475475
center: jax.Array
476476

477477

478-
class StatisticWarp(mjxw_types.StatisticWarp, Statistic):
479-
"""Warp-specific model statistics."""
480-
481-
# NB: StatisticWarp type annotations may not match those on Statistic.
482-
pass
483-
484-
485478
class OptionJAX(PyTreeNode):
486479
"""JAX-specific option."""
487480

@@ -654,7 +647,7 @@ class Model(PyTreeNode):
654647
npluginstate: int
655648
nhistory: int
656649
opt: Option
657-
stat: Union[Statistic, StatisticWarp]
650+
stat: Statistic
658651
qpos0: jax.Array
659652
qpos_spring: jax.Array
660653
body_parentid: np.ndarray

‎mjx/mujoco/mjx/codegen/generate_warp_shim.py‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -196,7 +196,6 @@ def _global_assignments():
196196
('_m', 'mjwarp.Model'),
197197
('_d', 'mjwarp.Data'),
198198
('_o', 'mjwarp.Option'),
199-
('_s', 'mjwarp.Statistic'),
200199
('_c', 'mjwarp.Contact'),
201200
('_e', 'mjwarp.Constraint'),
202201
('_cb', 'mjwp_types.Callback'),
@@ -325,7 +324,7 @@ def _jax_shim_fn(
325324
elif mjwarp_field_info[arg].param_source == 'Model':
326325
arg_jax = (
327326
arg.replace('__', '.')
328-
if arg.startswith('opt') or arg.startswith('stat')
327+
if arg.startswith('opt')
329328
else arg
330329
)
331330
public_field = arg.split('__')[0] in mjx_types.Model.__annotations__
@@ -451,7 +450,6 @@ def create_jax_warp_shim(
451450
def _{fn_name}_shim(
452451
{fn_args_raw_str}
453452
):
454-
_m.stat = _s
455453
_m.opt = _o
456454
_m.callback = _cb
457455
_d.efc = _e

‎mjx/mujoco/mjx/codegen/generate_warp_types.py‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,6 @@
4949
'Model': mjwarp.Model,
5050
'Data': mjwarp.Data,
5151
'Option': mjwarp.Option,
52-
'Statistic': mjwarp.Statistic,
5352
}
5453

5554

@@ -413,7 +412,7 @@ def write_core_cls(
413412
extra_annotations: dict[str, type] | None = None,
414413
defaults: Optional[Dict[str, Any]] = None,
415414
):
416-
"""Writes a core API class (e.g. Model/Data/Option/Statistic)."""
415+
"""Writes a core API class (e.g. Model/Data/Option)."""
417416
cls = _CLS_MAP[cls_name]
418417

419418
annotations = dict(cls.__annotations__)
@@ -562,7 +561,6 @@ def main(argv):
562561
write_nested_dataclass(target_fpath, mjwarp._src.types.TileSet)
563562
write_nested_dataclass(target_fpath, mjwarp._src.types.BlockDim)
564563

565-
write_core_cls('Statistic', target_fpath, mjx_types_fpath, set_diff=False)
566564
write_core_cls(
567565
'Option', target_fpath, mjx_types_fpath,
568566
extra_annotations={'graph_mode': GraphMode},

‎mjx/mujoco/mjx/codegen/trace.py‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -287,7 +287,6 @@ def get_mjwarp_field_info(
287287
"""Return field info for mujoco_warp/_src/types.py."""
288288
dataclass_map = {
289289
'opt': 'Option',
290-
'stat': 'Statistic',
291290
'efc': 'Constraint',
292291
'contact': 'Contact',
293292
}
@@ -314,7 +313,7 @@ def get_fields(cls_name, base_order, sub_order):
314313
# to Model or Data based solely on the field name lookup in mjwarp_field_info
315314
# (via field_info.param_source) rather than inspecting base variable names.
316315
#
317-
# Nested sub-dataclasses (Option, Statistic in Model; Constraint, Contact in
316+
# Nested sub-dataclasses (Option in Model; Constraint, Contact in
318317
# Data) are flattened with container prefixes (e.g. 'opt__timestep',
319318
# 'contact__pos'). This prefixing prevents collisions between sub-dataclasses
320319
# that share generic field names (such as 'pos' or 'type' in Contact and

0 commit comments

Comments
 (0)
⌂ 换网址