Check the unit tests of TriLQR (and test_warm_start.jl)