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.
Any existing Reeds-Shepp implementations?
graph algorithms;implementation
Copy of my comment above (I suppose that the question can be accepted/closed):I think that the question is not research level, however perhaps this can help you: http://msl.cs.uiuc.edu/~lavalle/cs326a/rs.c (README: http://msl.cs.uiuc.edu/~lavalle/cs326a/README_RS).You can also take a look at Chapter 13 of the book Planning algorithms.