Fix smooth planner

  - Corrected stuff in convert, it needs extensive unit testing.