Trajopt: contact test types and collision checking

[marip8]

Using the same setup from #263, I am also running into some strange behavior with the contact test type and collision checking:

Case 1

contact_test_type = FIRST
safety_margin = 0.1
safety_margin_buffer = 0.1
collision_as_constraint = true

Result: success

case_1.mp4

Case 2

contact_test_type = ALL
safety_margin = 0.1
safety_margin_buffer = 0.1
collision_as_constraint = true

Result: the optimization somehow succeeds, but clearly some collision constraints are violated, as seen in the video and the trajopt log

case_6.mp4

| =========================================================================== |
|                               ROS Industrial                                |
|                           TrajOpt Motion Planning                           |
| =========================================================================== |
|      merit |   oldexact |  new_exact |    dapprox |     dexact |      ratio | -                
| --------------------------------------------------------------------------- | COSTS
| ---------- |  2.691e-03 |  3.305e-03 | -6.141e-04 | -6.141e-04 |  1.000e+00 | joint_accel_cost 
| ---------- |  2.879e-05 |  3.743e-05 | -7.288e-06 | -8.639e-06 |  1.185e+00 | joint_jerk_cost 
| =========================================================================== |
| ---------- |  2.719e-03 |  3.342e-03 |   ------   |   ------   |   ------   | SUM COSTS 
| =========================================================================== |
| --------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  4.605e-19 |  7.336e-19 | -1.087e-08 | -2.731e-19 |   ------   | joint_waypoint_0 
|  1.000e+01 |  1.360e-19 |  2.128e-19 | -5.901e-09 | -7.681e-20 |   ------   | joint_waypoint_1 
|  1.000e+01 |  7.687e-21 |  8.106e-21 | -8.136e-10 | -4.188e-22 |   ------   | joint_waypoint_59 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_2     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_3     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_4     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_5     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_6     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_9     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_10    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_11    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_12    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_13    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_14    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_15    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_16    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_17    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_18    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_19    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_20    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_21    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_22    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_23    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_24    
|  1.000e+01 |  0.000e+00 |  1.005e-01 |  0.000e+00 | -1.005e-01 |   ------   | collision_25    
|  1.000e+01 |  0.000e+00 |  2.379e+00 |  0.000e+00 | -2.379e+00 |   ------   | collision_26    
|  1.000e+01 |  0.000e+00 |  4.698e+00 |  0.000e+00 | -4.698e+00 |   ------   | collision_27    
|  1.000e+01 |  0.000e+00 |  7.052e+00 |  0.000e+00 | -7.052e+00 |   ------   | collision_28    
|  1.000e+01 |  0.000e+00 |  9.432e+00 |  0.000e+00 | -9.432e+00 |   ------   | collision_29    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_30    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_31    
|  1.000e+01 |  0.000e+00 |  1.665e+01 |  0.000e+00 | -1.665e+01 |   ------   | collision_32    
|  1.000e+01 |  0.000e+00 |  1.916e+01 |  0.000e+00 | -1.916e+01 |   ------   | collision_33    
|  1.000e+01 |  0.000e+00 |  2.190e+01 |  0.000e+00 | -2.190e+01 |   ------   | collision_34    
|  1.000e+01 |  0.000e+00 |  2.260e+01 |  0.000e+00 | -2.260e+01 |   ------   | collision_35    
|  1.000e+01 |  0.000e+00 |  2.022e+01 |  0.000e+00 | -2.022e+01 |   ------   | collision_36    
|  1.000e+01 |  0.000e+00 |  1.787e+01 |  0.000e+00 | -1.787e+01 |   ------   | collision_37    
|  1.000e+01 |  0.000e+00 |  1.553e+01 |  0.000e+00 | -1.553e+01 |   ------   | collision_38    
|  1.000e+01 |  0.000e+00 |  1.324e+01 |  0.000e+00 | -1.324e+01 |   ------   | collision_39    
|  1.000e+01 |  0.000e+00 |  1.097e+01 |  0.000e+00 | -1.097e+01 |   ------   | collision_40    
|  1.000e+01 |  0.000e+00 |  8.753e+00 |  0.000e+00 | -8.753e+00 |   ------   | collision_41    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_42    
|  1.000e+01 |  0.000e+00 |  4.449e+00 |  0.000e+00 | -4.449e+00 |   ------   | collision_43    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_44    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_45    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_46    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_47    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_48    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_49    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_50    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_51    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_52    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_53    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_54    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_55    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_56    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_57    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_58    
| =========================================================================== |
| ---------- |  6.042e-20 |  1.950e+01 |   ------   |   ------   |   ------   | SUM CONSTRAINTS (WITHOUT MERIT) 
| =========================================================================== |
| ---------- |  2.719e-03 |  1.950e+02 | -6.214e-04 | -1.950e+02 |  3.138e+05 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| =========================================================================== |
[ERROR] approximate merit function got worse (-6.214e-04). (convexification is probably wrong to zeroth order)
[INFO] converged because improvement was small (-6.214e-04 < 1.000e-04)
[INFO] woo-hoo! constraints are satisfied (to tolerance 1.00e-04)
[INFO] 
==================
Optimization results:
status: CONVERGED
cost values: (0.00269062, 2.87918e-05)
constraint violations: (4.60512e-20, 1.36002e-20, 7.68725e-22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
n func evals: 4
n qp solves: 3
==================

Case 3

contact_test_type = ALL
safety_margin = 0.1
safety_margin_buffer = 0.0
collision_as_constraint = true

Result: optimization now fails because the collision constraints are not met and the safety_margin_buffer = 0.0

| =========================================================================== |
|                               ROS Industrial                                |
|                           TrajOpt Motion Planning                           |
| =========================================================================== |
|      merit |   oldexact |  new_exact |    dapprox |     dexact |      ratio | -                
| --------------------------------------------------------------------------- | COSTS
| ---------- |  1.189e-01 |  1.192e-01 | -2.929e-04 | -2.929e-04 |  1.000e+00 | joint_accel_cost 
| ---------- |  3.312e-01 |  3.294e-01 |  2.218e-01 |  1.838e-03 |  8.290e-03 | joint_jerk_cost 
| =========================================================================== |
| ---------- |  4.501e-01 |  4.486e-01 |   ------   |   ------   |   ------   | SUM COSTS 
| =========================================================================== |
| --------------------------------------------------------------------------- | CONSTRAINTS
|  1.000e+01 |  3.405e-29 |  3.843e-28 | -2.628e-13 | -3.502e-28 |   ------   | joint_waypoint_0 
|  1.000e+01 |  1.821e-28 |  6.018e-28 | -3.343e-13 | -4.197e-28 |   ------   | joint_waypoint_1 
|  1.000e+01 |  3.220e-29 |  1.012e-28 | -1.185e-13 | -6.896e-29 |   ------   | joint_waypoint_59 
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_2     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_3     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_4     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_5     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_6     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_7     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_8     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_9     
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_10    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_11    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_12    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_13    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_14    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_15    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_16    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_17    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_18    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_19    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_20    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_21    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_22    
|  1.000e+05 |  9.581e+03 |  9.430e+03 |  1.506e+02 |  1.506e+02 |  1.000e+00 | collision_23    
|  1.000e+05 |  2.303e+04 |  2.288e+04 |  1.514e+02 |  1.514e+02 |  1.000e+00 | collision_24    
|  1.000e+05 |  3.472e+04 |  3.456e+04 |  1.521e+02 |  1.521e+02 |  1.000e+00 | collision_25    
|  1.000e+05 |  4.692e+04 |  4.677e+04 |  1.527e+02 |  1.527e+02 |  1.000e+00 | collision_26    
|  1.000e+05 |  3.698e+03 |  3.507e+03 |  1.905e+02 |  1.905e+02 |  1.000e+00 | collision_27    
|  1.000e+05 |  1.108e+04 |  1.089e+04 |  1.912e+02 |  1.912e+02 |  9.999e-01 | collision_28    
|  1.000e+05 |  1.873e+04 |  1.854e+04 |  1.936e+02 |  1.936e+02 |  1.000e+00 | collision_29    
|  1.000e+05 |  3.132e+04 |  3.114e+04 |  1.798e+02 |  1.799e+02 |  1.000e+00 | collision_30    
|  1.000e+05 |  3.283e+04 |  3.267e+04 |  1.557e+02 |  1.557e+02 |  1.000e+00 | collision_31    
|  1.000e+05 |  2.988e+04 |  2.971e+04 |  1.662e+02 |  1.662e+02 |  1.000e+00 | collision_32    
|  1.000e+05 |  2.544e+04 |  2.526e+04 |  1.727e+02 |  1.727e+02 |  1.000e+00 | collision_33    
|  1.000e+05 |  2.213e+04 |  2.195e+04 |  1.790e+02 |  1.790e+02 |  9.999e-01 | collision_34    
|  1.000e+05 |  1.872e+04 |  1.855e+04 |  1.748e+02 |  1.748e+02 |  9.999e-01 | collision_35    
|  1.000e+05 |  1.453e+04 |  1.434e+04 |  1.877e+02 |  1.877e+02 |  1.000e+00 | collision_36    
|  1.000e+05 |  7.392e+03 |  7.198e+03 |  1.934e+02 |  1.934e+02 |  9.999e-01 | collision_37    
|  1.000e+05 |  2.440e-02 |  6.612e+04 |  2.440e-02 | -6.612e+04 | -2.710e+06 | collision_38    
|  1.000e+05 |  4.820e+04 |  4.804e+04 |  1.564e+02 |  1.564e+02 |  1.000e+00 | collision_39    
|  1.000e+05 |  3.056e+04 |  3.041e+04 |  1.555e+02 |  1.555e+02 |  1.000e+00 | collision_40    
|  1.000e+05 |  1.341e+04 |  1.325e+04 |  1.546e+02 |  1.546e+02 |  1.000e+00 | collision_41    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_42    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_43    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_44    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_45    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_46    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_47    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_48    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_49    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_50    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_51    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_52    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_53    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_54    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_55    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_56    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_57    
|  1.000e+01 |  0.000e+00 |  0.000e+00 |  0.000e+00 |  0.000e+00 |   ------   | collision_58    
| =========================================================================== |
| ---------- |  4.222e+00 |  4.852e+00 |   ------   |   ------   |   ------   | SUM CONSTRAINTS (WITHOUT MERIT) 
| =========================================================================== |
| ---------- |  4.222e+05 |  4.852e+05 |  3.058e+03 | -6.306e+04 | -2.062e+01 | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH MERIT)
| =========================================================================== |
[INFO] shrunk trust region. new box size: 0.0000
[INFO] converged because trust region is tiny
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_23
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_24
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_25
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_26
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_27
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_28
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_29
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_30
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_31
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_32
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_33
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_34
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_35
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_36
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_37
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_39
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_40
[INFO] Not all constraints are satisfied. Increasing constraint penalties for collision_41
[INFO] New merit_error_coeffs: (10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 1e+06, 100000, 1e+06, 1e+06, 1e+06, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10, 10)
[INFO] optimization couldn't satisfy all constraints
[INFO] 
==================
Optimization results:
status: PENALTY_ITERATION_LIMIT
cost values: (0.118916, 0.331213)
constraint violations: (3.40544e-30, 1.82119e-29, 3.22022e-30, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.0958098, 0.230267, 0.347153, 0.469239, 0.0369767, 0.110811, 0.18732, 0.313168, 0.328265, 0.29878, 0.254376, 0.221331, 0.1872, 0.145277, 0.0739182, 2.44035e-07, 0.481962, 0.305629, 0.134055, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0)
n func evals: 16
n qp solves: 15
==================

