Refactoring, use parabollic trajectory for zmp