Henry/self cost tuned implementation

[henryhshu]

Implemented a supplementary constraint during sampling that penalizes points for being too close to each other:

• prevents lolipopping and looping of the generated path when an obstacle is hit
• current implementation combines with virtual_tail_cost to avoid obstacles
• also added some ui improvements so we can have some usable visualizations for the report

This is a good improvement over the base cost gradient, but we still need to figure out how to incorporate guided diffusion (user input steering) since right now it's just diffusing a path out of nowhere and we want it to actually try to go towards a specified target.

[ingriddleee]

Some changes so that further down the road we don't run into annoying bugs.