MotionPlanner: optimization.
While building the new commands list, preserve the internal next links, and only fix the next link of the last command at the end.