When penetration exceeds a box's smallest half-size, the midpoint
between the contact surfaces can land outside both boxes. The outside-
box filter in mjc_BoxBox then removed every contact of the manifold,
returning nothing for a visibly overlapping pair, letting boxes fall
through each other. Fixes#1800.
If the filter would remove all contacts, restore the penetrating ones.
This is strictly additive: configurations where any contact survives
the filter are unchanged.
The removed midpoints are bitwise-identical to the witness midpoints
computed by mj_geomDistance for these configurations, so the positions
follow the engine-wide contact position convention; re-anchoring them
onto a box surface would not.
Fixes#1800
PiperOrigin-RevId: 957867315
Change-Id: Ia9c858661d4badeb2a832d25455e33402936011d
In the edge-edge path of the box-box collider, when line clipping yields
no points, the corner generators accept points whose projection
parameters are out of range and clamp them into the valid range. The
depth of such a point is the Euclidean distance between two unrelated
points, mixing lateral offset into penetration depth, and on the
penetrating side it is admitted with no margin check. For thin boxes
meeting edge-to-face within margin, this produced a contact with
penetration three orders of magnitude larger than the boxes' true
separation, exploding resting stacks.
No contact can penetrate deeper than the support-interval overlap along
the separating axis, which the SAT stage has already computed. Enforce
this bound on all points emitted by the edge-edge path. The bound
carries margin plus relative and size-scaled slack covering rounding
error: the depth of the deepest legitimate point is algebraically equal
to the bound, so an exact comparison would drop real contacts. The
slack is precision-dependent: in mjUSESINGLE builds the two
computations of the same overlap disagree by tens of ulps, and slack
calibrated for double precision rejects real single-precision contacts.
Differential fuzzing against the nativeccd oracle over 200k random
near-contact thin-box configurations, in both precisions: impossibly
deep contacts drop from 1018 to 4 (worst excess from 2.5x the bounding
diameter to 0.001x), with no legitimate shallow-penetration contact
lost.
PiperOrigin-RevId: 957628830
Change-Id: Iaa1f10742f91c55bf831296cba0936eb50ea09b0
<pid kp kv|dampratio [ki imax] [slewmax]> is a PID controller with real position and velocity setpoint inputs on a single force output, plus an optional feedforward input. With a zero velocity setpoint it reproduces <position> bit-exactly; the input signature is any subset of [pos, vel, ff], selected with input="..." and recorded as mjtCtrlInput bits in
actuator_ctrlspec; absent setpoint inputs are fixed at zero, so the control vector contains no inert entries.
kp and kv are single-sourced in the affine bias parameters (biasprm[1,2]) with no gainprm mirror: every consumer of the position-servo shape
(dampratio conversion, inheritrange, qDeriv) reads one location, which is what makes the bit-exact <position> parity possible. Controller state uses dyntype 'pid' with slot-gated activations in the order [slew, integral], following the dcmotor slot idiom: slewmax (dynprm[1]) rate limits the effective position setpoint through an activation holding it;
ki (gainprm[0]) integrates the position error -- wrapped on rotational transmissions -- with anti-windup clamping of the integrand at imax (dynprm[0]). Both features require the pos input. Servo input unpacking is shared with the dcmotor controller (unpackServoInputs); per-input ranges are exposed as posrange/velrange/ffrange.
This subsumes the functionality of the mujoco.pid plugin with proper activation state: correct under all integrators, visible to keyframes, act sensors and reset. Migration: kp/ki/kd map to kp/ki/kv, plugin imax is in force units (divide by ki), slewmax carries over; the single ctrl becomes input="pos".
PiperOrigin-RevId: 957588898
Change-Id: Id2786836ca6e76f58e5b5cc8323fc23be0a53784
The DC motor's LuGre bristle state must integrate the velocity of its own
transmission, so actuator ordering cannot affect it. The test places a
multi-output SO3 actuator before the DC motor, making the motor's actuator
id and output address diverge, and requires the bristle state to match the
motor-first ordering. Currently fails: the exact ZOH update in
mj_nextActivation reads actuator_velocity[actuator_id] instead of the
motor's own actuator_velocity[outadr], so the bristle integrates the SO3
actuator's velocity.
Every step, the flex block of the implicit effective metric M + K was
factorized by sparse Cholesky, because K depends on the configuration. On
model/flex/bag.xml, added here, that is roughly half the step, against a
comparable share for the constraint solve it exists to accelerate.
Keep only the metric's per-vertex 3x3 diagonal blocks, prefactored. Neither
consumer needs the exact inverse: the CG constraint solver only wants a
preconditioner, and qacc_smooth can come from an iterative solve using those
blocks. They are O(n) to build and to apply, but weaker, so CG runs about twice
the iterations and qacc_smooth becomes an iteration rather than a direct solve.
Net, the bag model steps roughly twice as fast.
The preconditioner, by metric state. Inactive, meaning no flex elasticity or an
explicit integrator: M^-1, unchanged. Bending only (nefmK == 0): M^-1 plus the
exact constant bending factor from mj_setConst on the dofs it covers,
unchanged; that factor is built at model compile time and costs nothing per
step. Per-step stiffness: M^-1 plus the 3x3 blocks, where before it was a
per-step sparse Cholesky, or, when M couples across the flex block, an inner
PCG of up to 50 iterations run once per outer CG iteration.
Only models carrying per-step stretch stiffness change in wall-clock. Both
ponchos hold their timing and take slightly fewer CG iterations than before,
because the preconditioner is now symmetric: it applies M^-1 and the covered
blocks to disjoint sets of dofs, where previously the two overlapped and the
operator was not symmetric, which PCG requires.
mjd_effSolve is the accurate solve of (M + K)x = b; what used to carry that
name only preconditions and is now mjd_effPrec. Its CG guarded the division by
pAp with mjMINVAL, an absolute floor on a quantity that scales with the square
of the right-hand side, so a small b aborted the solve while the curvature was
healthy: four flex models were quietly left short of tolerance. For an SPD
metric the guard is positivity, and with that the same solves converge. The qacc_smooth call site in
mj_fwdAcceleration is textually unchanged but now reaches the iterative solve,
which converges on opt.tolerance rather than a hardcoded threshold, floored in
mjUSESINGLE builds where the squared target is unreachable in float. Reaching
the iteration cap names the ill-conditioned flex stiffness and then reports it
through mjWARN_INERTIA, rather than returning an under-converged result.
Covered dofs are located by walking the covered rows of the stiffness matrix,
as they need not be 3-aligned from dof 0: any joint declared before a flexcomp
shifts them.
mjData.efm_L_rownnz, efm_L_rowadr and efm_L_colind described the sparsity of
the deleted factorization and are removed: left NULL with nonzero mjxmacro
extents they made the Python bindings hand back uninitialized arrays.
efm_active loses the value 2 for the same reason, nothing selects a solve path
on preconditioner exactness any more. Both are recorded under breaking changes.
model/flex/bag.xml is added because no shipped model carried per-step stretch
stiffness. The ponchos are bending-only and trampoline.xml uses an explicit
integrator, so the metric never activates there. It is excluded from
WriteReadCompareTest: stretch stiffness amplifies rest geometry that XML rounds
on save.
The implicit effective metric assembles the stretch stiffness from
world-space edge vectors, but a flex vertex body's slide dofs are
expressed in its parent body's frame. When that frame is rotated the
assembled operator is therefore not the Jacobian of the passive stretch
force, which mj_flexPassiveStretch already maps into the dof frame with
xmat^T. The metric is then inconsistent with the force it linearizes:
implicit integration loses its stability guarantee, and models that the
same flex handles comfortably in an unrotated frame diverge.
Apply the matching change of basis in both places that build or apply the
stretch stiffness: mjd_flexStretch_mul rotates the input dof vector into
world and the scattered result back, and mjd_flexStiff_assemble sandwiches
each 3x3 block as R_bi^T * blk * R_bj. Both are no-ops when the parent is
unrotated. Bending needs no change: its blocks are isotropic, and
R^T (q I) R = q I.
This completes the fix in fe9dc584, which covered the passive force paths
and the interp (trilinear) derivative but not the standard stretch one.
On a mesh flex inside a body with a 90-degree rotation, the metric's
directional agreement with the force Jacobian goes from cos = 0.57 to
cos = 1.0, and a hanging sheet that previously reached 176% strain
settles at 0.87%.
Fast path in mj_flexPassiveInterp, mj_flexPassiveBendInterp, and
mj_flexPassiveStretch assumed body slide joints are world-aligned
(J = I). When parent body has a non-identity quaternion, joint axes
are rotated (J = R_body), causing wrong force mapping and instability
(NaN/Inf in QACC).
Fix: project world-frame forces onto body local frame via
mju_mulMatTVec3(R_body^T, force) before adding to qfrc_spring/damper.
Also fix the derivative paths:
- mjd_flexInterp_kernel fast path: R^T * K_rot * R * vec
- mjd_flexStiff_assemble (CSR): R_bi^T * K_rot_block * R_bj
The CSR-assembled stiffness matrix is the actual path used by the
implicit CG solver (mj_flexCG gate). The test uses solver="CG" to
activate this path; without it, flex stiffness is integrated
explicitly and no derivative fix can help.
Ported from GitHub PR https://github.com/google-deepmind/mujoco/pull/3379
Original author: Devansh (https://github.com/devansh0703)
Fixes https://github.com/google-deepmind/mujoco/issues/3364
PiperOrigin-RevId: 952789284
Change-Id: If924f7160dd16c0cc88170d80e6e605da0fe2e04
https://youtu.be/17XpwnqyCXs
New transmission type mjTRN_SO3: a relative orientation, targeting a ball
joint or a site+refsite pair. It is the first transmission with more than
one force output: its length is the norm of the expmap vector of the
relative rotation and its moment axes are the 3 rows of the
relative rotational Jacobian, without projecting onto per-actuator gears.
New force law mjGAIN_SO3/mjBIAS_SO3: a geodesic PD servo, force =
kp * log(q_current^-1 * q_target) - kv * velocity, exact for arbitrary axis
combinations with a unique equilibrium at every commanded orientation.
Error, moment rows and velocity all live in the child frame (joint or
site): the right-difference error is the gradient of the geodesic
potential in that frame. The parent-frame (left) error is not: driving
child-frame torques with it pumps energy at large angles, settling into
steady-spinning limit cycles (the SO3LargeAngleConvergence test). The
integrator variant stores the 3D orientation setpoint in act (actnum = 3,
re-anchored to a bounded representative at integration time). Exposed in
MJCF as <orientation joint=|site=+refsite= kp kv|dampratio>, or via
<general gaintype="so3" biastype="so3">.
The setpoint input has two charts: an expmap target (3 controls, default)
or a quaternion target (4 controls) -- <orientation input="quat">, the
first actuator with different input and output widths. The signature is
recorded in a new per-actuator field actuator_ctrlspec (mjtCtrlChart),
whose meaning is scoped by the gain type the way gain/bias parameters are;
ctrlnum is derived from it at compile time and remains the layout
authority. An explicit field rather than width inference or a prm slot:
width-as-chart cannot express same-width signatures (upcoming servo input
subsets), and prm slots are the input_mode pattern this stack retires.
The force law normalizes the commanded quaternion, making it scale- and
antipodally-invariant. The all-zero ctrl still maps to the identity via
mju_normalize4, but it is a degenerate point (a nudge of any component
commands a half-turn), so quat inputs reset to the identity quaternion:
new mj_resetCtrl sets neutral ctrl values (zero, except qw = 1), called
by mj_resetData and the viewers' Clear All. The quat chart is
restricted to dyntype 'none': integrating a quaternion setpoint linearly
is not meaningful on the manifold. New mjsActuator.ctrlspec field carries
the signature through the spec and XML round-trip.
Actuator sensors (actuatorpos/vel/frc) now report one value per force
output; dim = 3 on an SO3 actuator.
As the first actuator with nu != nactuator, this commit also makes the
viewers multi-input aware: the control sliders in simulate and studio,
which indexed per-actuator arrays by control index (out of bounds on
this model class), are generated per control and labeled with the
actuator name plus an input suffix ("orient/qw"), via the new
introspection helper mj_actuatorInputName -- the single source of truth
for input names, extended by each new multi-input type (quaternion
components are w-first: qw, qx, qy, qz). Slider ranges now honor a
defined ctrlrange even when ctrllimited is false: range is the UI hint,
limited is the clamp -- wrapped and expmap setpoints are unbounded but
still want finite sliders, while quat components are truly bounded.
The rotational demo model is orientation.xml under
test/engine/testdata/actuation/, upgraded to a three-way contrast:
per-axis wrapped servos vs an expmap-commanded vs a quat-commanded
orientation actuator, on identical checker-textured boxes. It is loaded
by the mixed-axis contrast and input-name tests, and doubles as the
viewer test model (slider groups of 3 independent, 3 grouped, 4 grouped).
PiperOrigin-RevId: 951607063
Change-Id: If235dba8e2f2ca72672e7c62531a27e967c6a373
Store island topology in `mjData` so repeated island solves can reuse stable connect/weld equality partitions. Add cache invalidation checks for active equality changes and cover the fast path with an island regression test.
Signed-off-by: teerthsharma <teerths57@gmail.com>
https://youtu.be/GioWwB36XHI
The new geom attribute adhesion (units of force, signed; pair-level
override) translates the contact friction cone along its normal so
that the force origin lies strictly inside it. Consequences: each
contact can pull with up to the given force before breaking, and the
tangential friction budget becomes mu*(f_N + adhesion) -- the
Mohr-Coulomb yield condition with cohesion c = mu*adhesion -- so
lightly-squeezed grasps retain a guaranteed friction floor.
A translated cone factors exactly into {constant attractive force}
+ {original cone}, so no solver kernels change. The implementation is
this factorization: a constant attraction along contact normals
accumulated into the new mjData.qfrc_adhesion (summed into
qfrc_passive), plus a bias of adhesive contact rows' reference
acceleration (aref += R*adhesion), which makes resting penetration
exactly independent of adhesion. Contacts of adhesive pairs remain
active throughout the gap zone, producing rows with positive violation
whose reference acceleration pulls: a tether that resists pull-off
smoothly, captures objects released within the band into steady
contact, and detaches at the specified force. Adhesion values of the
two geoms combine by sum; explicit pairs override.
mj_contactForce reports the net interface force (cone force minus the
adhesive pull), whose normal component can now be negative. Negative
adhesion is allowed and produces a repulsive offset (air hockey).
PiperOrigin-RevId: 950858148
Change-Id: I879c08eba7ae501e5c0f8c2f807167344da4c2bc
- Wrapping in force path is local; act is re-anchored at integration time.
- Remove hardcoded `actrange` for intvelocity actuators.
PiperOrigin-RevId: 949566477
Change-Id: I349fdf17eedfbb2174d698cc1a6a91d52810b4a3
https://www.youtube.com/watch?v=PdSdrqhSiZA
The new geom attribute surfacevel (6 numbers: linear and angular velocity in the geom's local frame, angular about the geom frame origin) specifies the velocity of the geom's surface material relative to the geom frame. The relative surface velocity of the two geoms is added to the tangential contact rows of efc_vel in mj_referenceConstraint, so friction drives touching bodies toward the motion of the surface: objects on a conveyor are transported at belt speed, turntables impart omega x r with torsional spin-up for condim >= 4, and surface velocities compose with each other and with body motion. The component along the contact normal is projected out: probe experiments showed that velocity-space emission chatters mass-independently and ingestion merely deepens penetration; normal-direction effects belong to force-space features.
surfacevel is interpreted in the geom frame as authored: for mesh geoms, whose compiled frame absorbs the mesh centering and principal-axes transform, the compiler re-expresses the authored value in the compiled frame.
No special interaction with sleeping: objects being transported do not fall asleep because they are moving; objects at rest on an active surface may sleep like any other resting object.
Includes showcase models (model/surfacevel/): a luggage carousel whose ring is a spinning square-profile supertorus fed by a cascade of belts with matched spinning end rollers, bags dropping in and circulating indefinitely; and a treadmill with a passive humanoid.
PiperOrigin-RevId: 948647785
Change-Id: I0c6559a91cc7ece1237eb8ac2e51986e7342d962
This CL replaces the post-hoc implicit flex correction (`flexInterp_cgsolve`) with a **linearly-implicit effective metric** `M̃ = M + (h² + h·damping)·K` carried by the CG constraint solver itself. Contact/friction forces and implicit flex elasticity are now computed against one consistent metric, instead of the solver seeing `M` and a post-solve correction changing `qacc` behind its back.
Gate (unchanged semantics): `solver="CG"` + implicit/implicitfast integrator + pyramidal cones + flex stiffness present. Newton and PGS are untouched. `solver="CG"` remains the user-facing contract — the factorization is an implementation detail of the preconditioner.
### What's in the metric
- **mjData `efm_*`** (arena, efc-like lifetime/skip semantics; built in `mj_fwdPosition`, value-refreshed in `mj_fwdVelocity`): the per-step stiffness CSR `efm_B_*`, its reverse-Cholesky factor `efm_dofid` + `efm_L_*` (nested-dissection ordered, separators-first for the reverse factorization), and the smooth-force shift `efm_c = h·K·qvel`.
- **`mjd_flexStiff_assemble`** now assembles stretch (Gauss–Newton), standard dim-2 bending, and — via the cached corotated stiffness `d->flexelem_krot` — interp stiffness (all node bodies on simple sliders: point Jacobian is I₃, `flex_centered` not required; fixed nodes drop like pins) into one dof-level CSR. `mjd_effMulAdd`/`mjd_effSolve` apply the metric, with matrix-free operator fallbacks where assembly does not apply.
- **mjModel `efm0_*`** (`nefm0dof`/`nefm0L`): the constant part of the metric factor — currently the dim-2 bending factor, computed once in `mj_setConst` — so bending-only models pay zero per-step factorization cost. Naming mirrors mjData's `efm_*` with the standard `0`-suffix (reference/constant) idiom, and is deliberately not bending-specific: future constant contributors extend it without renames.
- The solver consumes the metric through pre-shifted `qfrc_smooth` and the metric products `Ma`/`Mv`/`Mgrad`; `qacc_smooth` becomes the unconstrained minimizer of the implicit dynamics, which makes the no-constraint shortcut and the warmstart choice consistent by construction.
- **`mj_inverse` adds `B·qacc − c`**, making inverse dynamics discrete-consistent with the gated forward dynamics — exact, since the gated path has no qDeriv term (new test `ForwardTest.GatedFlexInverseConsistency`).
### Performance
All numbers: ms/step over the same 2000-step window, models as shipped on each side (old code with the old model settings vs this CL with the new ones).
The new solver path activates on exactly two shipped models — the ponchos, the only flex models that need an implicit integrator (poncho on Euler degenerates to >200 ms/step). For them, this CL trades speed for consistency: the implicit bending solve now runs inside every solver iteration, where the contact solve can see the stiffness, instead of once after the solve. Solver iterations drop because the curvature is visible, but each iteration pays for the implicit solve:
| model | before | after | solver iters/step |
|---|---|---|---|
| poncho | 2.47 | 3.30 (1.33×) | 16.8 → 11.8 |
| poncho_edgeequality | 1.96 | 2.72 (1.39×) | 13.2 → 10.0 |
What that price buys: contact forces consistent with the implicit elasticity (previously the post-hoc correction changed `qacc` after the constraint solve), discrete-consistent inverse dynamics, and the removal of the post-hoc special case from the integration path. Raising poncho's timestep from 2 to 5 ms leaves its per-step cost nearly flat, so the consistency price can be recovered by taking fewer steps where accuracy allows.
Every other flex model was measured stable on Euler at its shipped timestep and switches to it (these models predate the post-hoc integrator; implicit was never load-bearing for them). They end up equal or faster than before: bunny_multicell 0.47 → 0.40, trampoline 0.28 → 0.25, plate 1.02 → 0.99, pancake 0.34 → 0.33.
Finally, the per-step factorization makes configurations practical that the old code could only integrate explicitly: implicit stretch elasticity (`elastic2d="stretch"`/`"both"`, dim-3 solids) and factorized interp stiffness. No before/after exists for these — stock has no implicit treatment of stretch at all.
### Behavior changes
- With the post-hoc correction deleted, interp/bending models running `solver="Newton"` (or elliptic cones, or islands) now integrate flex elasticity **explicitly** (previously: post-hoc implicit). Affects e.g. `gripper_trilinear` (stable, and faster, but different semantics). Follow-up options: Newton-side metric support, or a documented fallback.
- With the gate on, `mj_forward` outputs are timestep-dependent for gated models (they answer the linearly-implicit discrete problem); `qacc_smooth` and `mj_inverse` change accordingly. Non-gated models are bit-identical (full suite green throughout).
### Validation
- 1737/1737 tests, including new: `FlexStretchDerivatives` (FD-validated GN operator), `FlexStiffAssemble`/`FlexStiffAssembleInterp` (CSR ≡ operators), `GatedFlexInverseConsistency` (fails pre-change), equivalence tests vs the old post-hoc treatment (bending matches to 2e-11).
- Fingerprint discipline throughout: bending-only models bit-exact across every refactor; permutation/kernel changes verified iteration-identical.
### Known follow-ups (not in this CL)
3×3-block sparse Cholesky kernel (the numeric factorization is index-bound; projected ~3× on the factor); mjModel persistence of the factor's symbolic pattern (rest-pose ND makes sizes compile-time); the general effective-metric mode (all solvers, all PSD-safe force classes, behind an enable flag).
PiperOrigin-RevId: 948561856
Change-Id: I8b8e32ebd0428042af71647d0470d10773bf6daf
The gyroscopic (bias) derivatives applied to standalone free bodies by the
implicitfast integrator provide comparable stability for spinning bodies,
with none of midpoint's restrictions: they apply under contacts, fluid
forces and constraints, and preserve the linear force-velocity relation
required by discrete-time inverse dynamics. The invdiscrete flag reverts to
its original single meaning and no longer affects forward dynamics.
Restore implicitfast coverage in the DiscreteInverseMatch test, removed
when midpoint made discrete inverse dynamics untestable.
Add implicit gyroscopic (bias) derivatives for free bodies in implicitfast.
The implicitfast integrator drops the RNE (bias) derivative to stay on the
symmetric Cholesky path, so fast-spinning free bodies integrate gyroscopic
forces explicitly and can gain energy. Symmetrizing the gyroscopic Jacobian
is not an option: its stabilizing content is the antisymmetric part, and
adding only the symmetric part is destabilizing.
Instead, exploit the fact that for a standalone free body the 6x6 block of
M - h*D is decoupled from the rest of the system (qDeriv sparsity is
tree-local): after the global solve, rebuild the block with the exact bias
derivative in closed form (mjd_freeBias_vel) and re-solve it with dense
unsymmetric LU, overwriting the block's rows of qacc. For lone spinning
bodies this makes implicitfast match implicit to rounding, at ~150ns per
eligible body: cheaper than the midpoint machinery it will replace.
Eligibility is structural only; contacts, fluid and constraints need no
gating. The same block is mirrored in discrete inverse dynamics
(mj_discreteAcc), making invdiscrete exact for spinning free bodies.
PiperOrigin-RevId: 948472495
Change-Id: I813ef3d98c7b399881bc8603b9f9208cfb02eb58
The primal cost has curvature of at least M in every zone, making it strongly
convex in the M-norm and bounding the suboptimality of any point by the
Fenchel duality gap at its constraint forces:
cost(qacc) - cost* <= 0.5*grad'*M^-1*grad
Since M's factorization always exists, this certificate is evaluable before
the solver does any work: one triangular solve and one dot product. When the
warmstarted solution is already certified to satisfy the tolerance, CG and
Newton now return with zero iterations; for Newton this skips building and
factorizing the Hessian. If the certificate declines, Newton gets a second
exit after factorization: the Newton decrement, checked before the first
line search.
Because the gap bounds cost suboptimality, stiff constraints can convert it
into force errors of order sqrt(2*gap*stiffness). Newton solutions are
characteristically force-accurate, so Newton zero-iteration exits also
require the gradient criterion, preserving constraint-force accuracy at
rest; CG solutions are characteristically cost-accurate and exit on the gap
alone.
On a settling pile of 50 boxes (300 dofs, ~200 contacts), end-to-end time
per step drops 13% over a settle-then-rest run and 27% in the quiescent
limit, with Newton iterations falling from 0.98 to 0.40 per step.
Tests: WarmstartZeroIterations sweeps solver/cone/jacobian on a settled box,
asserting zero iterations, forward/inverse consistency, and agreement with a
tolerance=0 control solve from the same state. WarmstartZeroIterationsIslands
checks per-island exits with a kicked box next to a settled one.
RefsiteConservesMomentum now requests an exact solve (tolerance=0), since it
asserts momentum conservation tighter than the solver tolerance contract.
PiperOrigin-RevId: 947993735
Change-Id: I2fd855774bff619709b2c386f1ba2714286e0821
After an accepted line-search step, the solver has already rebuilt the gradient
and Hessian and solved for the next search direction, so the Newton decrement
0.5*g'*H^-1*g -- the quadratic model's predicted cost improvement of the next
iteration -- costs one dot product. Terminating when it falls below tolerance
avoids running one more iteration only to observe a correspondingly small
actual improvement.
This is a C port of Alain's proposal in MJWarp:
https://github.com/google-deepmind/mujoco_warp/pull/1520
PiperOrigin-RevId: 947768034
Change-Id: I94e5c71a4e2b4a7775611edd1dad254bba2633b4
mju_factorLU6/mju_solveLU6: same algorithm as mju_factorLU/mju_solveLU
with compile-time size, allowing full unrolling. At n=6, factor+solve is
25% faster than the runtime-sized version (93 vs 124 ns), and fixed-size
LU factorization is faster than generic dense Cholesky (55 vs 61 ns):
at this size, runtime-n loop overhead outweighs Cholesky's 2x flop
advantage. See new lu_benchmark_test. Results agree with the generic
version to rounding, not bitwise: the compiler may fuse (FMA) the
unrolled version differently.
Also add two DenseLU tests: a pivoting-required matrix with zero
diagonal, and fixed-vs-generic agreement.
PiperOrigin-RevId: 947705056
Change-Id: I24c54c9510964aa376886e9dd721890eda9889d3
The mid-phase BVH descent filter prunes body pairs using body_margin,
which was compiled as the max over geom margins, excluding gap. Broadphase
and the leaf-level test both use margin+gap, so any multi-geom body relying
on gap could silently lose its in-gap contacts when raw AABBs don't
overlap. Single-geom bodies take the leaf-leaf path and were unaffected.
PiperOrigin-RevId: 946967548
Change-Id: I6d92baa296f1a83be68b4dbfd64a96d1c7efd3c4
Reformulate the cost difference calculation (`ellipticCostDif`) to use mathematically equivalent formulas that avoid subtracting large, nearly equal values (cancellation errors) in single precision at high normal forces.
This is a C port of Alain's formulation in MJWarp:
https://github.com/google-deepmind/mujoco_warp/pull/1512
Also adds an integration test (`EllipticLineSearchPrecisionDiagnostics`) that reproduces the precision issue under large normal forces in the sliding regime, and asserts that the solver does not produce large negative improvements in either precision. This test failed before the change.
PiperOrigin-RevId: 946137815
Change-Id: Ia8fc1c4823b5fee770140c8989b9465737d22ad7
Extend mjpResourceProvider with an optional write callback (write)
so that mj_encode, mj_saveXML, and mj_saveModel can write to any
registered provider.
PiperOrigin-RevId: 945202741
Change-Id: I37903425260932e555f4a8c2392c4ff8c2e6cc06
The flexcomp compiler previously rejected pins on dim-2 flexes with
bending (elastic2d bend/both). Allow them: mj_flexPassiveBend treats a
pinned vertex (body without 3 free slide dofs) as static -- zero
velocity, and no bending force applied to it (the reaction is carried
by the pin) -- while its position still enters every neighbor's
bending force, which is exactly what the pin constrains.
PiperOrigin-RevId: 944410969
Change-Id: Ib0a69c8d5fb6f64d3e2a76af3b6b2c7be1898191
Yuval Tassa