Skip to content

Commit 00d5695

Browse files
committed
Test lookup table, add its results.
1 parent 197dafa commit 00d5695

File tree

8 files changed

+350
-15
lines changed

8 files changed

+350
-15
lines changed

PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py

Lines changed: 22 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,8 @@
22
Lookup Table generation for model predictive trajectory generator
33
44
author: Atsushi Sakai
5+
6+
The initial state is assumed to be (0, 0, 0) in this program. Other initial states can be rotated and translated to (0, 0, 0).
57
"""
68
from matplotlib import pyplot as plt
79
import numpy as np
@@ -37,7 +39,7 @@ def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookuptable):
3739
dx = tx - table[0]
3840
dy = ty - table[1]
3941
dyaw = tyaw - table[2]
40-
d = math.sqrt(dx ** 2 + dy ** 2 + dyaw ** 2)
42+
d = math.sqrt(dx ** 2 + dy ** 2 + dyaw ** 2) # Euclidean norm
4143
if d <= mind:
4244
minid = i
4345
mind = d
@@ -49,7 +51,7 @@ def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookuptable):
4951

5052
def save_lookup_table(fname, table):
5153
mt = np.array(table)
52-
print(mt)
54+
# print(mt)
5355
# save csv
5456
df = pd.DataFrame()
5557
df["x"] = mt[:, 0]
@@ -58,25 +60,30 @@ def save_lookup_table(fname, table):
5860
df["s"] = mt[:, 3]
5961
df["km"] = mt[:, 4]
6062
df["kf"] = mt[:, 5]
61-
df.to_csv(fname, index=None)
63+
# df.to_csv(fname, index=None)
64+
df.to_csv(fname)
6265

6366
print("lookup table file is saved as " + fname)
6467

6568

6669
def generate_lookup_table():
67-
states = calc_states_list()
70+
states = calc_states_list() # calculate target states
6871
k0 = 0.0
6972

70-
# x, y, yaw, s, km, kf
73+
# target_x, target_y, target_yaw -> s, km, kf
7174
lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]]
7275

73-
for state in states:
76+
print("Lookup Table is being generated......")
77+
78+
for idx, state in enumerate(states):
7479
bestp = search_nearest_one_from_lookuptable(
7580
state[0], state[1], state[2], lookuptable)
7681

7782
target = motion_model.State(x=state[0], y=state[1], yaw=state[2])
83+
# init_p = np.matrix(
84+
# [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).T
7885
init_p = np.matrix(
79-
[math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).T
86+
[bestp[3], bestp[4], bestp[5]]).T
8087

8188
x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
8289

@@ -85,6 +92,10 @@ def generate_lookup_table():
8592
lookuptable.append(
8693
[x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])])
8794

95+
## print percent of progress
96+
percent = 100 * (idx + 1.0) / len(states)
97+
print("Complete %{}...".format(percent))
98+
8899
print("finish lookup table generation")
89100

90101
save_lookup_table("lookuptable.csv", lookuptable)
@@ -94,7 +105,10 @@ def generate_lookup_table():
94105
table[3], table[4], table[5], k0)
95106
plt.plot(xc, yc, "-r")
96107
xc, yc, yawc = motion_model.generate_trajectory(
97-
table[3], -table[4], -table[5], k0)
108+
table[3], -table[4], -table[5], k0) # symmetrical, this is why target_yaw inlcude only -30 degree and exclude +30 degree
109+
# similar for target_y, note that not for target_x
110+
# also note that here change the sign of steering make the sign of target_yaw
111+
# and target_y change at the same time
98112
plt.plot(xc, yc, "-r")
99113

100114
plt.grid(True)

PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py

Lines changed: 15 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -3,21 +3,30 @@
33
44
author: Atsushi Sakai
55
6+
This sample program doesn't optimize the cost function in the original paper.
7+
8+
The initial state is assumed to be (0, 0, 0) in this program. Other initial states can be rotated and translated to (0, 0, 0).
9+
610
the design of p can be improved, only two steer, v is connstant, no acceleration and deacceleration
711
812
Jacobian: the calculation of Jacobian may be not right? (if steer limit is small, can't optimize!? or cost threshold too small?),
9-
Jacobian: up and down on the starting of optimization is also a problem
13+
Jacobian: up and down at the starting of optimization is also a problem
1014
Jacobian: what is the optimization's theory?
1115
Jacobian: if the initial path is too long, can't optimize!?
16+
Jacobian: what we are optimizing?
17+
Jacobian: maybe local minimum cause the failure of optimization?
18+
Jacobian: too close target also doesn't work, of cource this is partly related to the amplitude of steering, but the process is not too right
1219
"""
1320

