Fully implemented optimization based control. It's currently only deals with potentials f(x) and not yet f(x, xd), and because of that the potentials fight sometimes so I've removed the default configuration bias and reinstantiated the previous version (that loosens when approaching the goal).