initial commit

.gitignore

@@ -0,0 +1,3 @@
+__pycache__
+.DS_STORE
+*.pyc

LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2019 Nathan Lichtlé
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

README.md

@@ -0,0 +1,72 @@
+# Reeds-Shepp Curves
+
+A simple implementation of the Reeds-Shepp curves formulas provided in the following article:
+
+Reeds, J. A.; Shepp, L. A. Optimal paths for a car that goes both forwards and backwards. Pacific J. Math. 145 (1990), no. 2, 367-393.
+https://projecteuclid.org/euclid.pjm/1102645450
+
+### Requirements
+
+You will need the `turtle` module to run the demo and draw the generated paths, but you can also use the functions provided in `reeds_shepp.py` without drawing anything.
+
+I have only tested the code under Python 3.7.1. It uses the `enum` module so it will not run on a version lower than 3.4, but the code could easily be modified to make it work with Python 2.
+
+### Examples
+
+Draw all the paths going through all the vectors as well as the shortest one (the vectors represent a position and an angle).
+
+```
+$ python3 demo.py
+```
+
+![Reeds-Shepp curves implementation example](demo1.gif)
+
+Another example without drawing:
+
+```python
+import reeds_shepp as rs
+import utils
+import math
+
+# a list of vectors
+ROUTE = [(-2,4,180), (2,4,0), (2,-3,90), (-5,-6,240), (-6, -7, 160), (-7,-1,80)]
+
+full_path = []
+total_length = 0
+
+for i in range(len(ROUTE) - 1):
+    path = rs.get_optimal_path(ROUTE[i], ROUTE[i+1])
+    full_path += path
+    total_length += rs.path_length(path)
+
+print("Shortest path length: {}".format(round(total_length, 2)))
+
+for e in full_path:
+    print(e)
+```
+Output:
+```
+Shortest path length: 33.94
+
+{ Steering: left        Gear: backward  distance: 0.0 }
+{ Steering: straight    Gear: backward  distance: 2.0 }
+{ Steering: left        Gear: backward  distance: 1.57 }
+{ Steering: right       Gear: forward   distance: 1.57 }
+{ Steering: right       Gear: forward   distance: 1.74 }
+{ Steering: straight    Gear: forward   distance: 4.08 }
+{ Steering: right       Gear: forward   distance: 1.57 }
+{ Steering: left        Gear: backward  distance: 1.41 }
+{ Steering: right       Gear: backward  distance: 0.46 }
+{ Steering: left        Gear: forward   distance: 1.57 }
+{ Steering: straight    Gear: forward   distance: 5.95 }
+{ Steering: left        Gear: forward   distance: 0.59 }
+{ Steering: left        Gear: backward  distance: 0.22 }
+{ Steering: right       Gear: backward  distance: 0.46 }
+{ Steering: left        Gear: forward   distance: 0.46 }
+{ Steering: right       Gear: forward   distance: 2.1 }
+{ Steering: left        Gear: forward   distance: 0.6 }
+{ Steering: right       Gear: backward  distance: 1.57 }
+{ Steering: straight    Gear: backward  distance: 2.47 }
+{ Steering: left        Gear: backward  distance: 1.57 }
+{ Steering: right       Gear: forward   distance: 2.0 }
+```

demo.py

@@ -0,0 +1,66 @@
+import turtle
+import reeds_shepp as rs
+import utils
+import draw
+import math
+import random as rd
+
+
+
+def main():
+    # points to be followed
+    pts = [(-6,-7), (-6,0), (-4, 6), (0, 5), (0,-2), (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 = [(-2,4,180), (2,4,0), (2,-3,90), (-5,-6,240), \
+    #         (-6, -7, 160), (-7,-1,80)]
+
+    # or just generate a crazy 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
+
+    # draw vectors representing points in PATH
+    for pt in PATH:
+        draw.goto(tesla, pt)
+        draw.vec(tesla)
+
+    # draw all routes found
+    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)
+    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()

draw.py

@@ -0,0 +1,65 @@
+import turtle
+from utils import *
+import reeds_shepp as rs
+import random as rd
+
+
+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
+
+# 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):
+    """
+    Go to a position without drawing.
+    """
+    bob.up()
+    bob.setpos(scale(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)