NaNs Constraints Jacobian in Contact Computation

[badinkajink]

Description

I am trying to run the hand.py ball demo. When I run it with the default settings (CCPADMM), I get the following video of not-quite-expected behavior.

2026-01-20.17-33-18.mp4

The command I run is:

python hand.py --ball --debug --display

Here is some debug output from print statements to pin_utils.py:ComputeContacts at t=37, for example.

collision at 0
in computecontacts
nc=1
J=array([[0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0.]])
R=array([[ 0.98446006, -0. , 0.17560865],
[ 0.05034753, 0.95801951, -0.28224768],
[-0.16823652, 0.28670302, 0.94313194]])
e=array([0. , 0. , 0.00057289])
t= 37

Here is some debug output at t=62, for example.

collision at 5
nc=1
J=array([[0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0.]])
R=array([[ 0.36775387, 0. , -0.92992316],
[ 0.63868578, 0.72683166, 0.25257911],
[ 0.6758976 , -0.68681565, 0.26729516]])
e=array([0. , 0. , 0.00096299])
t= 62

When I switch to CCPNewtonPrimal, I observe new, additional issues after timestep 37.

At t=37, the outputs are the same as before:

collision at 0
in computecontacts
nc=1
J=array([[0., 0., 0., 0., 0., 0.],
       [0., 0., 0., 0., 0., 0.],
       [0., 0., 0., 0., 0., 0.]])
R=array([[ 0.98446006, -0.        ,  0.17560865],
       [ 0.05034753,  0.95801951, -0.28224768],
       [-0.16823652,  0.28670302,  0.94313194]])
e=array([0.        , 0.        , 0.00092447])
t= 37

Then, at t=38:

collision at 0
collision at 1
collision at 2
collision at 3
collision at 4
collision at 6
collision at 7
collision at 8
collision at 9
collision at 11
collision at 12
collision at 13
collision at 14
collision at 16
collision at 17
collision at 18
collision at 19
/home/humanoid/miniconda3/envs/contactbench/lib/python3.10/site-packages/pycontact/utils/pin_utils.py:174: RuntimeWarning: divide by zero encountered in divide
  ey /= np.linalg.norm(ey)
/home/humanoid/miniconda3/envs/contactbench/lib/python3.10/site-packages/pycontact/utils/pin_utils.py:174: RuntimeWarning: invalid value encountered in divide
  ey /= np.linalg.norm(ey)
in computecontacts
nc=17
J=array([[nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan],
       [nan, nan, nan, nan, nan, nan]])
R=array([[            nan,            -inf, 2.96439388e-323,
                    nan,            -inf, 2.96439388e-323,
                    nan,            -inf, 2.96439388e-323,
                    nan,            -inf, 2.96439388e-323,
                    nan,            -inf, 2.96439388e-323,
                    nan,            -inf, 2.96439388e-323,
                    nan,            -inf, 2.96439388e-323,
                    nan,            -inf, 2.96439388e-323,
                    nan,            -inf, 2.96439388e-323,
                    nan,            -inf, 2.96439388e-323,
                    nan,            -inf, 2.96439388e-323,
                    nan,            -inf, 2.96439388e-323,
                    nan,            -inf, 2.96439388e-323,
                    nan,            -inf, 2.96439388e-323,
                    nan,            -inf, 2.96439388e-323,
                    nan,            -inf, 2.96439388e-323,
                    nan,            -inf, 2.96439388e-323],
       [            inf,             nan, 2.96439388e-323,
                    inf,             nan, 2.96439388e-323,
                    inf,             nan, 2.96439388e-323,
                    inf,             nan, 2.96439388e-323,
                    inf,             nan, 2.96439388e-323,
                    inf,             nan, 2.96439388e-323,
                    inf,             nan, 2.96439388e-323,
                    inf,             nan, 2.96439388e-323,
                    inf,             nan, 2.96439388e-323,
                    inf,             nan, 2.96439388e-323,
                    inf,             nan, 2.96439388e-323,
                    inf,             nan, 2.96439388e-323,
                    inf,             nan, 2.96439388e-323,
                    inf,             nan, 2.96439388e-323,
                    inf,             nan, 2.96439388e-323,
                    inf,             nan, 2.96439388e-323,
                    inf,             nan, 2.96439388e-323],
       [            nan,             inf, 4.78346184e-310,
                    nan,             inf, 4.78346184e-310,
                    nan,             inf, 4.78346184e-310,
                    nan,             inf, 4.78346184e-310,
                    nan,             inf, 4.78346184e-310,
                    nan,             inf, 4.78346184e-310,
                    nan,             inf, 4.78346184e-310,
                    nan,             inf, 4.78346184e-310,
                    nan,             inf, 4.78346184e-310,
                    nan,             inf, 4.78346184e-310,
                    nan,             inf, 4.78346184e-310,
                    nan,             inf, 4.78346184e-310,
                    nan,             inf, 4.78346184e-310,
                    nan,             inf, 4.78346184e-310,
                    nan,             inf, 4.78346184e-310,
                    nan,             inf, 4.78346184e-310,
                    nan,             inf, 4.78346184e-310]])
