Implementation Details: We used Python to prototype our optimizer and CEM using Jax [1]} as our GPU accelerated linear algebra back-end. Our batch optimizer was always initialized with a Gaussian distribution proposed in [2]} centered around a straight-line trajectory. We also built an MPC on top of our batch optimizer, wherein we warm-started the Lagrange multipliers \(\lambda _l, \lambda _{\psi , l}\)  with the solution obtained at the previous control loop. We ran the MPC with a time-budget of \(0.04s\)  which was enough to perform 10 iterations our optimizer with a batch size of 1000. We used the following benchmarks for validation.
