Use of Dubins Set Classification for faster distance computations in DubinsStateSpace #1080
Conversation
|
I am running |
This adds dubins set classification in the DubinsStateSpace in order to make the dubins path computation more efficient. The classifications scheme is based on Shkel, Andrei M., and Vladimir Lumelsky. "Classification of the Dubins set." Robotics and Autonomous Systems 34.4 (2001): 179-202. Additional correction is required for the classification scheme, where work from Lim, Jaeyoung, et al. "Circling Back: Dubins set Classification Revisited." Workshop on Energy Efficient Aerial Robotic Systems, International Conference on Robotics and Automation 2023. 2023. has been used.
Jaeyoung-Lim
force-pushed
the
pr-dubins-set-classification
branch
from
31359b0 to
1e463c8
Compare
|
@mamoll Indeed, I could reproduce the issues. The problem was that in some cases, alpha and beta were not in the range of [0, 2pi], which was not really part of my test script. I have hardened this in the last commit by running a mod2pi in the Dubins calculation and it seems to be fixed on my side. Would you be able to check the fix? Thank you in advance! |
The alpha, beta ranges need to be in the range of [0, 2pi]
Jaeyoung-Lim
force-pushed
the
pr-dubins-set-classification
branch
from
3687e6a to
1c3abac
Compare
mamoll
left a comment
mamoll
left a comment
There was a problem hiding this comment.
This is great work. I did some testing to confirm that the old and new code are input/output equivalent and this appears to be the case. I also confirmed that the new code provides a significant performance improvement. Thank you for your contribution.
I have one small stylistic comment. Let me know if you agree.
| DUBINS_RIGHT = 2 | ||
| }; | ||
|
|
||
| enum DubinsClass |
There was a problem hiding this comment.
I'd move this to DubinsStateSpace.cpp inside an anonymous namespace, since it is only used internally and not in the public API.
This addresses a review comment moving the DubinsClass into the anonymous namespace
Jaeyoung-Lim
force-pushed
the
pr-dubins-set-classification
branch
from
ed3d009 to
b6319a7
Compare
|
Thanks again, @Jaeyoung-Lim! |
|
@mamoll Happy to contribute! Thanks for the merge! |
…DubinsStateSpace (ompl#1080) This adds dubins set classification in the DubinsStateSpace in order to make the dubins path computation more efficient. The classifications scheme is based on Shkel, Andrei M., and Vladimir Lumelsky. "Classification of the Dubins set." Robotics and Autonomous Systems 34.4 (2001): 179-202. Additional correction is required for the classification scheme, where work from Lim, Jaeyoung, et al. "Circling Back: Dubins set Classification Revisited." Workshop on Energy Efficient Aerial Robotic Systems, International Conference on Robotics and Automation 2023. 2023. has been used.
2 participants
Problem Description
Currently, the Dubins distance computation is done by going through all six path types exhaustively. This is quite inefficient for applications such as sampling-based planners, as the Dubins distance need to be computed many times repeatedly.
It has been proposed by Shkel(2001) that certain Dubins path types can be excluded by looking at the start and end configurations of the path. While the current OMPL implementation of DubinsStateSpace references the paper from Shkel(2001), the implementation goes through every Dubins path type exhaustively. This was discussed previously in an issue: #1014 with @zkingston and @mamoll
Proposed Solution
The proposed solution is to implement the Dubins Set Classification theme in the
DubinsStateSpace.While trying to implement the Dubins Set Classification method, I ran into some issues where the Dubins Set Classification results were not correct for some cases.
I realized these were related to some issues with the original Dubins set classifications scheme proposed in Shkel(2001), and presented the corrections in the Workshop on Energy Efficient Robotic Systems at ICRA 2023
This work was done with @acfloria @nrjl @rikba
This PR adds an implementation of the DubinsSetClassification including the corrections regarding the long path case classification and the classification of Dubins Path types for equivalency group a_12, a_21, a_34, a_43
Evaluation
The following shows a comparison between the Dubins Set Classification and exhaustive Dubins calculations. (Ours being the Dubins set classification)
The results show that the set classification takes 42% of the time compared to exhaustive Dubins calculations, while always returning the same answer with the exhaustive Dubins path calculations