diff --git a/modules/planning/testdata/garage_test/result_follow_0.pb.txt b/modules/planning/testdata/garage_test/result_follow_0.pb.txt
index a4752abea9996b40ee64509fcbb52e568552afbb..be72c73696682f761b147097a2b4917929a46a91 100644
--- a/modules/planning/testdata/garage_test/result_follow_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_follow_0.pb.txt
@@ -1,7 +1,7 @@
 header {
   timestamp_sec: 1498682818.1200001
 }
-total_path_length: 31.277473425223818
+total_path_length: 31.277473425223821
 total_path_time: 6.999999999999992
 is_replan: true
 gear: GEAR_DRIVE
@@ -17,1547 +17,1547 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.5999999046325684
-  a: 0.059324156185413437
+  a: 0.059324156185413646
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 586344.85191998817
-    y: 4140700.2691476513
-    theta: 1.3230173757753496
-    kappa: -0.025039269360930472
+    x: 586344.851934876
+    y: 4140700.2691437104
+    theta: 1.322971974344286
+    kappa: -0.025126249822892178
     s: 0.1120065451490263
-    dkappa: 0.0010180153683517284
+    dkappa: 0.00093042945511683493
     ddkappa: 0
   }
   v: 5.6003912066655328
-  a: -0.019697640263613717
+  a: -0.019697640263613508
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 586344.88056986756
-    y: 4140700.3774200282
-    theta: 1.3205339061263262
-    kappa: -0.024028391725367054
+    x: 586344.88059964229
+    y: 4140700.3774121474
+    theta: 1.3204431064179989
+    kappa: -0.024202346607212859
     s: 0.22400530978938207
-    dkappa: 0.0020359600205020986
+    dkappa: 0.0018607942781674526
     ddkappa: 0
   }
   v: 5.599231708509004
-  a: -0.095763013665600014
+  a: -0.0957630136655998
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 586344.9092099733
-    y: 4140700.4856554689
-    theta: 1.3180512837065361
-    kappa: -0.023017858948088412
+    x: 586344.90925462963
+    y: 4140700.485643649
+    theta: 1.3179151012084489
+    kappa: -0.023278758578720454
     s: 0.3359658663415262
-    dkappa: 0.0030535574034733057
+    dkappa: 0.0027908417096705628
     ddkappa: 0
   }
   v: 5.596580109569822
-  a: -0.16891486943060341
+  a: -0.16891486943060319
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 586344.93783281953
-    y: 4140700.5938256839
-    theta: 1.3155701573777527
-    kappa: -0.022007935143398213
+    x: 586344.93789234874
+    y: 4140700.5938099278
+    theta: 1.315388619438723
+    kappa: -0.022355727127713707
     s: 0.44785895263297226
-    dkappa: 0.0040705415565461534
+    dkappa: 0.0037203286710885148
     ddkappa: 0
   }
   v: 5.5924942511466265
-  a: -0.23919611296868176
+  a: -0.23919611296868154
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 586344.966431215
-    y: 4140700.701903495
-    theta: 1.3130911505404779
-    kappa: -0.020998874061779735
+    x: 586344.96650560421
+    y: 4140700.701883805
+    theta: 1.3128642959051957
+    kappa: -0.021433484172358824
     s: 0.5596564547361248
-    dkappa: 0.0050866569552750042
+    dkappa: 0.004649021622262571
     ddkappa: 0
   }
   v: 5.5870311164298538
-  a: -0.30664964968989294
+  a: -0.30664964968989272
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 586344.99499825714
-    y: 4140700.8098628172
-    theta: 1.3106148615144919
-    kappa: -0.019990919244798004
+    x: 586344.99508749018
+    y: 4140700.8098391984
+    theta: 1.3103427398650425
+    kappa: -0.020512252300263996
     s: 0.671331389806115
-    dkappa: 0.0061016583555027282
+    dkappa: 0.0055766964188481713
     ddkappa: 0
   }
   v: 5.5802468305017427
-  a: -0.371318385004295
+  a: -0.3713183850042947
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 586345.02352732862
-    y: 4140700.9176786421
-    theta: 1.3081418639194098
-    kappa: -0.018984304180001934
+    x: 586345.02363138564
+    y: 4140700.9176510996
+    theta: 1.307824535423749
+    kappa: -0.01959224491005402
     s: 0.7828578889186365
-    dkappa: 0.0071153106373756441
+    dkappa: 0.0065031381697502055
     ddkappa: 0
   }
   v: 5.5721966603363269
-  a: -0.43324522432194562
+  a: -0.4332452243219454
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 586345.05201209243
-    y: 4140701.0253270213
-    theta: 1.3056727070552387
-    kappa: -0.017979252455826461
+    x: 586345.0521309505
+    y: 4140701.0252955616
+    theta: 1.3053102419226263
+    kappa: -0.018673666352944859
     s: 0.89421117990778221
-    dkappa: 0.0081273886493584812
+    dkappa: 0.0074281410945582823
     ddkappa: 0
   }
   v: 5.562935014799443
-  a: -0.49247307305290278
+  a: -0.49247307305290244
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 586345.08064967545
-    y: 4140701.1327294218
-    theta: 1.3033088860463531
-    kappa: -0.016997907398114191
+    x: 586345.08079222008
+    y: 4140701.1326914947
+    theta: 1.3028932710612313
+    kappa: -0.017775902334510749
     s: 1.0053675702038793
-    dkappa: 0.00900374601257888
+    dkappa: 0.0082299338016583214
     ddkappa: 0
   }
   v: 5.5525154446487219
-  a: -0.54904483660722436
+  a: -0.54904483660722414
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 586345.11085902411
-    y: 4140701.2394738924
-    theta: 1.3017588950876915
-    kappa: -0.016194241138732853
+    x: 586345.11109619052
+    y: 4140701.2394091417
+    theta: 1.3012251879276677
+    kappa: -0.017033660821885902
     s: 1.1163044296713256
-    dkappa: 0.0088050626599164971
+    dkappa: 0.0080561059481328388
     ddkappa: 0
   }
   v: 5.5409906425335977
-  a: -0.60300342039496835
+  a: -0.60300342039496813
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 586345.14100271428
-    y: 4140701.3459863598
-    theta: 1.3002122729565322
-    kappa: -0.015392321607602844
+    x: 586345.14133429679
+    y: 4140701.3458948433
+    theta: 1.2995607302890173
+    kappa: -0.016293032533910318
     s: 1.227000173446426
-    dkappa: 0.0086068111355424269
+    dkappa: 0.00788265590071664
     ddkappa: 0
   }
   v: 5.5284124429953
-  a: -0.65439172982619254
+  a: -0.65439172982619231
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586345.171075148
-    y: 4140701.4522470427
-    theta: 1.2986693068683488
-    kappa: -0.014592297725196893
+    x: 586345.17150092358
+    y: 4140701.4521288248
+    theta: 1.2979002072433987
+    kappa: -0.015554155008965932
     s: 1.3374342447752272
-    dkappa: 0.0084090282557575356
+    dkappa: 0.0077096158699521255
     ddkappa: 0
   }
-  v: 5.5148318224668591
-  a: -0.703252670310955
+  v: 5.51483182246686
+  a: -0.70325267031095462
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 586345.201071003
-    y: 4140701.5582371363
-    theta: 1.2971302699138292
-    kappa: -0.013794311088322141
+    x: 586345.20159073151
+    y: 4140701.5580922831
+    theta: 1.2962439126879897
+    kappa: -0.014817159021521739
     s: 1.4475870978513556
-    dkappa: 0.008211749026297158
+    dkappa: 0.0075370164823198287
     ddkappa: 0
   }
   v: 5.5002988992731048
-  a: -0.7496291472593134
+  a: -0.74962914725931307
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586345.230985227
-    y: 4140701.6639387868
-    theta: 1.2955954212986587
-    kappa: -0.012998496094449011
+    x: 586345.231598653
+    y: 4140701.6637673718
+    theta: 1.2945921255770827
+    kappa: -0.014082168696960107
     s: 1.5574401806538509
-    dkappa: 0.0080150066730678149
+    dkappa: 0.0073648868071299663
     ddkappa: 0
   }
-  v: 5.4848629336306622
-  a: -0.7935640660813259
+  v: 5.4848629336306631
+  a: -0.79356406608132557
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586345.26081303437
-    y: 4140701.7693350846
-    theta: 1.2940650065833097
-    kappa: -0.01220498006604004
+    x: 586345.26151988714
+    y: 4140701.7691371841
+    theta: 1.2929451101801446
+    kappa: -0.013349301626403069
     s: 1.6669759177850045
-    dkappa: 0.0078188326728839581
+    dkappa: 0.0071932543834139582
     ddkappa: 0
   }
   v: 5.46857232764796
-  a: -0.83510033218705015
+  a: -0.83510033218704982
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 586345.29054990027
-    y: 4140701.8744100411
-    theta: 1.2925392579228259
-    kappa: -0.011413883374878749
+    x: 586345.29134989483
+    y: 4140701.8741857372
+    theta: 1.2913031163398667
+    kappa: -0.012618668981538669
     s: 1.7761776933081934
-    dkappa: 0.0076232567842046961
+    dkappa: 0.0070221452468159648
     ddkappa: 0
   }
   v: 5.451474625325222
-  a: -0.87428085098654418
+  a: -0.87428085098654384
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 586345.32019155647
-    y: 4140701.979148576
-    theta: 1.2910183943066116
-    kappa: -0.010625319566398471
+    x: 586345.3210843947
+    y: 4140701.978897952
+    theta: 1.2896663797302237
+    kappa: -0.011890375629447246
     s: 1.8850298335857176
-    dkappa: 0.0074283070778705185
+    dkappa: 0.00685158395648442
     ddkappa: 0
   }
   v: 5.4336165125544724
-  a: -0.91114852788986589
+  a: -0.91114852788986567
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 586345.34978918382
-    y: 4140702.0835205796
-    theta: 1.2895406157915055
-    kappa: -0.0098484311960246786
+    x: 586345.35078188253
+    y: 4140702.0832415069
+    theta: 1.2880705897844775
+    kappa: -0.011172428862302644
     s: 1.9935175901166353
-    dkappa: 0.0072352848548707192
+    dkappa: 0.0066827253648785124
     ddkappa: 0
   }
-  v: 5.4150438171195345
-  a: -0.945746268307073
+  v: 5.4150438171195354
+  a: -0.94574626830707276
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586345.38026145671
-    y: 4140702.187246717
-    theta: 1.2887410657894174
-    kappa: -0.0092343220212785741
+    x: 586345.38148092758
+    y: 4140702.186900754
+    theta: 1.2871072392070246
+    kappa: -0.010596766485134556
     s: 2.1016271223745995
-    dkappa: 0.0070655206277917392
+    dkappa: 0.0065344585428623739
     ddkappa: 0
   }
-  v: 5.39580150869603
-  a: -0.97811697764822392
+  v: 5.3958015086960307
+  a: -0.97811697764822325
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586345.41062347137
-    y: 4140702.2905975408
-    theta: 1.2879444088084666
-    kappa: -0.0086224348849525956
+    x: 586345.4120688939
+    y: 4140702.2901849304
+    theta: 1.2861473743322502
+    kappa: -0.010023187033888593
     s: 2.2093454806456929
-    dkappa: 0.00689637066060407
+    dkappa: 0.0063867281964250068
     ddkappa: 0
   }
   v: 5.3759336988513793
-  a: -1.0083035613233757
+  a: -1.0083035613233755
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586345.440871824
-    y: 4140702.3935614643
-    theta: 1.2871507341600825
-    kappa: -0.0080128383843417958
+    x: 586345.44254235248
+    y: 4140702.3930824562
+    theta: 1.2851911027684526
+    kappa: -0.0094517548112711038
     s: 2.3166605888662661
-    dkappa: 0.0067278539163313118
+    dkappa: 0.0062395508872843492
     ddkappa: 0
   }
   v: 5.3554836410448017
-  a: -1.036348924742587
+  a: -1.0363489247425868
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 586345.47100335232
-    y: 4140702.4961277237
-    theta: 1.2863601248213286
-    kappa: -0.007405596251516211
+    x: 586345.472898117
+    y: 4140702.4955825731
+    theta: 1.2842385244918741
+    kappa: -0.0088825295593547519
     s: 2.42356122746077
-    dkappa: 0.00655998801305516
+    dkappa: 0.0060929420025277025
     ddkappa: 0
   }
   v: 5.3344937306273152
-  a: -1.0622959733159154
+  a: -1.0622959733159152
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586345.50101513043
-    y: 4140702.5982863591
-    theta: 1.2855726575618327
-    kappa: -0.0068007674508094021
+    x: 586345.50313324039
+    y: 4140702.5976753295
+    theta: 1.2832897319996235
+    kappa: -0.0083155665509636913
     s: 2.5300370161795955
-    dkappa: 0.0063927892508651163
+    dkappa: 0.0059469157781487786
     ddkappa: 0
   }
   v: 5.3130055048417368
-  a: -1.0861876124534189
+  a: -1.0861876124534184
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586345.53090446407
-    y: 4140702.7000281997
-    theta: 1.2847884030707153
-    kappa: -0.0061984062763070242
+    x: 586345.53324500809
+    y: 4140702.6993515603
+    theta: 1.2823448104626154
+    kappa: -0.0077509166810587823
     s: 2.6360783969369068
-    dkappa: 0.0062262726388082109
+    dkappa: 0.0058014853225847492
     ddkappa: 0
   }
-  v: 5.2910596428226828
-  a: -1.1080667475651551
+  v: 5.2910596428226837
+  a: -1.1080667475651547
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586345.56066888629
-    y: 4140702.8013448478
-    theta: 1.2840074260835106
-    kappa: -0.0055985624493353776
+    x: 586345.56323093467
+    y: 4140702.8006028729
+    theta: 1.281403837878492
+    kappa: -0.007188626558122805
     s: 2.7416766166484794
-    dkappa: 0.006060451921838724
+    dkappa: 0.0056566626402533
     ddkappa: 0
   }
   v: 5.2686959655965691
-  a: -1.1279762840611822
+  a: -1.1279762840611818
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586345.59030615189
-    y: 4140702.9022286609
-    theta: 1.2832297855091
-    kappa: -0.005001281215949973
+    x: 586345.59308875841
+    y: 4140702.9014216289
+    theta: 1.2804668852245582
+    kappa: -0.0066287385955456609
     s: 2.8468237100695348
-    dkappa: 0.0058953396077679006
+    dkappa: 0.0055124586550896748
     ddkappa: 0
   }
-  v: 5.2459534360816065
-  a: -1.1459591273515581
+  v: 5.2459534360816074
+  a: -1.1459591273515577
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586345.61981423292
-    y: 4140703.0026727365
-    theta: 1.2824555345566342
-    kappa: -0.0044066034444240881
-    s: 2.9515124826325758
-    dkappa: 0.0057309469942136721
+    x: 586345.62281643634
+    y: 4140703.0018009311
+    theta: 1.2795340166107083
+    kappa: -0.0060712911030095908
+    s: 2.9515124826325763
+    dkappa: 0.0053688832340837347
     ddkappa: 0
   }
-  v: 5.22287015908781
-  a: -1.1620581828463408
+  v: 5.2228701590878108
+  a: -1.1620581828463403
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586345.649491764
-    y: 4140703.1025819182
-    theta: 1.28211533766558
-    kappa: -0.0039139627177643948
+    x: 586345.65282666974
+    y: 4140703.1016104836
+    theta: 1.2790170252625588
+    kappa: -0.0056031633198791375
     s: 3.0557364932852247
-    dkappa: 0.0055808081944835235
+    dkappa: 0.0052377593185642156
     ddkappa: 0
   }
   v: 5.19948338131699
-  a: -1.176316355955588
+  a: -1.1763163559555874
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586345.67916885007
-    y: 4140703.2020005709
-    theta: 1.281968041441254
-    kappa: -0.0034677175422054042
+    x: 586345.682885417
+    y: 4140703.200914409
+    theta: 1.2786851069587613
+    kappa: -0.0051756927656257563
     s: 3.159490037328057
-    dkappa: 0.0054373571514407725
+    dkappa: 0.0051124707119442562
     ddkappa: 0
   }
   v: 5.1758294913627569
-  a: -1.1887765520893574
+  a: -1.1887765520893567
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 586345.70870994043
-    y: 4140703.3009636365
-    theta: 1.2818214202040092
-    kappa: -0.0030235172916272908
+    x: 586345.71280641947
+    y: 4140703.2997632725
+    theta: 1.2783547096753978
+    kappa: -0.0047501811013791664
     s: 3.2627681292524393
-    dkappa: 0.0052945634748806595
+    dkappa: 0.0049877562421789421
     ddkappa: 0
   }
   v: 5.1519440197105189
-  a: -1.1994816766577072
+  a: -1.1994816766577066
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 586345.73811380984
-    y: 4140703.3994670091
-    theta: 1.2816754800349921
-    kappa: -0.0025813803893263935
-    s: 3.3655664855783631
-    dkappa: 0.0051524330872001539
+    x: 586345.74258843635
+    y: 4140703.3981529754
+    theta: 1.2780258471157646
+    kappa: -0.0043266459753227649
+    s: 3.3655664855783636
+    dkappa: 0.0048636210818264426
     ddkappa: 0
   }
-  v: 5.1278616387374836
-  a: -1.2084746350706954
+  v: 5.1278616387374845
+  a: -1.2084746350706945
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 586345.76737942919
-    y: 4140703.4975072425
-    theta: 1.2815302260430608
-    kappa: -0.0021413223129805865
+    x: 586345.772230425
+    y: 4140703.4960800717
+    theta: 1.2776985307921986
+    kappa: -0.0039051022139508246
     s: 3.4678815076922844
-    dkappa: 0.00501097096389066
+    dkappa: 0.0047400695764277306
     ddkappa: 0
   }
   v: 5.10361616271266
-  a: -1.2157983327383795
+  a: -1.2157983327383786
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586345.79650595982
-    y: 4140703.5950815268
-    theta: 1.2813856623891517
-    kappa: -0.0017033556684639564
+    x: 586345.8017315364
+    y: 4140703.5935417572
+    theta: 1.2773727700809792
+    kappa: -0.0034855618927776074
     s: 3.5697102646849541
-    dkappa: 0.0048701811572666575
+    dkappa: 0.0046171052652309273
     ddkappa: 0
   }
-  v: 5.07924054779685
-  a: -1.2214956750708177
+  v: 5.079240547796851
+  a: -1.2214956750708168
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 586345.82549274969
-    y: 4140703.6921876771
-    theta: 1.2812417923106425
-    kappa: -0.0012674902636614451
+    x: 586345.8310911099
+    y: 4140703.69053585
+    theta: 1.2770485722772387
+    kappa: -0.0030680344070464502
     s: 3.67105047618926
-    dkappa: 0.0047300668201943332
+    dkappa: 0.0044947309019156374
     ddkappa: 0
   }
-  v: 5.0547668920426609
-  a: -1.2256095674780676
+  v: 5.0547668920426618
+  a: -1.2256095674780667
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586345.85433932766
-    y: 4140703.7888241159
-    theta: 1.2810986181457196
-    kappa: -0.00083373318228353269
+    x: 586345.86030866811
+    y: 4140703.7870607716
+    theta: 1.2767259426498567
+    kappa: -0.0026525265424388821
     s: 3.7719004952180586
-    dkappa: 0.0045906302298202254
+    dkappa: 0.0043729484753172905
     ddkappa: 0
   }
   v: 5.0302264353944954
-  a: -1.2281829153701875
+  a: -1.2281829153701871
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586345.88304539875
-    y: 4140703.8849898549
-    theta: 1.2809561413577377
-    kappa: -0.00040208885768088729
+    x: 586345.88938391267
+    y: 4140703.8831155375
+    theta: 1.2764048844963725
+    kappa: -0.0022390425457837126
     s: 3.8722592910020119
-    dkappa: 0.0044518728112998588
+    dkappa: 0.0042517592301514794
     ddkappa: 0
   }
   v: 5.0056495596885542
-  a: -1.2292586241572352
+  a: -1.2292586241572343
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 586345.91161083966
-    y: 4140703.9806844806
-    theta: 1.2808143625595907
-    kappa: 2.7440853340970018e-05
+    x: 586345.91831671854
+    y: 4140703.978699734
+    theta: 1.2760853991978829
+    kappa: -0.0018275841957661307
     s: 3.9721264318274261
-    dkappa: 0.0043137951615263839
+    dkappa: 0.0041311636877382971
     ddkappa: 0
   }
-  v: 4.9810657886528391
-  a: -1.2288795992492685
+  v: 4.98106578865284
+  a: -1.2288795992492676
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 586345.9400387624
-    y: 4140704.0759072211
-    theta: 1.2809159902029323
-    kappa: 0.00034637974255623537
+    x: 586345.94726925145
+    y: 4140704.0737642646
+    theta: 1.276000921384524
+    kappa: -0.0015129690590297972
     s: 4.0715020678740839
-    dkappa: 0.0041923025361288238
+    dkappa: 0.00402503196825856
     ddkappa: 0
   }
-  v: 4.95650378790715
-  a: -1.2270887460563453
+  v: 4.9565037879071507
+  a: -1.2270887460563444
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 586345.96832660062
-    y: 4140704.1706595877
-    theta: 1.2810418916291955
-    kappa: 0.00065267016257737555
-    s: 4.17038691405308
-    dkappa: 0.0040730335598541468
+    x: 586345.976095249
+    y: 4140704.1683542994
+    theta: 1.2759405513355251
+    kappa: -0.0012095305043004084
+    s: 4.1703869140530809
+    dkappa: 0.003920832055793735
     ddkappa: 0
   }
-  v: 4.9319913649630847
-  a: -1.2239289699885232
+  v: 4.9319913649630855
+  a: -1.2239289699885225
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 586345.9964744004
-    y: 4140704.2649428844
-    theta: 1.2811671697830507
-    kappa: 0.00095744429820413207
-    s: 4.2687822328446643
-    dkappa: 0.0039543550221816538
+    x: 586346.00477854384
+    y: 4140704.262476068
+    theta: 1.2758804801472063
+    kappa: -0.00090759411586521712
+    s: 4.2687822328446652
+    dkappa: 0.0038171479828421275
     ddkappa: 0
   }
   v: 4.9075554692240413
-  a: -1.2194431764558609
+  a: -1.2194431764558602
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 586346.02448267466
-    y: 4140704.3587588305
-    theta: 1.2812918269472231
-    kappa: 0.0012607077028069598
+    x: 586346.033319659
+    y: 4140704.3561312854
+    theta: 1.2758207067249945
+    kappa: -0.000607154392061114
     s: 4.3666898171360682
-    dkappa: 0.0038362647606381946
+    dkappa: 0.0037139778601487121
     ddkappa: 0
   }
   v: 4.8832221919852152
-  a: -1.2136742708684154
+  a: -1.213674270868415
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 586346.05235208338
-    y: 4140704.4521096339
-    theta: 1.2814158660579089
-    kappa: 0.0015624675195041677
-    s: 4.464111973059345
-    dkappa: 0.0037187599937054715
+    x: 586346.06171926646
+    y: 4140704.4493221585
+    theta: 1.2757612296609766
+    kappa: -0.00030820425627924334
+    s: 4.4641119730593459
+    dkappa: 0.003611319257626699
     ddkappa: 0
   }
-  v: 4.8590167664336024
-  a: -1.2066651586362456
+  v: 4.8590167664336033
+  a: -1.2066651586362445
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 586346.08008342772
-    y: 4140704.5449979808
-    theta: 1.2815392906829253
-    kappa: 0.001862732428003065
+    x: 586346.08997818327
+    y: 4140704.5420513665
+    theta: 1.2757020472443754
+    kappa: -1.0735109628925944e-05
     s: 4.56105150282921
-    dkappa: 0.0036018373415200025
+    dkappa: 0.0035091692224421714
     ddkappa: 0
   }
-  v: 4.8349635676479963
-  a: -1.1984587451694082
+  v: 4.8349635676479972
+  a: -1.1984587451694075
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 586346.10767764668
-    y: 4140704.6374270148
-    theta: 1.2816621049998513
-    kappa: 0.0021615125914410636
+    x: 586346.11809736572
+    y: 4140704.6343220491
+    theta: 1.2756431574720306
+    kappa: 0.00028526311639844558
     s: 4.6575116875808673
-    dkappa: 0.0034854928465731117
+    dkappa: 0.0034075242970987131
     ddkappa: 0
   }
-  v: 4.81108611259899
-  a: -1.1890979358779616
+  v: 4.811086112598991
+  a: -1.189097935877961
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 586346.13513581082
-    y: 4140704.7294003223
-    theta: 1.2817843137741889
-    kappa: 0.0024588196032268397
+    x: 586346.1460779052
+    y: 4140704.7261377857
+    theta: 1.2755845580588723
+    kappa: 0.00057980190726549842
     s: 4.7534962702078536
-    dkappa: 0.0033697219944108887
+    dkappa: 0.0033063805375220406
     ddkappa: 0
   }
   v: 4.7874070601489755
-  a: -1.1786256361719647
+  a: -1.1786256361719636
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 586346.16245911806
-    y: 4140704.8209219165
-    theta: 1.2819059223375007
-    kappa: 0.0027546664338814497
-    s: 4.8490094381998707
-    dkappa: 0.0032545197343341694
+    x: 586346.1739210221
+    y: 4140704.8175025824
+    theta: 1.2755262464484032
+    kappa: 0.00087289411272497414
+    s: 4.8490094381998716
+    dkappa: 0.003205733531144634
     ddkappa: 0
   }
   v: 4.7639482110521421
-  a: -1.1670847514614735
+  a: -1.1670847514614726
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 586346.18964888866
-    y: 4140704.9119962179
-    theta: 1.2820269365655657
-    kappa: 0.003049067377879475
+    x: 586346.20162806183
+    y: 4140704.9084208538
+    theta: 1.2754682198231722
+    kappa: 0.0011645538941558167
     s: 4.9440558064806241
-    dkappa: 0.0031398805000985072
+    dkappa: 0.003105578414990371
     ddkappa: 0
   }
   v: 4.7407305079544795
-  a: -1.1545181871565475
+  a: -1.1545181871565464
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 586346.21653082676
-    y: 4140705.0026802127
-    theta: 1.282310838703463
-    kappa: 0.0032796508421754134
-    s: 5.0386404002456544
-    dkappa: 0.0030349827197668055
+    x: 586346.22913137835
+    y: 4140704.998918422
+    theta: 1.2755764872497473
+    kappa: 0.0014000641648687618
+    s: 5.0386404002456553
+    dkappa: 0.0030138016565488708
     ddkappa: 0
   }
-  v: 4.7177740353937754
-  a: -1.1409688486672438
+  v: 4.7177740353937763
+  a: -1.1409688486672431
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 586346.24317948311
-    y: 4140705.0929574207
-    theta: 1.28268972688325
-    kappa: 0.0034723495786197
-    s: 5.1327686378001758
-    dkappa: 0.0029360045707581602
+    x: 586346.25646143558
+    y: 4140705.0889916876
+    theta: 1.275781661977633
+    kappa: 0.0016023165646466258
+    s: 5.1327686378001767
+    dkappa: 0.002927099247201562
     ddkappa: 0
   }
-  v: 4.6950980197996177
-  a: -1.1264796414036207
+  v: 4.6950980197996186
+  a: -1.12647964140362
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 586346.26970058063
-    y: 4140705.1828025
-    theta: 1.283066801445738
-    kappa: 0.0036641259274552305
-    s: 5.2264463133969157
-    dkappa: 0.0028375001986920513
+    x: 586346.28366067237
+    y: 4140705.1786338002
+    theta: 1.2759858545993037
+    kappa: 0.0018036008464656556
+    s: 5.2264463133969166
+    dkappa: 0.0028408118547324475
     ddkappa: 0
   }
-  v: 4.6727208294933922
-  a: -1.1110934707757354
+  v: 4.6727208294933931
+  a: -1.1110934707757347
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586346.40045694145
-    y: 4140705.62576364
-    theta: 1.2849258835365447
-    kappa: 0.004609636473721427
-    s: 5.688303214752267
-    dkappa: 0.0023518463461322109
+    x: 586346.41776044737
+    y: 4140705.6205942594
+    theta: 1.27699258081154
+    kappa: 0.0027959881372505444
+    s: 5.6883032147522679
+    dkappa: 0.0024153910972472452
     ddkappa: 0
   }
-  v: 4.5659000809556645
-  a: -1.0222098565244118
+  v: 4.5659000809556654
+  a: -1.0222098565244109
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586346.52752849809
-    y: 4140706.05916602
-    theta: 1.2870437052463144
-    kappa: 0.0053873426416591423
+    x: 586346.54837786441
+    y: 4140706.0529420329
+    theta: 1.2783027630420651
+    kappa: 0.0036369436995883197
     s: 6.1399513411028721
-    dkappa: 0.0019002403366545316
+    dkappa: 0.0020191331593489453
     ddkappa: 0
   }
-  v: 4.4688118006084387
-  a: -0.91733796967523007
+  v: 4.46881180060844
+  a: -0.91733796967522929
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586346.650628709
-    y: 4140706.4841814023
-    theta: 1.2896441486815329
-    kappa: 0.0058917490007307713
+    x: 586346.67543799849
+    y: 4140706.4767904244
+    theta: 1.2801563192701551
+    kappa: 0.0042342288992569133
     s: 6.5824349420036761
-    dkappa: 0.0014986649474468196
+    dkappa: 0.0016654929434377472
     ddkappa: 0
   }
-  v: 4.3827866568986389
-  a: -0.80184098648542723
+  v: 4.38278665689864
+  a: -0.801840986485427
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 586346.77126253
-    y: 4140706.9015669338
-    theta: 1.2922208176600565
-    kappa: 0.0063568984907756022
+    x: 586346.80001825735
+    y: 4140706.8930153935
+    theta: 1.2820098851078452
+    kappa: 0.0047937492712571564
     s: 7.0169045179730283
-    dkappa: 0.0011098292255043115
+    dkappa: 0.001322900515678317
     ddkappa: 0
   }
   v: 4.3086190006474663
-  a: -0.6810820832122424
+  a: -0.68108208321224217
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586346.88765464863
-    y: 4140707.3130821986
-    theta: 1.2949885501283696
-    kappa: 0.0065158383243394558
-    s: 7.4445631887301129
-    dkappa: 0.00078133793285441817
+    x: 586346.92088863
+    y: 4140707.3032376403
+    theta: 1.284164671997873
+    kappa: 0.005079460040718021
+    s: 7.4445631887301138
+    dkappa: 0.0010313434062890318
     ddkappa: 0
   }
-  v: 4.2465668650503954
-  a: -0.560424436112911
+  v: 4.2465668650503963
+  a: -0.56042443611291082
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586347.00252027
-    y: 4140707.7192003885
-    theta: 1.2977199834320778
-    kappa: 0.0066726936416984864
-    s: 7.8666130614323757
-    dkappa: 0.00045715484543740127
+    x: 586347.04017377249
+    y: 4140707.7080797697
+    theta: 1.2862911985827949
+    kappa: 0.0053614236769682876
+    s: 7.8666130614323766
+    dkappa: 0.00074361010560475293
     ddkappa: 0
   }
-  v: 4.19635196567718
-  a: -0.44523122144467187
+  v: 4.1963519656771817
+  a: -0.44523122144467164
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 586347.11420757149
-    y: 4140708.1215741225
-    theta: 1.3004110740614445
-    kappa: 0.0066334024748366154
-    s: 8.28420159891295
-    dkappa: 0.00017731827115339936
+    x: 586347.15661258576
+    y: 4140708.1091049612
+    theta: 1.2884852215825786
+    kappa: 0.00546544734121251
+    s: 8.2842015989129525
+    dkappa: 0.00049321296266843849
     ddkappa: 0
   }
-  v: 4.1571597004718495
-  a: -0.34086561546476135
+  v: 4.1571597004718512
+  a: -0.34086561546476113
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586347.2242716332
-    y: 4140708.5208483161
-    theta: 1.3030759779951238
-    kappa: 0.0065243247546544749
+    x: 586347.27152944112
+    y: 4140708.5070095933
+    theta: 1.2906934894617628
+    kappa: 0.0055059584177577416
     s: 8.6983682072887323
-    dkappa: -8.5474591203904817e-05
+    dkappa: 0.00025715001706519744
     ddkappa: 0
   }
