Here is a description of some of the math of the control
And some more posts during the creation.
Here is an attempt at creating an Iterative LQR controller. Quite Slow, questionably working, but interesting.
MIT has a great course taught by Russ Tedrake (Lecture videos available online). This is where I learned about the control that we ended up using