Implementation of Effort Informed Trees (EIT*)

[marlinstrub]

Hi Mark,

This PR implements Effort Informed Trees (EIT*), an asymptotically optimal planner. It builds on and extends my previous work, AIT*, and simultaneously calculates and exploits multiple heuristics with an asymmetric bidirectional search. One of the heuristics it leverages is an effort heuristic which considers the expected number of collision checks required to validate a path. This effort heuristic is used to guide EIT*'s search towards solutions that are fast to find.

The associated publication is currently in revision for publication in IJRR. A preprint of it can be found on arxiv and a short trailer is available on youtube.

An implementation detail I'd like to explain because it touches core OMPL code: EIT* can leverage both admissible and inadmissible cost heuristics to guide its search, but OMPL currently only supports the admissible version (via OptimizationObjective::motionCostHeuristic(...)). This PR implements a new method, OptimizationObjective::motionCostBestEstimate(...), to denote a potentially inadmissible heuristic of the motion cost. This inadmissible heuristic can be more accurate than its admissible counterpart because it can include more problem-knowledge, e.g., information that may overestimate the true edge cost.

I made sure EIT* passes the 2dcircles_optimize.cpp test and added it to the OptimalPlanning.cpp demo, but I would love to have a second pair of eyes look over this code before it is merged. Let me know if I can provide any more info and/or help with merging this in any way.

[gammell]

Should we rebase/flatten the commits in this PR a bit?

[marlinstrub]

Thanks @zkingston !