Test lookup table, add its results.

Author: gbmxdwmssj

PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py

@@ -2,6 +2,8 @@
 Lookup Table generation for model predictive trajectory generator
 
 author: Atsushi Sakai
+
+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).
 """
 from matplotlib import pyplot as plt
 import numpy as np
@@ -37,7 +39,7 @@ 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) # Euclidean norm
         if d <= mind:
             minid = i
             mind = d
@@ -49,7 +51,7 @@ def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookuptable):
 
 def save_lookup_table(fname, table):
     mt = np.array(table)
-    print(mt)
+    # print(mt)
     # save csv
     df = pd.DataFrame()
     df["x"] = mt[:, 0]
@@ -58,25 +60,30 @@ def save_lookup_table(fname, table):
     df["s"] = mt[:, 3]
     df["km"] = mt[:, 4]
     df["kf"] = mt[:, 5]
-    df.to_csv(fname, index=None)
+    # df.to_csv(fname, index=None)
+    df.to_csv(fname)
 
     print("lookup table file is saved as " + fname)
 
 
 def generate_lookup_table():
-    states = calc_states_list()
+    states = calc_states_list() # calculate target states
     k0 = 0.0
 
-    # x, y, yaw, s, km, kf
+    # target_x, target_y, target_yaw -> s, km, kf
     lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]]
 
-    for state in states:
+    print("Lookup Table is being generated......")
+    
+    for idx, state in enumerate(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.matrix(
+        #     [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).T
         init_p = np.matrix(
-            [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).T
+            [bestp[3], bestp[4], bestp[5]]).T
 
         x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p)
 
@@ -85,6 +92,10 @@ def generate_lookup_table():
             lookuptable.append(
                 [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])])
 
+        ## print percent of progress
+        percent = 100 * (idx + 1.0) / len(states)
+        print("Complete %{}...".format(percent))
+
     print("finish lookup table generation")
 
     save_lookup_table("lookuptable.csv", lookuptable)
@@ -94,7 +105,10 @@ def generate_lookup_table():
             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)
+            table[3], -table[4], -table[5], k0) # symmetrical, this is why target_yaw inlcude only -30 degree and exclude +30 degree
+                                                # similar for target_y, note that not for target_x
+                                                # also note that here change the sign of steering make the sign of target_yaw
+                                                # and target_y change at the same time
         plt.plot(xc, yc, "-r")
 
     plt.grid(True)

PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py

@@ -3,21 +3,30 @@
 
 author: Atsushi Sakai
 
+This sample program doesn't optimize the cost function in the original paper.
+
+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).
+
 the design of p can be improved, only two steer, v is connstant, no acceleration and deacceleration
 
 Jacobian: the calculation of Jacobian may be not right? (if steer limit is small, can't optimize!? or cost threshold too small?),
-Jacobian: up and down on the starting of optimization is also a problem
+Jacobian: up and down at the starting of optimization is also a problem
 Jacobian: what is the optimization's theory?
 Jacobian: if the initial path is too long, can't optimize!?
+Jacobian: what we are optimizing?
+Jacobian: maybe local minimum cause the failure of optimization?
+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
 """
 
 import os
 import sys
-import warnings
 import numpy as np
 import matplotlib.pyplot as plt
 import math
 import motion_model
+from colorama import init as clr_ama_init
+from colorama import Fore
+clr_ama_init(autoreset = True) # anto reset to default color if color not set
 
 ## get directory of matplotrecorder
 github_root = os.path.join(os.path.dirname(__file__), '../../../')
@@ -31,7 +40,7 @@
 cost_th = 0.1
 
 matplotrecorder.DO_NOTHING = True
-show_graph = True
+show_graph = False
 
 def limitP(p, cfg):
     p[1, 0] = np.clip(p[1, 0], cfg.min_steer, cfg.max_steer)
@@ -149,7 +158,7 @@ def optimize_trajectory(target, k0, p):
             dp = limitDp(dp, mcfg, p)
         except np.linalg.linalg.LinAlgError:
             ## optimize fail
-            warnings.warn("cannot calc path LinAlgError")
+            print(Fore.YELLOW + "cannot calc path LinAlgError")
             xc, yc, yawc, p = None, None, None, None
             break
         alpha = selection_learning_param(dp, p, k0, target) # choose learning rate
@@ -164,7 +173,7 @@ def optimize_trajectory(target, k0, p):
         ## optimize fail
         ## if no break, enter here
         xc, yc, yawc, p = None, None, None, None
-        warnings.warn("cannot calc path")
+        print(Fore.YELLOW + "cannot calc path")
 
     return xc, yc, yawc, p
 
@@ -173,7 +182,7 @@ def test_optimize_trajectory():
     mcfg = motion_model.ModelConfig()
 
     #  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(0.0))
+    target = motion_model.State(x=1.0, y=0.0, yaw=math.radians(30.0))
     k0 = 0.0
 
     init_p_len = math.sqrt(target.x**2 + target.y**2)

PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py

@@ -38,7 +38,7 @@ def update(state, v, delta, dt, L):
 
     ## Ackermann model
     ## limit amplitude
-    ## TODO: outside also need amplitude limiting?
+    ## TODO: actual path length isn't equal to path length (s or p[0]) in parameterized control sequence (p)?
 
     model_cfg = ModelConfig()
     delta = np.clip(delta, model_cfg.min_steer, model_cfg.max_steer) # steering min and max [rad]

PathPlanning/ModelPredictiveTrajectoryGenerator/results/3/lookuptable.csv

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