[FETURE] Comfree solver#2872
Draft
Kashu7100 wants to merge 2 commits into
Draft
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>
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
1 participant
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
efc_force→qfrc_constraint→force→qacc→ publish/warmstart).func_solve_massfor theM^{-1} @ qfrc_totalsolve and respects the transposed constraint layout so adjacent lanes vary along the coalesced axis.Notes
qacc = M^{-1} qf_smooth = acc_smooth.Motivation and Context
How Has This Been / Can This Be Tested?
Screenshots (if appropriate):
Checklist:
Submitting Code Changessection of CONTRIBUTING document.