[Kamino][P-ADMM] Bounded implicit-PD effort constraints make DR Legs diverge to NaN

[AntoineRichard]

Summary

Kamino P-ADMM intermittently produces explosive velocities and eventually a fully non-finite articulation state for Isaac Lab's DR Legs robot.

The regression was bisected to #3990:

• Good: bc43a55, immediately before [Kamino] Implement joint effort limits #3990
• Bad: 3e57f004, [Kamino] Implement joint effort limits #3990 applied
• Newton main at 853c4fef still reproduces the problem

#3990 introduced bounded solver rows for implicit-PD joint effort limits. Disabling only the effort limit on the driven DR Legs joints eliminates the instability, while Kamino DVI remains stable with the original effort limit.

Related Isaac Lab issue: isaac-sim/IsaacLab#7625

Environment

• Isaac Lab: origin/develop at 7127f50cca
• Newton: 1.6.0rc1
• Warp: 1.17.0
• GPU: NVIDIA GeForce RTX 5090
• Task: IsaacContrib-DrLegs-Walk
• Environments per trial: 2
• Simulation steps per trial: 20
• Simulation timestep: 1 / 150 s
• Decimation: 3

Relevant actuator configuration:

ImplicitActuatorCfg(
    joint_names_expr=DR_LEGS_ACTUATED_JOINTS,
    stiffness=5.0,
    damping=0.2,
    joint_effort_limit=3.1,
    joint_velocity_limit=100.0,
)

Relevant solver configuration:

KaminoPADMMSolverCfg(
    use_fk_solver=True,
    max_contacts_per_world=32,
    sparse_jacobian=True,
    dynamics_solver_cfg=KaminoPADMMCfg(
        primal_tolerance=1.0e-5,
        dual_tolerance=1.0e-5,
        compl_tolerance=1.0e-5,
        rho_0=0.02,
    ),
)

CUDA graphs are enabled in the normal task preset.

Reproduction method

For each seed:

1. Create a fresh IsaacContrib-DrLegs-Walk environment with two worlds.
2. Set both the environment seed and batched action-space seed.
3. Reset the environment.
4. Execute 20 actions sampled from the task's standard action space.
5. Check root pose/velocity, joint position/velocity, and policy observations after every step.
6. Record the first non-finite step and peak finite velocities.

The precise failure seed varies because the GPU solve is not fully deterministic, but the regression is frequent.

Results

Configuration	Non-finite trials
Commit immediately before #3990, P-ADMM	0/13
#3990 applied, P-ADMM	6/13
Newton 1.6.0rc1, P-ADMM	6/13
Newton current main, P-ADMM	6/8
Newton 1.6.0rc1, Kamino DVI	0/13
P-ADMM with driven effort limit set to 1e9	0/13
P-ADMM with only passive effort limit set to 1e9	4/13
HoldPose with the original P-ADMM configuration	8/13

With the driven-joint effort limit effectively disabled, peak joint velocities stayed around 15-25 rad/s. With the original 3.1 N·m limit, velocities frequently reached thousands or millions of rad/s before the articulation became NaN.

The HoldPose result shows that this is not specific to Walk's contact sensor, velocity command, or gait rewards.

Additional ablations

The following did not resolve the problem:

• disabling CUDA graphs
• using a dense Jacobian
• disabling the FK solver
• disabling P-ADMM acceleration
• disabling warm-starting
• increasing the iteration count to 200
• increasing rho_0
• using two simulation substeps

Increasing rho_0 or the iteration count reduced the failure frequency in some runs but did not make the solver robust.

Observed contact counts were generally between 5 and 18, versus a total allocation of 64, so contact-capacity overflow does not appear involved.

Suspected failing path

The evidence points specifically to the bounded implicit-PD effort rows added by #3990 in the P-ADMM path:

• The parent of [Kamino] Implement joint effort limits #3990 is stable.
• Applying [Kamino] Implement joint effort limits #3990 immediately introduces the divergence.
• Removing only the driven-joint bound is stable.
• DVI is stable with the bound retained.
• Sparse and dense P-ADMM both fail.

For the DR Legs parameters and h = 1/150 s, the effort row uses approximately:

m_a = h * k_d + h² * k_p ≈ 0.00156
1 / m_a ≈ 642.9
h * tau_max ≈ 0.0207 N·m·s

This may help narrow the problem to P-ADMM conditioning, projection, or multiplier scaling for low-effort implicit-PD rows. The exact defect within that path has not yet been identified.

Expected behavior

P-ADMM should keep the simulation finite and enforce the configured 3.1 N·m driven-joint effort limit under random position targets.

Actual behavior

The constrained solve develops extreme joint and root velocities, then produces NaNs across the complete robot state.

Suggested regression coverage

• Add a P-ADMM regression using the DR Legs model and its original implicit-PD gains and effort limits.
• Run multiple seeded random-target sequences.
• Assert that root and joint state remain finite.
• Assert that reported actuator effort respects the configured limit.
• Exercise both sparse and dense P-ADMM.
• Keep a corresponding DVI run as a reference.

