clean up code folders

PathPlanning/DubinsPath/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()

PathPlanning/RRT/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)

PathPlanning/RRT/rrt_with_pathsmoothing.py

@@ -0,0 +1,277 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+u"""
+@brief: Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT) 
+
+@author: AtsushiSakai
+
+@license: MIT
+
+"""
+
+import random
+import math
+import copy
+
+class RRT():
+    u"""
+    Class for RRT Planning
+    """
+
+    def __init__(self, start, goal, obstacleList,randArea,expandDis=1.0,goalSampleRate=5,maxIter=500):
+        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])
+        self.end=Node(goal[0],goal[1])
+        self.minrand = randArea[0]
+        self.maxrand = randArea[1]
+        self.expandDis = expandDis
+        self.goalSampleRate = goalSampleRate
+        self.maxIter = maxIter
+
+    def Planning(self,animation=True):
+        u"""
+        Pathplanning 
+
+        animation: flag for animation on or off
+        """
+
+        self.nodeList = [self.start]
+        while True:
+            # Random Sampling
+            if random.randint(0, 100) > self.goalSampleRate:
+                rnd = [random.uniform(self.minrand, self.maxrand), random.uniform(self.minrand, self.maxrand)]
+            else:
+                rnd = [self.end.x, self.end.y]
+
+            # Find nearest node
+            nind = self.GetNearestListIndex(self.nodeList, rnd)
+            # print(nind)
+
+            # expand tree
+            nearestNode =self.nodeList[nind]
+            theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
+
+            newNode = copy.deepcopy(nearestNode)
+            newNode.x += self.expandDis * math.cos(theta)
+            newNode.y += self.expandDis * math.sin(theta)
+            newNode.parent = nind
+
+            if not self.__CollisionCheck(newNode, obstacleList):
+                continue
+
+            self.nodeList.append(newNode)
+
+            # check goal
+            dx = newNode.x - self.end.x
+            dy = newNode.y - self.end.y
+            d = math.sqrt(dx * dx + dy * dy)
+            if d <= self.expandDis:
+                print("Goal!!")
+                break
+
+            if animation:
+                self.DrawGraph(rnd)
+
+            
+        path=[[self.end.x,self.end.y]]
+        lastIndex = len(self.nodeList) - 1
+        while self.nodeList[lastIndex].parent is not None:
+            node = self.nodeList[lastIndex]
+            path.append([node.x,node.y])
+            lastIndex = node.parent
+        path.append([self.start.x, self.start.y])
+
+        return path
+
+    def DrawGraph(self,rnd=None):
+        import matplotlib.pyplot as plt
+        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([node.x, self.nodeList[node.parent].x], [node.y, self.nodeList[node.parent].y], "-g")
+        for (x,y,size) in obstacleList:
+            self.PlotCircle(x,y,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 PlotCircle(self,x,y,size):
+        deg=range(0,360,5)
+        deg.append(0)
+        xl=[x+size*math.cos(math.radians(d)) for d in deg]
+        yl=[y+size*math.sin(math.radians(d)) for d in deg]
+        plt.plot(xl, yl, "-k")
+
+    def GetNearestListIndex(self, nodeList, rnd):
+        dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in nodeList]
+        minind = dlist.index(min(dlist))
+        return minind
+
+    def __CollisionCheck(self, node, obstacleList):
+
+        for (ox, oy, size) in obstacleList:
+            dx = ox - node.x
+            dy = oy - node.y
+            d = math.sqrt(dx * dx + dy * dy)
+            if d <= size:
+                return False  # collision
+
+        return True  # safe
+
+class Node():
+    u"""
+    RRT Node
+    """
+
+    def __init__(self, x, y):
+        self.x = x
+        self.y = y
+        self.parent = None
+
+
+def GetPathLength(path):
+    l = 0
+    for i in range(len(path) - 1):
+        dx = path[i + 1][0] - path[i][0]
+        dy = path[i + 1][1] - path[i][1]
+        d = math.sqrt(dx * dx + dy * dy)
+        l += d
+
+    return l
+
+
+def GetTargetPoint(path, targetL):
+    l = 0
+    ti = 0
+    lastPairLen = 0
+    for i in range(len(path) - 1):
+        dx = path[i + 1][0] - path[i][0]
+        dy = path[i + 1][1] - path[i][1]
+        d = math.sqrt(dx * dx + dy * dy)
+        l += d
+        if l >= targetL:
+            ti = i-1
+            lastPairLen = d
+            break
+
+    partRatio = (l - targetL) / lastPairLen
+    #  print(partRatio)
+    #  print((ti,len(path),path[ti],path[ti+1]))
+
+    x = path[ti][0] + (path[ti + 1][0] - path[ti][0]) * partRatio
+    y = path[ti][1] + (path[ti + 1][1] - path[ti][1]) * partRatio
+    #  print((x,y))
+
+    return [x, y, ti]
+
+
+def LineCollisionCheck(first, second, obstacleList):
+    # Line Equation
+
+    x1=first[0]
+    y1=first[1]
+    x2=second[0]
+    y2=second[1]
+
+    try:
+        a=y2-y1
+        b=-(x2-x1)
+        c=y2*(x2-x1)-x2*(y2-y1)
+    except ZeroDivisionError:
+        return False
+
+    #  print(first)
+    #  print(second)
+
+    for (ox,oy,size) in obstacleList:
+        d=abs(a*ox+b*oy+c)/(math.sqrt(a*a+b*b))
+        #  print((ox,oy,size,d))
+        if d<=(size):
+            #  print("NG")
+            return False
+
+    #  print("OK")
+
+    return True  # OK
+
+
+def PathSmoothing(path, maxIter, obstacleList):
+    #  print("PathSmoothing")
+
+    l = GetPathLength(path)
+
+    for i in range(maxIter):
+        # Sample two points
+        pickPoints = [random.uniform(0, l), random.uniform(0, l)]
+        pickPoints.sort()
+        #  print(pickPoints)
+        first = GetTargetPoint(path, pickPoints[0])
+        #  print(first)
+        second = GetTargetPoint(path, pickPoints[1])
+        #  print(second)
+
+        if first[2]<=0 or second[2]<=0:
+            continue
+
+        if (second[2]+1) > len(path):
+            continue
+
+        if second[2]==first[2]:
+            continue
+
+        # collision check
+        if not LineCollisionCheck(first, second, obstacleList):
+            continue
+
+        #Create New path
+        newPath=[]
+        newPath.extend(path[:first[2]+1])
+        newPath.append([first[0],first[1]])
+        newPath.append([second[0],second[1]])
+        newPath.extend(path[second[2]+1:])
+        path=newPath
+        l = GetPathLength(path)
+
+    return path
+
+
+if __name__ == '__main__':
+    import matplotlib.pyplot as plt
+    #====Search Path with RRT====
+    # Parameter
+    obstacleList = [
+        (5, 5, 1),
+        (3, 6, 2),
+        (3, 8, 2),
+        (3, 10, 2),
+        (7, 5, 2),
+        (9, 5, 2)
+    ]  # [x,y,size]
+    rrt=RRT(start=[0,0],goal=[5,10],randArea=[-2,15],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')
+
+    #Path smoothing
+    maxIter=1000
+    smoothedPath = PathSmoothing(path, maxIter, obstacleList)
+    plt.plot([x for (x,y) in smoothedPath], [y for (x,y) in smoothedPath],'-b')
+
+    plt.grid(True)
+    plt.pause(0.01)  # Need for Mac
+    plt.show()

PathPlanning/RRT/simple_rrt.py

@@ -0,0 +1,178 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+u"""
+@brief: Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT)
+
+@author: AtsushiSakai
+
+@license: MIT
+
+"""
+
+
+import random
+import math
+import copy
+
+
+class RRT():
+    u"""
+    Class for RRT Planning
+    """
+
+    def __init__(self, start, goal, obstacleList,
+                 randArea, expandDis=1.0, goalSampleRate=5, maxIter=500):
+        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])
+        self.end = Node(goal[0], goal[1])
+        self.minrand = randArea[0]
+        self.maxrand = randArea[1]
+        self.expandDis = expandDis
+        self.goalSampleRate = goalSampleRate
+        self.maxIter = maxIter
+
+    def Planning(self, animation=True):
+        u"""
+        Pathplanning
+
+        animation: flag for animation on or off
+        """
+
+        self.nodeList = [self.start]
+        while True:
+            # Random Sampling
+            if random.randint(0, 100) > self.goalSampleRate:
+                rnd = [random.uniform(self.minrand, self.maxrand), random.uniform(
+                    self.minrand, self.maxrand)]
+            else:
+                rnd = [self.end.x, self.end.y]
+
+            # Find nearest node
+            nind = self.GetNearestListIndex(self.nodeList, rnd)
+            # print(nind)
+
+            # expand tree
+            nearestNode = self.nodeList[nind]
+            theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
+
+            newNode = copy.deepcopy(nearestNode)
+            newNode.x += self.expandDis * math.cos(theta)
+            newNode.y += self.expandDis * math.sin(theta)
+            newNode.parent = nind
+
+            if not self.__CollisionCheck(newNode, obstacleList):
+                continue
+
+            self.nodeList.append(newNode)
+
+            # check goal
+            dx = newNode.x - self.end.x
+            dy = newNode.y - self.end.y
+            d = math.sqrt(dx * dx + dy * dy)
+            if d <= self.expandDis:
+                print("Goal!!")
+                break
+
+            if animation:
+                self.DrawGraph(rnd)
+
+        path = [[self.end.x, self.end.y]]
+        lastIndex = len(self.nodeList) - 1
+        while self.nodeList[lastIndex].parent is not None:
+            node = self.nodeList[lastIndex]
+            path.append([node.x, node.y])
+            lastIndex = node.parent
+        path.append([self.start.x, self.start.y])
+
+        return path
+
+    def DrawGraph(self, rnd=None):
+        u"""
+        Draw Graph
+        """
+        import matplotlib.pyplot as plt
+        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([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")
+        plt.axis([-2, 15, -2, 15])
+        plt.grid(True)
+        plt.pause(0.01)
+        matplotrecorder.save_frame()  # save each frame
+
+    def GetNearestListIndex(self, nodeList, rnd):
+        dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1])
+                 ** 2 for node in nodeList]
+        minind = dlist.index(min(dlist))
+        return minind
+
+    def __CollisionCheck(self, node, obstacleList):
+
+        for (ox, oy, size) in obstacleList:
+            dx = ox - node.x
+            dy = oy - node.y
+            d = math.sqrt(dx * dx + dy * dy)
+            if d <= size:
+                return False  # collision
+
+        return True  # safe
+
+
+class Node():
+    u"""
+    RRT Node
+    """
+
+    def __init__(self, x, y):
+        self.x = x
+        self.y = y
+        self.parent = None
+
+
+if __name__ == '__main__':
+    print("start RRT path 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]
+    # Set Initial parameters
+    rrt = RRT(start=[0, 0], goal=[5, 10],
+              randArea=[-2, 15], 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.show()
+    for i in range(10):
+        matplotrecorder.save_frame()  # save each frame
+
+    matplotrecorder.save_movie("animation.gif", 0.1)

PathPlanning/RRTCar/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()

PathPlanning/RRTCar/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)

PathPlanning/RRTCar/rrt_car.py

@@ -0,0 +1,261 @@
+#!/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)
+
+            if self.__CollisionCheck(newNode, obstacleList):
+                self.nodeList.append(newNode)
+
+            if animation and i % 5 == 0:
+                self.DrawGraph(rnd=rnd)
+
+        # 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:
+            dx = newNode.x - self.nodeList[i].x
+            dy = newNode.y - self.nodeList[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)
+            else:
+                dlist.append(float("inf"))
+
+        mincost = min(dlist)
+        minind = nearinds[dlist.index(mincost)]
+
+        if mincost == float("inf"):
+            print("mincost is inf")
+            return newNode
+
+        newNode.cost = mincost
+        newNode.parent = 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[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
+
+        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):
+        u"""
+        Draw Graph
+        """
+        import matplotlib.pyplot as plt
+        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(node.path_x, node.path_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)
+        matplotrecorder.save_frame()  # save each frame
+
+    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))
+
+        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 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=False)
+
+    # 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)

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()

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)

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 and i % 5 == 0:
+                self.DrawGraph(rnd=rnd)
+                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)
+
+    for i in range(10):
+        matplotrecorder.save_frame()  # save each frame
+
+    plt.show()
+
+    matplotrecorder.save_movie("animation.gif", 0.1)

