LQR-RRT*

LQR heuristic as an extension to sample based motion planning algorithms, such as RRT or RRT*, can be a relatively low-cost distance metric and find optimal plans in domains with complex or underactuated dynamics. Below, is the phase plot propagation of the simple pendulum:

More on this at: http://people.csail.mit.edu/tlp/pdf/2012/ICRA12_1657_FI.pdf