first release universal sampling method

.idea/dictionaries/atsushisakai.xml

@@ -0,0 +1,7 @@
+<component name="ProjectDictionaryState">
+  <dictionary name="atsushisakai">
+    <words>
+      <w>atsushi</w>
+    </words>
+  </dictionary>
+</component>

PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py

@@ -8,6 +8,7 @@ import numpy as np
 import math
 import model_predictive_trajectory_generator as planner
 import motion_model
+import pandas as pd
 
 
 def calc_states_list():
@@ -22,13 +23,12 @@ def calc_states_list():
         for iy in y:
             for ix in x:
                 states.append([ix, iy, iyaw])
-    #  print(len(states))
+    # print(len(states))
 
     return states
 
 
 def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookuptable):
-
     mind = float("inf")
     minid = -1
 
@@ -37,18 +37,33 @@ def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookuptable):
         dx = tx - table[0]
         dy = ty - table[1]
         dyaw = tyaw - table[2]
-        d = math.sqrt(dx**2 + dy**2 + dyaw**2)
+        d = math.sqrt(dx ** 2 + dy ** 2 + dyaw ** 2)
         if d <= mind:
             minid = i
             mind = d
 
-    #  print(minid)
+    # print(minid)
 
     return lookuptable[minid]
 
 
-def generate_lookup_table():
+def save_lookup_table(fname, table):
+    mt = np.array(table)
+    print(mt)
+    # save csv
+    df = pd.DataFrame()
+    df["x"] = mt[:, 0]
+    df["y"] = mt[:, 1]
+    df["yaw"] = mt[:, 2]
+    df["s"] = mt[:, 3]
+    df["km"] = mt[:, 4]
+    df["kf"] = mt[:, 5]
+    df.to_csv(fname, index=None)
 
+    print("lookup table file is saved as " + fname)
+
+
+def generate_lookup_table():
     states = calc_states_list()
     k0 = 0.0
 
@@ -58,11 +73,10 @@ def generate_lookup_table():
     for state in states:
         bestp = search_nearest_one_from_lookuptable(
             state[0], state[1], state[2], lookuptable)
-        #  print(bestp)
 
         target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
         init_p = np.matrix(
-            [math.sqrt(state[0]**2 + state[1]**2), bestp[4], bestp[5]]).T
+            [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).T
 
         x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
 
@@ -71,7 +85,9 @@ def generate_lookup_table():
             lookuptable.append(
                 [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])])
 
