Feature Request: Access to Optimizer Intermediate Trajectories

[ttchalakov]

The current controller design doesn't allow the user to get the intermediate trajectory optimization for a chosen optimizer in a controller (LQR, iLQR, MPPI, etc.) after running the controller. I suggest that the internal trajectory optimization be saved for each iteration of the controller and then made accessible as part of the controller class through a getter function like internal_traj_optim() or something of the same idea. It would also be most convenient for the user, if possible, to get the optimized trajectory as a trajectory object like for iLQR. LQR will not provide a trajectory object and MPPI could provide a list of trajectories.

[williamedwards]

e8ada34

Added API and implementation for iLQR. Still need to add for other optimizers.