Periodic constraint for traj opt

Constraint to find periodic trajectories using Dircol. I tried to find the most elegant way for us to do this:
https://github.com/cmmccann/pigeon/issues/4

* The tolerance on a state can be set to Inf to ignore that state
* The example given here (quadrotor) is not that useful because the system zero initial state allows a trivial solution (so I specify a non-stable x0 to make it interesting). But most use-cases (like the pigeon) would not specify initial conditions.