Toppra Fails when Given Smooth-by-Construction Trajectory

[cohnt]

What happened?

Given a list of points along a path, I've constructed a smooth trajectory through those points with PiecewisePolynomial.CubicWithContinuousSecondDerivatives, and then passed this trajectory into Toppra with velocity and acceleration limits. However, I've noticed it frequently is failing with the error message

ERROR:drake:Toppra failed to find the maximum path acceleration at knot 3186/4999.

Modifying the output of this error message has confirmed that it's due to infeasible constraints, but I have no idea how to debug it further. I've uploaded a MWE to this gist. You'll have to download the path text file to the same directory as the script, but everything else should work out-of-the-box.

Version

fae6d4af9a

What operating system are you using?

Unknown / Other, Ubuntu 22.04

What installation option are you using?

compiled from source code using CMake

Relevant log output

No response

[sadraddini]

I believe this is possible if your Toppra grid points are not dense enough. Have you tried with much tighter numbers for CalcGridOptions?

[cohnt]

Even setting the grid points to be super dense (via either the evenly spaced ones, or using the below options in CalcGridPoints) leads to Toppra still failing.

opts = CalcGridPointsOptions(max_err=1e-4,
                             max_iter=1000,
                             max_seg_length=1e-2,
                             min_points=100000)
grid_points = Toppra.CalcGridPoints(traj, opts)

Notably, using the default options leads to Toppra succeeding, but the resulting trajectory violates the acceleration limits (which is why I was using higher numbers of grid points).

[sadraddini]

The forward program in Toppra should never be infeasible. This merits an investigation.

[jwnimmer-tri]

Assigning +@hongkai-dai as owner by rote. Feel free to delegate / investigate as appropriate.

[hongkai-dai]

Assign +@cohnt for the investigation.

[hongkai-dai]

BTW, I tried several solvers (CLP/Gurobi/Mosek/Clarabel) and they all fail (although fail at different point of the code path).

[hongkai-dai]

By reading the dual variable values, the infeasible constraint is

-0.0539083382 <= 0.0004000800 u_{0} <= 0.0000108821

This constraint looks suspicious from the numerical point of view, as the coefficients and bounds are pretty small.

[cohnt]

FYI I realized that Toppra currently prefers Clp over Mosek. I tried running the script with Mosek and Gurobi as the solver, but in both instances, the solve still failed (albeit at different knot points).

I also had to update the gist due to changes in where drake models are stored, but it should be up-to-date now.