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

fix rrt_star_dubins

Browse Source
This commit is contained in:
Atsushi Sakai
2019-07-27 15:08:25 +09:00
parent 2bf23adb27
commit d9508f6e72
3 changed files with 28 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
PathPlanning/DubinsPath/dubins_path_planning.py
View File

@@ -6,8 +6,9 @@ author Atsushi Sakai(@Atsushi_twi)
"""
import math
import numpy as np
import matplotlib.pyplot as plt
import numpy as np
show_animation = True
@@ -136,8 +137,8 @@ def LRL(alpha, beta, d):
return t, p, q, mode
def dubins_path_planning_from_origin(ex, ey, eyaw, c):
# nomalize
def dubins_path_planning_from_origin(ex, ey, eyaw, c, D_ANGLE):
# normalize
dx = ex
dy = ey
D = math.sqrt(dx ** 2.0 + dy ** 2.0)
@@ -165,12 +166,12 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c):
bcost = cost
# print(bmode)
px, py, pyaw = generate_course([bt, bp, bq], bmode, c)
px, py, pyaw = generate_course([bt, bp, bq], bmode, c, D_ANGLE)
return px, py, pyaw, bmode, bcost
def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c, D_ANGLE=np.deg2rad(10.0)):
"""
Dubins path plannner
@@ -199,7 +200,7 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
leyaw = eyaw - syaw
lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin(
lex, ley, leyaw, c)
lex, ley, leyaw, c, D_ANGLE)
px = [math.cos(-syaw) * x + math.sin(-syaw)
* y + sx for x, y in zip(lpx, lpy)]
@@ -210,7 +211,7 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
return px, py, pyaw, mode, clen
def generate_course(length, mode, c):
def generate_course(length, mode, c, D_ANGLE):
px = [0.0]
py = [0.0]
@@ -221,7 +222,7 @@ def generate_course(length, mode, c):
if m == "S":
d = 1.0 * c
else: # turning couse
d = np.deg2rad(3.0)
d = D_ANGLE
while pd < abs(l - d):
# print(pd, l)