Added a constraint for speed of the gait
Made function for computing trajectories
Wrote a script to plot work vs speed