[FEATURE] Add ComFree (complementarity-free) analytical constraint solver#2872
Draft
Kashu7100 wants to merge 4 commits into
Draft
[FEATURE] Add ComFree (complementarity-free) analytical constraint solver#2872Kashu7100 wants to merge 4 commits into
Kashu7100 wants to merge 4 commits into
Conversation
Port the ComFree analytical contact solver (arXiv:2603.12185, reference
impl asu-iris/comfree_warp) into Genesis as a third constraint_solver
option alongside CG and Newton.
Instead of an iterative complementarity solve, ComFree resolves contacts
in a single pass via an impedance-style prediction/correction update:
v_pred = v + acc_smooth * dt
efc_vel = J_c @ v_pred
efc_pen = efc_vel * dt + efc_dist
force = max(efc_mass * (-d*efc_vel - k*efc_pen), 0)
qacc = M^{-1} (qf_smooth + J^T force)
with k, d the user stiffness/damping scaled by 1/dt.
Changes:
- constants: add constraint_solver.ComFree
- options.RigidOptions: comfree_stiffness (0.2), comfree_damping (0.001)
- array_class.ConstraintState: add efc_dist, efc_mass (layout-flippable)
- geom: add comfree_imp (hardcoded width=0.01 impedance) and
comfree_efc_mass (single efc_mass formula shared by all constraint types,
mirroring reference constraint.py _efc_row:125)
- constraint/solver: populate efc_dist/efc_mass for contacts (both per-contact
and per-friction layout paths), equality connect/joint/weld, joint limits,
and frictionloss
- rigid solver: instantiate ComFreeSolver when selected
- new genesis/engine/solvers/rigid/comfree package (ComFreeSolver + kernels)
- tests/test_comfree.py: instantiation, stepping, analytic free-fall,
contact settling, 4-env batched, two-box stacking
Validated on CPU: all unit tests pass; a Franka arm scene (joint limits +
actuation + contacts) runs stably with no NaN.
Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
Replace the per-env serial resolution loop with a fixed sequence of fully data-parallel passes (efc_force, qfrc_constraint, force, qacc, publish), so the GPU is no longer left idle running one thread per env. Switch from func_solve_mass_batch to the batched func_solve_mass and respect the transposed constraint layout for coalesced memory access. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
…e the shared constraint pipeline. ComFreeSolver becomes a ConstraintSolver subclass overriding only resolve(): constraint-state allocation, reset/clear, constraint assembly, contact-force publication, qacc publication (including the NaN errno check), and the dev getters are all shared. The resolution itself is a single kernel: analytical per-row force, J^T @ force gather, and the pre-factored mass solve. Correctness and integration fixes over the original port: - Per-row force projection honors the constraint taxonomy: equality rows stay two-sided and frictionloss rows are box-bounded, while contact and joint-limit rows push only. - efc_dist / efc_mass are only allocated and populated when ComFree is selected (statically compiled out otherwise), so the other solvers keep their hot assembly path and memory footprint; the unused dense Hessian allocation is dropped for ComFree in exchange. - The stiffness / damping parameters live in RigidInfo as runtime constants instead of template arguments, avoiding one kernel recompilation per value. - The impedance curve is shared with imp_aref through constraint_imp instead of duplicating it with a hardcoded width. - The kernel follows the constraint layout flips on GPU and honors the sparse Jacobian representation on CPU, with a deterministic per-env scatter. - Incompatible options are rejected (elliptic friction cone, noslip, differentiable mode) or resolved (contact islands, cooperative kernels, skyline permutation are pure overhead for ComFree and stay off).
One packed scene exercises analytic free fall, compliant contact settling, the two-sided connect equality, symmetric frictionloss braking, joint-limit containment, and batched-env consistency, plus the incompatible-option rejections.
duburcqa
changed the title
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.
Summary
constraint_solveroption alongside CG and Newton: every constraint force is computed analytically in a single pass instead of iterating a complementarity solve to convergence.ComFreeSolversubclassesConstraintSolverand overrides onlyresolve(), sharing the whole assembly pipeline (contacts, equalities, joint limits, frictionloss), reset/clear, and force publication. The resolution is one kernel: analytical per-row force,J^T @ forcegather, pre-factored mass solve.comfree_stiffnessandcomfree_damping, tune the analytical force. Incompatible options are rejected (elliptic cone, noslip, differentiable mode) or resolved automatically (contact islands).efc_dist,efc_mass) are statically compiled out and their buffers are empty unless ComFree is selected.Motivation and Context
RL workloads step thousands of environments where the iterative constraint solve dominates the step budget. A single-pass analytical solver trades contact accuracy (compliant, tunable spring-damper behavior) for large throughput gains on contact-dense batched scenes.
Measured whole-step throughput vs Newton (interleaved A/B/A runs, Apple M-series):
How Has This Been / Can This Be Tested?
tests/rigid/test_constraints.py::test_comfree_solver(parametrized overn_envs=[0, 2]): one packed scene asserting analytic free fall, compliant contact settling and rest, two-sided connect equality holding a hung body, symmetric frictionloss braking of opposite spins, joint-limit containment (transient and steady-state), and identical batched trajectories; plustest_comfree_incompatible_optionsfor option validation. Passing locally on CPU and Metal.tests/rigidrequired suite passing on CPU (no regression for CG/Newton: their compiled kernels are unchanged).Screenshots (if appropriate):
Checklist:
Submitting Code Changessection of CONTRIBUTING document.