1421
import os
1522
import sys
16-
import warnings
1723
import numpy as np
1824
import matplotlib.pyplot as plt
1925
import math
2026
import motion_model
27+
from colorama import init as clr_ama_init
28+
from colorama import Fore
29+
clr_ama_init(autoreset = True) # anto reset to default color if color not set
2130

2231
## get directory of matplotrecorder
2332
github_root = os.path.join(os.path.dirname(__file__), '../../../')
@@ -31,7 +40,7 @@
3140
cost_th = 0.1
3241

3342
matplotrecorder.DO_NOTHING = True
34-
show_graph = True
43+
show_graph = False
3544

3645
def limitP(p, cfg):
3746
p[1, 0] = np.clip(p[1, 0], cfg.min_steer, cfg.max_steer)
@@ -149,7 +158,7 @@ def optimize_trajectory(target, k0, p):
149158
dp = limitDp(dp, mcfg, p)
150159
except np.linalg.linalg.LinAlgError:
151160
## optimize fail
152-
warnings.warn("cannot calc path LinAlgError")
161+
print(Fore.YELLOW + "cannot calc path LinAlgError")
153162
xc, yc, yawc, p = None, None, None, None
154163
break
155164
alpha = selection_learning_param(dp, p, k0, target) # choose learning rate
@@ -164,7 +173,7 @@ def optimize_trajectory(target, k0, p):
164173
## optimize fail
165174
## if no break, enter here
166175
xc, yc, yawc, p = None, None, None, None
167-
warnings.warn("cannot calc path")
176+
print(Fore.YELLOW + "cannot calc path")
168177

169178
return xc, yc, yawc, p
170179

@@ -173,7 +182,7 @@ def test_optimize_trajectory():
173182
mcfg = motion_model.ModelConfig()
174183

175184
# target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(00.0))
176-
target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(0.0))
185+
target = motion_model.State(x=1.0, y=0.0, yaw=math.radians(30.0))
177186
k0 = 0.0
178187

179188
init_p_len = math.sqrt(target.x**2 + target.y**2)

PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ def update(state, v, delta, dt, L):
3838

3939
## Ackermann model
4040
## limit amplitude
41-
## TODO: outside also need amplitude limiting?
41+
## TODO: actual path length isn't equal to path length (s or p[0]) in parameterized control sequence (p)?
4242

