The videos posted so far have been only the trajectory generation, i.e. the feedforward signal. In the videos below the trajectories were applied in a realistic simulation. The results are not good. Where the trajectory generation itself is flexible and robust, the MPC applied in simulation is far from it.