Do TVLQR and MPC on pendulum with Bilinear controller

[bjack205]

• [ ] Solve pendulum swing-up with ALTRO
• [ ] Track pendulum swing-up with bilinear TVLQR
• [x] Track pendulum swing-up with bilinear MPC

[bjack205]

Test Trajectories

Comments These were obtained by running a handful of ALTRO trajectories with different parameters to get a variety of different trajectories.

• Training trajectories: 400
• Trajectory discretization: dt = 0.05, tf = 4.0 sec (81 knot points)
• Basis functions: state, sine, cosine, chebyshev(4)
• Regression: L2 regularization with weight of 1.1 (solved using a sparse Cholesky decomp)
• Train error: 0.00756 (error is average L2 error between nonlinear and bilinear dynamics)
• Test error: 0.0673
• MPC horizon: 2 seconds (41 knot points)
• Solved using OSQP
• Have to update the constraint matrix for each solve, since the linearization changes