add RRTstar_car samples

scripts/PathPlanning/RRTStarCar/dubins_path_planning.py

@@ -0,0 +1,313 @@
+#! /usr/bin/python
+# -*- coding: utf-8 -*-
+"""
+
+Dubins path planner sample code
+
+author Atsushi Sakai(@Atsushi_twi)
+
+License MIT
+
+"""
+import math
+
+
+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)
+    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(math.degrees(t), p, math.degrees(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 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]))
+
+            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
+        else:
+            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 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
+
+
+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):
+            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!!")
+    import matplotlib.pyplot as plt
+
+    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]
+
+    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()

scripts/PathPlanning/RRTStarCar/matplotrecorder.py

@@ -0,0 +1,59 @@
+"""
+ A simple Python module for recording matplotlib animation
+
+ This tool use convert command of ImageMagick
+
+ author: Atsushi Sakai
+"""
+
+import matplotlib.pyplot as plt
+import subprocess
+
+iframe = 0
+donothing = False
+
+
+def save_frame():
+    """
+    Save a frame for movie
+    """
+
+    if not donothing:
+        global iframe
+        plt.savefig("recoder" + '{0:04d}'.format(iframe) + '.png')
+        iframe += 1
+
+
+def save_movie(fname, d_pause):
+    """
+    Save movie as gif
+    """
+    if not donothing:
+        cmd = "convert -delay " + str(int(d_pause * 100)) + \
+            " recoder*.png " + fname
+        subprocess.call(cmd, shell=True)
+        cmd = "rm recoder*.png"
+        subprocess.call(cmd, shell=True)
+
+
+if __name__ == '__main__':
+    print("A sample recording start")
+    import math
+
+    time = range(50)
+
+    x1 = [math.cos(t / 10.0) for t in time]
+    y1 = [math.sin(t / 10.0) for t in time]
+    x2 = [math.cos(t / 10.0) + 2 for t in time]
+    y2 = [math.sin(t / 10.0) + 2 for t in time]
+
+    for ix1, iy1, ix2, iy2 in zip(x1, y1, x2, y2):
+        plt.plot(ix1, iy1, "xr")
+        plt.plot(ix2, iy2, "xb")
+        plt.axis("equal")
+        plt.pause(0.1)
+
+        save_frame()  # save each frame
+
+    save_movie("animation.gif", 0.1)
+    #  save_movie("animation.mp4", 0.1)

scripts/PathPlanning/RRTStarCar/rrt_star_car.py

@@ -0,0 +1,305 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+"""
+@brief: Path Planning Sample Code with RRT for car like robot.
+
+@author: AtsushiSakai(@Atsushi_twi)
+
+@license: MIT
+
+"""
+
+import random
+import math
+import copy
+import numpy as np
+import dubins_path_planning
+
+
+class RRT():
+    u"""
+    Class for RRT Planning
+    """
+
+    def __init__(self, start, goal, obstacleList, randArea,
+                 goalSampleRate=10, maxIter=1000):
+        u"""
+        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.goalSampleRate = goalSampleRate
+        self.maxIter = maxIter
+
+    def Planning(self, animation=True):
+        u"""
+        Pathplanning
+
+        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)
+
+            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)
+                if i % 5 == 0:
+                    matplotrecorder.save_frame()  # save each frame
+
+        # generate coruse
+        lastIndex = self.get_best_last_index()
+        #  print(lastIndex)
+        path = self.gen_final_course(lastIndex)
+        return path
+
+    def choose_parent(self, newNode, nearinds):
+        if len(nearinds) == 0:
+            return newNode
+
+        dlist = []
+        for i in nearinds:
+            tNode = self.steer(newNode, i)
+            if self.CollisionCheck(tNode, 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):
+        while(angle >= math.pi):
+            angle = angle - 2.0 * math.pi
+
+        while(angle <= -math.pi):
+            angle = angle + 2.0 * math.pi
+
+        return angle
+
+    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 = math.radians(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)
+
+        mincost = min([self.nodeList[i].cost for i in fgoalinds])
+        for i in fgoalinds:
+            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])
+            #  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, obstacleList)
+            imporveCost = nearNode.cost > tNode.cost
+
+            if obstacleOK and imporveCost:
+                #  print("rewire")
+                self.nodeList[i] = tNode
+
+    def DrawGraph(self, rnd=None):
+        u"""
+        Draw Graph
+        """
+        import matplotlib.pyplot as plt
+        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")
+                #  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)
+
+        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()
+
+    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
+
+
+class Node():
+    u"""
+    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
+
+
+if __name__ == '__main__':
+    print("Start rrt start planning")
+    import matplotlib.pyplot as plt
+    import matplotrecorder
+    matplotrecorder.donothing = True
+
+    # ====Search Path with RRT====
+    obstacleList = [
+        (5, 5, 1),
+        (3, 6, 2),
+        (3, 8, 2),
+        (3, 10, 2),
+        (7, 5, 2),
+        (9, 5, 2)
+    ]  # [x,y,size(radius)]
+
+    # Set Initial parameters
+    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.001)
+    plt.show()
+
+    for i in range(10):
+        matplotrecorder.save_frame()  # save each frame
+
+    matplotrecorder.save_movie("animation.gif", 0.1)