First LocomotionPlanner implementation (by Jor)