add reeds-shepp curves

Search-based Planning/.idea/workspace.xml

@@ -20,7 +20,11 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
+      <change afterPath="$PROJECT_DIR$/Search_2D/Hybrid_Astar/ReedsSheppPlanner/draw.py" afterDir="false" />
+      <change afterPath="$PROJECT_DIR$/Search_2D/Hybrid_Astar/ReedsSheppPlanner/simulation.py" afterDir="false" />
+      <change afterPath="$PROJECT_DIR$/Search_2D/Hybrid_Astar/ReedsSheppPlanner/utils.py" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_2D/Hybrid_Astar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/Hybrid_Astar/ReedsSheppPlanner/ReedsShepp.py" afterDir="false" />
     </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
@@ -50,7 +54,7 @@
     <property name="ASKED_ADD_EXTERNAL_FILES" value="true" />
     <property name="RunOnceActivity.OpenProjectViewOnStart" value="true" />
     <property name="RunOnceActivity.ShowReadmeOnStart" value="true" />
-    <property name="last_opened_file_path" value="$PROJECT_DIR$/../../PythonRobotics-master/PathPlanning" />
+    <property name="last_opened_file_path" value="$PROJECT_DIR$" />
     <property name="restartRequiresConfirmation" value="false" />
     <property name="run.code.analysis.last.selected.profile" value="aDefault" />
     <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
@@ -72,7 +76,7 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.Field_D_star">
+  <component name="RunManager" selected="Python.simulation">
     <configuration name="D_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
@@ -94,7 +98,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="D_star_Lite" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="Field_D_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -106,7 +110,7 @@
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/D_star_Lite.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/Field_D_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -115,7 +119,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="Field_D_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="Potential_Field" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -127,7 +131,7 @@
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/Field_D_star.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/Potential_Field.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -136,7 +140,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="LPAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="ReedsShepp" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -144,11 +148,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D/Hybrid_Astar/ReedsSheppPlanner" />
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/LPAstar.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/Hybrid_Astar/ReedsSheppPlanner/ReedsShepp.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -157,7 +161,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="Potential_Field" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="dijkstra" type="PythonConfigurationType" factoryName="Python" nameIsGenerated="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -169,7 +173,7 @@
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/Potential_Field.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/dijkstra.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -178,7 +182,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="dijkstra" type="PythonConfigurationType" factoryName="Python" nameIsGenerated="true">
+    <configuration name="simulation" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -186,11 +190,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D/Hybrid_Astar/ReedsSheppPlanner" />
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/dijkstra.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/Hybrid_Astar/ReedsSheppPlanner/simulation.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -202,18 +206,18 @@
     <list>
       <item itemvalue="Python.dijkstra" />
       <item itemvalue="Python.D_star" />
-      <item itemvalue="Python.LPAstar" />
-      <item itemvalue="Python.D_star_Lite" />
       <item itemvalue="Python.Field_D_star" />
       <item itemvalue="Python.Potential_Field" />
+      <item itemvalue="Python.ReedsShepp" />
+      <item itemvalue="Python.simulation" />
     </list>
     <recent_temporary>
       <list>
+        <item itemvalue="Python.simulation" />
+        <item itemvalue="Python.ReedsShepp" />
+        <item itemvalue="Python.D_star" />
         <item itemvalue="Python.Field_D_star" />
         <item itemvalue="Python.Potential_Field" />
-        <item itemvalue="Python.D_star_Lite" />
-        <item itemvalue="Python.LPAstar" />
-        <item itemvalue="Python.D_star" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/ReedsShepp.py

