Revision history [back]

If you're okay with non-optimal paths, I recommend using the Open Motion Planning Library. It contains implementations of several sampling-based planning algorithms (see https://ompl.kavrakilab.org/planners.html). Many of OMPL's algorithms produce optimal solutions, but given the non-holonomic nature of the tractor-trailer system, it will be difficult to use them. I recommend using the RRT algorithm described in the above link under the control-based planners section. OMPL does not come with collision-detection, so you'll have to implement that on your own.

If you want optimal paths, you have a few options:

Option A: Implement your own version of the sbpl cart planner. The sbpl cart planner relies on a set of pre-defined motion primitives to incrementally construct and search a lattice graph. See https://www.cs.cmu.edu/~maxim/files/tutorials/robschooltutorial_oct10.pdf and https://github.com/AtsushiSakai/HybridAStarTrailer for more details.

Option B: Use a near-optimal sampling-based planning algorithm from OMPL's suite of control-based planners. I recommend the SST algorithm.

Option C: Use an asymptotically optimal planner from OMPL's suite of geometric planners. I recommend RRT*. In order to use this, you'll need to define an inverse kinematic function that can connect any two states by a kinematically feasible path. For instructions on how to do this, read http://www.cs.uu.nl/research/techreps/repo/CS-1996/1996-09.pdf.

Among these options, I recommend Option A. Lattice planning is not too difficult to implement and generally gives good results in practice.