[AntoineRichard]

Newton-side diagnosis update

I reproduced this directly with Newton main at 5ca5f49c, using the standalone Kamino DR Legs example with two worlds and random position targets. The evidence now points to P-ADMM conditioning and handling of a non-converged solution, rather than the actuator exceeding its configured effort limit.

The actuator torque remains clipped

On a failing solve with rho_0=0.02, 100 iterations, no acceleration, and no warm-starting:

• reported actuator effort remained within the configured bound: max(abs(lambda_tau_j)) = 3.0999997 N·m;
• the bounded effort impulse reached exactly its expected 0.031 N·m·s bound for the standalone example's dt=0.01 s;
• an unbounded bilateral/joint multiplier reached 5.662 N·m·s, approximately 566 N·m when divided by the timestep;
• the solve exhausted 100 iterations with converged=0, primal residual 0.00206, dual residual 0.209, and complementarity residual 0.0205.

SolverKaminoImpl._update_constraints() currently consumes the terminal P-ADMM multipliers unconditionally after solve(), including when status.converged == 0. The large unbounded joint/loop reactions are therefore applied to the bodies, producing the extreme velocity; the next step then compounds it into NaNs.

Why enabling the effort bound exposes it

The bounded implicit-PD formulation adds an effort row parallel to the corresponding joint-dynamics row. In the failing DR Legs matrix, each of the 12 effort rows had a correlation of 0.9957 to 0.999985 with an existing dynamic row. This is added to the robot's already redundant closed-loop constraint system.

The current scalar preconditioner is clamped to [1/50, 50]:

return wp.clamp(inv_sqrt_d, wp.float32(1.0 / 50.0), wp.float32(50.0))

All paired dynamic/effort rows hit the 0.02 lower clamp, whereas their diagonal-normalizing scales range from approximately 0.000465 to 0.00799. For the reproduced failing matrix:

Preconditioner	Approximate eigenvalue range	Condition estimate
Current 1/50 lower clamp	0.019996 to 5567.97	278,448
Diagnostic 1/2000 lower clamp	0.02001 to 4.425	221

This does not mean that globally changing the clamp is automatically safe. The clamp was added deliberately in #3878 to preserve float32 precision and improve other closed-loop examples, so those benchmarks need to be repeated.

Diagnostic preconditioner experiment

I temporarily changed only the lower clamp from 1/50 to 1/2000 and restored the source afterward.

With Newton's normal P-ADMM settings (rho_0=1, 200 iterations, acceleration and container warm-starting enabled):

• current source exceeded 100 rad/s at step 14 for seed 12;
• the diagnostic change kept 12/12 seeds finite for 100 steps each;
• the maximum joint speed across those runs was 10.46 rad/s;
• all three tests in test_kamino_solver_joint_effort_limit still passed.

However, the diagnostic change alone did not make Isaac Lab's unusually low rho_0=0.02 preset robust: the accelerated/warm-started reproduction still exceeded 100 rad/s at step 12. Thus the Isaac Lab preset mitigation in isaac-sim/IsaacLab#7638 remains useful even if the Newton preconditioner is improved.

Dependence on the effort threshold

The failure depends on whether the effort projection becomes active, not monotonically on how much torque is permitted. On current Newton source, seed 12, rho_0=0.02, and 100 iterations:

Effort limit [N·m]	Result
0.1	exceeded 100 rad/s at step 8
0.3	step 5
1.0	step 3
3.1	step 1
10	step 6
30	finite for 100 steps
100	finite for 100 steps
1e9	finite for 100 steps

The matrix diagonal and effort-row compliance are determined by mass/inertia, timestep, and PD gains; the effort threshold changes the projection's active set. Once the threshold is high enough that clipping is effectively inactive, the same run stays finite. The exact crossover is therefore trajectory-dependent rather than a universal safe force threshold.

Iteration cutoff and early-exit thresholds

Isaac Lab's KaminoPADMMCfg sets max_iterations=100 by default. The DR Legs preset does not override it, so each world stops after 100 P-ADMM iterations unless it converges earlier. (Newton's standalone PADMMSolverConfig default is 200.)

Early convergence is declared only after at least two iterations and only when all three infinity-norm conditions pass simultaneously:

r_p <= primal_tolerance
r_d <= dual_tolerance
r_c <= compl_tolerance

DR Legs overrides all three tolerances to 1e-5 (the Isaac Lab defaults are 1e-4). In the representative failing solve above, the terminal values were r_p=2.06e-3, r_d=2.09e-1, and r_c=2.05e-2, so none passed and the world stopped only because it reached iteration 100. The dominant miss was the dual residual, about 20,900 times its configured cutoff.

Candidate Newton work

