diff --git a/scripts/PathPlanning/DubinsPath/dubins_path_planning.py b/scripts/PathPlanning/DubinsPath/dubins_path_planning.py index e6db33ef..a5bc8bbd 100644 --- a/scripts/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/scripts/PathPlanning/DubinsPath/dubins_path_planning.py @@ -16,6 +16,16 @@ def mod2pi(theta): return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi) +def pi_2_pi(angle): + while(angle >= math.pi): + angle = angle - 2.0 * math.pi + + while(angle <= -math.pi): + angle = angle + 2.0 * math.pi + + return angle + + def LSL(alpha, beta, d): sa = math.sin(alpha) sb = math.sin(beta) @@ -153,10 +163,8 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c): for planner in planners: t, p, q, mode = planner(alpha, beta, d) if t is None: - print("".join(mode) + " cannot generate path") + # print("".join(mode) + " cannot generate path") continue - # px, py, pyaw = generate_course([t, p, q], mode, c) - # plt.plot(px, py, label=("".join(mode))) cost = (abs(t) + abs(p) + abs(q)) if bcost > cost: @@ -166,7 +174,7 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c): # print(bmode) px, py, pyaw = generate_course([bt, bp, bq], bmode, c) - return px, py, pyaw, bmode + return px, py, pyaw, bmode, bcost def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c): @@ -197,16 +205,24 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c): ley = - math.sin(syaw) * ex + math.cos(syaw) * ey leyaw = eyaw - syaw - lpx, lpy, lpyaw, mode = dubins_path_planning_from_origin( + lpx, lpy, lpyaw, mode, clen = dubins_path_planning_from_origin( lex, ley, leyaw, c) px = [math.cos(-syaw) * x + math.sin(-syaw) * y + sx for x, y in zip(lpx, lpy)] py = [- math.sin(-syaw) * x + math.cos(-syaw) * y + sy for x, y in zip(lpx, lpy)] - pyaw = leyaw - syaw + pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw] + # print(syaw) + # pyaw = lpyaw - return px, py, pyaw, mode + # plt.plot(pyaw, "-r") + # plt.plot(lpyaw, "-b") + # plt.plot(eyaw, "*r") + # plt.plot(syaw, "*b") + # plt.show() + + return px, py, pyaw, mode, clen def generate_course(length, mode, c): @@ -215,11 +231,14 @@ def generate_course(length, mode, c): py = [0.0] pyaw = [0.0] - d = 0.001 - for m, l in zip(mode, length): pd = 0.0 - while pd <= abs(l): + if m is "S": + d = 1.0 / c + else: # turning couse + d = math.radians(3.0) + + while pd < abs(l - d): # print(pd, l) px.append(px[-1] + d * c * math.cos(pyaw[-1])) py.append(py[-1] + d * c * math.sin(pyaw[-1])) @@ -231,8 +250,18 @@ def generate_course(length, mode, c): elif m is "R": # right turn pyaw.append(pyaw[-1] - d) pd += d + else: + d = l - pd + px.append(px[-1] + d * c * math.cos(pyaw[-1])) + py.append(py[-1] + d * c * math.sin(pyaw[-1])) - # print(px, py, pyaw) + if m is "L": # left turn + pyaw.append(pyaw[-1] + d) + elif m is "S": # Straight + pyaw.append(pyaw[-1]) + elif m is "R": # right turn + pyaw.append(pyaw[-1] - d) + pd += d return px, py, pyaw @@ -241,6 +270,7 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): u""" Plot arrow """ + import matplotlib.pyplot as plt if not isinstance(x, float): for (ix, iy, iyaw) in zip(x, y, yaw): @@ -265,14 +295,18 @@ if __name__ == '__main__': curvature = 1.0 - px, py, pyaw, mode = dubins_path_planning(start_x, start_y, start_yaw, - end_x, end_y, end_yaw, curvature) + px, py, pyaw, mode, clen = dubins_path_planning(start_x, start_y, start_yaw, + end_x, end_y, end_yaw, curvature) plt.plot(px, py, label="final course " + "".join(mode)) # plotting plot_arrow(start_x, start_y, start_yaw) plot_arrow(end_x, end_y, end_yaw) + + # for (ix, iy, iyaw) in zip(px, py, pyaw): + # plot_arrow(ix, iy, iyaw, fc="b") + plt.legend() plt.grid(True) plt.axis("equal") diff --git a/scripts/PathPlanning/RRTCar/dubins_path_planning.py b/scripts/PathPlanning/RRTCar/dubins_path_planning.py index 4545435d..a5bc8bbd 100644 --- a/scripts/PathPlanning/RRTCar/dubins_path_planning.py +++ b/scripts/PathPlanning/RRTCar/dubins_path_planning.py @@ -16,6 +16,16 @@ def mod2pi(theta): return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi) +def pi_2_pi(angle): + while(angle >= math.pi): + angle = angle - 2.0 * math.pi + + while(angle <= -math.pi): + angle = angle + 2.0 * math.pi + + return angle + + def LSL(alpha, beta, d): sa = math.sin(alpha) sb = math.sin(beta) @@ -155,8 +165,6 @@ def dubins_path_planning_from_origin(ex, ey, eyaw, c): if t is None: # print("".join(mode) + " cannot generate path") continue - # px, py, pyaw = generate_course([t, p, q], mode, c) - # plt.plot(px, py, label=("".join(mode))) cost = (abs(t) + abs(p) + abs(q)) if bcost > cost: @@ -204,7 +212,15 @@ def dubins_path_planning(sx, sy, syaw, ex, ey, eyaw, c): y + sx for x, y in zip(lpx, lpy)] py = [- math.sin(-syaw) * x + math.cos(-syaw) * y + sy for x, y in zip(lpx, lpy)] - pyaw = [iyaw - syaw for iyaw in lpyaw] + pyaw = [pi_2_pi(iyaw + syaw) for iyaw in lpyaw] + # print(syaw) + # pyaw = lpyaw + + # plt.plot(pyaw, "-r") + # plt.plot(lpyaw, "-b") + # plt.plot(eyaw, "*r") + # plt.plot(syaw, "*b") + # plt.show() return px, py, pyaw, mode, clen @@ -215,11 +231,14 @@ def generate_course(length, mode, c): py = [0.0] pyaw = [0.0] - d = 0.001 - for m, l in zip(mode, length): pd = 0.0 - while pd <= abs(l): + if m is "S": + d = 1.0 / c + else: # turning couse + d = math.radians(3.0) + + while pd < abs(l - d): # print(pd, l) px.append(px[-1] + d * c * math.cos(pyaw[-1])) py.append(py[-1] + d * c * math.sin(pyaw[-1])) @@ -231,8 +250,18 @@ def generate_course(length, mode, c): elif m is "R": # right turn pyaw.append(pyaw[-1] - d) pd += d + else: + d = l - pd + px.append(px[-1] + d * c * math.cos(pyaw[-1])) + py.append(py[-1] + d * c * math.sin(pyaw[-1])) - # print(px, py, pyaw) + if m is "L": # left turn + pyaw.append(pyaw[-1] + d) + elif m is "S": # Straight + pyaw.append(pyaw[-1]) + elif m is "R": # right turn + pyaw.append(pyaw[-1] - d) + pd += d return px, py, pyaw @@ -256,28 +285,13 @@ if __name__ == '__main__': print("Dubins path planner sample start!!") import matplotlib.pyplot as plt - # start_x = 12.50448605843218 - # start_y = 0.081546724750168 - # start_yaw = 1.9789999999998602 + start_x = 1.0 # [m] + start_y = 1.0 # [m] + start_yaw = math.radians(45.0) # [rad] - # end_x = 14.42965160407553 - # end_y = 7.971199401268141 - # end_yaw = 2.5221853071798117 - - start_x = 2.379024570195084 - start_y = 1.087711940696707 - start_yaw = -2.2749999999998605 - end_x = 12.50448605843218 - end_y = 0.081546724750168 - end_yaw = 1.9789999999998602 - - # start_x = 1.0 # [m] - # start_y = 1.0 # [m] - # start_yaw = math.radians(45.0) # [rad] - - # end_x = -3.0 # [m] - # end_y = -3.0 # [m] - # end_yaw = math.radians(-45.0) # [rad] + end_x = -3.0 # [m] + end_y = -3.0 # [m] + end_yaw = math.radians(-45.0) # [rad] curvature = 1.0 @@ -289,6 +303,10 @@ if __name__ == '__main__': # plotting plot_arrow(start_x, start_y, start_yaw) plot_arrow(end_x, end_y, end_yaw) + + # for (ix, iy, iyaw) in zip(px, py, pyaw): + # plot_arrow(ix, iy, iyaw, fc="b") + plt.legend() plt.grid(True) plt.axis("equal") diff --git a/scripts/PathPlanning/RRTCar/rrt_car.py b/scripts/PathPlanning/RRTCar/rrt_car.py index 9f6e1d92..29657d46 100644 --- a/scripts/PathPlanning/RRTCar/rrt_car.py +++ b/scripts/PathPlanning/RRTCar/rrt_car.py @@ -1,7 +1,7 @@ #!/usr/bin/python # -*- coding: utf-8 -*- -u""" -@brief: Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT) +""" +@brief: Path Planning Sample Code with RRT for car like robot. @author: AtsushiSakai(@Atsushi_twi) @@ -12,9 +12,9 @@ u""" import random import math import copy -import dubins_path_planning import numpy as np -import matplotlib.pyplot as plt +import dubins_path_planning +import matplotrecorder class RRT(): @@ -53,13 +53,9 @@ class RRT(): nind = self.GetNearestListIndex(self.nodeList, rnd) newNode = self.steer(rnd, nind) - # print(newNode.cost) if self.__CollisionCheck(newNode, obstacleList): - # nearinds = self.find_near_nodes(newNode) - # newNode = self.choose_parent(newNode, nearinds) self.nodeList.append(newNode) - # self.rewire(newNode, nearinds) if animation: self.DrawGraph(rnd=rnd) @@ -98,11 +94,6 @@ class RRT(): return newNode def pi_2_pi(self, angle): - u""" - """ - - # angle = float(angle) - while(angle >= math.pi): angle = angle - 2.0 * math.pi @@ -121,21 +112,15 @@ class RRT(): nearestNode.x, nearestNode.y, nearestNode.yaw, rnd[0], rnd[1], rnd[2], curvature) newNode = copy.deepcopy(nearestNode) - newNode.x = rnd[0] - newNode.y = rnd[1] - newNode.yaw = rnd[2] + newNode.x = px[-1] + newNode.y = py[-1] + newNode.yaw = pyaw[-1] + newNode.path_x = px newNode.path_y = py newNode.path_yaw = pyaw newNode.cost += clen newNode.parent = nind - # print(py) - # print(nearestNode.x, nearestNode.y, nearestNode.yaw) - # print(newNode.x, newNode.y, newNode.yaw) - # dubins_path_planning.plot_arrow( - # nearestNode.x, nearestNode.y, nearestNode.yaw) - # dubins_path_planning.plot_arrow(newNode.x, newNode.y, newNode.yaw) - # dubins_path_planning.plot_arrow(rnd[0], rnd[1], rnd[2], fc="b") return newNode @@ -156,7 +141,7 @@ class RRT(): disglist = [self.calc_dist_to_goal( node.x, node.y) for node in self.nodeList] - goalinds = [disglist.index(i) for i in disglist if i <= 0.5] + goalinds = [disglist.index(i) for i in disglist if i <= 0.1] # print(goalinds) mincost = min([self.nodeList[i].cost for i in goalinds]) @@ -180,32 +165,6 @@ class RRT(): def calc_dist_to_goal(self, x, y): return np.linalg.norm([x - self.end.x, y - self.end.y]) - def find_near_nodes(self, newNode): - nnode = len(self.nodeList) - r = 50.0 * math.sqrt((math.log(nnode) / nnode)) - # r = self.expandDis * 5.0 - dlist = [(node.x - newNode.x) ** 2 + - (node.y - newNode.y) ** 2 for node in self.nodeList] - nearinds = [dlist.index(i) for i in dlist if i <= r ** 2] - return nearinds - - def rewire(self, newNode, nearinds): - nnode = len(self.nodeList) - for i in nearinds: - nearNode = self.nodeList[i] - - dx = newNode.x - nearNode.x - dy = newNode.y - nearNode.y - d = math.sqrt(dx ** 2 + dy ** 2) - - scost = newNode.cost + d - - if nearNode.cost > scost: - theta = math.atan2(dy, dx) - if self.check_collision_extend(nearNode, theta, d): - nearNode.parent = nnode - 1 - nearNode.cost = scost - def DrawGraph(self, rnd=None): u""" Draw Graph @@ -217,19 +176,19 @@ class RRT(): for node in self.nodeList: if node.parent is not None: plt.plot(node.path_x, node.path_y, "-g") - # plt.plot([node.x, self.nodeList[node.parent].x], [ - # node.y, self.nodeList[node.parent].y], "-g") for (ox, oy, size) in obstacleList: plt.plot(ox, oy, "ok", ms=30 * size) - plt.plot(self.start.x, self.start.y, "xr") - plt.plot(self.end.x, self.end.y, "xr") + dubins_path_planning.plot_arrow( + self.start.x, self.start.y, self.start.yaw) + dubins_path_planning.plot_arrow( + self.end.x, self.end.y, self.end.yaw) + plt.axis([-2, 15, -2, 15]) plt.grid(True) plt.pause(0.01) - # plt.show() - # input() + matplotrecorder.save_frame() # save each frame def GetNearestListIndex(self, nodeList, rnd): dlist = [(node.x - rnd[0]) ** 2 + @@ -270,6 +229,7 @@ class Node(): if __name__ == '__main__': print("Start rrt start planning") + import matplotlib.pyplot as plt # ====Search Path with RRT==== obstacleList = [ (5, 5, 1), @@ -281,13 +241,17 @@ if __name__ == '__main__': ] # [x,y,size(radius)] # Set Initial parameters - rrt = RRT(start=[0.0, 0.0, math.radians(0.0)], goal=[10.0, 10.0, math.radians(0.0)], - randArea=[-2.0, 15.0], obstacleList=obstacleList) - path = rrt.Planning(animation=False) + start = [0.0, 0.0, math.radians(0.0)] + goal = [10.0, 10.0, math.radians(0.0)] + + rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList) + path = rrt.Planning(animation=True) # Draw final path rrt.DrawGraph() plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') plt.grid(True) - plt.pause(0.01) # Need for Mac + plt.pause(0.001) plt.show() + + matplotrecorder.save_movie("animation.gif", 0.1)