This project was developed at Kuriosity Robotics for the FIRST Tech Challenge competition.
It was a collaboration between Max Cai (maxster), Alec Petridis (chop0), and myself. My contributions were primarily advice and background, since I had to learn a lot of this stuff for my cartpole project.
Max and Alec developed the physics model (building on Iñigo Moreno Caireta's master's thesis) and attempted an MPC implementation for the 2022-23 FTC season, but it was not really successful - solve times were too high. Over the course of the 2023-24 (Center Stage) season, I worked with Max as a mentor/advisor to help bring the project to completion. I implemented a pure CasADi + IPOPT version, which worked well, but would require a decent amount of learning in order to interface with CasADi's generated C code. Max implemented the algorithm using Embotech's FORCES Pro system, which generated an embedded solver. He then interfaced with this solver using the JNI with the help of other team members.
We use a direct multiple shooting approach with Embotech's proprietary "PDIP_NLP" solver, which implements some version of an interior point method. Running on the REV Control Hub, a small(-ish) ARM chip with 1GB RAM, the solve takes about 0.013s, fast enough to control the robot in real time.
On my laptop, the CasADi solver takes 0.005s (5ms) to solve, which is significantly slower than the FORCES solver on Max's laptop, which only takes about 0.0008s (0.8ms). I guess that's what you get for spending lots of time and effort in R&D.
We use MPC primarily as a path following algorithm - it needs a precomputed path through state space. This way, the optimizer does not need to do any high-level path planning at runtime. This optimal path could be found using some sort of global optimization or a BVP solver like Matlab's bvp4c, but we found it was much easier and more practical to simply record a human driver driving the desired path. With practice (which is neccessary for the TeleOp portion of the game anyway), the human driver isn't far from optimal, and we can make tweaks to the path incredibly easily.
Here's a video of our results:
And here's a cool graph comparing real drive data with the simulation. We used a genetic algorithm to find the model parameters.