e=array([ 0.00000000e+000,  0.00000000e+000, -1.79769313e+308,
        0.00000000e+000,  0.00000000e+000, -1.79769313e+308,
        0.00000000e+000,  0.00000000e+000, -1.79769313e+308,
        0.00000000e+000,  0.00000000e+000, -1.79769313e+308,
        0.00000000e+000,  0.00000000e+000, -1.79769313e+308,
        0.00000000e+000,  0.00000000e+000, -1.79769313e+308,
        0.00000000e+000,  0.00000000e+000, -1.79769313e+308,
        0.00000000e+000,  0.00000000e+000, -1.79769313e+308,
        0.00000000e+000,  0.00000000e+000, -1.79769313e+308,
        0.00000000e+000,  0.00000000e+000, -1.79769313e+308,
        0.00000000e+000,  0.00000000e+000, -1.79769313e+308,
        0.00000000e+000,  0.00000000e+000, -1.79769313e+308,
        0.00000000e+000,  0.00000000e+000, -1.79769313e+308,
        0.00000000e+000,  0.00000000e+000, -1.79769313e+308,
        0.00000000e+000,  0.00000000e+000, -1.79769313e+308,
        0.00000000e+000,  0.00000000e+000, -1.79769313e+308,
        0.00000000e+000,  0.00000000e+000, -1.79769313e+308])
/home/humanoid/miniconda3/envs/contactbench/lib/python3.10/site-packages/pycontact/simulators.py:113: RuntimeWarning: overflow encountered in multiply
  constraint_cor = (1./dt) * err
t= 38

This then persists for the rest of the simulation.

Expected Outcome

I think this is a build and/or versioning issue. When building I had several warnings about hppfcl/coal. Or something silly of that nature.

Steps I've Tried

1. Clean rebuild
2. Reinstalling pinocchio, hppfcl (via coal), proxsuite, cppad via conda and from source.

Environment Information

• OS: Ubuntu 22.04
• Python 3.10, in a conda environment

Additional Information

I am happy to provide additional info and videos. If there is a suggested build and installation recipe, I would appreciate it and any other guidance.

[badinkajink]

Update: Claude code said that the issue was that pin.getConstraintsJacobian was reading from data.J but not populating it before calling it. So I added a call to compute the joint Jacobians before calling pin.getConstraintsJacobian.

    if nc > 0:
        pin.computeJointJacobians(model, data)
        Del = DelassusPinocchio(model, data, contact_models, contact_datas)
        J = pin.getConstraintsJacobian(model, data, contact_models, contact_datas)

Now, the ball hand demo is stable across CCPADMM and CCPNewton, but it is not really falling on the hand:

2026-01-22.12-00-09.mp4

On the simpler cube slide demo, here is a before/after the change. Skip to 0:26 for the after.

2026-01-22.11-58-42.mp4