Improve efficiency (add LocomotionSolver class)