-  v: 4.1276570141828044
-  a: -0.25158745073274552
+  v: 4.1276570141828062
+  a: -0.25158745073274391
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586347.33285050082
-    y: 4140708.9179099374
-    theta: 1.3056826659374128
-    kappa: 0.0063489296862959859
+    x: 586347.38504557358
+    y: 4140708.9026884884
+    theta: 1.2928969556699936
+    kappa: 0.0054845667550259972
     s: 9.1100098260756539
-    dkappa: -0.00033025516252552884
+    dkappa: 3.6224252820931e-05
     ddkappa: 0
   }
-  v: 4.1064704061330914
-  a: -0.17387343310221567
+  v: 4.1064704061330932
+  a: -0.17387343310221437
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586347.43923715036
-    y: 4140709.3137576384
-    theta: 1.308189041515238
-    kappa: 0.0060318388347536337
-    s: 9.51990435049193
-    dkappa: -0.00053910008015959634
+    x: 586347.49660447473
+    y: 4140709.297109751
+    theta: 1.2951093224157439
+    kappa: 0.0053333325649055838
+    s: 9.519904350491931
+    dkappa: -0.00015489667546909213
     ddkappa: 0
   }
-  v: 4.0925479259773807
-  a: -0.10619146302109753
+  v: 4.0925479259773825
+  a: -0.10619146302109653
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586347.54534616123
-    y: 4140709.7085722894
-    theta: 1.3106888761693725
-    kappa: 0.0057155754996781475
+    x: 586347.607872239
+    y: 4140709.6905016876
+    theta: 1.2973159155167995
+    kappa: 0.0051824930527838249
     s: 9.9287291673875284
-    dkappa: -0.00074739997226699623
+    dkappa: -0.00034551883277737978
     ddkappa: 0
   }
-  v: 4.0849203979102144
-  a: -0.047860961499699622
+  v: 4.0849203979102171
+  a: -0.047860961499698929
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 586347.64938561409
-    y: 4140710.1034342065
-    theta: 1.3128798179943608
-    kappa: 0.0052624201485081974
+    x: 586347.71724159759
+    y: 4140710.0839207964
+    theta: 1.2993629008510768
+    kappa: 0.0049023349024715362
     s: 10.337068148926821
-    dkappa: -0.0009106030047857695
+    dkappa: -0.00049825841322052606
     ddkappa: 0
   }
-  v: 4.0826867040251065
-  a: 0.0017986504516695478
+  v: 4.0826867040251082
+  a: 0.0017986504516699364
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586347.753148051
-    y: 4140710.4983814587
-    theta: 1.3150267609129074
-    kappa: 0.0047894825573150078
+    x: 586347.82636601815
+    y: 4140710.47742027
+    theta: 1.3013879021694699
+    kappa: 0.0046039874467823346
     s: 10.745418458378481
-    dkappa: -0.0010673512502488685
+    dkappa: -0.00064571531330708237
     ddkappa: 0
   }
-  v: 4.0850137843145369
-  a: 0.043467951822701467
+  v: 4.0850137843145395
+  a: 0.043467951822701578
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586347.856265736
-    y: 4140710.893939415
-    theta: 1.3168686261163423
-    kappa: 0.00426996228464512
-    s: 11.154197355905374
-    dkappa: -0.0012041693278167639
+    x: 586347.93486228923
+    y: 4140710.871536932
+    theta: 1.3031800742906077
+    kappa: 0.00425939124516335
+    s: 11.154197355905376
+    dkappa: -0.00077648600507550128
     ddkappa: 0
   }
-  v: 4.0911366366699582
-  a: 0.07782752160308759
+  v: 4.0911366366699609
+  a: 0.077827521603087479
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 586347.958540907
-    y: 4140711.2905151863
-    theta: 1.318290916308829
-    kappa: 0.0036860197713039604
-    s: 11.563749004354465
-    dkappa: -0.0013135846883124518
+    x: 586348.0425562734
+    y: 4140711.2666755891
+    theta: 1.3046569378856665
+    kappa: 0.00385183576318502
+    s: 11.563749004354467
+    dkappa: -0.00088466051681496453
     ddkappa: 0
   }
-  v: 4.1003583168817892
-  a: 0.10555793878251946
+  v: 4.1003583168817919
+  a: 0.10555793878251907
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 586348.06107844436
-    y: 4140711.6881082924
-    theta: 1.3197168551002605
-    kappa: 0.0031005792710896915
-    s: 11.974351275046704
-    dkappa: -0.0014232807318719589
+    x: 586348.15052652475
+    y: 4140711.662827895
+    theta: 1.3061375900767391
+    kappa: 0.0034432347797264595
+    s: 11.974351275046708
+    dkappa: -0.00099311252847075282
     ddkappa: 0
   }
-  v: 4.11204993863942
-  a: 0.12733978235068844
+  v: 4.112049938639422
+  a: 0.12733978235068788
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 586348.16273710225
-    y: 4140712.0872366317
-    theta: 1.3207613501691435
-    kappa: 0.0024528750103368982
-    s: 12.386222553566935
-    dkappa: -0.0014861285086315451
+    x: 586348.25749703927
+    y: 4140712.0605655261
+    theta: 1.3074070531493138
+    kappa: 0.0029659354027935141
+    s: 12.386222553566936
+    dkappa: -0.001062428890438089
     ddkappa: 0
   }
-  v: 4.125650673531208
-  a: 0.14385363129728612
+  v: 4.1256506735312106
+  a: 0.1438536312972854
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 586348.26472165622
-    y: 4140712.4877625285
-    theta: 1.3218003770809608
-    kappa: 0.0018014877364458635
-    s: 12.799528545553777
-    dkappa: -0.0015480815310824874
+    x: 586348.36481494294
+    y: 4140712.4596954682
+    theta: 1.3086768553943182
+    kappa: 0.0024856973313210069
+    s: 12.799528545553779
+    dkappa: -0.0011312398112206146
     ddkappa: 0
   }
-  v: 4.1406677510444814
-  a: 0.155780064612004
+  v: 4.1406677510444831
+  a: 0.15578006461200311
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 586348.3667579199
-    y: 4140712.889879141
-    theta: 1.3223339013980784
-    kappa: 0.0011393757862416093
-    s: 13.214389082489536
-    dkappa: -0.0015842999896413506
+    x: 586348.472033448
+    y: 4140712.8604612849
+    theta: 1.3095292670639767
+    kappa: 0.0019869811495293451
+    s: 13.214389082489539
+    dkappa: -0.0011785871296658458
     ddkappa: 0
   }
-  v: 4.1566764585655358
-  a: 0.16379966128453355
+  v: 4.1566764585655385
+  a: 0.16379966128453244
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 586348.46892421041
-    y: 4140713.2936499151
-    theta: 1.3224517485870475
-    kappa: 0.00046786766237491605
-    s: 13.630884927490097
-    dkappa: -0.0015993645603342393
+    x: 586348.57927043084
+    y: 4140713.262915032
+    theta: 1.3100458610496863
+    kappa: 0.0014729046183504649
+    s: 13.6308849274901
+    dkappa: -0.0012086688256237651
     ddkappa: 0
   }
-  v: 4.1733201413796381
-  a: 0.16859300030456609
+  v: 4.17332014137964
+  a: 0.16859300030456509
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 586348.57150816987
-    y: 4140713.6990518807
-    theta: 1.3225141351085705
-    kappa: -0.000201677033242061
-    s: 14.04906458109482
-    dkappa: -0.0016076117234031819
+    x: 586348.686875762
+    y: 4140713.6670131511
+    theta: 1.3105287922294391
+    kappa: 0.0009580358028072574
+    s: 14.049064581094822
+    dkappa: -0.0012330061732516496
     ddkappa: 0
   }
-  v: 4.1903102026710224
-  a: 0.17084066066179343
+  v: 4.1903102026710242
+  a: 0.17084066066179221
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 586348.67453802
-    y: 4140714.1061016661
-    theta: 1.3222484608236318
-    kappa: -0.00084649647977925774
-    s: 14.468951087056432
-    dkappa: -0.0015755212093368689
+    x: 586348.79455433111
+    y: 4140714.0728579536
+    theta: 1.3108129895489604
+    kappa: 0.00044828512185711325
+    s: 14.468951087056436
+    dkappa: -0.0012245117933179033
     ddkappa: 0
   }
-  v: 4.2074261035228933
-  a: 0.17122322134590673
+  v: 4.207426103522895
+  a: 0.17122322134590556
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586348.77798776783
-    y: 4140714.5148103805
-    theta: 1.321981703784628
-    kappa: -0.0014939438843739662
-    s: 14.890548838130933
-    dkappa: -0.0015432999105594817
+    x: 586348.90267174365
+    y: 4140714.4803567748
+    theta: 1.3110983451130709
+    kappa: -6.3543045149813885e-05
+    s: 14.890548838130936
+    dkappa: -0.0012159827945861196
     ddkappa: 0
   }
-  v: 4.2245153629174235
-  a: 0.17042126134659774
+  v: 4.2245153629174244
+  a: 0.17042126134659658
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 586348.88237776619
-    y: 4140714.9250381663
-    theta: 1.3212487473270906
-    kappa: -0.0020817834932807346
-    s: 15.313850381867489
-    dkappa: -0.0014735879032382558
+    x: 586349.01128994522
+    y: 4140714.8894854607
+    theta: 1.3110093321487231
+    kappa: -0.00053994442056899173
+    s: 15.313850381867493
+    dkappa: -0.0011765954952849536
     ddkappa: 0
   }
-  v: 4.2414935577357546
-  a: 0.16911535965355762
+  v: 4.2414935577357564
+  a: 0.16911535965355662
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 586348.98734584684
-    y: 4140715.3368641245
-    theta: 1.320369462201386
-    kappa: -0.0026527876293531683
-    s: 15.738843226398325
-    dkappa: -0.0013920788629080024
+    x: 586349.12036119332
+    y: 4140715.3002437605
+    theta: 1.3108078626336308
+    kappa: -0.0010070561656882685
+    s: 15.738843226398329
+    dkappa: -0.0011278492810274862
     ddkappa: 0
   }
-  v: 4.258344322758
-  a: 0.16798609525647834
+  v: 4.2583443227580018
+  a: 0.16798609525647751
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586349.09322464233
-    y: 4140715.7501906282
-    theta: 1.3191578811580769
-    kappa: -0.0031730747815679611
-    s: 16.165515816690487
-    dkappa: -0.00129065266928885
+    x: 586349.23007493245
+    y: 4140715.7125692707
+    theta: 1.3103070617337904
+    kappa: -0.0014397526886353413
+    s: 16.16551581669049
+    dkappa: -0.0010628901139907239
     ddkappa: 0
   }
-  v: 4.2750856583212329
-  a: 0.16667773607608619
+  v: 4.2750856583212338
+  a: 0.16667773607608546
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 586349.20024178852
-    y: 4140716.1649450986
-    theta: 1.3174608205194449
-    kappa: -0.0036179436109302606
-    s: 16.593854398110242
-    dkappa: -0.0011601817811854789
+    x: 586349.34051913209
+    y: 4140716.1264243415
+    theta: 1.3093777205868919
+    kappa: -0.0018223237797832656
+    s: 16.593854398110246
+    dkappa: -0.00097478548716867285
     ddkappa: 0
   }
-  v: 4.2916484206464478
-  a: 0.16442589709261354
+  v: 4.2916484206464487
+  a: 0.16442589709261293
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 586349.30778900313
-    y: 4140716.5812599007
-    theta: 1.3157525257693985
-    kappa: -0.0040513820911188669
-    s: 17.023836508600734
-    dkappa: -0.001025470072466197
+    x: 586349.45145617356
+    y: 4140716.5418488556
+    theta: 1.30844259768749
+    kappa: -0.0021962528732267965
+    s: 17.023836508600738
+    dkappa: -0.00088305608274970315
     ddkappa: 0
   }
-  v: 4.3079416417322838
-  a: 0.16129817788501011
+  v: 4.3079416417322856
+  a: 0.16129817788500952
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586349.41729341913
-    y: 4140716.998731487
-    theta: 1.3139763875830326
-    kappa: -0.0043155352875309683
-    s: 17.455430927302874
-    dkappa: -0.000841594803796092
+    x: 586349.56364046456
+    y: 4140716.9586082906
+    theta: 1.3074772994835255
+    kappa: -0.0024499080190734804
+    s: 17.455430927302878
+    dkappa: -0.00075142598240450845
     ddkappa: 0
   }
-  v: 4.3238811135352773
-  a: 0.15736217803222552
+  v: 4.3238811135352782
+  a: 0.15736217803222494
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 586349.52719693864
-    y: 4140717.4177246052
-    theta: 1.312193776019531
-    kappa: -0.0045806512261862874
-    s: 17.88859835055111
-    dkappa: -0.00065704937670641676
+    x: 586349.67623362655
+    y: 4140717.3768866626
+    theta: 1.3065084831189981
+    kappa: -0.002704487645590599
+    s: 17.888598350551113
+    dkappa: -0.00061931613825987216
     ddkappa: 0
   }
-  v: 4.3393893879698551
-  a: 0.15268549711320947
+  v: 4.339389387969856
+  a: 0.15268549711320886
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586349.63895350671
-    y: 4140717.8378059771
-    theta: 1.3102949542665243
-    kappa: -0.0046212856425065629
-    s: 18.323292067869239
-    dkappa: -0.00038683480786561855
+    x: 586349.79009885923
+    y: 4140717.7964019533
+    theta: 1.3054105320580476
+    kappa: -0.0027806244405371378
+    s: 18.323292067869247
+    dkappa: -0.00040860110792367876
     ddkappa: 0
   }
-  v: 4.3543957769083423
-  a: 0.14733573470691155
+  v: 4.3543957769083441
+  a: 0.14733573470691091
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 586349.7515609446
-    y: 4140718.2591856658
-    theta: 1.3083542848652154
-    kappa: -0.0045894394274756676
-    s: 18.75945863796618
-    dkappa: -8.8315604434705585e-05
+    x: 586349.90462475689
+    y: 4140718.2172642774
+    theta: 1.3042693839881592
+    kappa: -0.0028007058123853171
+    s: 18.759458637966187
+    dkappa: -0.00017263596396544
     ddkappa: 0
   }
-  v: 4.36883635218096
-  a: 0.14138049039228148
+  v: 4.3688363521809608
+  a: 0.14138049039228079
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 586349.86540764919
-    y: 4140718.6816950291
-    theta: 1.3063743411848439
-    kappa: -0.004405850989692239
-    s: 19.197038564731773
-    dkappa: 0.00022456603588669392
+    x: 586350.02005945588
+    y: 4140718.6393432575
+    theta: 1.3030506584996751
+    kappa: -0.0027014624481681662
+    s: 19.19703856473178
+    dkappa: 7.3251038758275893e-05
     ddkappa: 0
   }
-  v: 4.38265394557582
-  a: 0.1348873637482689
+  v: 4.3826539455758207
+  a: 0.13488736374826812
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586349.98062936682
-    y: 4140719.1052302741
-    theta: 1.304349658981458
-    kappa: -0.0040440783601006312
-    s: 19.635966973232556
-    dkappa: 0.00055410189539316682
+    x: 586350.13646984764
+    y: 4140719.0625533331
+    theta: 1.3017429937205902
+    kappa: -0.0024642562377649056
+    s: 19.635966973232559
+    dkappa: 0.00033045412288814334
     ddkappa: 0
   }
-  v: 4.3957981488389315
-  a: 0.12792395435382345
+  v: 4.3957981488389333
+  a: 0.12792395435382262
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586350.09647278115
-    y: 4140719.5299212062
-    theta: 1.3024656339590095
-    kappa: -0.0036125176160099015
-    s: 20.076174285707562
-    dkappa: 0.00089290563566682413
+    x: 586350.25339509849
+    y: 4140719.4869479826
+    theta: 1.3005500285988445
+    kappa: -0.0021715827690945619
+    s: 20.07617428570757
+    dkappa: 0.0005951402889508812
     ddkappa: 0
   }
-  v: 4.4082253136742011
-  a: 0.12055786178789479
+  v: 4.408225313674202
+  a: 0.12055786178789385
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586350.21391621977
-    y: 4140719.9554234184
-    theta: 1.3012338798776124
-    kappa: -0.0028714452058535862
-    s: 20.517586897564126
-    dkappa: 0.0012699041305766453
+    x: 586350.37141128723
+    y: 4140719.9122916944
+    theta: 1.299873773315916
+    kappa: -0.001637771069256534
+    s: 20.517586897564129
+    dkappa: 0.00089009597427652733
     ddkappa: 0
   }
-  v: 4.4198985517434259
-  a: 0.11285668562943257
+  v: 4.4198985517434268
+  a: 0.11285668562943157
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586350.33165986848
-    y: 4140720.3820133046
-    theta: 1.2999989771723826
-    kappa: -0.0021284784579501444
-    s: 20.960127853373638
-    dkappa: 0.001647866313339003
+    x: 586350.48972915031
+    y: 4140720.3387226746
+    theta: 1.2991957893815202
+    kappa: -0.0011025948338629983
+    s: 20.960127853373642
+    dkappa: 0.0011858056286891154
     ddkappa: 0
   }
-  v: 4.4307877346663007
-  a: 0.10488802545738646
+  v: 4.4307877346663016
+  a: 0.10488802545738539
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 586350.4504631653
-    y: 4140720.8093978069
-    theta: 1.2997767344760973
-    kappa: -0.00090211738882278725
-    s: 21.40371752286736
-    dkappa: 0.0020227037529734872
+    x: 586350.60868272919
+    y: 4140720.7660654285
+    theta: 1.2992409092815347
+    kappa: -0.00018566029906170621
+    s: 21.403717522867368
+    dkappa: 0.0014808723958069515
     ddkappa: 0
   }
-  v: 4.4408694940204159
-  a: 0.096719480850706127
+  v: 4.4408694940204168
+  a: 0.096719480850705
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 586350.56959518581
-    y: 4140721.2376947477
-    theta: 1.2996447078917166
-    kappa: 0.0003699313533758751
-    s: 21.848274276932209
-    dkappa: 0.0023979993298805192
+    x: 586350.72792633518
+    y: 4140721.1943313153
+    theta: 1.2993487261491783
+    kappa: 0.00076613901349785289
+    s: 21.848274276932216
+    dkappa: 0.0017764663870846807
     ddkappa: 0
   }
-  v: 4.450127221341253
-  a: 0.088418651388341235
+  v: 4.4501272213412539
+  a: 0.088418651388340055
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586350.68896413536
-    y: 4140721.6668434832
-    theta: 1.2995124187334177
-    kappa: 0.0016445099407822719
-    s: 22.293715163606546
-    dkappa: 0.0027740412930581603
+    x: 586350.84740709234
+    y: 4140721.6234489358
+    theta: 1.2994567574425577
+    kappa: 0.0017198312605415361
+    s: 22.293715163606549
+    dkappa: 0.0020726482544535206
     ddkappa: 0
   }
-  v: 4.4585510681221932
-  a: 0.0800531366492414
+  v: 4.458551068122194
+  a: 0.0800531366492402
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 586350.80810055183
-    y: 4140722.0968874982
-    theta: 1.3002780326284658
-    kappa: 0.0020923639923538522
-    s: 22.739956584075962
-    dkappa: 0.0020328398616758867
+    x: 586350.96656856325
+    y: 4140722.0534859775
+    theta: 1.3002790701774671
+    kappa: 0.0020915219368897392
+    s: 22.739956584075966
+    dkappa: 0.001557814541160648
     ddkappa: 0
   }
   v: 4.4661379458145092
-  a: 0.0716905362123563
+  a: 0.0716905362123551
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 586350.92729322263
-    y: 4140722.5276599489
-    theta: 1.3013164105088073
-    kappa: 0.0022903023778629676
-    s: 23.186914968669065
-    dkappa: 0.000952468310227135
+    x: 586351.08576137119
+    y: 4140722.484258391
+    theta: 1.3013168130500841
+    kappa: 0.0022887909760056185
+    s: 23.186914968669072
+    dkappa: 0.00079882253544523159
     ddkappa: 0
   }
   v: 4.47289152582737
-  a: 0.063398449656635575
+  a: 0.063398449656634437
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 586351.04611515929
-    y: 4140722.9591922192
-    theta: 1.302437966431973
-    kappa: 0.002453764706373783
-    s: 23.634507452853288
-    dkappa: 0.00040811916122693992
+    x: 586351.20458297047
+    y: 4140722.9157907544
+    theta: 1.3024375683900082
+    kappa: 0.002451646411544306
+    s: 23.634507452853292
+    dkappa: 0.00040669642967825046
     ddkappa: 0
   }
   v: 4.4788222395278394
-  a: 0.055244476561028918
+  a: 0.055244476561027864
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 586351.16460249771
-    y: 4140723.391389838
-    theta: 1.3036337530486257
-    kappa: 0.0025864405234450136
-    s: 24.082652553230648
-    dkappa: 0.00034236819171098683
+    x: 586351.323070953
+    y: 4140723.3479881962
+    theta: 1.3036321945081335
+    kappa: 0.0025838613615304195
+    s: 24.082652553230655
+    dkappa: 0.00034117725947336187
     ddkappa: 0
   }
   v: 4.4839472782408771
-  a: 0.04729621650448599
+  a: 0.047296216504485018
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 586351.2828711929
-    y: 4140723.8241375531
-    theta: 1.30479812998378
-    kappa: 0.0027015962793888143
-    s: 24.531270843533576
-    dkappa: 0.00027840198390798249
+    x: 586351.44134056161
+    y: 4140723.7807356622
+    theta: 1.3047953628612081
+    kappa: 0.0026986139313037259
+    s: 24.531270843533584
+    dkappa: 0.00027743561943380072
     ddkappa: 0
   }
   v: 4.4882905932493369
-  a: 0.0396212690659564
+  a: 0.039621269065955596
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 586351.40042760177
-    y: 4140724.257490492
-    theta: 1.305885901021508
-    kappa: 0.0027748902536533912
-    s: 24.980285630620656
-    dkappa: 0.00021878510943497929
+    x: 586351.5588984791
+    y: 4140724.2140881922
+    theta: 1.3058818087715753
+    kappa: 0.0027716403551687006
+    s: 24.980285630620664
+    dkappa: 0.00021802597955027992
     ddkappa: 0
   }
   v: 4.4918828957939674
-  a: 0.032287233824389812
+  a: 0.032287233824389257
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 586351.51798380446
-    y: 4140724.691178252
-    theta: 1.3069917542835485
-    kappa: 0.0028445596990509981
-    s: 25.429623630472463
-    dkappa: 0.00015955745958346818
+    x: 586351.67645689507
+    y: 4140724.6477753529
+    theta: 1.3069861864990138
+    kappa: 0.0028410794613934428
+    s: 25.429623630472467
+    dkappa: 0.00015900007131276907
     ddkappa: 0
   }
   v: 4.4947616570734121
-  a: 0.0253617103587359
+  a: 0.025361710358735624
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 586351.63445858681
-    y: 4140725.1254208153
-    theta: 1.3083323266813149
-    kappa: 0.0028645072634193979
-    s: 25.879215644187315
-    dkappa: 0.00010614404949277613
+    x: 586351.79293443612
+    y: 4140725.0820171759
+    theta: 1.3083250901439216
+    kappa: 0.0028609142999922528
+    s: 25.879215644187319
+    dkappa: 0.00010575767783412478
     ddkappa: 0
   }
   v: 4.49697110824421
-  a: 0.018912298247944364
+  a: 0.018912298247944392
   relative_time: 5.7999999999999963
 }
 trajectory_point {
   path_point {
-    x: 586351.75098248222
-    y: 4140725.5598464822
-    theta: 1.3096734643481369
-    kappa: 0.0028844632389268795
-    s: 26.328997233977095
-    dkappa: 5.2708116972137438e-05
+    x: 586351.90946109139
+    y: 4140725.5164421028
+    theta: 1.3096645583542363
+    kappa: 0.0028807575021979194
+    s: 26.328997233977102
+    dkappa: 5.2492834036818963e-05
     ddkappa: 0
   }
   v: 4.4985622404207941
-  a: 0.013006597070964784
+  a: 0.013006597070965228
   relative_time: 5.8999999999999959
 }
 trajectory_point {
   path_point {
-    x: 586351.86648527207
-    y: 4140725.9946796019
-    theta: 1.3110041161371484
-    kappa: 0.002864649261839697
-    s: 26.778909399163009
-    dkappa: 4.6105020405950821e-06
+    x: 586352.024969138
+    y: 4140725.9512738246
+    theta: 1.3109935416127509
+    kappa: 0.0028610010913853968
+    s: 26.778909399163016
+    dkappa: 4.54066073926525e-06
     ddkappa: 0
   }
   v: 4.4995928046754949
-  a: 0.0077122064067468663
+  a: 0.0077122064067477545
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 586351.98181294184
-    y: 4140726.4296398256
-    theta: 1.3123329867937503
-    kappa: 0.0028374770645680746
-    s: 27.228899252171406
-    dkappa: -4.2505459352641232e-05
+    x: 586352.140300801
+    y: 4140726.38623299
+    theta: 1.3123207258768348
+    kappa: 0.002833853014280734
+    s: 27.228899252171413
+    dkappa: -4.242584643278078e-05
     ddkappa: 0
   }
   v: 4.5001273120385346
-  a: 0.0030967258342402227
+  a: 0.0030967258342416938
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 586352.09638776176
-    y: 4140726.8648311975
-    theta: 1.313627033830107
-    kappa: 0.0027850006932392834
-    s: 27.678920694529534
-    dkappa: -8.5674816916169171e-05
+    x: 586352.25488258339
+    y: 4140726.8214225294
+    theta: 1.3136132198771016
+    kappa: 0.0027815429963178426
+    s: 27.678920694529538
+    dkappa: -8.5472319127391665e-05
     ddkappa: 0
   }
-  v: 4.5002370334980313
-  a: -0.00077224506760541906
+  v: 4.5002370334980322
+  a: -0.00077224506760333739
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
-    x: 586352.21049553249
-    y: 4140727.3001384176
-    theta: 1.3148997117988568
-    kappa: 0.0027170548916433794
-    s: 28.128935092861362
-    dkappa: -0.00012642845334293463
+    x: 586352.36899565544
+    y: 4140727.2567283595
+    theta: 1.3148842669178
+    kappa: 0.002713735118405365
+    s: 28.128935092861369
+    dkappa: -0.00012610133065863374
     ddkappa: 0
   }
-  v: 4.5
-  a: -0.0038271067198404991
+  v: 4.5000000000000009
+  a: -0.0038271067198376958
   relative_time: 6.2999999999999945
 }
 trajectory_point {
   path_point {
-    x: 586352.32413184992
-    y: 4140727.7355297571
-    theta: 1.3161353062418968
-    kappa: 0.002635456312235294
-    s: 28.578911954883345
-    dkappa: -0.00016463881894434771
+    x: 586352.48264046665
+    y: 4140727.6921174829
+    theta: 1.3161185238672832
+    kappa: 0.0026323904353128891
+    s: 28.578911954883353
+    dkappa: -0.00016421319792406659
     ddkappa: 0
   }
-  v: 4.4995010024483495
-  a: -0.0060002595435153187
+  v: 4.49950100244835
+  a: -0.0060002595435116549
   relative_time: 6.3999999999999941
 }
 trajectory_point {
   path_point {
-    x: 586352.4370527548
-    y: 4140728.1710464172
-    theta: 1.3173146671765616
-    kappa: 0.0025331573817486777
-    s: 29.028829605400233
-    dkappa: -0.00019899280283798242
+    x: 586352.59556781955
+    y: 4140728.1276324717
+    theta: 1.3172964415811999
+    kappa: 0.0025303179612045385
+    s: 29.02882960540024
+    dkappa: -0.00019847056500681555
     ddkappa: 0
   }
-  v: 4.4988315917048816
-  a: -0.00722410395968015
+  v: 4.4988315917048824
+  a: -0.0072241039596755707
   relative_time: 6.4999999999999938
 }
 trajectory_point {
   path_point {
-    x: 586352.54977072624
-    y: 4140728.6065416983
-    theta: 1.3184723910653231
-    kappa: 0.0024260219162863085
-    s: 29.478675862300847
-    dkappa: -0.00023222352344356786
+    x: 586352.70829548431
+    y: 4140728.5631252467
+    theta: 1.3184531191271418
+    kappa: 0.0024235009246575531
+    s: 29.478675862300854
+    dkappa: -0.00023162717217127673
     ddkappa: 0
   }
-  v: 4.4980900785892954
-  a: -0.0074310403893854621
+  v: 4.4980900785892972
+  a: -0.0074310403893798555
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
-    x: 586352.6615907551
-    y: 4140729.0421928177
-    theta: 1.3195279546333207
-    kappa: 0.0022958341999627847
-    s: 29.928448712553877
-    dkappa: -0.00026013481835099838
+    x: 586352.82012299821
+    y: 4140728.9987744452
+    theta: 1.3195074790259458
+    kappa: 0.0022936069735165843
+    s: 29.928448712553884
+    dkappa: -0.00025946497527541289
     ddkappa: 0
   }
