Smooth planner seems to work now