Avoid Gotcha when using GcsTrajectoryOptimization with Toppra

[axbycc-mark]

Is your feature request related to a problem? Please describe. The Drake documentation currently suggests using Toppra to enforce acceleration bounds. When I tried it in the most obvious way, Toppra said "Toppra failed to find the maximum path acceleration". Here is the relevant documentation.

https://drake.mit.edu/doxygen_cxx/classdrake_1_1planning_1_1trajectory__optimization_1_1_gcs_trajectory_optimization.html

Instead of using the full time-scaling curve, this problem uses a single time-scaling variable for each region. This formulation yields continuous trajectories, which are not differentiable at the transition times between the regions since non-convex continuity constraints are not supported yet. However, it supports continuity on the path r(s) for arbitrary degree. The path r(s) can be reconstructed from the gcs solution q(t) with NormalizeSegmentTimes() and post-processed with e.g. Toppra to enforce acceleration bounds.

I take this to mean that one should follow this skeleton.

gcs = GcsTrajectoryOptimization(num_positions=...)
subgraph = gcs.AddRegions(gcs_regions, gcs_edges, order=...)
gcs.AddTimeCost()
gcs.AddVelocityBounds(velocity_lb, velocity_ub)
gcs.AddPathLengthCost(1.0)
gcs.AddPathContinuityConstraints(1) # continuous derivative in the path

# Here I provide a heuristic generated chain of Vertices that terminates with Points
gcs_traj, result = gcs.SolveConvexRestriction(my_heuristic_chain)

# Normalize segment times, as advised by the documentation
gcs_traj = gcs.NormalizeSegmentTimes(gcs_traj)

# Run Toppra
grid_points = Toppra.CalcGridPoints(gcs_traj, CalcGridPointsOptions())
toppra = Toppra(gcs_traj, plant, grid_points)
toppra.AddJointVelocityLimit(velocity_lb, velocity_ub)
toppra.AddJointAccelerationLimit(accel_lb, accel_ub)        
toppra_times = toppra.SolvePathParameterization()

As mentioned, this results in Toppra failing. During my investigation, I found that GcsTrajectoryOptimization, after running NormalizeSegmentTimes, likes to return trajectories with stationary terminal segments which last one second. My hypothesis is that Toppra does not know how to travel through these segments since doing so would require infinite path speed. After trimming away these problematic segments, Toppra succeeded.

Describe the solution you'd like

1. Maybe update the documentation to point out this gotcha.
2. Clarify in the documentation under which circumstances there will be stationary segments. I imagine that a path whose terminal Vertices are not Points may not have them. Do we have these problematic segments if and only if the solution path is terminated by Points? Or might we have them if there is an internal Vertex on the optimal path which is a Point?

Describe alternatives you've considered I'm currently wrapping the trajectory in a PathParameterizedTrajectory, which reparameterizes the path to start at 1.0 and end at num_segments - 1.0 to get rid of the stationary segments. Passing this wrapped trajectory to Toppra succeeds.

Additional context My related StackOverflow issue.

A screenshot of the first derivative of a trajectory returned by GcsTrajectoryOptimization + NormalizeSegmentTimes.

[wrangelvid]

Thank you for bringing this up! A minimal runnable example would make it easier to help you. I attempted to replicate the issue using a basic 2D example by deliberately concatenating stationary Bezier curves at the beginning and the end of the trajectory. TOPPRA did not raise any issues.

[axbycc-mark]

Here is a reproduction branch which triggers the issue https://github.com/axbycc-mark/GCS-Toppra-Issue-Repro/tree/main

Just run repro.py which will crash, with Toppra saying "ERROR:drake:Toppra failed to find the maximum path acceleration at knot 37/184." Use this flag https://github.com/axbycc-mark/GCS-Toppra-Issue-Repro/blob/f01dcaca66963d45b54bc6cd19cd3be6d242d415/repro.py#L181 to turn on trimming behavior, which fixes the issue.

[wrangelvid]

Thank you for providing the example! I cloned your repository, and it went through without raising any errors. Which drake version are you on? Is MOSEK/Gurobi available to you? There might be an issue with Toppra and the involved solver, which is hard to debug until #20664 lands.

On another note, we have a slightly different to specify a start and goal. In your example you create a subgraph with a start point, an HPolyhedron region and an end point. That subgraph has order three, so for each of the regions it will create a Bézier curve with four control points, which is unnecessary for the points. The recommended approach is to define a separate subgraph for the start and goal of order zero containing only one point. There is a mechanism in SolvePath and SolveConvexRestriction that will automatically strip those points due to the zero order continuity, which would also fix your issue. Nevertheless, I would recommend still looking into why Toppra fails for you.

[axbycc-mark]

Oh very interesting, thanks for looking into this. How can I check my drake version? And yes I have a trial version of MOSEK for a couple weeks.

I will give the multiple subgraph mechanism a try. Thanks!

[wrangelvid]

It depends on the way you installed drake, for pip you can run pip show drake, which should report the version.

[axbycc-mark]

Name: drake
Version: 1.28.0
Summary: Model-based design and verification for robotics
Home-page: https://drake.mit.edu
Author: Drake Development Team
Author-email: drake-users@mit.edu
License: Various
Location: /home/axby/axby/env/lib/python3.10/site-packages
Requires: matplotlib, numpy, pydot, PyYAML
Required-by: 

[wrangelvid]

Looks like we have the same version. @hongkai-dai, do you know how we can debug Toppra to see if we are using the same solver?

[hongkai-dai]

@hongkai-dai, do you know how we can debug Toppra to see if we are using the same solver?

Toppra chooses the solver using https://github.com/RobotLocomotion/drake/blob/8f0a9d727e03bd0a0d0d804b4db9bc69c8b944b1/multibody/optimization/toppra.cc#L611-L612 so if you type this in your python console, you should see which solver is selected

print(pydrake.solvers.MakeFirstAvailableSolver([pydrake.solvers.ClpSolver.id(), pydrake.solvers.MosekSolver.id()]).solver_id().name())

[axbycc-mark]

Thanks everyone. I have updated the reproduction repo to add the solver output. It reports "CLP."

Here is the complete output of the repro.py script on my machine.

Solver infos
CLP
Running GCS
Running Toppra
ERROR:drake:Toppra failed to find the maximum path acceleration at knot 37/184.
Traceback (most recent call last):
  File "/home/axby/gcs_toppra_repro/repro.py", line 218, in <module>
    main()
  File "/home/axby/gcs_toppra_repro/repro.py", line 206, in main
    raise RuntimeError("Toppra failed")
RuntimeError: Toppra failed

[cohnt]

Could be connected to the same numerical issues as in #20619?

[wrangelvid]

Thank you @hongkai-dai ! I also get CLP. For a sanity check, let's make sure we end up using the same solver in GcsTrajOpt: PR

[axbycc-mark]

Merged. My computer reports "Solver used in convex restriction: Clarabel"

[wrangelvid]

Ah, I get Mosek! When I manually set the solver option with GraphOfConvexSetsOptions to Clarabel, Toppra also fails for me. Visually, both trajectories look the same, I tried to compare the numerically and only see discrepancies in the positions, velocities and accelerations that are less than 1e-14. So I don't quite understand why Toppra fails on the solution generated by Clarabel but not on the one from Mosek.

[wrangelvid]

Did enabling Mosek resolve your problem?

[axbycc-mark]

Yes I can confirm that the path produced by Mosek can be processed by Toppra without issue.