-  v: 4.4973815338791843
-  a: -0.0065534692536814727
+  v: 4.4973815338791869
+  a: -0.0065534692536747558
   relative_time: 6.6999999999999931
 }
 trajectory_point {
   path_point {
-    x: 586352.77339472971
-    y: 4140729.47778139
-    theta: 1.3205833666523787
-    kappa: 0.0021656651748966928
-    s: 30.378156988203653
-    dkappa: -0.00028804210598943984
+    x: 586352.93193445681
+    y: 4140729.4343610969
+    theta: 1.3205616875486221
+    kappa: 0.002163731671456713
+    s: 30.37815698820366
+    dkappa: -0.00028729878166189489
     ddkappa: 0
   }
-  v: 4.4968177883100378
-  a: -0.0045237909736185666
+  v: 4.49681778831004
+  a: -0.004523790973610684
   relative_time: 6.7999999999999927
 }
 trajectory_point {
   path_point {
-    x: 586352.88429567765
-    y: 4140729.9135550656
-    theta: 1.3214988669126484
-    kappa: 0.0020151474568115644
-    s: 30.827821042365962
-    dkappa: -0.00030980660480047704
+    x: 586353.04284684162
+    y: 4140729.8701318582
+    theta: 1.3214767568616379
+    kappa: 0.00201362361554993
+    s: 30.827821042365965
+    dkappa: -0.00030903339472580684
     ddkappa: 0
   }
-  v: 4.4965174325752377
-  a: -0.001274405970247182
+  v: 4.4965174325752413
+  a: -0.0012744059702378561
   relative_time: 6.8999999999999924
 }
 trajectory_point {
   path_point {
-    x: 586352.99514627422
-    y: 4140730.349329561
-    theta: 1.3224069030146612
-    kappa: 0.0018635500337320125
-    s: 31.277473425223818
-    dkappa: -0.00033124377363436241
+    x: 586353.15370575
+    y: 4140730.3059042385
+    theta: 1.3223838809614623
+    kappa: 0.0018623659798458561
+    s: 31.277473425223821
+    dkappa: -0.00033042010192270291
     ddkappa: 0
   }
-  v: 4.4966058173260626
-  a: 0.0032622853353824893
+  v: 4.4966058173260679
+  a: 0.0032622853353931752
   relative_time: 6.999999999999992
 }
 decision {
diff --git a/modules/planning/testdata/sunnyvale_big_loop_test/result_crosswalk_02_0.pb.txt b/modules/planning/testdata/sunnyvale_big_loop_test/result_crosswalk_02_0.pb.txt
index 51463a9408d50f95cb9013ae564079a8e46039f9..6ebadba5f0e6c8075f77ab77dce01eb0504a2297 100644
--- a/modules/planning/testdata/sunnyvale_big_loop_test/result_crosswalk_02_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_big_loop_test/result_crosswalk_02_0.pb.txt
@@ -24,8 +24,8 @@ decision {
         follow {
           distance_s: -3
           fence_point {
-            x: 586260.30218708527
-            y: 4141284.1751651387
+            x: 586260.27769637457
+            y: 4141284.2721197936
             z: 0
           }
           fence_heading: -1.3299784970826325
@@ -39,8 +39,8 @@ decision {
         follow {
           distance_s: -3
           fence_point {
-            x: 586260.25445978157
-            y: 4141284.3693824667
+            x: 586260.24285396188
+            y: 4141284.4180168672
             z: 0
           }
           fence_heading: -1.3430782827827037
diff --git a/modules/planning/testdata/sunnyvale_big_loop_test/result_crosswalk_02_1.pb.txt b/modules/planning/testdata/sunnyvale_big_loop_test/result_crosswalk_02_1.pb.txt
index f7a366485c9476de886c56145cf78e27510c6be2..f00ba840c5c5ebecfd5020385112df252c6e63ad 100644
--- a/modules/planning/testdata/sunnyvale_big_loop_test/result_crosswalk_02_1.pb.txt
+++ b/modules/planning/testdata/sunnyvale_big_loop_test/result_crosswalk_02_1.pb.txt
@@ -17,8 +17,8 @@ decision {
         follow {
           distance_s: -3
           fence_point {
-            x: 586260.30218708527
-            y: 4141284.1751651387
+            x: 586260.27769637457
+            y: 4141284.2721197936
             z: 0
           }
           fence_heading: -1.3299784970826325
@@ -32,8 +32,8 @@ decision {
         follow {
           distance_s: -3
           fence_point {
-            x: 586260.25445978157
-            y: 4141284.3693824667
+            x: 586260.24285396188
+            y: 4141284.4180168672
             z: 0
           }
           fence_heading: -1.3430782827827037
diff --git a/modules/planning/testdata/sunnyvale_big_loop_test/result_keep_clear_02_0.pb.txt b/modules/planning/testdata/sunnyvale_big_loop_test/result_keep_clear_02_0.pb.txt
index 20a4508895b027888c01e9e064378c0b4bf4c6d8..1a4fdec5cc12f632e01b39708e4bd405e3b42ebc 100644
--- a/modules/planning/testdata/sunnyvale_big_loop_test/result_keep_clear_02_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_big_loop_test/result_keep_clear_02_0.pb.txt
@@ -24,8 +24,8 @@ decision {
         follow {
           distance_s: -5.4097223281860352
           fence_point {
-            x: 586426.49758902891
-            y: 4140685.4610236813
+            x: 586426.49739531986
+            y: 4140685.4601982441
             z: 0
           }
           fence_heading: 1.3398228192102641
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
index 8bff1d79fc5d0fad4fa0535ca8160f525c6fc679..80eebb7e5b13c5804da3d1ec9e3010f77e179b72 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
@@ -1,7 +1,7 @@
 header {
   timestamp_sec: 1503436434.9700003
 }
-total_path_length: 42.246522607847787
+total_path_length: 42.246522607847773
 total_path_time: 5.7999999999999963
 is_replan: true
 gear: GEAR_DRIVE
@@ -17,731 +17,731 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.4222221374511719
-  a: 0.51007097649176225
+  a: 0.51007097649176214
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 587503.277776317
-    y: 4140719.236764343
-    theta: 2.3551578962997919
-    kappa: -0.00053369689995826889
+    x: 587503.27779276925
+    y: 4140719.2367804963
+    theta: 2.3550895225535768
+    kappa: -0.00066524907847594593
     s: 0.10854731956640416
-    dkappa: -0.00054326351476844574
+    dkappa: -0.00067717934033365858
     ddkappa: 0
   }
   v: 5.4325526594595255
-  a: 0.52292312417001063
+  a: 0.52292312417001041
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 587503.20157479728
-    y: 4140719.3143613781
-    theta: 2.3543593190561012
-    kappa: -0.0010684221699815441
+    x: 587503.20160773362
+    y: 4140719.314393715
+    theta: 2.3542224398156435
+    kappa: -0.0013317800123236044
     s: 0.21730379682903553
-    dkappa: -0.0010875738333239141
+    dkappa: -0.0013556635242260768
     ddkappa: 0
   }
   v: 5.4431367517298135
-  a: 0.53542866862444449
+  a: 0.53542866862444427
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 587503.1252232237
-    y: 4140719.3921112139
-    theta: 2.3535591692776556
-    kappa: -0.00160420040523455
+    x: 587503.12527267681
+    y: 4140719.3921597674
+    theta: 2.3533536496439575
+    kappa: -0.0019996234592265195
     s: 0.3262744341388637
-    dkappa: -0.0016329559917039665
+    dkappa: -0.0020354837591611726
     ddkappa: 0
   }
   v: 5.4539675221539117
-  a: 0.54759160549016794
+  a: 0.54759160549016772
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 587503.04871818749
-    y: 4140719.4700173223
-    theta: 2.352757411239466
-    kappa: -0.00214105552707962
+    x: 587503.04878419021
+    y: 4140719.4700821252
+    theta: 2.35248311324888
+    kappa: -0.0026688092369787433
     s: 0.43546409680382281
-    dkappa: -0.0021794343400657443
+    dkappa: -0.002716670397671192
     ddkappa: 0
   }
   v: 5.4650381585363963
-  a: 0.55941593040228488
+  a: 0.55941593040228466
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 587502.97205637442
-    y: 4140719.54808308
-    theta: 2.3519540102110845
-    kappa: -0.0026790107909348717
+    x: 587502.972138961
+    y: 4140719.5481641646
+    theta: 2.3516107929206314
+    kappa: -0.0033393663332799838
     s: 0.54487751468706569
-    dkappa: -0.0027270325506849979
+    dkappa: -0.0033992529473075191
     ddkappa: 0
   }
   v: 5.4763419285945467
-  a: 0.57090563899589941
+  a: 0.57090563899589919
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 587502.89523456455
-    y: 4140719.6263117651
-    theta: 2.3511489324448718
-    kappa: -0.003218088794132368
+    x: 587502.89533376915
+    y: 4140719.6264091646
+    theta: 2.3507366520165522
+    kappa: -0.00401132291553075
     s: 0.65451928380521729
-    dkappa: -0.0032757736259551133
+    dkappa: -0.004083260080611481
     ddkappa: 0
   }
   v: 5.4878721799583428
-  a: 0.58206472690611544
+  a: 0.58206472690611522
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 587502.81824962993
-    y: 4140719.7047065608
-    theta: 2.3503421451642623
-    kappa: -0.0037583114837762958
+    x: 587502.81836548808
+    y: 4140719.7048203112
+    theta: 2.3498606549483565
+    kappa: -0.0046847063406275074
     s: 0.76439386792662989
-    dkappa: -0.0038256799063861452
+    dkappa: -0.0047687196450851661
     ddkappa: 0
   }
   v: 5.4996223401704691
-  a: 0.59289718976803707
+  a: 0.59289718976803685
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 587502.74109853467
-    y: 4140719.7832705607
-    theta: 2.3495336165520282
-    kappa: -0.00429970016460113
+    x: 587502.74123108236
+    y: 4140719.7834006967
+    theta: 2.3489827671693941
+    kappa: -0.0053595431647578138
     s: 0.87450560016963585
-    dkappa: -0.0043767730786038392
+    dkappa: -0.0054556586731622208
     ddkappa: 0
   }
-  v: 5.51158591668631
-  a: 0.60340702321676809
+  v: 5.5115859166863093
+  a: 0.60340702321676787
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 587502.66378613852
-    y: 4140719.8620143947
-    theta: 2.3487327955025794
-    kappa: -0.0048395517120181581
+    x: 587502.66393746948
+    y: 4140719.8621629491
+    theta: 2.3481099892990773
+    kappa: -0.0060321996951823
     s: 0.9848586846008025
-    dkappa: -0.0049139717974077323
+    dkappa: -0.006124961023867591
     ddkappa: 0
   }
   v: 5.5237564968739523
-  a: 0.61359822288741284
+  a: 0.6135982228874125
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 587502.58664372808
-    y: 4140719.9412676576
-    theta: 2.3483454725074075
-    kappa: -0.0052612828908371358
+    x: 587502.58690256334
+    y: 4140719.9415207109
+    theta: 2.3475423892528839
+    kappa: -0.006546575886204168
     s: 1.095457197833186
-    dkappa: -0.004790776503765859
+    dkappa: -0.0059599730438076367
     ddkappa: 0
   }
   v: 5.5361277480141862
-  a: 0.623474784415075
+  a: 0.62347478441507465
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 587502.50932737533
-    y: 4140720.0206996221
-    theta: 2.3479572761692324
-    kappa: -0.0056839649968641144
-    s: 1.2063050906245854
-    dkappa: -0.004667303427113135
+    x: 587502.50969395763
+    y: 4140720.0210574106
+    theta: 2.346973509371681
+    kappa: -0.0070621119021091767
+    s: 1.2063050906245851
+    dkappa: -0.005794613045819884
     ddkappa: 0
   }
   v: 5.5486934173005027
-  a: 0.63304070343485863
+  a: 0.63304070343485841
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587502.43183441157
-    y: 4140720.10031303
-    theta: 2.3475681930874064
-    kappa: -0.0061076126212056478
+    x: 587502.4323089869
+    y: 4140720.1007757923
+    theta: 2.3464033300175737
+    kappa: -0.0075788255393472673
     s: 1.3174061894757962
-    dkappa: -0.0045435483051238261
+    dkappa: -0.0056288753216301567
     ddkappa: 0
   }
   v: 5.5614473318390951
-  a: 0.64229997558186791
+  a: 0.64229997558186758
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 587502.35416225332
-    y: 4140720.1801105356
-    theta: 2.3471782102908385
-    kappa: -0.0065322398872493543
+    x: 587502.35474507126
+    y: 4140720.1806785157
+    theta: 2.34583183218216
+    kappa: -0.0080967340239019372
     s: 1.428764198228865
-    dkappa: -0.0044195070121013473
+    dkappa: -0.0054627543459434061
     ddkappa: 0
   }
   v: 5.57438339864886
-  a: 0.6512565964912066
+  a: 0.65125659649120626
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587502.2763084016
-    y: 4140720.2600947069
-    theta: 2.346787315232401
-    kappa: -0.0069578604567583392
+    x: 587502.27699971525
+    y: 4140720.2607681495
+    theta: 2.3452589974783322
+    kappa: -0.0086158540187234768
     s: 1.540382699665344
-    dkappa: -0.0042951755571979666
+    dkappa: -0.0052962447740594722
     ddkappa: 0
   }
   v: 5.5874956046613935
-  a: 0.6599145617979788
+  a: 0.65991456179797847
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587502.19827044057
-    y: 4140720.3402680252
-    theta: 2.3463954957833328
-    kappa: -0.0073844875359656021
+    x: 587502.19907050673
+    y: 4140720.34104718
+    theta: 2.344684808132067
+    kappa: -0.0091362016311621851
     s: 1.6522651571045432
-    dkappa: -0.004170550082634528
+    dkappa: -0.005129341439488855
     ddkappa: 0
   }
   v: 5.6007780167209962
-  a: 0.66827786713728854
+  a: 0.66827786713728821
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 587502.1200460369
-    y: 4140720.4206328872
-    theta: 2.346002740227636
-    kappa: -0.0078121338816684628
+    x: 587502.12095511542
+    y: 4140720.4215180073
+    theta: 2.3441092469742344
+    kappa: -0.0096577924204015991
     s: 1.7644149160017861
-    dkappa: -0.0040456268619201491
+    dkappa: -0.00496203935156848
     ddkappa: 0
   }
   v: 5.6142247815846691
-  a: 0.6763505081442398
+  a: 0.67635050814423947
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 587502.0416329382
-    y: 4140720.5011916077
-    theta: 2.3456090372564837
-    kappa: -0.0082408118073229817
+    x: 587502.042651292
+    y: 4140720.5021829484
+    theta: 2.3435322974323842
+    kappa: -0.010180641404891727
     s: 1.8768352055466635
-    dkappa: -0.0039204022980719394
+    dkappa: -0.0047943336930774613
     ddkappa: 0
   }
-  v: 5.6278301259221175
-  a: 0.68413648045393649
+  v: 5.6278301259221166
+  a: 0.68413648045393627
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 587501.96313028364
-    y: 4140720.58204419
-    theta: 2.3450380179570312
-    kappa: -0.008654836992125936
+    x: 587501.96428250941
+    y: 4140720.583164922
+    theta: 2.3427655308917839
+    kappa: -0.010683815983891296
     s: 1.9895291402612874
-    dkappa: -0.0037972967787581035
+    dkappa: -0.00462965058726876
     ddkappa: 0
   }
   v: 5.6415883563157454
-  a: 0.69163977970148283
+  a: 0.69163977970148249
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587501.88503003714
-    y: 4140720.6636695764
-    theta: 2.3434296391481748
-    kappa: -0.0089776763422147448
+    x: 587501.88645535614
+    y: 4140720.6650507352
+    theta: 2.3408961574102047
+    kappa: -0.011065850897481407
     s: 2.1024997215985439
-    dkappa: -0.0036881272130836473
+    dkappa: -0.00448460609296056
     ddkappa: 0
   }
   v: 5.6554938592606625
-  a: 0.69886440152198248
+  a: 0.69886440152198226
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587501.80673653772
-    y: 4140720.745496938
-    theta: 2.3418172805355004
-    kappa: -0.0093013145322729529
+    x: 587501.80843562586
+    y: 4140720.7471391684
+    theta: 2.3390221583145117
+    kappa: -0.011448831125695023
     s: 2.2157498395403494
-    dkappa: -0.0035786875161117546
+    dkappa: -0.0043392026977334885
     ddkappa: 0
   }
   v: 5.669541101164679
-  a: 0.70581434155053979
+  a: 0.70581434155053946
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587501.72824786371
-    y: 4140720.8275282849
-    theta: 2.3402009025381822
-    kappa: -0.00962575950710032
-    s: 2.3292822741959038
-    dkappa: -0.0034689750012729644
+    x: 587501.73022140353
+    y: 4140720.8294322374
+    theta: 2.337143487601022
+    kappa: -0.011832766070083997
+    s: 2.3292822741959034
+    dkappa: -0.0041934368321668329
     ddkappa: 0
   }
   v: 5.6837246283483056
-  a: 0.71249359542225854
+  a: 0.71249359542225821
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 587501.64956216782
-    y: 4140720.9097655453
-    theta: 2.3385804671169161
-    kappa: -0.0099510189020774846
+    x: 587501.651810848
+    y: 4140720.911931877
+    theta: 2.3352601010577239
+    kappa: -0.012217664766046219
     s: 2.4430996973999424
-    dkappa: -0.0033589870866292705
+    dkappa: -0.0040473050658549614
     ddkappa: 0
   }
-  v: 5.6980390670447578
-  a: 0.71890615877224273
+  v: 5.6980390670447569
+  a: 0.71890615877224229
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587501.5706776767
-    y: 4140720.9922105744
-    theta: 2.3369559377511617
-    kappa: -0.010277100047733356
-    s: 2.5572046743109942
-    dkappa: -0.0032487212933296362
+    x: 587501.57320219243
+    y: 4140720.9946399485
+    theta: 2.3333719562378192
+    kappa: -0.012603535888230474
+    s: 2.5572046743109937
+    dkappa: -0.0039008041053553086
     ddkappa: 0
   }
   v: 5.7124791233999508
-  a: 0.72505602723559637
+  a: 0.725056027235596
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587501.49159268942
-    y: 4140721.0748651484
-    theta: 2.3353272794163891
-    kappa: -0.010604009974312477
+    x: 587501.494393742
+    y: 4140721.0775582334
+    theta: 2.3314790124332809
+    kappa: -0.012990387755941302
     s: 2.6715996650096314
-    dkappa: -0.0031381752440655243
+    dkappa: -0.0037539307921363469
     ddkappa: 0
   }
   v: 5.7270395834725045
-  a: 0.73094719644742356
+  a: 0.73094719644742323
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587501.41230557719
-    y: 4140721.1577309719
-    theta: 2.3336944585613262
-    kappa: -0.010931755416342417
+    x: 587501.415383873
+    y: 4140721.1606884412
+    theta: 2.3295812306484072
+    kappa: -0.013378228338543817
     s: 2.7862870260967267
-    dkappa: -0.0030273466615264111
+    dkappa: -0.0036066821005255722
     ddkappa: 0
   }
   v: 5.7417153132337386
-  a: 0.7365836620428281
+  a: 0.73658366204282788
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587501.33281478088
-    y: 4140721.2408096725
-    theta: 2.3320574430851995
-    kappa: -0.011260342817201136
+    x: 587501.3361710324
+    y: 4140721.2440322051
+    theta: 2.3276785735733725
+    kappa: -0.013767065260868584
     s: 2.9012690122917073
-    dkappa: -0.0029162333668553086
+    dkappa: -0.0034590551356574782
     ddkappa: 0
   }
   v: 5.7565012585676758
-  a: 0.74196941965691421
+  a: 0.741969419656914
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587501.25356422469
-    y: 4140721.3245242308
-    theta: 2.3306448482645195
-    kappa: -0.011541779285952103
+    x: 587501.25730310287
+    y: 4140721.3281061193
+    theta: 2.3259806909534779
+    kappa: -0.014093620970802518
     s: 3.0165477780308052
-    dkappa: -0.0028094817557373346
+    dkappa: -0.0033180818420988764
     ddkappa: 0
   }
   v: 5.77139244527104
-  a: 0.74710846492478589
+  a: 0.74710846492478544
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587501.17460214056
-    y: 4140721.4089231123
-    theta: 2.3294821333939453
-    kappa: -0.011770719949900494
+    x: 587501.17883343331
+    y: 4140721.412963016
+    theta: 2.3245084518268726
+    kappa: -0.014351593700170995
     s: 3.1321253790653159
-    dkappa: -0.0027076080358270681
+    dkappa: -0.0031844608545946976
     ddkappa: 0
   }
   v: 5.78638397905326
-  a: 0.75200479348154692
+  a: 0.75200479348154659
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587501.0954345552
-    y: 4140721.493541643
-    theta: 2.328316392525192
-    kappa: -0.012000256438371137
+    x: 587501.10015954415
+    y: 4140721.4980407543
+    theta: 2.3230323811564988
+    kappa: -0.01461023781082919
     s: 3.2480037740598506
-    dkappa: -0.0026054691866931458
+    dkappa: -0.0030504921147093134
     ddkappa: 0
   }
   v: 5.801471045536462
-  a: 0.75666240096230131
+  a: 0.756662400962301
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587501.016060196
-    y: 4140721.5783811854
-    theta: 2.3271476069146377
-    kappa: -0.012230392442016791
+    x: 587501.02128017042
+    y: 4140721.583340703
+    theta: 2.321552455209031
+    kappa: -0.014869557461443143
     s: 3.3641848261905887
-    dkappa: -0.0025030635660736903
+    dkappa: -0.0029161734683969685
     ddkappa: 0
   }
   v: 5.8166489102554788
-  a: 0.76108528300215328
+  a: 0.76108528300215306
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587500.93647785415
-    y: 4140721.6634430317
-    theta: 2.3259757587632048
-    kappa: -0.012461131465508845
-    s: 3.4806703047435343
-    dkappa: -0.0024003896144645649
+    x: 587500.94219411118
+    y: 4140721.6688641612
+    theta: 2.3200686514471256
+    kappa: -0.015129556601113151
+    s: 3.4806703047435334
+    dkappa: -0.0027815028701597389
     ddkappa: 0
   }
   v: 5.8319129186578431
-  a: 0.76527743523620673
+  a: 0.76527743523620639
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587500.85668638372
-    y: 4140721.7487284048
-    theta: 2.3248008312002675
-    kappa: -0.012692476830703226
+    x: 587500.86290022742
+    y: 4140721.75461236
+    theta: 2.3185809485090632
+    kappa: -0.015390238972941145
     s: 3.5974618867127681
-    dkappa: -0.0022974458537106278
+    dkappa: -0.002646478381199766
     ddkappa: 0
   }
   v: 5.8472584961037892
-  a: 0.76924285329956565
+  a: 0.76924285329956521
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587500.77668470109
-    y: 4140721.834238464
-    theta: 2.3236228082675892
-    kappa: -0.012924431679806267
+    x: 587500.78339744254
+    y: 4140721.8405864649
+    theta: 2.3170893261883894
+    kappa: -0.015651608117598005
     s: 3.7145611583987042
-    dkappa: -0.002194230885596977
+    dkappa: -0.0025110981675714968
     ddkappa: 0
   }
   v: 5.8626811478662546
-  a: 0.77298553282733384
+  a: 0.77298553282733351
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587500.69647178345
-    y: 4140721.9199743019
-    theta: 2.3224416749032315
-    kappa: -0.013156998978540588
+    x: 587500.70368473977
+    y: 4140721.9267875734
+    theta: 2.3155937654135554
+    kappa: -0.015913667376890938
     s: 3.8319696170063411
-    dkappa: -0.0020907433904402022
+    dkappa: -0.0023753604983339114
     ddkappa: 0
   }
-  v: 5.87817645913088
-  a: 0.77650946945461563
+  v: 5.8781764591308789
+  a: 0.7765094694546153
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587500.61604666756
-    y: 4140722.005936949
-    theta: 2.3212574169254827
-    kappa: -0.013390181519311003
+    x: 587500.62376116216
+    y: 4140722.0132167218
+    theta: 2.3140942482275619
+    kappa: -0.016176419897330818
     s: 3.9496886722435174
-    dkappa: -0.0019869821256796329
+    dkappa: -0.0022392637437027635
     ddkappa: 0
   }
-  v: 5.8937400949960042
-  a: 0.7798186588165148
+  v: 5.8937400949960033
+  a: 0.77981865881651447
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587500.53648920974
-    y: 4140722.0931251734
-    theta: 2.319251171313228
-    kappa: -0.013540176172123957
+    x: 587500.544949072
+    y: 4140722.1010790374
+    theta: 2.3117506450085159
+    kappa: -0.016329598230725117
     s: 4.0677196479191666
-    dkappa: -0.0018965018257951262
+    dkappa: -0.0021209827652344227
     ddkappa: 0
   }
   v: 5.9093678004726717
-  a: 0.78291709654813535
+  a: 0.78291709654813513
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 587500.45678866678
-    y: 4140722.1806075023
-    theta: 2.3171880841840871
-    kappa: -0.013685296089195092
+    x: 587500.46600214276
+    y: 4140722.18924205
+    theta: 2.3093536740845781
+    kappa: -0.0164769942705707
     s: 4.1860637835415684
-    dkappa: -0.0018066343431542031
+    dkappa: -0.0020034080772748236
     ddkappa: 0
   }
   v: 5.9250554004846281
-  a: 0.78580877828458151
+  a: 0.78580877828458107
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 587500.37687644269
-    y: 4140722.26832218
-    theta: 2.31511951758743
-    kappa: -0.013830801438301676
+    x: 587500.386845534
+    y: 4140722.2776392195
+    theta: 2.3069503369126378
+    kappa: -0.016624781787732433
     s: 4.3047222359166053
-    dkappa: -0.0017165281764824859
+    dkappa: -0.0018855211161813066
     ddkappa: 0
   }
   v: 5.9407987998683209
-  a: 0.788497699660957
+  a: 0.78849769966095673
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 587500.296751813
-    y: 4140722.3562700022
-    theta: 2.3130454527706057
-    kappa: -0.013976693538526788
+    x: 587500.30747852824
+    y: 4140722.3662713482
+    theta: 2.3045406117051712
+    kappa: -0.016772962121982461
     s: 4.4236960807460157
-    dkappa: -0.0016261825089198339
+    dkappa: -0.0017673208132461117
     ddkappa: 0
   }
-  v: 5.9565939833729
-  a: 0.79098785631236579
+  v: 5.9565939833728994
+  a: 0.79098785631236557
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 587500.21641410689
-    y: 4140722.4444517056
-    theta: 2.3109658723669826
-    kappa: -0.014122973611459134
+    x: 587500.22790046071
+    y: 4140722.4551391769
+    theta: 2.3021244782849841
+    kappa: -0.016921536514069403
     s: 4.5429863142256481
-    dkappa: -0.001535596583980827
+    dkappa: -0.0016488061787504368
     ddkappa: 0
   }
   v: 5.9724370156602165
-  a: 0.79328324387391214
+  a: 0.79328324387391191
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 587500.13586270588
-    y: 4140722.5328679681
-    theta: 2.3088807603680905
-    kappa: -0.014269642783152897
-    s: 4.6625938546437151
-    dkappa: -0.0014447697043410937
+    x: 587500.14811071893
+    y: 4140722.54424339
+    theta: 2.2997019180528442
+    kappa: -0.017070506107708959
+    s: 4.6625938546437142
+    dkappa: -0.0015299763003765797
     ddkappa: 0
   }
   v: 5.9883240413048249
-  a: 0.79538785798069989
+  a: 0.79538785798069966
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 587500.055097043
-    y: 4140722.6215194133
-    theta: 2.306790102095758
-    kappa: -0.014416702086087613
+    x: 587500.06810874119
+    y: 4140722.6335846153
+    theta: 2.2972729139551067
+    kappa: -0.017219871951574515
     s: 4.7825195439790464
-    dkappa: -0.0013537012306236388
+    dkappa: -0.0014108303416200698
     ddkappa: 0
   }
   v: 6.00425128479398
-  a: 0.79730569426783315
+  a: 0.79730569426783282
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 587499.97411660163
-    y: 4140722.710406607
-    theta: 2.3046938841742453
-    kappa: -0.014564152461128028
+    x: 587499.98789401562
+    y: 4140722.7231634241
+    theta: 2.2948374504513458
+    kappa: -0.017369635001287739
     s: 4.9027641494993457
-    dkappa: -0.0012623905801851674
+    dkappa: -0.0012913675402018126
     ddkappa: 0
   }
   v: 6.0202150505276411
-  a: 0.79904074837041561
+  a: 0.79904074837041539
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 587499.8937133553
-    y: 4140722.8002415034
-    theta: 2.3029025590735968
-    kappa: -0.014663055254179776
+    x: 587499.90843430511
+    y: 4140722.813832337
+    theta: 2.2927190595093148
+    kappa: -0.01745513734712402
     s: 5.0233283653594416
-    dkappa: -0.0011771893894872809
+    dkappa: -0.0011806112542907408
     ddkappa: 0
   }
   v: 6.0362117228184671
-  a: 0.8005970159235517
+  a: 0.80059701592355137
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 587499.8136146724
-    y: 4140722.8907801746
-    theta: 2.301309467891727
-    kappa: -0.014730222495409994
+    x: 587499.82936626545
+    y: 4140722.905272455
+    theta: 2.2907964508803644
+    kappa: -0.0175006163885134
     s: 5.1442128141995429
-    dkappa: -0.0010959151465775949
+    dkappa: -0.0010751783011245018
     ddkappa: 0
   }
   v: 6.0522377658918209
-  a: 0.801978492562345
+  a: 0.80197849256234477
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 587499.73330343526
-    y: 4140722.9815591043
-    theta: 2.2997121491943986
-    kappa: -0.014797567975371172
+    x: 587499.75008840626
+    y: 4140722.9969552225
+    theta: 2.2888687403101127
+    kappa: -0.017546216115619115
     s: 5.2654180487434941
-    dkappa: -0.001014425229811869
+    dkappa: -0.0009694655652036162
     ddkappa: 0
   }
-  v: 6.0682897238857665
-  a: 0.80318917392189981
+  v: 6.0682897238857656
+  a: 0.8031891739218997
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 587499.65277932282
-    y: 4140723.0725786556
-    theta: 2.2981105965978106
-    kappa: -0.014865091963214522
+    x: 587499.67060041113
+    y: 4140723.0888810074
+    theta: 2.2869359200943205
+    kappa: -0.017591936710683872
     s: 5.3869445533970248
-    dkappa: -0.00093271931350959246
+    dkappa: -0.000863472624039277
     ddkappa: 0
   }
-  v: 6.08436422085107
-  a: 0.80423305563732006
+  v: 6.0843642208510689
+  a: 0.80423305563731984
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 587499.57204205822
-    y: 4140723.1638391404
-    theta: 2.2965048045974346
-    kappa: -0.014932794691019591
-    s: 5.50879274584601
-    dkappa: -0.00085079711684801643
+    x: 587499.59090200649
+    y: 4140723.1810501255
+    theta: 2.2849979835898964
+    kappa: -0.017637778330849081
+    s: 5.5087927458460095
+    dkappa: -0.00075719911333437494
     ddkappa: 0
   }
-  v: 6.1004579607511991
-  a: 0.80511413334370951
+  v: 6.1004579607511982
+  a: 0.8051141333437094
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 587499.49109140807
-    y: 4140723.255340823
-    theta: 2.2948947685469587
-    kappa: -0.015000676354682291
+    x: 587499.51099296182
+    y: 4140723.2734628431
+    theta: 2.2830549251894769
+    kappa: -0.01768374110875616
     s: 5.6309629786547211
-    dkappa: -0.00076865840278760077
+    dkappa: -0.00065064472558953521
     ddkappa: 0
   }
   v: 6.1165677274623249
-  a: 0.80583640267617263
+  a: 0.8058364026761724
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 587499.40992718062
-    y: 4140723.3470839211
-    theta: 2.2932804846372186
-    kappa: -0.015068737114802944
+    x: 587499.43087308807
+    y: 4140723.3661193787
+    theta: 2.2811067402960044
+    kappa: -0.017729825153147836
     s: 5.7534555408640768
-    dkappa: -0.00068630297699746033
+    dkappa: -0.00054380920870915881
     ddkappa: 0
   }
   v: 6.13269038477332
-  a: 0.806403859269813
+  a: 0.80640385926981284
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587499.00565869093
-    y: 4140723.8135733968
-    theta: 2.2831997314042392
-    kappa: -0.01516473040964599
+    x: 587499.03278836654
+    y: 4140723.8378896676
+    theta: 2.2694410959899933
+    kappa: -0.017642691273695454
     s: 6.370759164098315
-    dkappa: -0.00030782440994789589
+    dkappa: -5.545577268520395e-05
     ddkappa: 0
   }
   v: 6.2133928568260179
-  a: 0.8070587983843156
+  a: 0.80705879838431538
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587498.59892476874
-    y: 4140724.2885998129
-    theta: 2.2722947626345302
-    kappa: -0.015121020735949895
+    x: 587498.63284508674
+    y: 4140724.3186445581
+    theta: 2.2570270592260595
+    kappa: -0.017373230210638475
     s: 6.9961306495053925
-    dkappa: 5.4368033223120671e-05
+    dkappa: 0.0004100171424288615
     ddkappa: 0
   }
   v: 6.2939931120100177
@@ -750,12 +750,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587498.193745356
-    y: 4140724.7754698535
-    theta: 2.2636524525687953
-    kappa: -0.014784103193979103
+    x: 587498.23558742309
+    y: 4140724.8119995431
+    theta: 2.2470879632820342
+    kappa: -0.016732025420295383
     s: 7.6295442529563955
-    dkappa: 0.00037371186438523205
+    dkappa: 0.00081459284122330829
     ddkappa: 0
   }
   v: 6.3741890257242231
@@ -764,26 +764,26 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587497.78708007443
-    y: 4140725.2714528851
-    theta: 2.2541327652372081
-    kappa: -0.014300819737321255
+    x: 587497.83750561788
+    y: 4140725.3148923153
+    theta: 2.2364190022838093
+    kappa: -0.015905090909224304
     s: 8.2709465151342378
-    dkappa: 0.00067426608255158056
+    dkappa: 0.0011904485863247756
     ddkappa: 0
   }
   v: 6.4537284188063371
-  a: 0.791395594050027
+  a: 0.79139559405002713
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587497.3789120334
-    y: 4140725.7764365315
-    theta: 2.2437506523023067
-    kappa: -0.013673784047024476
+    x: 587497.438465577
+    y: 4140725.8271194911
+    theta: 2.2250476356061775
+    kappa: -0.014900221770066339
     s: 8.9202612560775449
-    dkappa: 0.00095640540812388019
+    dkappa: 0.0015390051921403819
     ddkappa: 0
   }
   v: 6.5324090575328633
@@ -792,604 +792,604 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587496.97273582336
-    y: 4140726.2930044918
-    theta: 2.2356950460720473
-    kappa: -0.012816394800426475
+    x: 587497.04229332553
+    y: 4140726.3513993858
+    theta: 2.2164102345279222
+    kappa: -0.013616974652988525
     s: 9.577394569724536
-    dkappa: 0.001188639545580765
+    dkappa: 0.0018142629286222903
     ddkappa: 0
   }
   v: 6.6100786536191043
-  a: 0.77125798364233333
+  a: 0.77125798364233367
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 587496.56491727091
-    y: 4140726.8180636242
-    theta: 2.2279613628616719
-    kappa: -0.01185649791433358
+    x: 587496.644797676
+    y: 4140726.88431465
+    theta: 2.2082874075801815
+    kappa: -0.012216157649989779
     s: 10.242239818456897