1. Add a closed-loop, bounded-effort P-ADMM regression; the current effort-limit tests use a single world-to-body revolute DoF and do not exercise redundant constraint directions.
2. Revisit the absolute preconditioner clamp, potentially making its scale relative/configurable or adding a more appropriate block treatment for highly correlated rows. Validate against the examples and motivation from Clamp Kamino dual preconditioner scaling #3878.
3. Add targeted failure handling when P-ADMM exhausts its budget with a dangerous reaction solution, for example retrying with a safer penalty/cold start. Simply rejecting every converged=0 result would be too broad because some physically reasonable solves miss the strict residual tolerance.
4. Avoid the current balanced adaptive-penalty mode as a direct fix here: the failing solve is dual-residual dominated, so that rule decreases rho; in the reproduction it drove rho downward and made the divergence substantially worse.

The bounded-effort equations and projection signs/bounds appear internally consistent. The immediate problem is that the additional nearly parallel rows expose inadequate scaling/convergence in P-ADMM, and the resulting unconverged joint reactions are still integrated.

[AntoineRichard]

Here is a standalone Newton-only reproduction. It imports no Isaac Lab code and deliberately passes contacts=None to isolate the articulation solve.

Run from a current Newton checkout with its environment active:

python repro_kamino_dr_legs_effort_limit.py

The DR Legs asset is fetched through Newton's normal example asset path on the first run.

repro_kamino_dr_legs_effort_limit.py

#!/usr/bin/env python3
"""Reproduce DR Legs P-ADMM divergence using Newton only."""

from types import SimpleNamespace

import numpy as np
import warp as wp

import newton
from newton.examples.kamino.example_kamino_robot_dr_legs import Example
from newton.viewer import ViewerNull


NUM_WORLDS = 2
NUM_RANDOM_TARGETS = 100
MAX_REASONABLE_JOINT_SPEED = 100.0
RANDOM_SEED = 12


def main() -> int:
    wp.init()

    args = SimpleNamespace(
        world_count=NUM_WORLDS,
        use_kamino_contacts=False,
        dynamics_solver="padmm",
        joint_effort_limit=3.1,
    )
    example = Example(ViewerNull(num_frames=1), args)

    # Match the P-ADMM settings that expose the Isaac Lab failure.
    example.config.padmm.max_iterations = 100
    example.config.padmm.primal_tolerance = 1.0e-5
    example.config.padmm.dual_tolerance = 1.0e-5
    example.config.padmm.compl_tolerance = 1.0e-5
    example.config.padmm.rho_0 = 0.02
    example.config.padmm.use_acceleration = True
    example.config.padmm.warmstart_mode = "containers"

    example.solver = newton.solvers.SolverKamino(example.model, config=example.config)
    example.solver.reset(example.state_0, flags=0)
    example.solver.reset(example.state_1, flags=0)

    # Avoid CUDA graph capture and omit contacts to isolate the articulation solve.
    example.graph = None
    rng = np.random.default_rng(RANDOM_SEED)

    for step in range(NUM_RANDOM_TARGETS):
        target = rng.uniform(-1.0, 1.0, example.model.joint_coord_count).astype(np.float32)
        example.control.joint_target_q.assign(target)

        for _ in range(example.sim_substeps):
            example.state_0.clear_forces()
            example.solver.step(
                example.state_0,
                example.state_1,
                control=example.control,
                contacts=None,
                dt=example.sim_dt,
            )
            example.state_0, example.state_1 = example.state_1, example.state_0

        body_q = example.state_0.body_q.numpy()
        body_qd = example.state_0.body_qd.numpy()
        joint_q = example.state_0.joint_q.numpy()
        joint_qd = example.state_0.joint_qd.numpy()
        finite = all(np.isfinite(value).all() for value in (body_q, body_qd, joint_q, joint_qd))
        max_joint_speed = float(np.nanmax(np.abs(joint_qd)))

        if not finite or max_joint_speed > MAX_REASONABLE_JOINT_SPEED:
            print(f"DIVERGED at random-target step {step}")
            print(f"finite state: {finite}")
            print(f"max joint speed: {max_joint_speed:.6g} rad/s")
            print("P-ADMM status:")
            print(example.solver.status.numpy())
            return 1

    print(f"Unexpectedly remained finite for {NUM_RANDOM_TARGETS} random-target steps")
    print(f"max joint speed: {np.max(np.abs(joint_qd)):.6g} rad/s")
    return 0


if __name__ == "__main__":
    raise SystemExit(main())

Verified on main at 5ca5f49c with Warp 1.17.0 and an RTX 5090. It fails with exit status 1 and this result:

DIVERGED at random-target step 1
finite state: True
max joint speed: 1144.75 rad/s
P-ADMM status:
[(0, 100, 5.9890745e-06, 0.04670966, 5.6235374e-05, ...)
 (0, 100, 3.4232615e-04, 0.9042386,  5.0684731e-02, ...)]

Both worlds reached the 100-iteration cutoff without converging. The dual residuals (0.0467 and 0.904) remain far above the configured 1e-5 tolerance before the joint speed explodes.