github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 21:28:33 -05:00
Code Issues Packages Projects Releases Wiki Activity

Remove pandas dependency (#725)

Browse Source 
* Remove pandas dependency

* Remove pandas dependency
This commit is contained in:
Atsushi Sakai
2022-09-30 09:14:27 +09:00
committed by GitHub
parent e40b4d9dec
commit 955ad60ee3
9 changed files with 226 additions and 173 deletions
Download Patch File Download Diff File Expand all files Collapse all files

110
PathPlanning/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py Normal file
View File

@@ -0,0 +1,110 @@
"""
Lookup Table generation for model predictive trajectory generator
author: Atsushi Sakai
"""
import sys
import pathlib
path_planning_dir = pathlib.Path(__file__).parent.parent
sys.path.append(str(path_planning_dir))
from matplotlib import pyplot as plt
import numpy as np
import math
from ModelPredictiveTrajectoryGenerator import trajectory_generator,\
motion_model
def calc_states_list(max_yaw=np.deg2rad(-30.0)):
x = np.arange(10.0, 30.0, 5.0)
y = np.arange(0.0, 20.0, 2.0)
yaw = np.arange(-max_yaw, max_yaw, max_yaw)
states = []
for iyaw in yaw:
for iy in y:
for ix in x:
states.append([ix, iy, iyaw])
print("n_state:", len(states))
return states
def search_nearest_one_from_lookup_table(tx, ty, tyaw, lookup_table):
mind = float("inf")
minid = -1
for (i, table) in enumerate(lookup_table):
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
# print(minid)
return lookup_table[minid]
def save_lookup_table(file_name, table):
np.savetxt(file_name, np.array(table),
fmt='%s', delimiter=",", header="x,y,yaw,s,km,kf", comments="")
print("lookup table file is saved as " + file_name)
def generate_lookup_table():
states = calc_states_list(max_yaw=np.deg2rad(-30.0))
k0 = 0.0
# x, y, yaw, s, km, kf
lookup_table = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]]
for state in states:
best_p = search_nearest_one_from_lookup_table(
state[0], state[1], state[2], lookup_table)
target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
init_p = np.array(
[np.hypot(state[0], state[1]), best_p[4], best_p[5]]).reshape(3, 1)
x, y, yaw, p = trajectory_generator.optimize_trajectory(target,
k0, init_p)
if x is not None:
print("find good path")
lookup_table.append(
[x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])])
print("finish lookup table generation")
save_lookup_table("lookup_table.csv", lookup_table)
for table in lookup_table:
x_c, y_c, yaw_c = motion_model.generate_trajectory(
table[3], table[4], table[5], k0)
plt.plot(x_c, y_c, "-r")
x_c, y_c, yaw_c = motion_model.generate_trajectory(
table[3], -table[4], -table[5], k0)
plt.plot(x_c, y_c, "-r")
plt.grid(True)
plt.axis("equal")
plt.show()
print("Done")
def main():
generate_lookup_table()
if __name__ == '__main__':
main()

114
PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py
View File

