Visualization of Sampled Control and State Trajectories

[LHJordaan]

Good day,

I am curious to know whether the code base provides any functionality to query a subset of sampling or rollout data. For example, it would be useful to retrieve a set of sampled control trajectories and state trajectories that can be used for visualization purposes or data logging.

I scanned through the sampling distribution class template header and noticed there is mention of the visualization of control samples. I obviously don't fully understand the internal workings of the library, but I assume that the sampling and rollouts occur inside GPU kernels, which implies there must be functionality that explicitly copies those trajectories out to the CPU and provides methods to retrieve them. Does the library's API expose a straightforward way to obtain this data, or is it currently not possible?

Any feedback would be much appreciated. Having lots of fun (and headaches) with this awesome library!

Kind regards,
Louis

[JasonGibson274]

Yes! You can use the calculateSampledStateTrajectories() method from the controller class to calculate a per step cost that we find useful for visualization. It will return a collection of the best samples and additional random samples. We collect those samples at the end of any computeControl call. We just copy the output vector out of there so you if you just need samples to look at you can pull that out of sampled_outputs_d_ also in the controller class but would need to copy back to the CPU yourself (though we should add something for that). If you want the additional information from the cost function you would call calculateSampledStateTrajectories and then access sampled_trajectories_ in the controller and sampled_costs_ in the controller as well.

I can provide example code if the above isn't enough as well.

[bogidude]

We do have a method that returns the sampled outputs on the CPU already. You can get the sampled outputs (which are the same as state for the vast majority of dynamics) trajectories by calling getSampledOutputTrajectories() from the MPPI controller and you can set the percentage of samples you want to return by calling setPercentageSampledControlTrajectoriesHelper(perc) from the controller where perc is between 0 and 1 (0.01 returns 1% of sampled trajectories).

We also have a mechanism to return the best K samples as well if you would like; you can set K by calling setTopNSampledControlTrajectoriesHelper(K) from the controller. The top K trajectories are added to the output of getSampledOutputTrajectories() as the last K trajectories returned in the std::vector.

Getting the control samples that generated those state trajectories should also be possible but I don't remember the method name at the moment. If you require that as well, let us know.

[LHJordaan]

Thanks for the quick and helpful feedback! I'll give this a go sometime in the next few days and let you guys know if I succeeded.

[LHJordaan]

I think I succeeded!

I modified runControlIteration() in base_plant.hpp so that immediately after calling computeControl(), calculateSampledStateTrajectories() gets called as well. As I understand it, calculateSampledStateTrajectories() reads the subset of sampled control trajectories out of GPU memory, propagates each through the dynamics to produce full state trajectories, and re-evaluates per-step costs via the cost-function kernels. It then copies the trajectories into host vectors.

Using the provided setters/getters in the controller class to choose how many and which samples to return was straightforward. In my custom plant’s pubControl() method, I periodically retrieve the output, cost and crash-status trajectories and (for now) print some data to the terminal. I expect this data to be very useful for visualization and offline analysis of the optimization process—making it much easier to develop/iterate on my custom dynamics and cost function classes, selection of sampling distribution, etc.

I'm still relatively early in development of my project and might ask a few more questions in the future. Thanks again for the assistance. Please let me know if you see any issues with this approach or have a better recommendation. Otherwise, feel free to close this issue.

[bogidude]

I'm glad you found a solution that works for you! We had not included calculateSampledStateTrajectories() in the runControlIteration() because we want to have separate threads for calculating control and creating the visualization data. Ideally, you would create a thread that runs runControlLoop() from the base plant and a separate thread for running calculateSampledStateTrajectories(), getSampledOutputTrajectories(), and then producing the desired visualizations. Having them separate also means you can run the visualization at a different rate than your control computation loop which lets you keep a high rate of reoptimizing more easily. calculateSampledStateTrajectories() can take basically the same amount of time as computeControl() even with a low number of visualized samples because you have to copy all of the output trajectories back to the CPU. These are things to keep in mind if you are needing more replanning out of MPPI.

Having the visualizations is definitely helpful especially if you make use of getSampledCostTrajectories(). That method returns the cost at each timestep for each sampled trajectory which you can then use to color your trajectories to see which times in the trajectory are causing high or low costs. There might be an off by one error with the time index in the output from getSampledCostTrajectories() so let us know if you verify that. The other visualization trick we have used that is helpful is to visualize the top K trajectories and and giving each of those trajectories more/less color based on their relative contribution to the optimal control sequence. That can be very helpful for tuning the lambda parameter of MPPI.

If you have any more questions, please feel free to ask!

[LHJordaan]

Thanks for the explanation - it makes complete sense. I'd definitely like to avoid any unnecessary overhead. Also, thanks for all the pieces of advice and your willingness to help! Perhaps I can briefly validate my approach with you as I may need to make a few modifications depending on your suggestions.

For context (and your interest): I'm part of a Mining Robotics research group at a university in South Africa. My contribution to the project is developing control software to enable our Clearpath Husky A200 robot to drive autonomously in underground environments. Specifically, we're targeting frontier exploration - navigating to goal points in known parts of a map. I'm using your library as the core of the driving stack.

In my current implementation, upon receiving a driving task (i.e., navigate to goal), I activate a timer running a loop at the control frequency. This loop is responsible for managing all incoming data — incoming messages from ROS subs are cached, and flags are set in their respective callbacks, which the loop checks to process the data. It also handles goal monitoring and dynamic parameter updates for the cost function and dynamics classes. Additionally, within this timer loop I check for new data from my state estimator, and if received, call updateState() followed by runControlIteration() to execute a single control iteration. To add visualization, I could every 250ms or so call calculateSampledStateTrajectories(), getSampledOutputTrajectories(), etc., from within this same loop. Would this approach be acceptable, or would you recommend against it?

Alternatively, would it be better to change the software such that upon receiving a driving task, spin a dedicated thread that runs runControlLoop() and a second thread running at a lower rate for visualization (calculateSampledStateTrajectories(), getSampledOutputTrajectories()`, etc.). Meanwhile, my existing timer loop would remain active purely for managing incoming data, parameter updates and state estimates - not optimizing for the control. Both threads would be spawned when a driving task starts and destroyed upon reaching the goal.

Any of your guys' thoughts on this would be much appreciated!

[JasonGibson274]

Actually a lot of thoughts on this, but first are you using ROS or ROS2? The advice is slightly different for each.

[LHJordaan]

Awesome, I'm eager to improve my understanding of what best practice would be. I am using ROS2 Humble.

[bogidude]

I too have a lot of thoughts on this that might be best addressed by jumping on a call with you at some point. There are many decisions (CUDA thread allocations per block, sampling distribution to use, how many samples versus how many iterations, etc.) that are very dependent on the specific problem and constraints that affect both runtime and controller performance. In general, I would definitely recommend the second approach you describe with multiple threads each focused on a specific task.

If you would be interested in setting up a call, please reach out. I'm not going to post our emails directly to try to keep the amount of spam we receive down but you can find our emails in a paper we have put on arXiv that was also published to IEEE Robotics and Automation Letters (I was the first author on the specific paper I am thinking of).

[LHJordaan]

This would be fantastic. I'll reach out via email.

[LHJordaan]

Just to let you guys know, I have sent you an email. I'm only mentioning it here as well in case my email went into your junk/spam folder.