Numerical optimization for things that move : simulation, planning, and control