@@ -1,114 +0,0 @@
"""
Lookup Table generation for model predictive trajectory generator
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 = np.deg2rad(-30.0)
x = np.arange(10.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("nstate:", 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
# print(minid)
return lookuptable[minid]
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
# x, y, yaw, s, km, kf
lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]]
for state in states:
bestp = search_nearest_one_from_lookuptable(
state[0], state[1], state[2], lookuptable)
target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
init_p = np.array(
[math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).reshape(3, 1)
x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
if x is not None:
print("find good path")
lookuptable.append(
[x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])])
print("finish lookup table generation")
save_lookup_table("lookuptable.csv", lookuptable)
for table in lookuptable:
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():
generate_lookup_table()
if __name__ == '__main__':
main()

82
PathPlanning/StateLatticePlanner/lookup_table.csv Normal file
View File

@@ -0,0 +1,82 @@
x,y,yaw,s,km,kf
1.0,0.0,0.0,1.0,0.0,0.0
9.975352133559392,0.0482183513545736,0.5660837104214496,10.00000000000002,0.0015736624607596847,0.31729782170754367
15.021899857204536,0.023109001221800096,0.541061424167431,15.128443053611093,0.0006480950273134919,0.20847475849103875
20.062147834064536,0.0406648159648112,0.5374967866814861,20.205553097986094,0.000452860235044122,0.15502921850050788
24.924468605496358,-0.04047324014767662,0.5146575311501209,25.16775431470035,6.940620839146646e-05,0.12259452810198132
9.971782095506931,1.9821448683146543,0.5206511572266477,10.287478424823748,0.05861430948618236,0.07036494964262185
15.00170010872385,2.0003864283110473,0.5236741185892617,15.245993376540827,0.02657730439557895,0.09933479864250763
19.991716537639487,1.9940629519465154,0.5226444441451559,20.277923997037238,0.015108540596275507,0.09478988637993524
24.946447973869596,2.0815930190993206,0.5306354765686239,25.20115925294013,0.010036251429787917,0.08505936469481987
10.033694822745312,3.9724800521928056,0.5349578864544497,11.087694168363686,0.10279429366023954,-0.12501011715795404
15.010712586144749,4.0046776414868095,0.5234972445048012,15.729582155835587,0.05010930398580602,-0.0008557723200034717
19.9175798257299,4.053680042954521,0.5265397234296523,20.52466275843717,0.029584390559990882,0.035276591227371874
24.98769016158626,3.991699950598091,0.5229000018897194,25.543297770996556,0.018800715817231053,0.04750751098144048
10.018665105170687,6.004814533505462,0.5183921334245007,12.249438857228967,0.13207408248643182,-0.2742892277307502
14.988626131330372,5.991226410357179,0.5248160422552801,16.53593823576699,0.06924423592936522,-0.08634227857486092
20.014420271646646,6.006767110727591,0.5233060851224174,21.23271362632659,0.041402041787912916,-0.01770377839603589
24.998338724667267,5.997352722087869,0.5235621854299422,26.009046544833613,0.027285850882345728,0.011507045054418165
10.040118020822444,8.017131894913126,0.5076867575242261,13.8061261785273,0.14561700178565884,-0.3527538468214878
14.96397914886416,7.974972375534203,0.5303378183744862,17.667338175004062,0.08318912494381935,-0.15372672981944802
20.045725182938817,8.023486945646207,0.5201839069343577,22.126364299043573,0.05173969669894265,-0.06557547083017647
25.004687466358227,8.00036398460779,0.5234938146870878,26.740089158520917,0.034867425244601645,-0.02199309906456302
10.065138949829214,10.03244363616002,0.49375882493214895,15.701940360254637,0.14847998727328912,-0.39355037236614626
15.05373212031198,10.026401491912143,0.5111826366036252,19.105461052226858,0.09205576730549936,-0.20458802229704312
19.965550451103926,9.977668905006206,0.5278605653376056,23.14082140870299,0.06010674632014157,-0.10340577652521214
25.04062496016141,9.956781577401756,0.5252395961316738,27.641194908523495,0.04115083532669924,-0.05054407239730677
9.980172344087242,11.981944953180841,0.5354924711458446,17.764377273124804,0.14616069587267325,-0.40115138946106776
15.031707905116134,12.011530784459552,0.5157261778129998,20.745000892047745,0.0970285785481706,-0.2379719980195133
20.05384921212195,12.02621662711961,0.5153884987125119,24.513013940487117,0.06601543941341544,-0.13666530932375262
25.04326204059146,12.019946808479768,0.5198699857547844,28.74306622689766,0.04671545692054678,-0.07827401225777673
10.005005976167096,13.993516346269931,0.5249997047973469,20.063732124124442,0.14007166951362482,-0.39994549637994103
15.013469777117386,13.998572375088138,0.5211760827701193,22.591299061495683,0.0989196134377572,-0.25909446951756165
19.980150236409695,13.98233838451409,0.5278966095736082,25.971685915503254,0.07029833263412807,-0.15993299513197096
25.009669110020404,14.000751236643762,0.5227555344229664,29.949071374991423,0.05106114063333748,-0.09952052168406796
9.996712859814979,15.986637217372996,0.5301458018536311,22.47478825250167,0.1329741433122606,-0.38823042580907063
15.02373126750475,16.00384009060484,0.5182833077580984,24.557463511123004,0.0989491582793761,-0.26836010532851323
20.023756339113735,16.004847752803656,0.5197401980953318,27.669260302891157,0.07275720314277462,-0.178811991371391
25.015003771679122,16.002919774604504,0.5219791758565742,31.36524983655211,0.05429827198598215,-0.11766440355003502
10.078596822781892,18.025309925487992,0.49768408992179225,25.02580432036455,0.1252233187334947,-0.3747545825918585
15.001968473293188,17.988033772017467,0.5262415135796346,26.67625473617623,0.09746306394892065,-0.27167606206451594
20.026047062413117,18.00445752595148,0.5193568548054093,29.442158339897595,0.07417227896231118,-0.19015828496001386
24.984711558393403,17.982744830235063,0.5266809346220684,32.855828700083094,0.05675308229799072,-0.13090299334069386
9.999999973554228,8.906672256498078e-05,-0.00024912926939091307,9.993250237275609,1.9794429093204823e-06,-0.00016167063578544257
14.999999988951053,0.00030609885737583985,-9.259737492950393e-05,14.939586274030715,4.066161982234725e-06,-5.3230908443270726e-05
19.999999963637627,0.0008196433029304879,-0.00010277758318455454,19.985770355960977,6.0902800817987275e-06,-5.581407356116362e-05
24.999999906323,0.001558015443394581,-0.0001252423879458675,24.925430653319882,7.508303551937611e-06,-5.98269885073166e-05
9.93201732790474,1.9700581591656137,-0.006606314895513332,10.1625049701131,0.05795554613825405,-0.23594780459354886
15.017121844754504,2.000131018972639,-0.001958259181754851,15.130494387563031,0.026367577418638183,-0.10529363184139814
19.962697589600058,2.0003823634817484,0.0021983556339688626,20.055058569558643,0.014972854970102445,-0.0592998512022201
24.990093248087035,2.0008914594513647,0.0003319006512292333,25.020899019312747,0.009609456446194334,-0.03808543941908436
9.942924437331126,3.9434423219621073,-0.047789349898090805,10.916318098481405,0.10417074854184473,-0.42509733550937057
14.976393375378994,3.9987876606083557,0.004653465622298736,15.69826778341493,0.04981535482126709,-0.20027162173052074
19.954160472557877,4.000101578371634,0.0053292950039418585,20.459066225465484,0.02905576509783228,-0.11479451096219842
25.06247590490118,3.9997579161047643,-0.00486183691237807,25.40723367563786,0.01874893916371208,-0.07533000027879669
9.974854017566281,5.998183884411291,0.01394025812352817,12.27808815775426,0.13163310345287574,-0.5111693653344966
14.99829771854157,5.999020207860274,0.0007330116466723879,16.57520987140955,0.06880393034208837,-0.27508456151767885
19.98389776689381,5.999506195067484,0.002770060727207646,21.17690590277397,0.04131547230609369,-0.1652252863196287
25.022089217041394,5.998166050230614,-0.002551136444779001,25.974625009044832,0.02718132258204399,-0.10978755041013998
9.940106683734614,7.99448983539684,0.03735909486314526,13.864600506318645,0.14554135993596395,-0.5498471044599721
15.015405965817797,7.996301502316838,-0.004430455799697253,17.779484729664652,0.08234534796805798,-0.3300198333333338
19.965919061860355,7.998456498324741,0.00732927315681664,22.0665101267907,0.05178054118886435,-0.20507088323830897
24.97580637673196,7.998036396987909,0.0034859866489540536,26.699711792661176,0.03478260921646504,-0.13959734880932403
10.003237328881212,9.994037173180942,-0.002542633641336778,15.800576175296408,0.1482242831571022,-0.5606578442626601
14.95848212484301,9.995827033229693,0.016804720248816185,19.19635868417634,0.09159937492256161,-0.3610497877526804
20.018365340632464,9.997789133099982,-0.003880405312526758,23.259977677838524,0.05967179836565363,-0.23873172503708404
25.034844162753302,9.996613275552045,-0.005490232481425661,27.647073656497884,0.04122997694830456,-0.16548182742762063
10.041413516307436,11.988808245039152,-0.015743247245750158,18.0174427655263,0.14424296158815444,-0.5545987939832356
15.0710608536628,11.993636485613393,-0.025235844052727163,20.92474299071291,0.0960774359909814,-0.38199459745149106
20.061838597733104,11.995243972143648,-0.015325438311212025,24.63090823780847,0.06556771814265559,-0.2626353022718591
24.90813949494271,11.995929681233529,0.01760171116909426,28.6986397040137,0.046810556161518815,-0.1847353186190147
10.005191819464756,13.97797567430312,0.018961636911005275,20.358534835690133,0.13825179056925302,-0.5307789523538471
14.978392340358946,13.991362718235834,0.012411272386128935,22.755419658274054,0.0984622955030996,-0.38447788120958937
20.015767113356507,13.992558840024987,-0.002205036951612893,26.18420998778461,0.06961025144239422,-0.2786494668163888
25.01318440442437,13.994258255793202,-0.0016239998449329995,30.09124393513656,0.05071043613803722,-0.20387658283659768
10.038844117562423,15.966797017942504,0.016384527088525225,22.88736140380268,0.13044436631301143,-0.5070826347325453
14.91898245890566,15.984279670640529,0.03784081306841358,24.796728185207627,0.09830913950807817,-0.38207974071854045
19.999487117727806,15.99041117221354,0.0034823225688951354,27.881676426972927,0.07220430103629995,-0.2873083396987492
25.056418472201756,15.995103453935709,-0.011257522827095023,31.50238694595278,0.05406499488342877,-0.21526296035737832
10.076107447676621,17.952889979512353,0.017798231103724138,25.454959881832874,0.1231232463335769,-0.47600174850950705
15.032725028551983,17.978015286760307,0.0020752804670070013,27.089888269358894,0.09590219542773218,-0.3801465515462427
20.03544756240551,17.98685790169768,-0.005300968094156033,29.75070206477736,0.07340450527104486,-0.29182757725382324
24.960019173190652,17.98909417109214,0.011594018486178026,33.0995680641525,0.05634561447882407,-0.22402297280749597
1 x y yaw s km kf
2 1.0 0.0 0.0 1.0 0.0 0.0
3 9.975352133559392 0.0482183513545736 0.5660837104214496 10.00000000000002 0.0015736624607596847 0.31729782170754367
4 15.021899857204536 0.023109001221800096 0.541061424167431 15.128443053611093 0.0006480950273134919 0.20847475849103875
5 20.062147834064536 0.0406648159648112 0.5374967866814861 20.205553097986094 0.000452860235044122 0.15502921850050788
6 24.924468605496358 -0.04047324014767662 0.5146575311501209 25.16775431470035 6.940620839146646e-05 0.12259452810198132
7 9.971782095506931 1.9821448683146543 0.5206511572266477 10.287478424823748 0.05861430948618236 0.07036494964262185
8 15.00170010872385 2.0003864283110473 0.5236741185892617 15.245993376540827 0.02657730439557895 0.09933479864250763
9 19.991716537639487 1.9940629519465154 0.5226444441451559 20.277923997037238 0.015108540596275507 0.09478988637993524
10 24.946447973869596 2.0815930190993206 0.5306354765686239 25.20115925294013 0.010036251429787917 0.08505936469481987
11 10.033694822745312 3.9724800521928056 0.5349578864544497 11.087694168363686 0.10279429366023954 -0.12501011715795404
12 15.010712586144749 4.0046776414868095 0.5234972445048012 15.729582155835587 0.05010930398580602 -0.0008557723200034717
13 19.9175798257299 4.053680042954521 0.5265397234296523 20.52466275843717 0.029584390559990882 0.035276591227371874
14 24.98769016158626 3.991699950598091 0.5229000018897194 25.543297770996556 0.018800715817231053 0.04750751098144048
15 10.018665105170687 6.004814533505462 0.5183921334245007 12.249438857228967 0.13207408248643182 -0.2742892277307502
16 14.988626131330372 5.991226410357179 0.5248160422552801 16.53593823576699 0.06924423592936522 -0.08634227857486092
17 20.014420271646646 6.006767110727591 0.5233060851224174 21.23271362632659 0.041402041787912916 -0.01770377839603589
18 24.998338724667267 5.997352722087869 0.5235621854299422 26.009046544833613 0.027285850882345728 0.011507045054418165
19 10.040118020822444 8.017131894913126 0.5076867575242261 13.8061261785273 0.14561700178565884 -0.3527538468214878
20 14.96397914886416 7.974972375534203 0.5303378183744862 17.667338175004062 0.08318912494381935 -0.15372672981944802
21 20.045725182938817 8.023486945646207 0.5201839069343577 22.126364299043573 0.05173969669894265 -0.06557547083017647
22 25.004687466358227 8.00036398460779 0.5234938146870878 26.740089158520917 0.034867425244601645 -0.02199309906456302
23 10.065138949829214 10.03244363616002 0.49375882493214895 15.701940360254637 0.14847998727328912 -0.39355037236614626
24 15.05373212031198 10.026401491912143 0.5111826366036252 19.105461052226858 0.09205576730549936 -0.20458802229704312
25 19.965550451103926 9.977668905006206 0.5278605653376056 23.14082140870299 0.06010674632014157 -0.10340577652521214
26 25.04062496016141 9.956781577401756 0.5252395961316738 27.641194908523495 0.04115083532669924 -0.05054407239730677
27 9.980172344087242 11.981944953180841 0.5354924711458446 17.764377273124804 0.14616069587267325 -0.40115138946106776
28 15.031707905116134 12.011530784459552 0.5157261778129998 20.745000892047745 0.0970285785481706 -0.2379719980195133
29 20.05384921212195 12.02621662711961 0.5153884987125119 24.513013940487117 0.06601543941341544 -0.13666530932375262
30 25.04326204059146 12.019946808479768 0.5198699857547844 28.74306622689766 0.04671545692054678 -0.07827401225777673
31 10.005005976167096 13.993516346269931 0.5249997047973469 20.063732124124442 0.14007166951362482 -0.39994549637994103
32 15.013469777117386 13.998572375088138 0.5211760827701193 22.591299061495683 0.0989196134377572 -0.25909446951756165
33 19.980150236409695 13.98233838451409 0.5278966095736082 25.971685915503254 0.07029833263412807 -0.15993299513197096
34 25.009669110020404 14.000751236643762 0.5227555344229664 29.949071374991423 0.05106114063333748 -0.09952052168406796
35 9.996712859814979 15.986637217372996 0.5301458018536311 22.47478825250167 0.1329741433122606 -0.38823042580907063
36 15.02373126750475 16.00384009060484 0.5182833077580984 24.557463511123004 0.0989491582793761 -0.26836010532851323
37 20.023756339113735 16.004847752803656 0.5197401980953318 27.669260302891157 0.07275720314277462 -0.178811991371391
38 25.015003771679122 16.002919774604504 0.5219791758565742 31.36524983655211 0.05429827198598215 -0.11766440355003502
39 10.078596822781892 18.025309925487992 0.49768408992179225 25.02580432036455 0.1252233187334947 -0.3747545825918585
40 15.001968473293188 17.988033772017467 0.5262415135796346 26.67625473617623 0.09746306394892065 -0.27167606206451594
41 20.026047062413117 18.00445752595148 0.5193568548054093 29.442158339897595 0.07417227896231118 -0.19015828496001386
42 24.984711558393403 17.982744830235063 0.5266809346220684 32.855828700083094 0.05675308229799072 -0.13090299334069386
43 9.999999973554228 8.906672256498078e-05 -0.00024912926939091307 9.993250237275609 1.9794429093204823e-06 -0.00016167063578544257
44 14.999999988951053 0.00030609885737583985 -9.259737492950393e-05 14.939586274030715 4.066161982234725e-06 -5.3230908443270726e-05
45 19.999999963637627 0.0008196433029304879 -0.00010277758318455454 19.985770355960977 6.0902800817987275e-06 -5.581407356116362e-05
46 24.999999906323 0.001558015443394581 -0.0001252423879458675 24.925430653319882 7.508303551937611e-06 -5.98269885073166e-05
47 9.93201732790474 1.9700581591656137 -0.006606314895513332 10.1625049701131 0.05795554613825405 -0.23594780459354886
48 15.017121844754504 2.000131018972639 -0.001958259181754851 15.130494387563031 0.026367577418638183 -0.10529363184139814
49 19.962697589600058 2.0003823634817484 0.0021983556339688626 20.055058569558643 0.014972854970102445 -0.0592998512022201
50 24.990093248087035 2.0008914594513647 0.0003319006512292333 25.020899019312747 0.009609456446194334 -0.03808543941908436
51 9.942924437331126 3.9434423219621073 -0.047789349898090805 10.916318098481405 0.10417074854184473 -0.42509733550937057
52 14.976393375378994 3.9987876606083557 0.004653465622298736 15.69826778341493 0.04981535482126709 -0.20027162173052074
53 19.954160472557877 4.000101578371634 0.0053292950039418585 20.459066225465484 0.02905576509783228 -0.11479451096219842
54 25.06247590490118 3.9997579161047643 -0.00486183691237807 25.40723367563786 0.01874893916371208 -0.07533000027879669
55 9.974854017566281 5.998183884411291 0.01394025812352817 12.27808815775426 0.13163310345287574 -0.5111693653344966
56 14.99829771854157 5.999020207860274 0.0007330116466723879 16.57520987140955 0.06880393034208837 -0.27508456151767885
57 19.98389776689381 5.999506195067484 0.002770060727207646 21.17690590277397 0.04131547230609369 -0.1652252863196287
58 25.022089217041394 5.998166050230614 -0.002551136444779001 25.974625009044832 0.02718132258204399 -0.10978755041013998
59 9.940106683734614 7.99448983539684 0.03735909486314526 13.864600506318645 0.14554135993596395 -0.5498471044599721
60 15.015405965817797 7.996301502316838 -0.004430455799697253 17.779484729664652 0.08234534796805798 -0.3300198333333338
61 19.965919061860355 7.998456498324741 0.00732927315681664 22.0665101267907 0.05178054118886435 -0.20507088323830897
62 24.97580637673196 7.998036396987909 0.0034859866489540536 26.699711792661176 0.03478260921646504 -0.13959734880932403
63 10.003237328881212 9.994037173180942 -0.002542633641336778 15.800576175296408 0.1482242831571022 -0.5606578442626601
64 14.95848212484301 9.995827033229693 0.016804720248816185 19.19635868417634 0.09159937492256161 -0.3610497877526804
65 20.018365340632464 9.997789133099982 -0.003880405312526758 23.259977677838524 0.05967179836565363 -0.23873172503708404
66 25.034844162753302 9.996613275552045 -0.005490232481425661 27.647073656497884 0.04122997694830456 -0.16548182742762063
67 10.041413516307436 11.988808245039152 -0.015743247245750158 18.0174427655263 0.14424296158815444 -0.5545987939832356
68 15.0710608536628 11.993636485613393 -0.025235844052727163 20.92474299071291 0.0960774359909814 -0.38199459745149106
69 20.061838597733104 11.995243972143648 -0.015325438311212025 24.63090823780847 0.06556771814265559 -0.2626353022718591
70 24.90813949494271 11.995929681233529 0.01760171116909426 28.6986397040137 0.046810556161518815 -0.1847353186190147
71 10.005191819464756 13.97797567430312 0.018961636911005275 20.358534835690133 0.13825179056925302 -0.5307789523538471
72 14.978392340358946 13.991362718235834 0.012411272386128935 22.755419658274054 0.0984622955030996 -0.38447788120958937
73 20.015767113356507 13.992558840024987 -0.002205036951612893 26.18420998778461 0.06961025144239422 -0.2786494668163888
74 25.01318440442437 13.994258255793202 -0.0016239998449329995 30.09124393513656 0.05071043613803722 -0.20387658283659768
75 10.038844117562423 15.966797017942504 0.016384527088525225 22.88736140380268 0.13044436631301143 -0.5070826347325453
76 14.91898245890566 15.984279670640529 0.03784081306841358 24.796728185207627 0.09830913950807817 -0.38207974071854045
77 19.999487117727806 15.99041117221354 0.0034823225688951354 27.881676426972927 0.07220430103629995 -0.2873083396987492
78 25.056418472201756 15.995103453935709 -0.011257522827095023 31.50238694595278 0.05406499488342877 -0.21526296035737832
79 10.076107447676621 17.952889979512353 0.017798231103724138 25.454959881832874 0.1231232463335769 -0.47600174850950705
80 15.032725028551983 17.978015286760307 0.0020752804670070013 27.089888269358894 0.09590219542773218 -0.3801465515462427
81 20.03544756240551 17.98685790169768 -0.005300968094156033 29.75070206477736 0.07340450527104486 -0.29182757725382324
82 24.960019173190652 17.98909417109214 0.011594018486178026 33.0995680641525 0.05634561447882407 -0.22402297280749597

25
PathPlanning/StateLatticePlanner/lookuptable.csv
View File

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

62
PathPlanning/StateLatticePlanner/state_lattice_planner.py
View File

@@ -4,9 +4,9 @@ State lattice planner with model predictive trajectory generator
author: Atsushi Sakai (@Atsushi_twi)
- lookuptable.csv is generated with this script:
- plookuptable.csv is generated with this script:
https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning
/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py
/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py
Ref:
@@ -21,27 +21,25 @@ import os
from matplotlib import pyplot as plt
import numpy as np
import math
import pandas as pd
import pathlib
sys.path.append(str(pathlib.Path(__file__).parent.parent))
import ModelPredictiveTrajectoryGenerator.trajectory_generator as planner
import ModelPredictiveTrajectoryGenerator.motion_model as motion_model
table_path = os.path.dirname(os.path.abspath(__file__)) + "/lookuptable.csv"
TABLE_PATH = os.path.dirname(os.path.abspath(__file__)) + "/lookup_table.csv"
show_animation = True
def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookup_table):
def search_nearest_one_from_lookup_table(t_x, t_y, t_yaw, lookup_table):
mind = float("inf")
minid = -1
for (i, table) in enumerate(lookup_table):
dx = tx - table[0]
dy = ty - table[1]
dyaw = tyaw - table[2]
dx = t_x - table[0]
dy = t_y - table[1]
dyaw = t_yaw - table[2]
d = math.sqrt(dx ** 2 + dy ** 2 + dyaw ** 2)
if d <= mind:
minid = i
@@ -50,24 +48,22 @@ def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookup_table):
return lookup_table[minid]
def get_lookup_table():
data = pd.read_csv(table_path)
return np.array(data)
def get_lookup_table(table_path):
return np.loadtxt(table_path, delimiter=',', skiprows=1)
def generate_path(target_states, k0):
# x, y, yaw, s, km, kf
lookup_table = get_lookup_table()
lookup_table = get_lookup_table(TABLE_PATH)
result = []
for state in target_states:
bestp = search_nearest_one_from_lookuptable(
bestp = search_nearest_one_from_lookup_table(
state[0], state[1], state[2], lookup_table)
target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
init_p = np.array(
[math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).reshape(3, 1)
[np.hypot(state[0], state[1]), bestp[4], bestp[5]]).reshape(3, 1)
x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
@@ -82,18 +78,28 @@ def generate_path(target_states, k0):
def calc_uniform_polar_states(nxy, nh, d, a_min, a_max, p_min, p_max):
"""
calc uniform state
:param nxy: number of position sampling
:param nh: number of heading sampleing
:param d: distance of terminal state
:param a_min: position sampling min angle
:param a_max: position sampling max angle
:param p_min: heading sampling min angle
:param p_max: heading sampling max angle
:return: states list
Parameters
----------
nxy :
number of position sampling
nh :
number of heading sampleing
d :
distance of terminal state
a_min :
position sampling min angle
a_max :
position sampling max angle
p_min :
heading sampling min angle
p_max :
heading sampling max angle
Returns
-------
"""
angle_samples = [i / (nxy - 1) for i in range(nxy)]
states = sample_states(angle_samples, a_min, a_max, d, p_max, p_min, nh)
@@ -312,11 +318,11 @@ def lane_state_sampling_test1():
plt.close("all")
for table in result:
xc, yc, yawc = motion_model.generate_trajectory(
x_c, y_c, yaw_c = motion_model.generate_trajectory(
table[3], table[4], table[5], k0)
if show_animation:
plt.plot(xc, yc, "-r")
plt.plot(x_c, y_c, "-r")
if show_animation:
plt.grid(True)

2
README.md
View File

@@ -103,8 +103,6 @@ For running each sample code:
- [Matplotlib](https://matplotlib.org/)
- [pandas](https://pandas.pydata.org/)
- [cvxpy](https://www.cvxpy.org/)
For development:

2
docs/getting_started_main.rst
View File

@@ -30,7 +30,6 @@ Requirements
- `NumPy`_
- `SciPy`_
- `Matplotlib`_
- `pandas`_
- `cvxpy`_
For development:
@@ -45,7 +44,6 @@ For development:
.. _`NumPy`: https://numpy.org/
.. _`SciPy`: https://scipy.org/
.. _`Matplotlib`: https://matplotlib.org/
.. _`pandas`: https://pandas.pydata.org/
.. _`cvxpy`: https://www.cvxpy.org/

1
requirements/environment.yml
View File

@@ -6,6 +6,5 @@ dependencies:
- pip
- scipy
- numpy
- pandas
- cvxpy
- matplotlib

1
requirements/requirements.txt
View File

@@ -1,6 +1,5 @@
numpy == 1.23.3
scipy == 1.9.1
pandas == 1.4.4
matplotlib == 3.5.3
cvxpy == 1.2.1
pytest == 7.1.3 # For unit test