PathPlanning/RRTStarCar_reeds_sheep/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)

PathPlanning/RRTStarCar_reeds_sheep/reeds_shepp_path_planning.py

@@ -0,0 +1,90 @@
+#! /usr/bin/python
+# -*- coding: utf-8 -*-
+"""
+
+Reeds Shepp path planner sample code
+
+author Atsushi Sakai(@Atsushi_twi)
+
+License MIT
+
+"""
+import reeds_shepp
+import math
+
+
+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)
+
+
+def reeds_shepp_path_planning(start_x, start_y, start_yaw,
+                              end_x, end_y, end_yaw, curvature):
+    q0 = [start_x, start_y, start_yaw]
+    q1 = [end_x, end_y, end_yaw]
+    step_size = 0.1
+    qs = reeds_shepp.path_sample(q0, q1, curvature, step_size)
+    xs = [q[0] for q in qs]
+    ys = [q[1] for q in qs]
+    yaw = [q[2] for q in qs]
+
+    xs.append(end_x)
+    ys.append(end_y)
+    yaw.append(end_yaw)
+
+    clen = reeds_shepp.path_length(q0, q1, curvature)
+    pathtypeTuple = reeds_shepp.path_type(q0, q1, curvature)
+
+    ptype = ""
+    for t in pathtypeTuple:
+        if t == 1:
+            ptype += "L"
+        elif t == 2:
+            ptype += "S"
+        elif t == 3:
+            ptype += "R"
+
+    return xs, ys, yaw, ptype, clen
+
+
+if __name__ == '__main__':
+    print("Reeds Shepp path planner sample start!!")
+    import matplotlib.pyplot as plt
+
+    start_x = 1.0  # [m]
+    start_y = 1.0  # [m]
+    start_yaw = math.radians(0.0)  # [rad]
+
+    end_x = -0.0  # [m]
+    end_y = -3.0  # [m]
+    end_yaw = math.radians(-45.0)  # [rad]
+
+    curvature = 1.0
+
+    px, py, pyaw, mode, clen = reeds_shepp_path_planning(
+        start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
+
+    plt.plot(px, py, label="final course " + str(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")
+    #  print(clen)
+
+    plt.legend()
+    plt.grid(True)
+    plt.axis("equal")
+    plt.show()

PathPlanning/RRTStarCar_reeds_sheep/rrt_star_car.py

@@ -0,0 +1,320 @@
+#!/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 reeds_shepp_path_planning
+
+
+class RRT():
+    u"""
+    Class for RRT Planning
+    """
+
+    def __init__(self, start, goal, obstacleList, randArea,
+                 goalSampleRate=10, maxIter=2000):
+        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 and i % 5 == 0:
+                self.DrawGraph(rnd=rnd)
+                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 = reeds_shepp_path_planning.reeds_shepp_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(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))
+
+        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)
+
+        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)
+
+        #  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)]
+    obstacleList = [
+        (5, 5, 1),
+        (4, 6, 1),
+        (4, 8, 1),
+        (4, 10, 1),
+        (6, 5, 1),
+        (7, 5, 1),
+        (8, 6, 1),
+        (8, 8, 1),
+        (8, 10, 1)
+    ]  # [x,y,size(radius)]
+
+    # Set Initial parameters
+    start = [0.0, 0.0, math.radians(0.0)]
+    goal = [6.0, 7.0, math.radians(90.0)]
+
+    rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList)
+    path = rrt.Planning(animation=False)
+
+    # 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)
+
+    for i in range(10):
+        matplotrecorder.save_frame()  # save each frame
+
+    plt.show()
+
+    matplotrecorder.save_movie("animation.gif", 0.1)

