fix jittery path planner with 2025 code

[icros-personal]

When we try to run PathPlanner 2025 with simple paths, we get very strange velocity requests, where the requested velocity seems to jump up and down repeatedly without any apparent reason.

[icros-personal]

After some investigation, this also happens on simulator, and appears to be generated by the PathPlanner trajectory code.
Here are more graphs from attempts to tune the LTV parameters, specifically the velocity control effort, on simulator. Note that the oscillations change in amplitude but do not go away at all.

[icros-personal]

After more debugging in simulator, I noticed that there is actually some drift in heading as well - despite the name, "straightlong" is not perfectly straight, because the control points are not quite accurate. Fixing this makes the heading drift go away, but the velocity oscillations persist. This appears to have been introduced in the GUI tool and then committed in 14e88df. The change is probably not enough to be too concerned, but we should be careful when using the PathPlanner GUI to avoid unwanted changes.

[icros-personal]

Debugging into the PathPlannerTrajectory class, this is the line where the problem seems to start. Before this point, the states show a clear increase in velocity up to the maximum. After that, the velocities oscillate. This may be a PathPlanner 2025 bug?

[icros-personal]

More evidence of a PathPlanner bug. The 2024 code (here) uses the max acceleration as the value for vf^2 = v0^2 + 2ad. In the new code, the acceleration is calculated in a more fancy way, based on the robot config that is provided. However, it uses the forceVec from here which causes the acceleration term to be negative. This acceleration term must not be negative for this calculation to work, because it's using the robot's config to make sure our changes in velocity are within the spec. Since it's negative, this calculation is just wrong, and we get strange velocities as a result.
Do we open a bug with the PathPlanner library? Or just fix it ourselves?

[peter9477]

I'd open a bug, unless it is clear that nobody could possibly be using this with the apparent bug, in which case it's more likely our understanding that's wrong and maybe a chiefdelphi thread makes more sense.

[icros-personal]

I opened mjansen4857/pathplanner#1049. I did also attempt to fix it by overriding the code locally (lots of copy/paste, in retrospect I should probably just have forked the library) and that definitely made things better in terms of jitter. The problem I noticed was that the path did seem to consistently overshoot the stopping point. I'm not sure if this could be fixed with better tuning, or if it's a problem that's introduced by the change.