-    dkappa: 0.0014010531527900009
+    dkappa: 0.002058810605776039
     ddkappa: 0
   }
-  v: 6.6866348642191644
-  a: 0.75977889934703524
+  v: 6.6866348642191635
+  a: 0.75977889934703535
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587496.155965269
-    y: 4140727.3518590187
-    theta: 2.2205458840034371
-    kappa: -0.010782043744228304
-    s: 10.914682577083612
-    dkappa: 0.0015905876969568161
+    x: 587496.24644149013
+    y: 4140727.4260633122
+    theta: 2.2007106226235731
+    kappa: -0.010686737671012416
+    s: 10.91468257708361
+    dkappa: 0.00226855714513453
     ddkappa: 0
   }
-  v: 6.7620212334923844
+  v: 6.7620212334923835
   a: 0.74778069920322965
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587495.74808322126
-    y: 4140727.8958445536
-    theta: 2.213091492212234
-    kappa: -0.0095647873638579611
-    s: 11.594601674421483
-    dkappa: 0.0017287909382999868
+    x: 587495.84936376021
+    y: 4140727.9779852619
+    theta: 2.193537471489309
+    kappa: -0.0090117014765571449
+    s: 11.594601674421481
+    dkappa: 0.0024032244489125632
     ddkappa: 0
   }
-  v: 6.8361342270787562
+  v: 6.8361342270787553
   a: 0.73419549080315361
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587495.33847195166
-    y: 4140728.4476915905
-    theta: 2.2066661288218734
-    kappa: -0.0082879637098021187
-    s: 12.281861407328144
-    dkappa: 0.0018391008023418366
+    x: 587495.45048419363
+    y: 4140728.5376397939
+    theta: 2.1876619845743406
+    kappa: -0.0072875693079748243
+    s: 12.281861407328142
+    dkappa: 0.0024962947130419883
     ddkappa: 0
   }
-  v: 6.9088075580834287
-  a: 0.71902707531547094
+  v: 6.9088075580834278
+  a: 0.719027075315471
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587494.92728245328
-    y: 4140729.0073175863
-    theta: 2.2013444174389081
-    kappa: -0.0069543014879213123
-    s: 12.976310289787452
-    dkappa: 0.0019198114498905046
+    x: 587495.04990080011
+    y: 4140729.104906287
+    theta: 2.1831294279923217
+    kappa: -0.0055204056886999321
+    s: 12.976310289787449
+    dkappa: 0.0025454128172113094
     ddkappa: 0
   }
-  v: 6.9798947941215053
+  v: 6.9798947941215044
   a: 0.70251321921743692
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587494.515689266
-    y: 4140729.5753453556
-    theta: 2.1967261601684651
-    kappa: -0.0055805935665422125
-    s: 13.677783381377157
-    dkappa: 0.0019478701386801659
+    x: 587494.64828059915
+    y: 4140729.6800286327
+    theta: 2.1796872945672945
+    kappa: -0.0037590692463227377
+    s: 13.677783381377154
+    dkappa: 0.0025200541087651686
     ddkappa: 0
   }
   v: 7.0492732794558126
-  a: 0.68489168898630648
+  a: 0.68489168898630659
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587494.1019271689
-    y: 4140730.1502513755
-    theta: 2.1931904712917287
-    kappa: -0.0042145556978225706
-    s: 14.386104664933677
-    dkappa: 0.0019372355025064284
+    x: 587494.24392045918
+    y: 4140730.2615875453
+    theta: 2.1775877221666224
+    kappa: -0.0020525515956763235
+    s: 14.386104664933676
+    dkappa: 0.0024388855814729262
     ddkappa: 0
   }
-  v: 7.1168441349969056
-  a: 0.666400251099335
+  v: 7.1168441349969047
+  a: 0.66640025109933509
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587493.68578025326
-    y: 4140730.7316497378
-    theta: 2.190552635209654
-    kappa: -0.0028686962794655193
-    s: 15.101089424216877
-    dkappa: 0.0018905709447505164
+    x: 587493.83652858657
+    y: 4140730.849154572
+    theta: 2.176583169615963
+    kappa: -0.00041659598539655631
+    s: 15.101089424216875
+    dkappa: 0.0023065337865762581
     ddkappa: 0
   }
-  v: 7.1825322583030609
-  a: 0.64727667203377737
+  v: 7.18253225830306
+  a: 0.64727667203377748
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 587493.26730022556
-    y: 4140731.3193358388
-    theta: 2.1888060061021886
-    kappa: -0.0015814867149765383
-    s: 15.82254662157483
-    dkappa: 0.0017989512562446572
+    x: 587493.425803866
+    y: 4140731.4422866749
+    theta: 2.1766753618646018
+    kappa: 0.0010852019402594513
+    s: 15.822546621574826
+    dkappa: 0.0021109052943560766
     ddkappa: 0
   }
-  v: 7.246286323580283
-  a: 0.62775871826688889
+  v: 7.2462863235802821
+  a: 0.627758718266889
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587492.8458677287
-    y: 4140731.9126251796
-    theta: 2.1881362602713237
-    kappa: -0.00041760117641400285
-    s: 16.550281275608597
-    dkappa: 0.0016496689058057533
+    x: 587493.01084943151
+    y: 4140732.0401248075
+    theta: 2.1780063360606867
+    kappa: 0.0023535701734100503
+    s: 16.550281275608594
+    dkappa: 0.0018351901576816345
     ddkappa: 0
   }
-  v: 7.308078781682303
-  a: 0.60808415627592471
+  v: 7.3080787816823021
+  a: 0.60808415627592483
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 587492.42100524832
-    y: 4140732.5109370966
-    theta: 2.1880791216416959
-    kappa: 0.00062628756659830224
+    x: 587492.59146281029
+    y: 4140732.6422867356
+    theta: 2.179990605677725
+    kappa: 0.0034083338548783928
     s: 17.284096838837
-    dkappa: 0.0014602574934781147
+    dkappa: 0.0015030435223812545
     ddkappa: 0
   }
-  v: 7.3679058601105742
-  a: 0.58849075253813976
+  v: 7.3679058601105734
+  a: 0.58849075253814
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 587491.992589521
-    y: 4140733.1139437538
-    theta: 2.1886216153781461
-    kappa: 0.0015271072956839393
-    s: 18.023797575361396
-    dkappa: 0.0012296073032960035
+    x: 587492.16747620353
+    y: 4140733.24841503
+    theta: 2.1825811238362451
+    kappa: 0.0042158797449297868
+    s: 18.023797575361392
+    dkappa: 0.0011126141218899882
     ddkappa: 0
   }
-  v: 7.4257875630142784
-  a: 0.56921627353078941
+  v: 7.4257875630142776
+  a: 0.56921627353078963
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 587491.56009968848
-    y: 4140733.7210383192
-    theta: 2.19022896681673
-    kappa: 0.0021740162003202885
-    s: 18.769190938530439
-    dkappa: 0.00093512077354548386
+    x: 587491.73801636009
+    y: 4140733.8576567792
+    theta: 2.1861014894950586
+    kappa: 0.0046124913965742854
+    s: 18.769190938530436
+    dkappa: 0.00063610731700717064
     ddkappa: 0
   }
-  v: 7.4817676711903216
-  a: 0.55049848573112869
+  v: 7.4817676711903207
+  a: 0.55049848573112892
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 587491.12334574468
-    y: 4140734.3318531923
-    theta: 2.1918608557334807
-    kappa: 0.002545817041873919
+    x: 587491.303167558
+    y: 4140734.4698281609
+    theta: 2.1893837326428756
+    kappa: 0.0045650042012389726
     s: 19.520089948604863
-    dkappa: 0.00058805860825752586
+    dkappa: 8.5313617434076876e-05
     ddkappa: 0
   }
-  v: 7.535913742083336
-  a: 0.53257515561641278
+  v: 7.5359137420833351
+  a: 0.53257515561641289
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587490.6824477124
-    y: 4140734.9462513612
-    theta: 2.1938044472689935
-    kappa: 0.0026437709525032296
-    s: 20.276315570422256
-    dkappa: 0.00019210020974570196
+    x: 587490.86338312016
+    y: 4140735.0850225054
+    theta: 2.192654086473655
+    kappa: 0.0040875062724213783
+    s: 20.276315570422252
+    dkappa: -0.00053278316761710845
     ddkappa: 0
   }
   v: 7.5883171097856774
-  a: 0.5156840496638968
+  a: 0.51568404966389692
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587490.2374602831
-    y: 4140735.5640626336
-    theta: 2.1959766398376317
-    kappa: 0.0024451097233041978
+    x: 587490.41886586137
+    y: 4140735.7031719238
+    theta: 2.1957327515181131
+    kappa: 0.0031503279955538885
     s: 21.037699091061818
-    dkappa: -0.00026837403867196097
+    dkappa: -0.0012429798394079
     ddkappa: 0
   }
-  v: 7.6390928850374307
-  a: 0.50006293435083582
+  v: 7.63909288503743
+  a: 0.50006293435083593
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587489.78815051389
-    y: 4140736.184922494
-    theta: 2.1972754837408006
-    kappa: 0.002034998555475777
+    x: 587489.969595364
+    y: 4140736.3240601649
+    theta: 2.1972883577452116
+    kappa: 0.0020294824416180394
     s: 21.804084497509152
-    dkappa: -0.00058383182764153293
+    dkappa: -0.0015787891915631292
     ddkappa: 0
   }
-  v: 7.6883799552264023
-  a: 0.48594957615448503
+  v: 7.6883799552264014
+  a: 0.4859495761544852
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587489.33509730885
-    y: 4140736.8090729057
-    theta: 2.1992056552886643
-    kappa: 0.0021649351421450008
-    s: 22.575330854321027
-    dkappa: 0.00011911119470669142
+    x: 587489.51654585591
+    y: 4140736.9482132597
+    theta: 2.1992048358073006
+    kappa: 0.0021352715820283355
+    s: 22.575330854321024
+    dkappa: 3.6079620173934374e-05
     ddkappa: 0
   }
-  v: 7.7363409843881268
-  a: 0.47358174155209959
+  v: 7.7363409843881259
+  a: 0.4735817415520997
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 587488.87802666915
-    y: 4140737.4361591479
-    theta: 2.2006976596743861
-    kappa: 0.0023377638600513739
-    s: 23.351314681290148
-    dkappa: 0.00021055896193371613
+    x: 587489.05948668718
+    y: 4140737.5753078805
+    theta: 2.2006605495136116
+    kappa: 0.0022906604269738694
+    s: 23.351314681290145
+    dkappa: 0.00018608626210958829
     ddkappa: 0
   }
-  v: 7.7831624132058641
-  a: 0.46319719702093487
+  v: 7.7831624132058623
+  a: 0.46319719702093493
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587488.41713720281
-    y: 4140738.0661913268
-    theta: 2.2026599187779281
-    kappa: 0.0024768284474181862
-    s: 24.131930139458486
-    dkappa: 0.00018883561645727627
+    x: 587488.5986307353
+    y: 4140738.2053646096
+    theta: 2.2025845615068178
+    kappa: 0.0024159954710967647
+    s: 24.131930139458483
+    dkappa: 0.00017021855561906977
     ddkappa: 0
   }
-  v: 7.8289670925576438
-  a: 0.452427695701284
+  v: 7.828967092557642
+  a: 0.45242769570128405
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587487.95244923956
-    y: 4140738.6990462355
-    theta: 2.2047060741522833
-    kappa: 0.0025761307359092648
-    s: 24.917067779984151
-    dkappa: 0.00013178952071242483
+    x: 587488.13400494342
+    y: 4140738.8382651969
+    theta: 2.2045752454275505
+    kappa: 0.00250594341339376
+    s: 24.917067779984148
+    dkappa: 0.0001193903806323838
     ddkappa: 0
   }
-  v: 7.8735641683587323
+  v: 7.8735641683587305
   a: 0.43914467475996766
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587487.48399779445
-    y: 4140739.3345826254
-    theta: 2.2064749747410897
-    kappa: 0.0026373874330623817
-    s: 25.7065952704015
-    dkappa: 7.8850668246561092e-05
+    x: 587487.66564726864
+    y: 4140739.4738706946
+    theta: 2.2062908581622516
+    kappa: 0.0025619770327860083
+    s: 25.706595270401493
+    dkappa: 7.2177173992907535e-05
     ddkappa: 0
   }
-  v: 7.9167321532728163
+  v: 7.9167321532728145
   a: 0.42394542671403057
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587487.01146822528
-    y: 4140739.9723757142
-    theta: 2.2088852965226984
-    kappa: 0.0026543683681593327
-    s: 26.500361115973828
-    dkappa: 2.9507138755079259e-05
+    x: 587487.19326429022
+    y: 4140740.1117723635
+    theta: 2.2086403378807926
+    kappa: 0.0025785181914227636
+    s: 26.500361115973824
+    dkappa: 2.7896312641958687e-05
     ddkappa: 0
   }
-  v: 7.9583092892152862
+  v: 7.9583092892152845
   a: 0.40742724408051739
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 587486.53512901871
-    y: 4140740.6124134264
-    theta: 2.2112998848060759
-    kappa: 0.00263529574458983
-    s: 27.298200632618567
-    dkappa: -1.4732032606733544e-05
+    x: 587486.71710376325
+    y: 4140740.7519430527
+    theta: 2.210995189580343
+    kappa: 0.0025624361594608642
+    s: 27.298200632618563
+    dkappa: -1.183558706286026e-05
     ddkappa: 0
   }
-  v: 7.9981935473532353
+  v: 7.9981935473532335
   a: 0.39018741937647305
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587486.05518307677
-    y: 4140741.2546282387
-    theta: 2.2133019120139696
-    kappa: 0.0025863628735299697
-    s: 28.099941919832446
-    dkappa: -5.5422806260979169e-05
+    x: 587486.23737181467
+    y: 4140741.39431778
+    theta: 2.2129382616401889
+    kappa: 0.0025193765657205583
+    s: 28.099941919832439
+    dkappa: -4.8574159035282542e-05
     ddkappa: 0
   }
-  v: 8.036342628105464
+  v: 8.0363426281054622
   a: 0.37282324511894221
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587485.57173898187
-    y: 4140741.8988818196
-    theta: 2.2152081002770823
-    kappa: 0.0025098734205839216
-    s: 28.905411833616647
-    dkappa: -9.2266687736460663e-05
+    x: 587485.75417544181
+    y: 4140742.0387572059
+    theta: 2.2147960796516548
+    kappa: 0.0024513158344759084
+    s: 28.90541183361664
+    dkappa: -8.2007266692181593e-05
     ddkappa: 0
   }
-  v: 8.0727739611424756
+  v: 8.0727739611424738
   a: 0.35593201382496953
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 587485.08492899523
-    y: 4140742.5450593713
-    theta: 2.217427693741989
-    kappa: 0.002408503980870229
-    s: 29.714441959402
-    dkappa: -0.00012463368758902537
+    x: 587485.26764422609
+    y: 4140742.6851446843
+    theta: 2.2169692875783138
+    kappa: 0.0023605852116281149
+    s: 29.714441959401992
+    dkappa: -0.00011151536515362207
     ddkappa: 0
   }
-  v: 8.1075647053864781
+  v: 8.1075647053864763
   a: 0.34011101801159976
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587484.594657533
-    y: 4140743.1928870212
-    theta: 2.2189588751791014
-    kappa: 0.0022818432420923739
-    s: 30.526874584974131
-    dkappa: -0.00015231204198867958
+    x: 587484.77768435737
+    y: 4140743.3332080282
+    theta: 2.2184672767152289
+    kappa: 0.00224660467862353
+    s: 30.526874584974124
+    dkappa: -0.00013704887647960685
     ddkappa: 0
   }
-  v: 8.1408517490113823
+  v: 8.14085174901138
   a: 0.32595755019587769
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587484.10112927435
-    y: 4140743.8423378011
-    theta: 2.2209351446122607
-    kappa: 0.0021355381718453983
-    s: 31.342568673398649
-    dkappa: -0.00017503190152724343
+    x: 587484.284483165
+    y: 4140743.9829072347
+    theta: 2.220422761257776
+    kappa: 0.0021142160356648616
+    s: 31.342568673398645
+    dkappa: -0.00015822982644868297
     ddkappa: 0
   }
-  v: 8.17283170944281
-  a: 0.31406890289484796
+  v: 8.1728317094428089
+  a: 0.314068902894848
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587483.60464988672
-    y: 4140744.4934920678
-    theta: 2.2228644046087505
-    kappa: 0.00197561867859516
-    s: 32.161405835946312
-    dkappa: -0.00019304053639870576
+    x: 587483.78834193479
+    y: 4140744.634319203
+    theta: 2.222340557219634
+    kappa: 0.0019688924496212836
+    s: 32.161405835946304
+    dkappa: -0.00017529637224077723
     ddkappa: 0
   }
-  v: 8.2037609333580779
+  v: 8.2037609333580761
   a: 0.30504236862555534
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587483.10530764936
-    y: 4140745.1463018805
-    theta: 2.2244069158673057
-    kappa: 0.0018050745413699274
-    s: 32.983296305018186
-    dkappa: -0.00020654524372655877
+    x: 587483.28934214532
+    y: 4140745.2873908142
+    theta: 2.2238826209895919
+    kappa: 0.0018131846976976484
+    s: 32.983296305018179
+    dkappa: -0.00018847638003072854
     ddkappa: 0
   }
-  v: 8.2339554966862138
-  a: 0.2994752399050446
+  v: 8.2339554966862121
+  a: 0.29947523990504471
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 587482.603188297
-    y: 4140745.8007612443
-    theta: 2.2255954301074556
-    kappa: 0.0016271901118499342
-    s: 33.808184907070839
-    dkappa: -0.00021548559571069203
+    x: 587482.78756278381
+    y: 4140745.9421108817
+    theta: 2.2250838211502959
+    kappa: 0.0016499379299325359
+    s: 33.808184907070832
+    dkappa: -0.00019770052512934299
     ddkappa: 0
   }
-  v: 8.263791204607946
-  a: 0.29796480925036051
+  v: 8.2637912046079443
+  a: 0.29796480925036062
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 587482.09840447118
-    y: 4140746.45693618
-    theta: 2.2270967714895917
-    kappa: 0.0014458471497704936
-    s: 34.636057035541477
-    dkappa: -0.00021904880541857583
+    x: 587482.28311004082
+    y: 4140746.5985403797
+    theta: 2.22660837451647
+    kappa: 0.0014826048498449858
+    s: 34.636057035541462
+    dkappa: -0.00020212504269540978
     ddkappa: 0
   }
-  v: 8.2937035915557153
-  a: 0.30110836917854766
+  v: 8.2937035915557136
+  a: 0.30110836917854777
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587481.59084227378
-    y: 4140747.1147771943
-    theta: 2.2278881732847173
-    kappa: 0.0012665631640373944
-    s: 35.466944623773145
-    dkappa: -0.00021632142054605927
+    x: 587481.775855387
+    y: 4140747.2566185668
+    theta: 2.2274368713846311
+    kappa: 0.0013157006731282649
+    s: 35.466944623773138
+    dkappa: -0.00020108358923038631
     ddkappa: 0
   }
-  v: 8.3241879212136531
-  a: 0.30950321220665095
+  v: 8.3241879212136514
+  a: 0.30950321220665111
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 587481.08071574115
-    y: 4140747.7745543392
-    theta: 2.2289538642598465
-    kappa: 0.0010939596143511063
-    s: 36.300932117939894
-    dkappa: -0.00020842664259665969
+    x: 587481.26601019641
+    y: 4140747.9166131332
+    theta: 2.2285484879871422
+    kappa: 0.0011535912914978569
+    s: 36.300932117939887
+    dkappa: -0.00019546076581721891
     ddkappa: 0
   }
-  v: 8.35579918651761
-  a: 0.323746630851715
+  v: 8.3557991865176078
+  a: 0.32374663085171512
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 587480.56803707557
-    y: 4140748.4364569345
-    theta: 2.22997299135832
-    kappa: 0.000932368367526246
-    s: 37.138162449971944
-    dkappa: -0.00019492197153499186
+    x: 587480.75358188187
+    y: 4140748.5787095455
+    theta: 2.2296211322745183
+    kappa: 0.001000099038491313
+    s: 37.138162449971936
+    dkappa: -0.00018487142614607191
     ddkappa: 0
   }
-  v: 8.38915210965513
-  a: 0.34443591763078457
+  v: 8.3891521096551287
+  a: 0.34443591763078474
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 587480.05275448842
-    y: 4140749.1007065433
-    theta: 2.230687703698508
-    kappa: 0.00078621626651564414
-    s: 37.978843010480837
-    dkappa: -0.0001756664025131306
+    x: 587480.23851432349
+    y: 4140749.2431258885
+    theta: 2.2303954819163518
+    kappa: 0.00085912862454594623
+    s: 37.97884301048083
+    dkappa: -0.00016918817826067119
     ddkappa: 0
   }
-  v: 8.4249211420654682
-  a: 0.37216836506090445
+  v: 8.4249211420654664
+  a: 0.37216836506090467
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 587479.53475319222
-    y: 4140749.7675652206
-    theta: 2.2311365774470469
-    kappa: 0.00066012156255254952
-    s: 38.823251621684655
-    dkappa: -0.00015059412567702658
+    x: 587479.72068963049
+    y: 4140749.9101217
+    theta: 2.2309064477707405
+    kappa: 0.00073477017244325817
+    s: 38.823251621684648
+    dkappa: -0.00014833641377850356
     ddkappa: 0
   }
-  v: 8.4638404644395813
-  a: 0.40754126565911936
+  v: 8.46384046443958
+  a: 0.40754126565911947
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 587479.01385689946
-    y: 4140750.4373441627
-    theta: 2.23180286224078
-    kappa: 0.00055901067425634962
-    s: 39.671742510333132
-    dkappa: -0.00011946659536831557
+    x: 587479.199929855
+    y: 4140750.580006789
+    theta: 2.2316356792894965
+    kappa: 0.00063140393403689861
+    s: 39.671742510333125
+    dkappa: -0.00012207185363532111
     ddkappa: 0
   }
-  v: 8.50670398672013
-  a: 0.45115191194247395
+  v: 8.5067039867201277
+  a: 0.45115191194247406
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 587478.48983185494
-    y: 4140751.1104143085
-    theta: 2.2323616113495621
-    kappa: 0.00049422748184749677
-    s: 40.524752280632867
-    dkappa: -8.091281176241449e-05
+    x: 587478.67599478934
+    y: 4140751.253146979
+    theta: 2.2322530069964825
+    kappa: 0.00055902041773231079
+    s: 40.524752280632853
+    dkappa: -8.9158250443089352e-05
     ddkappa: 0
   }
-  v: 8.55436534810148
-  a: 0.503597596428013
+  v: 8.554365348101479
+  a: 0.50359759642801316
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 587477.96242373367
-    y: 4140751.7872426808
-    theta: 2.2328606369886348
-    kappa: 0.00046953321903750553
-    s: 41.382805887172481
-    dkappa: -3.5479682651161189e-05
+    x: 587478.14864051878
+    y: 4140751.9300173065
+    theta: 2.2328016976737173
+    kappa: 0.0005209853630707234
+    s: 41.382805887172474
+    dkappa: -5.0092321764276648e-05
     ddkappa: 0
   }
   v: 8.6077379170297057
-  a: 0.56547561163278126
+  a: 0.56547561163278137
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 587477.4312636035
-    y: 4140752.46832671
-    theta: 2.2331887163222479
-    kappa: 0.00048937032496825308
-    s: 42.246522607847787
-    dkappa: 1.6856096117925077e-05
+    x: 587477.61750660744
+    y: 4140752.6111217765
+    theta: 2.2331662212833763
+    kappa: 0.0005213864247759877
+    s: 42.246522607847773
+    dkappa: -4.8221946795316462e-06
     ddkappa: 0
   }
-  v: 8.66779479120258
-  a: 0.63738325007382346
+  v: 8.6677947912025779
+  a: 0.63738325007382357
   relative_time: 5.7999999999999963
 }
 decision {
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_qp_path_failure_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_qp_path_failure_0.pb.txt
index 896f1a60311886e8747cf24e88dde13dc4d41235..bb8dd1d4833f1d641e94059ab757c244e4ed45d8 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_qp_path_failure_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_qp_path_failure_0.pb.txt
@@ -1,7 +1,7 @@
 header {
   timestamp_sec: 1509683642.6191196
 }
-total_path_length: 29.958966337866656
+total_path_length: 29.971838223748293
 total_path_time: 6.999999999999992
 is_replan: true
 gear: GEAR_DRIVE
@@ -17,1547 +17,1547 @@ trajectory_point {
     ddkappa: 0
   }
   v: 4.8822846412658691
-  a: -0.63196180729610762
+  a: -0.6292845976249255
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 586916.51473567472
-    y: 4141003.5380659089
-    theta: 2.6092219887102956
-    kappa: -0.023845497056714588
-    s: 0.097516683573098326
-    dkappa: -0.0014059647018482076
+    x: 586916.51472966617
+    y: 4141003.5380576449
+    theta: 2.6092514086774985
+    kappa: -0.023790426928917578
+    s: 0.097517442652084868
+    dkappa: -0.0013515885544163491
     ddkappa: 0
   }
-  v: 4.8692544174137318
-  a: -0.67075186305652079
+  v: 4.8693413457090626
+  a: -0.66476847132576689
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 586916.43205225421
-    y: 4141003.5892587979
-    theta: 2.6051290083837344
-    kappa: -0.0252655733400802
-    s: 0.19476512767725518
-    dkappa: -0.002808062011870159
+    x: 586916.43203822232
+    y: 4141003.5892435503
+    theta: 2.605187667563527
+    kappa: -0.025155618103071077
+    s: 0.1947690328093509
+    dkappa: -0.0026994924019803856
     ddkappa: 0
   }
-  v: 4.8554668219998343
-  a: -0.70770363038300443
+  v: 4.8557048811551384
+  a: -0.69860553368166123
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 586916.3496094665
-    y: 4141003.6403027
-    theta: 2.6010479398141477
-    kappa: -0.026681516790590273
-    s: 0.29173055067215775
-    dkappa: -0.0042060788130508595
+    x: 586916.34958433767
+    y: 4141003.6402824074
+    theta: 2.6011356009220954
+    kappa: -0.026516887303254091
+    s: 0.2917412348395868
+    dkappa: -0.00404352393927393
     ddkappa: 0
   }
-  v: 4.8409583407782044
-  a: -0.7428451107406665
+  v: 4.8414079416502993
+  a: -0.73082000276208881
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 586916.26741926349
-    y: 4141003.6911902167
-    theta: 2.5969793746529053
-    kappa: -0.028093122132378592
-    s: 0.38839889503228092
-    dkappa: -0.0055998124284333071
+    x: 586916.26737896935
+    y: 4141003.6911674319
+    theta: 2.5970957472306218
+    kappa: -0.027874053630978529
+    s: 0.38842116214789146
+    dkappa: -0.00538350455851462
     ddkappa: 0
   }
-  v: 4.8257648994735662
-  a: -0.77620430559461506
+  v: 4.8264827368793615
+  a: -0.76143609663652956
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 586916.18549299124
-    y: 4141003.7419143226
-    theta: 2.5929238745463365
-    kappa: -0.029500194499951671
-    s: 0.48475681614630084
-    dkappa: -0.0069890704596319757
+    x: 586916.18543253129
+    y: 4141003.7418921795
+    theta: 2.5930686182507383
+    kappa: -0.02922694516277757
+    s: 0.484796567489446
+    dkappa: -0.0067192645132912489
     ddkappa: 0
   }
-  v: 4.8099218637813435
-  a: -0.80780921640995829
+  v: 4.8109609921657484
+  a: -0.79047803337446365
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 586916.10384139908
-    y: 4141003.7924683616
-    theta: 2.5888819716071376
-    kappa: -0.0309025492746315
-    s: 0.5807916711165092
-    dkappa: -0.0083736706253463161
+    x: 586916.10375490144
+    y: 4141003.7924505351
+    theta: 2.5890546994330723
+    kappa: -0.030575398814218998
+    s: 0.58085583328228629
+    dkappa: -0.00805064278429911
     ddkappa: 0
   }
-  v: 4.7934640393676577
-  a: -0.83768784465180413
+  v: 4.794873948471496
+  a: -0.81797003104537125
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 586916.02247464936
-    y: 4141003.8428460425
-    theta: 2.5848541688857747
-    kappa: -0.032300011920998323
-    s: 0.67649150755822718
-    dkappa: -0.0097534405998742479
+    x: 586916.022355431
+    y: 4141003.8428367106
+    theta: 2.585054450322037
+    kappa: -0.031919260203918561
+    s: 0.67658796192007509
+    dkappa: -0.0093774869450753313
     ddkappa: 0
   }
-  v: 4.776425671869327
-  a: -0.86586819178526075
+  v: 4.77825236239725
+  a: -0.8439363077187324
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 586915.94140232669
-    y: 4141003.8930414305
-    theta: 2.5808409408418997
-    kappa: -0.033692417823333418
-    s: 0.77184505239921908
-    dkappa: -0.011128217851625648
+    x: 586915.94124295213
+    y: 4141003.8930452387
+    theta: 2.581068304960624
+    kappa: -0.03325838351755335
+    s: 0.771982566084874
+    dkappa: -0.010699653027734226
     ddkappa: 0
   }
-  v: 4.7588404468938688
-  a: -0.89237825927543613
+  v: 4.7611265061822676
+  a: -0.86840108146402717
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 586915.86063344777
-    y: 4141003.9430489438
-    theta: 2.5768427338157558
-    kappa: -0.0350796121220618
-    s: 0.86684170067910671
-    dkappa: -0.012497849481635842
+    x: 586915.86042578635
+    y: 4141003.9430709681
+    theta: 2.5770966722951867
+    kappa: -0.034592631371875071
+    s: 0.8670298590599157
+    dkappa: -0.012017005388702618
     ddkappa: 0
   }
-  v: 4.7407414900194977
-  a: -0.9172460485874383
+  v: 4.7435261677044132
+  a: -0.89138857035073582
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 586915.78017647052
-    y: 4141003.992863345
-    theta: 2.5728599664995873
-    kappa: -0.036461449550195091
-    s: 0.96147150434878315
-    dkappa: -0.013862192062079117
+    x: 586915.77991175256
+    y: 4141003.9929090589
+    theta: 2.573139936580235
+    kappa: -0.035921874678723514
+    s: 0.961720645042377
+    dkappa: -0.013329416574455206
     ddkappa: 0
   }
-  v: 4.7221613667951257
-  a: -0.94049956118637534
+  v: 4.725480650480165
+  a: -0.91292299244833819
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 586915.70092493808
-    y: 4141004.0438511516
-    theta: 2.5689295934763896
-    kappa: -0.0377738496896209
-    s: 1.0557251610698266
-    dkappa: -0.014539309707586124
+    x: 586915.70058667182
+    y: 4141004.0439164918
+    theta: 2.5692558980089686
+    kappa: -0.0371895995418737
+    s: 1.0560463094561494
+    dkappa: -0.013981946422080643
     ddkappa: 0
   }
-  v: 4.7031320827403622
-  a: -0.96216679853735543
+  v: 4.70701877366461
+  a: -0.93302856582631466
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586915.62305269274
-    y: 4141004.0962655856
-    theta: 2.565058852327148
-    kappa: -0.039004666286331521
-    s: 1.1495940030139145
-    dkappa: -0.014400918969631218
+    x: 586915.62261004024
+    y: 4141004.0963256238
+    theta: 2.5654548741282071
+    kappa: -0.0383858935772146
+    s: 1.1499988092646132
+    dkappa: -0.013860694411899108
     ddkappa: 0
   }
-  v: 4.6836850833455168
-  a: -0.98227576210548651
+  v: 4.6881688720514472
+  a: -0.95172950855414529
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 586915.54550635791
-    y: 4141004.148460655
-    theta: 2.5612043109797833
-    kappa: -0.0402303316770468
-    s: 1.2430699856622376
-    dkappa: -0.01426310742373544
+    x: 586915.54494932876
+    y: 4141004.1485224222
+    theta: 2.5616692499839022
+    kappa: -0.03957734086245638
+    s: 1.2435706632834078
+    dkappa: -0.013739933650670593
     ddkappa: 0
   }