PathPlanning/RRTstar/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)

PathPlanning/RRTstar/rrt_star.py

@@ -0,0 +1,269 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+u"""
+@brief: Path Planning Sample Code with Randamized Rapidly-Exploring Random Trees (RRT)
+
+@author: AtsushiSakai(@Atsushi_twi)
+
+@license: MIT
+
+"""
+
+import random
+import math
+import copy
+import numpy as np
+
+
+class RRT():
+    u"""
+    Class for RRT Planning
+    """
+
+    def __init__(self, start, goal, obstacleList, randArea,
+                 expandDis=0.5, goalSampleRate=20, 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])
+        self.end = Node(goal[0], goal[1])
+        self.minrand = randArea[0]
+        self.maxrand = randArea[1]
+        self.expandDis = expandDis
+        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)
+
+        # generate coruse
+        lastIndex = self.get_best_last_index()
+        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:
+            dx = newNode.x - self.nodeList[i].x
+            dy = newNode.y - self.nodeList[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)
+            else:
+                dlist.append(float("inf"))
+
+        mincost = min(dlist)
+        minind = nearinds[dlist.index(mincost)]
+
+        if mincost == float("inf"):
+            print("mincost is inf")
+            return newNode
+
+        newNode.cost = mincost
+        newNode.parent = minind
+
+        return newNode
+
+    def steer(self, rnd, nind):
+
+        # expand tree
+        nearestNode = self.nodeList[nind]
+        theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)
+        newNode = copy.deepcopy(nearestNode)
+        newNode.x += self.expandDis * math.cos(theta)
+        newNode.y += self.expandDis * math.sin(theta)
+
+        newNode.cost += self.expandDis
+        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)]
+        else:  # goal point sampling
+            rnd = [self.end.x, self.end.y]
+
+        return rnd
+
+    def get_best_last_index(self):
+
+        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 <= self.expandDis]
+        #  print(goalinds)
+
+        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]
+            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 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 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, obstacleList):
+                return False
+
+        return True
+
+    def DrawGraph(self, rnd=None):
+        u"""
+        Draw Graph
+        """
+        import matplotlib.pyplot as plt
+        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([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")
+        plt.axis([-2, 15, -2, 15])
+        plt.grid(True)
+        plt.pause(0.01)
+        matplotrecorder.save_frame()  # save each frame
+
+    def GetNearestListIndex(self, nodeList, rnd):
+        dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1])
+                 ** 2 for node in nodeList]
+        minind = dlist.index(min(dlist))
+
+        return minind
+
+    def __CollisionCheck(self, 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:
+                return False  # collision
+
+        return True  # safe
+
+
+class Node():
+    u"""
+    RRT Node
+    """
+
+    def __init__(self, x, y):
+        self.x = x
+        self.y = y
+        self.cost = 0.0
+        self.parent = None
+
+
+if __name__ == '__main__':
+    print("Start rrt 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
+    rrt = RRT(start=[0, 0], goal=[5, 10],
+              randArea=[-2, 15], obstacleList=obstacleList)
+    path = rrt.Planning(animation=False)
+
+    # 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
+    for i in range(10):
+        matplotrecorder.save_frame()  # save each frame
+
+    plt.show()
+
+    matplotrecorder.save_movie("animation.gif", 0.1)

PathPlanning/ReedsSheppPath/ReedsSheppStateSpace.cpp

@@ -0,0 +1,709 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2010, Rice University
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Rice University nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Mark Moll */
+
+#include "ompl/base/spaces/ReedsSheppStateSpace.h"
+#include "ompl/base/SpaceInformation.h"
+#include "ompl/util/Exception.h"
+#include <queue>
+#include <boost/math/constants/constants.hpp>
+
+using namespace ompl::base;
+
+namespace
+{
+    // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper.
+
+    const double pi = boost::math::constants::pi<double>();
+    const double twopi = 2. * pi;
+    const double RS_EPS = 1e-6;
+    const double ZERO = 10 * std::numeric_limits<double>::epsilon();
+
+    inline double mod2pi(double x)
+    {
+        double v = fmod(x, twopi);
+        if (v < -pi)
+            v += twopi;
+        else if (v > pi)
+            v -= twopi;
+        return v;
+    }
+    inline void polar(double x, double y, double &r, double &theta)
+    {
+        r = sqrt(x * x + y * y);
+        theta = atan2(y, x);
+    }
+    inline void tauOmega(double u, double v, double xi, double eta, double phi, double &tau, double &omega)
+    {
+        double delta = mod2pi(u - v), A = sin(u) - sin(delta), B = cos(u) - cos(delta) - 1.;
+        double t1 = atan2(eta * A - xi * B, xi * A + eta * B), t2 = 2. * (cos(delta) - cos(v) - cos(u)) + 3;
+        tau = (t2 < 0) ? mod2pi(t1 + pi) : mod2pi(t1);
+        omega = mod2pi(tau - u + v - phi);
+    }
+
+    // formula 8.1 in Reeds-Shepp paper
+    inline bool LpSpLp(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        polar(x - sin(phi), y - 1. + cos(phi), u, t);
+        if (t >= -ZERO)
+        {
+            v = mod2pi(phi - t);
+            if (v >= -ZERO)
+            {
+                assert(fabs(u * cos(t) + sin(phi) - x) < RS_EPS);
+                assert(fabs(u * sin(t) - cos(phi) + 1 - y) < RS_EPS);
+                assert(fabs(mod2pi(t + v - phi)) < RS_EPS);
+                return true;
+            }
+        }
+        return false;
+    }
+    // formula 8.2
+    inline bool LpSpRp(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double t1, u1;
+        polar(x + sin(phi), y - 1. - cos(phi), u1, t1);
+        u1 = u1 * u1;
+        if (u1 >= 4.)
+        {
+            double theta;
+            u = sqrt(u1 - 4.);
+            theta = atan2(2., u);
+            t = mod2pi(t1 + theta);
+            v = mod2pi(t - phi);
+            assert(fabs(2 * sin(t) + u * cos(t) - sin(phi) - x) < RS_EPS);
+            assert(fabs(-2 * cos(t) + u * sin(t) + cos(phi) + 1 - y) < RS_EPS);
+            assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
+            return t >= -ZERO && v >= -ZERO;
+        }
+        return false;
+    }
+    void CSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+    {
+        double t, u, v, Lmin = path.length(), L;
+        if (LpSpLp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[14], t, u, v);
+            Lmin = L;
+        }
+        if (LpSpLp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[14], -t, -u, -v);
+            Lmin = L;
+        }
+        if (LpSpLp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[15], t, u, v);
+            Lmin = L;
+        }
+        if (LpSpLp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[15], -t, -u, -v);
+            Lmin = L;
+        }
+        if (LpSpRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[12], t, u, v);
+            Lmin = L;
+        }
+        if (LpSpRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[12], -t, -u, -v);
+            Lmin = L;
+        }
+        if (LpSpRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[13], t, u, v);
+            Lmin = L;
+        }
+        if (LpSpRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[13], -t, -u, -v);
+    }
+    // formula 8.3 / 8.4  *** TYPO IN PAPER ***
+    inline bool LpRmL(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x - sin(phi), eta = y - 1. + cos(phi), u1, theta;
+        polar(xi, eta, u1, theta);
+        if (u1 <= 4.)
+        {
+            u = -2. * asin(.25 * u1);
+            t = mod2pi(theta + .5 * u + pi);
+            v = mod2pi(phi - t + u);
+            assert(fabs(2 * (sin(t) - sin(t - u)) + sin(phi) - x) < RS_EPS);
+            assert(fabs(2 * (-cos(t) + cos(t - u)) - cos(phi) + 1 - y) < RS_EPS);
+            assert(fabs(mod2pi(t - u + v - phi)) < RS_EPS);
+            return t >= -ZERO && u <= ZERO;
+        }
+        return false;
+    }
+    void CCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+    {
+        double t, u, v, Lmin = path.length(), L;
+        if (LpRmL(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[0], t, u, v);
+            Lmin = L;
+        }
+        if (LpRmL(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[0], -t, -u, -v);
+            Lmin = L;
+        }
+        if (LpRmL(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[1], t, u, v);
+            Lmin = L;
+        }
+        if (LpRmL(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[1], -t, -u, -v);
+            Lmin = L;
+        }
+
+        // backwards
+        double xb = x * cos(phi) + y * sin(phi), yb = x * sin(phi) - y * cos(phi);
+        if (LpRmL(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[0], v, u, t);
+            Lmin = L;
+        }
+        if (LpRmL(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[0], -v, -u, -t);
+            Lmin = L;
+        }
+        if (LpRmL(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[1], v, u, t);
+            Lmin = L;
+        }
+        if (LpRmL(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[1], -v, -u, -t);
+    }
+    // formula 8.7
+    inline bool LpRupLumRm(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = .25 * (2. + sqrt(xi * xi + eta * eta));
+        if (rho <= 1.)
+        {
+            u = acos(rho);
+            tauOmega(u, -u, xi, eta, phi, t, v);
+            assert(fabs(2 * (sin(t) - sin(t - u) + sin(t - 2 * u)) - sin(phi) - x) < RS_EPS);
+            assert(fabs(2 * (-cos(t) + cos(t - u) - cos(t - 2 * u)) + cos(phi) + 1 - y) < RS_EPS);
+            assert(fabs(mod2pi(t - 2 * u - v - phi)) < RS_EPS);
+            return t >= -ZERO && v <= ZERO;
+        }
+        return false;
+    }
+    // formula 8.8
+    inline bool LpRumLumRp(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho = (20. - xi * xi - eta * eta) / 16.;
+        if (rho >= 0 && rho <= 1)
+        {
+            u = -acos(rho);
+            if (u >= -.5 * pi)
+            {
+                tauOmega(u, u, xi, eta, phi, t, v);
+                assert(fabs(4 * sin(t) - 2 * sin(t - u) - sin(phi) - x) < RS_EPS);
+                assert(fabs(-4 * cos(t) + 2 * cos(t - u) + cos(phi) + 1 - y) < RS_EPS);
+                assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
+                return t >= -ZERO && v >= -ZERO;
+            }
+        }
+        return false;
+    }
+    void CCCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+    {
+        double t, u, v, Lmin = path.length(), L;
+        if (LpRupLumRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[2], t, u, -u, v);
+            Lmin = L;
+        }
+        if (LpRupLumRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, u, -v);
+            Lmin = L;
+        }
+        if (LpRupLumRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[3], t, u, -u, v);
+            Lmin = L;
+        }
+        if (LpRupLumRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // timeflip + reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, u, -v);
+            Lmin = L;
+        }
+
+        if (LpRumLumRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[2], t, u, u, v);
+            Lmin = L;
+        }
+        if (LpRumLumRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[2], -t, -u, -u, -v);
+            Lmin = L;
+        }
+        if (LpRumLumRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[3], t, u, u, v);
+            Lmin = L;
+        }
+        if (LpRumLumRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + 2. * fabs(u) + fabs(v)))  // timeflip + reflect
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[3], -t, -u, -u, -v);
+    }
+    // formula 8.9
+    inline bool LpRmSmLm(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x - sin(phi), eta = y - 1. + cos(phi), rho, theta;
+        polar(xi, eta, rho, theta);
+        if (rho >= 2.)
+        {
+            double r = sqrt(rho * rho - 4.);
+            u = 2. - r;
+            t = mod2pi(theta + atan2(r, -2.));
+            v = mod2pi(phi - .5 * pi - t);
+            assert(fabs(2 * (sin(t) - cos(t)) - u * sin(t) + sin(phi) - x) < RS_EPS);
+            assert(fabs(-2 * (sin(t) + cos(t)) + u * cos(t) - cos(phi) + 1 - y) < RS_EPS);
+            assert(fabs(mod2pi(t + pi / 2 + v - phi)) < RS_EPS);
+            return t >= -ZERO && u <= ZERO && v <= ZERO;
+        }
+        return false;
+    }
+    // formula 8.10
+    inline bool LpRmSmRm(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
+        polar(-eta, xi, rho, theta);
+        if (rho >= 2.)
+        {
+            t = theta;
+            u = 2. - rho;
+            v = mod2pi(t + .5 * pi - phi);
+            assert(fabs(2 * sin(t) - cos(t - v) - u * sin(t) - x) < RS_EPS);
+            assert(fabs(-2 * cos(t) - sin(t - v) + u * cos(t) + 1 - y) < RS_EPS);
+            assert(fabs(mod2pi(t + pi / 2 - v - phi)) < RS_EPS);
+            return t >= -ZERO && u <= ZERO && v <= ZERO;
+        }
+        return false;
+    }
+    void CCSC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+    {
+        double t, u, v, Lmin = path.length() - .5 * pi, L;
+        if (LpRmSmLm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[4], t, -.5 * pi, u, v);
+            Lmin = L;
+        }
+        if (LpRmSmLm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
+        {
+            path =
+                ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[4], -t, .5 * pi, -u, -v);
+            Lmin = L;
+        }
+        if (LpRmSmLm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[5], t, -.5 * pi, u, v);
+            Lmin = L;
+        }
+        if (LpRmSmLm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
+        {
+            path =
+                ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[5], -t, .5 * pi, -u, -v);
+            Lmin = L;
+        }
+
+        if (LpRmSmRm(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[8], t, -.5 * pi, u, v);
+            Lmin = L;
+        }
+        if (LpRmSmRm(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
+        {
+            path =
+                ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[8], -t, .5 * pi, -u, -v);
+            Lmin = L;
+        }
+        if (LpRmSmRm(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[9], t, -.5 * pi, u, v);
+            Lmin = L;
+        }
+        if (LpRmSmRm(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
+        {
+            path =
+                ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[9], -t, .5 * pi, -u, -v);
+            Lmin = L;
+        }
+
+        // backwards
+        double xb = x * cos(phi) + y * sin(phi), yb = x * sin(phi) - y * cos(phi);
+        if (LpRmSmLm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[6], v, u, -.5 * pi, t);
+            Lmin = L;
+        }
+        if (LpRmSmLm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
+        {
+            path =
+                ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[6], -v, -u, .5 * pi, -t);
+            Lmin = L;
+        }
+        if (LpRmSmLm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[7], v, u, -.5 * pi, t);
+            Lmin = L;
+        }
+        if (LpRmSmLm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
+        {
+            path =
+                ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[7], -v, -u, .5 * pi, -t);
+            Lmin = L;
+        }
+
+        if (LpRmSmRm(xb, yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path =
+                ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[10], v, u, -.5 * pi, t);
+            Lmin = L;
+        }
+        if (LpRmSmRm(-xb, yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
+        {
+            path =
+                ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[10], -v, -u, .5 * pi, -t);
+            Lmin = L;
+        }
+        if (LpRmSmRm(xb, -yb, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
+        {
+            path =
+                ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[11], v, u, -.5 * pi, t);
+            Lmin = L;
+        }
+        if (LpRmSmRm(-xb, -yb, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
+            path =
+                ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[11], -v, -u, .5 * pi, -t);
+    }
+    // formula 8.11 *** TYPO IN PAPER ***
+    inline bool LpRmSLmRp(double x, double y, double phi, double &t, double &u, double &v)
+    {
+        double xi = x + sin(phi), eta = y - 1. - cos(phi), rho, theta;
+        polar(xi, eta, rho, theta);
+        if (rho >= 2.)
+        {
+            u = 4. - sqrt(rho * rho - 4.);
+            if (u <= ZERO)
+            {
+                t = mod2pi(atan2((4 - u) * xi - 2 * eta, -2 * xi + (u - 4) * eta));
+                v = mod2pi(t - phi);
+                assert(fabs(4 * sin(t) - 2 * cos(t) - u * sin(t) - sin(phi) - x) < RS_EPS);
+                assert(fabs(-4 * cos(t) - 2 * sin(t) + u * cos(t) + cos(phi) + 1 - y) < RS_EPS);
+                assert(fabs(mod2pi(t - v - phi)) < RS_EPS);
+                return t >= -ZERO && v >= -ZERO;
+            }
+        }
+        return false;
+    }
+    void CCSCC(double x, double y, double phi, ReedsSheppStateSpace::ReedsSheppPath &path)
+    {
+        double t, u, v, Lmin = path.length() - pi, L;
+        if (LpRmSLmRp(x, y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[16], t, -.5 * pi, u,
+                                                        -.5 * pi, v);
+            Lmin = L;
+        }
+        if (LpRmSLmRp(-x, y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[16], -t, .5 * pi, -u,
+                                                        .5 * pi, -v);
+            Lmin = L;
+        }
+        if (LpRmSLmRp(x, -y, -phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // reflect
+        {
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[17], t, -.5 * pi, u,
+                                                        -.5 * pi, v);
+            Lmin = L;
+        }
+        if (LpRmSLmRp(-x, -y, phi, t, u, v) && Lmin > (L = fabs(t) + fabs(u) + fabs(v)))  // timeflip + reflect
+            path = ReedsSheppStateSpace::ReedsSheppPath(ReedsSheppStateSpace::reedsSheppPathType[17], -t, .5 * pi, -u,
+                                                        .5 * pi, -v);
+    }
+
+    ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi)
+    {
+        ReedsSheppStateSpace::ReedsSheppPath path;
+        CSC(x, y, phi, path);
+        CCC(x, y, phi, path);
+        CCCC(x, y, phi, path);
+        CCSC(x, y, phi, path);
+        CCSCC(x, y, phi, path);
+        return path;
+    }
+}
+
+const ompl::base::ReedsSheppStateSpace::ReedsSheppPathSegmentType
+    ompl::base::ReedsSheppStateSpace::reedsSheppPathType[18][5] = {
+        {RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP},         // 0
+        {RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP},        // 1
+        {RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP},       // 2
+        {RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP},       // 3
+        {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP},    // 4
+        {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP},   // 5
+        {RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP},    // 6
+        {RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP},   // 7
+        {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP},   // 8
+        {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP},    // 9
+        {RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP},   // 10
+        {RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP},    // 11
+        {RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP},     // 12
+        {RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP},     // 13
+        {RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP},      // 14
+        {RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP},    // 15
+        {RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT},  // 16
+        {RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT}   // 17
+};
+
+ompl::base::ReedsSheppStateSpace::ReedsSheppPath::ReedsSheppPath(const ReedsSheppPathSegmentType *type, double t,
+                                                                 double u, double v, double w, double x)
+  : type_(type)
+{
+    length_[0] = t;
+    length_[1] = u;
+    length_[2] = v;
+    length_[3] = w;
+    length_[4] = x;
+    totalLength_ = fabs(t) + fabs(u) + fabs(v) + fabs(w) + fabs(x);
+}
+
+double ompl::base::ReedsSheppStateSpace::distance(const State *state1, const State *state2) const
+{
+    return rho_ * reedsShepp(state1, state2).length();
+}
+
+void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t,
+                                                   State *state) const
+{
+    bool firstTime = true;
+    ReedsSheppPath path;
+    interpolate(from, to, t, firstTime, path, state);
+}
+
+void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const State *to, const double t, bool &firstTime,
+                                                   ReedsSheppPath &path, State *state) const
+{
+    if (firstTime)
+    {
+        if (t >= 1.)
+        {
+            if (to != state)
+                copyState(state, to);
+            return;
+        }
+        if (t <= 0.)
+        {
+            if (from != state)
+                copyState(state, from);
+            return;
+        }
+        path = reedsShepp(from, to);
+        firstTime = false;
+    }
+    interpolate(from, path, t, state);
+}
+
+void ompl::base::ReedsSheppStateSpace::interpolate(const State *from, const ReedsSheppPath &path, double t,
+                                                   State *state) const
+{
+    auto *s = allocState()->as<StateType>();
+    double seg = t * path.length(), phi, v;
+
+    s->setXY(0., 0.);
+    s->setYaw(from->as<StateType>()->getYaw());
+    for (unsigned int i = 0; i < 5 && seg > 0; ++i)
+    {
+        if (path.length_[i] < 0)
+        {
+            v = std::max(-seg, path.length_[i]);
+            seg += v;
+        }
+        else
+        {
+            v = std::min(seg, path.length_[i]);
+            seg -= v;
+        }
+        phi = s->getYaw();
+        switch (path.type_[i])
+        {
+            case RS_LEFT:
+                s->setXY(s->getX() + sin(phi + v) - sin(phi), s->getY() - cos(phi + v) + cos(phi));
+                s->setYaw(phi + v);
+                break;
+            case RS_RIGHT:
+                s->setXY(s->getX() - sin(phi - v) + sin(phi), s->getY() + cos(phi - v) - cos(phi));
+                s->setYaw(phi - v);
+                break;
+            case RS_STRAIGHT:
+                s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
+                break;
+            case RS_NOP:
+                break;
+        }
+    }
+    state->as<StateType>()->setX(s->getX() * rho_ + from->as<StateType>()->getX());
+    state->as<StateType>()->setY(s->getY() * rho_ + from->as<StateType>()->getY());
+    getSubspace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
+    state->as<StateType>()->setYaw(s->getYaw());
+    freeState(s);
+}
+
+ompl::base::ReedsSheppStateSpace::ReedsSheppPath ompl::base::ReedsSheppStateSpace::reedsShepp(const State *state1,
+                                                                                              const State *state2) const
+{
+    const auto *s1 = static_cast<const StateType *>(state1);
+    const auto *s2 = static_cast<const StateType *>(state2);
+    double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
+    double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw();
+    double dx = x2 - x1, dy = y2 - y1, c = cos(th1), s = sin(th1);
+    double x = c * dx + s * dy, y = -s * dx + c * dy, phi = th2 - th1;
+    return ::reedsShepp(x / rho_, y / rho_, phi);
+}
+
+void ompl::base::ReedsSheppMotionValidator::defaultSettings()
+{
+    stateSpace_ = dynamic_cast<ReedsSheppStateSpace *>(si_->getStateSpace().get());
+    if (stateSpace_ == nullptr)
+        throw Exception("No state space for motion validator");
+}
+
+bool ompl::base::ReedsSheppMotionValidator::checkMotion(const State *s1, const State *s2,
+                                                        std::pair<State *, double> &lastValid) const
+{
+    /* assume motion starts in a valid configuration so s1 is valid */
+
+    bool result = true, firstTime = true;
+    ReedsSheppStateSpace::ReedsSheppPath path;
+    int nd = stateSpace_->validSegmentCount(s1, s2);
+
+    if (nd > 1)
+    {
+        /* temporary storage for the checked state */
+        State *test = si_->allocState();
+
+        for (int j = 1; j < nd; ++j)
+        {
+            stateSpace_->interpolate(s1, s2, (double)j / (double)nd, firstTime, path, test);
+            if (!si_->isValid(test))
+            {
+                lastValid.second = (double)(j - 1) / (double)nd;
+                if (lastValid.first != nullptr)
+                    stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
+                result = false;
+                break;
+            }
+        }
+        si_->freeState(test);
+    }
+
+    if (result)
+        if (!si_->isValid(s2))
+        {
+            lastValid.second = (double)(nd - 1) / (double)nd;
+            if (lastValid.first != nullptr)
+                stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
+            result = false;
+        }
+
+    if (result)
+        valid_++;
+    else
+        invalid_++;
+
+    return result;
+}
+
+bool ompl::base::ReedsSheppMotionValidator::checkMotion(const State *s1, const State *s2) const
+{
+    /* assume motion starts in a valid configuration so s1 is valid */
+    if (!si_->isValid(s2))
+        return false;
+
+    bool result = true, firstTime = true;
+    ReedsSheppStateSpace::ReedsSheppPath path;
+    int nd = stateSpace_->validSegmentCount(s1, s2);
+
+    /* initialize the queue of test positions */
+    std::queue<std::pair<int, int>> pos;
+    if (nd >= 2)
+    {
+        pos.push(std::make_pair(1, nd - 1));
+
+        /* temporary storage for the checked state */
+        State *test = si_->allocState();
+
+        /* repeatedly subdivide the path segment in the middle (and check the middle) */
+        while (!pos.empty())
+        {
+            std::pair<int, int> x = pos.front();
+
+            int mid = (x.first + x.second) / 2;
+            stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, firstTime, path, test);
+
+            if (!si_->isValid(test))
+            {
+                result = false;
+                break;
+            }
+
+            pos.pop();
+
+            if (x.first < mid)
+                pos.push(std::make_pair(x.first, mid - 1));
+            if (x.second > mid)
+                pos.push(std::make_pair(mid + 1, x.second));
+        }
+
+        si_->freeState(test);
+    }
+
+    if (result)
+        valid_++;
+    else
+        invalid_++;
+
+    return result;
+}

