Add clothoid backend to TrajectoryGenerator

[calcmogul]

We currently provide cubic and quintic hermite spline path backends, but users can't easily create constant or linear curvature paths with them. This would have been useful for the Infinite Recharge At Home courses involving trajectories around cones. Cornu spirals offer such a constant or linear curvature path: https://en.wikipedia.org/wiki/Euler_spiral.

As for naming, I dislike Euler spiral given the sheer amount of things named after Euler already: https://en.wikipedia.org/wiki/List_of_things_named_after_Leonhard_Euler. Alternate names include Cornu spiral or Clothoid.

SplineParameterizer returns a list of poses with curvatures, so the correct place to put the Cornu spiral generation is there (i.e., CornuSpiral and CornuSpiralParameterizer).

The path constraints of our cubic splines are start pose, start velocity, interior translations, and end pose. The path constraints of our quintic splines are start pose, interior poses, and end pose. The path constraints of Cornu spirals can be either that of cubic or quintic Hermite splines; the main difference is how the interpolation is done.

Ideally, users could combine multiple path backends in the same trajectory. Combining the trajectories after the fact is possible, but it enforces extra (possibly undesirable) dynamics constraints at the sub-trajectory boundaries. One way to facilitate that use case is overloading GenerateTrajectory() to take a list of poses with curvatures. Lists would be generated by the user with the spline or spiral parameterizers, then concatenated.

[shueja]

Following because “sub-trajectories” affects the design decisions of the PathWeaver successor, WayMaker, that I’m currently working on.

[calcmogul]

Here's some papers on numerically computing them. https://www.researchgate.net/publication/292669884_The_Clothoid_Computation_A_Simple_and_Efficient_Numerical_Algorithm https://www.researchgate.net/publication/230795109_Fast_and_accurate_clothoid_fitting

[chauser]

I'm very much in favor of adding a clothoid trajectory generator. I implemented clothoid paths for team 4061 in 2021 and we were very happy with the results for the autonomous driving tasks last year. We found it was much easier to specify and tune a path based on clothoids, arcs, and lines that our robot could follow successfully than it was to tune spline-based paths.

My approach was to make a generalized spline class (Cornu spirals and circles are not splines!) and basically lie to the trajectory-generating classes about what they were being passed. It occurred to me after I was done that in following a Cornu spiral at speed there may be only 2 or 3 changes to motor speeds between traveling straight and traveling in a circular arc; but making the transition in 2 or 3 steps seemed to work better than doing it in 1 sudden change. I'm happy to discuss further if you wish.

[calcmogul]

in following a Cornu spiral at speed there may be only 2 or 3 changes to motor speeds between traveling straight and traveling in a circular arc

If you set the boundary conditions correctly, that shouldn't happen. One of the defining features of clothoids is ensuring continuous curvature between straight line segments, constant-curvature arcs, and linearly-changing-curvature arcs.

Did you just evaluate the Fresnel integral numerically with first-order integration to draw the shape?

[chauser]

Maybe the way I was using the Cornu spirals differs from what you're envisioning: we made paths that over the great part of their length were straight lines and circular arcs with Cornu spirals connecting them. So the Cornu segments were only a couple of feet long or less. I guess I underestimated the number of steps in the motor control -- we were using a 20ms main loop so that's 5 updates/foot at 10'/s. Since the motor control is discretized you don't really get continuous curvature: my goal was simply "good enough".

For the cosine integral approximation I used: C(s) = s - s^5/10 and for sine integral: S(s) = s^3/3 - s^7/42. I have a note that says "probably bad beyond s=1 or so", but I felt they were good enough for the transitions that I needed and the physical sources of error -- primarily wheel slipping, I think.

[calcmogul]

Since the motor control is discretized you don't really get continuous curvature: my goal was simply "good enough".

Discretization shouldn't matter as long as you're sampling often enough for your drivetrain's dynamics (20 ms is probably fine, but my former team did 5 ms). Since the input is voltage, which has a linear mapping to acceleration, and the continuous variable is velocity, you can get perfect tracking with finite voltage commands from a model-based feedforward. Feedback deals with disturbances, of course.

Basically, you want to limit curvature derivative, and C2 continuity does that. When you discretize, even C1 continuity (discontinuous curvature) has a finite derivative because it's limited by the sample period (e.g., accel = (v₂ - v₁) / 20 ms). The numbers you get can still be huge tho, so C2 is better.