@@ -0,0 +1,445 @@
+"""
+This Reeds-Shepp Curves is from nathanlct's work
+(https://github.com/nathanlct/reeds-shepp-curves)
+"""
+
+import os
+import sys
+from enum import Enum
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../../../ReedsSheppPlanner/")
+
+from Search_2D.Hybrid_Astar.ReedsSheppPlanner.utils import *
+
+
+class Steering(Enum):
+    LEFT = 1
+    RIGHT = 2
+    STRAIGHT = 3
+
+
+class Gear(Enum):
+    FORWARD = 1
+    BACKWARD = 2
+
+
+class PathElement():
+    def __init__(self, param, steering, gear):
+        self.param = param
+        self.steering = steering
+        self.gear = gear
+
+    def __repr__(self):
+        if self.steering == Steering.LEFT:
+            steering_str = "left"
+        elif self.steering == Steering.RIGHT:
+            steering_str = "right"
+        else:
+            steering_str = "straight"
+
+        if self.gear == Gear.FORWARD:
+            gear_str = "forward"
+        else:
+            gear_str = "backward"
+
+        s = "{ Steering: " + steering_str + "\tGear: " + gear_str \
+            + "\tdistance: " + str(round(self.param, 2)) + " }"
+
+        return s
+
+    def reverse_steering(self):
+        if self.steering == Steering.LEFT:
+            self.steering = Steering.RIGHT
+        elif self.steering == Steering.RIGHT:
+            self.steering = Steering.LEFT
+
+    def reverse_gear(self):
+        if self.gear == Gear.FORWARD:
+            self.gear = Gear.BACKWARD
+        else:
+            self.gear = Gear.FORWARD
+
+
+def path_length(path):
+    """
+    this one's obvious
+    """
+    return sum([e.param for e in path])
+
+
+def get_optimal_path(start, end):
+    """
+    Return the shortest path from start to end among those that exist
+    """
+    paths = get_all_paths(start, end)
+    i_min = 0
+    L_min = path_length(paths[0])
+    for i in range(1, len(paths) - 1):
+        L = path_length(paths[i])
+        if L <= L_min:
+            L_min, i_min = L, i
+    return paths[i_min]
+
+
+def get_all_paths(start, end):
+    """
+    Return a list of all the paths from start to end generated by the
+    12 functions and their variants
+    """
+    path_fns = [path1, path2, path3, path4, path5, path6, path7, path8, path9, path10, path11, path12]
+    paths = []
+
+    # get coordinates of end in the set of axis where start is (0,0,0)
+    x, y, theta = change_of_basis(start, end)
+
+    for get_path in path_fns:
+        # get the four variants for each path type, cf article
+        paths.append(get_path(x, y, theta))
+        paths.append(timeflip(get_path(-x, y, -theta)))
+        paths.append(reflect(get_path(x, -y, -theta)))
+        paths.append(reflect(timeflip(get_path(-x, -y, theta))))
+
+    # remove path elements that have parameter 0
+    for i in range(len(paths)):
+        paths[i] = list(filter(lambda e: e.param != 0, paths[i]))
+
+    # remove empty paths
+    paths = list(filter(None, paths))
+
+    return paths
+
+
+def timeflip(path):
+    """
+    timeflip transform described around the end of the article
+    """
+    new_path = path.copy()
+    for e in new_path:
+        e.reverse_gear()
+    return new_path
+
+
+def reflect(path):
+    """
+    reflect transform described around the end of the article
+    """
+    new_path = path.copy()
+    for e in new_path:
+        e.reverse_steering()
+    return new_path
+
+
+def path1(x, y, phi):
+    """
+    Formula 8.1: CSC (same turns)
+    """
+    r, theta = R(x, y)
+    phi = deg2rad(phi)
+    path = []
+
+    u, t = R(x - math.sin(phi), y - 1 + math.cos(phi))
+    v = M(phi - t)
+
+    if t >= 0 and u >= 0 and v >= 0:
+        path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
+        path.append(PathElement(u, Steering.STRAIGHT, Gear.FORWARD))
+        path.append(PathElement(v, Steering.LEFT, Gear.FORWARD))
+
+    return path
+
+
+def path2(x, y, phi):
+    """
+    Formula 8.2: CSC (opposite turns)
+    """
+    r, theta = R(x, y)
+    phi = M(deg2rad(phi))
+    path = []
+
+    rho, t1 = R(x + math.sin(phi), y - 1 - math.cos(phi))
+
+    if rho * rho >= 4:
+        u = math.sqrt(rho * rho - 4)
+        t = M(t1 + math.atan2(2, u))
+        v = M(t - phi)
+
+        if t >= 0 and u >= 0 and v >= 0:
+            path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
+            path.append(PathElement(u, Steering.STRAIGHT, Gear.FORWARD))
+            path.append(PathElement(v, Steering.RIGHT, Gear.FORWARD))
+
+    return path
+
+
+def path3(x, y, phi):
+    """
+    Formula 8.3: C|C|C
+    """
+    r, theta = R(x, y)
+    phi = deg2rad(phi)
+    path = []
+
+    xi = x - math.sin(phi)
+    eta = y - 1 + math.cos(phi)
+    rho, theta = R(xi, eta)
+
+    if rho <= 4:
+        A = math.acos(rho / 4)
+        t = M(theta + math.pi / 2 + A)
+        u = M(math.pi - 2 * A)
+        v = M(phi - t - u)
+
+        if t >= 0 and u >= 0 and v >= 0:
+            path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
+            path.append(PathElement(u, Steering.RIGHT, Gear.BACKWARD))
+            path.append(PathElement(v, Steering.LEFT, Gear.FORWARD))
+
+    return path
+
+
+def path4(x, y, phi):
+    """
+    Formula 8.4 (1): C|CC
+    """
+    r, theta = R(x, y)
+    phi = deg2rad(phi)
+    path = []
+
+    xi = x - math.sin(phi)
+    eta = y - 1 + math.cos(phi)
+    rho, theta = R(xi, eta)
+
+    if rho <= 4:
+        A = math.acos(rho / 4)
+        t = M(theta + math.pi / 2 + A)
+        u = M(math.pi - 2 * A)
+        v = M(t + u - phi)
+
+        if t >= 0 and u >= 0 and v >= 0:
+            path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
+            path.append(PathElement(u, Steering.RIGHT, Gear.BACKWARD))
+            path.append(PathElement(v, Steering.LEFT, Gear.BACKWARD))
+
+    return path
+
+
+def path5(x, y, phi):
+    """
+    Formula 8.4 (2): CC|C
+    """
+    r, theta = R(x, y)
+    phi = deg2rad(phi)
+    path = []
+
+    xi = x - math.sin(phi)
+    eta = y - 1 + math.cos(phi)
+    rho, theta = R(xi, eta)
+
+    if rho <= 4:
+        u = math.acos(1 - rho * rho / 8)
+        A = math.asin(2 * math.sin(u) / rho)
+        t = M(theta + math.pi / 2 - A)
+        v = M(t - u - phi)
+
+        if t >= 0 and u >= 0 and v >= 0:
+            path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
+            path.append(PathElement(u, Steering.RIGHT, Gear.FORWARD))
+            path.append(PathElement(v, Steering.LEFT, Gear.BACKWARD))
+
+    return path
+
+
+def path6(x, y, phi):
+    """
+    Formula 8.7: CCu|CuC
+    """
+    r, theta = R(x, y)
+    phi = deg2rad(phi)
+    path = []
+
+    xi = x + math.sin(phi)
+    eta = y - 1 - math.cos(phi)
+    rho, theta = R(xi, eta)
+
+    if rho <= 4:
+        if rho <= 2:
+            A = math.acos((rho + 2) / 4)
+            t = M(theta + math.pi / 2 + A)
+            u = M(A)
+            v = M(phi - t + 2 * u)
+        else:
+            A = math.acos((rho - 2) / 4)
+            t = M(theta + math.pi / 2 - A)
+            u = M(math.pi - A)
+            v = M(phi - t + 2 * u)
+
+        if t >= 0 and u >= 0 and v >= 0:
+            path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
+            path.append(PathElement(u, Steering.RIGHT, Gear.FORWARD))
+            path.append(PathElement(u, Steering.LEFT, Gear.BACKWARD))
+            path.append(PathElement(v, Steering.RIGHT, Gear.BACKWARD))
+
+    return path
+
+
+def path7(x, y, phi):
+    """
+    Formula 8.8: C|CuCu|C
+    """
+    r, theta = R(x, y)
+    phi = deg2rad(phi)
+    path = []
+
+    xi = x + math.sin(phi)
+    eta = y - 1 - math.cos(phi)
+    rho, theta = R(xi, eta)
+    u1 = (20 - rho * rho) / 16
+
+    if rho <= 6 and 0 <= u1 <= 1:
+        u = math.acos(u1)
+        A = math.asin(2 * math.sin(u) / rho)
+        t = M(theta + math.pi / 2 + A)
+        v = M(t - phi)
+
+        if t >= 0 and u >= 0 and v >= 0:
+            path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
+            path.append(PathElement(u, Steering.RIGHT, Gear.BACKWARD))
+            path.append(PathElement(u, Steering.LEFT, Gear.BACKWARD))
+            path.append(PathElement(v, Steering.RIGHT, Gear.FORWARD))
+
+    return path
+
+
+def path8(x, y, phi):
+    """
+    Formula 8.9 (1): C|C[pi/2]SC
+    """
+    r, theta = R(x, y)
+    phi = deg2rad(phi)
+    path = []
+
+    xi = x - math.sin(phi)
+    eta = y - 1 + math.cos(phi)
+    rho, theta = R(xi, eta)
+
+    if rho >= 2:
+        u = math.sqrt(rho * rho - 4) - 2
+        A = math.atan2(2, u + 2)
+        t = M(theta + math.pi / 2 + A)
+        v = M(t - phi + math.pi / 2)
+
+        if t >= 0 and u >= 0 and v >= 0:
+            path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
+            path.append(PathElement(math.pi / 2, Steering.RIGHT, Gear.BACKWARD))
+            path.append(PathElement(u, Steering.STRAIGHT, Gear.BACKWARD))
+            path.append(PathElement(v, Steering.LEFT, Gear.BACKWARD))
+
+    return path
+
+
+def path9(x, y, phi):
+    """
+    Formula 8.9 (2): CSC[pi/2]|C
+    """
+    r, theta = R(x, y)
+    phi = deg2rad(phi)
+    path = []
+
+    xi = x - math.sin(phi)
+    eta = y - 1 + math.cos(phi)
+    rho, theta = R(xi, eta)
+
+    if rho >= 2:
+        u = math.sqrt(rho * rho - 4) - 2
+        A = math.atan2(u + 2, 2)
+        t = M(theta + math.pi / 2 - A)
+        v = M(t - phi - math.pi / 2)
+
+        if t >= 0 and u >= 0 and v >= 0:
+            path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
+            path.append(PathElement(u, Steering.STRAIGHT, Gear.FORWARD))
+            path.append(PathElement(math.pi / 2, Steering.RIGHT, Gear.FORWARD))
+            path.append(PathElement(v, Steering.LEFT, Gear.BACKWARD))
+
+    return path
+
+
+def path10(x, y, phi):
+    """
+    Formula 8.10 (1): C|C[pi/2]SC
+    """
+    r, theta = R(x, y)
+    phi = deg2rad(phi)
+    path = []
+
+    xi = x + math.sin(phi)
+    eta = y - 1 - math.cos(phi)
+    rho, theta = R(xi, eta)
+
+    if rho >= 2:
+        t = M(theta + math.pi / 2)
+        u = rho - 2
+        v = M(phi - t - math.pi / 2)
+
+        if t >= 0 and u >= 0 and v >= 0:
+            path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
+            path.append(PathElement(math.pi / 2, Steering.RIGHT, Gear.BACKWARD))
+            path.append(PathElement(u, Steering.STRAIGHT, Gear.BACKWARD))
+            path.append(PathElement(v, Steering.RIGHT, Gear.BACKWARD))
+
+    return path
+
+
+def path11(x, y, phi):
+    """
+    Formula 8.10 (2): CSC[pi/2]|C
+    """
+    r, theta = R(x, y)
+    phi = deg2rad(phi)
+    path = []
+
+    xi = x + math.sin(phi)
+    eta = y - 1 - math.cos(phi)
+    rho, theta = R(xi, eta)
+
+    if rho >= 2:
+        t = M(theta)
+        u = rho - 2
+        v = M(phi - t - math.pi / 2)
+
+        if t >= 0 and u >= 0 and v >= 0:
+            path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
+            path.append(PathElement(u, Steering.STRAIGHT, Gear.FORWARD))
+            path.append(PathElement(math.pi / 2, Steering.LEFT, Gear.FORWARD))
+            path.append(PathElement(v, Steering.RIGHT, Gear.BACKWARD))
+
+    return path
+
+
+def path12(x, y, phi):
+    """
+    Formula 8.11: C|C[pi/2]SC[pi/2]|C
+    """
+    r, theta = R(x, y)
+    phi = deg2rad(phi)
+    path = []
+
+    xi = x + math.sin(phi)
+    eta = y - 1 - math.cos(phi)
+    rho, theta = R(xi, eta)
+
+    if rho >= 4:
+        u = math.sqrt(rho * rho - 4) - 4
+        A = math.atan2(2, u + 4)
+        t = M(theta + math.pi / 2 + A)
+        v = M(t - phi)
+
+        if t >= 0 and u >= 0 and v >= 0:
+            path.append(PathElement(t, Steering.LEFT, Gear.FORWARD))
+            path.append(PathElement(math.pi / 2, Steering.RIGHT, Gear.BACKWARD))
+            path.append(PathElement(u, Steering.STRAIGHT, Gear.BACKWARD))
+            path.append(PathElement(math.pi / 2, Steering.LEFT, Gear.BACKWARD))
+            path.append(PathElement(v, Steering.RIGHT, Gear.FORWARD))
+
+    return path

Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/draw.py