-  v: 4.6638512540715942
-  a: -1.0008544533558767
+  v: 4.6689587960729826
+  a: -0.96905003870131012
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586915.46829209931
-    y: 4141004.2004322098
-    theta: 2.5573662759144256
-    kappa: -0.0414507484073479
-    s: 1.3361456766049136
-    dkappa: -0.014125886027493095
+    x: 586915.467610288
+    y: 4141004.2005030215
+    theta: 2.5578993059024766
+    kappa: -0.040763853170627876
+    s: 1.3367549424932059
+    dkappa: -0.013619673080759846
     ddkappa: 0
   }
-  v: 4.6436609203503
-  a: -1.0179308737536339
+  v: 4.6494159118001361
+  a: -0.98501437433728922
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586915.39141558437
-    y: 4141004.2521764357
-    theta: 2.5535450288323673
-    kappa: -0.042665826901979578
-    s: 1.4288142443404008
-    dkappa: -0.013989264852579945
+    x: 586915.39059821866
+    y: 4141004.2522638589
+    theta: 2.5541453002633965
+    kappa: -0.041945349182109626
+    s: 1.4295452603524836
+    dkappa: -0.013499920944427573
     ddkappa: 0
   }
-  v: 4.623143847584033
-  a: -1.0335330247638665
+  v: 4.6295671009424355
+  a: -0.99964673353156286
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 586915.3148819909
-    y: 4141004.3036898468
-    theta: 2.5497408271179278
-    kappa: -0.043875485317987059
-    s: 1.5210694470749135
-    dkappa: -0.0138532531012662
+    x: 586915.31391797913
+    y: 4141004.3038016688
+    theta: 2.5504074698910912
+    kappa: -0.043121754361286763
+    s: 1.5219357631102943
+    dkappa: -0.013380684796332467
     ddkappa: 0
   }
-  v: 4.6023292411458936
-  a: -1.0476889078516822
+  v: 4.6094387608480218
+  a: -1.0129713343536109
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 586915.23869601754
-    y: 4141004.3549692808
-    theta: 2.5459539043003181
-    kappa: -0.045079649397852979
-    s: 1.6129056215218338
-    dkappa: -0.013717859122929547
+    x: 586915.23757399363
+    y: 4141004.3551134774
+    theta: 2.5466860304468666
+    kappa: -0.044293000833201854
+    s: 1.6139211201190407
+    dkappa: -0.013261971516033224
     ddkappa: 0
   }
-  v: 4.5812457463796807
-  a: -1.0604265244821895
+  v: 4.5890568045036426
+  a: -1.0250123948729137
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 586915.16286189144
-    y: 4141004.4060118934
-    theta: 2.5421844705155028
-    kappa: -0.046278252322634314
-    s: 1.7043176717011279
-    dkappa: -0.01358309043056815
+    x: 586915.16157026007
+    y: 4141004.4061965989
+    theta: 2.5429811768208204
+    kappa: -0.045459027260207743
+    s: 1.7054965141472456
+    dkappa: -0.013143787320490556
     ddkappa: 0
   }
-  v: 4.559921448599888
-  a: -1.071773876120496
+  v: 4.56844666053466
+  a: -1.0357941331589511
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586915.087383379
-    y: 4141004.4568151478
-    theta: 2.5384327129680617
-    kappa: -0.047471234565099253
-    s: 1.795301057738758
-    dkappa: -0.013448953717313677
+    x: 586915.0859103587
+    y: 4141004.4570486262
+    theta: 2.5392930835237593
+    kappa: -0.046619778718620407
+    s: 1.7966576316923255
+    dkappa: -0.01302613777656921
     ddkappa: 0
   }
-  v: 4.5383838730917079
-  a: -1.0817589642317098
+  v: 4.5476332732050437
+  a: -1.0453407672812036
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586915.01226379431
-    y: 4141004.5073768143
-    theta: 2.5346987963930587
-    kappa: -0.048658543742864185
-    s: 1.8858517846660974
-    dkappa: -0.0133154548729443
+    x: 586915.01059745927
+    y: 4141004.5076674288
+    theta: 2.5356219050791058
+    kappa: -0.04777520657537182
+    s: 1.8874006532933627
+    dkappa: -0.012909027813539989
     ddkappa: 0
   }
-  v: 4.5166599851110325
-  a: -1.0904097902809393
+  v: 4.5266411024173738
+  a: -1.0536765153091507
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586914.93750600878
-    y: 4141004.5576949609
-    theta: 2.530982863517897
-    kappa: -0.049840134471530573
-    s: 1.9759663912193444
-    dkappa: -0.013182599000397709
+    x: 586914.93563433
+    y: 4141004.5580511456
+    theta: 2.5319677764148247
+    kappa: -0.048925268364662822
+    s: 1.9777222438438775
+    dkappa: -0.012792461735581764
     ddkappa: 0
   }
-  v: 4.4947761898844494
-  a: -1.0977543557332923
+  v: 4.5054941237128423
+  a: -1.0608255953122734
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 586914.86434606975
-    y: 4141004.6095036943
-    theta: 2.5268444508118719
-    kappa: -0.050957771748581404
-    s: 2.0656419386389357
-    dkappa: -0.013054613725887154
+    x: 586914.862288179
+    y: 4141004.6099821832
+    theta: 2.5278828223350249
+    kappa: -0.050016038743287056
+    s: 2.0676195429045992
+    dkappa: -0.012679750691661775
     ddkappa: 0
   }
-  v: 4.4727583326092457
-  a: -1.1038206620538771
+  v: 4.4842158282712514
+  a: -1.0668122253600509
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586914.7929491601
-    y: 4141004.6630312465
-    theta: 2.5222253823794887
-    kappa: -0.052003726117392769
-    s: 2.154875999468961
-    dkappa: -0.012932061241846869
+    x: 586914.790615175
+    y: 4141004.6635356229
+    theta: 2.5233479495132611
+    kappa: -0.051045178812058792
+    s: 2.1570901550162391
+    dkappa: -0.01257103945226485
     ddkappa: 0
   }
-  v: 4.4506316984534058
-  a: -1.1086367107078015
+  v: 4.4628292229110134
+  a: -1.0716606235219637
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586914.72190702951
-    y: 4141004.7162928153
-    theta: 2.5176292666103031
-    kappa: -0.053044483022971572
-    s: 2.2436666463565764
-    dkappa: -0.012810117734688226
+    x: 586914.71928553516
+    y: 4141004.7168325046
+    theta: 2.5188348019196063
+    kappa: -0.052069388576579452
+    s: 2.2461321400122642
+    dkappa: -0.012462849016111336
     ddkappa: 0
   }
-  v: 4.4284210125556109
-  a: -1.1122305031601736
+  v: 4.4413568300891493
+  a: -1.0753950078674919
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586914.651220829
-    y: 4141004.7692875359
-    theta: 2.5130561779635556
-    kappa: -0.05408002560456502
-    s: 2.3320124408514191
-    dkappa: -0.01268878517995356
+    x: 586914.64830045658
+    y: 4141004.7698719325
+    theta: 2.514343455306804
+    kappa: -0.053088650845588323
+    s: 2.3347440033316675
+    dkappa: -0.012355181199167117
     ddkappa: 0
   }
-  v: 4.4061504400252414
-  a: -1.1146300408761016
+  v: 4.4198206879012929
+  a: -1.0780395964661158
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586914.58089132723
-    y: 4141004.8220148338
-    theta: 2.5085061661709638
-    kappa: -0.055110342600788745
-    s: 2.4199124222050208
-    dkappa: -0.012568064897117861
+    x: 586914.57766078738
+    y: 4141004.8226532736
+    theta: 2.5098739633328062
+    kappa: -0.054102953441996157
+    s: 2.4229246863317404
+    dkappa: -0.012248037287735689
     ddkappa: 0
   }
-  v: 4.3838435859423761
-  a: -1.1158633253206933
+  v: 4.398242350081687
+  a: -1.0796186073873151
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586914.5109189197
-    y: 4141004.8744744109
-    theta: 2.5039792568164962
-    kappa: -0.056135428218339459
-    s: 2.5073660961702204
-    dkappa: -0.012447957564971456
+    x: 586914.50736703421
+    y: 4141004.8751761494
+    theta: 2.5054263580517855
+    kappa: -0.055112289091457309
+    s: 2.5106735566008451
+    dkappa: -0.012141418050228614
     ddkappa: 0
   }
-  v: 4.3615234953577886
-  a: -1.1159583579590571
+  v: 4.3766428860031867
+  a: -1.0801562587005704
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586914.44130363758
-    y: 4141004.9266662458
-    theta: 2.4994754519161564
-    kappa: -0.057155282000707686
-    s: 2.5943734238005813
-    dkappa: -0.012328463237002693
+    x: 586914.43741936993
+    y: 4141004.927440431
+    theta: 2.5010006504051336
+    kappa: -0.056116655310941982
+    s: 2.5979903982711878
+    dkappa: -0.012035323748935987
     ddkappa: 0
   }
-  v: 4.3392126532929538
-  a: -1.1149431402563008
+  v: 4.3550428806772539
+  a: -1.0796767684753612
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586914.37204515678
-    y: 4141004.9785905792
-    theta: 2.4949947304977673
-    kappa: -0.058169908696890343
-    s: 2.6809348102498025
-    dkappa: -0.012209581356780633
+    x: 586914.36781764159
+    y: 4141004.9794462323
+    theta: 2.4965968307124644
+    kappa: -0.057116054297308386
+    s: 2.6848754023315888
+    dkappa: -0.011929754151796906
     ddkappa: 0
   }
-  v: 4.3169329847400437
-  a: -1.1128456736775325
+  v: 4.3334624347539652
+  a: -1.0782043547811679
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 586914.30314280652
-    y: 4141005.0302479155
-    theta: 2.49053704918075
-    kappa: -0.059179318130103485
-    s: 2.767051093571133
-    dkappa: -0.012091310773337737
+    x: 586914.29856137792
+    y: 4141005.0311939046
+    theta: 2.4922148691626189
+    kappa: -0.058110492815874923
+    s: 2.7713291569402578
+    dkappa: -0.011824708544169914
     ddkappa: 0
   }
-  v: 4.2947058546619257
-  a: -1.1096939596878606
+  v: 4.3119211645220039
+  a: -1.0757632356874711
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 586914.2345955791
-    y: 4141005.08163901
-    theta: 2.4861023427559008
-    kappa: -0.060183525066494908
-    s: 2.8527235335167864
-    dkappa: -0.011973649756552553
+    x: 586914.22964979755
+    y: 4141005.0826840331
+    theta: 2.4878547163046676
+    kappa: -0.059099982088992413
+    s: 2.8573526377375629
+    dkappa: -0.011720185740603493
     ddkappa: 0
   }
-  v: 4.2725520679921676
-  a: -1.1055159997523925
+  v: 4.2904382019086666
+  a: -1.0723776292637501
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 586914.166402138
-    y: 4141005.1327648642
-    theta: 2.4816905247651864
-    kappa: -0.061182549083856841
-    s: 2.9379538003373549
-    dkappa: -0.011856596012532394
+    x: 586914.16108181619
+    y: 4141005.1339174276
+    theta: 2.4835163035389112
+    kappa: -0.060084537684616225
+    s: 2.9429471981588038
+    dkappa: -0.011616184096606497
     ddkappa: 0
   }
-  v: 4.2504918696350344
-  a: -1.100339795336237
+  v: 4.2690321944798573
+  a: -1.0680717535794855
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586914.09856082755
-    y: 4141005.1836267198
-    theta: 2.47730148808151
-    kappa: -0.062176414440338647
-    s: 3.022743963581223
-    dkappa: -0.011740146698996045
+    x: 586914.09285605466
+    y: 4141005.1848951182
+    theta: 2.4791995436078889
+    kappa: -0.061064179404878551
+    s: 3.0281145597469865
+    dkappa: -0.011512701520418634
     ddkappa: 0
   }
-  v: 4.2285449444654883
-  a: -1.0941933479045016
+  v: 4.247721305440094
+  a: -1.0628698267041572
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 586914.03383656312
-    y: 4141005.23768544
-    theta: 2.472316843053898
-    kappa: -0.063072654260053332
-    s: 3.1070964808939805
-    dkappa: -0.01163027103602066
+    x: 586914.02789381891
+    y: 4141005.2392894812
+    theta: 2.4742482174161857
+    kappa: -0.061948997941613841
+    s: 3.1128568024655916
+    dkappa: -0.011413895561062374
     ddkappa: 0
   }
-  v: 4.2067304173291911
-  a: -1.0871046589222946
+  v: 4.2265232136325022
+  a: -1.0567960667072454
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586913.96997499454
-    y: 4141005.2921269811
-    theta: 2.4672396729314849
-    kappa: -0.063946587921084666
-    s: 3.1910141868178394
-    dkappa: -0.011522103782427734
+    x: 586913.96357376163
+    y: 4141005.2938121115
+    theta: 2.4692501736694012
+    kappa: -0.062819614286371478
+    s: 3.197176355011349
+    dkappa: -0.011316035243474862
     ddkappa: 0
   }
-  v: 4.1850668530424988
-  a: -1.0791017298547241
+  v: 4.2054551135388181
+  a: -1.0498746916582302
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586913.906441883
-    y: 4141005.3462885153
-    theta: 2.4621886160495912
-    kappa: -0.06481602670813344
-    s: 3.2745002815910431
-    dkappa: -0.011414492861878862
+    x: 586913.89957402681
+    y: 4141005.3480632114
+    theta: 2.4642770208359632
+    kappa: -0.0636858948475518
+    s: 3.2810759851270084
+    dkappa: -0.011218662283103214
     ddkappa: 0
   }
-  v: 4.16357225639247
-  a: -1.0702125621668979
+  v: 4.18453371527939
+  a: -1.0421299196265916
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 586913.84323452343
-    y: 4141005.4001723477
-    theta: 2.4571634573405206
-    kappa: -0.0656810076408178
-    s: 3.3575583199472852
-    dkappa: -0.011307433692435382
+    x: 586913.83589225181
+    y: 4141005.4020447843
+    theta: 2.4593285753350962
+    kappa: -0.064547871603351389
+    s: 3.3645587899141147
+    dkappa: -0.011121773085486458
     ddkappa: 0
   }
-  v: 4.1422640721368573
-  a: -1.0604651573239243
+  v: 4.1637752446131753
+  a: -1.0335859686818099
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 586913.78034994937
-    y: 4141005.4537810078
-    theta: 2.4521639609666277
-    kappa: -0.066541571313888631
-    s: 3.4401921999151206
-    dkappa: -0.011200921249662481
+    x: 586913.77252583019
+    y: 4141005.45575904
+    theta: 2.4544046346374788
+    kappa: -0.065405579832640032
+    s: 3.4476281861457765
+    dkappa: -0.011025363685156424
     ddkappa: 0
   }
-  v: 4.1211591850041138
-  a: -1.0498875167909116
+  v: 4.143195442937742
+  a: -1.0242670568933652
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 586913.71778494166
-    y: 4141005.50711724
-    theta: 2.4471898709979749
-    kappa: -0.067397761780584681
-    s: 3.5224061516173806
-    dkappa: -0.011094950081066385
+    x: 586913.70947191934
+    y: 4141005.5092083868
+    theta: 2.4495049778394673
+    kappa: -0.066259058014938185
+    s: 3.5302879005794403
+    dkappa: -0.010929429756880635
     ddkappa: 0
   }
-  v: 4.1002739196933868
-  a: -1.0385076420329671
+  v: 4.1228095672892691
+  a: -1.0141974023307374
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 586913.65553603694
-    y: 4141005.5601839973
-    theta: 2.4422409120899875
-    kappa: -0.068249626435987629
-    s: 3.6042047260705865
-    dkappa: -0.010989514320531564
+    x: 586913.64672744717
+    y: 4141005.56239543
+    theta: 2.4446293662372911
+    kappa: -0.067108347730394327
+    s: 3.6125419602696618
+    dkappa: -0.010833966626905188
     ddkappa: 0
   }
-  v: 4.0796240408745268
-  a: -1.0263535345151995
+  v: 4.1026323903425439
+  a: -1.0034012230634066
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 586913.59359953646
-    y: 4141005.6129844314
-    theta: 2.4373167901611064
-    kappa: -0.069097215900377346
-    s: 3.6855927839843625
-    dkappa: -0.010884607702757922
+    x: 586913.58428912
+    y: 4141005.615322961
+    theta: 2.4397775439012754
+    kappa: -0.067953493559762518
+    s: 3.6943946828808794
+    dkappa: -0.010738969284197642
     ddkappa: 0
   }
-  v: 4.0592247531880776
-  a: -1.0134531957027169
+  v: 4.0826782004109674
+  a: -0.99190273716085331
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 586913.53197151376
-    y: 4141005.66552189
-    theta: 2.4324171930704477
-    kappa: -0.069940583902586922
-    s: 3.7665754845608519
-    dkappa: -0.010780223577698011
+    x: 586913.52215343039
+    y: 4141005.6679939535
+    theta: 2.4349492382500486
+    kappa: -0.068794542984379675
+    s: 3.7758506670001846
+    dkappa: -0.010644432391689908
     ddkappa: 0
   }
-  v: 4.0390907012452821
-  a: -0.999834627060627
+  v: 4.0629608014465486
+  a: -0.97972616269255708
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 586913.47064782411
-    y: 4141005.7177999076
-    theta: 2.4275417912954556
-    kappa: -0.07077978716335788
-    s: 3.8471582742941286
-    dkappa: -0.010676354924994216
+    x: 586913.46031666326
+    y: 4141005.7204115554
+    theta: 2.4301441606247458
+    kappa: -0.069631546286143084
+    s: 3.856914782450096
+    dkappa: -0.010550350297521133
     ddkappa: 0
   }
-  v: 4.0192359696280819
-  a: -0.98552583005403793
+  v: 4.0434935130399063
+  a: -0.96689571772799843
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 586913.40962411254
-    y: 4141005.7698221956
-    theta: 2.4226902386095563
-    kappa: -0.0716148852786953
-    s: 3.9273468757696142
-    dkappa: -0.010572994368415956
+    x: 586913.39877490432
+    y: 4141005.772579086
+    theta: 2.4253620068632333
+    kappa: -0.070464556447487808
+    s: 3.9375921606013304
+    dkappa: -0.010456717046280584
     ddkappa: 0
   }
-  v: 3.9996740828891166
-  a: -0.970554806148058
+  v: 4.024289170420273
+  a: -0.95343562033665741
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 586913.34889582277
-    y: 4141005.8215926383
-    theta: 2.4178621727598166
-    kappa: -0.0724459406032229
-    s: 4.0071472764634883
-    dkappa: -0.010470134190296892
+    x: 586913.33752404735
+    y: 4141005.8245000257
+    theta: 2.4206024578743062
+    kappa: -0.0712936290513641
+    s: 4.0178881846855754
+    dkappa: -0.010363526390250542
     ddkappa: 0
   }
-  v: 3.980418005551722
-  a: -0.954949556807795
+  v: 4.0053601244554891
+  a: -0.93937008858801385
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 586913.29026491719
-    y: 4141005.8750819452
-    theta: 2.4120617743065473
-    kappa: -0.073222413413921245
-    s: 4.0865657175421051
-    dkappa: -0.010364628987900866
+    x: 586913.27886551
+    y: 4141005.8787050964
+    theta: 2.4145705833563271
+    kappa: -0.072057477920676238
+    s: 4.0978084801082613
+    dkappa: -0.010265175409284098
     ddkappa: 0
   }
-  v: 3.9614801421099326
-  a: -0.9387380834983573
+  v: 3.9867182416520048
+  a: -0.92472334055154826
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 586913.23407944175
-    y: 4141005.930678552
-    theta: 2.4050941700306812
-    kappa: -0.073934484513688017
-    s: 4.1656086826614054
-    dkappa: -0.010255857453708078
+    x: 586913.222094442
+    y: 4141005.9344304572
+    theta: 2.4076592827071348
+    kappa: -0.07277479746640704
+    s: 4.1773589047613333
+    dkappa: -0.010163357264116478
     ddkappa: 0
   }
-  v: 3.9428723370284811
-  a: -0.92194838768485243
+  v: 3.9683749041548824
+  a: -0.90951959429674067
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 586913.17815608985
-    y: 4141005.9860157832
-    theta: 2.3981590718838124
-    kappa: -0.074643233571314041
-    s: 4.2442828867663334
-    dkappa: -0.010147593373925979
+    x: 586913.16558299225
+    y: 4141005.9899009825
+    theta: 2.4007795879559115
+    kappa: -0.073488836655874468
+    s: 4.2565455393360256
+    dkappa: -0.010062004740988874
     ddkappa: 0
   }
-  v: 3.9246058747427983
-  a: -0.90460847083238893
+  v: 3.9503410097477945
+  a: -0.89378306789307072
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 586913.12248993164
-    y: 4141006.0410985155
-    theta: 2.3912558685456542
-    kappa: -0.075348723062148321
-    s: 4.3225952648902481
-    dkappa: -0.010039827205210426
+    x: 586913.10932666936
+    y: 4141006.0451210802
+    theta: 2.3939309523002095
+    kappa: -0.074199652241360339
+    s: 4.33537467763563
+    dkappa: -0.0099611097843392924
     ddkappa: 0
   }
-  v: 3.9066914796590115
-  a: -0.88674633440607475
+  v: 3.9326269718530225
+  a: -0.877537979410019
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 586913.06707588932
-    y: 4141006.0959317745
-    theta: 2.3843839302825476
-    kappa: -0.076051017343339
-    s: 4.4005529609543359
-    dkappa: -0.0099325491167653948
+    x: 586913.05332083651
+    y: 4141006.1000953009
+    theta: 2.3871128112641369
+    kappa: -0.074907302809462353
+    s: 4.41385281688827
+    dkappa: -0.009860664078238322
     ddkappa: 0
   }
-  v: 3.8891393161539467
-  a: -0.868389979871018
+  v: 3.9152427195314594
+  a: -0.86080854691706576
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586912.793600292
-    y: 4141006.3665411291
-    theta: 2.3504700098815805
-    kappa: -0.07951693211708187
-    s: 4.7852844584932175
-    dkappa: -0.00940311762116763
+    x: 586912.77687744633
+    y: 4141006.371446684
+    theta: 2.3534586376250246
+    kappa: -0.078400248315975155
+    s: 4.801218949578888
+    dkappa: -0.0093648665849713989
     ddkappa: 0
   }
-  v: 3.8071285234472136
-  a: -0.7701749868433756
+  v: 3.8335818533557604
+  a: -0.77074385673557511
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586912.53114269231
-    y: 4141006.6368939322
-    theta: 2.3184802257574475
-    kappa: -0.082772963634328955
-    s: 5.1623210021911241
-    dkappa: -0.0088704867410827788
+    x: 586912.51243302971
+    y: 4141006.6435041437
+    theta: 2.3219535041815789
+    kappa: -0.081663584623725721
+    s: 5.1808847069436705
+    dkappa: -0.008859833420544562
     ddkappa: 0
   }
-  v: 3.7353761598836837
-  a: -0.66380476086337625
+  v: 3.7613764778451957
+  a: -0.67220327672855573
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586912.29058882338
-    y: 4141006.9185528676
-    theta: 2.290934712788431
-    kappa: -0.085533781081790508
-    s: 5.5327233774885753
-    dkappa: -0.0083042697204037518
+    x: 586912.26873544836
+    y: 4141006.9258193877
+    theta: 2.2946624078249149
+    kappa: -0.084473453353027722
+    s: 5.5538329720671182
+    dkappa: -0.0083165678967017827
     ddkappa: 0
   }
-  v: 3.6745227396016666
-  a: -0.55277948506952723
+  v: 3.6993228190480663
+  a: -0.5682140655810215
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 586912.05362429656
-    y: 4141007.1960091246
-    theta: 2.2638002107777861
-    kappa: -0.088253403935190711
-    s: 5.8975989203242278
-    dkappa: -0.0077465013134999441
+    x: 586912.02874946024
+    y: 4141007.20383488
+    theta: 2.2677869637031289
+    kappa: -0.087240526871694576
+    s: 5.9211011143451353
+    dkappa: -0.0077815764827066335
     ddkappa: 0
   }
-  v: 3.6248587584256229
-  a: -0.44059934260033551
+  v: 3.6478143771441744
+  a: -0.46180348197798476
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586911.83166573476
-    y: 4141007.4795626039
-    theta: 2.2324361865936
-    kappa: -0.090588900311568674
-    s: 6.2580665153034909
-    dkappa: -0.007110493127099801
+    x: 586911.805428468
+    y: 4141007.4891009694
+    theta: 2.236030046813573
+    kappa: -0.089580806094340923
+    s: 6.2837507168981812
+    dkappa: -0.0071519453983476186
     ddkappa: 0
   }
-  v: 3.5863246938661608
-  a: -0.33076451659430978
+  v: 3.6069419264448186
+  a: -0.35599878460446094
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586911.62540300365
-    y: 4141007.7711366694
-    theta: 2.19623528075637
-    kappa: -0.092507899824502166
-    s: 6.615221593867143
-    dkappa: -0.0063847645228879181
+    x: 586911.59620198468
+    y: 4141007.7809356069
+    theta: 2.2000337340461709
+    kappa: -0.091556266601426517
+    s: 6.64283730398442
+    dkappa: -0.0064401070168711768
     ddkappa: 0
   }
-  v: 3.5585110051200388
-  a: -0.2267751901899569
+  v: 3.5764935153927979
+  a: -0.2538272321454616
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 586911.420454433
-    y: 4141008.0608530315
-    theta: 2.1602650215294155
-    kappa: -0.094414672827558541
-    s: 6.9701011324599458
-    dkappa: -0.0056636597488999105
+    x: 586911.38845769293
+    y: 4141008.0707028443
+    theta: 2.1642924244989032
+    kappa: -0.093517732653196309
+    s: 6.9993800684128713
+    dkappa: -0.0057333114040990065
     ddkappa: 0
   }
-  v: 3.5406581330701634
-  a: -0.13213154652578446
+  v: 3.555954466562409
+  a: -0.15831608328600116
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586911.23341871961
-    y: 4141008.3604073045
-    theta: 2.1286857466519873
-    kappa: -0.095772547252792634
-    s: 7.3236487555285237
-    dkappa: -0.0048407052704257745
+    x: 586911.200751171
+    y: 4141008.3715385054
+    theta: 2.1334615260126011
+    kappa: -0.094869187404419353
+    s: 7.3543315987135136
+    dkappa: -0.0049114914293975475
     ddkappa: 0
   }
-  v: 3.531665108440992
-  a: -0.0497950980131892
+  v: 3.5445075568118671
+  a: -0.072461827960250622
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586911.05828303332
-    y: 4141008.6669448335
-    theta: 2.1000394074009634
-    kappa: -0.0967609058558272
-    s: 7.676689497407942
-    dkappa: -0.0039479899204262134
+    x: 586911.0228115815
+    y: 4141008.6778187938
+    theta: 2.1050259495035188
+    kappa: -0.095922771885262409
+    s: 7.7085492048070341
+    dkappa: -0.004033372745757078
     ddkappa: 0
   }
-  v: 3.5303388210058171
-  a: 0.021563057598749075
+  v: 3.541090871571698
+  a: 0.0023299792554802162
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586910.883044453
-    y: 4141008.973662457
-    theta: 2.0713762381185754
-    kappa: -0.097749845130135085
-    s: 8.0299376545074033
-    dkappa: -0.00305475009058965
+    x: 586910.84486463538
+    y: 4141008.9841117444
+    theta: 2.07658919737716
+    kappa: -0.096976399924642673
+    s: 8.0627814553842665
+    dkappa: -0.0031552177578941794
     ddkappa: 0
   }
-  v: 3.5356491057229888
-  a: 0.08306278607810777
+  v: 3.5446273403092086
+  a: 0.066735415858577021
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586910.73012120707
-    y: 4141009.2926017307
-    theta: 2.0363744346656114
-    kappa: -0.097939542166729787
-    s: 8.3840088563169246
-    dkappa: -0.0020151911244574239
+    x: 586910.69164476159
+    y: 4141009.3039023969
+    theta: 2.0410847360109763
+    kappa: -0.097152859661911783
+    s: 8.4176730756325657
+    dkappa: -0.0021165681644592705
     ddkappa: 0
   }
-  v: 3.5466480521925581
-  a: 0.13546273369104717
+  v: 3.5541185752427054
+  a: 0.12155946744043312
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 586910.58570924134
-    y: 4141009.6173597379
-    theta: 1.9987295923716095
-    kappa: -0.097809205470326538
-    s: 8.7394277345178537
-    dkappa: -0.00091395548946722374
+    x: 586910.544407704
+    y: 4141009.6281374786
+    theta: 2.0036385430376464
+    kappa: -0.097101721300540123
+    s: 8.7737729768890791
+    dkappa: -0.0010331464601542927
     ddkappa: 0
   }
-  v: 3.5624636146411897
-  a: 0.17952154670372761
+  v: 3.5686466871496334
+  a: 0.16760711959244196
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586910.44057042024
-    y: 4141009.9437523186
-    theta: 1.9608952758001017
-    kappa: -0.09767821276237218
-    s: 9.0966355094455444
-    dkappa: 0.00019282289224706409
+    x: 586910.3964803858
+    y: 4141009.953892604
+    theta: 1.9660167989451836
+    kappa: -0.097050343197932945
+    s: 9.13154230649665
+    dkappa: 5.5354422501859723e-05
     ddkappa: 0
   }
-  v: 3.5822996119221671
-  a: 0.21599787138230925
+  v: 3.5873742853665784
+  a: 0.205683357905997
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586910.3224527766
-    y: 4141010.2829038193
-    theta: 1.923312969071354
-    kappa: -0.096544718244367791
-    s: 9.455997576552
-    dkappa: 0.0013501522047729034
+    x: 586910.277950452
+    y: 4141010.2934930068
+    theta: 1.9286273620464689
+    kappa: -0.095926238384078544
+    s: 9.4913624976597433
+    dkappa: 0.0011926074326941254
     ddkappa: 0
   }
-  v: 3.605435727515387
-  a: 0.24565035399295243
+  v: 3.6095444777892642
+  a: 0.23659316797249158
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 586910.20872754871
-    y: 4141010.6263795719
-    theta: 1.885563765062896
-    kappa: -0.095216834515544468
-    s: 9.8178110928685509
-    dkappa: 0.0025235522043378445
+    x: 586910.16131230863
+    y: 4141010.6363785435
+    theta: 1.8910321657358047
+    kappa: -0.094700107306780051
+    s: 9.8535433193003463
+    dkappa: 0.0023410735805007329
     ddkappa: 0
   }
-  v: 3.6312275095273647
-  a: 0.26923764080181739
+  v: 3.6344808708725544
+  a: 0.26114153538331908
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 586910.095161074
-    y: 4141010.9726872407
-    theta: 1.8478460022826821
-    kappa: -0.093845368717684549
-    s: 10.182312563468507
-    dkappa: 0.0037034164648873016
+    x: 586910.04706902988
+    y: 4141010.9826634093
+    theta: 1.854156750485874
+    kappa: -0.093358188096255179
+    s: 10.218330925913898
+    dkappa: 0.0034908403866572746
     ddkappa: 0
   }
-  v: 3.65910637069123
-  a: 0.28751837807506447
+  v: 3.6615875696304525
+  a: 0.280133445729873
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 586910.01328481082
-    y: 4141011.3308200361
-    theta: 1.8199570525046491
-    kappa: -0.091368408442130844
-    s: 10.549685427929823
-    dkappa: 0.0048194251206443222
+    x: 586909.962232956
+    y: 4141011.3403246445
+    theta: 1.8262713815222087
+    kappa: -0.091004485184742537
+    s: 10.585915907425186
+    dkappa: 0.0045842073806311306
     ddkappa: 0
   }
-  v: 3.6885795883667289
-  a: 0.30125121207885391
+  v: 3.6903491776361
+  a: 0.29437388460354663
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 586909.930737854
-    y: 4141011.6918864958
-    theta: 1.7918396489075414
-    kappa: -0.088871158014414753
-    s: 10.920067646797758
-    dkappa: 0.0059445756208077087
+    x: 586909.87671824661
+    y: 4141011.7008469459
+    theta: 1.7981629470354674
+    kappa: -0.088631954121025647
+    s: 10.956441339044275
+    dkappa: 0.0056863206271914775
     ddkappa: 0
   }
