[trajoptlib] Make constraints use current and next index

[calcmogul]

Fixes #811.

[spacey-sooty]

This introduces a regression in differential generation. The straight line path passes on main.
choreo-diagnostics-kj9Stldq.zip

[calcmogul]

I get "locally infeasible" for that project's straight line path on both main and this PR. Here's the solver output on main:

iter   time (ms)      error          cost       infeasibility
=============================================================
  40       0.263   1.575792e+01   7.871685e+01   1.022015e-01
The problem is infeasible due to violated inequality constraints.
Violated constraints (cᵢ(x) ≥ 0) in order of declaration:
  8/297: -1.616065222258007e-06 ≥ 0
  13/297: -2.663494520752877e-06 ≥ 0
  23/297: -2.241180900109177e-08 ≥ 0
  38/297: -6.995848632047105e-06 ≥ 0
  48/297: -1.5678674351732313e-06 ≥ 0

[spacey-sooty]

I get "locally infeasible" for that project's straight line path on both main and this PR. Here's the solver output on main:

iter   time (ms)      error          cost       infeasibility
=============================================================
  40       0.263   1.575792e+01   7.871685e+01   1.022015e-01
The problem is infeasible due to violated inequality constraints.
Violated constraints (cᵢ(x) ≥ 0) in order of declaration:
  8/297: -1.616065222258007e-06 ≥ 0
  13/297: -2.663494520752877e-06 ≥ 0
  23/297: -2.241180900109177e-08 ≥ 0
  38/297: -6.995848632047105e-06 ≥ 0
  48/297: -1.5678674351732313e-06 ≥ 0

Ah yep nvm it was passing on beta 2 we've introduced a regression somewhere betweeen there and here. Interesting because I thought I tested differential stuff for #821 and a similar path was passing

[spacey-sooty]

Tested and seems fine

[calcmogul]

Beta 2 also works for me, and the commit right after beta 2 has feasibility restoration converge to a locally infeasible point.

commit 3c0603a8037815a4924e1dc0f7f6697266a8a4ae
Author: bruingineer <[email protected]>
Date:   Wed Oct 2 20:40:06 2024 -0700

    [trajoptlib] Make GetIndex() usage consistent and fix GetIndex() (#820)