Discrete step optimizer

[mpowelson]

We've discussed similar things in the past like using trajopt costs/cnts in descartes and optimizing with various seeds with Pagmo. However, one thing that I think would be useful is an "optimizer" that takes trajopt(_ifopt) costs/cnts and a vector of states and returns which ones satisfy the constraints and the order of lowest to highest cost. That is what I am wanting to do, but it may be a subset of a more general optimizer.

An example use case could determining the best pose for performing some discrete function (be it a pick and place, welding, drilling, etc) where the end goal optimality is more important than the path. If you want to determine the best pose for that regardless of how you get there, this would be useful to determine the goal. Then you pass the goal into your motion planning pipeline and it can be tweaked to make the whole trajectory optimal.