-  v: 3.7192303045402242
-  a: 0.311194789079346
+  v: 3.7203307970217789
+  a: 0.30466783759573351
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 586909.85717302747
-    y: 4141012.0577486604
-    theta: 1.761555207903295
-    kappa: -0.08609520474312618
-    s: 11.293559288047538
-    dkappa: 0.0069973898358516862
+    x: 586909.80209252809
+    y: 4141012.0665408643
+    theta: 1.7674982914129238
+    kappa: -0.085937755098962523
+    s: 11.330010831122415
+    dkappa: 0.0067007097962508248
     ddkappa: 0
   }
-  v: 3.7507175258246939
-  a: 0.31810775534270097
+  v: 3.7511780284789102
+  a: 0.311820290297827
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 586909.80609396391
-    y: 4141012.430940086
-    theta: 1.7264002402071696
-    kappa: -0.082679937595398725
-    s: 11.670230113547021
-    dkappa: 0.0078638044677176479
+    x: 586909.748091459
+    y: 4141012.4393357635
+    theta: 1.7323155946337323
+    kappa: -0.082667195501642254
+    s: 11.70669657900795
+    dkappa: 0.0075461668688177261
     ddkappa: 0
   }
-  v: 3.7827761234597319
-  a: 0.32274875713507911
+  v: 3.7826169712580544
+  a: 0.3166362283012204
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 586909.75457740249
-    y: 4141012.8073279369
-    theta: 1.6909441663999383
-    kappa: -0.079235418314142672
-    s: 12.050127165519349
-    dkappa: 0.00873764003487394
+    x: 586909.69363664836
+    y: 4141012.8152630562
+    theta: 1.6968372767381279
+    kappa: -0.079369155166119421
+    s: 12.08654741290224
+    dkappa: 0.0083987278585527323
     ddkappa: 0
   }
-  v: 3.815216833311549
-  a: 0.32587644072264077
+  v: 3.8144542231689109
+  a: 0.31992063719730734
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 586909.7210727873
-    y: 4141013.188746627
-    theta: 1.6574576078929315
-    kappa: -0.075446266681601593
-    s: 12.43328235300562
-    dkappa: 0.0093802148317793984
+    x: 586909.65863051417
+    y: 4141013.1964507625
+    theta: 1.6634077099157549
+    kappa: -0.07570566332377282
+    s: 12.469596847715573
+    dkappa: 0.0090164112940604446
     ddkappa: 0
   }
-  v: 3.8479262558729719
-  a: 0.32824945237154612
+  v: 3.8465768805803187
+  a: 0.322478502577481
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586909.69838578755
-    y: 4141013.5745177828
-    theta: 1.625052319406822
-    kappa: -0.071435045771161221
-    s: 12.819720038327549
-    dkappa: 0.00988462248805024
+    x: 586909.633218287
+    y: 4141013.5818882305
+    theta: 1.6308627905921975
+    kappa: -0.071843591498368053
+    s: 12.855871132923077
+    dkappa: 0.00951905684032734
     ddkappa: 0
   }
-  v: 3.8808668562634421
-  a: 0.33062643834795558
+  v: 3.8789525384202563
+  a: 0.32511481003313497
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 586909.67621219566
-    y: 4141013.963607219
-    theta: 1.5926715384018504
-    kappa: -0.067386158338197222
-    s: 13.209464623550117
-    dkappa: 0.010383328509078506
+    x: 586909.60908415471
+    y: 4141013.9706164021
+    theta: 1.5986610840987083
+    kappa: -0.067940624143443873
+    s: 13.245397302420637
+    dkappa: 0.010006596067618388
     ddkappa: 0
   }
-  v: 3.9140769642290185
-  a: 0.333766044918029
+  v: 3.9116292901758416
+  a: 0.32863454515566259
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 586909.68032345071
-    y: 4141014.3566692322
-    theta: 1.5713064480370438
-    kappa: -0.063177574958626193
-    s: 13.602548136944248
-    dkappa: 0.010511455427839298
+    x: 586909.610803245
+    y: 4141014.3634265624
+    theta: 1.5769663242619156
+    kappa: -0.063858324488399884
+    s: 13.638211224380802
+    dkappa: 0.010160532047797096
     ddkappa: 0
   }
-  v: 3.9476707741423751
-  a: 0.33842691834792737
+  v: 3.9447357278933297
+  a: 0.33384269353645646
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586909.68447009171
-    y: 4141014.7531143706
-    theta: 1.549757466134162
-    kappa: -0.058932767867497377
-    s: 13.999014960712229
-    dkappa: 0.010640685148215125
+    x: 586909.61253694142
+    y: 4141014.7595742121
+    theta: 1.5550872361428176
+    kappa: -0.0597413398085419
+    s: 14.034362667292825
+    dkappa: 0.01031577593619995
     ddkappa: 0
   }
-  v: 3.9817239181173543
-  a: 0.34193102854110141
+  v: 3.9783614770183564
+  a: 0.33795465974629768
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 586909.69971988257
-    y: 4141015.1525493618
-    theta: 1.5294608737578343
-    kappa: -0.054761163224157382
-    s: 14.398897847507184
-    dkappa: 0.010596819786991984
+    x: 586909.625936818
+    y: 4141015.1587137589
+    theta: 1.5344640198002528
+    kappa: -0.055682137836199082
+    s: 14.433890347804608
+    dkappa: 0.01029994599871098
     ddkappa: 0
   }
-  v: 4.015927089513224
-  a: 0.34153842861312006
+  v: 4.0121942999344782
+  a: 0.33809582871280136
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 586909.72651515121
-    y: 4141015.554953807
-    theta: 1.5104743332905262
-    kappa: -0.050667184890275267
-    s: 14.802193426139075
-    dkappa: 0.010372895148433089
+    x: 586909.65072697122
+    y: 4141015.5608562748
+    theta: 1.5150624110151591
+    kappa: -0.05167849888007145
+    s: 14.836796235902588
+    dkappa: 0.010117125467249417
     ddkappa: 0
   }
-  v: 4.0499239058720846
-  a: 0.33791488964831917
+  v: 4.045870614723694
+  a: 0.3349361565626145
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586909.75388602552
-    y: 4141015.9606962022
-    theta: 1.4913810090823469
-    kappa: -0.046547827397783653
-    s: 15.208866016027487
-    dkappa: 0.010142396647703207
+    x: 586909.67615527089
+    y: 4141015.9663025998
+    theta: 1.4955612869987007
+    kappa: -0.047651018793226446
+    s: 15.243049293162036
+    dkappa: 0.009927511918442428
     ddkappa: 0
   }
-  v: 4.0834245618444713
-  a: 0.3317261827310346
+  v: 4.0790938350806663
+  a: 0.32914559942238414
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 586909.80009847332
-    y: 4141016.368071693
-    theta: 1.4748990995315481
-    kappa: -0.042866761927724359
-    s: 15.618854284912487
-    dkappa: 0.0096609414005839975
+    x: 586909.72083684232
+    y: 4141016.373400778
+    theta: 1.4786198603351561
+    kappa: -0.044006042633471174
+    s: 15.652592172308319
+    dkappa: 0.0095039244717188883
     ddkappa: 0
   }
-  v: 4.1162058291893526
-  a: 0.32363807894560243
+  v: 4.1116343703127241
+  a: 0.32139411341875751
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586909.84667559888
-    y: 4141016.7786619179
-    theta: 1.4582871257917631
-    kappa: -0.039156647954911528
-    s: 16.032077906565455
-    dkappa: 0.00917568683118014
+    x: 586909.76586894074
+    y: 4141016.783692643
+    theta: 1.4615455281200065
+    kappa: -0.04033247162295607
+    s: 16.065347916778176
+    dkappa: 0.0090770139799898637
     ddkappa: 0
   }
-  v: 4.1481110567741313
-  a: 0.31431634937635844
+  v: 4.1433296253398595
+  a: 0.3123516546783815
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 586909.90305025922
-    y: 4141017.1911197347
-    theta: 1.4399415493537697
-    kappa: -0.035815103297603283
-    s: 16.448444218499937
-    dkappa: 0.00858057322452633
+    x: 586909.82086371037
+    y: 4141017.1958393864
+    theta: 1.4427914194365137
+    kappa: -0.036984172802883214
+    s: 16.481226660280974
+    dkappa: 0.0085476175622250829
     ddkappa: 0
   }
-  v: 4.1790501705746417
-  a: 0.30442676510763855
+  v: 4.1740840006947293
+  a: 0.30268817932790315
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 586909.96626799437
-    y: 4141017.6057386203
-    theta: 1.4203672904020177
-    kappa: -0.032719337025682034
-    s: 16.867854879682479
-    dkappa: 0.0079088076526648877
+    x: 586909.88302937988
+    y: 4141017.6101066754
+    theta: 1.4228097167063503
+    kappa: -0.033859944796342363
+    s: 16.900132326359984
+    dkappa: 0.00794452515824277
     ddkappa: 0
   }
-  v: 4.2089996736751543
-  a: 0.29463509722377856
+  v: 4.2038688925226584
+  a: 0.29307364349396958
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586910.03269838309
-    y: 4141018.0228058407
-    theta: 1.4024803503116612
-    kappa: -0.029775200771891292
-    s: 17.290212528243487
-    dkappa: 0.0072074618317302238
+    x: 586909.94822835131
+    y: 4141018.0268411553
+    theta: 1.4043444896687518
+    kappa: -0.030854408310289818
+    s: 17.321969327953635
+    dkappa: 0.0073176014652473562
     ddkappa: 0
   }
-  v: 4.23800264626837
-  a: 0.28560711680911444
+  v: 4.2327226925816293
+  a: 0.28417800330322784
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586910.10945315356
-    y: 4141018.4410359515
-    theta: 1.3909816056527493
-    kappa: -0.027429596227106889
-    s: 17.715427439188051
-    dkappa: 0.0064126963250630033
+    x: 586910.02439535712
+    y: 4141018.4446349298
+    theta: 1.3924663700587336
+    kappa: -0.028398158718384373
+    s: 17.746649266956798
+    dkappa: 0.0066069858660654027
     ddkappa: 0
   }
-  v: 4.2661687456554276
-  a: 0.27800859494798214
+  v: 4.2607507882422979
+  a: 0.27667121488232488
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586910.18671006756
-    y: 4141018.8620021986
-    theta: 1.3794076341503487
-    kappa: -0.025068646318005545
-    s: 18.143424182106788
-    dkappa: 0.0056127313203501832
+    x: 586910.10105888452
+    y: 4141018.8651522417
+    theta: 1.3805108186847388
+    kappa: -0.025925897186465638
+    s: 18.174097633782026
+    dkappa: 0.0058917378653619958
     ddkappa: 0
   }
-  v: 4.2936742062458952
-  a: 0.27250530272471746
+  v: 4.288125562487977
+  a: 0.27122323435790779
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586910.27368560084
-    y: 4141019.2838392179
-    theta: 1.3691464266118754
-    kappa: -0.023464970501615987
-    s: 18.574148278886707
-    dkappa: 0.0047424333714000478
+    x: 586910.1874026563
+    y: 4141019.2865420431
+    theta: 1.3699556627508471
+    kappa: -0.024110903602626185
+    s: 18.604260506920852
+    dkappa: 0.0051110383434940908
     ddkappa: 0
   }
-  v: 4.320761839557778
-  a: 0.26976301122365631
+  v: 4.31508639391465
+  a: 0.26850401785662364
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 586910.36247013381
-    y: 4141019.7080728509
-    theta: 1.3590107912883411
-    kappa: -0.021957021949256063
-    s: 19.007572861422034
-    dkappa: 0.0038577439266422923
+    x: 586910.27591849887
+    y: 4141019.7102456111
+    theta: 1.3595967159176672
+    kappa: -0.022404073895769257
+    s: 19.037111252505035
+    dkappa: 0.0043146444116074862
     ddkappa: 0
   }
-  v: 4.3477410342175116
-  a: 0.27044749152913461
+  v: 4.3419396567309612
+  a: 0.26918352150511948
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 586910.45593063615
-    y: 4141020.1340581765
-    theta: 1.3512831362863373
-    kappa: -0.02093629681528093
-    s: 19.443705329325066
-    dkappa: 0.0025395541927026623
+    x: 586910.36890118988
+    y: 4141020.135734316
+    theta: 1.3515682171133898
+    kappa: -0.021111836803623735
+    s: 19.472657223867838
+    dkappa: 0.0031397670184522583
     ddkappa: 0
   }
-  v: 4.3749877559599675
-  a: 0.27522451472548826
+  v: 4.3690587207582219
+  a: 0.27393170143004236
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586910.55533630121
-    y: 4141020.5615301291
-    theta: 1.3443962815560813
-    kappa: -0.019612441650192772
-    s: 19.882594007636996
-    dkappa: 0.0021652185910985317
+    x: 586910.46791678935
+    y: 4141020.5626819069
+    theta: 1.3447167645755131
+    kappa: -0.019756193253611509
+    s: 19.910946461105272
+    dkappa: 0.0025483549810484721
     ddkappa: 0
   }
-  v: 4.40294454762845
-  a: 0.28475985189705311
+  v: 4.3968839514304054
+  a: 0.28341851375803923
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 586910.65935974394
-    y: 4141020.9908482572
-    theta: 1.3358126214280333
-    kappa: -0.017121723071713873
-    s: 20.324334804538779
-    dkappa: 0.003996973363587477
+    x: 586910.57183216081
+    y: 4141020.9913956178
+    theta: 1.3360402988378972
+    kappa: -0.017506169584894753
+    s: 20.352074390637394
+    dkappa: 0.0038613166874479797
     ddkappa: 0
   }
-  v: 4.4321205291746963
-  a: 0.29971927412816507
+  v: 4.4259227097941531
+  a: 0.29831391461575729
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 586910.76479788311
-    y: 4141021.4229071466
-    theta: 1.3274296383686881
-    kappa: -0.01467259111935184
-    s: 20.769077869061963
-    dkappa: 0.0055798377152232086
+    x: 586910.67686642532
+    y: 4141021.4229094605
+    theta: 1.3274396089640712
+    kappa: -0.015269417199161667
+    s: 20.79619052476956
+    dkappa: 0.005072107158514437
     ddkappa: 0
   }
-  v: 4.4630913976588786
-  a: 0.32076855250315994
+  v: 4.4567493525087691
+  a: 0.31928786012984345
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 586910.87676842627
-    y: 4141021.8566438355
-    theta: 1.3210980848991651
-    kappa: -0.012682889206129047
-    s: 21.217034248799539
-    dkappa: 0.0050431879635147829
+    x: 586910.78893124335
+    y: 4141021.8559589507
+    theta: 1.3208113020429284
+    kappa: -0.013447655704932657
+    s: 21.243505161253669
+    dkappa: 0.0046123739767143339
     ddkappa: 0
   }
-  v: 4.4964994272496011
-  a: 0.34857345810637386
+  v: 4.4900052318462222
+  a: 0.34701030642694475
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 586910.98961180449
-    y: 4141022.2937615984
-    theta: 1.3147171755696432
-    kappa: -0.010677677126318881
-    s: 21.668482547616769
-    dkappa: 0.0045023549082709126
+    x: 586910.901866968
+    y: 4141022.2923738644
+    theta: 1.3141314835395868
+    kappa: -0.011611736475089989
+    s: 21.69429608284948
+    dkappa: 0.0041490679996491154
     ddkappa: 0
   }
-  v: 4.5330534692239048
-  a: 0.38379976202214239
+  v: 4.5263986956911468
+  a: 0.38215120963370841
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 586911.107512436
-    y: 4141022.7335216985
-    theta: 1.310031018136808
-    kappa: -0.0090443279945202747
-    s: 22.123775583362054
-    dkappa: 0.0040524834262138073
+    x: 586911.01992496033
+    y: 4141022.7313924041
+    theta: 1.3089278636922108
+    kappa: -0.010069657359739142
+    s: 22.148915256885822
+    dkappa: 0.0037632750794502874
     ddkappa: 0
   }
-  v: 4.5735289519672593
-  a: 0.42711323533480178
+  v: 4.56670508754084
+  a: 0.42538052587678121
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 586911.22703801363
-    y: 4141023.1772778984
-    theta: 1.3055215944489795
-    kappa: -0.00744472201208597
-    s: 22.583347045577749
-    dkappa: 0.0036104469590172504
+    x: 586911.13991100236
+    y: 4141023.1743082637
+    theta: 1.303977956380149
+    kappa: -0.008574183215285346
+    s: 22.6077955348219
+    dkappa: 0.0033899378579329035
     ddkappa: 0
   }
-  v: 4.6187678809735724
-  a: 0.47917964912868755
+  v: 4.6117667465052667
+  a: 0.47736821128281043
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 586911.35005529923
-    y: 4141023.625055809
-    theta: 1.3028641225895363
-    kappa: -0.0060859112162882714
-    s: 23.047718153211044
-    dkappa: 0.0032230497172464033
+    x: 586911.2635194906
+    y: 4141023.6211863263
+    theta: 1.3006737504704633
+    kappa: -0.0072609413832359672
+    s: 23.071457351808547
+    dkappa: 0.0030611629010402455
     ddkappa: 0
   }
-  v: 4.669678838845182
-  a: 0.54066477448813588
+  v: 4.6624930073070558
+  a: 0.538784221978443
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 586911.47543683485
-    y: 4141024.0778013291
-    theta: 1.3009626858079377
-    kappa: -0.0048179716887509182
-    s: 23.517504312324757
-    dkappa: 0.002855692434793848
+    x: 586911.38984295353
+    y: 4141024.0729139652
+    theta: 1.2982443241178885
+    kappa: -0.006038851650835295
+    s: 23.540515426249495
+    dkappa: 0.0027546317786592235
     ddkappa: 0
   }
-  v: 4.727236985292862
-  a: 0.61223438249748252
+  v: 4.7198602002814969
+  a: 0.61029851409032609
   relative_time: 5.7999999999999963
 }
 trajectory_point {
   path_point {
-    x: 586911.6036285921
-    y: 4141024.5361278756
-    theta: 1.298936961241755
-    kappa: -0.0037104797226985143
-    s: 23.993421773808222
-    dkappa: 0.0025219090107938893
+    x: 586911.51913892862
+    y: 4141024.5301528014
+    theta: 1.2956855823601598
+    kappa: -0.0049327290239365639
+    s: 24.015685459362629
+    dkappa: 0.0024735324926922486
     ddkappa: 0
   }
-  v: 4.79248405713582
-  a: 0.6945542442410636
+  v: 4.7849116513765511
+  a: 0.69258104374510643
   relative_time: 5.8999999999999959
 }
 trajectory_point {
   path_point {
-    x: 586911.73456015869
-    y: 4141025.0009103976
-    theta: 1.2968082120436293
-    kappa: -0.0027174446780922354
-    s: 24.476294291088113
-    dkappa: 0.0022115676105086152
+    x: 586911.65156704164
+    y: 4141024.9937133556
+    theta: 1.2929978341908406
+    kappa: -0.00393425148461222
+    s: 24.497790834741274
+    dkappa: 0.0022159488724312793
     ddkappa: 0
   }
-  v: 4.8665283683016938
-  a: 0.78829013080321475
+  v: 4.8587576821528362
+  a: 0.78630176706943156
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 586911.86815708608
-    y: 4141025.4731416292
-    theta: 1.2952208772329943
-    kappa: -0.0018357146976591447
-    s: 24.967059777839296
-    dkappa: 0.0019289537642170488
+    x: 586911.7868586682
+    y: 4141025.464642683
+    theta: 1.2907849828038405
+    kappa: -0.0030114897192344489
+    s: 24.987769317915436
+    dkappa: 0.0019790612085809015
     ddkappa: 0
   }
-  v: 4.9505448098265585
-  a: 0.89410781326827149
+  v: 4.9425756097836429
+  a: 0.89213064018994792
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 586912.00470789976
-    y: 4141025.9538402283
-    theta: 1.294171878712862
-    kappa: -0.0010634642622477345
-    s: 25.466776965695644
-    dkappa: 0.0016734774984058882
+    x: 586911.92550083133
+    y: 4141025.9439025037
+    theta: 1.2891863323378656
+    kappa: -0.0021880072696210907
+    s: 25.486679755913109
+    dkappa: 0.0017692738787036969
     ddkappa: 0
   }
-  v: 5.0457748498549213
-  a: 1.0126730627205707
+  v: 5.03760974705492
+  a: 1.0107376192333031
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
-    x: 586912.1442072927
-    y: 4141026.4442401724
-    theta: 1.2936479784269812
-    kappa: -0.0003845669162438922
-    s: 25.976632061960924
-    dkappa: 0.0014363510165038577
+    x: 586912.067336906
+    y: 4141026.4327714788
+    theta: 1.2880654308475483
+    kappa: -0.00142661851821034
+    s: 25.995708776821484
+    dkappa: 0.0015717684779462989
     ddkappa: 0
   }
-  v: 5.1535265336397247
-  a: 1.1446516502444475
+  v: 5.1451714023652846
+  a: 1.1427926603261436
   relative_time: 6.2999999999999945
 }
 trajectory_point {
   path_point {
-    x: 586912.287017945
-    y: 4141026.945611015
-    theta: 1.2936521909966192
-    kappa: 0.00020186322045057735
-    s: 26.497945407319609
-    dkappa: 0.0012171485209557025
+    x: 586912.21284700441
+    y: 4141026.9324858887
+    theta: 1.2875689218367548
+    kappa: -0.000748445509197412
+    s: 26.516177489348269
+    dkappa: 0.0013908800078402286
     ddkappa: 0
   }
-  v: 5.2751744835423411
-  a: 1.2907093469242381
+  v: 5.2666388797260195
+  a: 1.288965719595117
   relative_time: 6.3999999999999941
 }
 trajectory_point {
   path_point {
-    x: 586912.43324392906
-    y: 4141027.4594422672
-    theta: 1.2938070501494083
-    kappa: 0.00069313031690110846
-    s: 27.032178133547745
-    dkappa: 0.0010152148114897924
+    x: 586912.36208651832
+    y: 4141027.4445521436
+    theta: 1.2872523652298744
+    kappa: -0.000135663158745195
+    s: 27.049548182382928
+    dkappa: 0.0012208033180768026
     ddkappa: 0
   }
-  v: 5.41215989903258
-  a: 1.4515119238442784
+  v: 5.4034574787610667
+  a: 1.44992675316687
   relative_time: 6.4999999999999938
 }
 trajectory_point {
   path_point {
-    x: 586912.583357757
-    y: 4141027.9872719091
-    theta: 1.294073854885009
-    kappa: 0.0011192537626123909
-    s: 27.580938821223757
-    dkappa: 0.00082403559799237793
+    x: 586912.51549986668
+    y: 4141027.9705178775
+    theta: 1.2871051981374055
+    kappa: 0.00041768606635769393
+    s: 27.597431024557942
+    dkappa: 0.001060260638325283
     ddkappa: 0
   }
-  v: 5.565990556688682
-  a: 1.6277251520889033
+  v: 5.55713949470704
+  a: 1.6263457171680502
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
-    x: 586912.73743424972
-    y: 4141028.5309109096
-    theta: 1.2947874969032505
-    kappa: 0.0014424383360176151
-    s: 28.145990157439329
-    dkappa: 0.000660939513347408
+    x: 586912.67325754534
+    y: 4141028.5121715534
+    theta: 1.2874553082891591
+    kappa: 0.0008966113878265343
+    s: 28.161590763810104
+    dkappa: 0.00091911258733153359
     ddkappa: 0
   }
-  v: 5.7382408101973228
-  a: 1.8200148027424508
+  v: 5.7292642184132117
+  a: 1.8188925677253034
   relative_time: 6.6999999999999931
 }
 trajectory_point {
   path_point {
-    x: 586912.89629537612
-    y: 4141029.0921253883
-    theta: 1.2956677095655049
-    kappa: 0.0017354571408045051
-    s: 28.729255593510238
-    dkappa: 0.00050424882114527949
+    x: 586912.83600907447
+    y: 4141029.0713300384
+    theta: 1.2880430799006186
+    kappa: 0.0013499882620683979
+    s: 28.743953426941747
+    dkappa: 0.000784314179554772
     ddkappa: 0
   }
-  v: 5.9305515903536126
-  a: 2.0290466468892556
+  v: 5.9214779363415238
+  a: 2.0282372609652786
   relative_time: 6.7999999999999927
 }
 trajectory_point {
   path_point {
-    x: 586913.05969891185
-    y: 4141029.6731559588
-    theta: 1.2965492457893681
-    kappa: 0.0019079368661407854
-    s: 29.332826002687188
-    dkappa: 0.00037080830665981747
+    x: 586913.00367027137
+    y: 4141029.6501981406
+    theta: 1.2887864249376584
+    kappa: 0.0017036178378480658
+    s: 29.346613019182055
+    dkappa: 0.000662961362357259
     ddkappa: 0
   }
-  v: 6.1446304050610925
-  a: 2.2554864556136525
+  v: 6.1354939305665779
+  a: 2.255049753014621
   relative_time: 6.8999999999999924
 }
 trajectory_point {
   path_point {
-    x: 586913.228751468
-    y: 4141030.2760428353
-    theta: 1.2976842895706691
-    kappa: 0.0020475064408520503
-    s: 29.958966337866656
-    dkappa: 0.00024640935995809356
+    x: 586913.17720186408
+    y: 4141030.2508586342
+    theta: 1.2898090371649147
+    kappa: 0.0020330419117133816
+    s: 29.971838223748293
+    dkappa: 0.00054621504145275362
     ddkappa: 0
   }
-  v: 6.38225133933174
-  a: 2.4999999999999791
+  v: 6.3730924787756464
+  a: 2.4999999999999787
   relative_time: 6.999999999999992
 }
 decision {
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_slowdown_01_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_slowdown_01_0.pb.txt
index 0ef5b6fc6b4ab0a80f4eb18cb9988383d6172d02..468fbf83f645612d3a5260a6a67f60b7040c62e3 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_slowdown_01_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_slowdown_01_0.pb.txt
@@ -1,7 +1,7 @@
 header {
   timestamp_sec: 1505249218.2976625
 }
-total_path_length: 8.5462798029703535
+total_path_length: 8.60572345460797
 total_path_time: 6.999999999999992
 is_replan: true
 gear: GEAR_DRIVE
@@ -17,1547 +17,1547 @@ trajectory_point {
     ddkappa: 0
   }
   v: 1.7111110687255859
-  a: -0.570132219168194
+  a: -0.570013889304769
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 587258.3766194199
-    y: 4140947.0422441363
-    theta: -1.793814089950982
-    kappa: 0.00082587449539007368
-    s: 0.034108996853073718
-    dkappa: -0.00018088468900636972
+    x: 587258.37661941291
+    y: 4140947.0422441061
+    theta: -1.7938140899283463
+    kappa: 0.00082587433039370414
+    s: 0.034109028084377865
+    dkappa: -0.0001808848546302596
     ddkappa: 0
   }
-  v: 1.6998285328834912
-  a: -0.55815729099662059
+  v: 1.6998320320789695
+  a: -0.55792614112238126
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 587258.36908041209
-    y: 4140947.0092077041
-    theta: -1.7937895302101057
-    kappa: 0.00064685469295910523
-    s: 0.067994723642217564
-    dkappa: -0.00036058534623800405
+    x: 587258.36908037763
+    y: 4140947.0092075532
+    theta: -1.7937895300978841
+    kappa: 0.00064685387495844659
+    s: 0.067994878477305357
+    dkappa: -0.00036058616734971328
     ddkappa: 0
   }
-  v: 1.6887833475794758
-  a: -0.54639678876184916
+  v: 1.6887925760804063
+  a: -0.54605544298264563
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 587258.36159003153
-    y: 4140946.97638436
-    theta: -1.7937651288816234
-    kappa: 0.00046898958331010411
-    s: 0.1016618846436451
-    dkappa: -0.00053912691911694424
+    x: 587258.36158993922
+    y: 4140946.9763839552
+    theta: -1.7937651285809184
+    kappa: 0.00046898739142409271
+    s: 0.10166229953434172
+    dkappa: -0.00053912911933921847
     ddkappa: 0
   }
-  v: 1.6779712468907089
-  a: -0.53484845287332816
+  v: 1.677988382620051
+  a: -0.5343995057848081
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 587258.35414725041
-    y: 4140946.9437696
-    theta: -1.7937408826174739
-    kappa: 0.00029225476189620795
-    s: 0.1351150992670315
-    dkappa: -0.00071653390500633679
+    x: 587258.35414706031
+    y: 4140946.9437687662
+    theta: -1.7937408819981919
+    kappa: 0.00029225024785358084
+    s: 0.13511595370666946
+    dkappa: -0.00071653843621693949
     ddkappa: 0
   }
-  v: 1.6673880100861713
-  a: -0.52351002374050559
+  v: 1.6674151793700731
+  a: -0.52295604042811428
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 587258.34675105987
-    y: 4140946.9113590023
-    theta: -1.7937167881304494
-    kappa: 0.00011662626774924703
-    s: 0.16835890295934949
-    dkappa: -0.000892830356003601
+    x: 587258.34675072262
+    y: 4140946.9113575243
+    theta: -1.7937167870320454
+    kappa: 0.00011661826130912665
+    s: 0.16836041845673463
+    dkappa: -0.00089283839289411358
     ddkappa: 0
   }
-  v: 1.6570294616266543
-  a: -0.51237924177282967
+  v: 1.6570687397846571
+  a: -0.51172275781181
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 587258.339400469
-    y: 4140946.8791482258
-    theta: -1.793692842193543
-    kappa: -5.7919421295263241e-05
-    s: 0.20139774810870587
-    dkappa: -0.0010680398837335961
+    x: 587258.33939992636
+    y: 4140946.8791458476
+    theta: -1.7936928404257542
+    kappa: -5.7932306985145868e-05
+    s: 0.20140018717388705
+    dkappa: -0.0010680528184308158
     ddkappa: 0
   }
-  v: 1.6468914711647609
-  a: -0.50145384737974874
+  v: 1.6469448831000031
+  a: -0.500697368835141
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 587258.33209450566
-    y: 4140946.84713301
-    theta: -1.7936690416392911
-    kappa: -0.00023140539337312971
-    s: 0.23423600494817756
-    dkappa: -0.0012421856641417893
+    x: 587258.33209369029
+    y: 4140946.8471294371
+    theta: -1.7936690389828647
+    kappa: -0.00023142475648057473
+    s: 0.23423967009002084
+    dkappa: -0.0012422051008917278
     ddkappa: 0
   }
-  v: 1.6369699535449043
-  a: -0.4907315809707109
+  v: 1.6370394743343257
+  a: -0.48987758439735296
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 587258.32483221591
-    y: 4140946.8153091744
-    theta: -1.79364538335912
-    kappa: -0.00040385430736648669
-    s: 0.26687796245964779
-    dkappa: -0.0014152904422874224
+    x: 587258.32483105164
+    y: 4140946.8153040735
+    theta: -1.793645379566531
+    kappa: -0.00040388195214132426
+    s: 0.26688319519521431
+    dkappa: -0.0014153181922019005
     ddkappa: 0
   }
-  v: 1.6272608688033092
-  a: -0.48021018295516438
+  v: 1.6273484242878549
+  a: -0.47926111539769173
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 587258.31761266279
-    y: 4140946.783672615
-    theta: -1.7936218643026898
-    kappa: -0.00057528839767880609
-    s: 0.29932782927764257
-    dkappa: -0.001587376537136681
+    x: 587258.31761106546
+    y: 4140946.7836656151
+    theta: -1.7936218590988502
+    kappa: -0.00057532632928175966
+    s: 0.29933500915337086
+    dkappa: -0.0015874146130025241
     ddkappa: 0
   }
-  v: 1.6177602221680114
-  a: -0.46988739374255739
+  v: 1.6178676895428354
+  a: -0.468845672735403
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 587258.31043492816
-    y: 4140946.7522193058
-    theta: -1.7935984814772408
-    kappa: -0.000745729479009901
-    s: 0.3315897345931666
-    dkappa: -0.0017584658463558594
+    x: 587258.31043280486
+    y: 4140946.752210001
+    theta: -1.7935984745602
+    kappa: -0.00074577989840380959
+    s: 0.33159927821785873
+    dkappa: -0.0017585164575066904
     ddkappa: 0
   }
