diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py index 366ec5ac..37ac75ad 100644 --- a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py +++ b/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py @@ -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] diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py index 2aa0ba61..a07dc48a 100644 --- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py +++ b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py @@ -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) diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index dc7485ad..706b5a49 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -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) diff --git a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py index 3d068559..f22b058f 100644 --- a/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py +++ b/PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py @@ -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] diff --git a/PathPlanning/InformedRRTStar/informed_rrt_star.py b/PathPlanning/InformedRRTStar/informed_rrt_star.py index 2a999fd0..4a32801d 100644 --- a/PathPlanning/InformedRRTStar/informed_rrt_star.py +++ b/PathPlanning/InformedRRTStar/informed_rrt_star.py @@ -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() \ No newline at end of file + main() diff --git a/PathPlanning/LQRPlanner/LQRplanner.py b/PathPlanning/LQRPlanner/LQRplanner.py index fef1020e..e2000a95 100644 --- a/PathPlanning/LQRPlanner/LQRplanner.py +++ b/PathPlanning/LQRPlanner/LQRplanner.py @@ -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") diff --git a/PathPlanning/LQRRRTStar/lqr_rrt_star.py b/PathPlanning/LQRRRTStar/lqr_rrt_star.py index 0fb25fe4..2e276b9f 100644 --- a/PathPlanning/LQRRRTStar/lqr_rrt_star.py +++ b/PathPlanning/LQRRRTStar/lqr_rrt_star.py @@ -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) diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py index ad3bb11f..3ba1dd9f 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py @@ -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: diff --git a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py index e2411d2b..ecef7d45 100644 --- a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py +++ b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py @@ -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 diff --git a/PathPlanning/RRT/rrt.py b/PathPlanning/RRT/rrt.py index f257ef4d..887cb154 100644 --- a/PathPlanning/RRT/rrt.py +++ b/PathPlanning/RRT/rrt.py @@ -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 diff --git a/PathPlanning/RRTDubins/rrt_dubins.py b/PathPlanning/RRTDubins/rrt_dubins.py index 54752ef8..339acd2c 100644 --- a/PathPlanning/RRTDubins/rrt_dubins.py +++ b/PathPlanning/RRTDubins/rrt_dubins.py @@ -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) diff --git a/PathPlanning/RRTStar/rrt_star.py b/PathPlanning/RRTStar/rrt_star.py index da24d2c8..eb37964d 100644 --- a/PathPlanning/RRTStar/rrt_star.py +++ b/PathPlanning/RRTStar/rrt_star.py @@ -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 diff --git a/PathPlanning/RRTStarDubins/dubins_path_planning.py b/PathPlanning/RRTStarDubins/dubins_path_planning.py deleted file mode 100644 index b2946520..00000000 --- a/PathPlanning/RRTStarDubins/dubins_path_planning.py +++ /dev/null @@ -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() diff --git a/PathPlanning/RRTStarDubins/rrt_star_dubins.py b/PathPlanning/RRTStarDubins/rrt_star_dubins.py index 0494bbab..4d731293 100644 --- a/PathPlanning/RRTStarDubins/rrt_star_dubins.py +++ b/PathPlanning/RRTStarDubins/rrt_star_dubins.py @@ -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) diff --git a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py index aa03418f..5d78f98f 100644 --- a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py +++ b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py @@ -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() diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index da1b4f34..54b5adbe 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -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) diff --git a/tests/test_closed_loop_rrt_star_car.py b/tests/test_closed_loop_rrt_star_car.py index f82d3f25..18513ce6 100644 --- a/tests/test_closed_loop_rrt_star_car.py +++ b/tests/test_closed_loop_rrt_star_car.py @@ -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 diff --git a/tests/test_dubins_path_planning.py b/tests/test_dubins_path_planning.py index d2ddb8ca..ad140b6c 100644 --- a/tests/test_dubins_path_planning.py +++ b/tests/test_dubins_path_planning.py @@ -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): diff --git a/tests/test_grid_based_sweep_coverage_path_planner.py b/tests/test_grid_based_sweep_coverage_path_planner.py index 3caaac89..39333567 100644 --- a/tests/test_grid_based_sweep_coverage_path_planner.py +++ b/tests/test_grid_based_sweep_coverage_path_planner.py @@ -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): diff --git a/tests/test_grid_map_lib.py b/tests/test_grid_map_lib.py new file mode 100644 index 00000000..63b3cc86 --- /dev/null +++ b/tests/test_grid_map_lib.py @@ -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() diff --git a/tests/test_rrt_star.py b/tests/test_rrt_star.py index c5f8f870..514ed348 100644 --- a/tests/test_rrt_star.py +++ b/tests/test_rrt_star.py @@ -7,7 +7,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + try: import rrt_star as m -except: +except ImportError: raise diff --git a/tests/test_rrt_star_reeds_shepp.py b/tests/test_rrt_star_reeds_shepp.py index 4a1fa132..f7eeae3e 100644 --- a/tests/test_rrt_star_reeds_shepp.py +++ b/tests/test_rrt_star_reeds_shepp.py @@ -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