Add an IPC-style integrator for penetration-free flex contact - #3420
Open
smallquail wants to merge 15 commits into
Open
Add an IPC-style integrator for penetration-free flex contact#3420smallquail wants to merge 15 commits into
smallquail wants to merge 15 commits into
Conversation
…lexes Port of the IPC integrator from feature/ipc-shell (99cb713), restricted to edge-equality flexes: the solver carries no elastic energy of its own -- stretch rides the native edge-equality efc rows, and FEM elasticity under integrator="ipc" is rejected at load with a clear error (FEM belongs to the CG integrators' effective metric, which is untouched). Contents: engine_ipc.c/h (AL/CCD contact over flex vertices with injected extra-primal rows), the extra-primal hook in the CG solver, integrator dispatch in forward/inverse, the mjINT_IPC enum, XML parsing, CMake, and the engine_ipc_test suite.
beta is the fraction of the step's motion the committed positions have absorbed, not a residual: terminating at the old feasibility threshold (beta >= 0.9) discards up to 10% of the motion while time still advances by h -- time-dilated physics that reads as a slow-motion freeze when contact-rich phases sustain beta ~0.9 commits for hundreds of steps. Partial advances remain legal within the outer loop; a commit requires beta ~ 1, and the existing stall path is the loud escape when the CCD cannot complete the step.
Two guard clauses carried a trailing statement on the same line (-Werror=misleading-indentation on the GCC CI build); split them.
Mechanical reformat (Google style, 100-column limit): one statement per line, split compressed brace-openers, wrapped long lines. Verified behavior-neutral: the token stream is unchanged modulo preprocessor line continuations, and the benchmark trajectory is bit-identical.
Move the distance kernels (point-triangle, segment-segment, geom SDF), sharp vertex/edge extraction, BVH traversal, candidate generation, contact linearization, and the conservative CCD advance into a new engine_collision_continuous.c/h, alongside the discrete collision pipeline files. engine_ipc.c retains only the integrator: the augmented Lagrangian state machine, merit evaluation, solver hook, and step loop. Functions in the new file drop the ipc_ prefix: MJAPI test kernels are mjc_PtTri, mjc_SegSeg, mjc_GeomDist, mjc_GeomVerts, mjc_GeomEdges (the mj_ipc* pose-unpacking wrappers are deleted; tests unpack the pose locally), cross-file internals use the mjc_ prefix, and file statics are bare. The CCD advance is mjc_advance since mjc_ccd is taken by the convex-collision entry point. Pure code motion: trajectories are bit-identical before and after.
The gap of a flex-flex pair (types 0/1) subtracted both participants' radii, so geometry tighter than the combined radii sat permanently at negative gap. addCand rejects g <= 0 as invalid, so those pairs got neither CCD coverage nor contact force: nothing kept the two sides from passing through one another. On scene_ipc_humanoid_bag the drawstring threaded through the bag hem holds the two midsurfaces 1.03 mm apart while r1 + r2 is 3.9 mm, so up to 40 pairs per step were discarded from the first step onwards. Measure the flex-flex gap at the midsurface instead. Only the origin the gap is measured from moves: the rest target is still the standoff min(ghc, IPC_DELTACAP), and mjc_conGhat, the active-set admission test and the conservative advance are all unchanged. Broad-phase reach is preserved by adding r1 + r2 back into the detection band alone (bandOffset) -- that offset belongs to the band, not to the rest target, since a target of r1 + r2 would ask for clearance the hem does not have and leave every such pair permanently violated. Flex-vs-geom pairs (types 2/3/4) still subtract the flex radius: the geom side is a true solid surface, where the cloth half-thickness is the physically meaningful separation. On scene_ipc_humanoid_bag over 1400 steps: discarded pairs 40/step -> 0, outer iterations 3.07 -> 3.01 in steady state and 3.59-3.75 -> 3.03-3.17 through the humanoid impact, 26.7 -> 31.8 ms/step -- the added cost being the string-bag contact the discarded pairs were never computing.
mj_IPC took all of its per-step scratch from mju_malloc, making it the
only engine function that touches the heap during mj_step. That breaks
the allocation-free stepping invariant the rest of the engine keeps: it
is not real-time safe, and in parallel rollouts every thread contends on
the global allocator even though mjData is already per-thread.
Move the 52 per-step allocations onto the mjData stack in three frames:
- ipc_efcCost: qacc/jar, scoped to one line-search evaluation.
- mj_IPC: the per-step block. Marked AFTER the efc build, whose
allocations come off the arena end and must outlive the frame.
- the extra-primal row block: a NESTED frame inside the outer loop.
epcol/epval are sized by the live contact count (2*nacon+1) and run
to tens of MB at a full active set, so holding them in the outer
frame would accumulate across outer iterations.
The persistent AL state stays on the heap: the multiplier warm-start, the
active-set age map and its key scratch all survive across steps, so they
cannot live in a per-step frame.
Also drops two per-step allocations, held and actpt, that nothing read.
Their mju_free calls were the only references, which is what kept
-Wunused-variable from reporting them.
Behaviour is unchanged: over 1400 steps of the humanoid-in-bag scene the
outer iteration count is identical at every checkpoint and wall-clock is
within noise, and the rigid-only path (no dim-2 flex, where the
zero-length arrays now come back NULL rather than as a heap pointer) runs
clean. Peak stack demand is ~164 MB against that model's 691 MB arena.
One consequence worth noting: exhaustion is now a hard mj_stackAlloc
error rather than a heap request, so a model with a tight size/memory
that previously ran may need it raised.
Picks up the actions/setup-node bump from node 20 to 24.x; node 20 is deprecated as an action runtime and was failing the CI actions scan. No conflicts. Verified against the merge: mujoco builds clean, the humanoid-in-bag scene reproduces identical outer-iteration counts at every checkpoint over 1400 steps, and engine_ipc_test (11), engine_derivative_test (27) and engine_forward_test (79) all pass.
mj_step cast away const on mjModel to set mjDSBL_CONSTRAINT|mjDSBL_CONTACT around the predictor's mj_forward, and mj_IPC cast a second time to clear them again for its own collision pass: two const violations undoing each other, which also made the model unsafe to share between threads. The predictor skips the constraint stage because mj_IPC rebuilds and owns the constraint set, so whatever the forward pipeline solves is discarded. That saving is substantial rather than cosmetic: leaving it in costs about 170 ms/step on the humanoid-in-bag scene (183 vs 11). Express the requirement directly instead of encoding it in the model. ipcSkipConstraint(m) holds for the IPC integrator when a dim-2 flex is present, and mj_forwardSkip skips mj_fwdConstraint for it, substituting qacc = qacc_smooth and zeroing solver_niter -- exactly what the disabled path did. mj_step then runs the ordinary mj_forward, and its duplicate dim-2 flex scan goes away with the cast. Collision stays enabled: measured, it costs nothing (11.3 ms either way), and mj_collision is interleaved with mj_wakeCollision/mj_updateSleep inside mj_fwdPosition, so gating it would be invasive for no gain. mj_IPC no longer overrides the flags for its own collision and constraint build, so mjDSBL_CONTACT and mjDSBL_CONSTRAINT are now honoured -- both mj_collision and mj_makeConstraint test them internally, so a model that disables either gets an empty set under IPC just as it would under any other integrator. mj_step2 is left untouched on purpose: IPC is absent from its integrator dispatch, so mj_step1/mj_step2 already runs Euler for an IPC model, and skipping the constraint solve there would give unconstrained Euler. The humanoid-in-bag trajectory is byte-identical over 1400 steps, wall-clock is unchanged, and engine_ipc_test (11), engine_forward_test (79) and engine_derivative_test (27) all pass.
mj_IPC hand-rolled its position/velocity commit and ended with a bare
d->time += h, so it skipped everything else mj_advance does: sensor and
ctrl history buffers, activation dynamics, island sleep, plugin state
advance. Under this integrator those were all dead.
IPC is a position-level integrator -- the solve yields q_{n+1} directly --
so it keeps its own commit and passes zero qacc/qvel to neutralise
mj_advance's two integration stages, taking the rest. mj_advance loses
static and is declared in engine_forward.h; nothing else about it changes.
The warmstart save is deliberately reverted: the inner mj_fwdConstraint
solves a different subproblem each outer iteration, so the previous step's
acceleration is a poor initial guess, and its path dependence breaks the
Euler agreement IpcTest.RigidContactMatchesEuler asserts.
Bit-identical to 12 decimals on the humanoid-in-bag scene; 117 tests pass.
The repaired stages are all inert in that scene (na=0, nu=0, nsensor=0,
nplugin=0, sleep off), so they still want a targeted test.
Comments carried numbers taken from one benchmark scene -- a hem clearance and radii sum, a ms/step figure with a vertex count, a divergence magnitude and step index. Those describe one model, not the code, and go stale or mislead as soon as either changes. State the mechanism and the decision instead; the measurements belong in the change description, not the source.
The AL inner subproblem needs matrix-free CG over the monolithic (non-island) problem regardless of what solver the model asks for, and mj_IPC expressed that by casting away const on mjModel: forcing opt.solver = mjSOL_CG, adding mjDSBL_ISLAND to opt.disableflags, calling mj_fwdConstraint, then restoring both. Four writes through a const pointer per outer iteration, which also made the model unsafe to share between threads. Make the choice a parameter instead. mj_fwdConstraint becomes a thin wrapper over a static fwdConstraint(m, d, solver, flg_island) -- the reads of m->opt.solver and the island test become the two arguments -- and a second entry point, mj_fwdConstraintCG, pins CG and the monolithic path. mj_IPC calls that and drops the casts along with the saved/savedsol bookkeeping. Equivalent by construction: flg_island = 0 is what dflags |= mjDSBL_ISLAND produced, every other disable flag is still read from the model, and both the solver-validity check and the trailing noslip test see mjSOL_CG as before. No ((mjModel*)m) casts remain in engine_ipc.c or engine_forward.c. Trajectory bit-identical to 12 decimals over the humanoid-in-bag run, outer counts identical at every checkpoint with wall-clock inside noise, 117 tests pass, and the rigid-only scenes are unaffected.
Factoring mj_fwdConstraint into a static fwdConstraint moved the unknown-solver-type check into the internal function, and mjERROR prefixes its message with __func__ -- so the text became "fwdConstraint: unknown solver type N". rollout_test.py's test_intercept_mj_errors asserts the exact string and failed on every Unix CI job. Do the check in mj_fwdConstraint instead, which is also where it belongs: it validates the model's solver setting, i.e. user input, while mj_fwdConstraintCG pins a valid solver by construction. The added nefc condition mirrors fwdConstraint's early-out so the check still fires exactly when it did before -- a model with no constraints and no injected rows returns without validating.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
2 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
What
A new opt-in integrator,
integrator="ipc", providing penetration-free contact for deformable flexes: flex–flex contact including self-collision (vertex–triangle and edge–edge) and flex-vs-static-geom. All other constraints — rigid contacts, limits, friction, equality — remain on the native pipeline, solved simultaneously. The integrator is fully opt-in: models that don't select it are unaffected.How it works
IPC methods prevent penetration by combining an implicit position-level step with continuous collision detection (CCD). This implementation makes that work inside MuJoCo's existing solver architecture rather than beside it:
Per-step implicit minimization. Each step minimizes an incremental potential over the flex vertex positions (and coupled joint dofs). Flex elasticity is not part of this potential: stretch rides the native edge-equality efc rows, so the supported flex class is edge-constraint flexes. FEM elasticity under
integrator="ipc"is rejected at load with a clear error pointing at the alternatives.Augmented-Lagrangian contact (why not just a barrier). Classic IPC enforces non-penetration with a stiff log-barrier, which brings severe ill-conditioning and barrier-parameter tuning. Here each contact pair instead carries a persistent Lagrange multiplier — a running contact-force estimate — and a few outer iterations per step alternate between solving the contact-augmented problem and updating the multipliers. Forces converge to the right values at moderate, fixed stiffness: no barrier, no conditioning cliff.
MuJoCo's own solver as the inner solver. Each outer iteration's subproblem is exactly one MuJoCo constraint solve (the CG solver on the standard primal objective): the IPC contact terms enter through a small hook as additional quadratic terms in the objective, so native constraints and IPC contact are resolved together by the existing machinery. There is no separate optimizer.
CCD-bounded commits. Candidate pairs are gathered with a conservative motion margin; every accepted position update advances only along a verified intersection-free path, and a step commits only once its full motion has been absorbed — so the committed trajectory never tunnels, and a step that cannot complete fails loudly rather than silently losing motion.
Scope and limitations
Testing
engine_ipc_testcovers the distance kernels (point–triangle, segment–segment, vertex–geom), pinned-vertex handling, and contact scenarios; exercised on cloth/bag models with heavy self-contact.