Overview

Problem statement

Gen 1 had some significant accuracy issues, caused by arc interpolation. While more accurate and faster than pathfinder, it used an imperfect approximation to solve for the inefficient path length parameterization.

This generation will attempt to take the best of both worlds by doing smart numerical integration of the path length, and reusing past calculations as much as possible.

Proposed work

Strategy 1

Lachlan came up with this new strategy:

So part of my current strategies have made the assumption that it is necessary to pre-calculate/approximate the total arc length of the path before generation is started (as I use that to set a displacement for the velocity profile and create an approximation for how much memory I need to reserve to reduce overhead of constantly stretching a std::vector), and as far as I can tell that calculating the arc length first tends to be done elsewhere as well (pathfinder, wpilib, etc).

And so, when you calculate without a geometrical approximation of the path or an arc-length approximation, what you tend to do is iterate along it very slowly like pathfinder does (I might add, pathfinder does this really really inefficiently, as it starts calculating from the start each time it needs to do a lookup) making thousands of tiny advancements.

Now, what my thoughts are, is that you don't necessarily need to pre-compute the arc length at all.

The rough strategy I've come up with goes roughly as such:

• Use a raw parametric, no interpolation or geometric approximations, raw equations for x/y/theta/curvature
• Use lots of very tiny iterations along the path for calculating the distances bit by bit, but do this efficiently, keep iterating from the last known position. i.e. last loop it was calculated 0.3 meters as at t = 0.12, so you can start from there to figure out where 0.31 meters is, duh
• Use iteratively calculated kinematic constraints (i.e., velocity += acceleration * dt, acceleration += jerk * dt) etc whilst respecting constraints, instead of calculating the parameters to kinematic equations beforehand.
• Start at the end of the path, and work in reverse until you reach maximum velocity. (now this is the key part) This is all you need to calculate before you can start execution. Then you save at what point you reach maximum velocity (i.e., when the trajectory is executed, this is where you start to slow down), let's say t = 0.8
• Then for execution, you can just do this math in iteratively during execution. Cause since most of it can all be forward calculated, there's no need to pre-generate most of it. But, you need to know at what point to start decelerating.
• Just realized as well, if you take the hypotenuse of the derivative of a parametric, you can probably use that to get good control over how far to increment t as you sample over it

Lachlan got 3.8mm of error and 0.14 degrees on a path that executes in 3.11 seconds, and 22mm of error and 0.2 degrees of error w/ 13.2s.

Strategy 2

Slightly increased generation time.