Does anyone know of any open source implementations for finding the optimal path of a Reeds-Shepp car?
I'm trying to implement the formulas myself, but I'm having trouble with one of them. I think it's a typo in their paper; their formula just doesn't spit out the expected result.
The specific formula is found in Section 8.3 in Reeds and Shepp's 1990 paper, found here. I'm trying to find the optimal path from (x = 0, y = 0, theta = 0) to (x = 0, y = 0, theta = -pi), or put differently: the car goes back to the starting position, but pointed in the opposite direction. The correct solution is shown in Figure D of Section 1. The formula in Section 8.3 should give the same turn lengths, but it doesn't. The three turn lengths should all be pi / 3, but working it out by hand gives me completely different values.