Cart Pole Trajectory optimization using Cvxpy

I’ve been watching Russ Tedrake’s underactuated robotics and been trying some stuff out. The Drake package probably does this stuff better. Also IpOpt and Snopt are the libraries that get mentioned when people want to do this stuff for real.

It’s  too slow to be practical. That is mostly the fault of the cvxpy overhead. The ECOS solver itself says that it solves each iteration in about 0.02 seconds on my macbook.

The idea is to use linearized dynamics as constraints. Then iteratively ask cvxpy to solve for us. Until hopefully it converges. This is Sequential Convex Programming https://web.stanford.edu/class/ee364b/lectures/seq_slides.pdf

I used the discretization of the equations of motion using the mehotd described here https://epubs.siam.org/doi/pdf/10.1137/16M1062569

It is possible I did it right.

The Hermite Collocation for the trajectory and trapezoidal for the control

If we used just an absolute value cost, this all would be a linear program. Something to consider. Also CVXOPT would probably be faster.

It hurts me that the constraint and cost matrices are banded and could be solved so quickly. But the next level up in programming complexity takes a lot more work it seems to me.

 

This is the result

Green is cart acceleration. You can see it slamming into the maximum acceleration constraint

Blue is pole angle and orange is angular velocity. So it does a pretty good job. For some settings it is just awful though.

figure_1

Leave a Reply

Your email address will not be published. Required fields are marked *