Differential drive motion planning