StaticArray: Use ProgPtr.
MotionPlanner: Micro-optimization.
MotionPlanner: Simplification.
MotionPlanner: Remove unneeded code.