Hard unilateral contact with PGS: removing contact regularization (R=0) still allows penetration to accumulate (normal relative velocity not driven to zero)

[kankanzheli]

Intro

Hi MuJoCo team,

I’m trying to achieve a “hard” unilateral normal contact behavior using the built-in PGS solver: when contact is active, I want the relative normal velocity at the contact to be driven to zero (or ≥ 0) each step, so that penetration depth does not keep increasing over time (small overlap at first is acceptable, but the overlap should not grow further).

My target behavior

Unilateral constraint (push-only, no pull): normal contact force ≥ 0.

When contact is active, the solver should make relative normal velocity ~ 0 at the end of the step (or at least prevent continued closing).

Result: the penetration depth should not accumulate across steps.

Simulation setup

Solver: PGS

timestep = 0.002

Contact parameters on robot geoms (example):

condim="1"

solref="0 -500" (direct format, intended as B ≈ 1/dt)

solimp="1 1 0 0.005 2"

With the default MuJoCo solver behavior, contacts still appear “soft” to me (penetration can keep increasing).

What I modified in MuJoCo source

I patched engine_core_constraint.c mainly in these places:

Remove contact-row regularization in mj_makeImpedance
In mj_makeImpedance, for PGS contact rows (mjCNSTR_CONTACT_*), I force:

R[i] = 0 for contact rows (for PGS only)

I also skip/guard the friction-related R rescaling logic to avoid 0/0 issues when R=0.

Recompute D for contact rows from diag(A) in mj_projectConstraint
Because D = 1/R no longer works for contact rows when R=0, in mj_projectConstraint I set for PGS contact rows:

D[i] = 1 / max(eps, diag(A))
Where diag(A) is taken from the diagonal of the computed A = B*B' (in sparse path I use the diag index after building AR; for contact rows R=0 so diag(AR)=diag(A)).

I kept the rest of the pipeline mostly unchanged:

KBIP computation stays the same (I did not fully redesign aref/K/B logic beyond using solref as usual).

Contact constraints remain unilateral via the existing projection logic (force >= 0).

I added a debug print to confirm the patched code is active, e.g.:
[OHYA_PATCH] mj_makeImpedance: PGS contact row hit ...

Current observed behavior (problem)

After these changes:

Contacts do generate pushing forces and objects can separate (so contact is not “dead”).

However, penetration depth can still keep increasing over time in some cases.

This suggests that the solver is not actually driving the relative normal velocity to 0 sufficiently, even with solref="0 -500" and small timestep.

My setup

mujoco 3.3.7

My question

With PGS, is it expected that even with strong velocity feedback (B ≈ 1/dt) the solver may still allow penetration to accumulate unless iterations are very high?

Does removing R for contact rows fundamentally break assumptions of MuJoCo’s PGS update / stopping criteria (tolerance/cost), causing early termination before the normal velocity is sufficiently corrected?

If the goal is “hard” unilateral contact (no additional penetration accumulation), what is the recommended approach in MuJoCo?

Is there a correct way to set R=0 (or near 0) and compute D so that PGS can converge reliably?

Are there internal solver parameters or logic (e.g., iteration limit / tolerance / scaling) that must be adjusted when R=0?

Is there an official/accepted method to emulate “v_n → 0” per step without moving to a custom plugin solver?

If helpful, I can provide:

A minimal XML and model to reproduce the penetration accumulation,

A clean patch/PR (diff) with the exact changes above,

Logs for solver_niter, contact efc_vel (normal), and penetration depth.

Minimal model and/or code that explain my question

No response

Confirmations

• I searched the latest documentation thoroughly before posting.
• I searched previous Issues and Discussions, I am certain this has not been raised before.

[AryanVBW]

Hi @kankanzheli,

This is a really interesting deep dive into MuJoCo's PGS solver internals. Your modifications to achieve hard unilateral contacts are quite ambitious. Let me address your questions:

Why penetration still accumulates

The issue you're experiencing is expected behavior, even with your modifications. Here's why:

1. PGS is inherently iterative and approximate: Setting R=0 removes regularization, but PGS still solves the constraint problem approximately within a finite iteration budget. The solver may terminate before fully driving normal velocity to zero, especially with complex contact configurations.

2. Velocity vs. position correction: MuJoCo's contact model primarily corrects velocity, not position directly. Even if relative normal velocity is reduced significantly, numerical drift and integration error can still cause penetration accumulation over many timesteps.

3. Solver tolerance and early termination: With R=0, you've changed the conditioning of the system. The solver's convergence criteria (based on cost reduction or residual) may be satisfied before the normal velocity constraint is tight enough.

Recommended approaches

Option 1: Increase solver iterations

<option iterations="100" tolerance="1e-8"/>

With R=0, you need significantly more iterations for convergence. Try 50-100+ iterations and tighter tolerance.

Option 2: Use CG solver instead of PGS
The Conjugate Gradient solver often handles poorly conditioned systems better:

<option solver="CG" iterations="100"/>

Option 3: Add Baumgarte stabilization
Instead of removing regularization entirely, use position-based error correction via solimp parameters:

solimp="0.9 0.95 0.001 0.5 2"

The first two parameters control position error feedback (Baumgarte stabilization).

Option 4: Smaller timestep with subcycling

<option timestep="0.0005"/>

Smaller timesteps reduce integration error that causes drift.

Regarding your source modifications

Setting R=0 and recomputing D from diag(A) is theoretically sound, but:

• MuJoCo's PGS implementation assumes some regularization for numerical stability
• The stopping criteria may not be appropriate for R=0 cases
• You may need to also adjust mjMIN_CONSTRAINT_FORCE and related tolerances

What I'd try first

Before modifying source code, test this configuration:

<option solver="CG" iterations="100" tolerance="1e-8"/>
<geom ... condim="1" solref="0.001 1" solimp="0.95 0.99 0.001 0.5 2"/>

This uses:

• CG solver (better for stiff systems)
• Strong Baumgarte stabilization (0.95, 0.99)
• Small but non-zero stiffness (0.001) for numerical stability
• High damping ratio (1)

If you still need R=0 behavior, you might need to implement a custom constraint plugin with your own solver logic.

Hope this helps! Let me know if you need clarification on any of these approaches.