Navigation Path Following

[qhdwight]

Motivation

Although the current path following algorithm works (stop and turn and go in straight line) it could be better. We lose time stopping and as we have seen time is critical for interventions (and in general misc things that go wrong). Hence we propose a path following algorithm with extra features.

Requirements

• Follow a spline cleanly (do not want to stop and turn)
• Resilient to stalls (such as recovery, not moving, etc)
• Support for constraints?

Options

• Linear MPC
• Pure pursuit

[qhdwight]

@niwhsa9 's MPC implementation in Python: https://github.com/niwhsa9/mpc-trajectory-tracker
Paper: http://www.ece.ufrgs.br/~fetter/sbai05_10022.pdf
General info about state space: https://file.tavsys.net/control/controls-engineering-in-frc.pdf (read 6.1 onward)
General info about lin alg for robo: https://github.com/michiganrobotics/rob101/blob/main/Fall%202021/Textbook/ROB_101_December_2021_Grizzle.pdf