-  v: 1.6084640640588577
-  a: -0.45976095374233805
+  v: 1.6085932724635279
+  a: -0.45862896730973257
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 587258.30329811061
-    y: 4140946.7209452977
-    theta: -1.793575231946936
-    kappa: -0.00091519895113093355
-    s: 0.36366772905753952
-    dkappa: -0.0019285798511045292
+    x: 587258.30329536065
+    y: 4140946.7209332474
+    theta: -1.7935752229885742
+    kappa: -0.00091526425003433671
+    s: 0.36368008914715144
+    dkappa: -0.0019286453983551612
     ddkappa: 0
   }
-  v: 1.5993684900875056
-  a: -0.44982860336395469
+  v: 1.5995212211962071
+  a: -0.44860871001992619
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587258.29620132619
-    y: 4140946.6898467178
-    theta: -1.7935521128322085
-    kappa: -0.0010837178036594247
-    s: 0.39556578568623263
-    dkappa: -0.0020977396208287087
+    x: 587258.29619784118
+    y: 4140946.6898314459
+    theta: -1.7935521014789195
+    kappa: -0.0010838005595625081
+    s: 0.39558145012046836
+    dkappa: -0.0020978226914721348
     ddkappa: 0
   }
-  v: 1.5904696410574242
-  a: -0.44008808301685542
+  v: 1.5906476296691634
+  a: -0.43878261176522959
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 587258.28914370807
-    y: 4140946.6589197675
-    theta: -1.7935291213091049
-    kappa: -0.0012513066208342568
-    s: 0.42728780076270417
-    dkappa: -0.0022659658180540269
+    x: 587258.28913937171
+    y: 4140946.6589007652
+    theta: -1.7935291071824719
+    kappa: -0.0012514095920771618
+    s: 0.42730729165341474
+    dkappa: -0.0022660691809210094
     ddkappa: 0
   }
-  v: 1.5817637029638931
-  a: -0.43053713311048847
+  v: 1.5819686375927022
+  a: -0.42914838344488848
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587258.28212440631
-    y: 4140946.6281607216
-    theta: -1.7935062546086304
-    kappa: -0.0014179855862906845
-    s: 0.45883759474223662
-    dkappa: -0.0024332787031788932
+    x: 587258.282119095
+    y: 4140946.6281374469
+    theta: -1.7935062373060924
+    kappa: -0.0014181117072041736
+    s: 0.458861467513622
+    dkappa: -0.0024334053037601525
     ddkappa: 0
   }
-  v: 1.5732469069940034
-  a: -0.421173494054302
+  v: 1.5734804304591439
+  a: -0.41970373595814869
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587258.27514258749
-    y: 4140946.5975659285
-    theta: -1.7934835100160949
-    kappa: -0.001583774487835339
-    s: 0.49021891315577204
-    dkappa: -0.0025996981392676652
+    x: 587258.27513617044
+    y: 4140946.5975378091
+    theta: -1.7934834891116043
+    kappa: -0.0015839268639438312
+    s: 0.49024775563638834
+    dkappa: -0.0025998510948986656
     ddkappa: 0
   }
-  v: 1.5649155295266568
-  a: -0.41199490625774426
+  v: 1.5651792395428239
+  a: -0.41044638020425595
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 587258.26819743472
-    y: 4140946.5671318094
-    theta: -1.7934608848704554
-    kappa: -0.0017486927222212351
-    s: 0.52143542751374872
-    dkappa: -0.0027652435968438132
+    x: 587258.26818977436
+    y: 4140946.5670982408
+    theta: -1.7934608599151292
+    kappa: -0.0017488746255081935
+    s: 0.52146985904031851
+    dkappa: -0.0027654261919521483
     ddkappa: 0
   }
-  v: 1.5567658921325664
-  a: -0.40299911013026346
+  v: 1.5570613419000929
+  a: -0.40137402708245595
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 587258.26128814777
-    y: 4140946.5368548553
-    theta: -1.7934383765636641
-    kappa: -0.0019127592999227813
-    s: 0.55249073620993738
-    dkappa: -0.0029299341586830922
+    x: 587258.26127909927
+    y: 4140946.5368152042
+    theta: -1.7934383470864226
+    kappa: -0.0019129741641584698
+    s: 0.55253140674296464
+    dkappa: -0.002930149840098469
     ddkappa: 0
   }
-  v: 1.5487943615742561
-  a: -0.3941838460813078
+  v: 1.5491230603693169
+  a: -0.39248438749199455
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 587258.25441394164
-    y: 4140946.5067316284
-    theta: -1.7934159825400109
-    kappa: -0.002075992849910782
-    s: 0.58338836542527717
-    dkappa: -0.0030937885246067037
+    x: 587258.2544033539
+    y: 4140946.5066852318
+    theta: -1.7934159480482119
+    kappa: -0.00207624426604238
+    s: 0.58343595467646636
+    dkappa: -0.0030940408969335263
     ddkappa: 0
   }
-  v: 1.540997349806061
-  a: -0.38554685452032544
+  v: 1.5413607635708759
+  a: -0.38377517233211744
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587258.24757404812
-    y: 4140946.4767587613
-    theta: -1.7933937002954692
-    kappa: -0.002238411624427449
-    s: 0.61413177003171238
-    dkappa: -0.00325682501627447
+    x: 587258.24756176327
+    y: 4140946.4767049281
+    theta: -1.7933936602755309
+    kappa: -0.0022387033360315271
+    s: 0.61418698660319082
+    dkappa: -0.0032571178373270184
     ddkappa: 0
   }
-  v: 1.533371313974127
-  a: -0.37708587585676456
+  v: 1.5337708659071665
+  a: -0.37524409250207036
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587258.24076771387
-    y: 4140946.4469329533
-    theta: -1.7933715273770405
-    kappa: -0.0024000335037614056
-    s: 0.64472433449602784
-    dkappa: -0.003419061581977995
+    x: 587258.24075356824
+    y: 4140946.4468709663
+    theta: -1.7933714812950579
+    kappa: -0.0024003694025587627
+    s: 0.64478791503137356
+    dkappa: -0.0034193987582782073
     ddkappa: 0
   }
-  v: 1.5259127564164112
-  a: -0.36879865050007349
+  v: 1.5263498275625993
+  a: -0.36688885890109912
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587258.23399420152
-    y: 4140946.4172509732
-    theta: -1.7933494613821002
-    kappa: -0.0025608760010226946
-    s: 0.67516937378368613
-    dkappa: -0.0035805158014338367
+    x: 587258.23397802515
+    y: 4140946.4171800874
+    theta: -1.7933494086844504
+    kappa: -0.0025612601224555605
+    s: 0.67524208213075831
+    dkappa: -0.0035809013837716868
     ddkappa: 0
   }
-  v: 1.5186182246626818
-  a: -0.36068291885970039
+  v: 1.5190941545036005
+  a: -0.35870718242844951
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 587258.22725278884
-    y: 4140946.3877096558
-    theta: -1.7933274999577413
-    kappa: -0.0027209562669177848
-    s: 0.705470134262663
-    dkappa: -0.0037412048905766716
+    x: 587258.2272344057
+    y: 4140946.3876291
+    theta: -1.7933274400716828
+    kappa: -0.0027213927857893771
+    s: 0.70555276064823724
+    dkappa: -0.0037416430696331441
     ddkappa: 0
   }
-  v: 1.5114843114345182
-  a: -0.35273642134509331
+  v: 1.512000398478611
+  a: -0.35069677398336713
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587258.22054276848
-    y: 4140946.3583059018
-    theta: -1.7933056408001204
-    kappa: -0.0028802910945245792
-    s: 0.735629794607284
-    dkappa: -0.0039011457063524622
+    x: 587258.22052199731
+    y: 4140946.3582148817
+    theta: -1.7933055731343821
+    kappa: -0.0028807843207010304
+    s: 0.735723154823492
+    dkappa: -0.0039016408083851349
     ddkappa: 0
   }
-  v: 1.50450765464531
-  a: -0.34495689836570065
+  v: 1.5050651570180871
+  a: -0.3428553444650978
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587258.21386344824
-    y: 4140946.3290366777
-    theta: -1.7932838816538015
-    kappa: -0.0030388969240674195
-    s: 0.76565146670206041
-    dkappa: -0.0040603547515116285
+    x: 587258.21384010208
+    y: 4140946.3289343733
+    theta: -1.7932838055991647
+    kappa: -0.0030394512982420572
+    s: 0.76575640130463318
+    dkappa: -0.0040609112341028382
     ddkappa: 0
   }
-  v: 1.497684937400259
-  a: -0.33734209033097046
+  v: 1.4982850734345
+  a: -0.33518060477288736
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587258.20721415034
-    y: 4140946.2998990137
-    theta: -1.7932622203111022
-    kappa: -0.0031967898476920964
-    s: 0.79553819654552616
-    dkappa: -0.0042188481794022091
+    x: 587258.20718803676
+    y: 4140946.2997845821
+    theta: -1.7932621352409717
+    kappa: -0.0031974099372120918
+    s: 0.79565557006384113
+    dkappa: -0.0042194706272698328
     ddkappa: 0
   }
-  v: 1.4910128879963771
-  a: -0.329889737650351
+  v: 1.4916568368223362
+  a: -0.32767026580598152
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587258.2005942117
-    y: 4140946.270890004
-    theta: -1.7932406546114372
-    kappa: -0.0033539856142408538
-    s: 0.82529296515407291
-    dkappa: -0.0043766417987630337
+    x: 587258.20056513313
+    y: 4140946.2707625795
+    theta: -1.793240559882407
+    kappa: -0.0033546761089962244
+    s: 0.82542366531300571
+    dkappa: -0.0043773349196338544
     ddkappa: 0
   }
-  v: 1.4844882799224879
-  a: -0.32259758073329059
+  v: 1.485177182058097
+  a: -0.32032203846362595
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587258.19400298328
-    y: 4140946.2420068039
-    theta: -1.7932191824406645
-    kappa: -0.0035104996340273964
-    s: 0.85491868946578731
-    dkappa: -0.0045337510785168882
+    x: 587258.19397073728
+    y: 4140946.2418654994
+    theta: -1.7932190773930718
+    kappa: -0.00351126534240238
+    s: 0.8550636264193674
+    dkappa: -0.00453451969906257
     ddkappa: 0
   }
-  v: 1.4781079318592254
-  a: -0.31546335998923719
+  v: 1.478842889800299
+  a: -0.31313363364506652
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587258.18743983016
-    y: 4140946.2132466314
-    theta: -1.7931978017304298
-    kappa: -0.0036663469836118981
-    s: 0.88441822324428654
-    dkappa: -0.004690191152563682
+    x: 587258.18740420928
+    y: 4140946.2130905385
+    theta: -1.7931976856889027
+    kappa: -0.0036671928284986791
+    s: 0.88457832882115706
+    dkappa: -0.0046910402143993373
     ddkappa: 0
   }
-  v: 1.4718687076790353
-  a: -0.3084848158276392
+  v: 1.472650786489474
+  a: -0.30610276224954885
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587258.18090413115
-    y: 4140946.1846067649
-    theta: -1.7931765104575108
-    kappa: -0.003821542410576007
-    s: 0.91379435798255493
-    dkappa: -0.0048459768245736187
+    x: 587258.18086492363
+    y: 4140946.184434955
+    theta: -1.7931763827315064
+    kappa: -0.0038224734254508094
+    s: 0.91397058494323624
+    dkappa: -0.0048469113803189749
     ddkappa: 0
   }
-  v: 1.4657675164461739
-  a: -0.30165968865794479
+  v: 1.4665977443481686
+  a: -0.29922713517631883
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587258.174395279
-    y: 4140946.1560845436
-    theta: -1.7931553066431634
-    kappa: -0.0039761003382978539
-    s: 0.94304982380678015
-    dkappa: -0.0050011225727803589
+    x: 587258.17435226822
+    y: 4140946.1558960672
+    theta: -1.7931551665274974
+    kappa: -0.00397712166335939
+    s: 0.94324314511273721
+    dkappa: -0.0050021477821835256
     ddkappa: 0
   }
-  v: 1.4598013124167086
-  a: -0.29498571888960207
+  v: 1.4606806813809443
+  a: -0.2925044633246221
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587258.16791267972
-    y: 4140946.1276773638
-    theta: -1.7931341883524656
-    kappa: -0.0041300348707270576
-    s: 0.97218729038018914
-    dkappa: -0.0051556425547741895
+    x: 587258.16786564491
+    y: 4140946.1274712542
+    theta: -1.7931340351278335
+    kappa: -0.0041311517490973514
+    s: 0.97239869847470461
+    dkappa: -0.0051567636808980263
     ddkappa: 0
   }
-  v: 1.4539670950385177
-  a: -0.28846064693205931
+  v: 1.4548965613743785
+  a: -0.2859324575937045
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587258.16143503
-    y: 4140946.0993874562
-    theta: -1.7931556500275998
-    kappa: -0.0042822565358850654
-    s: 1.0012093678068847
-    dkappa: -0.0052819340408764813
+    x: 587258.16138279065
+    y: 4140946.0991629479
+    theta: -1.7931574428013002
+    kappa: -0.0042834234298176083
+    s: 1.0014398739077337
+    dkappa: -0.005281882829023009
     ddkappa: 0
   }
-  v: 1.4482619089512911
-  a: -0.28208221319476473
+  v: 1.4492423938970627
+  a: -0.27950882888281181
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587258.15488334955
-    y: 4140946.0712304031
-    theta: -1.7933804932074142
-    kappa: -0.0044286041436479
-    s: 1.0301186075356814
-    dkappa: -0.00527551123582481
+    x: 587258.15482654865
+    y: 4140946.0709862909
+    theta: -1.7933824425223057
+    kappa: -0.0044298729282799022
+    s: 1.0303692409396124
+    dkappa: -0.005275455552261079
     ddkappa: 0
   }
-  v: 1.4426828439865291
-  a: -0.27584815808716651
+  v: 1.4437152342996042
+  a: -0.27323128809118963
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587258.14835667622
-    y: 4140946.0431808238
-    theta: -1.79360447818078
-    kappa: -0.0045743931555894349
-    s: 1.0589175032639417
-    dkappa: -0.0052691129460495957
+    x: 587258.14829507668
+    y: 4140946.0429160884
+    theta: -1.7936065921775668
+    kappa: -0.0045757691296029971
+    s: 1.0591893106629606
+    dkappa: -0.0052690525582306186
     ddkappa: 0
   }
-  v: 1.4372270351675436
-  a: -0.26975622201871274
+  v: 1.4383121837146251
+  a: -0.26709754611808384
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587258.14185445779
-    y: 4140946.0152363433
-    theta: -1.7938276239004287
-    kappa: -0.0047196359078047843
-    s: 1.0876084918414131
-    dkappa: -0.0052627386301526179
+    x: 587258.14178781863
+    y: 4140946.014949949
+    theta: -1.793829910849881
+    kappa: -0.0047211244545408105
+    s: 1.0879025366508712
+    dkappa: -0.0052626733018179637
     ddkappa: 0
   }
-  v: 1.4318916627094569
-  a: -0.26380414539885177
+  v: 1.4330303890567624
+  a: -0.26110531386274011
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587258.13537615479
-    y: 4140945.9873946435
-    theta: -1.7940499488839852
-    kappa: -0.0048643444531841652
-    s: 1.1161939541740631
-    dkappa: -0.0052563877591647653
+    x: 587258.13530423143
+    y: 4140945.9870855394
+    theta: -1.7940524171818066
+    kappa: -0.0048659510373006871
+    s: 1.1165113158725493
+    dkappa: -0.0052563172504852089
     ddkappa: 0
   }
-  v: 1.4266739520192024
-  a: -0.25798966863703182
+  v: 1.4278670430226685
+  a: -0.25525230222440437
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 587258.12892124
-    y: 4140945.9596534586
-    theta: -1.7942714712209988
-    kappa: -0.0050085305659883939
-    s: 1.1446762161279158
-    dkappa: -0.0052500598163452246
+    x: 587258.1288437841
+    y: 4140945.9593205787
+    theta: -1.7942741293827829
+    kappa: -0.0050102607301786631
+    s: 1.1450179896089536
+    dkappa: -0.0052499838840667865
     ddkappa: 0
   }
-  v: 1.4215711736955257
-  a: -0.252310532142701
+  v: 1.4228193840910111
+  a: -0.24953622210232218
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 587258.12248919858
-    y: 4140945.9320105775
-    theta: -1.7944922085799722
-    kappa: -0.005152205746424398
-    s: 1.1730575494328892
-    dkappa: -0.0052437542969806751
+    x: 587258.12240595883
+    y: 4140945.9316528388
+    theta: -1.7944950652362528
+    kappa: -0.005154065108194719
+    s: 1.1734248443684367
+    dkappa: -0.0052436726945660275
     ddkappa: 0
   }
-  v: 1.4165806435289816
-  a: -0.24676447632530751
+  v: 1.4178846965224725
+  a: -0.24395478439573925
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 587258.11607952777
-    y: 4140945.9044638369
-    theta: -1.794712178215391
-    kappa: -0.0052953812252207024
-    s: 1.2013401725866293
-    dkappa: -0.0052374707081844817
+    x: 587258.11599024932
+    y: 4140945.9040801465
+    theta: -1.7947152421067833
+    kappa: -0.0052973754737280377
+    s: 1.2017341128023844
+    dkappa: -0.0052373831859517436
     ddkappa: 0
   }
-  v: 1.4116997225019374
-  a: -0.2413492415942996
+  v: 1.41306031035975
+  a: -0.23850570000390159
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 587258.10969173652
-    y: 4140945.8770111287
-    theta: -1.7949313969747538
-    kappa: -0.00543806796820295
-    s: 1.2295262517583485
-    dkappa: -0.0052312085686958883
+    x: 587258.10959616187
+    y: 4140945.8766003791
+    theta: -1.7949346769471879
+    kappa: -0.0054402028611522614
+    s: 1.2299479746208566
+    dkappa: -0.0052311148739547907
     ddkappa: 0
   }
-  v: 1.4069258167885708
-  a: -0.23606256835912548
+  v: 1.4083436014275563
+  a: -0.23318667982605479
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 587258.1033253459
-    y: 4140945.8496503932
-    theta: -1.7951498813056015
-    kappa: -0.0055802766808693872
-    s: 1.2576179016926607
-    dkappa: -0.0052249674086792125
+    x: 587258.10322321416
+    y: 4140945.8492114628
+    theta: -1.7951533863056472
+    kappa: -0.0055825580414707518
+    s: 1.258068557508228
+    dkappa: -0.00522486728586464
     ddkappa: 0
   }
-  v: 1.4022563777548704
-  a: -0.23090219702923326
+  v: 1.4037319913326196
+  a: -0.22799543476144463
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 587258.09697988781
-    y: 4140945.8223796189
-    theta: -1.7953676472625471
-    kappa: -0.0057220178129663707
-    s: 1.2856171866134172
-    dkappa: -0.0052187467695230371
+    x: 587258.09687093541
+    y: 4140945.821911376
+    theta: -1.7953713863328304
+    kappa: -0.0057244515269518443
+    s: 1.2860979380388275
+    dkappa: -0.0052186399603259531
     ddkappa: 0
   }
-  v: 1.3976889019586365
-  a: -0.22586586801407132
+  v: 1.3992229474636821
+  a: -0.22292967570931677
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 587258.09065490577
-    y: 4140945.7951968443
-    theta: -1.795584710514305
-    kappa: -0.0058633015630638729
-    s: 1.313526121127544
-    dkappa: -0.0052125462036394067
+    x: 587258.0905388667
+    y: 4140945.7946981448
+    theta: -1.7955886927890177
+    kappa: -0.0058658935757641088
+    s: 1.3140381425925798
+    dkappa: -0.0052124324471351513
     ddkappa: 0
   }
-  v: 1.3932209311494796
-  a: -0.22095132172308768
+  v: 1.3948139829915021
+  a: -0.21798711356891715
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 587258.08434995427
-    y: 4140945.7681001546
-    theta: -1.7958010863507217
-    kappa: -0.0060041378831309777
-    s: 1.3413466711288773
-    dkappa: -0.0052063652742630161
+    x: 587258.08422655985
+    y: 4140945.7675698446
+    theta: -1.7958053210512201
+    kappa: -0.0060068941966116051
+    s: 1.3418911482706444
+    dkappa: -0.0052062443070369849
     ddkappa: 0
   }
-  v: 1.3888500522688221
-  a: -0.2161562985657306
+  v: 1.3905026568688523
+  a: -0.21316545923949137
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 587258.07806459861
-    y: 4140945.7410876816
-    theta: -1.7960167896898034
-    kappa: -0.0061445364831113816
-    s: 1.3690807547019992
-    dkappa: -0.00520020355525041
+    x: 587258.07793357759
+    y: 4140945.7405245951
+    theta: -1.7960212861203024
+    kappa: -0.0061474631533691339
+    s: 1.3696588838110557
+    dkappa: -0.0052000751115211042
     ddkappa: 0
   }
-  v: 1.3845738974498967
-  a: -0.2114785389514483
+  v: 1.3862865738305208
+  a: -0.20846242362028516
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 587258.07179841469
-    y: 4140945.7141576027
-    theta: -1.7962318350847479
-    kappa: -0.0062845068354989032
-    s: 1.3967302430260755
-    dkappa: -0.0051940606308791722
+    x: 587258.07165949373
+    y: 4140945.7135605644
+    theta: -1.7962366026281043
+    kappa: -0.0062876099697175107
+    s: 1.3973432305043656
+    dkappa: -0.0051939244426186327
     ddkappa: 0
   }
-  v: 1.3803901440177473
-  a: -0.2069157832896889
+  v: 1.382163384393311
+  a: -0.20387571761054446
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 587258.06555098877
-    y: 4140945.687308141
-    theta: -1.7964462367309728
-    kappa: -0.0064240581799129785
-    s: 1.4242969612786909
-    dkappa: -0.0051879360956471193
+    x: 587258.06540389231
+    y: 4140945.6866759667
+    theta: -1.7964512848445606
+    kappa: -0.0064273439337788046
+    s: 1.4249460231092808
+    dkappa: -0.0051877918926987376
     ddkappa: 0
   }
-  v: 1.3762965144892292
-  a: -0.20246577198990073
+  v: 1.3781307848560409
+  a: -0.19940305210951487
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 587258.05932191771
-    y: 4140945.6605375614
-    theta: -1.7966600084731454
-    kappa: -0.0065631995276741548
-    s: 1.4517826895396841
-    dkappa: -0.0051818295540714959
+    x: 587258.0591663681
+    y: 4140945.6598690581
+    theta: -1.7966653466848244
+    kappa: -0.0065666741027516091
+    s: 1.4524690507683051
+    dkappa: -0.0051816770642651982
     ddkappa: 0
   }
-  v: 1.3722907765730086
-  a: -0.198126245461532
+  v: 1.374186517299544
+  a: -0.19504213801644221
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 587258.0531108079
-    y: 4140945.6338441735
-    theta: -1.7968731638122131
-    kappa: -0.0067019396663796109
-    s: 1.479189163694987
-    dkappa: -0.0051757406204881676
+    x: 587258.05294652563
+    y: 4140945.63313814
+    theta: -1.7968788017163875
+    kappa: -0.0067056093075462944
+    s: 1.4799140579233794
+    dkappa: -0.0051755795697529805
     ddkappa: 0
   }
-  v: 1.3683707431695622
-  a: -0.19389494411403085
+  v: 1.3703283695866686
+  a: -0.1907906862305721
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 587258.04691727576
-    y: 4140945.6072263294
-    theta: -1.7970857159124316
-    kappa: -0.006840287164478643
-    s: 1.5065180763404578
-    dkappa: -0.0051696689188508137
+    x: 587258.04674397944
+    y: 4140945.6064815568
+    theta: -1.7970916631662019
+    kappa: -0.0068441581574202638
+    s: 1.5072827452315212
+    dkappa: -0.0051694990313248031
     ddkappa: 0
   }
-  v: 1.3645342723711784
-  a: -0.18976960835684548
+  v: 1.3665541753622783
+  a: -0.18664640765115059
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587258.01620051626
-    y: 4140945.4752154015
-    theta: -1.798139865897123
-    kappa: -0.007526420177764585
-    s: 1.6420555413536861
-    dkappa: -0.005139556376177805
+    x: 587258.015977872
+    y: 4140945.4742585467
+    theta: -1.7981475066919017
+    kappa: -0.00753139347532333
+    s: 1.64303795748994
+    dkappa: -0.005139338111450612
     ddkappa: 0
   }
-  v: 1.3465335541120576
-  a: -0.17065332775634212
+  v: 1.3488698888788788
+  a: -0.16745249432437009
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587257.9858709434
-    y: 4140945.3448684798
-    theta: -1.7991807282536025
-    kappa: -0.00820390444038243
-    s: 1.7758845479217051
-    dkappa: -0.0051098234040377966
+    x: 587257.985591702
+    y: 4140945.3436683887
+    theta: -1.7991903113689627
+    kappa: -0.00821014197006426
+    s: 1.7771166981675752
+    dkappa: -0.0051095496555443095
     ddkappa: 0
   }
-  v: 1.3303266001140726
-  a: -0.15384724833096364
+  v: 1.3329858109958017
+  a: -0.1505945460456628
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587257.9558904164
-    y: 4140945.2160216467
-    theta: -1.8002096119144462
-    kappa: -0.0088735919201408473
-    s: 1.9081733922127846
-    dkappa: -0.0050804326118425221
+    x: 587257.95554721064
+    y: 4140945.2145466567
+    theta: -1.8002213901879967
+    kappa: -0.008881258250776649
+    s: 1.909687785195209
+    dkappa: -0.0050800961570969475
     ddkappa: 0
   }
-  v: 1.3156965127006597
-  a: -0.13906892126173515
+  v: 1.3186826520879533
+  a: -0.13578642522074796
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587257.92579170223
-    y: 4140945.0886337152
-    theta: -1.8014793205867492
-    kappa: -0.0095195123872591249
-    s: 2.0390700928716319
-    dkappa: -0.0050454843593094227
+    x: 587257.92536101525
+    y: 4140945.0868556886
+    theta: -1.8015028804587847
+    kappa: -0.0095281536515470083
+    s: 2.0408995382290858
+    dkappa: -0.0050448603699753333
     ddkappa: 0
   }
-  v: 1.302454639077153
-  a: -0.12603589772968182
+  v: 1.3057697362896683
+  a: -0.12274199425534477
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587257.89527307241
-    y: 4140944.9626421346
-    theta: -1.8031487811629749
-    kappa: -0.010131835324987296
-    s: 2.1687052155075843
-    dkappa: -0.0050012682620542261
+    x: 587257.8947604642
+    y: 4140944.960525909
+    theta: -1.8031768223629114
+    kappa: -0.010142120245857584
+    s: 2.1708826400268544
+    dkappa: -0.0050005255835859821
     ddkappa: 0
   }
-  v: 1.2904405713307834
-  a: -0.11446572891582885
+  v: 1.2940850014947098
+  a: -0.11117511555517229
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587257.86502414872
-    y: 4140944.8377639963
-    theta: -1.8048034880028707
-    kappa: -0.010738746903321513
-    s: 2.2971946971827957
-    dkappa: -0.0049574429213402327
+    x: 587257.864421875
+    y: 4140944.8352775997
+    theta: -1.8048364341826943
+    kappa: -0.01075083086738976
+    s: 2.2997529978235125
+    dkappa: -0.0049565703332048738
     ddkappa: 0
   }
-  v: 1.2795221464306796
-  a: -0.1040759660012015
+  v: 1.283494999356269
+  a: -0.10079965152594977
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 587257.8350204163
-    y: 4140944.7138980925
-    theta: -1.8064447821468888
-    kappa: -0.011340738987622907
-    s: 2.4246426709004281
-    dkappa: -0.0049139728193625318
+    x: 587257.83432076522
+    y: 4140944.7110096882
+    theta: -1.8064830551567408
+    kappa: -0.011354776720625995
+    s: 2.427614604707347
+    dkappa: -0.004912959148749424
     ddkappa: 0
   }
-  v: 1.269595446227868
-  a: -0.094584160166825082
+  v: 1.2738948952869642
+  a: -0.091329464573396524
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587257.80523947661
-    y: 4140944.5909519554
-    theta: -1.8080738888607886
-    kappa: -0.011938260979536583
-    s: 2.5511442796541326
-    dkappa: -0.0048708255046390342
+    x: 587257.8044347869
+    y: 4140944.5876299143
+    theta: -1.8081179078024188
+    kappa: -0.011954406199159374
+    s: 2.5545623902569305
+    dkappa: -0.0048696596515294044
     ddkappa: 0
   }
-  v: 1.260583949813453
-  a: -0.085759975615415929
+  v: 1.2652075966774619
+  a: -0.082531990533819113
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587257.7756605933
-    y: 4140944.4688399779
-    theta: -1.8096919424844247
-    kappa: -0.012531728931002968
-    s: 2.6767876059588636
-    dkappa: -0.004827970933883873
+    x: 587257.77474326792
+    y: 4140944.4650529376
+    theta: -1.8097421229332802
+    kappa: -0.012550134061556672
+    s: 2.6806841631665885
+    dkappa: -0.0048266418916540054
     ddkappa: 0
   }
-  v: 1.2524164982147421
-  a: -0.07772396656086282
+  v: 1.2573611540273009
+  a: -0.074533723830567625
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587257.74626484211
-    y: 4140944.3474840331
-    theta: -1.8112999782350889
-    kappa: -0.013121522537896488
-    s: 2.8016530353729809
-    dkappa: -0.0047853816890980208
+    x: 587257.74522737274
+    y: 4140944.343200997
+    theta: -1.811356730917177
+    kappa: -0.013142338225020138
+    s: 2.8060599324313173
+    dkappa: -0.0047838785795628665
     ddkappa: 0
   }
-  v: 1.2450128675010632
-  a: -0.0704766275128256
+  v: 1.2502741956463921
+  a: -0.067335145075996777
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587257.71703515307
-    y: 4140944.2268136521
-    theta: -1.8128989298822411
-    kappa: -0.013707984287165765
-    s: 2.925813075938974
-    dkappa: -0.0047430330391546559
+    x: 587257.71587014617
+    y: 4140944.2220040951
+    theta: -1.8129626592589334
+    kappa: -0.013731358878762329
+    s: 2.9307617196390261
+    dkappa: -0.0047413451500496542
     ddkappa: 0
   }
-  v: 1.2382962697923507
-  a: -0.063976376059574735
+  v: 1.243868900761701
+  a: -0.060893293833440859
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587257.68705938861
-    y: 4140944.1069895145
-    theta: -1.814487577111523
-    kappa: -0.014261579698432959
-    s: 3.0493327648233857
-    dkappa: -0.004693480531354364
+    x: 587257.68568462937
+    y: 4140944.1016421933
+    theta: -1.8145585081383897
+    kappa: -0.014285164582358215
+    s: 3.0548539791025044
+    dkappa: -0.0046909769861018767
     ddkappa: 0
   }
-  v: 1.2321940754497129
-  a: -0.058181629789380705
+  v: 1.2380717446438589
+  a: -0.0551652096662342
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587257.65644851141
-    y: 4140943.9879241604
-    theta: -1.8160669525638393
-    kappa: -0.014786729105720414
-    s: 3.1722700841409273
-    dkappa: -0.0046377357030300843
+    x: 587257.65492367512
+    y: 4140943.9819930932
+    theta: -1.8161456268493141
+    kappa: -0.014812888657792272
+    s: 3.1783940274637876
+    dkappa: -0.0046349588556814066
     ddkappa: 0
   }
-  v: 1.2266378130754303
-  a: -0.05305080629051398
+  v: 1.2328134986071646
+  a: -0.050107932137711135
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587257.62596985768
-    y: 4140943.8693731073
-    theta: -1.8176395059176476
-    kappa: -0.015309610134717724
-    s: 3.2946763767785976
-    dkappa: -0.0045822316640061513
+    x: 587257.624287618
+    y: 4140943.862829797
+    theta: -1.8177263014737592
+    kappa: -0.015338470045627713
+    s: 3.3014324732985258
+    dkappa: -0.0045791681724420248
     ddkappa: 0
   }
-  v: 1.221563169512957
-  a: -0.04854232315124505
+  v: 1.2280292300095828
+  a: -0.045678500811205953
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 587257.59561219276
-    y: 4140943.751292658
-    theta: -1.8192058168228447
-    kappa: -0.015830415521610269
-    s: 3.416596762219799
-    dkappa: -0.0045269479552562074
+    x: 587257.59376541956
+    y: 4140943.7441093707
+    theta: -1.8193011015205904
+    kappa: -0.0158620981101115
+    s: 3.4240136467203475
+    dkappa: -0.0045235848352900392
     ddkappa: 0
   }
-  v: 1.2169099898469191
-  a: -0.04461459795984437
+  v: 1.223658302252745
+  a: -0.041833955250053
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587257.56536572811
-    y: 4140943.6336447373
-    theta: -1.8207663903193683
-    kappa: -0.016349313194437796
-    s: 3.5380705523684552
-    dkappa: -0.0044718667511488858
+    x: 587257.56334749842
+    y: 4140943.6257945462
+    theta: -1.8208705213584127
+    kappa: -0.016383937231017658
+    s: 3.546176028985228
+    dkappa: -0.0044681913952439685
     ddkappa: 0
   }
-  v: 1.2126222774031168
-  a: -0.041226048304582423
+  v: 1.2196443747819494
+  a: -0.038531335017586571
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 587257.5352220179
-    y: 4140943.5163964955
-    theta: -1.822321662179287
-    kappa: -0.016866448049363745
-    s: 3.6591316673731278
-    dkappa: -0.0044169726708960851
+    x: 587257.53302562214
+    y: 4140943.5078533022
+    theta: -1.8224349857346969
+    kappa: -0.016904128638781428
+    s: 3.6679526820958559
+    dkappa: -0.0044129728606342854
     ddkappa: 0
   }
-  v: 1.2086481937485223
-  a: -0.038335091773729686
+  v: 1.2159354030861604
+  a: -0.035727679677141015
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 587257.50517385511
-    y: 4140943.399519898
-    theta: -1.823872004248893
-    kappa: -0.017381943726944574
-    s: 3.7798090514511369
-    dkappa: -0.004362252590001235
+    x: 587257.502792801
+    y: 4140943.39025845
+    theta: -1.8239948552949057
+    kappa: -0.017422792249633494
+    s: 3.789371678406
+    dkappa: -0.0043579165023031529
     ddkappa: 0
   }
-  v: 1.2049400586912815
-  a: -0.035900145955556642
+  v: 1.2124836386980093
+  a: -0.033380028792050619
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 587257.47521516809
-    y: 4140943.28299133
-    theta: -1.8254177297907923
-    kappa: -0.01789590438839906
-    s: 3.9001270887126722
-    dkappa: -0.0043076954517075779
+    x: 587257.47264318052
+    y: 4140943.2729872195
+    theta: -1.8255504321016207
+    kappa: -0.017940028500734159
+    s: 3.9104565302248742
+    dkappa: -0.0043030116588041708
     ddkappa: 0
   }
-  v: 1.2014543502807127
-  a: -0.033879628438333746
+  v: 1.2092456291937939
+  a: -0.031445421925649725
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 587257.4446286048
-    y: 4140943.166981237
-    theta: -1.8273031296662248
-    kappa: -0.018386962952430606
-    s: 4.0201060189849169
-    dkappa: -0.0042500832136284708
+    x: 587257.44166432857
+    y: 4140943.1562629906
+    theta: -1.8275403182317291
+    kappa: -0.018428584772454175
+    s: 4.0312266194215072
+    dkappa: -0.0042441609088240007
     ddkappa: 0
   }
-  v: 1.1981517048073074
-  a: -0.032231956810331489
+  v: 1.2061822181934785
+  a: -0.029880898641272614
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587257.41273334611
-    y: 4140943.0516541763
-    theta: -1.8298552503481305
-    kappa: -0.0188348087728008
-    s: 4.1397623536361605
-    dkappa: -0.0041863599201218847
+    x: 587257.409551913
+    y: 4140943.0401507323
+    theta: -1.8301098148737707
+    kappa: -0.018879479724151479
+    s: 4.1516976270291055
+    dkappa: -0.0041800037591069525
     ddkappa: 0
   }
-  v: 1.19499691680273
-  a: -0.030915548659820345
+  v: 1.2032585453606941
+  a: -0.028643498502253666
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587257.38092055963
-    y: 4140942.9366253186
-    theta: -1.8324007719795323
-    kappa: -0.019281496592607968
-    s: 4.259109291399918
-    dkappa: -0.0041228013967357343
+    x: 587257.37751591194
+    y: 4140942.9243147736
+    theta: -1.8326731971634098
+    kappa: -0.019329301730048425
+    s: 4.2718819628494256
+    dkappa: -0.0041159992771988754
     ddkappa: 0
   }
-  v: 1.191958939039818
-  a: -0.029888821575070786
+  v: 1.2004440464027382
+  a: -0.027690261071927152
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587257.34918749891
-    y: 4140942.8218847346
-    theta: -1.8349399142876222
-    kappa: -0.019727064969552427
-    s: 4.3781571341990455
-    dkappa: -0.004059402157154098
+    x: 587257.34555377485
+    y: 4140942.8087458932
+    theta: -1.8352306691779028
+    kappa: -0.019778086601596565
+    s: 4.3917891950571315
+    dkappa: -0.0040521423675433646
     ddkappa: 0
   }
-  v: 1.1890108825325814
-  a: -0.029110193144353286
+  v: 1.1977124530705745
+  a: -0.026978225913627411
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587257.31753207953
-    y: 4140942.7074248856
-    theta: -1.8374728440832366
-    kappa: -0.020171543175579131
-    s: 4.4969137029698611
-    dkappa: -0.0039961580363169144
+    x: 587257.31366359419
+    y: 4140942.6934371935
+    theta: -1.8377823835490632
+    kappa: -0.020225861122607637
+    s: 4.5114264798041717
+    dkappa: -0.0039884292191130124
     ddkappa: 0
   }
-  v: 1.1861300165362039
-  a: -0.028538080955938322
+  v: 1.195041793158834
+  a: -0.026464432590688758
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 587257.28595276689
-    y: 4140942.5932402238
-    theta: -1.8399996841298689
-    kappa: -0.020614952753210583
-    s: 4.61538475348626
-    dkappa: -0.0039330659689717652
+    x: 587257.2818439909
+    y: 4140942.5783836879
+    theta: -1.840328450626189
+    kappa: -0.020672644657162717
+    s: 4.6307989908241405
+    dkappa: -0.00392485707662248
     ddkappa: 0
   }
-  v: 1.1832977685470414
-  a: -0.028130902598096228
+  v: 1.1924143905058138
+  a: -0.026105920666445376
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587257.254448416
-    y: 4140942.4793266095
-    theta: -1.8425205260794242
-    kappa: -0.021057309785511721
-    s: 4.7335745828534348
-    dkappa: -0.0038701236666840561
+    x: 587257.25009395159
+    y: 4140942.463581712
+    theta: -1.8428689514836596
+    kappa: -0.021118451432183827
+    s: 4.7499105292942838
+    dkappa: -0.0038614239157449
     ddkappa: 0
   }
-  v: 1.1805072621955839
-  a: -0.027625383438409389
+  v: 1.1898239874885526
+  a: -0.025650445380598203
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587257.22301763785
-    y: 4140942.3656790196
-    theta: -1.8450354810419229
-    kappa: -0.021498633770055162
-    s: 4.8514884004715135
-    dkappa: -0.0038073283551741462
+    x: 587257.21841222362
+    y: 4140942.3490267363
+    theta: -1.84540398637862
+    kappa: -0.021563299040782379
+    s: 4.8687657957770076
+    dkappa: -0.0037981272331834865
     ddkappa: 0
   }
-  v: 1.1777823752708283
-  a: -0.026829361934656258
+  v: 1.1872936000851573
+  a: -0.024916010430854521
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587257.19165832526
-    y: 4140942.2522898363
-    theta: -1.8475447176419859
-    kappa: -0.021938954297068419
-    s: 4.969134112316179
-    dkappa: -0.0037446758240940385
+    x: 587257.18649265391
+    y: 4140942.2347994135
+    theta: -1.84798622118684
+    kappa: -0.02199770954655229
+    s: 4.9873720826116008
+    dkappa: -0.00373319458666987
     ddkappa: 0
   }
-  v: 1.1751489036460028
-  a: -0.02580792531040197
+  v: 1.1848480036495002
+  a: -0.023965028067558663
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587257.158005308
-    y: 4140942.1398301907
-    theta: -1.8504562435557586
-    kappa: -0.022304492560094827
-    s: 5.0865218785143211
-    dkappa: -0.00366842618121146
+    x: 587257.15248396085
+    y: 4140942.1214234903
+    theta: -1.8509349772041805
+    kappa: -0.022363952487743761
+    s: 5.1057388476114882
+    dkappa: -0.0036558729284807312
     ddkappa: 0
   }
-  v: 1.1726261344719791
-  a: -0.024626160789211655
+  v: 1.1825057323104191
+  a: -0.022859910541054983
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 587257.12434862938
-    y: 4140942.0276277903
-    theta: -1.8533744778301831
-    kappa: -0.022666944623767046
-    s: 5.2036634624717983
-    dkappa: -0.0035919048577259111
+    x: 587257.118540926
+    y: 4140942.008266455
+    theta: -1.8538780402586592
+    kappa: -0.022729488348576472
+    s: 5.2238770899417251
+    dkappa: -0.0035787005499627166
     ddkappa: 0
   }
-  v: 1.1702268461772727
-  a: -0.02334915559465045
+  v: 1.1802790789717179
+  a: -0.021663070101687824
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587257.09075902938
-    y: 4140941.9156490127
-    theta: -1.8562868959827035
-    kappa: -0.02302867431041658
-    s: 5.3205715800012046
-    dkappa: -0.003515536043361904
+    x: 587257.08466012578
+    y: 4140941.8953168928
+    theta: -1.856815707211807
+    kappa: -0.023094354000001947
+    s: 5.3417987259964965
+    dkappa: -0.0035016696668753924
     ddkappa: 0
   }
-  v: 1.1679573084680426
-  a: -0.022041996950283489
+  v: 1.1781740953121655
+  a: -0.020436918999801532
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587257.057232768
-    y: 4140941.8038813886
-    theta: -1.859193822301819
-    kappa: -0.023389721897493637
-    s: 5.43725924844963
-    dkappa: -0.0034393112346948322
+    x: 587257.05083805206
+    y: 4140941.782563109
+    theta: -1.8597482822270344
+    kappa: -0.023458587219883656
+    s: 5.4595159652766148
+    dkappa: -0.0034247723035099658
     ddkappa: 0
   }
-  v: 1.1658172823280908
-  a: -0.020769772079675897
+  v: 1.1761905917854969
+  a: -0.019243869485740441
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 587257.02376620565
-    y: 4140941.692312785
-    theta: -1.8620955723733177
-    kappa: -0.02375012658154976
-    s: 5.5537391358264268
-    dkappa: -0.0033632221565007407
+    x: 587257.017071292
+    y: 4140941.6699937261
+    theta: -1.8626760612214763
+    kappa: -0.023822224761876717
+    s: 5.57704068626701
+    dkappa: -0.0033480007003897669
     ddkappa: 0
   }
-  v: 1.1638000200188634
-  a: -0.019597568206392812
+  v: 1.1743221376204118
+  a: -0.018146333809848904
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587256.99035598966
-    y: 4140941.5809320253
-    theta: -1.8649924368657298
-    kappa: -0.024109924464350181
-    s: 5.6700229099309754
-    dkappa: -0.0032872611869307483
+    x: 587256.983356707
+    y: 4140941.5575982817
+    theta: -1.8655993163178339
+    kappa: -0.024185300424307613
+    s: 5.6943838123142321
+    dkappa: -0.0032713477219707236
     ddkappa: 0
   }
-  v: 1.1618922650794492
-  a: -0.018590472553999364
+  v: 1.1725560608205765
+  a: -0.017206724222471263
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587256.95699924231
-    y: 4140941.4697295157
-    theta: -1.8678846653157817
-    kappa: -0.024469146538986238
-    s: 5.7861205874804478
-    dkappa: -0.0032114217826854623
+    x: 587256.94969161251
+    y: 4140941.4453678248
+    theta: -1.86851828029622
+    kappa: -0.024547843119053909
+    s: 5.8115546875039481
+    dkappa: -0.0031948072643418436
     ddkappa: 0
   }
-  v: 1.1600742523265815
-  a: -0.017813572346060683
+  v: 1.1708734481646221
+  a: -0.016487452973951863
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587256.92369374691
-    y: 4140941.3586978666
-    theta: -1.8707724499138487
-    kappa: -0.024827816675987723
-    s: 5.9020398832375687
-    dkappa: -0.0031356989041894058
+    x: 587256.91607395688
+    y: 4140941.3332955162
+    theta: -1.8714331310460002
+    kappa: -0.024909874940423942
+    s: 5.9285604525384326
+    dkappa: -0.0031183746629256971
     ddkappa: 0
   }
-  v: 1.1583197078546363
-  a: -0.017331954806141907
+  v: 1.169249145206146
+  a: -0.016050932314635041
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587256.88921335014
-    y: 4140941.2482164078
-    theta: -1.8738053017351135
-    kappa: -0.025146551953941804
-    s: 6.0177855591383853
-    dkappa: -0.0030544472685276589
+    x: 587256.88062867511
+    y: 4140941.2219645493
+    theta: -1.8745725347880207
+    kappa: -0.025211133899142053
+    s: 6.0454054206140722
+    dkappa: -0.0030334150174596516
     ddkappa: 0
   }
-  v: 1.1565958490356336
-  a: -0.017210707157808154
+  v: 1.1676517562737103
+  a: -0.015959574494865143
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 587256.853291429
-    y: 4140941.1383674964
-    theta: -1.8770157302199069
-    kappa: -0.025416790202526938
-    s: 6.1333587734200279
-    dkappa: -0.0029664394100650847
+    x: 587256.84436118382
+    y: 4140941.1110588871
+    theta: -1.8778138477082218
+    kappa: -0.0254839718492227
+    s: 6.1620904532988545
+    dkappa: -0.0029445605202904545
     ddkappa: 0
   }
-  v: 1.1548633845192364
-  a: -0.01751491662462458
+  v: 1.1660436444708433
+  a: -0.016275791764986516
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 587256.817424074
-    y: 4140941.0286854468
-    theta: -1.8802212820015891
-    kappa: -0.025686617953973173
-    s: 6.248756429748477
-    dkappa: -0.002878565237245786
+    x: 587256.80814440211
+    y: 4140941.0003082938
+    theta: -1.8810506286091127
+    kappa: -0.025756428315969634
+    s: 6.27861233640987
+    dkappa: -0.0028558302599064477
     ddkappa: 0
   }
-  v: 1.1530765142327517
-  a: -0.018309670430156296
+  v: 1.1643809316760387
+  a: -0.017061996375343502
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587256.78161377227
-    y: 4140940.9191778661
-    theta: -1.8834217348043127
-    kappa: -0.025956016498181224
-    s: 6.36397052634632
-    dkappa: -0.002790830843344042
+    x: 587256.77198078926
+    y: 4140940.8897202909
+    theta: -1.8842826576515894
+    kappa: -0.026028484794396806
+    s: 6.3949631558908058
+    dkappa: -0.0027672302627831588
     ddkappa: 0
   }
-  v: 1.1511829293811291
-  a: -0.019660055797968444
+  v: 1.1626134985427554
+  a: -0.018380600576280444
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 587256.74586473755
-    y: 4140940.8098576395
-    theta: -1.8866167120071771
-    kappa: -0.02622495413303623
-    s: 6.47898750512053
-    dkappa: -0.002703246552713463
+    x: 587256.73587446043
+    y: 4140940.7793074618
+    theta: -1.887509567104944
+    kappa: -0.026300110330722527
+    s: 6.511129673689446
+    dkappa: -0.0026787706095662916
     ddkappa: 0
   }
-  v: 1.1491238124469632
-  a: -0.021631159951626143
+  v: 1.1606849844994183
+  a: -0.020294016618141685
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 587256.71018311358
-    y: 4140940.7007435542
-    theta: -1.8898056645640975
-    kappa: -0.026493384642510338
-    s: 6.5937876007902165
-    dkappa: -0.0026158274164197644
+    x: 587256.69983137865
+    y: 4140940.6690880414
+    theta: -1.890730824009796
+    kappa: -0.026571260063019388
+    s: 6.6270927036351619
+    dkappa: -0.0025904659103348293
     ddkappa: 0
   }
-  v: 1.1468338371904905
-  a: -0.02428807011469454
+  v: 1.158532787749418
+  a: -0.022864656751271573
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 587256.674577175
-    y: 4140940.5918609141
-    theta: -1.8929878529236759
-    kappa: -0.026761245774765317
-    s: 6.7083441900143939
-    dkappa: -0.0025285937078735271
+    x: 587256.66385955014
+    y: 4140940.5590865132
+    theta: -1.8939457128410242
+    kappa: -0.02684187376186431
+    s: 6.742826487316413
+    dkappa: -0.0025023357798641429
     ddkappa: 0
   }
-  v: 1.1442411686495917
-  a: -0.027695873510738774
+  v: 1.1560880652711103
+  a: -0.026154933226014452
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 587256.63905753091
-    y: 4140940.4832421616
-    theta: -1.896162328949069
-    kappa: -0.027028457720255181
-    s: 6.8226231405197506
-    dkappa: -0.002441571418462957
+    x: 587256.62796921772
+    y: 4140940.4493341991
+    theta: -1.8971533181706997
+    kappa: -0.027111874370988484
+    s: 6.8582980699582441
+    dkappa: -0.0024144053128890984
     ddkappa: 0
   }
-  v: 1.1412674631397912
-  a: -0.031919657363323951
+  v: 1.1532757328178167
+  a: -0.030227258292714658
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 587256.60363732616
-    y: 4140940.3749274937
-    theta: -1.8993279178378597
-    kappa: -0.027294921589828786
-    s: 6.936582160228407
-    dkappa: -0.0023547927531866542
+    x: 587256.591778946
+    y: 4140940.3400047584
+    theta: -1.9002847944423977
+    kappa: -0.027368042993210072
+    s: 6.97346667629978
+    dkappa: -0.0023250540142236856
     ddkappa: 0
   }
-  v: 1.1378278682542566
-  a: -0.037024508896015211
+  v: 1.1500144649178241
+  a: -0.035144044201716539
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 587256.56594312075
-    y: 4140940.2677833457
-    theta: -1.9020726841493585
-    kappa: -0.027480955081552218
-    s: 7.0501701463856836
-    dkappa: -0.0022582839751867229
+    x: 587256.55310564069
+    y: 4140940.2318974803
+    theta: -1.9029610629046552
+    kappa: -0.027537059605286983
+    s: 7.0882830864717219
+    dkappa: -0.0022251068256456631
     ddkappa: 0
   }
-  v: 1.1338310228637991
-  a: -0.043075515332377717
+  v: 1.1462166948743853
+  a: -0.040967703203364444
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 587256.52782895626
-    y: 4140940.1612390885
-    theta: -1.9047102589774041
-    kappa: -0.02764752804230803
-    s: 7.1633265346878634
-    dkappa: -0.0021597818284682419
+    x: 587256.514570598
+    y: 4140940.1241767015
+    theta: -1.9056277633318026
+    kappa: -0.027705471959270471
+    s: 7.2026890118738427
+    dkappa: -0.0021255169622847962
     ddkappa: 0
   }
-  v: 1.1291790571168734
-  a: -0.050137763895976566
+  v: 1.1417886147657181
+  a: -0.0477606475480027
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 587256.48988397175
-    y: 4140940.055167757
-    theta: -1.9073361262314537
-    kappa: -0.027813361624888448
-    s: 7.2759806484099592
-    dkappa: -0.0020617169095998433
+    x: 587256.47619671584
+    y: 4140940.01690643
+    theta: -1.9082833111365836
+    kappa: -0.027873179982401221
+    s: 7.31661647105249
+    dkappa: -0.0020263436017025314
     ddkappa: 0
   }
-  v: 1.1237675924395771
-  a: -0.058276341810376886
+  v: 1.1366301754450068
+  a: -0.055585289485975672
   relative_time: 5.7999999999999963
 }
 trajectory_point {
   path_point {
-    x: 587256.452135598
-    y: 4140939.9496460306
-    theta: -1.9099483876170664
-    kappa: -0.027978335944723022
-    s: 7.3880510475334713
-    dkappa: -0.0019641601117654762
+    x: 587256.43801036733
+    y: 4140939.9101603897
+    theta: -1.9109258812522028
+    kappa: -0.028040068414715263
+    s: 7.4299871655780754
+    dkappa: -0.0019276549023432561
     ddkappa: 0
   }
-  v: 1.1174857415356525
-  a: -0.067556336299143857
+  v: 1.1306350865404005
+  a: -0.06450404126762771
   relative_time: 5.8999999999999959
 }
 trajectory_point {
   path_point {
-    x: 587256.41461511131
-    y: 4140939.844761339
-    theta: -1.9125448787857451
-    kappa: -0.028142314314907727
-    s: 7.4994448778741614
-    dkappa: -0.0018671922641310102
+    x: 587256.40004161058
+    y: 4140939.8040226051
+    theta: -1.9135533935845856
+    kappa: -0.028206005890298362
+    s: 7.5427118559225717
+    dkappa: -0.0018295285468302373
     ddkappa: 0
   }
-  v: 1.1102161083864839
-  a: -0.078042834585842547
+  v: 1.1236908164550139
+  a: -0.074579315143303113
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 587256.37735785113
-    y: 4140939.7406124705
-    theta: -1.9151231541636811
-    kappa: -0.028305142288082168
-    s: 7.6100572202098071
-    dkappa: -0.0017709046984257103
+    x: 587256.36232439941
+    y: 4140939.6985879904
+    theta: -1.9161634984646378
+    kappa: -0.02837084401854046
+    s: 7.6546897373370149
+    dkappa: -0.00173205228526156
     ddkappa: 0
   }
-  v: 1.1018347882511008
-  a: -0.089800923894038112
+  v: 1.1156785923669275
+  a: -0.085873523363346277
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 587256.34040344018
-    y: 4140939.6373101864
-    theta: -1.9176804717805014
-    kappa: -0.028466646698306775
-    s: 7.7197704394079762
-    dkappa: -0.0016753998155236928
+    x: 587256.32489679358
+    y: 4140939.5939629381
+    theta: -1.9187535621005045
+    kappa: -0.028534416465390035
+    s: 7.765807815728996
+    dkappa: -0.0016353244785060686
     ddkappa: 0
   }
-  v: 1.0922113676661744
-  a: -0.10289569144729573
+  v: 1.1064734002291874
+  a: -0.098449078178101487
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
-    x: 587256.30379600346
-    y: 4140939.5349778328
-    theta: -1.9202137780980164
-    kappa: -0.02862663470294001
-    s: 7.8284535335537813
-    dkappa: -0.0015807916520254035
+    x: 587256.287801168
+    y: 4140939.4902659031
+    theta: -1.9213206520298292
+    kappa: -0.028696538034608553
+    s: 7.8759402835401575
+    dkappa: -0.0015394546414993109
     ddkappa: 0
   }
-  v: 1.0812089244460208
-  a: -0.11739222446918046
+  v: 1.0959439847698054
+  a: -0.11236839183791314
   relative_time: 6.2999999999999945
 }
 trajectory_point {
   path_point {
-    x: 587256.26758438756
-    y: 4140939.4337519566
-    theta: -1.9227196928389652
-    kappa: -0.028784892824515566
-    s: 7.9359614830776506
-    dkappa: -0.0014872064468390764
+    x: 587256.250242518
+    y: 4140939.3879427528
+    theta: -1.924086023251415
+    kappa: -0.028826461075777608
+    s: 7.9849478956236943
+    dkappa: -0.0014412702960552384
     ddkappa: 0
   }
-  v: 1.0686840276825986
-  a: -0.13335561018325751
+  v: 1.0839528494917583
+  a: -0.12769387659312559
   relative_time: 6.3999999999999941
 }
 trajectory_point {
   path_point {
-    x: 587256.229456208
-    y: 4140939.3346675383
-    theta: -1.9258254510298396
-    kappa: -0.02885534607412208
-    s: 8.0421345998830915
-    dkappa: -0.0013855263191973439
+    x: 587256.21108485409
+    y: 4140939.2875818457
+    theta: -1.9273627918541572
+    kappa: -0.02888087520945963
+    s: 8.0926773451218459
+    dkappa: -0.001336258685095828
     ddkappa: 0
   }
-  v: 1.0544867377455107
-  a: -0.15085093581309192
+  v: 1.0703562566729889
+  a: -0.1444879446940831
   relative_time: 6.4999999999999938
 }
 trajectory_point {
   path_point {
-    x: 587256.1914130412
-    y: 4140939.2371630822
-    theta: -1.9290089569346069
-    kappa: -0.028908211484189351
-    s: 8.1467978764744515
-    dkappa: -0.0012835035265965366
+    x: 587256.172452841
+    y: 4140939.1885681786
+    theta: -1.9305955732628017
+    kappa: -0.028934558890262588
+    s: 8.198960639343392
+    dkappa: -0.0012326567452709239
     ddkappa: 0
   }
-  v: 1.038460606282003
-  a: -0.16994328858224889
+  v: 1.0550042273664055
+  a: -0.16281300839113014
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
-    x: 587256.15398809034
-    y: 4140939.1412431104
-    theta: -1.9321407296597701
-    kappa: -0.028960217811272115
-    s: 8.2497603350846891
-    dkappa: -0.0011831386431095177
+    x: 587256.13441310544
+    y: 4140939.0910725174
+    theta: -1.9337787920254941
+    kappa: -0.028987419532038552
+    s: 8.3036144756411563
+    dkappa: -0.0011306431548004814
     ddkappa: 0
   }
-  v: 1.0204426762169647
-  a: -0.19069775571429354
+  v: 1.0377405413998819
+  a: -0.18273147993461092
   relative_time: 6.6999999999999931
 }
 trajectory_point {
   path_point {
-    x: 587256.1172568138
-    y: 4140939.0471010236
-    theta: -1.9352144547426113
-    kappa: -0.029011260197182302
-    s: 8.3508143768031253
-    dkappa: -0.0010846340302645915
+    x: 587256.09703806671
+    y: 4140938.9952804702
+    theta: -1.9369063880291435
+    kappa: -0.029039356500338211
+    s: 8.4064396172894913
+    dkappa: -0.0010304121239920812
     ddkappa: 0
   }
-  v: 1.0002634817529295
-  a: -0.21317942443279098
+  v: 1.0184027373762576
+  a: -0.20430577157486979
   relative_time: 6.7999999999999927
 }
 trajectory_point {
   path_point {
-    x: 587256.08130094782
-    y: 4140938.9549463093
-    theta: -1.9382232923636091
-    kappa: -0.0290612250596384
-    s: 8.4497351307032247
-    dkappa: -0.0009882088858610917
+    x: 587256.0604061624
+    y: 4140938.9013930731
+    theta: -1.9399717975157116
+    kappa: -0.029090260797166707
+    s: 8.5072202693617918
+    dkappa: -0.00093217400361782189
     ddkappa: 0
   }
-  v: 0.97774704837007287
-  a: -0.23745338196130647
+  v: 0.99682211267333731
+  a: -0.22759829556225122
   relative_time: 6.8999999999999924
 }
 trajectory_point {
   path_point {
-    x: 587256.0462087437
-    y: 4140938.865005157
-    theta: -1.9411598575490869
-    kappa: -0.029109989763510029
-    s: 8.5462798029703535
-    dkappa: -0.00089409987842117649
+    x: 587256.02460207487
+    y: 4140938.8096273672
+    theta: -1.9429679340984971
+    kappa: -0.029140014745739351
+    s: 8.60572345460797
+    dkappa: -0.00083615589329124956
     ddkappa: 0
   }
-  v: 0.95271089282621535
-  a: -0.2635847155234049
+  v: 0.97282372344389179
+  a: -0.25267146414709946
   relative_time: 6.999999999999992
 }
 decision {
@@ -1589,8 +1589,8 @@ decision {
         follow {
           distance_s: -4.2777776718139648
           fence_point {
-            x: 587254.73635642952
-            y: 4140935.0919172089
+            x: 587254.69007334835
+            y: 4140935.0032725451
             z: 0
           }
           fence_heading: -2.0476944556176004
diff --git a/modules/planning/toolkits/optimizers/road_graph/trajectory_cost.cc b/modules/planning/toolkits/optimizers/road_graph/trajectory_cost.cc
index 5ad24bca80dbaf7bb23e5db706d46691802f22b7..e41b500293d0f6a2973eaecf1f63d2a9b2dcfab3 100644
--- a/modules/planning/toolkits/optimizers/road_graph/trajectory_cost.cc
+++ b/modules/planning/toolkits/optimizers/road_graph/trajectory_cost.cc
@@ -118,27 +118,17 @@ ComparableCost TrajectoryCost::CalculatePathCost(
     return (b + std::exp(-k * (x - l0))) / (1.0 + std::exp(-k * (x - l0)));
   };
 
-  const auto &vehicle_config =
-      common::VehicleConfigHelper::Instance()->GetConfig();
-  const double width = vehicle_config.vehicle_param().width();
-
   for (double curve_s = 0.0; curve_s < (end_s - start_s);
        curve_s += config_.path_resolution()) {
     const double l = curve.Evaluate(0, curve_s);
 
     path_cost += l * l * config_.path_l_cost() * quasi_softmax(std::fabs(l));
 
-    double left_width = 0.0;
-    double right_width = 0.0;
-    reference_line_->GetLaneWidth(curve_s + start_s, &left_width, &right_width);
-
-    constexpr double kBuff = 0.2;
-    if (!is_change_lane_path_ && (l + width / 2.0 + kBuff > left_width ||
-                                  l - width / 2.0 - kBuff < -right_width)) {
+    const double dl = std::fabs(curve.Evaluate(1, curve_s));
+    if (!is_change_lane_path_ && IsOffRoad(curve_s + start_s, l, dl)) {
       cost.cost_items[ComparableCost::OUT_OF_BOUNDARY] = true;
     }
 
-    const double dl = std::fabs(curve.Evaluate(1, curve_s));
     path_cost += dl * dl * config_.path_dl_cost();
 
     const double ddl = std::fabs(curve.Evaluate(2, curve_s));
@@ -155,6 +145,33 @@ ComparableCost TrajectoryCost::CalculatePathCost(
   return cost;
 }
 
+bool TrajectoryCost::IsOffRoad(const double ref_s, const double l,
+                               const double dl) {
+  Vec2d position(ref_s, l);
+
+  const auto &param = common::VehicleConfigHelper::GetConfig().vehicle_param();
+  Vec2d vec_to_center(
+      (param.front_edge_to_center() - param.back_edge_to_center()) / 2.0,
+      (param.left_edge_to_center() - param.right_edge_to_center()) / 2.0);
+
+  Vec2d center(position + vec_to_center.rotate(std::atan(dl)));
+
+  const double buffer = 0.1;  // in meters
+  const auto ego_box = Box2d(center, std::atan(dl), param.length() + buffer,
+                             param.width() + buffer);
+
+  double left_width = 0.0;
+  double right_width = 0.0;
+  reference_line_->GetLaneWidth(ref_s, &left_width, &right_width);
+
+  for (const auto &corner : ego_box.GetAllCorners()) {
+    if (corner.y() > left_width || corner.y() < -right_width) {
+      return true;
+    }
+  }
+  return false;
+}
+
 ComparableCost TrajectoryCost::CalculateStaticObstacleCost(
     const QuinticPolynomialCurve1d &curve, const double start_s,
     const double end_s) {
diff --git a/modules/planning/toolkits/optimizers/road_graph/trajectory_cost.h b/modules/planning/toolkits/optimizers/road_graph/trajectory_cost.h
index 5048874e362097b72145bdb7bfcc13f4e9fcfccf..965e104320c6ae29f8a777fd921b4133e2f1bb9b 100644
--- a/modules/planning/toolkits/optimizers/road_graph/trajectory_cost.h
+++ b/modules/planning/toolkits/optimizers/road_graph/trajectory_cost.h
@@ -73,6 +73,8 @@ class TrajectoryCost {
   common::math::Box2d GetBoxFromSLPoint(const common::SLPoint &sl,
                                         const double dl) const;
 
+  bool IsOffRoad(const double ref_s, const double l, const double dl);
+
   const DpPolyPathConfig config_;
   const ReferenceLine *reference_line_ = nullptr;
   bool is_change_lane_path_ = false;