-    print("finish lookuptable generation")
+    print("finish lookup table generation")
+
+    save_lookup_table("lookuptable.csv", lookuptable)
 
     for table in lookuptable:
         xc, yc, yawc = motion_model.generate_trajectory(

PathPlanning/StateLatticePlanner/lookuptable.csv

@@ -0,0 +1,25 @@
+x,y,yaw,s,km,kf
+1.0,0.0,0.0,1.0,0.0,0.0
+0.9734888894493215,-0.009758406565994977,0.5358080146312756,0.9922329557399788,-0.10222538550473198,3.0262632253145982
+10.980728996433243,-0.0003093605787364978,0.522622783944529,11.000391678142623,0.00010296091030877934,0.2731556687244648
+16.020309241920156,0.0001292339008200291,0.5243399938698222,16.100019813021202,0.00013263212395994706,0.18999138959173634
+20.963495745193626,-0.00033031017429944326,0.5226120033275024,21.10082901143343,0.00011687467551566884,0.14550546012583987
+6.032553475650599,2.008504211720188,0.5050517859971599,6.400329805864408,0.1520002249689879,-0.13105940607691127
+10.977487445230075,2.0078696810700034,0.5263634407901872,11.201040572298973,0.04895863722280565,0.08356555007223682
+15.994057699325753,2.025659106131227,0.5303858891065698,16.200300421483128,0.0235708657178127,0.10002225103921249
+20.977228843605943,2.0281289825388513,0.5300376140865567,21.20043308669372,0.013795675421657671,0.09331700188063087
+25.95453914157977,1.9926432818499131,0.5226203078411618,26.200880299840527,0.00888830054451281,0.0830622000626594
+0.9999999999999999,0.0,0.0,1.0,0.0,0.0
+5.999999999670752,5.231312388722274e-05,1.4636120911014667e-05,5.996117185283419,4.483756968024291e-06,-3.4422519205046243e-06
+10.999749470720566,-0.011978787043239986,0.022694802592583763,10.99783855994015,-0.00024209503125174496,0.01370491008661795
+15.999885224357776,-0.018937230084507616,0.011565580878015763,15.99839381389597,-0.0002086399372890716,0.005267247862667496
+20.999882688881286,-0.030304200126461317,0.009117836885732596,20.99783120184498,-0.00020013159571184735,0.0034529188783636866
+25.999911270030413,-0.025754431694664327,0.0074809531598503615,25.99674782258235,-0.0001111138115390646,0.0021946603965658368
+10.952178818975062,1.993067260835455,0.0045572480669707136,11.17961498195845,0.04836813285436623,-0.19328716251760758
+16.028306009258,2.0086286208315407,0.009306594796759554,16.122411866381054,0.02330689045417979,-0.08877002085985948
+20.971603115419715,1.9864158336174966,0.007016819403167119,21.093006725076872,0.013439123130720928,-0.05238318300611623
+25.997019676818372,1.9818581321430093,0.007020172975955249,26.074021794586585,0.00876496148602802,-0.03362579291686346
+16.017903482982604,4.009490840390534,-5.293114796312698e-05,16.600937712976638,0.044543450568614244,-0.17444651314466567
+20.98845988414163,3.956600199823583,-0.01050744134070728,21.40149119463485,0.02622674388276059,-0.10625681152519345
+25.979448381017914,3.9968223055054977,-0.00012819253386682928,26.30504721211744,0.017467093413146118,-0.06914750106424669
+25.96268055563514,5.9821266846168,4.931311239565056e-05,26.801388563459287,0.025426008913226557,-0.10047663078001536

PathPlanning/StateLatticePlanner/matplotrecorder/.gitignore

@@ -0,0 +1,89 @@
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+env/
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+*.egg-info/
+.installed.cfg
+*.egg
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*,cover
+.hypothesis/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+target/
+
+# IPython Notebook
+.ipynb_checkpoints
+
+# pyenv
+.python-version
+
+# celery beat schedule file
+celerybeat-schedule
+
+# dotenv
+.env
+
+# virtualenv
+venv/
+ENV/
+
+# Spyder project settings
+.spyderproject
+
+# Rope project settings
+.ropeproject

PathPlanning/StateLatticePlanner/matplotrecorder/LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2017 Atsushi Sakai
+
+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.

PathPlanning/StateLatticePlanner/matplotrecorder/README.md

@@ -0,0 +1,59 @@
+# matplotrecorder
+A simple Python module for recording matplotlib animation
+
+It can generate a matplotlib animation movie (mp4, gif, etc.)
+
+This tool use "convert" command of ImageMagick.
+
+# Sample gif
+
+![matplotrecorder/animation.gif at master · AtsushiSakai/matplotrecorder](https://github.com/AtsushiSakai/matplotrecorder/blob/master/animation.gif)
+
+# Requrements
+
+- [ImageMagic](https://www.imagemagick.org/script/index.php)
+
+
+# How to use
+
+Call save_frame() at each animation iteration,
+
+And then, call savemovie() for movie generation.
+
+A sample code:
+
+
+      import matplotrecorder
+
+      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)
+
+          matplotrecorder.save_frame()  # save each frame
+
+      # generate movie
+      matplotrecorder.save_movie("animation.mp4", 0.1)
+      #  matplotrecorder.save_movie("animation.gif", 0.1) #gif is ok.
+
+
+
+# License 
+
+MIT
+
+# Author
+
+Atsushi Sakai ([@Atsushi_twi](https://twitter.com/Atsushi_twi))
+

PathPlanning/StateLatticePlanner/matplotrecorder/matplotrecorder.py

@@ -0,0 +1,74 @@
+"""
+ A simple Python module for recording matplotlib animation
+
+ This tool use convert command of ImageMagick
+
+ author: Atsushi Sakai
+
+ How to use:
+
+ - import
+
+    from matplotrecorder import matplotrecorder
+
+ - save file
+
+    matplotrecorder.save_frame()
+
+ - generate movie
+
+    matplotrecorder.save_movie("animation.gif", 0.1)
+
+"""
+
+import matplotlib.pyplot as plt
+import subprocess
+
+iframe = 0
+donothing = False  # switch to stop all recordering
+
+
+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/StateLatticePlanner/model_predictive_trajectory_generator.py

@@ -0,0 +1,183 @@
+"""
+Model trajectory generator
+
+author: Atsushi Sakai
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+import math
+import motion_model
+from matplotrecorder import matplotrecorder
+
+# optimization parameter
+max_iter = 100
+h = np.matrix([0.5, 0.02, 0.02]).T  # parameter sampling distanse
+cost_th = 0.1
+
+matplotrecorder.donothing = True
+show_graph = False
+
+
+def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
+    u"""
+    Plot arrow
+    """
+    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)
+    plt.plot(0, 0)
+
+
+def calc_diff(target, x, y, yaw):
+    d = np.array([target.x - x[-1],
+                  target.y - y[-1],
+                  motion_model.pi_2_pi(target.yaw - yaw[-1])])
+
+    return d
+
+
+def calc_J(target, p, h, k0):
+    xp, yp, yawp = motion_model.generate_last_state(
+        p[0, 0] + h[0, 0], p[1, 0], p[2, 0], k0)
+    dp = calc_diff(target, [xp], [yp], [yawp])
+    xn, yn, yawn = motion_model.generate_last_state(
+        p[0, 0] - h[0, 0], p[1, 0], p[2, 0], k0)
+    dn = calc_diff(target, [xn], [yn], [yawn])
+    d1 = np.matrix((dp - dn) / (2.0 * h[1, 0])).T
+
+    xp, yp, yawp = motion_model.generate_last_state(
+        p[0, 0], p[1, 0] + h[1, 0], p[2, 0], k0)
+    dp = calc_diff(target, [xp], [yp], [yawp])
+    xn, yn, yawn = motion_model.generate_last_state(
+        p[0, 0], p[1, 0] - h[1, 0], p[2, 0], k0)
+    dn = calc_diff(target, [xn], [yn], [yawn])
+    d2 = np.matrix((dp - dn) / (2.0 * h[2, 0])).T
+
+    xp, yp, yawp = motion_model.generate_last_state(
+        p[0, 0], p[1, 0], p[2, 0] + h[2, 0], k0)
+    dp = calc_diff(target, [xp], [yp], [yawp])
+    xn, yn, yawn = motion_model.generate_last_state(
+        p[0, 0], p[1, 0], p[2, 0] - h[2, 0], k0)
+    dn = calc_diff(target, [xn], [yn], [yawn])
+    d3 = np.matrix((dp - dn) / (2.0 * h[2, 0])).T
+
+    J = np.hstack((d1, d2, d3))
+
+    return J
+
+
+def selection_learning_param(dp, p, k0, target):
+
+    mincost = float("inf")
+    mina = 1.0
+    maxa = 2.0
+    da = 0.5
+
+    for a in np.arange(mina, maxa, da):
+        tp = p[:, :] + a * dp
+        xc, yc, yawc = motion_model.generate_last_state(
+            tp[0], tp[1], tp[2], k0)
+        dc = np.matrix(calc_diff(target, [xc], [yc], [yawc])).T
+        cost = np.linalg.norm(dc)
+
+        if cost <= mincost and a != 0.0:
+            mina = a
+            mincost = cost
+
+    #  print(mincost, mina)
+    #  input()
+
+    return mina
+
+
+def show_trajectory(target, xc, yc):
+
+    plt.clf()
+    plot_arrow(target.x, target.y, target.yaw)
+    plt.plot(xc, yc, "-r")
+    plt.axis("equal")
+    plt.grid(True)
+    plt.pause(0.1)
+    matplotrecorder.save_frame()
+
+
+def optimize_trajectory(target, k0, p):
+
+    for i in range(max_iter):
+        xc, yc, yawc = motion_model.generate_trajectory(p[0], p[1], p[2], k0)
+        dc = np.matrix(calc_diff(target, xc, yc, yawc)).T
+        #  print(dc.T)
+
+        cost = np.linalg.norm(dc)
+        if cost <= cost_th:
+            print("path is ok cost is:" + str(cost))
+            break
+
+        J = calc_J(target, p, h, k0)
+        try:
+            dp = - np.linalg.inv(J) * dc
+        except np.linalg.linalg.LinAlgError:
+            print("cannot calc path LinAlgError")
+            xc, yc, yawc, p = None, None, None, None
+            break
+        alpha = selection_learning_param(dp, p, k0, target)
+
+        p += alpha * np.array(dp)
+        #  print(p.T)
+
+        if show_graph:
+            show_trajectory(target, xc, yc)
+    else:
+        xc, yc, yawc, p = None, None, None, None
+        print("cannot calc path")
+
+    return xc, yc, yawc, p
+
+
+def test_optimize_trajectory():
+
+    #  target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(00.0))
+    target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(90.0))
+    k0 = 0.0
+
+    init_p = np.matrix([6.0, 0.0, 0.0]).T
+
+    x, y, yaw, p = optimize_trajectory(target, k0, init_p)
+
+    show_trajectory(target, x, y)
+    matplotrecorder.save_movie("animation.gif", 0.1)
+
+    #  plt.plot(x, y, "-r")
+    plot_arrow(target.x, target.y, target.yaw)
+    plt.axis("equal")
+    plt.grid(True)
+    plt.show()
+
+
+def test_trajectory_generate():
+    s = 5.0  # [m]
+    k0 = 0.0
+    km = math.radians(30.0)
+    kf = math.radians(-30.0)
+
+    #  plt.plot(xk, yk, "xr")
+    #  plt.plot(t, kp)
+    #  plt.show()
+
+    x, y = motion_model.generate_trajectory(s, km, kf, k0)
+
+    plt.plot(x, y, "-r")
+    plt.axis("equal")
+    plt.grid(True)
+    plt.show()
+
+
+def main():
+    print(__file__ + " start!!")
+    #  test_trajectory_generate()
+    test_optimize_trajectory()
+
+
+if __name__ == '__main__':
+    main()

PathPlanning/StateLatticePlanner/motion_model.py

@@ -0,0 +1,82 @@
+import math
+import numpy as np
+import scipy.interpolate
+
+# motion parameter
+L = 1.0  # wheel base
+ds = 0.1  # course distanse
+v = 10.0 / 3.6  # velocity [m/s]
+
+
+class State:
+
+    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
+        self.x = x
+        self.y = y
+        self.yaw = yaw
+        self.v = v
+
+
+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 update(state, v, delta, dt, L):
+
+    state.v = v
+    state.x = state.x + state.v * math.cos(state.yaw) * dt
+    state.y = state.y + state.v * math.sin(state.yaw) * dt
+    state.yaw = state.yaw + state.v / L * math.tan(delta) * dt
+    state.yaw = pi_2_pi(state.yaw)
+
+    return state
+
+
+def generate_trajectory(s, km, kf, k0):
+
+    n = s / ds
+    time = s / v  # [s]
+    tk = np.array([0.0, time / 2.0, time])
+    kk = np.array([k0, km, kf])
+    t = np.arange(0.0, time, time / n)
+    kp = scipy.interpolate.spline(tk, kk, t, order=2)
+    dt = float(time / n)
+
+    #  plt.plot(t, kp)
+    #  plt.show()
+
+    state = State()
+    x, y, yaw = [state.x], [state.y], [state.yaw]
+
+    for ikp in kp:
+        state = update(state, v, ikp, dt, L)
+        x.append(state.x)
+        y.append(state.y)
+        yaw.append(state.yaw)
+
+    return x, y, yaw
+
+
+def generate_last_state(s, km, kf, k0):
+
+    n = s / ds
+    time = s / v  # [s]
+    tk = np.array([0.0, time / 2.0, time])
+    kk = np.array([k0, km, kf])
+    t = np.arange(0.0, time, time / n)
+    kp = scipy.interpolate.spline(tk, kk, t, order=2)
+    dt = time / n
+
+    #  plt.plot(t, kp)
+    #  plt.show()
+
+    state = State()
+
+    [update(state, v, ikp, dt, L) for ikp in kp]
+    return state.x, state.y, state.yaw

PathPlanning/StateLatticePlanner/state_lattice_planner.py

@@ -0,0 +1,137 @@
+"""
+State lattice planner
+
+author: Atsushi Sakai
+"""
+from matplotlib import pyplot as plt
+import numpy as np
+import math
+import model_predictive_trajectory_generator as planner
+import motion_model
+import pandas as pd
+
+
+def calc_states_list():
+    maxyaw = math.radians(-30.0)
+
+    x = np.arange(1.0, 30.0, 5.0)
+    y = np.arange(0.0, 20.0, 2.0)
+    yaw = np.arange(-maxyaw, maxyaw, maxyaw)
+
+    states = []
+    for iyaw in yaw:
+        for iy in y:
+            for ix in x:
+                states.append([ix, iy, iyaw])
+    # print(len(states))
+
+    return states
+
+
+def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookuptable):
+    mind = float("inf")
+    minid = -1
+
+    for (i, table) in enumerate(lookuptable):
+
+        dx = tx - table[0]
+        dy = ty - table[1]
+        dyaw = tyaw - table[2]
+        d = math.sqrt(dx ** 2 + dy ** 2 + dyaw ** 2)
+        if d <= mind:
+            minid = i
+            mind = d
+
+
+    return lookuptable[minid]
+
+
+def get_lookup_table():
+
+    data = pd.read_csv("lookuptable.csv")
+
+    return np.array(data)
+
+
+def generate_path(states, k0):
+
+    # x, y, yaw, s, km, kf
+    lookup_table = get_lookup_table()
+    result = []
+
+    for state in states:
+        bestp = search_nearest_one_from_lookuptable(
+            state[0], state[1], state[2], lookup_table)
+
+        target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
+        init_p = np.matrix(
+            [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).T
+
+        x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
+
+        if x is not None:
+            print("find good path")
+            result.append(
+                [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])])
+
+    print("finish path generation")
+    return result
+
+
+
+def calc_uniform_states():
+    np = 5
+    nh = 3
+    d = 20
+    a_min = - math.radians(45.0)
+    a_max = math.radians(45.0)
+    p_min = - math.radians(45.0)
+    p_max = math.radians(45.0)
+    x0 = 0.0
+    y0 = 0.0
+    yaw0 = 0.0
+
+    states = []
+
+    for i in range(np):
+        for j in range(nh):
+            n = i * nh + j
+            a = a_min + (a_max - a_min) * i / (np - 1)
+            xf = x0 + d * math.cos(a + yaw0)
+            yf = y0 + d * math.sin(a + yaw0)
+            yawf = yaw0+p_min+(p_max-p_min)*j/(nh-1)+a
+            states.append([xf, yf, yawf])
+
+    # print(states)
+    # print(len(states))
+
+    return states
+
+
+def uniform_terminal_state_sampling():
+    k0 = 0.0
+    states = calc_uniform_states()
+    result = generate_path(states, k0)
+
+    for table in result:
+        xc, yc, yawc = motion_model.generate_trajectory(
+            table[3], table[4], table[5], k0)
+        plt.plot(xc, yc, "-r")
+        xc, yc, yawc = motion_model.generate_trajectory(
+            table[3], -table[4], -table[5], k0)
+        plt.plot(xc, yc, "-r")
+
+    plt.grid(True)
+    plt.axis("equal")
+    plt.show()
+
+    print("Done")
+
+
+
+def main():
+    uniform_terminal_state_sampling()
+
+
+if __name__ == '__main__':
+    main()