MotionPlanner: use indices instead of pointers. Improves code size and
RAM size.