diff --git a/.idea/dictionaries/atsushisakai.xml b/.idea/dictionaries/atsushisakai.xml new file mode 100644 index 00000000..d4608ae6 --- /dev/null +++ b/.idea/dictionaries/atsushisakai.xml @@ -0,0 +1,7 @@ + + + + atsushi + + + \ No newline at end of file diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py index 957b73c6..57597a9d 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py +++ b/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( diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/matplotrecorder b/PathPlanning/ModelPredictiveTrajectoryGenerator/matplotrecorder index 52d99328..c6f46592 160000 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/matplotrecorder +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/matplotrecorder @@ -1 +1 @@ -Subproject commit 52d99328ad6790adcd04ba4e242414eeacd85766 +Subproject commit c6f465922d6b3712e0023adc46872269f79452f5 diff --git a/PathPlanning/StateLatticePlanner/Figure_1.png b/PathPlanning/StateLatticePlanner/Figure_1.png new file mode 100644 index 00000000..259b2dfa Binary files /dev/null and b/PathPlanning/StateLatticePlanner/Figure_1.png differ diff --git a/PathPlanning/StateLatticePlanner/lookuptable.csv b/PathPlanning/StateLatticePlanner/lookuptable.csv new file mode 100644 index 00000000..85eff9b9 --- /dev/null +++ b/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 diff --git a/PathPlanning/StateLatticePlanner/matplotrecorder/.gitignore b/PathPlanning/StateLatticePlanner/matplotrecorder/.gitignore new file mode 100644 index 00000000..72364f99 --- /dev/null +++ b/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 diff --git a/PathPlanning/StateLatticePlanner/matplotrecorder/LICENSE b/PathPlanning/StateLatticePlanner/matplotrecorder/LICENSE new file mode 100644 index 00000000..a872dc68 --- /dev/null +++ b/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. diff --git a/PathPlanning/StateLatticePlanner/matplotrecorder/README.md b/PathPlanning/StateLatticePlanner/matplotrecorder/README.md new file mode 100644 index 00000000..39315fca --- /dev/null +++ b/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)) + diff --git a/PathPlanning/StateLatticePlanner/matplotrecorder/__init__.py b/PathPlanning/StateLatticePlanner/matplotrecorder/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/PathPlanning/StateLatticePlanner/matplotrecorder/animation.gif b/PathPlanning/StateLatticePlanner/matplotrecorder/animation.gif new file mode 100644 index 00000000..69d2df58 Binary files /dev/null and b/PathPlanning/StateLatticePlanner/matplotrecorder/animation.gif differ diff --git a/PathPlanning/StateLatticePlanner/matplotrecorder/animation.mp4 b/PathPlanning/StateLatticePlanner/matplotrecorder/animation.mp4 new file mode 100644 index 00000000..cdf4d376 Binary files /dev/null and b/PathPlanning/StateLatticePlanner/matplotrecorder/animation.mp4 differ diff --git a/PathPlanning/StateLatticePlanner/matplotrecorder/matplotrecorder.py b/PathPlanning/StateLatticePlanner/matplotrecorder/matplotrecorder.py new file mode 100644 index 00000000..701e4638 --- /dev/null +++ b/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) diff --git a/PathPlanning/StateLatticePlanner/model_predictive_trajectory_generator.py b/PathPlanning/StateLatticePlanner/model_predictive_trajectory_generator.py new file mode 100644 index 00000000..cdca7f7d --- /dev/null +++ b/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() diff --git a/PathPlanning/StateLatticePlanner/motion_model.py b/PathPlanning/StateLatticePlanner/motion_model.py new file mode 100644 index 00000000..b31d7319 --- /dev/null +++ b/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 diff --git a/PathPlanning/StateLatticePlanner/state_lattice_planner.py b/PathPlanning/StateLatticePlanner/state_lattice_planner.py new file mode 100644 index 00000000..88f60cff --- /dev/null +++ b/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()