@@ -0,0 +1,89 @@
+import turtle
+import random as rd
+import os
+import sys
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../../../ReedsSheppPlanner/")
+
+from Search_2D.Hybrid_Astar.ReedsSheppPlanner.utils import *
+import Search_2D.Hybrid_Astar.ReedsSheppPlanner.ReedsShepp as rs
+
+
+# drawing n units (eg turtle.forward(n)) will draw n * SCALE pixels
+SCALE = 40
+
+
+def scale(x):
+    """
+    Scale the input coordinate(s).
+    """
+    if type(x) is tuple or type(x) is list:
+        return [p * SCALE for p in x]
+    return x * SCALE
+
+
+def unscale(x):
+    """
+    Unscale the input coordinate(s).
+    """
+    if type(x) is tuple or type(x) is list:
+        return [p / SCALE for p in x]
+    return x / SCALE
+
+
+# note: bob is a turtle
+
+def vec(bob):
+    """
+    Draw an arrow.
+    """
+    bob.down()
+    bob.pensize(3)
+    bob.forward(scale(1.2))
+    bob.right(25)
+    bob.backward(scale(.4))
+    bob.forward(scale(.4))
+    bob.left(50)
+    bob.backward(scale(.4))
+    bob.forward(scale(.4))
+    bob.right(25)
+    bob.pensize(1)
+    bob.up()
+
+
+def goto(bob, pos, scale_pos=True):
+    """
+    Go to a position without drawing.
+    """
+    bob.up()
+    if scale_pos:
+        bob.setpos(scale(pos[:2]))
+    else:
+        bob.setpos(pos[:2])
+    bob.setheading(pos[2])
+    bob.down()
+
+
+def draw_path(bob, path):
+    """
+    Draw the path (list of RS.PathElements).
+    """
+    for e in path:
+        gear = 1 if e.gear == rs.Gear.FORWARD else -1
+        if e.steering == rs.Steering.LEFT:
+            bob.circle(scale(1), gear * rad2deg(e.param))
+        elif e.steering == rs.Steering.RIGHT:
+            bob.circle(- scale(1), gear * rad2deg(e.param))
+        elif e.steering == rs.Steering.STRAIGHT:
+            bob.forward(gear * scale(e.param))
+
+
+def set_random_pencolor(bob):
+    """
+    Draws noodles.
+    """
+    r, g, b = 1, 1, 1
+    while r + g + b > 2.5:
+        r, g, b = rd.uniform(0, 1), rd.uniform(0, 1), rd.uniform(0, 1)
+    bob.pencolor(r, g, b)

Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/simulation.py

