An actuator now owns a block of consecutive controls
(actuator_ctrladr/ctrlnum, width defined by the actuator type) and a block
of consecutive force outputs (actuator_outadr/outnum, width defined by the
transmission type). Force outputs are the scalars of actuation space: one
force, length, velocity and moment row each. nout = dim(actuator_force) is
derived from transmission types; all current types have width 1, so all
three counts coincide for every existing model and behavior is bit-exact.
Array re-keying: ctrlrange/ctrllimited by nu; forcerange/forcelimited/gear/
acc0/length0/lengthrange and the moment row structure by nout; everything
else per actuator. The mjModel actuator block is re-sorted by size key.
Layout-breaking, not behavior-breaking: saved .mjb files are invalidated
(size list changed) and recompilation is required.
PiperOrigin-RevId: 948351772
Change-Id: Icbc196ffa083cb1eaa6f1a3710869c89d8f62540
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
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
Testing on 50 million random doubles we have branchless clip is 1.14x faster than the branched version and branchless sign is 2.27x faster than the branched version.
PiperOrigin-RevId: 943812426
Change-Id: If8003a602365601a31bec1d8b080c44d16000a4e
Deletes mj_factorI_legacy and mj_solveLD_legacy from the engine and headers.
Updates factorI, solveLD, and inertia benchmarks to remove legacy targets
and only benchmark CSR.
Rewrites engine_core_smooth_test to verify CSR solver against mj_mulM
instead of legacy solver.
PiperOrigin-RevId: 942507341
Change-Id: I4281decb7018cfa3e46cb446efd5fe1179f6ae1f
The `body_sameframe`, `geom_sameframe`, and `site_sameframe` flags are now recomputed within `mj_setConst` based on the current model geometry. This allows these flags to be updated if the model's body/geom/site frames are modified after the initial compilation. Tests are added to verify the correct recomputation and its effect on forward kinematics. The compiler also checks that its initial computation matches the result of `mj_setConst`.
Fixes#3029, #1628.
PiperOrigin-RevId: 941758649
Change-Id: I7b995f8c67ad305bca5b52732436ba578c905d73
The line search now requires a negative cost (improvement) in addition to a small derivative to declare convergence, preventing premature termination when no actual improvement has been made.
Follows the proposal in github.com/google-deepmind/mujoco_warp/pull/1471
PiperOrigin-RevId: 941579503
Change-Id: I8fcd20f7b959e50d77cd5d6de0a3c6d95f86b9e1
Similar to LightManager, the RenderableManager manages
renderables based on an mjModel and its mjData.
PiperOrigin-RevId: 941510745
Change-Id: I97b44cc857f136aa7c90ceb85a42e299fc759b73
The lazy init of log_config used a single atomic flag (env_checked) with
a load-then-store pattern, allowing two threads to both enter the init
path and concurrently write to the non-atomic log_config.topics field.
Replace with a two-phase atomic init: an atomic exchange on
env_init_claimed ensures exactly one thread enters the init, while
env_init_done (with acquire/release semantics) signals completion and
provides the happens-before edge that makes log_config writes visible to
other threads.
Also adds mj_atomic_exchange_bool to engine_crossplatform.h (both MSVC
and GCC/Clang variants).
PiperOrigin-RevId: 941202181
Change-Id: I59b6801c7b4b8b0e1647d82d2aeb534a90fb66fb
Widen snprintf buffer from 1024 to 2048 bytes in mju_defaultLogHandler
to avoid truncation warning when formatting "func: subject".
PiperOrigin-RevId: 930823501
Change-Id: I0b2246f23c44e983bea76976b684d2efdfdff29c
The sensor engine now correctly retrieves body IDs for contacts involving Flex elements by using mj_flexBody when a geom ID is -1. This allows contact sensors to detect and report forces/torques on bodies involved in Flex contacts.
PiperOrigin-RevId: 928607416
Change-Id: Ia815e1b3345bcfb393c9c4a829dc98959f2da50c
Yuval Tassa