I found out that if the goal point is on the radius of curvature as follows:
Then sometimes the path returned is:
But what is expected is:
.
I added a simple check, that checks if the straight path length is zero, if so it calculates the difference in orientation between the start and goal orientations and normalizes it within 0,2*pi, and sets to out[0] and out[1] and out[2] are set to 0.
Please let me know if there is anything wrong with this implementation.
Hi @AndrewWalker ,
I found out that if the goal point is on the radius of curvature as follows: Then sometimes the path returned is: But what is expected is: .
I added a simple check, that checks if the straight path length is zero, if so it calculates the difference in orientation between the start and goal orientations and normalizes it within 0,2*pi, and sets to out[0] and out[1] and out[2] are set to 0.
Please let me know if there is anything wrong with this implementation.
Thanks, Jacob