@@ -0,0 +1,76 @@
+import turtle
+import random as rd
+import math
+
+import os
+import sys
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+                "/../../../../ReedsSheppPlanner/")
+
+import Search_2D.Hybrid_Astar.ReedsSheppPlanner.ReedsShepp as RS
+from Search_2D.Hybrid_Astar.ReedsSheppPlanner import draw
+
+
+def main():
+    # points to be followed
+    # pts = [(-6, -7), (-6, 0), (-4, 6), (0, 5), (0, -2), (-2, -6), (3, -5), (3, 6), (6, 4)]
+    #
+    # # generate PATH so the vectors are pointing at each other
+    # PATH = []
+    # for i in range(len(pts) - 1):
+    #     dx = pts[i + 1][0] - pts[i][0]
+    #     dy = pts[i + 1][1] - pts[i][1]
+    #     theta = math.atan2(dy, dx)
+    #     PATH.append((pts[i][0], pts[i][1], utils.rad2deg(theta)))
+    # PATH.append((pts[-1][0], pts[-1][1], 0))
+
+    # or you can also manually set the angles:
+    PATH = [(-5, 5, 90), (-5, 5, -90), (1, 4, 180), (5, 4, 0), (6, -3, 90), (4, -4, -40), (-2, 0, 240),
+            (-6, -7, 160), (-7, -1, 80)]
+
+    # or just generate a random route:
+    # PATH = []
+    # for _ in range(10):
+    #     PATH.append((rd.randint(-7, 7), rd.randint(-7, 7), rd.randint(0, 359)))
+
+    # init turtle
+    tesla = turtle.Turtle()
+    tesla.speed(0)  # 0: fast; 1: slow, 8.4: cool
+    tesla.shape('arrow')
+    tesla.resizemode('user')
+    tesla.shapesize(1, 1)
+
+    # draw vectors representing points in PATH
+    for pt in PATH:
+        draw.goto(tesla, pt)
+        draw.vec(tesla)
+
+    # draw all routes found
+    tesla.speed(0)
+    for i in range(len(PATH) - 1):
+        paths = RS.get_all_paths(PATH[i], PATH[i + 1])
+
+        for path in paths:
+            draw.set_random_pencolor(tesla)
+            draw.goto(tesla, PATH[i])
+            draw.draw_path(tesla, path)
+
+    # draw shortest route
+    tesla.pencolor(1, 0, 0)
+    tesla.pensize(3)
+    tesla.speed(2)
+    draw.goto(tesla, PATH[0])
+    path_length = 0
+    for i in range(len(PATH) - 1):
+        path = RS.get_optimal_path(PATH[i], PATH[i + 1])
+        path_length += RS.path_length(path)
+        draw.draw_path(tesla, path)
+
+    print("Shortest path length: {} px.".format(int(draw.scale(path_length))))
+
+    turtle.done()
+
+
+if __name__ == '__main__':
+    main()

