Get multiple trajectories from solver interface

[marip8]

Proposal

We should consider adding the ability to get some number of prioritized plans from the solver interface rather than just the single best plan. Since the search is a graph with potentially many samples per waypoint, most searches will have more than one valid trajectory. We could add an additional function (or modify the existing function) in the solver like below:

// n_trajectories is the number of trajectories the search should return, including the best trajectory.
// Numbers less than zero would indicate that all known solutions should be returned 
virtual std::vector<SearchResult<FloatType>> search(int n_trajectories = -1) = 0;

Use case

It sometimes makes sense to run descartes with "lazy" collision checking (i.e. collision check the state samples, but don't collision check the connections between them) since collision checking is so computationally expensive. In the case that a trajectory fails a post-search collision check, it would be valuable to leverage the next best trajectories until one is found to be collision-free rather than failing simply because the best trajectory was in collision

[Levi-Armstrong]

Sounds good to me.