Changed to a one link pendulum.
Added constraint plotting function and changed lateral force.