Initial multi-axis code for MotionPlanner.