github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 21:48:06 -05:00
Code Issues Packages Projects Releases Wiki Activity

bug fix for dubins path

Browse Source
This commit is contained in:
AtsushiSakai
2017-05-21 10:53:15 -07:00
parent 92a1d97423
commit 11758d7742
3 changed files with 117 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
scripts/PathPlanning/DubinsPath/dubins_path_planning.py
View File

@@ -16,6 +16,16 @@ def mod2pi(theta):
return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi)
def pi_2_pi(angle):
while(angle >= math.pi):
angle = angle - 2.0 * math.pi
while(angle <= -math.pi):
angle = angle + 2.0 * math.pi
return angle
def LSL(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
@@ -153,10 +163,8 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c):
for planner in planners:
t, p, q, mode = planner(alpha, beta, d)
if t is None:
print("".join(mode) + " cannot generate path")
# print("".join(mode) + " cannot generate path")
continue
# px, py, pyaw = generate_course([t, p, q], mode, c)
# plt.plot(px, py, label=("".join(mode)))
cost = (abs(t) + abs(p) + abs(q))
if bcost > cost:
@@ -166,7 +174,7 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c):
# print(bmode)
px, py, pyaw = generate_course([bt, bp, bq], bmode, c)
return px, py, pyaw, bmode
return px, py, pyaw, bmode, bcost
def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
@@ -197,16 +205,24 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
ley = - math.sin(syaw) * ex + math.cos(syaw) * ey
leyaw = eyaw - syaw
lpx, lpy, lpyaw, mode = dubins_path_planning_from_origin(
lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin(
lex, ley, leyaw, c)
px = [math.cos(-syaw) * x + math.sin(-syaw) *
y + sx for x, y in zip(lpx, lpy)]
py = [- math.sin(-syaw) * x + math.cos(-syaw) *
y + sy for x, y in zip(lpx, lpy)]
pyaw = leyaw - syaw
pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw]
# print(syaw)
# pyaw = lpyaw
return px, py, pyaw, mode
# plt.plot(pyaw, "-r")
# plt.plot(lpyaw, "-b")
# plt.plot(eyaw, "*r")
# plt.plot(syaw, "*b")
# plt.show()
return px, py, pyaw, mode, clen
def generate_course(length, mode, c):
@@ -215,11 +231,14 @@ def generate_course(length, mode, c):
py = [0.0]
pyaw = [0.0]
d = 0.001
for m, l in zip(mode, length):
pd = 0.0
while pd <= abs(l):
if m is "S":
d = 1.0 / c
else: # turning couse
d = math.radians(3.0)
while pd < abs(l - d):
# print(pd, l)
px.append(px[-1] + d * c * math.cos(pyaw[-1]))
py.append(py[-1] + d * c * math.sin(pyaw[-1]))
@@ -231,8 +250,18 @@ def generate_course(length, mode, c):
elif m is "R": # right turn
pyaw.append(pyaw[-1] - d)
pd += d
else:
d = l - pd
px.append(px[-1] + d * c * math.cos(pyaw[-1]))
py.append(py[-1] + d * c * math.sin(pyaw[-1]))
# print(px, py, pyaw)
if m is "L": # left turn
pyaw.append(pyaw[-1] + d)
elif m is "S": # Straight
pyaw.append(pyaw[-1])
elif m is "R": # right turn
pyaw.append(pyaw[-1] - d)
pd += d
return px, py, pyaw
@@ -241,6 +270,7 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
u"""
Plot arrow
"""
import matplotlib.pyplot as plt
if not isinstance(x, float):
for (ix, iy, iyaw) in zip(x, y, yaw):
@@ -265,14 +295,18 @@ if __name__ == '__main__':
curvature = 1.0
px, py, pyaw, mode = dubins_path_planning(start_x, start_y, start_yaw,
end_x, end_y, end_yaw, curvature)
px, py, pyaw, mode, clen = dubins_path_planning(start_x, start_y, start_yaw,
end_x, end_y, end_yaw, curvature)
plt.plot(px, py, label="final course " + "".join(mode))
# plotting
plot_arrow(start_x, start_y, start_yaw)
plot_arrow(end_x, end_y, end_yaw)
# for (ix, iy, iyaw) in zip(px, py, pyaw):
# plot_arrow(ix, iy, iyaw, fc="b")
plt.legend()
plt.grid(True)
plt.axis("equal")

74
scripts/PathPlanning/RRTCar/dubins_path_planning.py
View File

@@ -16,6 +16,16 @@ def mod2pi(theta):
return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi)
def pi_2_pi(angle):
while(angle >= math.pi):
angle = angle - 2.0 * math.pi
while(angle <= -math.pi):
angle = angle + 2.0 * math.pi
return angle
def LSL(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
@@ -155,8 +165,6 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c):
if t is None:
# print("".join(mode) + " cannot generate path")
continue
# px, py, pyaw = generate_course([t, p, q], mode, c)
# plt.plot(px, py, label=("".join(mode)))
cost = (abs(t) + abs(p) + abs(q))
if bcost > cost:
@@ -204,7 +212,15 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
y + sx for x, y in zip(lpx, lpy)]
py = [- math.sin(-syaw) * x + math.cos(-syaw) *
y + sy for x, y in zip(lpx, lpy)]
pyaw = [iyaw - syaw for iyaw in lpyaw]
pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw]
# print(syaw)
# pyaw = lpyaw
# plt.plot(pyaw, "-r")
# plt.plot(lpyaw, "-b")
# plt.plot(eyaw, "*r")
# plt.plot(syaw, "*b")
# plt.show()
return px, py, pyaw, mode, clen
@@ -215,11 +231,14 @@ def generate_course(length, mode, c):
py = [0.0]
pyaw = [0.0]
d = 0.001
for m, l in zip(mode, length):
pd = 0.0
while pd <= abs(l):
if m is "S":
d = 1.0 / c
else: # turning couse
d = math.radians(3.0)
while pd < abs(l - d):
# print(pd, l)
px.append(px[-1] + d * c * math.cos(pyaw[-1]))
py.append(py[-1] + d * c * math.sin(pyaw[-1]))
@@ -231,8 +250,18 @@ def generate_course(length, mode, c):
elif m is "R": # right turn
pyaw.append(pyaw[-1] - d)
pd += d
else:
d = l - pd
px.append(px[-1] + d * c * math.cos(pyaw[-1]))
py.append(py[-1] + d * c * math.sin(pyaw[-1]))
# print(px, py, pyaw)
if m is "L": # left turn
pyaw.append(pyaw[-1] + d)
elif m is "S": # Straight
pyaw.append(pyaw[-1])
elif m is "R": # right turn
pyaw.append(pyaw[-1] - d)
pd += d
return px, py, pyaw
@@ -256,28 +285,13 @@ if __name__ == '__main__':
print("Dubins path planner sample start!!")
import matplotlib.pyplot as plt
# start_x = 12.50448605843218
# start_y = 0.081546724750168
# start_yaw = 1.9789999999998602
start_x = 1.0 # [m]
start_y = 1.0 # [m]
start_yaw = math.radians(45.0) # [rad]
# end_x = 14.42965160407553
# end_y = 7.971199401268141
# end_yaw = 2.5221853071798117
start_x = 2.379024570195084
start_y = 1.087711940696707
start_yaw = -2.2749999999998605
end_x = 12.50448605843218
end_y = 0.081546724750168
end_yaw = 1.9789999999998602
# start_x = 1.0 # [m]
# start_y = 1.0 # [m]
# start_yaw = math.radians(45.0) # [rad]
# end_x = -3.0 # [m]
# end_y = -3.0 # [m]
# end_yaw = math.radians(-45.0) # [rad]
end_x = -3.0 # [m]
end_y = -3.0 # [m]
end_yaw = math.radians(-45.0) # [rad]
curvature = 1.0
@@ -289,6 +303,10 @@ if __name__ == '__main__':
# plotting
plot_arrow(start_x, start_y, start_yaw)
plot_arrow(end_x, end_y, end_yaw)
# for (ix, iy, iyaw) in zip(px, py, pyaw):
# plot_arrow(ix, iy, iyaw, fc="b")
plt.legend()
plt.grid(True)
plt.axis("equal")

84
scripts/PathPlanning/RRTCar/rrt_car.py
View File

@@ -1,7 +1,7 @@
#!/usr/bin/python
# -*- coding: utf-8 -*-
u"""
@brief: Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT)
"""
@brief: Path Planning Sample Code with RRT for car like robot.
@author: AtsushiSakai(@Atsushi_twi)
@@ -12,9 +12,9 @@ u"""
import random
import math
import copy
import dubins_path_planning
import numpy as np
import matplotlib.pyplot as plt
import dubins_path_planning
import matplotrecorder
class RRT():
@@ -53,13 +53,9 @@ class RRT():
nind = self.GetNearestListIndex(self.nodeList, rnd)
newNode = self.steer(rnd, nind)
# print(newNode.cost)
if self.__CollisionCheck(newNode, obstacleList):
# nearinds = self.find_near_nodes(newNode)
# newNode = self.choose_parent(newNode, nearinds)
self.nodeList.append(newNode)
# self.rewire(newNode, nearinds)
if animation:
self.DrawGraph(rnd=rnd)
@@ -98,11 +94,6 @@ class RRT():
return newNode
def pi_2_pi(self, angle):
u"""
"""
# angle = float(angle)
while(angle >= math.pi):
angle = angle - 2.0 * math.pi
@@ -121,21 +112,15 @@ class RRT():
nearestNode.x, nearestNode.y, nearestNode.yaw, rnd[0], rnd[1], rnd[2], curvature)
newNode = copy.deepcopy(nearestNode)
newNode.x = rnd[0]
newNode.y = rnd[1]
newNode.yaw = rnd[2]
newNode.x = px[-1]
newNode.y = py[-1]
newNode.yaw = pyaw[-1]
newNode.path_x = px
newNode.path_y = py
newNode.path_yaw = pyaw
newNode.cost += clen
newNode.parent = nind
# print(py)
# print(nearestNode.x, nearestNode.y, nearestNode.yaw)
# print(newNode.x, newNode.y, newNode.yaw)
# dubins_path_planning.plot_arrow(
# nearestNode.x, nearestNode.y, nearestNode.yaw)
# dubins_path_planning.plot_arrow(newNode.x, newNode.y, newNode.yaw)
# dubins_path_planning.plot_arrow(rnd[0], rnd[1], rnd[2], fc="b")
return newNode
@@ -156,7 +141,7 @@ class RRT():
disglist = [self.calc_dist_to_goal(
node.x, node.y) for node in self.nodeList]
goalinds = [disglist.index(i) for i in disglist if i <= 0.5]
goalinds = [disglist.index(i) for i in disglist if i <= 0.1]
# print(goalinds)
mincost = min([self.nodeList[i].cost for i in goalinds])
@@ -180,32 +165,6 @@ class RRT():
def calc_dist_to_goal(self, x, y):
return np.linalg.norm([x - self.end.x, y - self.end.y])
def find_near_nodes(self, newNode):
nnode = len(self.nodeList)
r = 50.0 * math.sqrt((math.log(nnode) / nnode))
# r = self.expandDis * 5.0
dlist = [(node.x - newNode.x) ** 2 +
(node.y - newNode.y) ** 2 for node in self.nodeList]
nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
return nearinds
def rewire(self, newNode, nearinds):
nnode = len(self.nodeList)
for i in nearinds:
nearNode = self.nodeList[i]
dx = newNode.x - nearNode.x
dy = newNode.y - nearNode.y
d = math.sqrt(dx ** 2 + dy ** 2)
scost = newNode.cost + d
if nearNode.cost > scost:
theta = math.atan2(dy, dx)
if self.check_collision_extend(nearNode, theta, d):
nearNode.parent = nnode - 1
nearNode.cost = scost
def DrawGraph(self, rnd=None):
u"""
Draw Graph
@@ -217,19 +176,19 @@ class RRT():
for node in self.nodeList:
if node.parent is not None:
plt.plot(node.path_x, node.path_y, "-g")
# plt.plot([node.x, self.nodeList[node.parent].x], [
# node.y, self.nodeList[node.parent].y], "-g")
for (ox, oy, size) in obstacleList:
plt.plot(ox, oy, "ok", ms=30 * size)
plt.plot(self.start.x, self.start.y, "xr")
plt.plot(self.end.x, self.end.y, "xr")
dubins_path_planning.plot_arrow(
self.start.x, self.start.y, self.start.yaw)
dubins_path_planning.plot_arrow(
self.end.x, self.end.y, self.end.yaw)
plt.axis([-2, 15, -2, 15])
plt.grid(True)
plt.pause(0.01)
# plt.show()
# input()
matplotrecorder.save_frame() # save each frame
def GetNearestListIndex(self, nodeList, rnd):
dlist = [(node.x - rnd[0]) ** 2 +
@@ -270,6 +229,7 @@ class Node():
if __name__ == '__main__':
print("Start rrt start planning")
import matplotlib.pyplot as plt
# ====Search Path with RRT====
obstacleList = [
(5, 5, 1),
@@ -281,13 +241,17 @@ if __name__ == '__main__':
] # [x,y,size(radius)]
# Set Initial parameters
rrt = RRT(start=[0.0, 0.0, math.radians(0.0)], goal=[10.0, 10.0, math.radians(0.0)],
randArea=[-2.0, 15.0], obstacleList=obstacleList)
path = rrt.Planning(animation=False)
start = [0.0, 0.0, math.radians(0.0)]
goal = [10.0, 10.0, math.radians(0.0)]
rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList)
path = rrt.Planning(animation=True)
# Draw final path
rrt.DrawGraph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.grid(True)
plt.pause(0.01) # Need for Mac
plt.pause(0.001)
plt.show()
matplotrecorder.save_movie("animation.gif", 0.1)