github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-02-03 19:54:57 -05:00
Code Issues Packages Projects Releases Wiki Activity

fix bug of reedsshepppath

Browse Source
This commit is contained in:
Atsushi Sakai
2017-10-19 12:31:19 -07:00
parent 47cd9c0f25
commit 2723a0a2eb
4 changed files with 10 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
PathPlanning/DubinsPath/dubins_path_planning.py
View File

@@ -132,11 +132,11 @@ def LRL(alpha, beta, d):
c_ab = math.cos(alpha - beta)
mode = ["L", "R", "L"]
tmp_lrl = (6. - d * d + 2 * c_ab + 2 * d * (- sa + sb)) / 8.
tmp_lrl = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (- sa + sb)) / 8.0
if abs(tmp_lrl) > 1:
return None, None, None, mode
p = mod2pi(2 * math.pi - math.acos(tmp_lrl))
t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.)
t = mod2pi(-alpha - math.atan2(ca - cb, d + sa - sb) + p / 2.0)
q = mod2pi(mod2pi(beta) - alpha - t + mod2pi(p))
return t, p, q, mode