Fixing comparison for finding path point closest to final trajectories

[SteveMacenski]

This is a pretty significant bug I found while writing unit tests. It seemed worth bringing here immediately rather than waiting

[SteveMacenski]

Requires some testing when I'm back at my desk to see how this impacts the system. If the value used from here was ever used (e.g. if path align ever called after the other path critics) then that would serious change the path tracking behavior and offsetting logic. Could actually be contributing to other issues since it impacts the 3 path critics + anything involving path ratio

See if this was contributing to the short cutting (or if we can do something about that shortcutting)

[SteveMacenski]

• test changes - shouldn't impact in this branch because it shouldn't have been called -- check that.
• Retune -- if called and impacted the system
• change default order of plugins so that path align is always first to get from plugin foremost (lower compute)