Search-based Planning/Search_2D/Hybrid_Astar/ReedsSheppPlanner/utils.py

@@ -0,0 +1,50 @@
+import math
+
+
+def M(theta):
+    """
+    Return the angle phi = theta mod (2 pi) such that -pi <= theta < pi.
+    """
+    theta = theta % (2 * math.pi)
+    if theta < -math.pi:
+        return theta + 2 * math.pi
+    if theta >= math.pi:
+        return theta - 2 * math.pi
+    return theta
+
+
+def R(x, y):
+    """
+    Return the polar coordinates (r, theta) of the point (x, y).
+    """
+    r = math.sqrt(x * x + y * y)
+    theta = math.atan2(y, x)
+    return r, theta
+
+
+def change_of_basis(p1, p2):
+    """
+    Given p1 = (x1, y1, theta1) and p2 = (x2, y2, theta2) represented in a
+    coordinate system with origin (0, 0) and rotation 0 (in degrees), return
+    the position and rotation of p2 in the coordinate system which origin
+    (x1, y1) and rotation theta1.
+    """
+    theta1 = deg2rad(p1[2])
+    dx = p2[0] - p1[0]
+    dy = p2[1] - p1[1]
+    new_x = dx * math.cos(theta1) + dy * math.sin(theta1)
+    new_y = -dx * math.sin(theta1) + dy * math.cos(theta1)
+    new_theta = p2[2] - p1[2]
+    return new_x, new_y, new_theta
+
+
+def rad2deg(rad):
+    return 180 * rad / math.pi
+
+
+def deg2rad(deg):
+    return math.pi * deg / 180
+
+
+def sign(x):
+    return 1 if x >= 0 else -1