4343
model_cfg = ModelConfig()
4444
delta = np.clip(delta, model_cfg.min_steer, model_cfg.max_steer) # steering min and max [rad]
Lines changed: 104 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,104 @@
1+
x,y,yaw,s,km,kf
2+
1.0,0.0,0.0,1.0,0.0,0.0
3+
5.934183915194526,0.005405611332146738,0.5514520266182511,6.0,0.004365827717049513,0.5324282429269979
4+
10.969274192116142,0.03216071121115148,0.5493819212957336,11.000661862845583,0.0009399497626589406,0.28364246593382375
5+
15.94579103502118,0.03555018800694434,0.48807487892997536,16.007140502807186,0.0005655371642669476,0.1767655799081687
6+
20.95416262014933,0.046955608322188876,0.534339975385096,21.100379687851962,0.00044513207975985827,0.1474577637627991
7+
25.908865241713197,-0.012384398120037735,0.5193588602291844,26.107910115747732,6.199851140250445e-05,0.11765912164508979
8+
5.945747279038813,1.9590562091669046,0.517638668441123,6.312543943336395,0.1528613033555711,-0.11804270366817947
9+
10.977653810076534,2.0073571475798366,0.5262312225312316,11.202688358067665,0.048951803364724066,0.08355544937094767
10+
15.907573553457881,1.9748365587785348,0.5201426739804866,16.12570285645692,0.023327802848135563,0.0990533712226529
11+
20.977532394950202,2.027075674620532,0.5298010817437455,21.20212425288145,0.013792007019379266,0.09329829384322663
12+
25.94572698767402,2.025677459358005,0.528678153141794,26.200936432591117,0.009039525768775734,0.08383289991871018
13+
5.95990852961219,3.941060388037972,0.5323693326412191,7.5065412620116865,0.23177598606042482,-0.48257450643011246
14+
10.935884647118447,3.925370124215551,0.5068006963580796,11.808417839398922,0.08802610934843358,-0.09311976839250888
15+
15.952417762803327,3.9775236065103514,0.5198766857284765,16.60075955935451,0.04442226833579014,0.009951918859625267
16+
20.949087555318403,3.9940948490948003,0.5238093624227865,21.502281294208785,0.026454875983218443,0.039768176225094376
17+
25.985268455942407,4.016660219807042,0.5261824300175458,26.501662198568287,0.017481590315867546,0.048627417442342245
18+
5.97924611215104,5.9500654137079385,0.5370931053328587,9.308018129091295,0.24820988067134767,-0.6138815031889495
19+
11.029887109905536,5.964622548372629,0.4979648891556849,13.002553061864567,0.11441688874913211,-0.2231234874931787
20+
15.989215639588322,5.965673171119106,0.5133288440673606,17.403147450340132,0.06175740911797624,-0.06910069097022259
21+
20.970483915713434,5.977342589990561,0.5194936917077476,22.10066411645723,0.037838925396880295,-0.010227631827625573
22+
25.93082342700779,5.972937112084193,0.5216823291066396,26.90108441245977,0.02535774511747541,0.01474553148231285
23+
6.004728749524293,7.971997510978571,0.522820660295624,11.507234925211806,0.23646959035027065,-0.636676769786486
24+
10.965827964891812,7.944698640247776,0.5288305112808684,14.405314612395072,0.13008707469637584,-0.29373530466377706
25+
15.936143379989273,7.930464775985118,0.524642370499414,18.410856653889642,0.07530372487041755,-0.1287581784532251
26+
20.965162258270695,7.942946435675094,0.5149380641775931,22.905925418402315,0.047534917611530864,-0.0546998371842749
27+
25.9701051346819,7.970710048321492,0.5194346204670051,27.60059638234513,0.03245556867532292,-0.016751087853224586
28+
5.99013914239,9.965673147630298,0.5374906698523683,13.907128842934997,0.21741570009117225,-0.6064383409544339
29+
10.98052148510162,9.983014128462647,0.5307161812650916,16.20588701497373,0.1356436727565411,-0.3382860668017521
30+
16.031476186550417,9.942281738920256,0.5031905777370667,19.803130702616883,0.08397228170866237,-0.18075335188175684
31+
21.017729998698556,9.966901423121303,0.5124905755008868,24.000352901989118,0.05537507259203249,-0.09230295969694866
32+
25.940755548083505,9.920002489717708,0.5159824688426303,28.4050840303573,0.03863181923115752,-0.04514799663051309
33+
6.01302612184734,11.975672612665589,0.5233110935499182,16.506998456741183,0.1966009995549466,-0.5699064484075126
34+
10.999617614932653,11.981314969497422,0.5233488274974134,18.205708249597116,0.1351920531253496,-0.35970535479777493
35+
15.965440595463313,11.939667218575659,0.5271765548232162,21.308615341930427,0.0900760981263959,-0.20907795596297815
36+
21.00551572105455,11.917747906409005,0.5089483573924627,25.203887966491692,0.06136602152526883,-0.12296600895727972
37+
26.0022720147687,11.9530254718608,0.5147702244666441,29.505425256342157,0.04381936697848015,-0.07000303244821515
38+
1.047189675722912,13.969722827151239,0.5391101706919981,20.812958911675786,0.20748371809921923,-0.6401683985581305
39+
6.013020941274695,13.980226927883933,0.5234283532864134,19.204029242841038,0.17797853667449678,-0.5269238240747315
40+
10.994848377699613,13.917218176909216,0.5307095983315354,20.30777911183574,0.13178728804332976,-0.3621854400850839
41+
16.06713018814428,13.930758927275294,0.4981356239925087,23.102474647656077,0.09227688127603328,-0.23601851810797148
42+
20.954902639764526,13.930153243560216,0.5257166248098417,26.610630483144114,0.06597500393334328,-0.14402640117438303
43+
26.015461721905297,13.945391653149361,0.5128704389430422,30.705904668742022,0.04802964927670199,-0.09113855133464518
44+
1.0276276840001535,15.97521264134705,0.5368166290929105,23.910740487195902,0.18227521043745992,-0.569752825742308
45+
6.0357040634317745,15.9326924498329,0.5342589485444893,21.913418729870425,0.1617810534646692,-0.4852883338606933
46+
11.003632083132342,15.982729623803403,0.5234563553969972,22.706229176369558,0.12625951398130855,-0.3587793894776
47+
15.980283701178461,15.974007905686557,0.5285133370149352,25.006801073636026,0.09353607914412403,-0.24387818761652774
48+
20.963501220483074,15.932045796564047,0.5262556386798449,28.208370409178734,0.06884687139009235,-0.16184852807204098
49+
25.954566708145673,15.939710795417964,0.5251303371290894,32.00376163249387,0.05152015448877997,-0.10670024992489906
50+
1.0102855855867496,17.97908939009517,0.5351185364650618,27.00677056231159,0.16248635238417589,-0.5124629125588577
51+
5.991197853878843,17.976744834652358,0.5333416828096283,24.807593611535314,0.14764178417129628,-0.44846797159561813
52+
10.994854776378206,17.955905546296897,0.5307863398435816,25.10908889543992,0.12032598779817263,-0.34755677631489823
53+
15.986191435340405,17.917291883603518,0.5282323737398233,27.01190628221328,0.0927556838860843,-0.2504707482220659
54+
21.064409500600785,17.928762441968917,0.5044862533055923,30.00382679300119,0.07015760671635658,-0.1777818328125267
55+
25.962618329143424,17.944694664224894,0.5255710054553666,33.507658249041874,0.053977247788378244,-0.12083501407795985
56+
0.9999999999999999,0.0,0.0,1.0,0.0,0.0
57+
5.9996121147529635,0.00730929049373172,-0.04016757104872042,5.996792870451032,0.0002864504738219918,-0.0433286610355218
58+
10.999686154598917,-0.013246527379861444,0.02542201208530931,10.99092136807903,-0.00027174671528077494,0.015304601794634609
59+
15.999921755173412,-0.015974401494053728,0.009501951052983916,15.994350910320302,-0.00017683928255137296,0.004343822144805803
60+
20.999865351191875,-0.03319847294695509,0.009663654293547484,20.99100136883842,-0.00022006178980510388,0.0036855392330720493
61+
25.999867564251073,-0.04191720377669392,0.007913430867482894,25.989148275454486,-0.0001830536696644254,0.0025824399042456425
62+
5.952100129553903,1.9778464945327534,-0.03107351924056645,6.323896141120901,0.1530705954454068,-0.5978649824552916
63+
10.952344146457863,1.992260151157743,0.004172569722418373,11.177441626328559,0.04836641615798731,-0.19326095905805046
64+
16.02849691482329,2.0071012583632584,0.008548786999047172,16.116223585841446,0.02330598084400782,-0.0888390736513063
65+
20.97180567983339,1.984648268735215,0.0067309519348971785,21.086967679142997,0.013434437164992038,-0.05235572297760982
66+
25.99736702293876,1.9784967849643929,0.006979712145014468,26.06544930827501,0.008755545918577988,-0.033530501305256005
67+
10.935453773277805,3.965683306928956,0.008154294377782721,11.80388116383699,0.08886880047091393,-0.3385833051458505
68+
15.926636111946964,3.9746885810760655,0.003919863693058943,16.50472075483697,0.044665717816590095,-0.17374076638227892
69+
20.988211535034793,3.957896415640341,-0.010125219536219721,21.401607458148856,0.026235140421129836,-0.10618805394239166
70+
25.985620837457553,3.9578134064180754,-0.00914313088831196,26.30054227897991,0.017301177552678577,-0.07045081415177712
71+
10.970369922967693,5.988300531407694,0.009867910763688807,13.002854860052054,0.11548209060570723,-0.43739215537843784
72+
16.01875224981119,5.933042699839213,-0.02415142953131343,17.404430948165004,0.06135332753306763,-0.24795240546066102
73+
20.963597565242466,5.91955641888566,-0.014303946557529654,22.00653504555401,0.03763776370731319,-0.152216645161062
74+
25.972488075994065,5.9408149625139925,-0.009626526982450022,26.80558528296207,0.02523944465141781,-0.10196944067307973
75+
10.964959897904377,7.965054036754775,0.011763242119061185,14.508063641306126,0.12968168929746343,-0.4905958676797794
76+
15.97418470714956,7.994579085018131,0.006234293453112865,18.505922046409598,0.0753462289009922,-0.29218614286940786
77+
20.99601824559866,7.912482592869397,-0.018203447219137164,22.906133215439876,0.04731184105019175,-0.1909752142054268
78+
25.966383934972487,7.913829381946609,-0.01224617006905617,27.504765109324275,0.03234389854294942,-0.13049086684002664
79+
10.969134173040011,9.940332735358124,0.012891249402341633,16.31251897818367,0.13477893606522556,-0.5107483210238818
80+
15.99341595981961,9.990328961894047,0.00013932100337728237,19.90575912817816,0.08414790828182576,-0.3279089886455371
81+
20.96679302538247,9.986820506741104,0.004674761097046998,24.00727865692623,0.055616249438317306,-0.21774804434967535
82+
25.941902331672214,9.969282080340271,0.0034441445579561723,28.40511012829969,0.03881504072090396,-0.15266476306313967
83+
6.021387265124099,11.982584554803791,0.0619205367251186,16.807632400271277,0.19376349049457703,-0.6981317007977318
84+
10.978055479305647,11.971174051816263,0.012018418719579226,18.407379812244866,0.1341782485690368,-0.5092616898369976
85+
15.975525182408454,11.993577495261796,0.007467004371503862,21.505219245161303,0.08968698135931524,-0.3472766990624416
86+
20.96549643457338,11.980511052381956,0.0052444147732229884,25.302220154530886,0.06157444802296901,-0.2406694491660568
87+
25.953703755756163,11.973597439317075,0.0038229396719086234,29.50313647754905,0.043997517782207375,-0.17295345774509066
88+
6.0165336608823745,13.952107280107047,0.017430531922791037,19.610668909286897,0.17479324115073436,-0.6504808406230865
89+
10.993564813822987,13.917657524716438,0.013034187987974781,20.614125970500076,0.13019583708624366,-0.4967080460397468
90+
15.973434879881712,13.977597735935445,0.007959243395739744,23.30779254206784,0.09222221346693725,-0.3574646080513337
91+
20.970859212501967,13.98547236516948,0.005532903491342632,26.808041612485553,0.06575348990187455,-0.25734142418207917
92+
26.03581471613072,13.908897344375086,-0.018534119669600446,30.803643028824563,0.047797416328487136,-0.19221312068619223
93+
1.0628058924296457,15.984930849178589,0.018551885964324236,24.81534267554288,0.17609904286262462,-0.6578471506907968
94+
6.01962610936885,15.957570491711625,0.015509828050043348,22.51160003719807,0.15825763969556614,-0.5960207124638933
95+
11.008156936600685,15.908632907600232,0.012728793080601541,23.013028478847996,0.12462667450164781,-0.4772745140567172
96+
15.976273988698015,15.986680147056422,0.007846666865800225,25.308082572604224,0.09270099854647953,-0.35977902410825735
97+
20.954961154758088,15.929119185200921,0.006474182682246497,28.40896665795771,0.06849272704911687,-0.26800510926057863
98+
25.985860464061282,15.978545293919986,0.0002249129336966485,32.20170071598359,0.05133234241162024,-0.20248367036552462
99+
1.0079858111878683,17.99135843905021,0.014549187840126543,28.108487380717616,0.1567296513076089,-0.592361253858606
100+
5.995182546251699,17.981229571789623,0.01300492680865073,25.509101765362605,0.14408506171121896,-0.5475450175053673
101+
10.99256109126772,17.96396089500506,0.010759396656982077,25.610723074710805,0.11835766490426275,-0.455224799951496
102+
15.980714121403354,17.950968795069087,0.008369095811351257,27.41059404283746,0.09174908542307228,-0.35666362687139425
103+
20.960834081332603,17.91599620367308,0.006770598303988799,30.215200725543003,0.06998426392022473,-0.27426813569485314
104+
25.95412767133884,17.945700743778684,0.004909078296815111,33.70820574556154,0.053756122035816714,-0.21146781281706867

0 commit comments

Comments
 (0)