Some semblance of optimization now functions, though without constraint checking. Gonna try on real robot.
Reversed accidental changes to biped_controller
Added simple test script for go-to-point functionality. Testing on rbpi asap.