Questions

• Why does the optimization succeed even though the collision constraint is not met?
• Why does the safety_margin_buffer seem to have something to do with how the collision constraint is evaluated?
• Why does the contact test type ALL tend to fail for a case like this?
  • My guess is that many of the contact vectors cancel each other out when all are considered, so the robot never really gets pushed away from this type of obstacle. This may also be why using the "weighted sum" option also is not successful with this application.

[Levi-Armstrong]

Why does the optimization succeed even though the collision constraint is not met?

In this case the constraint violations: does have all zeros, but clearly it is in collision. I suspect there is an issues with the initialization of cost and constraints. What I think it has done is it has not initialized the cost/constraints errors so when it tries to solve the first iteration it fails and then check the current errors and says it is valid. Can you post a setup to repeat the issue? This is a good find

Why does the safety_margin_buffer seem to have something to do with how the collision constraint is evaluated?

This will impact the solver behavior because it results in an equation being added to the solver. I would expect different behavior when changing the value. I usually do not set this value to a vary large number because currently I believe this only modeled as a linear equation and not a quadratic. This is most to prevent bouncing around at the threshold which speeds up convergence. I usually only set this to a few centimeters.

Why does the contact test type ALL tend to fail for a case like this?

This is a known limitation for you environment. Where joint 5 and 6 will provide gradients to move away from the robots origin and then joint 4 will provide a gradient to move toward the robots origin. This will prevent it from solving, which is why when you only the closest is provided it succeeds.

[marip8]

Here are the settings for the trajopt default composite profile:

Here are the settings for the simple planner LVS default plan profile:

The URDF (the mesh file names are relative to a non-existent package, so they will need to be modified for testing)

trajopt_test_urdf.zip

Waypoint 1:

Waypoint 2: