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

Merge pull request #214 from AtsushiSakai/develop

Browse Source 
Fix RRT rewire bug and code clean up
This commit is contained in:
Atsushi Sakai
2019-08-03 15:42:48 +09:00
committed by GitHub
parent c9a2beab73 76074568ce
commit 4676cc5af3
22 changed files with 1130 additions and 1777 deletions
Download Patch File Download Diff File Expand all files Collapse all files

250
PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py
View File

@@ -14,10 +14,11 @@ author: Karan Chawla(@karanchawla)
Reference: https://arxiv.org/abs/1405.5848
"""
import random
import numpy as np
import math
import random
import matplotlib.pyplot as plt
import numpy as np
show_animation = True
@@ -26,8 +27,14 @@ class RTree(object):
# Class to represent the explicit tree created
# while sampling through the state space
def __init__(self, start=[0, 0], lowerLimit=[0, 0], upperLimit=[10, 10], resolution=1):
def __init__(self, start=None, lowerLimit=None, upperLimit=None, resolution=1.0):
if upperLimit is None:
upperLimit = [10, 10]
if lowerLimit is None:
lowerLimit = [0, 0]
if start is None:
start = [0, 0]
self.vertices = dict()
self.edges = []
self.start = start
@@ -42,20 +49,21 @@ class RTree(object):
self.num_cells[idx] = np.ceil(
(upperLimit[idx] - lowerLimit[idx]) / resolution)
vertex_id = self.realWorldToNodeId(start)
vertex_id = self.real_world_to_node_id(start)
self.vertices[vertex_id] = []
def getRootId(self):
@staticmethod
def get_root_id():
# return the id of the root of the tree
return 0
def addVertex(self, vertex):
def add_vertex(self, vertex):
# add a vertex to the tree
vertex_id = self.realWorldToNodeId(vertex)
vertex_id = self.real_world_to_node_id(vertex)
self.vertices[vertex_id] = []
return vertex_id
def addEdge(self, v, x):
def add_edge(self, v, x):
# create an edge between v and x vertices
if (v, x) not in self.edges:
self.edges.append((v, x))
@@ -63,7 +71,7 @@ class RTree(object):
self.vertices[v].append(x)
self.vertices[x].append(v)
def realCoordsToGridCoord(self, real_coord):
def real_coords_to_grid_coord(self, real_coord):
# convert real world coordinates to grid space
# depends on the resolution of the grid
# the output is the same as real world coords if the resolution
@@ -71,10 +79,10 @@ class RTree(object):
coord = [0] * self.dimension
for i in range(len(coord)):
start = self.lowerLimit[i] # start of the grid space
coord[i] = np.around((real_coord[i] - start) / self.resolution)
coord[i] = int(np.around((real_coord[i] - start) / self.resolution))
return coord
def gridCoordinateToNodeId(self, coord):
def grid_coordinate_to_node_id(self, coord):
# This function maps a grid coordinate to a unique
# node id
nodeId = 0
@@ -85,12 +93,12 @@ class RTree(object):
nodeId = nodeId + coord[i] * product
return nodeId
def realWorldToNodeId(self, real_coord):
def real_world_to_node_id(self, real_coord):
# first convert the given coordinates to grid space and then
# convert the grid space coordinates to a unique node id
return self.gridCoordinateToNodeId(self.realCoordsToGridCoord(real_coord))
return self.grid_coordinate_to_node_id(self.real_coords_to_grid_coord(real_coord))
def gridCoordToRealWorldCoord(self, coord):
def grid_coord_to_real_world_coord(self, coord):
# This function smaps a grid coordinate in discrete space
# to a configuration in the full configuration space
config = [0] * self.dimension
@@ -102,7 +110,7 @@ class RTree(object):
config[i] = start + grid_step
return config
def nodeIdToGridCoord(self, node_id):
def node_id_to_grid_coord(self, node_id):
# This function maps a node id to the associated
# grid coordinate
coord = [0] * len(self.lowerLimit)
@@ -115,12 +123,10 @@ class RTree(object):
node_id = node_id - (coord[i] * prod)
return coord
def nodeIdToRealWorldCoord(self, nid):
# This function maps a node in discrete space to a configuraiton
def node_id_to_real_world_coord(self, nid):
# This function maps a node in discrete space to a configuration
# in the full configuration space
return self.gridCoordToRealWorldCoord(self.nodeIdToGridCoord(nid))
# Uses Batch Informed Trees to find a path from start to goal
return self.grid_coord_to_real_world_coord(self.node_id_to_grid_coord(nid))
class BITStar(object):
@@ -135,6 +141,8 @@ class BITStar(object):
self.maxrand = randArea[1]
self.maxIter = maxIter
self.obstacleList = obstacleList
self.startId = None
self.goalId = None
self.vertex_queue = []
self.edge_queue = []
@@ -154,8 +162,8 @@ class BITStar(object):
upperLimit=upperLimit, resolution=0.01)
def setup_planning(self):
self.startId = self.tree.realWorldToNodeId(self.start)
self.goalId = self.tree.realWorldToNodeId(self.goal)
self.startId = self.tree.real_world_to_node_id(self.start)
self.goalId = self.tree.real_world_to_node_id(self.goal)
# add goal to the samples
self.samples[self.goalId] = self.goal
@@ -163,9 +171,9 @@ class BITStar(object):
self.f_scores[self.goalId] = 0
# add the start id to the tree
self.tree.addVertex(self.start)
self.tree.add_vertex(self.start)
self.g_scores[self.startId] = 0
self.f_scores[self.startId] = self.computeHeuristicCost(
self.f_scores[self.startId] = self.compute_heuristic_cost(
self.startId, self.goalId)
# max length we expect to find in our 'informed' sample space, starts as infinite
@@ -182,11 +190,11 @@ class BITStar(object):
# first column of idenity matrix transposed
id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)
M = np.dot(a1, id1_t)
U, S, Vh = np.linalg.svd(M, 1, 1)
U, S, Vh = np.linalg.svd(M, True, True)
C = np.dot(np.dot(U, np.diag(
[1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh)
self.samples.update(self.informedSample(
self.samples.update(self.informed_sample(
200, cBest, cMin, xCenter, C))
return etheta, cMin, xCenter, C, cBest
@@ -207,7 +215,7 @@ class BITStar(object):
else:
m = 100
cBest = self.g_scores[self.goalId]
self.samples.update(self.informedSample(
self.samples.update(self.informed_sample(
m, cBest, cMin, xCenter, C))
# make the old vertices the new vertices
@@ -225,26 +233,25 @@ class BITStar(object):
foundGoal = False
# run until done
while (iterations < self.maxIter):
while iterations < self.maxIter:
cBest = self.setup_sample(iterations,
foundGoal, cMin, xCenter, C, cBest)
# expand the best vertices until an edge is better than the vertex
# this is done because the vertex cost represents the lower bound
# on the edge cost
while(self.bestVertexQueueValue() <= self.bestEdgeQueueValue()):
self.expandVertex(self.bestInVertexQueue())
while self.best_vertex_queue_value() <= self.best_edge_queue_value():
self.expand_vertex(self.best_in_vertex_queue())
# add the best edge to the tree
bestEdge = self.bestInEdgeQueue()
bestEdge = self.best_in_edge_queue()
self.edge_queue.remove(bestEdge)
# Check if this can improve the current solution
estimatedCostOfVertex = self.g_scores[bestEdge[0]] + self.computeDistanceCost(
bestEdge[0], bestEdge[1]) + self.computeHeuristicCost(bestEdge[1], self.goalId)
estimatedCostOfEdge = self.computeDistanceCost(self.startId, bestEdge[0]) + self.computeHeuristicCost(
bestEdge[0], bestEdge[1]) + self.computeHeuristicCost(bestEdge[1], self.goalId)
actualCostOfEdge = self.g_scores[bestEdge[0]] + \
self.computeDistanceCost(bestEdge[0], bestEdge[1])
estimatedCostOfVertex = self.g_scores[bestEdge[0]] + self.compute_distance_cost(
bestEdge[0], bestEdge[1]) + self.compute_heuristic_cost(bestEdge[1], self.goalId)
estimatedCostOfEdge = self.compute_distance_cost(self.startId, bestEdge[0]) + self.compute_heuristic_cost(
bestEdge[0], bestEdge[1]) + self.compute_heuristic_cost(bestEdge[1], self.goalId)
actualCostOfEdge = self.g_scores[bestEdge[0]] + self.compute_distance_cost(bestEdge[0], bestEdge[1])
f1 = estimatedCostOfVertex < self.g_scores[self.goalId]
f2 = estimatedCostOfEdge < self.g_scores[self.goalId]
@@ -252,46 +259,44 @@ class BITStar(object):
if f1 and f2 and f3:
# connect this edge
firstCoord = self.tree.nodeIdToRealWorldCoord(
firstCoord = self.tree.node_id_to_real_world_coord(
bestEdge[0])
secondCoord = self.tree.nodeIdToRealWorldCoord(
secondCoord = self.tree.node_id_to_real_world_coord(
bestEdge[1])
path = self.connect(firstCoord, secondCoord)
lastEdge = self.tree.realWorldToNodeId(secondCoord)
lastEdge = self.tree.real_world_to_node_id(secondCoord)
if path is None or len(path) == 0:
continue
nextCoord = path[len(path) - 1, :]
nextCoordPathId = self.tree.realWorldToNodeId(
nextCoordPathId = self.tree.real_world_to_node_id(
nextCoord)
bestEdge = (bestEdge[0], nextCoordPathId)
if(bestEdge[1] in self.tree.vertices.keys()):
if bestEdge[1] in self.tree.vertices.keys():
continue
else:
try:
del self.samples[bestEdge[1]]
except(KeyError):
except KeyError:
pass
eid = self.tree.addVertex(nextCoord)
eid = self.tree.add_vertex(nextCoord)
self.vertex_queue.append(eid)
if eid == self.goalId or bestEdge[0] == self.goalId or bestEdge[1] == self.goalId:
print("Goal found")
foundGoal = True
self.tree.addEdge(bestEdge[0], bestEdge[1])
self.tree.add_edge(bestEdge[0], bestEdge[1])
g_score = self.computeDistanceCost(
g_score = self.compute_distance_cost(
bestEdge[0], bestEdge[1])
self.g_scores[bestEdge[1]] = g_score + \
self.g_scores[bestEdge[0]]
self.f_scores[bestEdge[1]] = g_score + \
self.computeHeuristicCost(bestEdge[1], self.goalId)
self.updateGraph()
self.g_scores[bestEdge[1]] = g_score + self.g_scores[bestEdge[0]]
self.f_scores[bestEdge[1]] = g_score + self.compute_heuristic_cost(bestEdge[1], self.goalId)
self.update_graph()
# visualize new edge
if animation:
self.drawGraph(xCenter=xCenter, cBest=cBest,
cMin=cMin, etheta=etheta, samples=self.samples.values(),
start=firstCoord, end=secondCoord, tree=self.tree.edges)
self.draw_graph(xCenter=xCenter, cBest=cBest,
cMin=cMin, etheta=etheta, samples=self.samples.values(),
start=firstCoord, end=secondCoord)
self.remove_queue(lastEdge, bestEdge)
@@ -306,14 +311,13 @@ class BITStar(object):
return self.find_final_path()
def find_final_path(self):
plan = []
plan.append(self.goal)
plan = [self.goal]
currId = self.goalId
while (currId != self.startId):
plan.append(self.tree.nodeIdToRealWorldCoord(currId))
while currId != self.startId:
plan.append(self.tree.node_id_to_real_world_coord(currId))
try:
currId = self.nodes[currId]
except(KeyError):
except KeyError:
print("cannot find Path")
return []
@@ -324,29 +328,30 @@ class BITStar(object):
def remove_queue(self, lastEdge, bestEdge):
for edge in self.edge_queue:
if(edge[1] == bestEdge[1]):
if self.g_scores[edge[1]] + self.computeDistanceCost(edge[1], bestEdge[1]) >= self.g_scores[self.goalId]:
if(lastEdge, bestEdge[1]) in self.edge_queue:
if edge[1] == bestEdge[1]:
dist_cost = self.compute_distance_cost(edge[1], bestEdge[1])
if self.g_scores[edge[1]] + dist_cost >= self.g_scores[self.goalId]:
if (lastEdge, bestEdge[1]) in self.edge_queue:
self.edge_queue.remove(
(lastEdge, bestEdge[1]))
def connect(self, start, end):
# A function which attempts to extend from a start coordinates
# to goal coordinates
steps = int(self.computeDistanceCost(self.tree.realWorldToNodeId(
start), self.tree.realWorldToNodeId(end)) * 10)
steps = int(self.compute_distance_cost(self.tree.real_world_to_node_id(
start), self.tree.real_world_to_node_id(end)) * 10)
x = np.linspace(start[0], end[0], num=steps)
y = np.linspace(start[1], end[1], num=steps)
for i in range(len(x)):
if(self._collisionCheck(x[i], y[i])):
if(i == 0):
if self._collision_check(x[i], y[i]):
if i == 0:
return None
# if collision, send path until collision
return np.vstack((x[0:i], y[0:i])).transpose()
return np.vstack((x, y)).transpose()
def _collisionCheck(self, x, y):
def _collision_check(self, x, y):
for (ox, oy, size) in self.obstacleList:
dx = ox - x
dy = oy - y
@@ -355,45 +360,44 @@ class BITStar(object):
return True # collision
return False
# def prune(self, c):
def computeHeuristicCost(self, start_id, goal_id):
def compute_heuristic_cost(self, start_id, goal_id):
# Using Manhattan distance as heuristic
start = np.array(self.tree.nodeIdToRealWorldCoord(start_id))
goal = np.array(self.tree.nodeIdToRealWorldCoord(goal_id))
start = np.array(self.tree.node_id_to_real_world_coord(start_id))
goal = np.array(self.tree.node_id_to_real_world_coord(goal_id))
return np.linalg.norm(start - goal, 2)
def computeDistanceCost(self, vid, xid):
def compute_distance_cost(self, vid, xid):
# L2 norm distance
start = np.array(self.tree.nodeIdToRealWorldCoord(vid))
stop = np.array(self.tree.nodeIdToRealWorldCoord(xid))
start = np.array(self.tree.node_id_to_real_world_coord(vid))
stop = np.array(self.tree.node_id_to_real_world_coord(xid))
return np.linalg.norm(stop - start, 2)
# Sample free space confined in the radius of ball R
def informedSample(self, m, cMax, cMin, xCenter, C):
def informed_sample(self, m, cMax, cMin, xCenter, C):
samples = dict()
print("g_Score goal id: ", self.g_scores[self.goalId])
for i in range(m + 1):
if cMax < float('inf'):
r = [cMax / 2.0,
math.sqrt(cMax**2 - cMin**2) / 2.0,
math.sqrt(cMax**2 - cMin**2) / 2.0]
math.sqrt(cMax ** 2 - cMin ** 2) / 2.0,
math.sqrt(cMax ** 2 - cMin ** 2) / 2.0]
L = np.diag(r)
xBall = self.sampleUnitBall()
xBall = self.sample_unit_ball()
rnd = np.dot(np.dot(C, L), xBall) + xCenter
rnd = [rnd[(0, 0)], rnd[(1, 0)]]
random_id = self.tree.realWorldToNodeId(rnd)
random_id = self.tree.real_world_to_node_id(rnd)
samples[random_id] = rnd
else:
rnd = self.sampleFreeSpace()
random_id = self.tree.realWorldToNodeId(rnd)
rnd = self.sample_free_space()
random_id = self.tree.real_world_to_node_id(rnd)
samples[random_id] = rnd
return samples
# Sample point in a unit ball
def sampleUnitBall(self):
@staticmethod
def sample_unit_ball():
a = random.random()
b = random.random()
@@ -404,63 +408,64 @@ class BITStar(object):
b * math.sin(2 * math.pi * a / b))
return np.array([[sample[0]], [sample[1]], [0]])
def sampleFreeSpace(self):
def sample_free_space(self):
rnd = [random.uniform(self.minrand, self.maxrand),
random.uniform(self.minrand, self.maxrand)]
return rnd
def bestVertexQueueValue(self):
if(len(self.vertex_queue) == 0):
def best_vertex_queue_value(self):
if len(self.vertex_queue) == 0:
return float('inf')
values = [self.g_scores[v]
+ self.computeHeuristicCost(v, self.goalId) for v in self.vertex_queue]
+ self.compute_heuristic_cost(v, self.goalId) for v in self.vertex_queue]
values.sort()
return values[0]
def bestEdgeQueueValue(self):
if(len(self.edge_queue) == 0):
def best_edge_queue_value(self):
if len(self.edge_queue) == 0:
return float('inf')
# return the best value in the queue by score g_tau[v] + c(v,x) + h(x)
values = [self.g_scores[e[0]] + self.computeDistanceCost(e[0], e[1])
+ self.computeHeuristicCost(e[1], self.goalId) for e in self.edge_queue]
values = [self.g_scores[e[0]] + self.compute_distance_cost(e[0], e[1])
+ self.compute_heuristic_cost(e[1], self.goalId) for e in self.edge_queue]
values.sort(reverse=True)
return values[0]
def bestInVertexQueue(self):
def best_in_vertex_queue(self):
# return the best value in the vertex queue
v_plus_vals = [(v, self.g_scores[v] + self.computeHeuristicCost(v, self.goalId))
v_plus_vals = [(v, self.g_scores[v] + self.compute_heuristic_cost(v, self.goalId))
for v in self.vertex_queue]
v_plus_vals = sorted(v_plus_vals, key=lambda x: x[1])
# print(v_plus_vals)
return v_plus_vals[0][0]
def bestInEdgeQueue(self):
e_and_values = [(e[0], e[1], self.g_scores[e[0]] + self.computeDistanceCost(
e[0], e[1]) + self.computeHeuristicCost(e[1], self.goalId)) for e in self.edge_queue]
def best_in_edge_queue(self):
e_and_values = [(e[0], e[1], self.g_scores[e[0]] + self.compute_distance_cost(
e[0], e[1]) + self.compute_heuristic_cost(e[1], self.goalId)) for e in self.edge_queue]
e_and_values = sorted(e_and_values, key=lambda x: x[2])
return (e_and_values[0][0], e_and_values[0][1])
return e_and_values[0][0], e_and_values[0][1]
def expandVertex(self, vid):
def expand_vertex(self, vid):
self.vertex_queue.remove(vid)
# get the coordinates for given vid
currCoord = np.array(self.tree.nodeIdToRealWorldCoord(vid))
currCoord = np.array(self.tree.node_id_to_real_world_coord(vid))
# get the nearest value in vertex for every one in samples where difference is
# less than the radius
neigbors = []
for sid, scoord in self.samples.items():
scoord = np.array(scoord)
if(np.linalg.norm(scoord - currCoord, 2) <= self.r and sid != vid):
if np.linalg.norm(scoord - currCoord, 2) <= self.r and sid != vid:
neigbors.append((sid, scoord))
# add an edge to the edge queue is the path might improve the solution
for neighbor in neigbors:
sid = neighbor[0]
estimated_f_score = self.computeDistanceCost(
self.startId, vid) + self.computeHeuristicCost(sid, self.goalId) + self.computeDistanceCost(vid, sid)
h_cost = self.compute_heuristic_cost(sid, self.goalId)
estimated_f_score = self.compute_distance_cost(
self.startId, vid) + h_cost + self.compute_distance_cost(vid, sid)
if estimated_f_score < self.g_scores[self.goalId]:
self.edge_queue.append((vid, sid))
@@ -469,22 +474,22 @@ class BITStar(object):
def add_vertex_to_edge_queue(self, vid, currCoord):
if vid not in self.old_vertices:
neigbors = []
neighbors = []
for v, edges in self.tree.vertices.items():
if v != vid and (v, vid) not in self.edge_queue and (vid, v) not in self.edge_queue:
vcoord = self.tree.nodeIdToRealWorldCoord(v)
if(np.linalg.norm(vcoord - currCoord, 2) <= self.r):
neigbors.append((vid, vcoord))
v_coord = self.tree.node_id_to_real_world_coord(v)
if np.linalg.norm(currCoord - v_coord, 2) <= self.r:
neighbors.append((vid, v_coord))
for neighbor in neigbors:
for neighbor in neighbors:
sid = neighbor[0]
estimated_f_score = self.computeDistanceCost(self.startId, vid) + \
self.computeDistanceCost(
vid, sid) + self.computeHeuristicCost(sid, self.goalId)
if estimated_f_score < self.g_scores[self.goalId] and (self.g_scores[vid] + self.computeDistanceCost(vid, sid)) < self.g_scores[sid]:
estimated_f_score = self.compute_distance_cost(self.startId, vid) + self.compute_distance_cost(
vid, sid) + self.compute_heuristic_cost(sid, self.goalId)
if estimated_f_score < self.g_scores[self.goalId] and (
self.g_scores[vid] + self.compute_distance_cost(vid, sid)) < self.g_scores[sid]:
self.edge_queue.append((vid, sid))
def updateGraph(self):
def update_graph(self):
closedSet = []
openSet = []
currId = self.startId
@@ -498,22 +503,21 @@ class BITStar(object):
openSet.remove(currId)
# Check if we're at the goal
if(currId == self.goalId):
self.nodes[self.goalId]
if currId == self.goalId:
break
if(currId not in closedSet):
if currId not in closedSet:
closedSet.append(currId)
# find a non visited successor to the current node
successors = self.tree.vertices[currId]
for succesor in successors:
if(succesor in closedSet):
if succesor in closedSet:
continue
else:
# claculate tentative g score
g_score = self.g_scores[currId] + \
self.computeDistanceCost(currId, succesor)
self.compute_distance_cost(currId, succesor)
if succesor not in openSet:
# add the successor to open set
openSet.append(succesor)
@@ -522,14 +526,13 @@ class BITStar(object):
# update g and f scores
self.g_scores[succesor] = g_score
self.f_scores[succesor] = g_score + \
self.computeHeuristicCost(succesor, self.goalId)
self.f_scores[succesor] = g_score + self.compute_heuristic_cost(succesor, self.goalId)
# store the parent and child
self.nodes[succesor] = currId
def drawGraph(self, xCenter=None, cBest=None, cMin=None, etheta=None,
samples=None, start=None, end=None, tree=None):
def draw_graph(self, xCenter=None, cBest=None, cMin=None, etheta=None,
samples=None, start=None, end=None):
plt.clf()
for rnd in samples:
if rnd is not None:
@@ -549,9 +552,10 @@ class BITStar(object):
plt.grid(True)
plt.pause(0.01)
def plot_ellipse(self, xCenter, cBest, cMin, etheta): # pragma: no cover
@staticmethod
def plot_ellipse(xCenter, cBest, cMin, etheta): # pragma: no cover
a = math.sqrt(cBest**2 - cMin**2) / 2.0
a = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0
b = cBest / 2.0
angle = math.pi / 2.0 - etheta
cx = xCenter[0]

325
PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py
View File

@@ -1,14 +1,13 @@
"""
Path planning Sample Code with Closed loop RRT for car like robot.
author: AtsushiSakai(@Atsushi_twi)
"""
import copy
import math
import os
import random
import sys
import matplotlib.pyplot as plt
@@ -18,92 +17,49 @@ import pure_pursuit
sys.path.append(os.path.dirname(
os.path.abspath(__file__)) + "/../ReedsSheppPath/")
sys.path.append(os.path.dirname(
os.path.abspath(__file__)) + "/../RRTStarReedsShepp/")
try:
import reeds_shepp_path_planning
import unicycle_model
except:
from rrt_star_reeds_shepp import RRTStarReedsShepp
except ImportError:
raise
show_animation = True
target_speed = 10.0 / 3.6
STEP_SIZE = 0.1
class RRT():
class ClosedLoopRRTStar(RRTStarReedsShepp):
"""
Class for RRT planning
Class for Closed loop RRT star planning
"""
def __init__(self, start, goal, obstacleList, randArea,
maxIter=50):
def __init__(self, start, goal, obstacle_list, rand_area,
max_iter=200,
connect_circle_dist=50.0
):
super().__init__(start, goal, obstacle_list, rand_area,
max_iter=max_iter,
connect_circle_dist=connect_circle_dist,
)
self.target_speed = 10.0 / 3.6
self.yaw_th = np.deg2rad(3.0)
self.xy_th = 0.5
self.invalid_travel_ratio = 5.0
def planning(self, animation=True):
"""
Setting Parameter
start:Start Position [x,y]
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:Ramdom Samping Area [min,max]
"""
self.start = Node(start[0], start[1], start[2])
self.end = Node(goal[0], goal[1], goal[2])
self.minrand = randArea[0]
self.maxrand = randArea[1]
self.obstacleList = obstacleList
self.maxIter = maxIter
def try_goal_path(self):
goal = Node(self.end.x, self.end.y, self.end.yaw)
newNode = self.steer(goal, len(self.nodeList) - 1)
if newNode is None:
return
if self.CollisionCheck(newNode, self.obstacleList):
# print("goal path is OK")
self.nodeList.append(newNode)
def Planning(self, animation=True):
"""
Pathplanning
do planning
animation: flag for animation on or off
"""
self.nodeList = [self.start]
self.try_goal_path()
for i in range(self.maxIter):
print("loop:", i)
rnd = self.get_random_point()
nind = self.GetNearestListIndex(self.nodeList, rnd)
newNode = self.steer(rnd, nind)
# print(newNode.cost)
if newNode is None:
continue
if self.CollisionCheck(newNode, self.obstacleList):
nearinds = self.find_near_nodes(newNode)
newNode = self.choose_parent(newNode, nearinds)
if newNode is None:
continue
self.nodeList.append(newNode)
self.rewire(newNode, nearinds)
self.try_goal_path()
if animation and i % 5 == 0:
self.DrawGraph(rnd=rnd)
# planning with RRTStarReedsShepp
super().planning(animation=animation)
# generate coruse
path_indexs = self.get_best_last_indexs()
path_indexs = self.get_goal_indexes()
flag, x, y, yaw, v, t, a, d = self.search_best_feasible_path(
path_indexs)
@@ -120,10 +76,9 @@ class RRT():
# pure pursuit tracking
for ind in path_indexs:
path = self.gen_final_course(ind)
path = self.generate_final_course(ind)
flag, x, y, yaw, v, t, a, d = self.check_tracking_path_is_feasible(
path)
flag, x, y, yaw, v, t, a, d = self.check_tracking_path_is_feasible(path)
if flag and best_time >= t[-1]:
print("feasible path is found")
@@ -142,121 +97,46 @@ class RRT():
return False, None, None, None, None, None, None, None
def check_tracking_path_is_feasible(self, path):
# print("check_tracking_path_is_feasible")
cx = np.array(path[:, 0])
cy = np.array(path[:, 1])
cyaw = np.array(path[:, 2])
cx = np.array([state[0] for state in path])[::-1]
cy = np.array([state[1] for state in path])[::-1]
cyaw = np.array([state[2] for state in path])[::-1]
goal = [cx[-1], cy[-1], cyaw[-1]]
cx, cy, cyaw = pure_pursuit.extend_path(cx, cy, cyaw)
speed_profile = pure_pursuit.calc_speed_profile(
cx, cy, cyaw, target_speed)
cx, cy, cyaw, self.target_speed)
t, x, y, yaw, v, a, d, find_goal = pure_pursuit.closed_loop_prediction(
cx, cy, cyaw, speed_profile, goal)
yaw = [self.pi_2_pi(iyaw) for iyaw in yaw]
yaw = [reeds_shepp_path_planning.pi_2_pi(iyaw) for iyaw in yaw]
if not find_goal:
print("cannot reach goal")
if abs(yaw[-1] - goal[2]) >= math.pi / 4.0:
if abs(yaw[-1] - goal[2]) >= self.yaw_th * 10.0:
print("final angle is bad")
find_goal = False
travel = sum([abs(iv) * unicycle_model.dt for iv in v])
# print(travel)
origin_travel = sum([math.sqrt(dx ** 2 + dy ** 2)
for (dx, dy) in zip(np.diff(cx), np.diff(cy))])
# print(origin_travel)
if (travel / origin_travel) >= 5.0:
if (travel / origin_travel) >= self.invalid_travel_ratio:
print("path is too long")
find_goal = False
if not self.CollisionCheckWithXY(x, y, self.obstacleList):
if not self.collision_check_with_xy(x, y, self.obstacle_list):
print("This path is collision")
find_goal = False
return find_goal, x, y, yaw, v, t, a, d
def choose_parent(self, newNode, nearinds):
if not nearinds:
return newNode
dlist = []
for i in nearinds:
tNode = self.steer(newNode, i)
if tNode is None:
continue
if self.CollisionCheck(tNode, self.obstacleList):
dlist.append(tNode.cost)
else:
dlist.append(float("inf"))
mincost = min(dlist)
minind = nearinds[dlist.index(mincost)]
if mincost == float("inf"):
print("mincost is inf")
return newNode
newNode = self.steer(newNode, minind)
if newNode is None:
return None
return newNode
def pi_2_pi(self, angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
def steer(self, rnd, nind):
# print(rnd)
nearestNode = self.nodeList[nind]
px, py, pyaw, mode, clen = reeds_shepp_path_planning.reeds_shepp_path_planning(
nearestNode.x, nearestNode.y, nearestNode.yaw,
rnd.x, rnd.y, rnd.yaw, unicycle_model.curvature_max, STEP_SIZE)
if px is None:
return None
newNode = copy.deepcopy(nearestNode)
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 += sum([abs(c) for c in clen])
newNode.parent = nind
return newNode
def get_random_point(self):
rnd = [random.uniform(self.minrand, self.maxrand),
random.uniform(self.minrand, self.maxrand),
random.uniform(-math.pi, math.pi)
]
node = Node(rnd[0], rnd[1], rnd[2])
return node
def get_best_last_indexs(self):
# print("search_finish_node")
YAWTH = np.deg2rad(1.0)
XYTH = 0.5
def get_goal_indexes(self):
goalinds = []
for (i, node) in enumerate(self.nodeList):
if self.calc_dist_to_goal(node.x, node.y) <= XYTH:
for (i, node) in enumerate(self.node_list):
if self.calc_dist_to_goal(node.x, node.y) <= self.xy_th:
goalinds.append(i)
print("OK XY TH num is")
print(len(goalinds))
@@ -264,106 +144,17 @@ class RRT():
# angle check
fgoalinds = []
for i in goalinds:
if abs(self.nodeList[i].yaw - self.end.yaw) <= YAWTH:
if abs(self.node_list[i].yaw - self.end.yaw) <= self.yaw_th:
fgoalinds.append(i)
print("OK YAW TH num is")
print(len(fgoalinds))
return fgoalinds
def gen_final_course(self, goalind):
path = [[self.end.x, self.end.y, self.end.yaw]]
while self.nodeList[goalind].parent is not None:
node = self.nodeList[goalind]
path_x = reversed(node.path_x)
path_y = reversed(node.path_y)
path_yaw = reversed(node.path_yaw)
for (ix, iy, iyaw) in zip(path_x, path_y, path_yaw):
path.append([ix, iy, iyaw])
# path.append([node.x, node.y])
goalind = node.parent
path.append([self.start.x, self.start.y, self.start.yaw])
@staticmethod
def collision_check_with_xy(x, y, obstacle_list):
path = np.array(path[::-1])
return path
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
+ (node.yaw - newNode.yaw) ** 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]
tNode = self.steer(nearNode, nnode - 1)
if tNode is None:
continue
obstacleOK = self.CollisionCheck(tNode, self.obstacleList)
imporveCost = nearNode.cost > tNode.cost
if obstacleOK and imporveCost:
# print("rewire")
self.nodeList[i] = tNode
def DrawGraph(self, rnd=None): # pragma: no cover
"""
Draw Graph
"""
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.nodeList:
if node.parent is not None:
plt.plot(node.path_x, node.path_y, "-g")
for (ox, oy, size) in self.obstacleList:
plt.plot(ox, oy, "ok", ms=30 * size)
reeds_shepp_path_planning.plot_arrow(
self.start.x, self.start.y, self.start.yaw)
reeds_shepp_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)
def GetNearestListIndex(self, nodeList, rnd):
dlist = [(node.x - rnd.x) ** 2
+ (node.y - rnd.y) ** 2
+ (node.yaw - rnd.yaw) ** 2 for node in nodeList]
minind = dlist.index(min(dlist))
return minind
def CollisionCheck(self, node, obstacleList):
for (ox, oy, size) in obstacleList:
for (ix, iy) in zip(node.path_x, node.path_y):
dx = ox - ix
dy = oy - iy
d = dx * dx + dy * dy
if d <= size ** 2:
return False # collision
return True # safe
def CollisionCheckWithXY(self, x, y, obstacleList):
for (ox, oy, size) in obstacleList:
for (ox, oy, size) in obstacle_list:
for (ix, iy) in zip(x, y):
dx = ox - ix
dy = oy - iy
@@ -374,26 +165,10 @@ class RRT():
return True # safe
class Node():
"""
RRT Node
"""
def __init__(self, x, y, yaw):
self.x = x
self.y = y
self.yaw = yaw
self.path_x = []
self.path_y = []
self.path_yaw = []
self.cost = 0.0
self.parent = None
def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), maxIter=200):
def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), max_iter=100):
print("Start" + __file__)
# ====Search Path with RRT====
obstacleList = [
obstacle_list = [
(5, 5, 1),
(4, 6, 1),
(4, 8, 1),
@@ -409,16 +184,18 @@ def main(gx=6.0, gy=7.0, gyaw=np.deg2rad(90.0), maxIter=200):
start = [0.0, 0.0, np.deg2rad(0.0)]
goal = [gx, gy, gyaw]
rrt = RRT(start, goal, randArea=[-2.0, 20.0],
obstacleList=obstacleList, maxIter=maxIter)
flag, x, y, yaw, v, t, a, d = rrt.Planning(animation=show_animation)
closed_loop_rrt_star = ClosedLoopRRTStar(start, goal,
obstacle_list,
[-2.0, 20.0],
max_iter=max_iter)
flag, x, y, yaw, v, t, a, d = closed_loop_rrt_star.planning(animation=show_animation)
if not flag:
print("cannot find feasible path")
# Draw final path
if show_animation:
rrt.DrawGraph()
closed_loop_rrt_star.draw_graph()
plt.plot(x, y, '-r')
plt.grid(True)
plt.pause(0.001)

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)

4
PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py
View File

@@ -259,7 +259,7 @@ def planning(ox, oy, reso,
return rx, ry
def planning_animation(ox, oy, reso):
def planning_animation(ox, oy, reso): # pragma: no cover
px, py = planning(ox, oy, reso)
# animation
@@ -281,7 +281,7 @@ def planning_animation(ox, oy, reso):
plt.pause(0.1)
def main():
def main(): # pragma: no cover
print("start!!")
ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0, 0.0]

181
PathPlanning/InformedRRTStar/informed_rrt_star.py
View File

@@ -4,22 +4,22 @@ Informed RRT* path planning
author: Karan Chawla
Atsushi Sakai(@Atsushi_twi)
Reference: Informed RRT*: Optimal Sampling-based Path Planning Focused via
Reference: Informed RRT*: Optimal Sampling-based Path planning Focused via
Direct Sampling of an Admissible Ellipsoidal Heuristichttps://arxiv.org/pdf/1404.2334.pdf
"""
import random
import numpy as np
import math
import copy
import math
import random
import matplotlib.pyplot as plt
import numpy as np
show_animation = True
class InformedRRTStar():
class InformedRRTStar:
def __init__(self, start, goal,
obstacleList, randArea,
@@ -27,16 +27,17 @@ class InformedRRTStar():
self.start = Node(start[0], start[1])
self.goal = Node(goal[0], goal[1])
self.minrand = randArea[0]
self.maxrand = randArea[1]
self.expandDis = expandDis
self.goalSampleRate = goalSampleRate
self.maxIter = maxIter
self.obstacleList = obstacleList
self.min_rand = randArea[0]
self.max_rand = randArea[1]
self.expand_dis = expandDis
self.goal_sample_rate = goalSampleRate
self.max_iter = maxIter
self.obstacle_list = obstacleList
self.node_list = None
def InformedRRTStarSearch(self, animation=True):
def informed_rrt_star_search(self, animation=True):
self.nodeList = [self.start]
self.node_list = [self.start]
# max length we expect to find in our 'informed' sample space, starts as infinite
cBest = float('inf')
pathLen = float('inf')
@@ -55,62 +56,62 @@ class InformedRRTStar():
# first column of idenity matrix transposed
id1_t = np.array([1.0, 0.0, 0.0]).reshape(1, 3)
M = a1 @ id1_t
U, S, Vh = np.linalg.svd(M, 1, 1)
U, S, Vh = np.linalg.svd(M, True, True)
C = np.dot(np.dot(U, np.diag(
[1.0, 1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh)
for i in range(self.maxIter):
for i in range(self.max_iter):
# Sample space is defined by cBest
# cMin is the minimum distance between the start point and the goal
# xCenter is the midpoint between the start and the goal
# cBest changes when a new path is found
rnd = self.informed_sample(cBest, cMin, xCenter, C)
nind = self.getNearestListIndex(self.nodeList, rnd)
nearestNode = self.nodeList[nind]
nind = self.get_nearest_list_index(self.node_list, rnd)
nearestNode = self.node_list[nind]
# steer
theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
newNode = self.getNewNode(theta, nind, nearestNode)
d = self.lineCost(nearestNode, newNode)
newNode = self.get_new_node(theta, nind, nearestNode)
d = self.line_cost(nearestNode, newNode)
isCollision = self.__CollisionCheck(newNode, self.obstacleList)
isCollision = self.collision_check(newNode, self.obstacle_list)
isCollisionEx = self.check_collision_extend(nearestNode, theta, d)
if isCollision and isCollisionEx:
nearInds = self.findNearNodes(newNode)
newNode = self.chooseParent(newNode, nearInds)
nearInds = self.find_near_nodes(newNode)
newNode = self.choose_parent(newNode, nearInds)
self.nodeList.append(newNode)
self.node_list.append(newNode)
self.rewire(newNode, nearInds)
if self.isNearGoal(newNode):
if self.is_near_goal(newNode):
solutionSet.add(newNode)
lastIndex = len(self.nodeList) - 1
tempPath = self.getFinalCourse(lastIndex)
tempPathLen = self.getPathLen(tempPath)
lastIndex = len(self.node_list) - 1
tempPath = self.get_final_course(lastIndex)
tempPathLen = self.get_path_len(tempPath)
if tempPathLen < pathLen:
path = tempPath
cBest = tempPathLen
if animation:
self.drawGraph(xCenter=xCenter,
cBest=cBest, cMin=cMin,
etheta=etheta, rnd=rnd)
self.draw_graph(xCenter=xCenter,
cBest=cBest, cMin=cMin,
etheta=etheta, rnd=rnd)
return path
def chooseParent(self, newNode, nearInds):
def choose_parent(self, newNode, nearInds):
if len(nearInds) == 0:
return newNode
dList = []
for i in nearInds:
dx = newNode.x - self.nodeList[i].x
dy = newNode.y - self.nodeList[i].y
dx = newNode.x - self.node_list[i].x
dy = newNode.y - self.node_list[i].y
d = math.sqrt(dx ** 2 + dy ** 2)
theta = math.atan2(dy, dx)
if self.check_collision_extend(self.nodeList[i], theta, d):
dList.append(self.nodeList[i].cost + d)
if self.check_collision_extend(self.node_list[i], theta, d):
dList.append(self.node_list[i].cost + d)
else:
dList.append(float('inf'))
@@ -126,29 +127,30 @@ class InformedRRTStar():
return newNode
def findNearNodes(self, newNode):
nnode = len(self.nodeList)
def find_near_nodes(self, newNode):
nnode = len(self.node_list)
r = 50.0 * math.sqrt((math.log(nnode) / nnode))
dlist = [(node.x - newNode.x) ** 2
+ (node.y - newNode.y) ** 2 for node in self.nodeList]
+ (node.y - newNode.y) ** 2 for node in self.node_list]
nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
return nearinds
def informed_sample(self, cMax, cMin, xCenter, C):
if cMax < float('inf'):
r = [cMax / 2.0,
math.sqrt(cMax**2 - cMin**2) / 2.0,
math.sqrt(cMax**2 - cMin**2) / 2.0]
math.sqrt(cMax ** 2 - cMin ** 2) / 2.0,
math.sqrt(cMax ** 2 - cMin ** 2) / 2.0]
L = np.diag(r)
xBall = self.sampleUnitBall()
xBall = self.sample_unit_ball()
rnd = np.dot(np.dot(C, L), xBall) + xCenter
rnd = [rnd[(0, 0)], rnd[(1, 0)]]
else:
rnd = self.sampleFreeSpace()
rnd = self.sample_free_space()
return rnd
def sampleUnitBall(self):
@staticmethod
def sample_unit_ball():
a = random.random()
b = random.random()
@@ -159,16 +161,17 @@ class InformedRRTStar():
b * math.sin(2 * math.pi * a / b))
return np.array([[sample[0]], [sample[1]], [0]])
def sampleFreeSpace(self):
if random.randint(0, 100) > self.goalSampleRate:
rnd = [random.uniform(self.minrand, self.maxrand),
random.uniform(self.minrand, self.maxrand)]
def sample_free_space(self):
if random.randint(0, 100) > self.goal_sample_rate:
rnd = [random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand)]
else:
rnd = [self.goal.x, self.goal.y]
return rnd
def getPathLen(self, path):
@staticmethod
def get_path_len(path):
pathLen = 0
for i in range(1, len(path)):
node1_x = path[i][0]
@@ -176,52 +179,55 @@ class InformedRRTStar():
node2_x = path[i - 1][0]
node2_y = path[i - 1][1]
pathLen += math.sqrt((node1_x - node2_x)
** 2 + (node1_y - node2_y)**2)
** 2 + (node1_y - node2_y) ** 2)
return pathLen
def lineCost(self, node1, node2):
return math.sqrt((node1.x - node2.x)**2 + (node1.y - node2.y)**2)
@staticmethod
def line_cost(node1, node2):
return math.sqrt((node1.x - node2.x) ** 2 + (node1.y - node2.y) ** 2)
def getNearestListIndex(self, nodes, rnd):
dList = [(node.x - rnd[0])**2
+ (node.y - rnd[1])**2 for node in nodes]
@staticmethod
def get_nearest_list_index(nodes, rnd):
dList = [(node.x - rnd[0]) ** 2
+ (node.y - rnd[1]) ** 2 for node in nodes]
minIndex = dList.index(min(dList))
return minIndex
def __CollisionCheck(self, newNode, obstacleList):
@staticmethod
def collision_check(newNode, obstacleList):
for (ox, oy, size) in obstacleList:
dx = ox - newNode.x
dy = oy - newNode.y
d = dx * dx + dy * dy
if d <= 1.1 * size**2:
if d <= 1.1 * size ** 2:
return False # collision
return True # safe
def getNewNode(self, theta, nind, nearestNode):
def get_new_node(self, theta, nind, nearestNode):
newNode = copy.deepcopy(nearestNode)
newNode.x += self.expandDis * math.cos(theta)
newNode.y += self.expandDis * math.sin(theta)
newNode.x += self.expand_dis * math.cos(theta)
newNode.y += self.expand_dis * math.sin(theta)
newNode.cost += self.expandDis
newNode.cost += self.expand_dis
newNode.parent = nind
return newNode
def isNearGoal(self, node):
d = self.lineCost(node, self.goal)
if d < self.expandDis:
def is_near_goal(self, node):
d = self.line_cost(node, self.goal)
if d < self.expand_dis:
return True
return False
def rewire(self, newNode, nearInds):
nnode = len(self.nodeList)
n_node = len(self.node_list)
for i in nearInds:
nearNode = self.nodeList[i]
nearNode = self.node_list[i]
d = math.sqrt((nearNode.x - newNode.x)**2
+ (nearNode.y - newNode.y)**2)
d = math.sqrt((nearNode.x - newNode.x) ** 2
+ (nearNode.y - newNode.y) ** 2)
scost = newNode.cost + d
@@ -229,30 +235,30 @@ class InformedRRTStar():
theta = math.atan2(newNode.y - nearNode.y,
newNode.x - nearNode.x)
if self.check_collision_extend(nearNode, theta, d):
nearNode.parent = nnode - 1
nearNode.parent = n_node - 1
nearNode.cost = scost
def check_collision_extend(self, nearNode, theta, d):
tmpNode = copy.deepcopy(nearNode)
for i in range(int(d / self.expandDis)):
tmpNode.x += self.expandDis * math.cos(theta)
tmpNode.y += self.expandDis * math.sin(theta)
if not self.__CollisionCheck(tmpNode, self.obstacleList):
for i in range(int(d / self.expand_dis)):
tmpNode.x += self.expand_dis * math.cos(theta)
tmpNode.y += self.expand_dis * math.sin(theta)
if not self.collision_check(tmpNode, self.obstacle_list):
return False
return True
def getFinalCourse(self, lastIndex):
def get_final_course(self, lastIndex):
path = [[self.goal.x, self.goal.y]]
while self.nodeList[lastIndex].parent is not None:
node = self.nodeList[lastIndex]
while self.node_list[lastIndex].parent is not None:
node = self.node_list[lastIndex]
path.append([node.x, node.y])
lastIndex = node.parent
path.append([self.start.x, self.start.y])
return path
def drawGraph(self, xCenter=None, cBest=None, cMin=None, etheta=None, rnd=None):
def draw_graph(self, xCenter=None, cBest=None, cMin=None, etheta=None, rnd=None):
plt.clf()
if rnd is not None:
@@ -260,13 +266,13 @@ class InformedRRTStar():
if cBest != float('inf'):
self.plot_ellipse(xCenter, cBest, cMin, etheta)
for node in self.nodeList:
for node in self.node_list:
if node.parent is not None:
if node.x or node.y is not None:
plt.plot([node.x, self.nodeList[node.parent].x], [
node.y, self.nodeList[node.parent].y], "-g")
plt.plot([node.x, self.node_list[node.parent].x], [
node.y, self.node_list[node.parent].y], "-g")
for (ox, oy, size) in self.obstacleList:
for (ox, oy, size) in self.obstacle_list:
plt.plot(ox, oy, "ok", ms=30 * size)
plt.plot(self.start.x, self.start.y, "xr")
@@ -275,9 +281,10 @@ class InformedRRTStar():
plt.grid(True)
plt.pause(0.01)
def plot_ellipse(self, xCenter, cBest, cMin, etheta): # pragma: no cover
@staticmethod
def plot_ellipse(xCenter, cBest, cMin, etheta): # pragma: no cover
a = math.sqrt(cBest**2 - cMin**2) / 2.0
a = math.sqrt(cBest ** 2 - cMin ** 2) / 2.0
b = cBest / 2.0
angle = math.pi / 2.0 - etheta
cx = xCenter[0]
@@ -295,7 +302,7 @@ class InformedRRTStar():
plt.plot(px, py, "--c")
class Node():
class Node:
def __init__(self, x, y):
self.x = x
@@ -320,12 +327,12 @@ def main():
# Set params
rrt = InformedRRTStar(start=[0, 0], goal=[5, 10],
randArea=[-2, 15], obstacleList=obstacleList)
path = rrt.InformedRRTStarSearch(animation=show_animation)
path = rrt.informed_rrt_star_search(animation=show_animation)
print("Done!!")
# Plot path
if show_animation:
rrt.drawGraph()
rrt.draw_graph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.grid(True)
plt.pause(0.01)
@@ -333,4 +340,4 @@ def main():
if __name__ == '__main__':
main()
main()

150
PathPlanning/LQRPlanner/LQRplanner.py
View File

@@ -6,113 +6,113 @@ author: Atsushi Sakai (@Atsushi_twi)
"""
import matplotlib.pyplot as plt
import numpy as np
import scipy.linalg as la
import math
import random
show_animation = True
import matplotlib.pyplot as plt
import numpy as np
import scipy.linalg as la
MAX_TIME = 100.0 # Maximum simulation time
DT = 0.1 # Time tick
SHOW_ANIMATION = True
def LQRplanning(sx, sy, gx, gy):
class LQRPlanner:
rx, ry = [sx], [sy]
def __init__(self):
self.MAX_TIME = 100.0 # Maximum simulation time
self.DT = 0.1 # Time tick
self.GOAL_DIST = 0.1
self.MAX_ITER = 150
self.EPS = 0.01
x = np.array([sx - gx, sy - gy]).reshape(2, 1) # State vector
def lqr_planning(self, sx, sy, gx, gy, show_animation=SHOW_ANIMATION):
# Linear system model
A, B = get_system_model()
rx, ry = [sx], [sy]
found_path = False
x = np.array([sx - gx, sy - gy]).reshape(2, 1) # State vector
time = 0.0
while time <= MAX_TIME:
time += DT
# Linear system model
A, B = self.get_system_model()
u = LQR_control(A, B, x)
found_path = False
x = A @ x + B @ u
time = 0.0
while time <= self.MAX_TIME:
time += self.DT
rx.append(x[0, 0] + gx)
ry.append(x[1, 0] + gy)
u = self.lqr_control(A, B, x)
d = math.sqrt((gx - rx[-1])**2 + (gy - ry[-1])**2)
if d <= 0.1:
# print("Goal!!")
found_path = True
break
x = A @ x + B @ u
# animation
if show_animation: # pragma: no cover
plt.plot(sx, sy, "or")
plt.plot(gx, gy, "ob")
plt.plot(rx, ry, "-r")
plt.axis("equal")
plt.pause(1.0)
rx.append(x[0, 0] + gx)
ry.append(x[1, 0] + gy)
if not found_path:
print("Cannot found path")
return [], []
d = math.sqrt((gx - rx[-1]) ** 2 + (gy - ry[-1]) ** 2)
if d <= self.GOAL_DIST:
found_path = True
break
return rx, ry
# animation
if show_animation: # pragma: no cover
plt.plot(sx, sy, "or")
plt.plot(gx, gy, "ob")
plt.plot(rx, ry, "-r")
plt.axis("equal")
plt.pause(1.0)
if not found_path:
print("Cannot found path")
return [], []
def solve_DARE(A, B, Q, R):
"""
solve a discrete time_Algebraic Riccati equation (DARE)
"""
X = Q
maxiter = 150
eps = 0.01
return rx, ry
for i in range(maxiter):
Xn = A.T * X * A - A.T * X * B * \
la.inv(R + B.T * X * B) * B.T * X * A + Q
if (abs(Xn - X)).max() < eps:
break
X = Xn
def solve_dare(self, A, B, Q, R):
"""
solve a discrete time_Algebraic Riccati equation (DARE)
"""
X, Xn = Q, Q
return Xn
for i in range(self.MAX_ITER):
Xn = A.T * X * A - A.T * X * B * \
la.inv(R + B.T * X * B) * B.T * X * A + Q
if (abs(Xn - X)).max() < self.EPS:
break
X = Xn
return Xn
def dlqr(A, B, Q, R):
"""Solve the discrete time lqr controller.
x[k+1] = A x[k] + B u[k]
cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]
# ref Bertsekas, p.151
"""
def dlqr(self, A, B, Q, R):
"""Solve the discrete time lqr controller.
x[k+1] = A x[k] + B u[k]
cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k]
# ref Bertsekas, p.151
"""
# first, try to solve the ricatti equation
X = solve_DARE(A, B, Q, R)
# first, try to solve the ricatti equation
X = self.solve_dare(A, B, Q, R)
# compute the LQR gain
K = la.inv(B.T @ X @ B + R) @ (B.T @ X @ A)
# compute the LQR gain
K = la.inv(B.T @ X @ B + R) @ (B.T @ X @ A)
eigVals, eigVecs = la.eig(A - B @ K)
eigValues = la.eigvals(A - B @ K)
return K, X, eigVals
return K, X, eigValues
def get_system_model(self):
def get_system_model():
A = np.array([[self.DT, 1.0],
[0.0, self.DT]])
B = np.array([0.0, 1.0]).reshape(2, 1)
A = np.array([[DT, 1.0],
[0.0, DT]])
B = np.array([0.0, 1.0]).reshape(2, 1)
return A, B
return A, B
def lqr_control(self, A, B, x):
Kopt, X, ev = self.dlqr(A, B, np.eye(2), np.eye(1))
def LQR_control(A, B, x):
u = -Kopt @ x
Kopt, X, ev = dlqr(A, B, np.eye(2), np.eye(1))
u = -Kopt @ x
return u
return u
def main():
@@ -121,15 +121,17 @@ def main():
ntest = 10 # number of goal
area = 100.0 # sampling area
lqr_planner = LQRPlanner()
for i in range(ntest):
sx = 6.0
sy = 6.0
gx = random.uniform(-area, area)
gy = random.uniform(-area, area)
rx, ry = LQRplanning(sx, sy, gx, gy)
rx, ry = lqr_planner.lqr_planning(sx, sy, gx, gy)
if show_animation: # pragma: no cover
if SHOW_ANIMATION: # pragma: no cover
plt.plot(sx, sy, "or")
plt.plot(gx, gy, "ob")
plt.plot(rx, ry, "-r")

357
PathPlanning/LQRRRTStar/lqr_rrt_star.py
View File

@@ -5,117 +5,166 @@ Path planning code with LQR RRT*
author: AtsushiSakai(@Atsushi_twi)
"""
import copy
import math
import os
import random
import sys
import matplotlib.pyplot as plt
import numpy as np
import copy
import math
import random
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../LQRPlanner/")
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../RRTStar/")
try:
import LQRplanner
except:
from LQRplanner import LQRPlanner
from rrt_star import RRTStar
except ImportError:
raise
show_animation = True
LQRplanner.show_animation = False
STEP_SIZE = 0.05 # step size of local path
XYTH = 0.5 # [m] acceptance xy distance in final paths
class RRT():
class LQRRRTStar(RRTStar):
"""
Class for RRT Planning
Class for RRT star planning with LQR planning
"""
def __init__(self, start, goal, obstacleList, randArea,
goalSampleRate=10, maxIter=200):
def __init__(self, start, goal, obstacle_list, rand_area,
goal_sample_rate=10,
max_iter=200,
connect_circle_dist=50.0,
step_size=0.2
):
"""
Setting Parameter
start:Start Position [x,y]
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:Ramdom Samping Area [min,max]
randArea:Random Sampling Area [min,max]
"""
self.start = Node(start[0], start[1])
self.end = Node(goal[0], goal[1])
self.minrand = randArea[0]
self.maxrand = randArea[1]
self.goalSampleRate = goalSampleRate
self.maxIter = maxIter
self.obstacleList = obstacleList
self.start = self.Node(start[0], start[1])
self.end = self.Node(goal[0], goal[1])
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.goal_sample_rate = goal_sample_rate
self.max_iter = max_iter
self.obstacle_list = obstacle_list
self.connect_circle_dist = connect_circle_dist
def planning(self, animation=True):
self.curvature = 1.0
self.goal_xy_th = 0.5
self.step_size = step_size
self.lqr_planner = LQRPlanner()
def planning(self, animation=True, search_until_max_iter=True):
"""
Pathplanning
RRT Star planning
animation: flag for animation on or off
"""
self.nodeList = [self.start]
for i in range(self.maxIter):
rnd = self.get_random_point()
nind = self.get_nearest_index(self.nodeList, rnd)
self.node_list = [self.start]
for i in range(self.max_iter):
print("Iter:", i, ", number of nodes:", len(self.node_list))
rnd = self.get_random_node()
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
new_node = self.steer(self.node_list[nearest_ind], rnd)
newNode = self.steer(rnd, nind)
if newNode is None:
continue
if self.check_collision(newNode, self.obstacleList):
nearinds = self.find_near_nodes(newNode)
newNode = self.choose_parent(newNode, nearinds)
if newNode is None:
continue
self.nodeList.append(newNode)
self.rewire(newNode, nearinds)
if self.check_collision(new_node, self.obstacle_list):
near_indexes = self.find_near_nodes(new_node)
new_node = self.choose_parent(new_node, near_indexes)
if new_node:
self.node_list.append(new_node)
self.rewire(new_node, near_indexes)
if animation and i % 5 == 0:
self.draw_graph(rnd=rnd)
self.draw_graph(rnd)
# generate coruse
lastIndex = self.get_best_last_index()
if lastIndex is None:
if (not search_until_max_iter) and new_node: # check reaching the goal
last_index = self.search_best_goal_node()
if last_index:
return self.generate_final_course(last_index)
print("reached max iteration")
last_index = self.search_best_goal_node()
if last_index:
return self.generate_final_course(last_index)
else:
print("Cannot find path")
return None
def draw_graph(self, rnd=None):
plt.clf()
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.node_list:
if node.parent:
plt.plot(node.path_x, node.path_y, "-g")
for (ox, oy, size) in self.obstacle_list:
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")
plt.axis([-2, 15, -2, 15])
plt.grid(True)
plt.pause(0.01)
def search_best_goal_node(self):
dist_to_goal_list = [self.calc_dist_to_goal(n.x, n.y) for n in self.node_list]
goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.goal_xy_th]
if not goal_inds:
return None
path = self.gen_final_course(lastIndex)
min_cost = min([self.node_list[i].cost for i in goal_inds])
for i in goal_inds:
if self.node_list[i].cost == min_cost:
return i
return None
def calc_new_cost(self, from_node, to_node):
wx, wy = self.lqr_planner.lqr_planning(
from_node.x, from_node.y, to_node.x, to_node.y, show_animation=False)
px, py, course_lengths = self.sample_path(wx, wy, self.step_size)
if not course_lengths:
return float("inf")
return from_node.cost + sum(course_lengths)
def get_random_node(self):
if random.randint(0, 100) > self.goal_sample_rate:
rnd = self.Node(random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand)
)
else: # goal point sampling
rnd = self.Node(self.end.x, self.end.y)
return rnd
def generate_final_course(self, goal_index):
print("final")
path = [[self.end.x, self.end.y]]
node = self.node_list[goal_index]
while node.parent:
for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)):
path.append([ix, iy])
node = node.parent
path.append([self.start.x, self.start.y])
return path
def choose_parent(self, newNode, nearinds):
if not nearinds:
return newNode
dlist = []
for i in nearinds:
tNode = self.steer(newNode, i)
if tNode is None:
continue
if self.check_collision(tNode, self.obstacleList):
dlist.append(tNode.cost)
else:
dlist.append(float("inf"))
mincost = min(dlist)
minind = nearinds[dlist.index(mincost)]
if mincost == float("inf"):
print("mincost is inf")
return newNode
newNode = self.steer(newNode, minind)
return newNode
def pi_2_pi(self, angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
def sample_path(self, wx, wy, step):
px, py, clen = [], [], []
@@ -129,160 +178,30 @@ class RRT():
dx = np.diff(px)
dy = np.diff(py)
clen = [math.sqrt(idx**2 + idy**2) for (idx, idy) in zip(dx, dy)]
clen = [math.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)]
return px, py, clen
def steer(self, rnd, nind):
def steer(self, from_node, to_node):
nearestNode = self.nodeList[nind]
wx, wy = self.lqr_planner.lqr_planning(
from_node.x, from_node.y, to_node.x, to_node.y, show_animation=False)
wx, wy = LQRplanner.LQRplanning(
nearestNode.x, nearestNode.y, rnd.x, rnd.y)
px, py, clen = self.sample_path(wx, wy, STEP_SIZE)
px, py, course_lens = self.sample_path(wx, wy, self.step_size)
if px is None:
return None
newNode = copy.deepcopy(nearestNode)
newNode = copy.deepcopy(from_node)
newNode.x = px[-1]
newNode.y = py[-1]
newNode.path_x = px
newNode.path_y = py
newNode.cost += sum([abs(c) for c in clen])
newNode.parent = nind
newNode.cost += sum([abs(c) for c in course_lens])
newNode.parent = from_node
return newNode
def get_random_point(self):
if random.randint(0, 100) > self.goalSampleRate:
rnd = [random.uniform(self.minrand, self.maxrand),
random.uniform(self.minrand, self.maxrand),
random.uniform(-math.pi, math.pi)
]
else: # goal point sampling
rnd = [self.end.x, self.end.y]
node = Node(rnd[0], rnd[1])
return node
def get_best_last_index(self):
# print("get_best_last_index")
goalinds = []
for (i, node) in enumerate(self.nodeList):
if self.calc_dist_to_goal(node.x, node.y) <= XYTH:
goalinds.append(i)
if not goalinds:
return None
mincost = min([self.nodeList[i].cost for i in goalinds])
for i in goalinds:
if self.nodeList[i].cost == mincost:
return i
return None
def gen_final_course(self, goalind):
path = [[self.end.x, self.end.y]]
while self.nodeList[goalind].parent is not None:
node = self.nodeList[goalind]
for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)):
path.append([ix, iy])
goalind = node.parent
path.append([self.start.x, self.start.y])
return path
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))
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]
tNode = self.steer(nearNode, nnode - 1)
if tNode is None:
continue
obstacleOK = self.check_collision(tNode, self.obstacleList)
imporveCost = nearNode.cost > tNode.cost
if obstacleOK and imporveCost:
# print("rewire")
self.nodeList[i] = tNode
def draw_graph(self, rnd=None):
plt.clf()
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.nodeList:
if node.parent is not None:
plt.plot(node.path_x, node.path_y, "-g")
for (ox, oy, size) in self.obstacleList:
plt.plot(ox, oy, "ok", ms=30 * size)
plt.plot(self.start.x, self.start.y, "or")
plt.plot(self.end.x, self.end.y, "or")
plt.axis([-2, 15, -2, 15])
plt.grid(True)
plt.pause(0.01)
def get_nearest_index(self, nodeList, rnd):
dlist = [(node.x - rnd.x) ** 2
+ (node.y - rnd.y) ** 2
for node in nodeList]
minind = dlist.index(min(dlist))
return minind
def check_collision(self, node, obstacleList):
px = np.array(node.path_x)
py = np.array(node.path_y)
for (ox, oy, size) in obstacleList:
dx = ox - px
dy = oy - py
d = dx ** 2 + dy ** 2
dmin = min(d)
if dmin <= size ** 2:
return False # collision
return True # safe
class Node():
"""
RRT Node
"""
def __init__(self, x, y):
self.x = x
self.y = y
self.path_x = []
self.path_y = []
self.cost = 0.0
self.parent = None
def main(maxIter=200):
print("Start " + __file__)
@@ -301,14 +220,14 @@ def main(maxIter=200):
start = [0.0, 0.0]
goal = [6.0, 7.0]
rrt = RRT(start, goal, randArea=[-2.0, 15.0],
obstacleList=obstacleList,
maxIter=maxIter)
path = rrt.planning(animation=show_animation)
lqr_rrt_star = LQRRRTStar(start, goal,
obstacleList,
[-2.0, 15.0])
path = lqr_rrt_star.planning(animation=show_animation)
# Draw final path
if show_animation: # pragma: no cover
rrt.draw_graph()
lqr_rrt_star.draw_graph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.grid(True)
plt.pause(0.001)

11
PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py
View File

@@ -6,9 +6,11 @@ author: Atsushi Sakai(@Atsushi_twi)
"""
import numpy as np
import matplotlib.pyplot as plt
import math
import matplotlib.pyplot as plt
import numpy as np
import motion_model
# optimization parameter
@@ -37,7 +39,7 @@ def calc_diff(target, x, y, yaw):
return d
def calc_J(target, p, h, k0):
def calc_j(target, p, h, k0):
xp, yp, yawp = motion_model.generate_last_state(
p[0, 0] + h[0], p[1, 0], p[2, 0], k0)
dp = calc_diff(target, [xp], [yp], [yawp])
@@ -68,7 +70,6 @@ def calc_J(target, p, h, k0):
def selection_learning_param(dp, p, k0, target):
mincost = float("inf")
mina = 1.0
maxa = 2.0
@@ -110,7 +111,7 @@ def optimize_trajectory(target, k0, p):
print("path is ok cost is:" + str(cost))
break
J = calc_J(target, p, h, k0)
J = calc_j(target, p, h, k0)
try:
dp = - np.linalg.inv(J) @ dc
except np.linalg.linalg.LinAlgError:

25
PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py
View File

@@ -1,19 +1,20 @@
"""
Quinitc Polynomials Planner
Quintic Polynomials Planner
author: Atsushi Sakai (@Atsushi_twi)
Ref:
- [Local Path Planning And Motion Control For Agv In Positioning](http://ieeexplore.ieee.org/document/637936/)
- [Local Path planning And Motion Control For Agv In Positioning](http://ieeexplore.ieee.org/document/637936/)
"""
import numpy as np
import matplotlib.pyplot as plt
import math
import matplotlib.pyplot as plt
import numpy as np
# parameter
MAX_T = 100.0 # maximum time to the goal [s]
MIN_T = 5.0 # minimum time to the goal[s]
@@ -21,7 +22,7 @@ MIN_T = 5.0 # minimum time to the goal[s]
show_animation = True
class quinic_polynomial:
class QuinticPolynomial:
def __init__(self, xs, vxs, axs, xe, vxe, axe, T):
@@ -72,9 +73,9 @@ class quinic_polynomial:
return xt
def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt):
def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt):
"""
quinic polynomial planner
quintic polynomial planner
input
sx: start x position [m]
@@ -109,9 +110,11 @@ def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_a
axg = ga * math.cos(gyaw)
ayg = ga * math.sin(gyaw)
time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []
for T in np.arange(MIN_T, MAX_T, MIN_T):
xqp = quinic_polynomial(sx, vxs, axs, gx, vxg, axg, T)
yqp = quinic_polynomial(sy, vys, ays, gy, vyg, ayg, T)
xqp = QuinticPolynomial(sx, vxs, axs, gx, vxg, axg, T)
yqp = QuinticPolynomial(sy, vys, ays, gy, vyg, ayg, T)
time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []
@@ -146,7 +149,7 @@ def quinic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_a
break
if show_animation: # pragma: no cover
for i, _ in enumerate(rx):
for i, _ in enumerate(time):
plt.cla()
plt.grid(True)
plt.axis("equal")
@@ -194,7 +197,7 @@ def main():
max_jerk = 0.5 # max jerk [m/sss]
dt = 0.1 # time tick [s]
time, x, y, yaw, v, a, j = quinic_polynomials_planner(
time, x, y, yaw, v, a, j = quintic_polynomials_planner(
sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt)
if show_animation: # pragma: no cover

93
PathPlanning/RRT/rrt.py
View File

@@ -19,7 +19,7 @@ class RRT:
Class for RRT planning
"""
class Node():
class Node:
"""
RRT Node
"""
@@ -27,17 +27,19 @@ class RRT:
def __init__(self, x, y):
self.x = x
self.y = y
self.path_x = []
self.path_y = []
self.parent = None
def __init__(self, start, goal, obstacle_list,
rand_area, expand_dis=1.0, goal_sample_rate=5, max_iter=500):
def __init__(self, start, goal, obstacle_list, rand_area,
expand_dis=3.0, path_resolution=1.0, goal_sample_rate=5, max_iter=500):
"""
Setting Parameter
start:Start Position [x,y]
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:Ramdom Samping Area [min,max]
randArea:Random Sampling Area [min,max]
"""
self.start = self.Node(start[0], start[1])
@@ -45,9 +47,10 @@ class RRT:
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.expand_dis = expand_dis
self.path_resolution = path_resolution
self.goal_sample_rate = goal_sample_rate
self.max_iter = max_iter
self.obstacleList = obstacle_list
self.obstacle_list = obstacle_list
self.node_list = []
def planning(self, animation=True):
@@ -59,35 +62,54 @@ class RRT:
self.node_list = [self.start]
for i in range(self.max_iter):
rnd = self.get_random_point()
nearest_ind = self.get_nearest_list_index(self.node_list, rnd)
rnd_node = self.get_random_node()
nearest_ind = self.get_nearest_node_index(self.node_list, rnd_node)
nearest_node = self.node_list[nearest_ind]
new_node = self.steer(rnd, nearest_node)
new_node.parent = nearest_node
new_node = self.steer(nearest_node, rnd_node, self.expand_dis)
if not self.check_collision(new_node, self.obstacleList):
continue
if self.check_collision(new_node, self.obstacle_list):
self.node_list.append(new_node)
self.node_list.append(new_node)
print("nNodelist:", len(self.node_list))
if animation and i % 5 == 0:
self.draw_graph(rnd_node)
# check goal
if self.calc_dist_to_goal(new_node.x, new_node.y) <= self.expand_dis:
print("Goal!!")
return self.generate_final_course(len(self.node_list) - 1)
if animation and i % 5:
self.draw_graph(rnd)
self.draw_graph(rnd_node)
return None # cannot find path
def steer(self, rnd, nearest_node):
new_node = self.Node(rnd[0], rnd[1])
d, theta = self.calc_distance_and_angle(nearest_node, new_node)
if d > self.expand_dis:
new_node.x = nearest_node.x + self.expand_dis * math.cos(theta)
new_node.y = nearest_node.y + self.expand_dis * math.sin(theta)
def steer(self, from_node, to_node, extend_length=float("inf")):
new_node = self.Node(from_node.x, from_node.y)
d, theta = self.calc_distance_and_angle(new_node, to_node)
new_node.path_x = [new_node.x]
new_node.path_y = [new_node.y]
if extend_length > d:
extend_length = d
n_expand = math.floor(extend_length / self.path_resolution)
for _ in range(n_expand):
new_node.x += self.path_resolution * math.cos(theta)
new_node.y += self.path_resolution * math.sin(theta)
new_node.path_x.append(new_node.x)
new_node.path_y.append(new_node.y)
d, _ = self.calc_distance_and_angle(new_node, to_node)
if d <= self.path_resolution:
new_node.x = to_node.x
new_node.y = to_node.y
new_node.path_x[-1] = to_node.x
new_node.path_y[-1] = to_node.y
new_node.parent = from_node
return new_node
@@ -106,25 +128,23 @@ class RRT:
dy = y - self.end.y
return math.sqrt(dx ** 2 + dy ** 2)
def get_random_point(self):
def get_random_node(self):
if random.randint(0, 100) > self.goal_sample_rate:
rnd = [random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand)]
rnd = self.Node(random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand))
else: # goal point sampling
rnd = [self.end.x, self.end.y]
rnd = self.Node(self.end.x, self.end.y)
return rnd
def draw_graph(self, rnd=None):
plt.clf()
if rnd is not None:
plt.plot(rnd[0], rnd[1], "^k")
plt.plot(rnd.x, rnd.y, "^k")
for node in self.node_list:
if node.parent:
plt.plot([node.x, node.parent.x],
[node.y, node.parent.y],
"-g")
plt.plot(node.path_x, node.path_y, "-g")
for (ox, oy, size) in self.obstacleList:
for (ox, oy, size) in self.obstacle_list:
plt.plot(ox, oy, "ok", ms=30 * size)
plt.plot(self.start.x, self.start.y, "xr")
@@ -134,8 +154,8 @@ class RRT:
plt.pause(0.01)
@staticmethod
def get_nearest_list_index(node_list, rnd):
dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1])
def get_nearest_node_index(node_list, rnd_node):
dlist = [(node.x - rnd_node.x) ** 2 + (node.y - rnd_node.y)
** 2 for node in node_list]
minind = dlist.index(min(dlist))
@@ -144,10 +164,11 @@ class RRT:
@staticmethod
def check_collision(node, obstacleList):
for (ox, oy, size) in obstacleList:
dx = ox - node.x
dy = oy - node.y
d = dx * dx + dy * dy
if d <= size ** 2:
dx_list = [ox - x for x in node.path_x]
dy_list = [oy - y for y in node.path_y]
d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)]
if min(d_list) <= size ** 2:
return False # collision
return True # safe

280
PathPlanning/RRTDubins/rrt_dubins.py
View File

@@ -1,203 +1,205 @@
"""
Path Planning Sample Code with RRT for car like robot.
Path planning Sample Code with RRT with Dubins path
author: AtsushiSakai(@Atsushi_twi)
"""
import matplotlib.pyplot as plt
import numpy as np
import copy
import math
import os
import random
import sys
import os
import matplotlib.pyplot as plt
import numpy as np
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../DubinsPath/")
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../RRT/")
try:
from rrt import RRT
import dubins_path_planning
except:
except ImportError:
raise
show_animation = True
class RRT():
class RRTDubins(RRT):
"""
Class for RRT Planning
Class for RRT planning with Dubins path
"""
def __init__(self, start, goal, obstacleList, randArea,
goalSampleRate=10, maxIter=1000):
class Node(RRT.Node):
"""
RRT Node
"""
def __init__(self, x, y, yaw):
super().__init__(x, y)
self.cost = 0
self.yaw = yaw
self.path_yaw = []
def __init__(self, start, goal, obstacle_list, rand_area,
goal_sample_rate=10,
max_iter=200,
):
"""
Setting Parameter
start:Start Position [x,y]
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:Ramdom Samping Area [min,max]
randArea:Random Sampling Area [min,max]
"""
self.start = Node(start[0], start[1], start[2])
self.end = Node(goal[0], goal[1], goal[2])
self.minrand = randArea[0]
self.maxrand = randArea[1]
self.goalSampleRate = goalSampleRate
self.maxIter = maxIter
self.obstacleList = obstacleList
self.start = self.Node(start[0], start[1], start[2])
self.end = self.Node(goal[0], goal[1], goal[2])
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.goal_sample_rate = goal_sample_rate
self.max_iter = max_iter
self.obstacle_list = obstacle_list
def Planning(self, animation=False):
self.curvature = 1.0 # for dubins path
self.goal_yaw_th = np.deg2rad(1.0)
self.goal_xy_th = 0.5
def planning(self, animation=True, search_until_max_iter=True):
"""
Pathplanning
execute planning
animation: flag for animation on or off
"""
self.nodeList = [self.start]
for i in range(self.maxIter):
rnd = self.get_random_point()
nind = self.GetNearestListIndex(self.nodeList, rnd)
self.node_list = [self.start]
for i in range(self.max_iter):
print("Iter:", i, ", number of nodes:", len(self.node_list))
rnd = self.get_random_node()
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
new_node = self.steer(self.node_list[nearest_ind], rnd)
newNode = self.steer(rnd, nind)
if self.__CollisionCheck(newNode, self.obstacleList):
self.nodeList.append(newNode)
if self.check_collision(new_node, self.obstacle_list):
self.node_list.append(new_node)
if animation and i % 5 == 0:
self.DrawGraph(rnd=rnd)
self.plot_start_goal_arrow()
self.draw_graph(rnd)
# generate coruse
lastIndex = self.get_best_last_index()
# print(lastIndex)
if (not search_until_max_iter) and new_node: # check reaching the goal
last_index = self.search_best_goal_node()
if last_index:
return self.generate_final_course(last_index)
if lastIndex is None:
return None
print("reached max iteration")
path = self.gen_final_course(lastIndex)
return path
def pi_2_pi(self, angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
def steer(self, rnd, nind):
# print(rnd)
curvature = 1.0
nearestNode = self.nodeList[nind]
px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning(
nearestNode.x, nearestNode.y, nearestNode.yaw, rnd[0], rnd[1], rnd[2], curvature)
newNode = copy.deepcopy(nearestNode)
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
return newNode
def get_random_point(self):
if random.randint(0, 100) > self.goalSampleRate:
rnd = [random.uniform(self.minrand, self.maxrand),
random.uniform(self.minrand, self.maxrand),
random.uniform(-math.pi, math.pi)
]
else: # goal point sampling
rnd = [self.end.x, self.end.y, self.end.yaw]
return rnd
def get_best_last_index(self):
# print("get_best_last_index")
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.1]
# print(goalinds)
mincost = min([self.nodeList[i].cost for i in goalinds])
for i in goalinds:
if self.nodeList[i].cost == mincost:
return i
last_index = self.search_best_goal_node()
if last_index:
return self.generate_final_course(last_index)
else:
print("Cannot find path")
return None
def gen_final_course(self, goalind):
path = [[self.end.x, self.end.y]]
while self.nodeList[goalind].parent is not None:
node = self.nodeList[goalind]
for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)):
path.append([ix, iy])
# path.append([node.x, node.y])
goalind = node.parent
path.append([self.start.x, self.start.y])
return path
def calc_dist_to_goal(self, x, y):
return np.linalg.norm([x - self.end.x, y - self.end.y])
def DrawGraph(self, rnd=None): # pragma: no cover
def draw_graph(self, rnd=None): # pragma: no cover
plt.clf()
if rnd is not None:
plt.plot(rnd[0], rnd[1], "^k")
for node in self.nodeList:
if node.parent is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.node_list:
if node.parent:
plt.plot(node.path_x, node.path_y, "-g")
for (ox, oy, size) in self.obstacleList:
for (ox, oy, size) in self.obstacle_list:
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")
plt.axis([-2, 15, -2, 15])
plt.grid(True)
self.plot_start_goal_arrow()
plt.pause(0.01)
def plot_start_goal_arrow(self): # pragma: no cover
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)
def steer(self, from_node, to_node):
def GetNearestListIndex(self, nodeList, rnd):
dlist = [(node.x - rnd[0]) ** 2
+ (node.y - rnd[1]) ** 2
+ (node.yaw - rnd[2] ** 2) for node in nodeList]
minind = dlist.index(min(dlist))
px, py, pyaw, mode, course_length = dubins_path_planning.dubins_path_planning(
from_node.x, from_node.y, from_node.yaw,
to_node.x, to_node.y, to_node.yaw, self.curvature)
return minind
new_node = copy.deepcopy(from_node)
new_node.x = px[-1]
new_node.y = py[-1]
new_node.yaw = pyaw[-1]
def __CollisionCheck(self, node, obstacleList):
new_node.path_x = px
new_node.path_y = py
new_node.path_yaw = pyaw
new_node.cost += course_length
new_node.parent = from_node
for (ox, oy, size) in obstacleList:
for (ix, iy) in zip(node.path_x, node.path_y):
dx = ox - ix
dy = oy - iy
d = dx * dx + dy * dy
if d <= size ** 2:
return False # collision
return new_node
return True # safe
def calc_new_cost(self, from_node, to_node):
_, _, _, _, course_length = dubins_path_planning.dubins_path_planning(
from_node.x, from_node.y, from_node.yaw,
to_node.x, to_node.y, to_node.yaw, self.curvature)
class Node():
"""
RRT Node
"""
return from_node.cost + course_length
def __init__(self, x, y, yaw):
self.x = x
self.y = y
self.yaw = yaw
self.path_x = []
self.path_y = []
self.path_yaw = []
self.cost = 0.0
self.parent = None
def get_random_node(self):
if random.randint(0, 100) > self.goal_sample_rate:
rnd = self.Node(random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand),
random.uniform(-math.pi, math.pi)
)
else: # goal point sampling
rnd = self.Node(self.end.x, self.end.y, self.end.yaw)
return rnd
def search_best_goal_node(self):
goal_indexes = []
for (i, node) in enumerate(self.node_list):
if self.calc_dist_to_goal(node.x, node.y) <= self.goal_xy_th:
goal_indexes.append(i)
# angle check
final_goal_indexes = []
for i in goal_indexes:
if abs(self.node_list[i].yaw - self.end.yaw) <= self.goal_yaw_th:
final_goal_indexes.append(i)
if not final_goal_indexes:
return None
min_cost = min([self.node_list[i].cost for i in final_goal_indexes])
for i in final_goal_indexes:
if self.node_list[i].cost == min_cost:
return i
return None
def generate_final_course(self, goal_index):
print("final")
path = [[self.end.x, self.end.y]]
node = self.node_list[goal_index]
while node.parent:
for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)):
path.append([ix, iy])
node = node.parent
path.append([self.start.x, self.start.y])
return path
def main():
@@ -216,12 +218,12 @@ def main():
start = [0.0, 0.0, np.deg2rad(0.0)]
goal = [10.0, 10.0, np.deg2rad(0.0)]
rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList)
path = rrt.Planning(animation=show_animation)
rrt_dubins = RRTDubins(start, goal, obstacleList, [-2.0, 15.0])
path = rrt_dubins.planning(animation=show_animation)
# Draw final path
if show_animation: # pragma: no cover
rrt.DrawGraph()
rrt_dubins.draw_graph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.grid(True)
plt.pause(0.001)

93
PathPlanning/RRTStar/rrt_star.py
View File

@@ -6,7 +6,6 @@ author: Atsushi Sakai(@Atsushi_twi)
"""
import copy
import math
import os
import sys
@@ -29,21 +28,20 @@ class RRTStar(RRT):
Class for RRT Star planning
"""
class Node:
class Node(RRT.Node):
def __init__(self, x, y):
self.x = x
self.y = y
super().__init__(x, y)
self.cost = 0.0
self.parent = None
def __init__(self, start, goal, obstacle_list, rand_area,
expand_dis=0.5,
expand_dis=3.0,
path_resolution=1.0,
goal_sample_rate=20,
max_iter=500,
max_iter=300,
connect_circle_dist=50.0
):
super().__init__(start, goal, obstacle_list,
rand_area, expand_dis, goal_sample_rate, max_iter)
rand_area, expand_dis, path_resolution, goal_sample_rate, max_iter)
"""
Setting Parameter
@@ -55,7 +53,7 @@ class RRTStar(RRT):
"""
self.connect_circle_dist = connect_circle_dist
def planning(self, animation=True, search_until_maxiter=True):
def planning(self, animation=True, search_until_max_iter=True):
"""
rrt star path planning
@@ -65,11 +63,12 @@ class RRTStar(RRT):
self.node_list = [self.start]
for i in range(self.max_iter):
rnd = self.get_random_point()
nearest_ind = self.get_nearest_list_index(self.node_list, rnd)
new_node = self.steer(rnd, self.node_list[nearest_ind])
print("Iter:", i, ", number of nodes:", len(self.node_list))
rnd = self.get_random_node()
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
new_node = self.steer(self.node_list[nearest_ind], rnd, self.expand_dis)
if self.check_collision(new_node, self.obstacleList):
if self.check_collision(new_node, self.obstacle_list):
near_inds = self.find_near_nodes(new_node)
new_node = self.choose_parent(new_node, near_inds)
if new_node:
@@ -79,10 +78,10 @@ class RRTStar(RRT):
if animation and i % 5 == 0:
self.draw_graph(rnd)
if not search_until_maxiter and new_node: # check reaching the goal
d, _ = self.calc_distance_and_angle(new_node, self.end)
if d <= self.expand_dis:
return self.generate_final_course(len(self.node_list) - 1)
if (not search_until_max_iter) and new_node: # check reaching the goal
last_index = self.search_best_goal_node()
if last_index:
return self.generate_final_course(last_index)
print("reached max iteration")
@@ -99,9 +98,10 @@ class RRTStar(RRT):
# search nearest cost in near_inds
costs = []
for i in near_inds:
d, theta = self.calc_distance_and_angle(self.node_list[i], new_node)
if self.check_collision_extend(self.node_list[i], theta, d):
costs.append(self.node_list[i].cost + d)
near_node = self.node_list[i]
t_node = self.steer(near_node, new_node)
if t_node and self.check_collision(t_node, self.obstacle_list):
costs.append(self.calc_new_cost(near_node, new_node))
else:
costs.append(float("inf")) # the cost of collision node
min_cost = min(costs)
@@ -110,9 +110,10 @@ class RRTStar(RRT):
print("There is no good path.(min_cost is inf)")
return None
new_node.cost = min_cost
min_ind = near_inds[costs.index(min_cost)]
new_node = self.steer(self.node_list[min_ind], new_node)
new_node.parent = self.node_list[min_ind]
new_node.cost = min_cost
return new_node
@@ -141,34 +142,30 @@ class RRTStar(RRT):
def rewire(self, new_node, near_inds):
for i in near_inds:
near_node = self.node_list[i]
d, theta = self.calc_distance_and_angle(near_node, new_node)
new_cost = new_node.cost + d
edge_node = self.steer(new_node, near_node)
if not edge_node:
continue
edge_node.cost = self.calc_new_cost(new_node, near_node)
if near_node.cost > new_cost:
if self.check_collision_extend(near_node, theta, d):
near_node.parent = new_node
near_node.cost = new_cost
self.propagate_cost_to_leaves(new_node)
no_collision = self.check_collision(edge_node, self.obstacle_list)
improved_cost = near_node.cost > edge_node.cost
if no_collision and improved_cost:
near_node = edge_node
near_node.parent = new_node
self.propagate_cost_to_leaves(new_node)
def calc_new_cost(self, from_node, to_node):
d, _ = self.calc_distance_and_angle(from_node, to_node)
return from_node.cost + d
def propagate_cost_to_leaves(self, parent_node):
for node in self.node_list:
if node.parent == parent_node:
d, _ = self.calc_distance_and_angle(parent_node, node)
node.cost = parent_node.cost + d
node.cost = self.calc_new_cost(parent_node, node)
self.propagate_cost_to_leaves(node)
def check_collision_extend(self, near_node, theta, d):
tmp_node = copy.deepcopy(near_node)
for i in range(int(d / self.expand_dis)):
tmp_node.x += self.expand_dis * math.cos(theta)
tmp_node.y += self.expand_dis * math.sin(theta)
if not self.check_collision(tmp_node, self.obstacleList):
return False
return True
def main():
print("Start " + __file__)
@@ -184,11 +181,11 @@ def main():
] # [x,y,size(radius)]
# Set Initial parameters
rrt = RRTStar(start=[0, 0],
goal=[10, 10],
rand_area=[-2, 15],
obstacle_list=obstacle_list)
path = rrt.planning(animation=show_animation, search_until_maxiter=False)
rrt_star = RRTStar(start=[0, 0],
goal=[10, 10],
rand_area=[-2, 15],
obstacle_list=obstacle_list)
path = rrt_star.planning(animation=show_animation)
if path is None:
print("Cannot find path")
@@ -197,7 +194,7 @@ def main():
# Draw final path
if show_animation:
rrt.draw_graph()
rrt_star.draw_graph()
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

303
PathPlanning/RRTStarDubins/dubins_path_planning.py
View File

@@ -1,303 +0,0 @@
"""
Dubins path planner sample code
author Atsushi Sakai (@Atsushi_twi)
"""
import math
import matplotlib.pyplot as plt
import numpy as np
def mod2pi(theta):
return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi)
def pi_2_pi(angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
def LSL(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
tmp0 = d + sa - sb
mode = ["L", "S", "L"]
p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sa - sb))
if p_squared < 0:
return None, None, None, mode
tmp1 = math.atan2((cb - ca), tmp0)
t = mod2pi(-alpha + tmp1)
p = math.sqrt(p_squared)
q = mod2pi(beta - tmp1)
# print(np.rad2deg(t), p, np.rad2deg(q))
return t, p, q, mode
def RSR(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
tmp0 = d - sa + sb
mode = ["R", "S", "R"]
p_squared = 2 + (d * d) - (2 * c_ab) + (2 * d * (sb - sa))
if p_squared < 0:
return None, None, None, mode
tmp1 = math.atan2((ca - cb), tmp0)
t = mod2pi(alpha - tmp1)
p = math.sqrt(p_squared)
q = mod2pi(-beta + tmp1)
return t, p, q, mode
def LSR(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
p_squared = -2 + (d * d) + (2 * c_ab) + (2 * d * (sa + sb))
mode = ["L", "S", "R"]
if p_squared < 0:
return None, None, None, mode
p = math.sqrt(p_squared)
tmp2 = math.atan2((-ca - cb), (d + sa + sb)) - math.atan2(-2.0, p)
t = mod2pi(-alpha + tmp2)
q = mod2pi(-mod2pi(beta) + tmp2)
return t, p, q, mode
def RSL(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
p_squared = (d * d) - 2 + (2 * c_ab) - (2 * d * (sa + sb))
mode = ["R", "S", "L"]
if p_squared < 0:
return None, None, None, mode
p = math.sqrt(p_squared)
tmp2 = math.atan2((ca + cb), (d - sa - sb)) - math.atan2(2.0, p)
t = mod2pi(alpha - tmp2)
q = mod2pi(beta - tmp2)
return t, p, q, mode
def RLR(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
mode = ["R", "L", "R"]
tmp_rlr = (6.0 - d * d + 2.0 * c_ab + 2.0 * d * (sa - sb)) / 8.0
if abs(tmp_rlr) > 1.0:
return None, None, None, mode
p = mod2pi(2 * math.pi - math.acos(tmp_rlr))
t = mod2pi(alpha - math.atan2(ca - cb, d - sa + sb) + mod2pi(p / 2.0))
q = mod2pi(alpha - beta - t + mod2pi(p))
return t, p, q, mode
def LRL(alpha, beta, d):
sa = math.sin(alpha)
sb = math.sin(beta)
ca = math.cos(alpha)
cb = math.cos(beta)
c_ab = math.cos(alpha - beta)
mode = ["L", "R", "L"]
tmp_lrl = (6. - d * d + 2 * c_ab + 2 * d * (- sa + sb)) / 8.
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.)
q = mod2pi(mod2pi(beta) - alpha - t + mod2pi(p))
return t, p, q, mode
def dubins_path_planning_from_origin(ex, ey, eyaw, c):
# nomalize
dx = ex
dy = ey
D = math.sqrt(dx ** 2.0 + dy ** 2.0)
d = D / c
# print(dx, dy, D, d)
theta = mod2pi(math.atan2(dy, dx))
alpha = mod2pi(- theta)
beta = mod2pi(eyaw - theta)
# print(theta, alpha, beta, d)
planners = [LSL, RSR, LSR, RSL, RLR, LRL]
bcost = float("inf")
bt, bp, bq, bmode = None, None, None, None
for planner in planners:
t, p, q, mode = planner(alpha, beta, d)
if t is None:
# print("".join(mode) + " cannot generate path")
continue
cost = (abs(t) + abs(p) + abs(q))
if bcost > cost:
bt, bp, bq, bmode = t, p, q, mode
bcost = cost
# print(bmode)
px, py, pyaw = generate_course([bt, bp, bq], bmode, c)
return px, py, pyaw, bmode, bcost
def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c):
"""
Dubins path plannner
input:
sx x position of start point [m]
sy y position of start point [m]
syaw yaw angle of start point [rad]
ex x position of end point [m]
ey y position of end point [m]
eyaw yaw angle of end point [rad]
c curvature [1/m]
output:
px
py
pyaw
mode
"""
ex = ex - sx
ey = ey - sy
lex = math.cos(syaw) * ex + math.sin(syaw) * ey
ley = - math.sin(syaw) * ex + math.cos(syaw) * ey
leyaw = eyaw - syaw
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 = [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
def generate_course(length, mode, c):
px = [0.0]
py = [0.0]
pyaw = [0.0]
for m, l in zip(mode, length):
pd = 0.0
if m == "S":
d = 1.0 / c
else: # turning couse
d = np.deg2rad(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]))
if m == "L": # left turn
pyaw.append(pyaw[-1] + d)
elif m == "S": # Straight
pyaw.append(pyaw[-1])
elif m == "R": # right turn
pyaw.append(pyaw[-1] - d)
pd += d
d = l - pd
px.append(px[-1] + d * c * math.cos(pyaw[-1]))
py.append(py[-1] + d * c * math.sin(pyaw[-1]))
if m == "L": # left turn
pyaw.append(pyaw[-1] + d)
elif m == "S": # Straight
pyaw.append(pyaw[-1])
elif m == "R": # right turn
pyaw.append(pyaw[-1] - d)
pd += d
return px, py, pyaw
def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover
"""
Plot arrow
"""
if not isinstance(x, float):
for (ix, iy, iyaw) in zip(x, y, yaw):
plot_arrow(ix, iy, iyaw)
else:
plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
fc=fc, ec=ec, head_width=width, head_length=width)
plt.plot(x, y)
if __name__ == '__main__':
print("Dubins path planner sample start!!")
start_x = 1.0 # [m]
start_y = 1.0 # [m]
start_yaw = np.deg2rad(45.0) # [rad]
end_x = -3.0 # [m]
end_y = -3.0 # [m]
end_yaw = np.deg2rad(-45.0) # [rad]
curvature = 1.0
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")
plt.show()

363
PathPlanning/RRTStarDubins/rrt_star_dubins.py
View File

@@ -1,270 +1,211 @@
"""
Path Planning Sample Code with RRT and Dubins path
Path planning Sample Code with RRT and Dubins path
author: AtsushiSakai(@Atsushi_twi)
"""
import random
import math
import copy
import numpy as np
import dubins_path_planning
import math
import os
import random
import sys
import matplotlib.pyplot as plt
import numpy as np
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../DubinsPath/")
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../RRTStar/")
try:
import dubins_path_planning
from rrt_star import RRTStar
except ImportError:
raise
show_animation = True
class RRT():
class RRTStarDubins(RRTStar):
"""
Class for RRT Planning
Class for RRT star planning with Dubins path
"""
def __init__(self, start, goal, obstacleList, randArea,
goalSampleRate=10, maxIter=100):
class Node(RRTStar.Node):
"""
RRT Node
"""
def __init__(self, x, y, yaw):
super().__init__(x, y)
self.yaw = yaw
self.path_yaw = []
def __init__(self, start, goal, obstacle_list, rand_area,
goal_sample_rate=10,
max_iter=200,
connect_circle_dist=50.0
):
"""
Setting Parameter
start:Start Position [x,y]
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:Ramdom Samping Area [min,max]
randArea:Random Sampling Area [min,max]
"""
self.start = Node(start[0], start[1], start[2])
self.end = Node(goal[0], goal[1], goal[2])
self.minrand = randArea[0]
self.maxrand = randArea[1]
self.goalSampleRate = goalSampleRate
self.maxIter = maxIter
self.obstacleList = obstacleList
self.start = self.Node(start[0], start[1], start[2])
self.end = self.Node(goal[0], goal[1], goal[2])
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.goal_sample_rate = goal_sample_rate
self.max_iter = max_iter
self.obstacle_list = obstacle_list
self.connect_circle_dist = connect_circle_dist
def Planning(self, animation=True):
self.curvature = 1.0 # for dubins path
self.goal_yaw_th = np.deg2rad(1.0)
self.goal_xy_th = 0.5
def planning(self, animation=True, search_until_max_iter=True):
"""
Pathplanning
RRT Star planning
animation: flag for animation on or off
"""
self.nodeList = [self.start]
for i in range(self.maxIter):
rnd = self.get_random_point()
nind = self.GetNearestListIndex(self.nodeList, rnd)
self.node_list = [self.start]
for i in range(self.max_iter):
print("Iter:", i, ", number of nodes:", len(self.node_list))
rnd = self.get_random_node()
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
new_node = self.steer(self.node_list[nearest_ind], rnd)
newNode = self.steer(rnd, nind)
# print(newNode.cost)
if self.CollisionCheck(newNode, self.obstacleList):
nearinds = self.find_near_nodes(newNode)
newNode = self.choose_parent(newNode, nearinds)
self.nodeList.append(newNode)
self.rewire(newNode, nearinds)
if self.check_collision(new_node, self.obstacle_list):
near_indexes = self.find_near_nodes(new_node)
new_node = self.choose_parent(new_node, near_indexes)
if new_node:
self.node_list.append(new_node)
self.rewire(new_node, near_indexes)
if animation and i % 5 == 0:
self.DrawGraph(rnd=rnd)
self.plot_start_goal_arrow()
self.draw_graph(rnd)
# generate coruse
lastIndex = self.get_best_last_index()
# print(lastIndex)
if (not search_until_max_iter) and new_node: # check reaching the goal
last_index = self.search_best_goal_node()
if last_index:
return self.generate_final_course(last_index)
if lastIndex is None:
return None
print("reached max iteration")
path = self.gen_final_course(lastIndex)
return path
def choose_parent(self, newNode, nearinds):
if not nearinds:
return newNode
dlist = []
for i in nearinds:
tNode = self.steer(newNode, i)
if self.CollisionCheck(tNode, self.obstacleList):
dlist.append(tNode.cost)
else:
dlist.append(float("inf"))
mincost = min(dlist)
minind = nearinds[dlist.index(mincost)]
if mincost == float("inf"):
print("mincost is inf")
return newNode
newNode = self.steer(newNode, minind)
return newNode
def pi_2_pi(self, angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
def steer(self, rnd, nind):
# print(rnd)
curvature = 1.0
nearestNode = self.nodeList[nind]
px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning(
nearestNode.x, nearestNode.y, nearestNode.yaw, rnd.x, rnd.y, rnd.yaw, curvature)
newNode = copy.deepcopy(nearestNode)
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
return newNode
def get_random_point(self):
if random.randint(0, 100) > self.goalSampleRate:
rnd = [random.uniform(self.minrand, self.maxrand),
random.uniform(self.minrand, self.maxrand),
random.uniform(-math.pi, math.pi)
]
else: # goal point sampling
rnd = [self.end.x, self.end.y, self.end.yaw]
node = Node(rnd[0], rnd[1], rnd[2])
return node
def get_best_last_index(self):
# print("get_best_last_index")
YAWTH = np.deg2rad(1.0)
XYTH = 0.5
goalinds = []
for (i, node) in enumerate(self.nodeList):
if self.calc_dist_to_goal(node.x, node.y) <= XYTH:
goalinds.append(i)
# angle check
fgoalinds = []
for i in goalinds:
if abs(self.nodeList[i].yaw - self.end.yaw) <= YAWTH:
fgoalinds.append(i)
if not fgoalinds:
return None
mincost = min([self.nodeList[i].cost for i in fgoalinds])
for i in fgoalinds:
if self.nodeList[i].cost == mincost:
return i
last_index = self.search_best_goal_node()
if last_index:
return self.generate_final_course(last_index)
else:
print("Cannot find path")
return None
def gen_final_course(self, goalind):
path = [[self.end.x, self.end.y]]
while self.nodeList[goalind].parent is not None:
node = self.nodeList[goalind]
for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)):
path.append([ix, iy])
# path.append([node.x, node.y])
goalind = node.parent
path.append([self.start.x, self.start.y])
return path
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 +
(node.yaw - newNode.yaw) ** 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]
tNode = self.steer(nearNode, nnode - 1)
obstacleOK = self.CollisionCheck(tNode, self.obstacleList)
imporveCost = nearNode.cost > tNode.cost
if obstacleOK and imporveCost:
# print("rewire")
self.nodeList[i] = tNode
def DrawGraph(self, rnd=None): # pragma: no cover
"""
Draw Graph
"""
def draw_graph(self, rnd=None):
plt.clf()
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.nodeList:
if node.parent is not None:
for node in self.node_list:
if node.parent:
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 self.obstacleList:
for (ox, oy, size) in self.obstacle_list:
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")
plt.axis([-2, 15, -2, 15])
plt.grid(True)
self.plot_start_goal_arrow()
plt.pause(0.01)
def plot_start_goal_arrow(self):
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)
def steer(self, from_node, to_node):
# plt.show()
# input()
px, py, pyaw, mode, course_length = dubins_path_planning.dubins_path_planning(
from_node.x, from_node.y, from_node.yaw,
to_node.x, to_node.y, to_node.yaw, self.curvature)
def GetNearestListIndex(self, nodeList, rnd):
dlist = [(node.x - rnd.x) ** 2 +
(node.y - rnd.y) ** 2 +
(node.yaw - rnd.yaw) ** 2 for node in nodeList]
minind = dlist.index(min(dlist))
new_node = copy.deepcopy(from_node)
new_node.x = px[-1]
new_node.y = py[-1]
new_node.yaw = pyaw[-1]
return minind
new_node.path_x = px
new_node.path_y = py
new_node.path_yaw = pyaw
new_node.cost += course_length
new_node.parent = from_node
def CollisionCheck(self, node, obstacleList):
return new_node
for (ox, oy, size) in obstacleList:
for (ix, iy) in zip(node.path_x, node.path_y):
dx = ox - ix
dy = oy - iy
d = dx * dx + dy * dy
if d <= size ** 2:
return False # collision
def calc_new_cost(self, from_node, to_node):
return True # safe
_, _, _, _, course_length = dubins_path_planning.dubins_path_planning(
from_node.x, from_node.y, from_node.yaw,
to_node.x, to_node.y, to_node.yaw, self.curvature)
return from_node.cost + course_length
class Node():
"""
RRT Node
"""
def get_random_node(self):
def __init__(self, x, y, yaw):
self.x = x
self.y = y
self.yaw = yaw
self.path_x = []
self.path_y = []
self.path_yaw = []
self.cost = 0.0
self.parent = None
if random.randint(0, 100) > self.goal_sample_rate:
rnd = self.Node(random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand),
random.uniform(-math.pi, math.pi)
)
else: # goal point sampling
rnd = self.Node(self.end.x, self.end.y, self.end.yaw)
return rnd
def search_best_goal_node(self):
goal_indexes = []
for (i, node) in enumerate(self.node_list):
if self.calc_dist_to_goal(node.x, node.y) <= self.goal_xy_th:
goal_indexes.append(i)
# angle check
final_goal_indexes = []
for i in goal_indexes:
if abs(self.node_list[i].yaw - self.end.yaw) <= self.goal_yaw_th:
final_goal_indexes.append(i)
if not final_goal_indexes:
return None
min_cost = min([self.node_list[i].cost for i in final_goal_indexes])
for i in final_goal_indexes:
if self.node_list[i].cost == min_cost:
return i
return None
def generate_final_course(self, goal_index):
print("final")
path = [[self.end.x, self.end.y]]
node = self.node_list[goal_index]
while node.parent:
for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)):
path.append([ix, iy])
node = node.parent
path.append([self.start.x, self.start.y])
return path
def main():
@@ -284,12 +225,12 @@ def main():
start = [0.0, 0.0, np.deg2rad(0.0)]
goal = [10.0, 10.0, np.deg2rad(0.0)]
rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList)
path = rrt.Planning(animation=show_animation)
rrtstar_dubins = RRTStarDubins(start, goal, rand_area=[-2.0, 15.0], obstacle_list=obstacleList)
path = rrtstar_dubins.planning(animation=show_animation)
# Draw final path
if show_animation: # pragma: no cover
rrt.DrawGraph()
rrtstar_dubins.draw_graph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
plt.grid(True)
plt.pause(0.001)

386
PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py
View File

@@ -1,289 +1,229 @@
"""
Path Planning Sample Code with RRT for car like robot.
Path planning Sample Code with RRT with Reeds-Shepp path
author: AtsushiSakai(@Atsushi_twi)
"""
import matplotlib.pyplot as plt
import numpy as np
import copy
import math
import os
import random
import sys
import os
import matplotlib.pyplot as plt
import numpy as np
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../ReedsSheppPath/")
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../RRTStar/")
try:
import reeds_shepp_path_planning
except:
from rrt_star import RRTStar
except ImportError:
raise
show_animation = True
STEP_SIZE = 0.1
curvature = 1.0
class RRT():
class RRTStarReedsShepp(RRTStar):
"""
Class for RRT Planning
Class for RRT star planning with Reeds Shepp path
"""
def __init__(self, start, goal, obstacleList, randArea,
goalSampleRate=10, maxIter=400):
class Node(RRTStar.Node):
"""
RRT Node
"""
def __init__(self, x, y, yaw):
super().__init__(x, y)
self.yaw = yaw
self.path_yaw = []
def __init__(self, start, goal, obstacle_list, rand_area,
max_iter=200,
connect_circle_dist=50.0
):
"""
Setting Parameter
start:Start Position [x,y]
goal:Goal Position [x,y]
obstacleList:obstacle Positions [[x,y,size],...]
randArea:Ramdom Samping Area [min,max]
randArea:Random Sampling Area [min,max]
"""
self.start = Node(start[0], start[1], start[2])
self.end = Node(goal[0], goal[1], goal[2])
self.minrand = randArea[0]
self.maxrand = randArea[1]
self.goalSampleRate = goalSampleRate
self.maxIter = maxIter
self.obstacleList = obstacleList
self.start = self.Node(start[0], start[1], start[2])
self.end = self.Node(goal[0], goal[1], goal[2])
self.min_rand = rand_area[0]
self.max_rand = rand_area[1]
self.max_iter = max_iter
self.obstacle_list = obstacle_list
self.connect_circle_dist = connect_circle_dist
def Planning(self, animation=True):
self.curvature = 1.0
self.goal_yaw_th = np.deg2rad(1.0)
self.goal_xy_th = 0.5
def planning(self, animation=True, search_until_max_iter=True):
"""
Pathplanning
planning
animation: flag for animation on or off
"""
self.nodeList = [self.start]
for i in range(self.maxIter):
rnd = self.get_random_point()
nind = self.GetNearestListIndex(self.nodeList, rnd)
self.node_list = [self.start]
for i in range(self.max_iter):
print("Iter:", i, ", number of nodes:", len(self.node_list))
rnd = self.get_random_node()
nearest_ind = self.get_nearest_node_index(self.node_list, rnd)
new_node = self.steer(self.node_list[nearest_ind], rnd)
newNode = self.steer(rnd, nind)
if newNode is None:
continue
if self.CollisionCheck(newNode, self.obstacleList):
nearinds = self.find_near_nodes(newNode)
newNode = self.choose_parent(newNode, nearinds)
if newNode is None:
continue
self.nodeList.append(newNode)
self.rewire(newNode, nearinds)
if self.check_collision(new_node, self.obstacle_list):
near_indexes = self.find_near_nodes(new_node)
new_node = self.choose_parent(new_node, near_indexes)
if new_node:
self.node_list.append(new_node)
self.rewire(new_node, near_indexes)
self.try_goal_path(new_node)
if animation and i % 5 == 0:
self.DrawGraph(rnd=rnd)
self.plot_start_goal_arrow()
self.draw_graph(rnd)
# generate coruse
lastIndex = self.get_best_last_index()
if lastIndex is None:
return None
path = self.gen_final_course(lastIndex)
return path
if (not search_until_max_iter) and new_node: # check reaching the goal
last_index = self.search_best_goal_node()
if last_index:
return self.generate_final_course(last_index)
def choose_parent(self, newNode, nearinds):
if not nearinds:
return newNode
print("reached max iteration")
dlist = []
for i in nearinds:
tNode = self.steer(newNode, i)
if tNode is None:
continue
if self.CollisionCheck(tNode, self.obstacleList):
dlist.append(tNode.cost)
else:
dlist.append(float("inf"))
mincost = min(dlist)
minind = nearinds[dlist.index(mincost)]
if mincost == float("inf"):
print("mincost is inf")
return newNode
newNode = self.steer(newNode, minind)
return newNode
def pi_2_pi(self, angle):
return (angle + math.pi) % (2 * math.pi) - math.pi
def steer(self, rnd, nind):
nearestNode = self.nodeList[nind]
px, py, pyaw, mode, clen = reeds_shepp_path_planning.reeds_shepp_path_planning(
nearestNode.x, nearestNode.y, nearestNode.yaw, rnd.x, rnd.y, rnd.yaw, curvature, STEP_SIZE)
if px is None:
return None
newNode = copy.deepcopy(nearestNode)
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 += sum([abs(c) for c in clen])
newNode.parent = nind
return newNode
def get_random_point(self):
if random.randint(0, 100) > self.goalSampleRate:
rnd = [random.uniform(self.minrand, self.maxrand),
random.uniform(self.minrand, self.maxrand),
random.uniform(-math.pi, math.pi)
]
else: # goal point sampling
rnd = [self.end.x, self.end.y, self.end.yaw]
node = Node(rnd[0], rnd[1], rnd[2])
return node
def get_best_last_index(self):
# print("get_best_last_index")
YAWTH = np.deg2rad(3.0)
XYTH = 0.5
goalinds = []
for (i, node) in enumerate(self.nodeList):
if self.calc_dist_to_goal(node.x, node.y) <= XYTH:
goalinds.append(i)
# print("OK XY TH num is")
# print(len(goalinds))
# angle check
fgoalinds = []
for i in goalinds:
if abs(self.nodeList[i].yaw - self.end.yaw) <= YAWTH:
fgoalinds.append(i)
# print("OK YAW TH num is")
# print(len(fgoalinds))
if not fgoalinds:
return None
mincost = min([self.nodeList[i].cost for i in fgoalinds])
for i in fgoalinds:
if self.nodeList[i].cost == mincost:
return i
last_index = self.search_best_goal_node()
if last_index:
return self.generate_final_course(last_index)
else:
print("Cannot find path")
return None
def gen_final_course(self, goalind):
path = [[self.end.x, self.end.y]]
while self.nodeList[goalind].parent is not None:
node = self.nodeList[goalind]
for (ix, iy) in zip(reversed(node.path_x), reversed(node.path_y)):
path.append([ix, iy])
goalind = node.parent
path.append([self.start.x, self.start.y])
return path
def try_goal_path(self, node):
def calc_dist_to_goal(self, x, y):
return np.linalg.norm([x - self.end.x, y - self.end.y])
goal = self.Node(self.end.x, self.end.y, self.end.yaw)
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 +
(node.yaw - newNode.yaw) ** 2
for node in self.nodeList]
nearinds = [dlist.index(i) for i in dlist if i <= r ** 2]
return nearinds
new_node = self.steer(node, goal)
if new_node is None:
return
def rewire(self, newNode, nearinds):
if self.check_collision(new_node, self.obstacle_list):
self.node_list.append(new_node)
nnode = len(self.nodeList)
for i in nearinds:
nearNode = self.nodeList[i]
tNode = self.steer(nearNode, nnode - 1)
if tNode is None:
continue
obstacleOK = self.CollisionCheck(tNode, self.obstacleList)
imporveCost = nearNode.cost > tNode.cost
if obstacleOK and imporveCost:
# print("rewire")
self.nodeList[i] = tNode
def DrawGraph(self, rnd=None): # pragma: no cover
def draw_graph(self, rnd=None):
plt.clf()
if rnd is not None:
plt.plot(rnd.x, rnd.y, "^k")
for node in self.nodeList:
if node.parent is not None:
for node in self.node_list:
if node.parent:
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 self.obstacleList:
for (ox, oy, size) in self.obstacle_list:
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")
plt.axis([-2, 15, -2, 15])
plt.grid(True)
self.plot_start_goal_arrow()
plt.pause(0.01)
def plot_start_goal_arrow(self):
reeds_shepp_path_planning.plot_arrow(
self.start.x, self.start.y, self.start.yaw)
reeds_shepp_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)
def steer(self, from_node, to_node):
# plt.show()
# input()
px, py, pyaw, mode, course_lengths = reeds_shepp_path_planning.reeds_shepp_path_planning(
from_node.x, from_node.y, from_node.yaw,
to_node.x, to_node.y, to_node.yaw, self.curvature)
def GetNearestListIndex(self, nodeList, rnd):
dlist = [(node.x - rnd.x) ** 2 +
(node.y - rnd.y) ** 2 +
(node.yaw - rnd.yaw) ** 2 for node in nodeList]
minind = dlist.index(min(dlist))
if not px:
return None
return minind
new_node = copy.deepcopy(from_node)
new_node.x = px[-1]
new_node.y = py[-1]
new_node.yaw = pyaw[-1]
def CollisionCheck(self, node, obstacleList):
new_node.path_x = px
new_node.path_y = py
new_node.path_yaw = pyaw
new_node.cost += sum([abs(l) for l in course_lengths])
new_node.parent = from_node
for (ox, oy, size) in obstacleList:
for (ix, iy) in zip(node.path_x, node.path_y):
dx = ox - ix
dy = oy - iy
d = dx * dx + dy * dy
if d <= size ** 2:
return False # collision
return new_node
return True # safe
def calc_new_cost(self, from_node, to_node):
_, _, _, _, course_lengths = reeds_shepp_path_planning.reeds_shepp_path_planning(
from_node.x, from_node.y, from_node.yaw,
to_node.x, to_node.y, to_node.yaw, self.curvature)
if not course_lengths:
return float("inf")
return from_node.cost + sum([abs(l) for l in course_lengths])
def get_random_node(self):
rnd = self.Node(random.uniform(self.min_rand, self.max_rand),
random.uniform(self.min_rand, self.max_rand),
random.uniform(-math.pi, math.pi)
)
return rnd
def search_best_goal_node(self):
goal_indexes = []
for (i, node) in enumerate(self.node_list):
if self.calc_dist_to_goal(node.x, node.y) <= self.goal_xy_th:
goal_indexes.append(i)
print("goal_indexes:", len(goal_indexes))
# angle check
final_goal_indexes = []
for i in goal_indexes:
if abs(self.node_list[i].yaw - self.end.yaw) <= self.goal_yaw_th:
final_goal_indexes.append(i)
print("final_goal_indexes:", len(final_goal_indexes))
if not final_goal_indexes:
return None
min_cost = min([self.node_list[i].cost for i in final_goal_indexes])
print("min_cost:", min_cost)
for i in final_goal_indexes:
if self.node_list[i].cost == min_cost:
return i
return None
def generate_final_course(self, goal_index):
path = [[self.end.x, self.end.y, self.end.yaw]]
node = self.node_list[goal_index]
while node.parent:
for (ix, iy, iyaw) in zip(reversed(node.path_x), reversed(node.path_y), reversed(node.path_yaw)):
path.append([ix, iy, iyaw])
node = node.parent
path.append([self.start.x, self.start.y, self.start.yaw])
return path
class Node():
"""
RRT Node
"""
def __init__(self, x, y, yaw):
self.x = x
self.y = y
self.yaw = yaw
self.path_x = []
self.path_y = []
self.path_yaw = []
self.cost = 0.0
self.parent = None
def main(maxIter=200):
def main(max_iter=100):
print("Start " + __file__)
# ====Search Path with RRT====
@@ -303,15 +243,15 @@ def main(maxIter=200):
start = [0.0, 0.0, np.deg2rad(0.0)]
goal = [6.0, 7.0, np.deg2rad(90.0)]
rrt = RRT(start, goal, randArea=[-2.0, 15.0],
obstacleList=obstacleList,
maxIter=maxIter)
path = rrt.Planning(animation=show_animation)
rrt_star_reeds_shepp = RRTStarReedsShepp(start, goal,
obstacleList,
[-2.0, 15.0], max_iter=max_iter)
path = rrt_star_reeds_shepp.planning(animation=show_animation)
# Draw final path
if show_animation: # pragma: no cover
rrt.DrawGraph()
plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
if path and show_animation: # pragma: no cover
rrt_star_reeds_shepp.draw_graph()
plt.plot([x for (x, y, yaw) in path], [y for (x, y, yaw) in path], '-r')
plt.grid(True)
plt.pause(0.001)
plt.show()

6
PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
View File

@@ -5,10 +5,10 @@ Reeds Shepp path planner sample code
author Atsushi Sakai(@Atsushi_twi)
"""
import numpy as np
import math
import matplotlib.pyplot as plt
import matplotlib.pyplot as plt
import numpy as np
show_animation = True
@@ -353,7 +353,7 @@ def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size):
def reeds_shepp_path_planning(sx, sy, syaw,
gx, gy, gyaw, maxc, step_size):
gx, gy, gyaw, maxc, step_size=0.2):
paths = calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size)

7
tests/test_closed_loop_rrt_star_car.py
View File

@@ -1,6 +1,7 @@
from unittest import TestCase
import sys
import os
import sys
from unittest import TestCase
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../PathPlanning/ClosedLoopRRTStar/")
@@ -17,7 +18,7 @@ class Test(TestCase):
def test1(self):
m.show_animation = False
m.main(gx=1.0, gy=0.0, gyaw=0.0, maxIter=5)
m.main(gx=1.0, gy=0.0, gyaw=0.0, max_iter=5)
if __name__ == '__main__': # pragma: no cover

8
tests/test_dubins_path_planning.py
View File

@@ -1,7 +1,9 @@
from unittest import TestCase
from PathPlanning.DubinsPath import dubins_path_planning
import numpy as np
from PathPlanning.DubinsPath import dubins_path_planning
class Test(TestCase):
@@ -19,8 +21,8 @@ class Test(TestCase):
px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning(
start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
assert(abs(px[-1] - end_x) <= 0.1)
assert(abs(py[-1] - end_y) <= 0.1)
assert (abs(px[-1] - end_x) <= 0.5)
assert (abs(py[-1] - end_y) <= 0.5)
assert(abs(pyaw[-1] - end_yaw) <= 0.1)
def test2(self):

2
tests/test_grid_based_sweep_coverage_path_planner.py
View File

@@ -6,7 +6,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../PathPlanning/G
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../Mapping/grid_map_lib")
try:
import grid_based_sweep_coverage_path_planner
except:
except ImportError:
raise
class TestPlanning(TestCase):

38
tests/test_grid_map_lib.py Normal file
View File

@@ -0,0 +1,38 @@
import os
import sys
import unittest
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../Mapping/grid_map_lib")
try:
from grid_map_lib import GridMap
except ImportError:
raise
class MyTestCase(unittest.TestCase):
def test_position_set(self):
grid_map = GridMap(100, 120, 0.5, 10.0, -0.5)
grid_map.set_value_from_xy_pos(10.1, -1.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, -0.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, 1.1, 1.0)
grid_map.set_value_from_xy_pos(11.1, 0.1, 1.0)
grid_map.set_value_from_xy_pos(10.1, 0.1, 1.0)
grid_map.set_value_from_xy_pos(9.1, 0.1, 1.0)
self.assertEqual(True, True)
def test_polygon_set(self):
ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0]
oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0]
grid_map = GridMap(600, 290, 0.7, 60.0, 30.5)
grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False)
self.assertEqual(True, True)
if __name__ == '__main__':
unittest.main()

2
tests/test_rrt_star.py
View File

@@ -7,7 +7,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
try:
import rrt_star as m
except:
except ImportError:
raise

6
tests/test_rrt_star_reeds_shepp.py
View File

@@ -1,7 +1,7 @@
import os
import sys
from unittest import TestCase
import sys
import os
sys.path.append(os.path.dirname(os.path.abspath(__file__))
+ "/../PathPlanning/RRTStarReedsShepp/")
sys.path.append(os.path.dirname(os.path.abspath(__file__))
@@ -20,7 +20,7 @@ class Test(TestCase):
def test1(self):
m.show_animation = False
m.main(maxIter=5)
m.main(max_iter=5)
if __name__ == '__main__': # pragma: no cover