PathPlanning/ReedsSheppPath/ReedsSheppStateSpace.h

@@ -0,0 +1,151 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2010, Rice University
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Rice University nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Mark Moll */
+
+#ifndef OMPL_BASE_SPACES_REEDS_SHEPP_STATE_SPACE_
+#define OMPL_BASE_SPACES_REEDS_SHEPP_STATE_SPACE_
+
+#include "ompl/base/spaces/SE2StateSpace.h"
+#include "ompl/base/MotionValidator.h"
+#include <boost/math/constants/constants.hpp>
+
+namespace ompl
+{
+    namespace base
+    {
+        /** \brief An SE(2) state space where distance is measured by the
+            length of Reeds-Shepp curves.
+
+            The notation and solutions are taken from:
+            J.A. Reeds and L.A. Shepp, “Optimal paths for a car that goes both
+            forwards and backwards,” Pacific Journal of Mathematics,
+            145(2):367–393, 1990.
+
+            This implementation explicitly computes all 48 Reeds-Shepp curves
+            and returns the shortest valid solution. This can be improved by
+            using the configuration space partition described in:
+            P. Souères and J.-P. Laumond, “Shortest paths synthesis for a
+            car-like robot,” IEEE Trans. on Automatic Control, 41(5):672–688,
+            May 1996.
+            */
+        class ReedsSheppStateSpace : public SE2StateSpace
+        {
+        public:
+            /** \brief The Reeds-Shepp path segment types */
+            enum ReedsSheppPathSegmentType
+            {
+                RS_NOP = 0,
+                RS_LEFT = 1,
+                RS_STRAIGHT = 2,
+                RS_RIGHT = 3
+            };
+            /** \brief Reeds-Shepp path types */
+            static const ReedsSheppPathSegmentType reedsSheppPathType[18][5];
+            /** \brief Complete description of a ReedsShepp path */
+            class ReedsSheppPath
+            {
+            public:
+                ReedsSheppPath(const ReedsSheppPathSegmentType *type = reedsSheppPathType[0],
+                               double t = std::numeric_limits<double>::max(), double u = 0., double v = 0.,
+                               double w = 0., double x = 0.);
+                double length() const
+                {
+                    return totalLength_;
+                }
+
+                /** Path segment types */
+                const ReedsSheppPathSegmentType *type_;
+                /** Path segment lengths */
+                double length_[5];
+                /** Total length */
+                double totalLength_;
+            };
+
+            ReedsSheppStateSpace(double turningRadius = 1.0) : rho_(turningRadius)
+            {
+            }
+
+            double distance(const State *state1, const State *state2) const override;
+
+            void interpolate(const State *from, const State *to, double t, State *state) const override;
+            virtual void interpolate(const State *from, const State *to, double t, bool &firstTime,
+                                     ReedsSheppPath &path, State *state) const;
+
+            void sanityChecks() const override
+            {
+                double zero = std::numeric_limits<double>::epsilon();
+                double eps = .1;  // rarely such a large error will occur
+                StateSpace::sanityChecks(zero, eps, ~STATESPACE_INTERPOLATION);
+            }
+
+            /** \brief Return the shortest Reeds-Shepp path from SE(2) state state1 to SE(2) state state2 */
+            ReedsSheppPath reedsShepp(const State *state1, const State *state2) const;
+
+        protected:
+            virtual void interpolate(const State *from, const ReedsSheppPath &path, double t, State *state) const;
+
+            /** \brief Turning radius */
+            double rho_;
+        };
+
+        /** \brief A Reeds-Shepp motion validator that only uses the state validity checker.
+            Motions are checked for validity at a specified resolution.
+
+            This motion validator is almost identical to the DiscreteMotionValidator
+            except that it remembers the optimal ReedsSheppPath between different calls to
+            interpolate. */
+        class ReedsSheppMotionValidator : public MotionValidator
+        {
+        public:
+            ReedsSheppMotionValidator(SpaceInformation *si) : MotionValidator(si)
+            {
+                defaultSettings();
+            }
+            ReedsSheppMotionValidator(const SpaceInformationPtr &si) : MotionValidator(si)
+            {
+                defaultSettings();
+            }
+            ~ReedsSheppMotionValidator() override = default;
+            bool checkMotion(const State *s1, const State *s2) const override;
+            bool checkMotion(const State *s1, const State *s2, std::pair<State *, double> &lastValid) const override;
+
+        private:
+            ReedsSheppStateSpace *stateSpace_;
+            void defaultSettings();
+        };
+    }
+}
+
+#endif

PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py

@@ -0,0 +1,90 @@
+#! /usr/bin/python
+# -*- coding: utf-8 -*-
+"""
+
+Reeds Shepp path planner sample code
+
+author Atsushi Sakai(@Atsushi_twi)
+
+License MIT
+
+"""
+import reeds_shepp
+import math
+
+
+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)
+
+
+def reeds_shepp_path_planning(start_x, start_y, start_yaw,
+                              end_x, end_y, end_yaw, curvature):
+    q0 = [start_x, start_y, start_yaw]
+    q1 = [end_x, end_y, end_yaw]
+    step_size = 0.1
+    qs = reeds_shepp.path_sample(q0, q1, curvature, step_size)
+    xs = [q[0] for q in qs]
+    ys = [q[1] for q in qs]
+    yaw = [q[2] for q in qs]
+
+    xs.append(end_x)
+    ys.append(end_y)
+    yaw.append(end_yaw)
+
+    clen = reeds_shepp.path_length(q0, q1, curvature)
+    pathtypeTuple = reeds_shepp.path_type(q0, q1, curvature)
+
+    ptype = ""
+    for t in pathtypeTuple:
+        if t == 1:
+            ptype += "L"
+        elif t == 2:
+            ptype += "S"
+        elif t == 3:
+            ptype += "R"
+
+    return xs, ys, yaw, ptype, clen
+
+
+if __name__ == '__main__':
+    print("Reeds Shepp path planner sample start!!")
+    import matplotlib.pyplot as plt
+
+    start_x = 10.0  # [m]
+    start_y = 1.0  # [m]
+    start_yaw = math.radians(180.0)  # [rad]
+
+    end_x = -0.0  # [m]
+    end_y = -3.0  # [m]
+    end_yaw = math.radians(-45.0)  # [rad]
+
+    curvature = 1.0
+
+    px, py, pyaw, mode, clen = reeds_shepp_path_planning(
+        start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature)
+
+    plt.plot(px, py, label="final course " + str(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")
+    #  print(clen)
+
+    plt.legend()
+    plt.grid(True)
+    plt.axis("equal")
+    plt.show()