diff --git a/modules/planning/conf/planning_config.pb.txt b/modules/planning/conf/planning_config.pb.txt
index abcdbeda9872c737fb6f0d3708b28b08b9b8eb6d..2050aedd1e271274d3e2aa859d11a119179794bc 100644
--- a/modules/planning/conf/planning_config.pb.txt
+++ b/modules/planning/conf/planning_config.pb.txt
@@ -72,7 +72,7 @@ em_planner_config {
         max_spline_length: 20.0
         time_resolution: 0.1
         regularization_weight: 1.0
-        first_spline_weight_factor: 0.2
+        first_spline_weight_factor: 0.0
         derivative_weight: 500.0
         second_derivative_weight: 1000.0
         third_derivative_weight: 10.0
@@ -83,7 +83,7 @@ em_planner_config {
         history_path_weight: 0.0
         lane_change_mid_l: 0.0
         point_constraint_s_position: 110.0
-        lane_change_lateral_shift: 1.0
+        lane_change_lateral_shift: 1.3
     }
     qp_st_speed_config {
         total_path_length: 250.0
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 2b9e68b8f11253f8d633307e3e404a393f5cfa1d..66971f79a7e7327a38f16cc7bdebefcb20d92674 100644
--- a/modules/planning/testdata/garage_test/result_follow_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_follow_0.pb.txt
@@ -5,1136 +5,1136 @@ is_replan: true
 gear: GEAR_DRIVE
 trajectory_point {
   path_point {
-    x: 586344.823005
+    x: 586344.8230049999
     y: 4140700.15992
     theta: 1.3255010179499997
-    kappa: -0.026050217221752475
-    s: 9.2157184661267877e-16
-    dkappa: -2.4634021055154906e-17
+    kappa: -0.026050217221752478
+    s: 9.5381839091465e-16
+    dkappa: -2.5511517585253842e-17
     ddkappa: 0
   }
   v: 5.5999999046325595
-  a: 0.0092643384831259043
+  a: 0.00926433848301609
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 586344.96486972412
-    y: 4140700.7016887618
-    theta: 1.3123498936942251
-    kappa: -0.02287571716801988
-    s: 0.560034812697087
-    dkappa: -0.01496997702165913
+    x: 586344.964870778
+    y: 4140700.7016884862
+    theta: 1.3123462406305215
+    kappa: -0.022883803244222865
+    s: 0.56003481269708655
+    dkappa: -0.014979096763667384
     ddkappa: 0
   }
-  v: 5.600491030807575
-  a: -0.0030042338672741954
+  v: 5.6004910308075644
+  a: -0.00300423386736892
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 586345.11091717728
-    y: 4140701.242272215
-    theta: 1.3003096949450041
-    kappa: -0.019830345798251509
-    s: 1.1200230344046993
-    dkappa: -0.01533553548780001
+    x: 586345.1109218084
+    y: 4140701.2422709428
+    theta: 1.3002995725065598
+    kappa: -0.019846134276253789
+    s: 1.1200230344046973
+    dkappa: -0.015349055536875369
     ddkappa: 0
   }
-  v: 5.5987371118270666
-  a: -0.035131187178824935
+  v: 5.598737111827047
+  a: -0.035131187178912365
   relative_time: 0.2
 }
 trajectory_point {
   path_point {
-    x: 586345.26274426491
-    y: 4140701.7809059606
-    theta: 1.2898311496719039
-    kappa: -0.016967450651312749
-    s: 1.6796459224769653
-    dkappa: 0.004752541006316233
+    x: 586345.26275600353
+    y: 4140701.7809026847
+    theta: 1.2898106153382514
+    kappa: -0.016990400001706249
+    s: 1.6796459224769611
+    dkappa: 0.0047412249228800246
     ddkappa: 0
   }
-  v: 5.5929039228294748
-  a: -0.084084256760409121
+  v: 5.5929039228294473
+  a: -0.084084256760495552
   relative_time: 0.30000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586345.42089047912
-    y: 4140702.3168209288
-    theta: 1.2813778750910954
-    kappa: -0.014344869418508061
-    s: 2.2384164731053109
-    dkappa: -0.0082259427162446382
+    x: 586345.420918785
+    y: 4140702.3168127388
+    theta: 1.281342014348684
+    kappa: -0.014374199344731414
+    s: 2.2384164731053038
+    dkappa: -0.0082447862826344111
     ddkappa: 0
   }
-  v: 5.5814604654223539
-  a: -0.14683117792090947
+  v: 5.5814604654223174
+  a: -0.14683117792099967
   relative_time: 0.4
 }
 trajectory_point {
   path_point {
-    x: 586345.58136093616
-    y: 4140702.8505044724
-    theta: 1.2738736846986769
-    kappa: -0.011839508654330656
-    s: 2.7957097439653746
-    dkappa: -0.0171872042414219
+    x: 586345.58141152887
+    y: 4140702.8504895447
+    theta: 1.2738203103655277
+    kappa: -0.011874825751119445
+    s: 2.7957097439653631
+    dkappa: -0.017212086575500021
     ddkappa: 0
   }
-  v: 5.5631789676823669
-  a: -0.22033968596920875
+  v: 5.5631789676823207
+  a: -0.22033968596930603
   relative_time: 0.5
 }
 trajectory_point {
   path_point {
-    x: 586345.74564025644
-    y: 4140703.3807213358
-    theta: 1.2680316510559095
-    kappa: -0.009540340324166497
-    s: 3.3507931768639154
-    dkappa: 0.0074267983989009757
+    x: 586345.74572512321
+    y: 4140703.3806957882
+    theta: 1.2679569725206761
+    kappa: -0.0095809277189604965
+    s: 3.3507931768638985
+    dkappa: 0.00740657375981076
     ddkappa: 0
   }
-  v: 5.5371348841552894
-  a: -0.30157751621418982
+  v: 5.5371348841552335
+  a: -0.30157751621429596
   relative_time: 0.6
 }
 trajectory_point {
   path_point {
-    x: 586345.91164653085
-    y: 4140703.9072306426
-    theta: 1.2634521746828768
-    kappa: -0.0074088276321835726
-    s: 3.9028569203857266
-    dkappa: 7.96086740011883e-05
+    x: 586345.911778098
+    y: 4140703.9071903294
+    theta: 1.263353583822969
+    kappa: -0.00745408436734124
+    s: 3.9028569203857035
+    dkappa: 5.4666081834906996e-05
     ddkappa: 0
   }
-  v: 5.50270689585601
-  a: -0.38751240396473541
+  v: 5.5027068958559431
+  a: -0.38751240396485048
   relative_time: 0.7
 }
 trajectory_point {
   path_point {
-    x: 586346.0780112393
-    y: 4140704.4295637361
-    theta: 1.25963296226639
-    kappa: -0.0053844102636744643
-    s: 4.4510441525405442
-    dkappa: -0.023663120240253209
+    x: 586346.07819676306
+    y: 4140704.4295062311
+    theta: 1.2595090181390143
+    kappa: -0.0054339623995702316
+    s: 4.4510441525405149
+    dkappa: -0.023697822728041128
     ddkappa: 0
   }
-  v: 5.4595769102685274
-  a: -0.47511208452972825
+  v: 5.4595769102684484
+  a: -0.47511208452985121
   relative_time: 0.79999999999999993
 }
 trajectory_point {
   path_point {
-    x: 586346.24538104853
-    y: 4140704.9465853916
-    theta: 1.2574943204839633
-    kappa: -0.0035989303130010896
-    s: 4.9944814034099636
-    dkappa: 0.0063014077319010209
+    x: 586346.24564212293
+    y: 4140704.9465034236
+    theta: 1.2573419185851966
+    kappa: -0.003651897155527422
+    s: 4.9944814034099245
+    dkappa: 0.0062738327352640927
     ddkappa: 0
   }
-  v: 5.4077300613459531
-  a: -0.56134429321805113
+  v: 5.4077300613458616
+  a: -0.56134429321817914
   relative_time: 0.89999999999999991
 }
 trajectory_point {
   path_point {
-    x: 586346.41146758129
-    y: 4140705.45812552
-    theta: 1.25594045062622
-    kappa: -0.0019137379426819162
-    s: 5.5323088777943408
-    dkappa: 0.012537427942054237
+    x: 586346.41181293863
+    y: 4140705.4580161641
+    theta: 1.2557588675533484
+    kappa: -0.0019697592659696033
+    s: 5.5323088777942919
+    dkappa: 0.012510816159956504
     ddkappa: 0
   }
-  v: 5.34745470951051
-  a: -0.64317676533858692
+  v: 5.3474547095104059
+  a: -0.64317676533871571
   relative_time: 0.99999999999999989
 }
 trajectory_point {
   path_point {
-    x: 586346.57627927279
-    y: 4140705.9633235857
-    theta: 1.2553033431127854
-    kappa: -0.00037934209754287592
-    s: 6.0637107778597148
-    dkappa: -0.018683838183456498
+    x: 586346.57672312157
+    y: 4140705.9631820885
+    theta: 1.2550913430751722
+    kappa: -0.00043786837251971956
+    s: 6.0637107778596553
+    dkappa: -0.018720860004799316
     ddkappa: 0
   }
-  v: 5.2793424416535331
-  a: -0.71757723620021829
+  v: 5.2793424416534158
+  a: -0.71757723620034219
   relative_time: 1.0999999999999999
 }
 trajectory_point {
   path_point {
-    x: 586346.73884777038
-    y: 4140706.4617146561
-    theta: 1.2555686533009685
-    kappa: 0.000989967245590473
-    s: 6.5879456257847124
-    dkappa: -0.0019984545951063307
+    x: 586346.73940737429
+    y: 4140706.4615353867
+    theta: 1.2553253242839082
+    kappa: 0.0009295531064224537
+    s: 6.5879456257846405
+    dkappa: -0.0020332559690414247
     ddkappa: 0
   }
-  v: 5.2042880711354673
-  a: -0.7815134411118283
+  v: 5.2042880711353376
+  a: -0.78151344111193977
   relative_time: 1.2
 }
 trajectory_point {
   path_point {
-    x: 586346.898988032
-    y: 4140706.9526892318
-    theta: 1.2561899377867671
-    kappa: 0.0022807492121454257
-    s: 7.1043765864074588
-    dkappa: 0.033559946237746947
+    x: 586346.8996691542
+    y: 4140706.9524703096
+    theta: 1.2559152506713931
+    kappa: 0.0022187042399250071
+    s: 7.1043765864073736
+    dkappa: 0.033534328748393676
     ddkappa: 0
   }
-  v: 5.1234896377858714
-  a: -0.83195311538229921
+  v: 5.123489637785732
+  a: -0.83195311538238947
   relative_time: 1.3
 }
 trajectory_point {
   path_point {
-    x: 586347.05559782253
-    y: 4140707.4360778895
-    theta: 1.2578419016243108
-    kappa: 0.0033613432411468787
-    s: 7.6125017898724927
-    dkappa: -0.0044187493441426012
+    x: 586347.05642517249
+    y: 4140707.4358115681
+    theta: 1.2575352577633263
+    kappa: 0.0032984881323757627
+    s: 7.6125017898723932
+    dkappa: -0.00445218256397446
     ddkappa: 0
   }
-  v: 5.0384484079034157
-  a: -0.865863994320514
+  v: 5.0384484079032692
+  a: -0.86586399432057315
   relative_time: 1.4000000000000001
 }
 trajectory_point {
   path_point {
-    x: 586347.20912962244
-    y: 4140707.911378325
-    theta: 1.259670004790757
-    kappa: 0.0043802816641115251
-    s: 8.1119846542776752
-    dkappa: -0.02300266418838132
+    x: 586347.21010713838
+    y: 4140707.91106349
+    theta: 1.2593318500809327
+    kappa: 0.00431681760718847
+    s: 8.1119846542775615
+    dkappa: -0.02304618168792482
     ddkappa: 0
   }
-  v: 4.9509688742558815
-  a: -0.88021381323535552
+  v: 4.9509688742557305
+  a: -0.8802138132353714
   relative_time: 1.5000000000000002
 }
 trajectory_point {
   path_point {
-    x: 586347.35863701231
-    y: 4140708.3787471224
-    theta: 1.2621157689524651
-    kappa: 0.0052434424252786422
-    s: 8.6026842083211
-    dkappa: 0.019318395736815014
+    x: 586347.35978221579
+    y: 4140708.3783786125
+    theta: 1.2617464383897119
+    kappa: 0.0051799548944045405
+    s: 8.60268420832097
+    dkappa: 0.019285689949643404
     ddkappa: 0
   }
-  v: 4.8631587560801606
-  a: -0.87197030743570669
+  v: 4.8631587560800114
+  a: -0.87197030743566606
   relative_time: 1.6000000000000003
 }
 trajectory_point {
   path_point {
-    x: 586347.50452448823
-    y: 4140708.8381386185
-    theta: 1.2647990675586076
-    kappa: 0.0060195349382636907
-    s: 9.0846854139480051
-    dkappa: 0.02458612357808216
+    x: 586347.50584544591
+    y: 4140708.8377142926
+    theta: 1.2643991907335272
+    kappa: 0.0059563531454263083
+    s: 9.08468541394786
+    dkappa: 0.024562886503710149
     ddkappa: 0
   }
-  v: 4.77742899908226
-  a: -0.83810121223044964
+  v: 4.777428999082117
+  a: -0.83810121223033784
   relative_time: 1.7000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586347.64653810393
-    y: 4140709.2899912968
-    theta: 1.26782529603023
-    kappa: 0.0066826700932154214
-    s: 9.5583294889976838
-    dkappa: -0.020569509151629736
+    x: 586347.64804660226
+    y: 4140709.2895079856
+    theta: 1.2673956468990637
+    kappa: 0.0066202094441491265
+    s: 9.5583294889975257
+    dkappa: -0.020602899862109461
     ddkappa: 0
   }
-  v: 4.6964937754372951
-  a: -0.77557426292846776
+  v: 4.6964937754371681
+  a: -0.775574262928268
   relative_time: 1.8000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586347.7847644689
-    y: 4140709.7349272072
-    theta: 1.2710738407485267
-    kappa: 0.007247290302158556
-    s: 10.024244229850403
-    dkappa: -0.014166988663451945
+    x: 586347.78647108388
+    y: 4140709.7343823407
+    theta: 1.2706153634764232
+    kappa: 0.0071859546902238378
+    s: 10.024244229850234
+    dkappa: -0.014209945421223233
     ddkappa: 0
   }
-  v: 4.6233704837894942
-  a: -0.681357194838644
+  v: 4.6233704837893921
+  a: -0.68135719483833856
   relative_time: 1.9000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586347.9197223977
-    y: 4140710.1737743444
-    theta: 1.2745072180832104
-    kappa: 0.0077287732556547356
-    s: 10.483374334074304
-    dkappa: 0.0355582435027654
+    x: 586347.92163529631
+    y: 4140710.1731659863
+    theta: 1.2740208348976669
+    kappa: 0.0076688656023421037
+    s: 10.483374334074123
+    dkappa: 0.035527060924184339
     ddkappa: 0
   }
-  v: 4.5613797492521977
-  a: -0.55241774326985782
+  v: 4.561379749252132
+  a: -0.55241774326943083
   relative_time: 2.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586348.05122002866
-    y: 4140710.6079061152
-    theta: 1.2781112744059806
-    kappa: 0.00810991560778109
-    s: 10.936987031028259
-    dkappa: 0.02225533811987181
+    x: 586348.05335116945
+    y: 4140710.6072316347
+    theta: 1.277598217432462
+    kappa: 0.0080518807647622891
+    s: 10.936987031028073
+    dkappa: 0.022239237712090482
     ddkappa: 0
   }
-  v: 4.5131563656287685
-  a: -0.4154641777508663
+  v: 4.5131563656287517
+  a: -0.41546417775032862
   relative_time: 2.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586348.18029626878
-    y: 4140711.0384140015
-    theta: 1.2818219669740627
-    kappa: 0.0084252082059525727
-    s: 11.386428590397093
-    dkappa: -0.032089143601032023
+    x: 586348.18265238032
+    y: 4140711.0376720047
+    theta: 1.281283163473113
+    kappa: 0.0083692931586206824
+    s: 11.386428590396909
+    dkappa: -0.032114845300517836
     ddkappa: 0
   }
-  v: 4.4776270813975625
-  a: -0.29830558136571295
+  v: 4.4776270813976033
+  a: -0.2983055813651096
   relative_time: 2.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586348.30648982525
-    y: 4140711.4666475779
-    theta: 1.2856394350955902
-    kappa: 0.0086439455438358619
-    s: 11.832871749014958
-    dkappa: -0.014477395242467543
+    x: 586348.30908337119
+    y: 4140711.4658355815
+    theta: 1.2850763872529045
+    kappa: 0.0085906173197153567
+    s: 11.83287174901478
+    dkappa: -0.014520930979790091
     ddkappa: 0
   }
-  v: 4.4528814518631039
-  a: -0.19956090975622304
+  v: 4.4528814518632061
+  a: -0.19956090975559421
   relative_time: 2.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 586348.43089984171
-    y: 4140711.8933127336
-    theta: 1.289518839880341
-    kappa: 0.0088049102211386829
-    s: 12.277305104468244
-    dkappa: 0.046099848963799393
+    x: 586348.43373715179
+    y: 4140711.8924295832
+    theta: 1.2889325219775829
+    kappa: 0.0087543997944241857
+    s: 12.277305104468081
+    dkappa: 0.046069129259184188
     ddkappa: 0
   }
-  v: 4.4371471367657351
-  a: -0.1178491185642217
+  v: 4.4371471367659
+  a: -0.11784911856360269
   relative_time: 2.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586348.55289253162
-    y: 4140712.3194327313
-    theta: 1.2934362503662926
-    kappa: 0.0088738160987979452
-    s: 12.720546925539173
-    dkappa: 0.026562044376960879
+    x: 586348.55598567775
+    y: 4140712.3184762918
+    theta: 1.2928284666064267
+    kappa: 0.0088266015765220765
+    s: 12.720546925539029
+    dkappa: 0.026559762787817767
     ddkappa: 0
   }
-  v: 4.4287899002816165
-  a: -0.051789163431534169
+  v: 4.4287899002818412
+  a: -0.05178916343095552
   relative_time: 2.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586348.67348059441
-    y: 4140712.7454051096
-    theta: 1.2973782666106066
-    kappa: 0.0088872874403718664
-    s: 13.163258962649369
-    dkappa: -0.041251932424148055
+    x: 586348.67683573416
+    y: 4140712.7443744149
+    theta: 1.2967501237952368
+    kappa: 0.008843604412851121
+    s: 13.16325896264925
+    dkappa: -0.041263589803234163
     ddkappa: 0
   }
-  v: 4.4263136110227235
-  a: 1.4321877017664519e-14
+  v: 4.4263136110230032
+  a: 5.2713389209202433e-13
   relative_time: 2.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 586348.79199753306
-    y: 4140713.1719438266
-    theta: 1.3012880701173823
-    kappa: 0.0088126680541250144
-    s: 13.605960258303448
-    dkappa: -0.020673177902641161
+    x: 586348.79562570225
+    y: 4140713.1708372058
+    theta: 1.3006417098449086
+    kappa: 0.0087730006159829968
+    s: 13.605960258303359
+    dkappa: -0.0207171858418178
     ddkappa: 0
   }
-  v: 4.4283602420368533
-  a: 0.038899416088598704
+  v: 4.42836024203718
+  a: 0.038899416089024585
   relative_time: 2.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586348.9093159266
-    y: 4140713.5992106055
-    theta: 1.3051810818607241
-    kappa: 0.0086826091025841469
-    s: 14.049040957532588
-    dkappa: 0.05548784542274722
+    x: 586348.913222874
+    y: 4140713.5980273406
+    theta: 1.3045178130489776
+    kappa: 0.0086472043142529591
+    s: 14.049040957532535
+    dkappa: 0.055458292583573196
     ddkappa: 0
   }
-  v: 4.4337098708076175
-  a: 0.066290129192393632
+  v: 4.4337098708079816
+  a: 0.066290129192716929
   relative_time: 2.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586349.02481436427
-    y: 4140714.0276475782
-    theta: 1.3089710918164899
-    kappa: 0.0084678325453119169
-    s: 14.492776118338133
-    dkappa: 0.033267661677735351
+    x: 586349.02900955267
+    y: 4140714.0263865292
+    theta: 1.3082934391644172
+    kappa: 0.0084371786561901682
+    s: 14.492776118338117
+    dkappa: 0.0332843120955178
     ddkappa: 0
   }
-  v: 4.4412806792544455
-  a: 0.083553183669573983
+  v: 4.4412806792548363
+  a: 0.083553183669783593
   relative_time: 2.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586349.13923369709
-    y: 4140714.4572343989
-    theta: 1.312697581115035
-    kappa: 0.00819775264665542
-    s: 14.937339522135151
-    dkappa: -0.052969319309862359
+    x: 586349.14372199308
+    y: 4140714.4558951743
+    theta: 1.3120070904551249
+    kappa: 0.00817210592579295
+    s: 14.937339522135172
+    dkappa: -0.05296267879796325
     ddkappa: 0
   }
-  v: 4.4501289537325839
-  a: 0.092069623878314633
+  v: 4.4501289537329907
+  a: 0.092069623878404117
   relative_time: 3.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586349.25201790431
-    y: 4140714.8881960036
-    theta: 1.3162498993114282
-    kappa: 0.0078458209537078279
-    s: 15.382817484196025
-    dkappa: -0.028411856717909516
+    x: 586349.25680666
+    y: 4140714.886778051
+    theta: 1.3155495327439981
+    kappa: 0.0078256733230079121
+    s: 15.382817484196089
+    dkappa: -0.028454416821859153
     ddkappa: 0
   }
-  v: 4.459449085033099
-  a: 0.093220494176790014
+  v: 4.4594490850335085
+  a: 0.093220494176758373
   relative_time: 3.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 586349.3638175521
-    y: 4140715.320374697
-    theta: 1.3196911150571955
-    kappa: 0.0074401631041826153
-    s: 15.82922266409404
-    dkappa: 0.068534714599215452
+    x: 586349.36891024082
+    y: 4140715.3188780067
+    theta: 1.3189826509702427
+    kappa: 0.0074257726956878968
+    s: 15.829222664094145
+    dkappa: 0.068509648517373678
     ddkappa: 0
   }
-  v: 4.4685735683828725
-  a: 0.088386838923175448
+  v: 4.4685735683832721
+  a: 0.08838683892302579
   relative_time: 3.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586349.474121893
-    y: 4140715.7538433103
-    theta: 1.322882416917462
-    kappa: 0.0069548504275482965
-    s: 16.276507876146958
-    dkappa: 0.038641927840240557
+    x: 586349.47952317214
+    y: 4140715.7522679833
+    theta: 1.322169313060934
+    kappa: 0.0069467129050720814
+    s: 16.2765078761471
+    dkappa: 0.038680908851411379
     ddkappa: 0
   }
-  v: 4.4769730034446038
-  a: 0.0789497024756457
+  v: 4.4769730034449831
+  a: 0.078949702475386241
   relative_time: 3.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586349.58353792527
-    y: 4140716.1883506984
-    theta: 1.3259174219366214
-    kappa: 0.0064189387450286446
-    s: 16.724579899860604
-    dkappa: -0.071439887766547083
+    x: 586349.58924946678
+    y: 4140716.1866971245
+    theta: 1.3252016497182977
+    kappa: 0.0064173051173893741
+    s: 16.724579899860785
+    dkappa: -0.071413558527564966
     ddkappa: 0
   }
-  v: 4.4842560943168106
-  a: 0.066290129192375091
+  v: 4.4842560943171588
+  a: 0.066290129192019265
   relative_time: 3.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 586349.69160562567
-    y: 4140716.623875211
-    theta: 1.3286227251204716
-    kappa: 0.0058056139687945206
-    s: 17.173313290372441
-    dkappa: -0.034737050488182764
+    x: 586349.69762873347
+    y: 4140716.6221442078
+    theta: 1.3279082897013572
+    kappa: 0.0058109834184863849
+    s: 17.173313290372654
+    dkappa: -0.0347741264344044
     ddkappa: 0
   }
-  v: 4.4901696495338266
-  a: 0.051789163431538388
+  v: 4.4901696495341357
+  a: 0.0517891634311044
   relative_time: 3.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 586349.79890695959
-    y: 4140717.0601237565
-    theta: 1.3311339532386297
-    kappa: 0.00514636492104296
-    s: 17.622564188895161
-    dkappa: 0.088725956122223254
+    x: 586349.80524129351
+    y: 4140717.0583160855
+    theta: 1.3304229596834833
+    kappa: 0.0051589739410637632
+    s: 17.622564188895403
+    dkappa: 0.088712196561590087
     ddkappa: 0
   }
-  v: 4.4945985820658052
-  a: 0.036827849551311131
+  v: 4.4945985820660672
+  a: 0.036827849550821523
   relative_time: 3.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 586349.90505465725
-    y: 4140717.497033224
-    theta: 1.3332451825764433
-    kappa: 0.0044598458507542236
-    s: 18.07218413316026
-    dkappa: 0.040938724954451861
+    x: 586349.91169750656
+    y: 4140717.4951504762
+    theta: 1.3325420784276902
+    kappa: 0.0044797090745460439
+    s: 18.072184133160526
+    dkappa: 0.041000472428025195
     ddkappa: 0
   }
-  v: 4.4975659093187152
-  a: 0.022787231909867534
+  v: 4.4975659093189266
+  a: 0.022787231909350059
   relative_time: 3.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 586350.01058893825
-    y: 4140717.9343286608
-    theta: 1.3351413223890967
-    kappa: 0.0037586048618805752
-    s: 18.522033867861623
-    dkappa: -0.099003306858915743
+    x: 586350.017537969
+    y: 4140717.9323719088
+    theta: 1.3344482150786241
+    kappa: 0.0037857114179181473
+    s: 18.522033867861907
+    dkappa: -0.09896136971022354
     ddkappa: 0
   }
-  v: 4.4992327531343443
-  a: 0.011048354865382803
+  v: 4.4992327531345042
+  a: 0.011048354864869769
   relative_time: 3.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 586350.11522271321
-    y: 4140718.3719566939
-    theta: 1.3366293267062697
-    kappa: 0.0030846890024071334
-    s: 18.971997155099103
-    dkappa: -0.038619780770246019
+    x: 586350.12247091532
+    y: 4140718.3699282953
+    theta: 1.3359505217050032
+    kappa: 0.003118538556625818
+    s: 18.9719971550994
+    dkappa: -0.038647310107816329
     ddkappa: 0
   }
-  v: 4.4998983397902963
-  a: 0.0029922627760311515
+  v: 4.4998983397904064
+  a: 0.00299226277556075
   relative_time: 3.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 586350.21941803244
-    y: 4140718.8097249232
-    theta: 1.3379207099981496
-    kappa: 0.0024239635380589849
-    s: 19.421994584822095
-    dkappa: 0.11823663444592541
+    x: 586350.2269614304
+    y: 4140718.807626158
+    theta: 1.3372580266807599
+    kappa: 0.0024643317548983827
+    s: 19.421994584822407
+    dkappa: 0.11824133281214083
     ddkappa: 0
   }
-  v: 4.4999999999999956
-  a: -1.1082714607146826e-14
+  v: 4.5000000000000613
+  a: -3.8629212675833635e-13
   relative_time: 4.0000000000000018
 }
 trajectory_point {
   path_point {
-    x: 586350.32300518721
-    y: 4140719.2476399085
-    theta: 1.3388161688791536
-    kappa: 0.0017982879656638635
-    s: 19.871994584822094
-    dkappa: 0.040890724938082046
+    x: 586350.33083265973
+    y: 4140719.2454738412
+    theta: 1.3381735892454953
+    kappa: 0.0018446513932571323
+    s: 19.87199458482241
+    dkappa: 0.040967915124464058
     ddkappa: 0
   }
-  v: 4.4999999999999947
-  a: -8.9515364792933512e-15
+  v: 4.5000000000000275
+  a: -2.8295312097590921e-13
   relative_time: 4.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 586350.42633250472
-    y: 4140719.6856164662
-    theta: 1.3395426326190147
-    kappa: 0.0011875743707616257
-    s: 20.32199458482209
-    dkappa: -0.13642028751610516
+    x: 586350.43443897879
+    y: 4140719.6833844841
+    theta: 1.3389216504160295
+    kappa: 0.0012397277061377489
+    s: 20.321994584822409
+    dkappa: -0.13637642202542008
     ddkappa: 0
   }
-  v: 4.4999999999999938
-  a: -7.0531872443241137e-15
+  v: 4.5000000000000044
+  a: -1.9369591325833682e-13
   relative_time: 4.2000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586350.529367906
-    y: 4140720.1236617519
-    theta: 1.3398936446919465
-    kappa: 0.0006200627804790296
-    s: 20.771994584822089
-    dkappa: -0.040059789609556237
+    x: 586350.53773839888
+    y: 4140720.1213675803
+    theta: 1.3392978842571388
+    kappa: 0.000677462264871336
+    s: 20.771994584822409
+    dkappa: -0.040074002242703272
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: -5.3760819309755259e-15
+  v: 4.4999999999999885
+  a: -1.1772417689661626e-13
   relative_time: 4.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 586350.63229375717
-    y: 4140720.5617328295
-    theta: 1.340103757440102
-    kappa: 6.8764028396889353e-05
-    s: 21.221994584822088
-    dkappa: 0.15900408876779462
+    x: 586350.64092251693
+    y: 4140720.5593778971
+    theta: 1.3395344224295265
+    kappa: 0.00013122355402680902
+    s: 21.221994584822404
+    dkappa: 0.1590376802391163
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: -3.9086355679840379e-15
+  v: 4.4999999999999805
+  a: -5.4241585181746651e-14
   relative_time: 4.4
 }
 trajectory_point {
   path_point {
-    x: 586350.73523500562
-    y: 4140720.99980029
-    theta: 1.3399675169606444
-    kappa: -0.00043078516860508724
-    s: 21.671994584822084
-    dkappa: 0.038630046483338482
+    x: 586350.74410341948
+    y: 4140720.997388972
+    theta: 1.3394278395983521
+    kappa: -0.0003638279918400915
+    s: 21.6719945848224
+    dkappa: 0.0387108305727983
     ddkappa: 0
   }
-  v: 4.499999999999992
-  a: -2.6392631840860964e-15
+  v: 4.4999999999999778
+  a: -2.4518114047269307e-15
   relative_time: 4.5
 }
 trajectory_point {
   path_point {
-    x: 586350.83818128507
-    y: 4140721.4378665672
-    theta: 1.3397181043363526
-    kappa: -0.00091342501538578492
-    s: 22.121994584822083
-    dkappa: -0.18612146660375586
+    x: 586350.84728327033
+    y: 4140721.4354002941
+    theta: 1.3392090316876395
+    kappa: -0.00084213767814569082
+    s: 22.1219945848224
+    dkappa: -0.18609523856252563
     ddkappa: 0
   }
-  v: 4.499999999999992
-  a: -1.5563798080181513e-15
+  v: 4.49999999999998
+  a: 3.8441471143443745e-14
   relative_time: 4.6
 }
 trajectory_point {
   path_point {
-    x: 586350.94143273146
-    y: 4140721.8758609863
-    theta: 1.3391604570452336
-    kappa: -0.0013355003561473756
-    s: 22.571994584822079
-    dkappa: -0.038184618015211683
+    x: 586350.95074632589
+    y: 4140721.873344779
+    theta: 1.3386847973644389
+    kappa: -0.0012604619425926086
+    s: 22.571994584822395
+    dkappa: -0.038178132967654391
     ddkappa: 0
   }
-  v: 4.499999999999992
-  a: -6.4840046851665431e-16
+  v: 4.4999999999999849
+  a: 6.9234589171766371e-14
   relative_time: 4.6999999999999993
 }
 trajectory_point {
   path_point {
-    x: 586351.04476996313
-    y: 4140722.313835205
-    theta: 1.3385161621103923
-    kappa: -0.0017405504615542597
-    s: 23.021994584822078
-    dkappa: 0.21451833020085406
+    x: 586351.05428905517
+    y: 4140722.3112704614
+    theta: 1.338074637675269
+    kappa: -0.0016619105676595853
+    s: 23.021994584822391
+    dkappa: 0.21459681458942179
     ddkappa: 0
   }
-  v: 4.499999999999992
-  a: 9.6259805681946336e-17
+  v: 4.4999999999999929
+  a: 9.0723869389241939e-14
   relative_time: 4.7999999999999989
 }
 trajectory_point {
   path_point {
-    x: 586351.14867684909
-    y: 4140722.7516745338
-    theta: 1.3376114768488374
-    kappa: -0.0020759622140103331
-    s: 23.471994584822074
-    dkappa: 0.032954805360572867
+    x: 586351.1583764524
+    y: 4140722.74906692
+    theta: 1.3372064428545636
+    kappa: -0.0019943147547821362
+    s: 23.47199458482239
+    dkappa: 0.033024022161793856
     ddkappa: 0
   }
-  v: 4.499999999999992
-  a: 6.8918598584120222e-16
+  v: 4.5000000000000027
+  a: 1.0370563850487124e-13
   relative_time: 4.8999999999999986
 }
 trajectory_point {
   path_point {
-    x: 586351.25271943538
-    y: 4140723.1894817287
-    theta: 1.3366447625063527
-    kappa: -0.0023947850736817005
-    s: 23.921994584822073
-    dkappa: -0.25205746081645725
+    x: 586351.2625936683
+    y: 4140723.1868325784
+    theta: 1.3362767465079333
+    kappa: -0.0023102625608634533
+    s: 23.921994584822389
+    dkappa: -0.25207328471463158
     ddkappa: 0
   }
-  v: 4.499999999999992
-  a: 1.1419630432246634e-15
+  v: 4.5000000000000133
+  a: 1.0897622322765532e-13
   relative_time: 4.9999999999999982
 }
 trajectory_point {
   path_point {
-    x: 586351.357550191
-    y: 4140723.6271007215
-    theta: 1.3354760774435908
-    kappa: -0.002635820852199532
-    s: 24.371994584822069
-    dkappa: -0.031368769010402
+    x: 586351.36757140933
+    y: 4140723.6244163448
+    theta: 1.3351469551422459
+    kappa: -0.0025490305389610614
+    s: 24.371994584822389
+    dkappa: -0.031331752073435359
     ddkappa: 0
   }
-  v: 4.499999999999992
-  a: 1.4661759490958798e-15
+  v: 4.500000000000024
+  a: 1.0733195026659512e-13
   relative_time: 5.0999999999999979
 }
 trajectory_point {
   path_point {
-    x: 586351.46253696107
-    y: 4140724.0646824604
-    theta: 1.33426741280056
-    kappa: -0.0028614590438116624
-    s: 24.821994584822068
-    dkappa: 0.28942104927284845
+    x: 586351.47269973089
+    y: 4140724.0619640984
+    theta: 1.3339775454209664
+    kappa: -0.0027725172359608563
+    s: 24.821994584822392
+    dkappa: 0.28956374176203781
     ddkappa: 0
   }
-  v: 4.499999999999992
-  a: 1.6734096747184017e-15
+  v: 4.5000000000000346
+  a: 9.9569146330691591e-14
   relative_time: 5.1999999999999975
 }
 trajectory_point {
   path_point {
-    x: 586351.56848544953
-    y: 4140724.5020321612
-    theta: 1.3329238642433241
-    kappa: -0.0030011910638978335
-    s: 25.271994584822068
-    dkappa: 0.022149220969595568
+    x: 586351.578759866
+    y: 4140724.4992867485
+    theta: 1.3326746233584663
+    kappa: -0.0029107178885506685
+    s: 25.271994584822394
+    dkappa: 0.022190963960509569
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: 1.7752491913557821e-15
+  v: 4.5000000000000444
+  a: 8.6484138128945664e-14
   relative_time: 5.2999999999999972
 }
 trajectory_point {
   path_point {
-    x: 586351.6745879869
-    y: 4140724.9393446944
-    theta: 1.3315587093136325
-    kappa: -0.003127162560381069
-    s: 25.721994584822063
-    dkappa: -0.33931882192562263
+    x: 586351.68496924464
+    y: 4140724.9365733452
+    theta: 1.3313503168191936
+    kappa: -0.0030352587211167023
+    s: 25.721994584822397
+    dkappa: -0.33940166858808585
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: 1.7832794702715726e-15
+  v: 4.5000000000000515
+  a: 6.8873252370358491e-14
   relative_time: 5.3999999999999968
 }
 trajectory_point {
   path_point {
-    x: 586351.78176747239
-    y: 4140725.3763943538
-    theta: 1.3301351132123882
-    kappa: -0.0031595017951740268
-    s: 26.171994584822059
-    dkappa: -0.017505977840494152
+    x: 586351.792223834
+    y: 4140725.37360459
+    theta: 1.3299684121717328
+    kappa: -0.003066800884586887
+    s: 26.1719945848224
+    dkappa: -0.017430387996785007
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: 1.7090854827293187e-15
+  v: 4.5000000000000577
+  a: 4.7532815763930608e-14
   relative_time: 5.4999999999999964
 }
 trajectory_point {
   path_point {
-    x: 586351.88908135844
-    y: 4140725.8134112065
-    theta: 1.3287042232587667
-    kappa: -0.0031801559557353364
-    s: 26.621994584822058
-    dkappa: 0.38957862772191337
+    x: 586351.89960879181
+    y: 4140725.8106039846
+    theta: 1.3285793228545406
+    kappa: -0.00308674338000419
+    s: 26.621994584822406
+    dkappa: 0.38980140828189513
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: 1.5642521999925734e-15
+  v: 4.5000000000000613
+  a: 2.3259155018663263e-14
   relative_time: 5.5999999999999961
 }
 trajectory_point {
   path_point {
-    x: 586351.99751923024
-    y: 4140726.2501503797
-    theta: 1.3273003579541816
-    kappa: -0.0031016756364784675
-    s: 27.071994584822058
-    dkappa: 0.003923793334687703
+    x: 586352.00808461988
+    y: 4140726.24733374
+    theta: 1.3272175474528225
+    kappa: -0.0030081968332161028
+    s: 27.071994584822409
+    dkappa: 0.0039277342076767252
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: 1.3603645933248884e-15
+  v: 4.5000000000000622
+  a: -3.1514031564428535e-15
   relative_time: 5.6999999999999957
 }
 trajectory_point {
   path_point {
-    x: 586352.10606013949
-    y: 4140726.6868640971
-    theta: 1.3258989696407881
-    kappa: -0.0030141075123229174
-    s: 27.521994584822053
-    dkappa: -0.4544026420834345
+    x: 586352.11666033557
+    y: 4140726.6840388048
+    theta: 1.3258582727442871
+    kappa: -0.0029206319279478786
+    s: 27.521994584822416
+    dkappa: -0.45456832794179519
     ddkappa: 0
   }
-  v: 4.4999999999999938
-  a: 1.1090076339898122e-15
+  v: 4.50000000000006
+  a: -3.0902532052386294e-14
   relative_time: 5.7999999999999954
 }
 trajectory_point {
   path_point {
-    x: 586352.21571687749
-    y: 4140727.1232988797
-    theta: 1.3245327622148142
-    kappa: -0.00305061564104155
-    s: 27.971994584822049
-    dkappa: 0.0059046659590921147
+    x: 586352.22631785378
+    y: 4140727.1204733974
+    theta: 1.324533923722097
+    kappa: -0.0029577734399735536
+    s: 27.971994584822419
+    dkappa: 0.0060179691050188389
     ddkappa: 0
   }
-  v: 4.4999999999999938
-  a: 8.2176629325089955e-16
+  v: 4.500000000000056
+  a: -5.9197904960166318e-14
   relative_time: 5.899999999999995
 }
 trajectory_point {
   path_point {
-    x: 586352.32544132427
-    y: 4140727.5597167364
-    theta: 1.3231686893473864
-    kappa: -0.0030946527638596776
-    s: 28.421994584822052
-    dkappa: 0.521955018088599
+    x: 586352.33604088228
+    y: 4140727.5568916108
+    theta: 1.3232116894661747
+    kappa: -0.0030024667326322926
+    s: 28.421994584822421
+    dkappa: 0.52226065105151809
     ddkappa: 0
   }
-  v: 4.4999999999999938
-  a: 5.1022554237169732e-16
+  v: 4.5000000000000488
+  a: -8.7241195170782487e-14
   relative_time: 5.9999999999999947
 }
 trajectory_point {
   path_point {
-    x: 586352.43632670294
-    y: 4140727.9958410165
-    theta: 1.3217564579870065
-    kappa: -0.0031352584074654357
-    s: 28.871994584822048
-    dkappa: -0.025664728856659269
+    x: 586352.44689042075
+    y: 4140727.9930250044
+    theta: 1.3218403762499289
+    kappa: -0.0030444792092926575
+    s: 28.871994584822424
+    dkappa: -0.025702082848782393
     ddkappa: 0
   }
-  v: 4.4999999999999938
-  a: 1.8597035261575555e-16
+  v: 4.5000000000000391
+  a: -1.1423607597523396e-13
   relative_time: 6.0999999999999943
 }
 trajectory_point {
   path_point {
-    x: 586352.54724871763
-    y: 4140728.4319560318
-    theta: 1.3203427067749662
-    kappa: -0.0031757557622767519
-    s: 29.321994584822043
-    dkappa: -0.60685121060743752
+    x: 586352.55777536135
+    y: 4140728.4291494479
+    theta: 1.320467520571631
+    kappa: -0.0030864074373356727
+    s: 29.321994584822427
+    dkappa: -0.60710904974221369
     ddkappa: 0
   }
-  v: 4.4999999999999938
-  a: -1.394143047533757e-16
+  v: 4.5000000000000258
+  a: -1.3938622066451802e-13
   relative_time: 6.199999999999994
 }
 trajectory_point {
   path_point {
-    x: 586352.65939238959
-    y: 4140728.8677584995
-    theta: 1.3189087186229829
-    kappa: -0.0031810850020900254
-    s: 29.771994584822043
-    dkappa: 0.042784509450065622
+    x: 586352.66984774312
+    y: 4140728.8649702547
+    theta: 1.319072839376852
+    kappa: -0.0030938842061402818
+    s: 29.771994584822426
+    dkappa: 0.042936549405276747
     ddkappa: 0
   }
-  v: 4.4999999999999938
-  a: -4.5434345847214009e-16
+  v: 4.5000000000000107
+  a: -1.6189530252963596e-13
   relative_time: 6.2999999999999936
 }
 trajectory_point {
   path_point {
-    x: 586352.77156887157
-    y: 4140729.3035524883
-    theta: 1.3174749003374071
-    kappa: -0.0031854742139525664
-    s: 30.221994584822042
-    dkappa: 0.66561460904153846
+    x: 586352.78195206111
+    y: 4140729.3007828151
+    theta: 1.3176782750333296
+    kappa: -0.0031004400678157546
+    s: 30.221994584822426
+    dkappa: 0.665807916203442
     ddkappa: 0
   }
   v: 4.4999999999999938
-  a: -7.47232137276986e-16
+  a: -1.8096699486158546e-13
   relative_time: 6.3999999999999932
 }
 trajectory_point {
   path_point {
-    x: 586352.88494693907
-    y: 4140729.7390354546
-    theta: 1.3160518074261338
-    kappa: -0.0031554655505263815
-    s: 30.671994584822038
-    dkappa: -0.071428006954042444
+    x: 586352.89522573061
+    y: 4140729.7362929475
+    theta: 1.3162922503495407
+    kappa: -0.0030733112064291433
+    s: 30.671994584822421
+    dkappa: -0.071518434706324019
     ddkappa: 0
   }
-  v: 4.4999999999999938
-  a: -1.0064953699043681e-15
+  v: 4.4999999999999751
+  a: -1.95804970951366e-13
   relative_time: 6.4999999999999929
 }
 trajectory_point {
   path_point {
-    x: 586352.99838470866
-    y: 4140730.174502789
-    theta: 1.3146308043899821
-    kappa: -0.00312371997305115
-    s: 31.121994584822033
-    dkappa: -0.73092286351781721
+    x: 586353.00855775981
+    y: 4140730.1717878175
+    theta: 1.3149081787503594
+    kappa: -0.0030444763350576166
+    s: 31.121994584822417
+    dkappa: -0.73104048551119327
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: -1.2205481850907458e-15
+  v: 4.4999999999999547
+  a: -2.0561290408997672e-13
   relative_time: 6.5999999999999925
 }
 trajectory_point {
   path_point {
-    x: 586353.11296929361
-    y: 4140730.6096698521
-    theta: 1.3132499417911347
-    kappa: -0.0030586101304047779
-    s: 31.571994584822033
-    dkappa: 0.0991919762246799
+    x: 586353.12300737738
+    y: 4140730.606990397
+    theta: 1.3135615052053289
+    kappa: -0.0029829713419676513
+    s: 31.571994584822413
+    dkappa: 0.099391202009226687
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: -1.3778056115725534e-15
+  v: 4.4999999999999343
+  a: -2.0959446756841657e-13
   relative_time: 6.6999999999999922
 }
 trajectory_point {
   path_point {
-    x: 586353.2276421265
-    y: 4140731.0448135533
-    theta: 1.3118746825998586
-    kappa: -0.0029908313958220667
-    s: 32.021994584822032
-    dkappa: 0.79477161491760628
+    x: 586353.23754329281
+    y: 4140731.0421701614
+    theta: 1.3122201768927346
+    kappa: -0.0029188476135367219
+    s: 32.0219945848224
+    dkappa: 0.79497356277746012
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: -1.466682678086247e-15
+  v: 4.499999999999913
+  a: -2.0695333467768469e-13
   relative_time: 6.7999999999999918
 }
 trajectory_point {
   path_point {
-    x: 586353.34338436788
-    y: 4140731.4796741442
-    theta: 1.3105673230089241
-    kappa: -0.0028907104310981419
-    s: 32.471994584822028
-    dkappa: -0.13997643976476165
+    x: 586353.35312278522
+    y: 4140731.4770740373
+    theta: 1.3109434834339255
+    kappa: -0.0028230562828833895
+    s: 32.47199458482239
+    dkappa: -0.14013763022618092
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: -1.4755944133682672e-15
+  v: 4.4999999999998925
+  a: -1.9689317870877954e-13
   relative_time: 6.8999999999999915
 }
 trajectory_point {
   path_point {
-    x: 586353.45923764457
-    y: 4140731.9145050347
-    theta: 1.309270710280984
-    kappa: -0.002786993649780078
-    s: 32.921994584822031
-    dkappa: -0.870024355753668
+    x: 586353.46881095483
+    y: 4140731.9119488895
+    theta: 1.3096771229525759
+    kappa: -0.00272373852405864
+    s: 32.921994584822372
+    dkappa: -0.87016757596801486
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: -1.3929558461550734e-15
+  v: 4.4999999999998739
+  a: -1.786176729527013e-13
   relative_time: 6.9999999999999911
 }
 trajectory_point {
   path_point {
-    x: 586353.57605801662
-    y: 4140732.3490772387
-    theta: 1.3080676973423111
-    kappa: -0.0026519580499896091
-    s: 33.371994584822026
-    dkappa: 0.18282357880482458
+    x: 586353.58544412535
+    y: 4140732.3465713733
+    theta: 1.3085006098697869
+    kappa: -0.0025937567281991564
+    s: 33.37199458482236
+    dkappa: 0.18308535134968995
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: -1.207182005183114e-15
+  v: 4.499999999999857
+  a: -1.5133049070044848e-13
   relative_time: 7.0999999999999908
 }
 trajectory_point {
   path_point {
-    x: 586353.69300486788
-    y: 4140732.7836152907
-    theta: 1.3068822403230413
-    kappa: -0.002512403044409335
-    s: 33.821994584822022
-    dkappa: 0.94524414912803612
+    x: 586353.702201252
+    y: 4140732.7811604473
+    theta: 1.3073410489580739
+    kappa: -0.0024593440935064314
+    s: 33.821994584822349
+    dkappa: 0.94545065067220846
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: -9.06687919188831e-16
+  v: 4.4999999999998437
+  a: -1.1423530524301953e-13
   relative_time: 7.19999999999999
 }
 trajectory_point {
   path_point {
-    x: 586353.81080002617
-    y: 4140733.2179242829
-    theta: 1.30581452997562
-    kappa: -0.0023425359988985123
-    s: 34.271994584822018
-    dkappa: -0.24025637262762634
+    x: 586353.8197886222
+    y: 4140733.2155257431
+    theta: 1.3062950294395301
+    kappa: -0.002295254228880772
+    s: 34.271994584822323
+    dkappa: -0.240514681192731
     ddkappa: 0
   }
-  v: 4.499999999999992
-  a: -4.7988861690867918e-16
+  v: 4.4999999999998348
+  a: -6.6535789871414057e-14
   relative_time: 7.29999999999999
 }
 trajectory_point {
   path_point {
-    x: 586353.92872757465
-    y: 4140733.6521972292
-    theta: 1.3047728441522992
-    kappa: -0.0021672295713979091
-    s: 34.721994584822014
-    dkappa: -1.031233076378776
+    x: 586353.93750598119
+    y: 4140733.6498557162
+    theta: 1.3052742068135319
+    kappa: -0.0021258326757727366
+    s: 34.721994584822312
+    dkappa: -1.0314020708854794
     ddkappa: 0
   }
-  v: 4.499999999999992
-  a: 8.4800872920888412e-17
+  v: 4.4999999999998312
+  a: -7.4356178766318837e-15
   relative_time: 7.39999999999999
 }
 trajectory_point {
   path_point {
-    x: 586354.04736863694
-    y: 4140734.0862759068
-    theta: 1.3038714154268749
-    kappa: -0.0019626077011953803
-    s: 35.171994584822016
-    dkappa: 0.30406861013292508
+    x: 586354.05592307146
+    y: 4140734.0839955471
+    theta: 1.304389017806463
+    kappa: -0.001927710596831373
+    s: 35.171994584822286
+    dkappa: 0.3044185650291078
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: 7.9896552156343284e-16
+  v: 4.4999999999998339
+  a: 6.38615374503294e-14
   relative_time: 7.4999999999999893
 }
 trajectory_point {
   path_point {
-    x: 586354.16613656178
-    y: 4140734.5203198022
-    theta: 1.303004792727716
-    kappa: -0.0017561494481416516
-    s: 35.621994584822012
-    dkappa: 1.1191526460802834
+    x: 586354.17446506652
+    y: 4140734.5181012033
+    theta: 1.3035375829810099
+    kappa: -0.0017277655500192648
+    s: 35.621994584822275
+    dkappa: 1.1193555327120757
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: 1.6741903002824852e-15
+  v: 4.4999999999998446
+  a: 1.4815200281847069e-13
   relative_time: 7.599999999999989
 }
 trajectory_point {
   path_point {
-    x: 586354.28546991083
-    y: 4140734.954208672
-    theta: 1.302293298927004
-    kappa: -0.0015415064730716149
-    s: 36.071994584822008
-    dkappa: -0.38436308874149827
+    x: 586354.29356315592
+    y: 4140734.9520547092
+    theta: 1.3028364257304674
+    kappa: -0.0015197043917750544
+    s: 36.071994584822257
+    dkappa: -0.3847554446576219
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: 2.7220601803416271e-15
+  v: 4.4999999999998641
+  a: 2.4623210493679075e-13
   relative_time: 7.6999999999999886
 }
 trajectory_point {
   path_point {
-    x: 586354.40491428459
-    y: 4140735.3880669344
-    theta: 1.3016204387677082
-    kappa: -0.0013299517253127216
-    s: 36.521994584822011
-    dkappa: -1.2165941231807045
+    x: 586354.41277102625
+    y: 4140735.3859780142
+    theta: 1.3021727566180608
+    kappa: -0.0013146166430572238
+    s: 36.521994584822238
+    dkappa: -1.2167827592229647
     ddkappa: 0
   }
-  v: 4.4999999999999929
-  a: 3.9541601330043881e-15
+  v: 4.4999999999998943
+  a: 3.5889817051429362e-13
   relative_time: 7.7999999999999883
 }
 trajectory_point {
   path_point {
-    x: 586354.52477637655
-    y: 4140735.8218100369
-    theta: 1.3010929328349339
-    kappa: -0.0011300160770166463
-    s: 36.971994584822006
-    dkappa: 0.47680618870562141
+    x: 586354.532391519
+    y: 4140735.8197878087
+    theta: 1.3016499901176131
+    kappa: -0.0011207047576172404
+    s: 36.971994584822227
+    dkappa: 0.47728498985524703
     ddkappa: 0
   }
-  v: 4.4999999999999938
-  a: 5.3820751295343088e-15
+  v: 4.4999999999999361
+  a: 4.869465262599774e-13
   relative_time: 7.8999999999999879
 }
 trajectory_point {
   path_point {
-    x: 586354.64472766884
-    y: 4140736.2555284477
-    theta: 1.3006058766071169
-    kappa: -0.0009335745424951411
-    s: 37.421994584822
-    dkappa: 1.3180899577580212
+    x: 586354.652100855
+    y: 4140736.2535730638
+    theta: 1.3011664942288708
+    kappa: -0.00093015738901996872
+    s: 37.42199458482223
+    dkappa: 1.3182806879272462
     ddkappa: 0
   }
-  v: 4.4999999999999938
-  a: 7.0173901411949706e-15
+  v: 4.499999999999992
+  a: 6.3117349888284576e-13
   relative_time: 7.9999999999999876
 }
 decision {
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_avoid_change_left_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_avoid_change_left_0.pb.txt
index 431704fc49eab139e00b75739ccd52b13abd9d85..84d007bf4526ddf1e2a191ccf66fa9847e9d7fe3 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_avoid_change_left_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_avoid_change_left_0.pb.txt
@@ -9,7 +9,7 @@ trajectory_point {
     y: 4141500.3527275259
     z: 0
     theta: 2.8987073227943752
-    kappa: -3.793934227329437e-21
+    kappa: -3.8071761913511895e-21
     dkappa: 0
     ddkappa: 0
   }
@@ -19,12 +19,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587567.91298981872
-    y: 4141500.6237686076
-    theta: 2.8989498908121156
-    kappa: 0.00038798398568978083
+    x: 587567.91298400785
+    y: 4141500.6237451313
+    theta: 2.8990024040426574
+    kappa: 0.00047220636677523467
     s: 1.1274213544025757
-    dkappa: 0.00034341096769422417
+    dkappa: 0.0004179698991923397
     ddkappa: 0
   }
   v: 11.299146435412135
@@ -33,12 +33,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587566.81354262051
-    y: 4141500.8955374951
-    theta: 2.8995997746112643
-    kappa: 0.00072981918396994967
+    x: 587566.81350402755
+    y: 4141500.8953812765
+    theta: 2.8997938697379939
+    kappa: 0.00088926476069530976
     s: 2.2599594632243107
-    dkappa: 0.00039283785036943952
+    dkappa: 0.00047901259100453878
     ddkappa: 0
   }
   v: 11.35219968654174
@@ -47,12 +47,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587565.70856167085
-    y: 4141501.1677025147
-    theta: 2.9006092933361347
-    kappa: 0.0010254884706016221
+    x: 587565.70844097552
+    y: 4141501.1672125189
+    theta: 2.9010247946802945
+    kappa: 0.0012511274786177417
     s: 3.3979651532149941
-    dkappa: 0.00059019173258787871
+    dkappa: 0.000720348225132545
     ddkappa: 0
   }
   v: 11.408370314254398
@@ -61,12 +61,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587564.59769088984
-    y: 4141501.4399595661
-    theta: 2.9019302675199481
-    kappa: 0.0012747681229848392
+    x: 587564.59741905553
+    y: 4141501.43885175
+    theta: 2.9026376386554174
+    kappa: 0.0015575160318996178
     s: 4.5417126044024
-    dkappa: 0.0006976119397357263
+    dkappa: 0.00085281053048030743
     ddkappa: 0
   }
   v: 11.466914823244277
@@ -75,12 +75,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587563.48065644864
-    y: 4141501.7120324518
-    theta: 2.9035137971556906
-    kappa: 0.0014771622072002016
+    x: 587563.48014750576
+    y: 4141501.7099487744
+    theta: 2.9045739745944816
+    kappa: 0.001807859302033304
     s: 5.691403944475149
-    dkappa: 0.00073919948905850036
+    dkappa: 0.00090573285760810077
     ddkappa: 0
   }
   v: 11.527135662034116
@@ -89,12 +89,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587562.35726391978
-    y: 4141501.983680577
-    theta: 2.9053094594515971
-    kappa: 0.0016321649879890137
+    x: 587562.35641784465
+    y: 4141501.9801983554
+    theta: 2.9067736459880158
+    kappa: 0.0020015588641197754
     s: 6.8471738431655664
-    dkappa: 0.000913264381549689
+    dkappa: 0.0011201686940977211
     ddkappa: 0
   }
   v: 11.588381222975229
@@ -103,12 +103,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587561.22738185234
-    y: 4141502.2546505323
-    theta: 2.9072676764009415
-    kappa: 0.0017380702458307164
+    x: 587561.22608488286
+    y: 4141502.2492807726
+    theta: 2.9091777192580608
+    kappa: 0.0021365871984845549
     s: 8.009094106632535
-    dkappa: 0.00079882474335316793
+    dkappa: 0.000984414993881264
     ddkappa: 0
   }
   v: 11.650045842247509
@@ -117,12 +117,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587560.09091489844
-    y: 4141502.5245817564
-    theta: 2.9093358129228468
-    kappa: 0.0017899944755404552
+    x: 587560.089034858
+    y: 4141502.5167446514
+    theta: 2.9117241862382008
+    kappa: 0.0022071418488688876
     s: 9.1771782718443564
-    dkappa: 0.00095850565161558583
+    dkappa: 0.0011824805625441472
     ddkappa: 0
   }
   v: 11.711569799859422
@@ -131,12 +131,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587558.94793614559
-    y: 4141502.7935852222
-    theta: 2.9114443206927145
-    kappa: 0.0017920186184957961
+    x: 587558.94534771657
+    y: 4141502.7827192852
+    theta: 2.9143290994111388
+    kappa: 0.0022181472752202517
     s: 10.35138620096161
-    dkappa: 0.00086933857934265186
+    dkappa: 0.0010785728816260604
     ddkappa: 0
   }
   v: 11.772439319648015
@@ -145,12 +145,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587557.79852942261
-    y: 4141503.0616064807
-    theta: 2.9135358787204626
-    kappa: 0.0017431460714854138
+    x: 587557.79510565894
+    y: 4141503.047131524
+    theta: 2.9169242119457426
+    kappa: 0.0021684715977315907
     s: 11.531628675720004
-    dkappa: 0.00084520639776406283
+    dkappa: 0.0010544150881990487
     ddkappa: 0
   }
   v: 11.832186569278905
@@ -159,12 +159,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587556.64283723093
-    y: 4141503.3286468685
-    theta: 2.915551747265519
-    kappa: 0.0016424566786174741
+    x: 587556.63845224911
+    y: 4141503.3099762867
+    theta: 2.9194395686084937
+    kappa: 0.0020570542436830208
     s: 12.717771991813242
-    dkappa: 0.00084295406495575359
+    dkappa: 0.0010573398887907661
     ddkappa: 0
   }
   v: 11.890389660246289
@@ -173,12 +173,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587555.48105587682
-    y: 4141503.5947650978
-    theta: 2.9174317892286736
-    kappa: 0.0014890441919738777
+    x: 587555.475587421
+    y: 4141503.5713187209
+    theta: 2.9218035312757693
+    kappa: 0.0018828449726492596
     s: 13.909642553275875
-    dkappa: 0.00062233127004308517
+    dkappa: 0.00079572916321856986
     ddkappa: 0
   }
   v: 11.946672647872937
@@ -187,12 +187,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587554.31338879687
-    y: 4141503.8598940764
-    theta: 2.9190858111490203
-    kappa: 0.0012763314956262186
+    x: 587554.306710138
+    y: 4141503.8310597762
+    theta: 2.9239098476229155
+    kappa: 0.001638069506197292
     s: 15.107031466866154
-    dkappa: 0.00065689364919056413
+    dkappa: 0.00084491073315638677
     ddkappa: 0
   }
   v: 12.000705531310201
@@ -201,12 +201,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587553.14016861981
-    y: 4141504.124400381
-    theta: 2.9204532729001689
-    kappa: 0.0010065709300440819
+    x: 587553.13216994936
+    y: 4141504.0896410318
+    theta: 2.9256858292176084
+    kappa: 0.0013254242875770234
     s: 16.309699136448902
-    dkappa: 0.00041492701914425307
+    dkappa: 0.00055856567559142776
     ddkappa: 0
   }
   v: 12.052204253538003
@@ -215,12 +215,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587551.96175095486
-    y: 4141504.3886432173
-    theta: 2.92147599903322
-    kappa: 0.000680829098419135
+    x: 587551.95233814023
+    y: 4141504.347495961
+    theta: 2.9270621507809791
+    kappa: 0.00094622979396132706
     s: 17.517379857378359
-    dkappa: 0.00025477123938433044
+    dkappa: 0.00037291555420942393
     ddkappa: 0
   }
   v: 12.100930701364842
@@ -229,12 +229,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587550.77851007471
-    y: 4141504.6529736333
-    theta: 2.9220874584295315
-    kappa: 0.00029831829417681762
+    x: 587550.76760242844
+    y: 4141504.6050434951
+    theta: 2.9279596149554843
+    kappa: 0.00049954246976319163
     s: 18.729786410881044
-    dkappa: 0.00011995065583685176
+    dkappa: 0.00021806916553558555
     ddkappa: 0
   }
   v: 12.146692705427794
@@ -243,12 +243,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587549.59085310181
-    y: 4141504.9178167717
-    theta: 2.9222175950679352
-    kappa: -0.00013541900938769598
+    x: 587549.5783853631
+    y: 4141504.8627908891
+    theta: 2.9282947291262178
+    kappa: -1.2565349039625337e-05
     s: 19.946614658438616
-    dkappa: -0.00025786897666095607
+    dkappa: -0.0002354721932733734
     ddkappa: 0
   }
   v: 12.189344040192514
@@ -257,12 +257,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587548.399213131
-    y: 4141505.1836602017
-    theta: 2.92199441100602
-    kappa: -0.00018352846724201639
+    x: 587548.38513663714
+    y: 4141505.1213175431
+    theta: 2.9281634917506238
+    kappa: -0.00014209441845732906
     s: 21.167548136170716
-    dkappa: -2.2802172568977191e-06
+    dkappa: 1.4731204993988437e-05
     ddkappa: 0
   }
   v: 12.228784423953231
@@ -271,12 +271,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587547.20394099853
-    y: 4141505.45058745
-    theta: 2.9217714856001091
-    kappa: -0.00018121999270832121
+    x: 587547.18823780352
+    y: 4141505.3808533
+    theta: 2.9279464959432393
+    kappa: -0.00020822099178336622
     s: 22.392262649217859
-    dkappa: -0.00010232956349653341
+    dkappa: -0.00013094664846351371
     ddkappa: 0
   }
   v: 12.264959518832745
@@ -285,12 +285,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587546.00535614393
-    y: 4141505.7185268151
-    theta: 2.9215475777820039
-    kappa: -0.000185781375318753
+    x: 587545.988028442
+    y: 4141505.6414192924
+    theta: 2.9276523740253149
+    kappa: -0.00027297961077246783
     s: 23.620430866124245
-    dkappa: -0.00016202379312263312
+    dkappa: -0.00022435632882518563
     ddkappa: 0
   }
   v: 12.297860930782438
@@ -299,12 +299,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587544.80377727794
-    y: 4141505.9874119833
-    theta: 2.9213137430839646
-    kappa: -0.00019665893043778173
+    x: 587544.78484531376
+    y: 4141505.9030322726
+    theta: 2.9272813578128294
+    kappa: -0.00033586794716445841
     s: 24.851727343303313
-    dkappa: 4.5940027354720076e-05
+    dkappa: -1.6575829728034248e-05
     ddkappa: 0
   }
   v: 12.327543022331355
@@ -313,12 +313,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587543.59952335246
-    y: 4141506.2572261267
-    theta: 2.9210601397496836
-    kappa: -0.00021403624678359103
+    x: 587543.57902926893
+    y: 4141506.1657770695
+    theta: 2.9268274391478668
+    kappa: -0.00039615835140526445
     s: 26.085837225438219
-    dkappa: -0.00022353121908726006
+    dkappa: -0.00033678216552967676
     ddkappa: 0
   }
   v: 12.354157728543237
@@ -327,12 +327,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587542.39288780233
-    y: 4141506.5279148314
-    theta: 2.92078051747684
-    kappa: -0.00023700832476838043
+    x: 587542.37088670919
+    y: 4141506.42965817
+    theta: 2.9262989766714291
+    kappa: -0.0004535308108097897
     s: 27.322462368204437
-    dkappa: -9.2967089419639492e-05
+    dkappa: -0.00020288613999767022
     ddkappa: 0
   }
   v: 12.377876779761349
@@ -341,12 +341,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587541.18414884
-    y: 4141506.7994340318
-    theta: 2.9204702634648947
-    kappa: -0.00026500688713043582
+    x: 587541.1607045217
+    y: 4141506.6946705561
+    theta: 2.9257033567648936
+    kappa: -0.00050779167744652266
     s: 28.561321754159671
-    dkappa: -8.70763125053466e-05
+    dkappa: -0.00020782406876188657
     ddkappa: 0
   }
   v: 12.398870983286846
@@ -355,12 +355,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587539.97357287037
-    y: 4141507.0717647304
-    theta: 2.9201236418348655
-    kappa: -0.00029754232345299256
+    x: 587539.948757585
+    y: 4141506.9608335663
+    theta: 2.9250442740195046
+    kappa: -0.00055843758140411036
     s: 29.802151400439627
-    dkappa: -0.00029095281568622443
+    dkappa: -0.00043922661024356052
     ddkappa: 0
   }
   v: 12.417310223378767
@@ -369,12 +369,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587538.76142181153
-    y: 4141507.3449448994
-    theta: 2.9197310878283478
-    kappa: -0.00033430434833747605
+    x: 587538.73532141955
+    y: 4141507.2282474674
+    theta: 2.9243210901436605
+    kappa: -0.00060438157536944152
     s: 31.044704266453806
-    dkappa: -5.0555876653321279e-05
+    dkappa: -0.00017089779322392598
     ddkappa: 0
   }
   v: 12.43336346125403
@@ -383,12 +383,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587537.547932359
-    y: 4141507.6189768221
-    theta: 2.9192885741918015
-    kappa: -0.00037460508537166874
+    x: 587537.5206357867
+    y: 4141507.496927334
+    theta: 2.9235413899639489
+    kappa: -0.00064504727977113274
     s: 32.288750161581305
-    dkappa: -0.00023576898010582359
+    dkappa: -0.00037614949374193942
     ddkappa: 0
   }
   v: 12.447198735087445
@@ -397,12 +397,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587536.33331835037
-    y: 4141507.8938373858
-    theta: 2.9187956881750878
-    kappa: -0.00041767642035130234
+    x: 587536.30490902322
+    y: 4141507.7668233253
+    theta: 2.922715600607428
+    kappa: -0.000680220993159819
     s: 33.5340756528666
-    dkappa: -0.00026324655579084712
+    dkappa: -0.00039647595942538991
     ddkappa: 0
   }
   v: 12.458983160011702
@@ -411,12 +411,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587535.11778474972
-    y: 4141508.1695395913
-    theta: 2.9182491689868164
-    kappa: -0.00046309258049077022
+    x: 587535.08834433218
+    y: 4141508.0379393515
+    theta: 2.9218501689592395
+    kappa: -0.00070943149336395934
     s: 34.7804839727153
-    dkappa: -0.00010976564020873272
+    dkappa: -0.00021437208591064795
     ddkappa: 0
   }
   v: 12.468882928117374
@@ -425,12 +425,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587533.90153701545
-    y: 4141508.4461729056
-    theta: 2.9176419523128558
-    kappa: -0.00051024869007150258
+    x: 587533.87114857859
+    y: 4141508.3103709053
+    theta: 2.9209507997890167
+    kappa: -0.00073131565853744426
     s: 36.027794926590005
-    dkappa: -0.00039340243376701997
+    dkappa: -0.00051440759086314556
     ddkappa: 0
   }
   v: 12.477063308452921
@@ -439,12 +439,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587532.68475255161
-    y: 4141508.7237768676
-    theta: 2.9169728590631445
-    kappa: -0.00055836082963775762
+    x: 587532.65349085955
+    y: 4141508.5841189427
+    theta: 2.9200279830320648
+    kappa: -0.000745043987097039
     s: 37.275844800706025
-    dkappa: -0.00024775164682389197
+    dkappa: -0.0003274995699892159
     ddkappa: 0
   }
   v: 12.483688647024689
@@ -453,12 +453,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587531.46757793415
-    y: 4141509.0023290846
-    theta: 2.9162459281790736
-    kappa: -0.0006066927403707154
+    x: 587531.43550088862
+    y: 4141508.8590839063
+    theta: 2.9190936230030169
+    kappa: -0.00075050873074773467
     s: 38.524486269727227
-    dkappa: -0.00026711675903463465
+    dkappa: -0.00032527878783577982
     ddkappa: 0
   }
   v: 12.488922366796904
@@ -467,12 +467,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587530.25015646231
-    y: 4141509.2818657178
-    theta: 2.9154607103627788
-    kappa: -0.00065468541947883427
+    x: 587530.21730910987
+    y: 4141509.1352441888
+    theta: 2.918157231253808
+    kappa: -0.0007470920491615252
     s: 39.773588304461768
-    dkappa: -0.00047238670398348651
+    dkappa: -0.00052223636835393181
     ddkappa: 0
   }
   v: 12.492926967691682
@@ -481,12 +481,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587529.03263846657
-    y: 4141509.5625248072
-    theta: 2.9146046597288278
-    kappa: -0.00071825688701279884
+    x: 587528.99905103177
+    y: 4141509.4126731362
+    theta: 2.9172203336951044
+    kappa: -0.000756447669724207
     s: 41.023036079557926
-    dkappa: -0.00024898913315126391
+    dkappa: -0.00024157035887810964
     ddkappa: 0
   }
   v: 12.495864026589016
@@ -495,12 +495,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587527.8151424767
-    y: 4141509.8443765556
-    theta: 2.9136444277993263
-    kappa: -0.00081682252593394118
+    x: 587527.78082574857
+    y: 4141509.691355844
+    theta: 2.916245667913234
+    kappa: -0.00080637885933251352
     s: 42.272730881199855
-    dkappa: -0.00046937451520361907
+    dkappa: -0.00046054946451816957
     ddkappa: 0
   }
   v: 12.49789419732679
@@ -509,12 +509,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587526.59776427643
-    y: 4141510.1274628965
-    theta: 2.9125460766762155
-    kappa: -0.00094862953040549089
+    x: 587526.56271123746
+    y: 4141509.9712569304
+    theta: 2.9151876563207879
+    kappa: -0.00089522202660921916
     s: 43.522590014803427
-    dkappa: -0.00055910336252526982
+    dkappa: -0.00052933210673547287
     ddkappa: 0
   }
   v: 12.499177210700768
@@ -523,12 +523,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587525.38061130722
-    y: 4141510.4119450459
-    theta: 2.9112713921843305
-    kappa: -0.001113197603201332
+    x: 587525.34480101836
+    y: 4141510.2524795784
+    theta: 2.9140014686782454
+    kappa: -0.0010225065230491672
     s: 44.772546712711943
-    dkappa: -0.00044898652142831542
+    dkappa: -0.00038086550398162182
     ddkappa: 0
   }
   v: 12.499871874464601
@@ -537,12 +537,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587524.16383579664
-    y: 4141510.6982407346
-    theta: 2.9097682110067185
-    kappa: -0.00127568096622347
+    x: 587524.12722893467
+    y: 4141510.5353685375
+    theta: 2.91263198820241
+    kappa: -0.0011551334109150663
     s: 46.022550041891996
-    dkappa: -0.00078489828652395714
+    dkappa: -0.000735055482251239
     ddkappa: 0
   }
   v: 12.500136073329825
@@ -551,12 +551,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587522.94750378735
-    y: 4141510.9864643458
-    theta: 2.9082154350771887
-    kappa: -0.0011786485252885822
+    x: 587522.91005222977
+    y: 4141510.8200040692
+    theta: 2.9112449704334429
+    kappa: -0.0010358400936852771
     s: 47.272564811629209
-    dkappa: -0.00052595515062983482
+    dkappa: -0.000445061137414581
     ddkappa: 0
   }
   v: 12.500126768965856
@@ -565,12 +565,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587521.73158462672
-    y: 4141511.2763898615
-    theta: 2.9068473888209838
-    kappa: -0.00099878976220888079
+    x: 587521.69323432236
+    y: 4141511.1061343011
+    theta: 2.9100671683721964
+    kappa: -0.00083834372451782884
     s: 48.522571481224041
-    dkappa: -0.000378832738285126
+    dkappa: -0.00029011053427370958
     ddkappa: 0
   }
   v: 12.5
@@ -579,12 +579,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587520.516027435
-    y: 4141511.5677749291
-    theta: 2.9057178752049482
-    kappa: -0.00077924950370017888
+    x: 587520.47671890631
+    y: 4141511.3934927322
+    theta: 2.9091456341571336
+    kappa: -0.00060711963141263142
     s: 49.772565403395092
-    dkappa: -0.00054063638795803217
+    dkappa: -0.000468448544038838
     ddkappa: 0
   }
   v: 12.499884320248489
@@ -593,12 +593,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587519.30078672641
-    y: 4141511.8604351333
-    theta: 2.9049103480264828
-    kappa: -0.000523547119286797
+    x: 587519.2604484224
+    y: 4141511.6818434559
+    theta: 2.90855640326852
+    kappa: -0.0003464552143292429
     s: 51.0225494406465
-    dkappa: -6.9602564441010358e-05
+    dkappa: 3.4577783936764229e-05
     ddkappa: 0
   }
   v: 12.499801510634402
@@ -607,12 +607,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587518.0857400666
-    y: 4141512.1538709681
-    theta: 2.9044404917931423
-    kappa: -0.00024247051772239035
+    x: 587518.0443060681
+    y: 4141511.9707046356
+    theta: 2.9083081804893163
+    kappa: -6.6405649494970663e-05
     s: 52.2725266471774
-    dkappa: -8.60414663927451e-05
+    dkappa: -3.1239072723994107e-06
     ddkappa: 0
   }
   v: 12.499746959637889
@@ -621,12 +621,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587516.87078040536
-    y: 4141512.4476489
-    theta: 2.9043178178844471
-    kappa: 5.5362376620466682e-05
+    x: 587516.82818844169
+    y: 4141512.2596516442
+    theta: 2.9084024316650274
+    kappa: 0.00022464679835935091
     s: 53.522499629466587
-    dkappa: -5.2338262479261524e-06
+    dkappa: 7.1566706700501963e-05
     ddkappa: 0
   }
   v: 12.499716324371331
@@ -635,12 +635,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587515.65580878558
-    y: 4141512.7413686956
-    theta: 2.9045577848495849
-    kappa: 0.00036289997687825431
+    x: 587515.61200029822
+    y: 4141512.5482928311
+    theta: 2.9088479437239783
+    kappa: 0.00051972282737370061
     s: 54.772470573135664
-    dkappa: 0.00045447497430688963
+    dkappa: 0.000546389968027799
     ddkappa: 0
   }
   v: 12.499705530579327
@@ -649,12 +649,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587514.44069322722
-    y: 4141513.0344912433
-    theta: 2.9052193975882106
-    kappa: 0.00067233703476637984
+    x: 587514.39560999349
+    y: 4141512.8360795951
+    theta: 2.9096925454051563
+    kappa: 0.00081034611285234562
     s: 56.02244126981234
-    dkappa: 0.00026373113994242723
+    dkappa: 0.00031385003345606036
     ddkappa: 0
   }
   v: 12.4997107726387
@@ -663,12 +663,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587513.22532817593
-    y: 4141513.3265823293
-    theta: 2.9062603965156972
-    kappa: 0.000975051088307274
+    x: 587513.178925751
+    y: 4141513.1226258958
+    theta: 2.9108908195569705
+    kappa: 0.0010885384878553711
     s: 57.272413143993596
-    dkappa: 0.00056552076296314526
+    dkappa: 0.00062094258051923965
     ddkappa: 0
   }
   v: 12.4997285135585
@@ -677,12 +677,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587512.009618137
-    y: 4141513.6172436033
-    theta: 2.9076578851399213
-    kappa: 0.0012636011293737065
+    x: 587511.96186434175
+    y: 4141513.407575557
+    theta: 2.9124128148336674
+    kappa: 0.0013470766660936692
     s: 58.522387279908941
-    dkappa: 0.00078422406476482277
+    dkappa: 0.0008259910143982739
     ddkappa: 0
   }
   v: 12.499755484980003
@@ -691,12 +691,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587510.79347210354
-    y: 4141513.9060882954
-    theta: 2.9093893782281484
-    kappa: 0.0015306038275528424
+    x: 587510.74434653367
+    y: 4141513.6905821897
+    theta: 2.9142296221488215
+    kappa: 0.0015786026842011301
     s: 59.772364448383613
-    dkappa: 0.00060356970982674012
+    dkappa: 0.00060273463927237414
     ddkappa: 0
   }
   v: 12.499788687176704
@@ -705,12 +705,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587509.576724605
-    y: 4141514.192403554
-    theta: 2.9114674803400615
-    kappa: 0.001773448134709108
+    x: 587509.52622247732
+    y: 4141513.9709830768
+    theta: 2.9163419572370177
+    kappa: 0.0017844203400173642
     s: 61.022345133701826
-    dkappa: 0.0010682077836120872
+    dkappa: 0.0010810389680784673
     ddkappa: 0
   }
   v: 12.499825389054324
@@ -719,12 +719,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587508.35934693448
-    y: 4141514.4760429831
-    theta: 2.9138326276202582
-    kappa: 0.001995311816367069
+    x: 587508.30747653462
+    y: 4141514.2486844743
+    theta: 2.9186981761687925
+    kappa: 0.0019722726514946123
     s: 62.272329560469956
-    dkappa: 0.0010159139251734951
+    dkappa: 0.000994540931228775
     ddkappa: 0
   }
   v: 12.499863128150809
@@ -733,12 +733,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587507.14128608082
-    y: 4141514.7567500486
-    theta: 2.9164494209150282
-    kappa: 0.0021892641952431617
+    x: 587507.08806943614
+    y: 4141514.5234845597
+    theta: 2.9212669994827074
+    kappa: 0.0021352495287707109
     s: 63.5223177204798
-    dkappa: 0.0010377347035379408
+    dkappa: 0.000998027727494191
     ddkappa: 0
   }
   v: 12.499899710636329
@@ -747,12 +747,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587505.92249118013
-    y: 4141515.0342689753
-    theta: 2.9192798631920174
-    kappa: 0.0023482639034620957
+    x: 587505.86796090018
+    y: 4141514.7951700324
+    theta: 2.9240139233393556
+    kappa: 0.0022662286470530096
     s: 64.772309399571782
-    dkappa: 0.001400420982204664
+    dkappa: 0.0013725723620190964
     ddkappa: 0
   }
   v: 12.499933211313278
@@ -761,12 +761,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587504.7028224516
-    y: 4141515.3079357496
-    theta: 2.9222933490315466
-    kappa: 0.0024594446123482588
+    x: 587504.64702726936
+    y: 4141515.0631373986
+    theta: 2.9269080449458471
+    kappa: 0.0023526510763354916
     s: 66.02230420449817
-    dkappa: 0.0011030934952413641
+    dkappa: 0.0010316320157718272
     ddkappa: 0
   }
   v: 12.499961973616275
@@ -775,12 +775,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587503.48231947527
-    y: 4141515.5778685217
-    theta: 2.9254130405761525
-    kappa: 0.00251901419586662
+    x: 587503.42531156307
+    y: 4141515.3275275757
+    theta: 2.9298794014508016
+    kappa: 0.0023902862248799993
     s: 67.272301589786309
-    dkappa: 0.0012943365460944248
+    dkappa: 0.0012294312343620146
     ddkappa: 0
   }
   v: 12.499984609612161
@@ -789,12 +789,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587502.26100035221
-    y: 4141515.8440926489
-    theta: 2.9285709079490654
-    kappa: 0.0025222919753905173
+    x: 587502.20283711283
+    y: 4141515.5883956971
+    theta: 2.9328642181874738
+    kappa: 0.0023743202927889754
     s: 68.522300884601833
-    dkappa: 0.0013304884436973752
+    dkappa: 0.0012615278182527569
     ddkappa: 0
   }
   v: 12.5
@@ -803,12 +803,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587501.03887822106
-    y: 4141516.1066125091
-    theta: 2.9316976525312466
-    kappa: 0.0024639858770884996
+    x: 587500.97962153715
+    y: 4141515.8457729821
+    theta: 2.9357963295979332
+    kappa: 0.0022994016163527592
     s: 69.772301319611927
-    dkappa: 0.0010238845500752331
+    dkappa: 0.00092014278775952416
     ddkappa: 0
   }
   v: 12.500007294111086
@@ -817,12 +817,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587499.81590514688
-    y: 4141516.3651410048
-    theta: 2.9346982217402626
-    kappa: 0.0023317768186717413
+    x: 587499.75562642585
+    y: 4141516.0994190578
+    theta: 2.9385817615951173
+    kappa: 0.0021533450102313922
     s: 71.022302053848463
-    dkappa: 0.0012785326707627615
+    dkappa: 0.0012007913890517729
     ddkappa: 0
   }
   v: 12.500005909908932
@@ -831,12 +831,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587498.59217863018
-    y: 4141516.6200763881
-    theta: 2.9374949580845127
-    kappa: 0.002159117102867729
+    x: 587498.53094972
+    y: 4141516.3497505891
+    theta: 2.9411477624033653
+    kappa: 0.0019695801461263466
     s: 72.2723022015713
-    dkappa: 0.00100578225567964
+    dkappa: 0.00090729614138768751
     ddkappa: 0
   }
   v: 12.499995533989274
@@ -845,12 +845,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587497.36779280659
-    y: 4141516.8718188806
-    theta: 2.9401106156487868
-    kappa: 0.002040881433704411
+    x: 587497.30568218825
+    y: 4141516.5971666547
+    theta: 2.9435209028211884
+    kappa: 0.0018428613475230742
     s: 73.5223008591315
-    dkappa: 0.00093075990485011126
+    dkappa: 0.0008220409188460624
     ddkappa: 0
   }
   v: 12.49997612158008
@@ -859,12 +859,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587496.14279653865
-    y: 4141517.1205628468
-    theta: 2.942610106001939
-    kappa: 0.0019526795421479387
+    x: 587496.07987297431
+    y: 4141516.8418734944
+    theta: 2.9457696408747474
+    kappa: 0.0017493792211714034
     s: 74.77229713183452
-    dkappa: 0.0011375321425651952
+    dkappa: 0.0010556185816209915
     ddkappa: 0
   }
   v: 12.499947896541531
@@ -873,12 +873,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587494.9171685474
-    y: 4141517.36615897
-    theta: 2.9450024537094581
-    kappa: 0.0018778215528280073
+    x: 587494.85350523028
+    y: 4141517.0837491183
+    theta: 2.9479060434421127
+    kappa: 0.0016725009046929212
     s: 76.022290160803436
-    dkappa: 0.00076600308791670681
+    dkappa: 0.00065114412819725358
     ddkappa: 0
   }
   v: 12.499911351366043
@@ -887,12 +887,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587493.69095133641
-    y: 4141517.6087753391
-    theta: 2.947306409214955
-    kappa: 0.001816060299063442
+    x: 587493.62662201957
+    y: 4141517.3229753077
+    theta: 2.9499537822042505
+    kappa: 0.001611781181598119
     s: 77.272279149842163
-    dkappa: 0.00094348420592033511
+    dkappa: 0.00084357019167947468
     ddkappa: 0
   }
   v: 12.499867247178246
@@ -901,12 +901,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587492.46421733568
-    y: 4141517.8487400515
-    theta: 2.9495443681684166
-    kappa: 0.0017664669798467389
+    x: 587492.39928742952
+    y: 4141517.5598496282
+    theta: 2.9519385759139718
+    kappa: 0.0015661539946950445
     s: 78.52226339229874
-    dkappa: 0.0009350601412592372
+    dkappa: 0.00084633190748031883
     ddkappa: 0
   }
   v: 12.499816613735003
@@ -915,12 +915,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587491.2369791636
-    y: 4141518.0860851849
-    theta: 2.9517294842981783
-    kappa: 0.0017290164618868496
+    x: 587491.17151168978
+    y: 4141517.794398563
+    theta: 2.9538769789183279
+    kappa: 0.0015355976994337992
     s: 79.772242297928472
-    dkappa: 0.00068679893958679017
+    dkappa: 0.00057082028734299953
     ddkappa: 0
   }
   v: 12.499760749425397
@@ -929,12 +929,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587490.00921287655
-    y: 4141518.3206525566
-    theta: 2.9538722117549723
-    kappa: 0.0017003512823620802
+    x: 587489.94327045756
+    y: 4141518.0264665997
+    theta: 2.9557833977457642
+    kappa: 0.0015158221964148876
     s: 81.02221541975716
-    dkappa: 0.0010052275196141998
+    dkappa: 0.00092703673432617463
     ddkappa: 0
   }
   v: 12.499701221270731
@@ -943,12 +943,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587488.78094222862
-    y: 4141518.5525315967
-    theta: 2.9559803984933031
-    kappa: 0.0016749064107448506
+    x: 587488.71458736318
+    y: 4141518.2561505353
+    theta: 2.9576672098193737
+    kappa: 0.0014997473478207538
     s: 82.2721824809444
-    dkappa: 0.00077948425218743391
+    dkappa: 0.00069364094329466388
     ddkappa: 0
   }
   v: 12.499639864924543
@@ -957,12 +957,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587487.55221098033
-    y: 4141518.7819237616
-    theta: 2.9580591521264221
-    kappa: 0.001651939800969889
+    x: 587487.48549599352
+    y: 4141518.4836057173
+    theta: 2.9595331016373674
+    kappa: 0.0014865526589564692
     s: 83.52214340164673
-    dkappa: 0.00076454240234466453
+    dkappa: 0.00067308695059010884
     ddkappa: 0
   }
   v: 12.499578784672584
@@ -971,12 +971,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587486.32303486113
-    y: 4141519.0088873971
-    theta: 2.96011191186676
-    kappa: 0.0016311635241879369
+    x: 587486.25600841583
+    y: 4141518.7088764445
+    theta: 2.9613849612892946
+    kappa: 0.0014759493866383546
     s: 84.772098325880847
-    dkappa: 0.0009757189044646761
+    dkappa: 0.00092314468606841951
     ddkappa: 0
   }
   v: 12.499520353432835
@@ -985,12 +985,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587485.09338371223
-    y: 4141519.2332321047
-    theta: 2.9621388352971421
-    kappa: 0.0016124916093694058
+    x: 587485.02609141846
+    y: 4141518.93175942
+    theta: 2.9632244285480849
+    kappa: 0.0014678989285451481
     s: 86.022047648386888
-    dkappa: 0.00063827705712970269
+    dkappa: 0.000557077799547633
     ddkappa: 0
   }
   v: 12.499467212755501
@@ -999,12 +999,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587483.86327221664
-    y: 4141519.4550108733
-    theta: 2.9641429224658467
-    kappa: 0.0015955661320903427
+    x: 587483.79576144647
+    y: 4141519.1523232455
+    theta: 2.9650551904878695
+    kappa: 0.0014620073458453862
     s: 87.271992041491615
-    dkappa: 0.00084946419041272343
+    dkappa: 0.00078481753400862935
     ddkappa: 0
   }
   v: 12.499422272823008
@@ -1013,12 +1013,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587482.63274087454
-    y: 4141519.67442548
-    theta: 2.9661273767905989
-    kappa: 0.0015799859022366464
+    x: 587482.5650490463
+    y: 4141519.3707203432
+    theta: 2.9668798228261632
+    kappa: 0.0014578547567934389
     s: 88.521932481971646
-    dkappa: 0.00083915393245421023
+    dkappa: 0.00079278352887543653
     ddkappa: 0
   }
   v: 12.499388712450008
@@ -1027,12 +1027,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587481.40179694421
-    y: 4141519.8914984716
-    theta: 2.9680936730096028
-    kappa: 0.001565469182961397
+    x: 587481.33395771123
+    y: 4141519.5869561052
+    theta: 2.96870022931244
+    kappa: 0.0014551599466224919
     s: 89.771870277916662
-    dkappa: 0.00061895010827224442
+    dkappa: 0.00054241308446815247
     ddkappa: 0
   }
   v: 12.499369979083379
@@ -1041,12 +1041,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587480.17041887122
-    y: 4141520.1060895566
-    theta: 2.9700419875115562
-    kappa: 0.0015517521230235354
+    x: 587480.10246104817
+    y: 4141519.8008657992
+    theta: 2.970518231048378
+    kappa: 0.0014536956085005489
     s: 91.021807095592678
-    dkappa: 0.00093834426765909544
+    dkappa: 0.00090275838378514812
     ddkappa: 0
   }
   v: 12.499369788802221
@@ -1055,12 +1055,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587478.93860535556
-    y: 4141520.31817314
-    theta: 2.971972520801152
-    kappa: 0.0015385443063062007
+    x: 587478.87056095025
+    y: 4141520.0124460505
+    theta: 2.9723347285187405
+    kappa: 0.0014531538445937244
     s: 92.27174498630518
-    dkappa: 0.00070861194784888428
+    dkappa: 0.00066987484814337215
     ddkappa: 0
   }
   v: 12.499392126317856
@@ -1069,12 +1069,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587477.70639994484
-    y: 4141520.5279884036
-    theta: 2.9738874326083753
-    kappa: 0.0015254889498434493
+    x: 587477.63829113648
+    y: 4141520.2218829608
+    theta: 2.9741510347534312
+    kappa: 0.0014531542649393393
     s: 93.521686413262444
-    dkappa: 0.00070977233423094089
+    dkappa: 0.000663245624417016
     ddkappa: 0
   }
   v: 12.499441244973836
@@ -1083,12 +1083,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587476.47380685131
-    y: 4141520.7355533019
-    theta: 2.9757867252686676
-    kappa: 0.001512325799557637
+    x: 587476.40565263131
+    y: 4141520.4291782109
+    theta: 2.9759676313082437
+    kappa: 0.0014534338457160259
     s: 94.771634278438654
-    dkappa: 0.00091077786874148975
+    dkappa: 0.00090957825044106261
     ddkappa: 0
   }
   v: 12.499521666745931
@@ -1097,12 +1097,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587475.24079836044
-    y: 4141520.9406960057
-    theta: 2.9776685949715391
-    kappa: 0.0014987656376843331
+    x: 587475.17261082167
+    y: 4141520.6341206725
+    theta: 2.9777844259851829
+    kappa: 0.0014537305841402042
     s: 96.021591949437266
-    dkappa: 0.00057956186792566583
+    dkappa: 0.0005498741675779912
     ddkappa: 0
   }
   v: 12.499638182242139
@@ -1111,12 +1111,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587474.00737879868
-    y: 4141521.1434367453
-    theta: 2.9795324758021517
-    kappa: 0.0014844729257593565
+    x: 587473.93917420588
+    y: 4141520.8367576683
+    theta: 2.979601639349942
+    kappa: 0.0014537160210635336
     s: 97.27156328635408
-    dkappa: 0.00079827420605367149
+    dkappa: 0.00078396040736025837
     ddkappa: 0
   }
   v: 12.499795850702681
@@ -1125,12 +1125,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587472.77358113287
-    y: 4141521.3439758169
-    theta: 2.9813785917610045
-    kappa: 0.0014692592709396398
+    x: 587472.70536699682
+    y: 4141521.0372380856
+    theta: 2.9814184651029638
+    kappa: 0.0014531657245650939
     s: 98.521552668640538
-    dkappa: 0.000779472312779815
+    dkappa: 0.0007888363919012415
     ddkappa: 0
   }
   v: 12.500000000000002
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_follow_03_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_follow_03_0.pb.txt
index 07b7d1b63ef5f02066aa86d20252f63da9cbe3c6..801456953222282ce13d8f4155a1fa25e37154cc 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_follow_03_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_follow_03_0.pb.txt
@@ -10,7 +10,7 @@ trajectory_point {
     theta: -1.827644940261357
     kappa: 0.0012502171579704896
     s: 6.9388939039072284e-18
-    dkappa: 2.2718723152798832e-20
+    dkappa: 2.2714845851057969e-20
     ddkappa: 0
   }
   v: 4.5999999046325684
@@ -19,12 +19,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.48809757736
-    y: 4140954.804695786
-    theta: -1.8266021830601393
-    kappa: 0.0021607046102455093
+    x: 587260.48809753591
+    y: 4140954.8046957962
+    theta: -1.8266023110345211
+    kappa: 0.0021604520974907292
     s: 0.45353828391573847
-    dkappa: 0.0014849356185825458
+    dkappa: 0.0014846821913357647
     ddkappa: 0
   }
   v: 4.4717664540689475
@@ -33,12 +33,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.37648272607
-    y: 4140954.3780453941
-    theta: -1.8255882337775911
-    kappa: 0.0030460381133439649
+    x: 587260.37648264435
+    y: 4140954.3780454155
+    theta: -1.8255884861909049
+    kappa: 0.0030455400639799674
     s: 0.89454670933124525
-    dkappa: 0.00292884706380942
+    dkappa: 0.0029283472107260026
     ddkappa: 0
   }
   v: 4.3493362385936312
@@ -47,12 +47,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.26890223986
-    y: 4140953.9627132276
-    theta: -1.8240473113930702
-    kappa: 0.0037088094130395305
+    x: 587260.26890194532
+    y: 4140953.962713304
+    theta: -1.8240478569455043
+    kappa: 0.0037080914154542736
     s: 1.3235859907273784
-    dkappa: 0.002646971486453356
+    dkappa: 0.0026464343811948158
     ddkappa: 0
   }
   v: 4.2323204754703543
@@ -61,12 +61,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.164496358
-    y: 4140953.5583825223
-    theta: -1.8223795214125853
-    kappa: 0.0042937559950741279
+    x: 587260.16449580365
+    y: 4140953.5583826657
+    theta: -1.8223804044154064
+    kappa: 0.0042928296507904994
     s: 1.7411789836583234
-    dkappa: 0.0018616875070318986
+    dkappa: 0.0018611754891467366
     ddkappa: 0
   }
   v: 4.1203507689019272
@@ -75,12 +75,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.06351693324
-    y: 4140953.1644874332
-    theta: -1.8205841841002242
-    kappa: 0.0047729385804845545
+    x: 587260.063515969
+    y: 4140953.1644876814
+    theta: -1.8205854683466081
+    kappa: 0.0047718183767082147
     s: 2.1478127234455
-    dkappa: 0.0018095868178942803
+    dkappa: 0.0018090118576516096
     ddkappa: 0
   }
   v: 4.0130791100302332
@@ -89,12 +89,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.96622950886
-    y: 4140952.780492194
-    theta: -1.8185649226684886
-    kappa: 0.0050970910986487521
+    x: 587259.96622789744
+    y: 4140952.7804926061
+    theta: -1.8185667124374738
+    kappa: 0.0050957962817930313
     s: 2.5439404638714715
-    dkappa: 0.0028831040606846275
+    dkappa: 0.0028823298754558631
     ddkappa: 0
   }
   v: 3.9101778769362303
@@ -103,12 +103,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.8714187952
-    y: 4140952.4062725781
-    theta: -1.8165970689220645
-    kappa: 0.0054129914496877938
+    x: 587259.871416553
+    y: 4140952.40627315
+    theta: -1.8165993513427243
+    kappa: 0.0054115264649944594
     s: 2.9299837158738491
-    dkappa: 0.0039292920581264759
+    dkappa: 0.0039283237197151322
     ddkappa: 0
   }
   v: 3.8113398346399503
@@ -117,12 +117,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.78066767764
-    y: 4140952.0410282305
-    theta: -1.814507130774196
-    kappa: 0.0055445812939350159
+    x: 587259.78066436353
+    y: 4140952.0410290686
+    theta: -1.8145100237103233
+    kappa: 0.0055429689854655378
     s: 3.3063342862392009
-    dkappa: 0.0033077223522296191
+    dkappa: 0.0033067629172804222
     ddkappa: 0
   }
   v: 3.7162781351004988
@@ -131,12 +131,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.69246827567
-    y: 4140951.6847614259
-    theta: -1.8124381138521364
-    kappa: 0.00564115677390621
+    x: 587259.69246383384
+    y: 4140951.6847625431
+    theta: -1.8124416256303966
+    kappa: 0.0056394041434312484
     s: 3.6733563162969611
-    dkappa: 0.0024060496745459754
+    dkappa: 0.0024051346628058516
     ddkappa: 0
   }
   v: 3.624726317216056
@@ -145,12 +145,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.60668606614
-    y: 4140951.3371583992
-    theta: -1.8104196632761993
-    kappa: 0.0057114798525346767
+    x: 587259.60668043967
+    y: 4140951.3371598083
+    theta: -1.8104237951876496
+    kappa: 0.0057095930878493621
     s: 4.031388320613333
-    dkappa: 0.0017453813570741633
+    dkappa: 0.0017444831478999761
     ddkappa: 0
   }
   v: 3.5364383068238761
@@ -159,12 +159,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.52464788919
-    y: 4140950.9975704364
-    theta: -1.8084493292178938
-    kappa: 0.0056251703400116808
+    x: 587259.52464055642
+    y: 4140950.9975722572
+    theta: -1.8084541729780672
+    kappa: 0.0056231705648859872
     s: 4.3807452256852013
-    dkappa: 0.0025206131184768218
+    dkappa: 0.0025195589141194913
     ddkappa: 0
   }
   v: 3.4511884167002869
@@ -173,12 +173,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.44457796076
-    y: 4140950.66612982
-    theta: -1.8065262627241583
-    kappa: 0.0055409315532709436
+    x: 587259.44456896279
+    y: 4140950.666132044
+    theta: -1.8065318012592633
+    kappa: 0.0055388214790355883
     s: 4.7217204086340363
-    dkappa: 0.0032772456276228511
+    dkappa: 0.0032760391706970469
     ddkappa: 0
   }
   v: 3.368771346560691
@@ -187,12 +187,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.366801542
-    y: 4140950.342477343
-    theta: -1.8046846931392282
-    kappa: 0.00542406561473123
+    x: 587259.36679076042
+    y: 4140950.3424799941
+    theta: -1.8046909316159456
+    kappa: 0.0054218522563312275
     s: 5.0545877358998013
-    dkappa: 0.0036330256092285316
+    dkappa: 0.0036317172478011004
     ddkappa: 0
   }
   v: 3.2890021830595635
@@ -201,12 +201,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.29218719259
-    y: 4140950.026142058
-    theta: -1.8030084193394853
-    kappa: 0.0051919482567106476
+    x: 587259.29217413219
+    y: 4140950.0261452473
+    theta: -1.8030154152088655
+    kappa: 0.0051896489966259483
     s: 5.3796036019348623
-    dkappa: 0.0026757539351812921
+    dkappa: 0.0026745050745154405
     ddkappa: 0
   }
   v: 3.2117163997904559
@@ -215,12 +215,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.21931999642
-    y: 4140949.7172140116
-    theta: -1.8013713939638607
-    kappa: 0.0049652661077199695
+    x: 587259.21930471051
+    y: 4140949.7172177252
+    theta: -1.8013791294948032
+    kappa: 0.004962882957405536
     s: 5.6970089678978928
-    dkappa: 0.0017408975242200206
+    dkappa: 0.0017397067710589941
     ddkappa: 0
   }
   v: 3.1367698572859917
@@ -229,12 +229,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.1482881686
-    y: 4140949.4154389324
-    theta: -1.7998007865894363
-    kappa: 0.0047309010150575021
+    x: 587259.1482706354
+    y: 4140949.4154431745
+    theta: -1.79980925169107
+    kappa: 0.0047284377715075291
     s: 6.0070314003477847
-    dkappa: 0.00097388537701732748
+    dkappa: 0.00097273354747365033
     ddkappa: 0
   }
   v: 3.06403880301787
@@ -243,12 +243,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.08014745009
-    y: 4140949.1203483981
-    theta: -1.7985184660735591
-    kappa: 0.0043867401527766577
+    x: 587259.08012707043
+    y: 4140949.1203532983
+    theta: -1.7985277077062172
+    kappa: 0.0043842148550603159
     s: 6.30988710993755
-    dkappa: 0.00152385473787468
+    dkappa: 0.0015225848992175928
     ddkappa: 0
   }
   v: 2.9934198713968629
@@ -257,12 +257,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.01357265143
-    y: 4140948.8320392445
-    theta: -1.7972656119093202
-    kappa: 0.0040504883403117906
+    x: 587259.01354949072
+    y: 4140948.8320447863
+    theta: -1.7972756122318647
+    kappa: 0.0040479024144758693
     s: 6.605782990108235
-    dkappa: 0.0020611854301645057
+    dkappa: 0.002059800294323396
     ddkappa: 0
   }
   v: 2.9248300837728163
@@ -271,12 +271,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.94851886039
-    y: 4140948.5503169741
-    theta: -1.7960413828605801
-    kappa: 0.0037219187381421557
+    x: 587258.94849298231
+    y: 4140948.5503231436
+    theta: -1.7960521245365144
+    kappa: 0.0037192735693329865
     s: 6.8949186557828224
-    dkappa: 0.0025862399436406207
+    dkappa: 0.0025847421447640091
     ddkappa: 0
   }
   v: 2.8582068484346523
@@ -285,12 +285,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.88565921248
-    y: 4140948.274828014
-    theta: -1.7951157666047093
-    kappa: 0.0033232906535010892
+    x: 587258.88563014718
+    y: 4140948.2748349095
+    theta: -1.7951272684541659
+    kappa: 0.00332059982186228
     s: 7.1774884820601388
-    dkappa: 0.0020408405641975822
+    dkappa: 0.0020393636143668935
     ddkappa: 0
   }
   v: 2.7935079606103637
@@ -299,12 +299,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.82448201
-    y: 4140948.0054935692
-    theta: -1.7943108854277272
-    kappa: 0.0029050722444140357
+    x: 587258.82444963418
+    y: 4140948.0055012167
+    theta: -1.7943231436701588
+    kappa: 0.0029023412290523883
     s: 7.4536835097449465
-    dkappa: 0.00111743986098962
+    dkappa: 0.0011160307245563761
     ddkappa: 0
   }
   v: 2.7307060988600158
@@ -313,12 +313,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.76467518392
-    y: 4140947.7421922479
-    theta: -1.7935240336164218
-    kappa: 0.002496221977441977
+    x: 587258.76463957177
+    y: 4140947.7422006307
+    theta: -1.793537031308728
+    kappa: 0.0024934516784776632
     s: 7.7236917359533335
-    dkappa: 0.00021472344338166683
+    dkappa: 0.00021338060131769859
     ddkappa: 0
   }
   v: 2.6697652320939582
@@ -327,12 +327,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.70623743767
-    y: 4140947.4847356239
-    theta: -1.7927869933561915
-    kappa: 0.0020899752066694849
+    x: 587258.70619860827
+    y: 4140947.4847447374
+    theta: -1.7928007159502466
+    kappa: 0.0020871677028825937
     s: 7.9876972361616065
-    dkappa: -0.000574639552494644
+    dkappa: -0.00057592968294339182
     ddkappa: 0
   }
   v: 2.6106447366124983
@@ -341,12 +341,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.64957884513
-    y: 4140947.2328464943
-    theta: -1.7924675890769701
-    kappa: 0.0016121344679230567
+    x: 587258.64953621663
+    y: 4140947.2328564622
+    theta: -1.7924820485718438
+    kappa: 0.0016093046541092787
     s: 8.2458799804989376
-    dkappa: -0.00018801536528793096
+    dkappa: -0.00018939143343756618
     ddkappa: 0
   }
   v: 2.5533031291727122
@@ -355,12 +355,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.5941594902
-    y: 4140946.9864666886
-    theta: -1.7921551707588104
-    kappa: 0.0011447450693269632
+    x: 587258.59411314584
+    y: 4140946.9864774924
+    theta: -1.7921703510373719
+    kappa: 0.0011418934334515578
     s: 8.4984157477930484
-    dkappa: 0.00019015257161780378
+    dkappa: 0.00018869244539802604
     ddkappa: 0
   }
   v: 2.4976980669884452
@@ -369,12 +369,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.5399417365
-    y: 4140946.7454288844
-    theta: -1.7918495262954093
-    kappa: 0.00068748959835217261
+    x: 587258.53989175684
+    y: 4140946.7454405064
+    theta: -1.7918654117295922
+    kappa: 0.00068461661356064465
     s: 8.74547603961588
-    dkappa: 0.00056012107885566127
+    dkappa: 0.00055857871711114163
     ddkappa: 0
   }
   v: 2.4437863477303119
@@ -383,12 +383,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.48689464456
-    y: 4140946.509568728
-    theta: -1.7915957726842375
-    kappa: 0.00023327960659931985
+    x: 587258.48684104451
+    y: 4140946.509581164
+    theta: -1.7916123489049223
+    kappa: 0.00023038711740877528
     s: 8.9872279943292757
-    dkappa: 0.00080609540638904389
+    dkappa: 0.000804488219583999
     ddkappa: 0
   }
   v: 2.3915239095256964
@@ -397,12 +397,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.43503140879
-    y: 4140946.2787165046
-    theta: -1.7917798646441954
-    kappa: -0.00027594373039717922
+    x: 587258.43497366272
+    y: 4140946.2787298718
+    theta: -1.7917971296592325
+    kappa: -0.00027884287112988379
     s: 9.2238343011306565
-    dkappa: -6.0379297845513714e-05
+    dkappa: -6.19141041118173e-05
     ddkappa: 0
   }
   v: 2.3408658309587507
@@ -411,12 +411,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.38426141324
-    y: 4140946.0527304821
-    theta: -1.7919600760879633
-    kappa: -0.00077443299911207544
+    x: 587258.38419960882
+    y: 4140946.052744762
+    theta: -1.7919780153781932
+    kappa: -0.00077733865117712918
     s: 9.4554531140986953
-    dkappa: -0.00090858932730221583
+    dkappa: -0.00091005327876041262
     ddkappa: 0
   }
   v: 2.2917663310703982
@@ -425,12 +425,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.33455100446
-    y: 4140945.8314608634
-    theta: -1.7921365264600608
-    kappa: -0.0012625186327891332
+    x: 587258.33448522608
+    y: 4140945.8314760355
+    theta: -1.7921551259531436
+    kappa: -0.0012654306602928875
     s: 9.6822379662389935
-    dkappa: -0.00173909693438852
+    dkappa: -0.0017404915098017662
     ddkappa: 0
   }
   v: 2.2441787693583293
@@ -439,12 +439,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.28586756
-    y: 4140945.6147624403
-    theta: -1.7923093315441347
-    kappa: -0.0017405209342228288
+    x: 587258.28579788981
+    y: 4140945.6147784865
+    theta: -1.7923285776008635
+    kappa: -0.0017434392054554734
     s: 9.9043376835297678
-    dkappa: -0.0025524471339318038
+    dkappa: -0.00255377376653571
     ddkappa: 0
   }
   v: 2.1980556457770053
@@ -453,12 +453,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.23788272345
-    y: 4140945.4025616539
-    theta: -1.7928440450065741
-    kappa: -0.00225141261313951
+    x: 587258.23780878773
+    y: 4140945.4025786645
+    theta: -1.7928639238094657
+    kappa: -0.0022543277982120281
     s: 10.12189629896751
-    dkappa: -0.0025606626370458145
+    dkappa: -0.0025620173305896114
     ddkappa: 0
   }
   v: 2.153348600737655
@@ -467,12 +467,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.190765636
-    y: 4140945.1946776835
-    theta: -1.7934949482486739
-    kappa: -0.0027667945221570289
+    x: 587258.1906873628
+    y: 4140945.1946956776
+    theta: -1.7935154474075101
+    kappa: -0.0027697035574362769
     s: 10.335052966612679
-    dkappa: -0.0022946694834574742
+    dkappa: -0.0022960832199288152
     ddkappa: 0
   }
   v: 2.1100084151082781
@@ -481,12 +481,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.14459191286
-    y: 4140944.9909559037
-    theta: -1.7941328196186026
-    kappa: -0.0032718576097631588
+    x: 587258.14450938907
+    y: 4140944.9909748612
+    theta: -1.7941539267137097
+    kappa: -0.0032747606183784455
     s: 10.543941875635372
-    dkappa: -0.0020340019647051774
+    dkappa: -0.0020354735619643439
     ddkappa: 0
   }
   v: 2.067985010213643
@@ -495,12 +495,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.09933300852
-    y: 4140944.79127037
-    theta: -1.7947580529354452
-    kappa: -0.0037669141137002646
+    x: 587258.09924631822
+    y: 4140944.791290272
+    theta: -1.7947797559214886
+    kappa: -0.0037698112150551231
     s: 10.748692164360998
-    dkappa: -0.0017784989321333199
+    dkappa: -0.0017800272438110187
     ddkappa: 0
   }
   v: 2.0272274478352865
@@ -509,12 +509,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.054961513
-    y: 4140944.5955001484
-    theta: -1.7953710263921805
-    kappa: -0.0042522638516757014
+    x: 587258.05487073807
+    y: 4140944.5955209769
+    theta: -1.7953933135833868
+    kappa: -0.0042551551615960916
     s: 10.949427834315962
-    dkappa: -0.0015280056471760229
+    dkappa: -0.0015295895612505294
     ddkappa: 0
   }
   v: 1.987683930211515
@@ -523,12 +523,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.01066664094
-    y: 4140944.403708979
-    theta: -1.7964471905714667
-    kappa: -0.0047731921626386559
+    x: 587258.01057133719
+    y: 4140944.4037308525
+    theta: -1.7964700398951603
+    kappa: -0.0047760674584934396
     s: 11.146267664273338
-    dkappa: -0.0022534814779008017
+    dkappa: -0.0022550003383796187
     ddkappa: 0
   }
   v: 1.9493018000374049
@@ -537,12 +537,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.967187147
-    y: 4140944.215611354
-    theta: -1.797524340084057
-    kappa: -0.00528616216453194
+    x: 587257.96708737756
+    y: 4140944.2156342603
+    theta: -1.7975477401999775
+    kappa: -0.0052890212776008395
     s: 11.339325124298544
-    dkappa: -0.0030092920721679424
+    dkappa: -0.0030107415642090262
     ddkappa: 0
   }
   v: 1.9120275404648008
@@ -551,12 +551,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.92453516042
-    y: 4140944.0310936272
-    theta: -1.7985809906408308
-    kappa: -0.0057893692550561784
+    x: 587257.92443101038
+    y: 4140944.0311175464
+    theta: -1.7986049310685168
+    kappa: -0.0057922124933320659
     s: 11.528708289795018
-    dkappa: -0.0037507179815287881
+    dkappa: -0.0037520994253612103
     ddkappa: 0
   }
   v: 1.8758067751023169
@@ -565,12 +565,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.88268757518
-    y: 4140943.85005584
-    theta: -1.7996177122839845
-    kappa: -0.0062830860372392319
+    x: 587257.88257912733
+    y: 4140943.8500807527
+    theta: -1.7996421828320286
+    kappa: -0.006285913700115108
     s: 11.714519755549905
-    dkappa: -0.004478160859599979
+    dkappa: -0.0044794755385889678
     ddkappa: 0
   }
   v: 1.8405842680153366
@@ -579,12 +579,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.84162241919
-    y: 4140943.6724029374
-    theta: -1.8006350472313186
-    kappa: -0.00676757173975137
+    x: 587257.84150975372
+    y: 4140943.6724288245
+    theta: -1.8006600379834043
+    kappa: -0.0067703841184422872
     s: 11.896857082879581
-    dkappa: -0.0051920026542042006
+    dkappa: -0.00519325181665974
     ddkappa: 0
   }
   v: 1.8063250982620174
@@ -593,12 +593,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.8005086066
-    y: 4140943.4982293169
-    theta: -1.801965101542895
-    kappa: -0.0072693652160724072
+    x: 587257.80039147579
+    y: 4140943.4982562591
+    theta: -1.8019905908943694
+    kappa: -0.0072721564239580545
     s: 12.075818338640316
-    dkappa: -0.0052955237981694913
+    dkappa: -0.0052967760173464927
     ddkappa: 0
   }
   v: 1.7730722051637569
@@ -607,12 +607,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.75976079935
-    y: 4140943.327331692
-    theta: -1.8034290016330348
-    kappa: -0.007774516834023578
+    x: 587257.75963913463
+    y: 4140943.3273597155
+    theta: -1.8034549725327678
+    kappa: -0.00777728413823584
     s: 12.251506647813205
-    dkappa: -0.0051123221955346829
+    dkappa: -0.0051136135706300774
     ddkappa: 0
   }
   v: 1.740872402606467
@@ -621,12 +621,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.71974730864
-    y: 4140943.1595138144
-    theta: -1.8048665278517864
-    kappa: -0.0082705651120041634
+    x: 587257.71962119185
+    y: 4140943.1595428991
+    theta: -1.8048929716287467
+    kappa: -0.008273308943311266
     s: 12.424028877040147
-    dkappa: -0.0049324220699469134
+    dkappa: -0.0049337518953316891
     ddkappa: 0
   }
   v: 1.7097537099201829
@@ -635,12 +635,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.68044291646
-    y: 4140942.99466992
-    theta: -1.8062785800075396
-    kappa: -0.0087578226749878713
+    x: 587257.68031242653
+    y: 4140942.9947000472
+    theta: -1.8063054882826335
+    kappa: -0.0087605434493646869
     s: 12.593493755168252
-    dkappa: -0.0047557100427804968
+    dkappa: -0.0047570776370582032
     ddkappa: 0
   }
   v: 1.6797253518790616
@@ -649,12 +649,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.64182240935
-    y: 4140942.8326942604
-    theta: -1.8076660583896798
-    kappa: -0.0092366020983488638
+    x: 587257.64168762241
+    y: 4140942.8327254127
+    theta: -1.8076934230763202
+    kappa: -0.00923930021697926
     s: 12.760009993794245
-    dkappa: -0.0045820727533978419
+    dkappa: -0.0045834774594007482
     ddkappa: 0
   }
   v: 1.6507777587013832
@@ -663,12 +663,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.60386101343
-    y: 4140942.6734829345
-    theta: -1.8090298480460472
-    kappa: -0.0097072105039164042
+    x: 587257.60372200259
+    y: 4140942.6735150944
+    theta: -1.8090576613455114
+    kappa: -0.0097098863534513073
     s: 12.923684407808896
-    dkappa: -0.0044113988189797611
+    dkappa: -0.004412840003346131
     ddkappa: 0
   }
   v: 1.6228825660495498
@@ -677,12 +677,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.56528900855
-    y: 4140942.5172391785
-    theta: -1.8107608110998379
-    kappa: -0.010196498543851807
+    x: 587257.56514550024
+    y: 4140942.5172724491
+    theta: -1.8107890484819147
+    kappa: -0.010199146403140749
     s: 13.084620035941407
-    dkappa: -0.0049241062214452408
+    dkappa: -0.0049255095695221931
     ddkappa: 0
   }
   v: 1.5959926150300865
@@ -691,12 +691,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.52707763645
-    y: 4140942.3636261816
-    theta: -1.8125486750748372
-    kappa: -0.010683563025633221
+    x: 587257.526929647
+    y: 4140942.3636605674
+    theta: -1.8125773203048972
+    kappa: -0.01068618164232616
     s: 13.242914261303852
-    dkappa: -0.0055772187962919786
+    dkappa: -0.0055785590936391332
     ddkappa: 0
   }
   v: 1.5700419521936411
@@ -705,12 +705,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.48948219593
-    y: 4140942.2124892827
-    theta: -1.8143077351464667
-    kappa: -0.011162776483918269
+    x: 587257.48932979722
+    y: 4140942.2125247652
+    theta: -1.8143367816603369
+    kappa: -0.011165366329378479
     s: 13.398656931935568
-    dkappa: -0.006219803807822789
+    dkappa: -0.0062210820707588868
     ddkappa: 0
   }
   v: 1.5449458295349829
@@ -719,12 +719,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.45248327067
-    y: 4140942.0637504263
-    theta: -1.8160388884437859
-    kappa: -0.011634386409139875
+    x: 587257.45232653292
+    y: 4140942.0637869881
+    theta: -1.8160683298767826
+    kappa: -0.011636947939871686
     s: 13.551928481347577
-    dkappa: -0.00685219311992827
+    dkappa: -0.0068534103327342983
     ddkappa: 0
   }
   v: 1.520600704493005
@@ -733,12 +733,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.41606417089
-    y: 4140941.917342518
-    theta: -1.8177429055659875
-    kappa: -0.012098605540631194
+    x: 587257.415903162
+    y: 4140941.917380142
+    theta: -1.8177727357245226
+    kappa: -0.012101139200366397
     s: 13.702798049066997
-    dkappa: -0.0074746719981920251
+    dkappa: -0.007475829117609762
     ddkappa: 0
   }
   v: 1.4968842399507223
@@ -747,12 +747,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.3802113866
-    y: 4140941.7732112464
-    theta: -1.8194204092696613
-    kappa: -0.012555606083622103
+    x: 587257.380046173
+    y: 4140941.7732499167
+    theta: -1.8194506220979467
+    kappa: -0.012558112305753554
     s: 13.851321601181454
-    dkappa: -0.008087471355368513
+    dkappa: -0.0080885693158476869
     ddkappa: 0
   }
   v: 1.4736553042352725
@@ -761,12 +761,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.34432951163
-    y: 4140941.6314665973
-    theta: -1.8212208776136212
-    kappa: -0.013014391139661729
+    x: 587257.34416001209
+    y: 4140941.6315063536
+    theta: -1.8212514675714069
+    kappa: -0.013016868906608543
     s: 13.997540050883494
-    dkappa: -0.0084582856403346181
+    dkappa: -0.0084593357063653882
     ddkappa: 0
   }
   v: 1.4507539711179158
@@ -775,12 +775,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.30777561991
-    y: 4140941.4922481813
-    theta: -1.8233067908098373
-    kappa: -0.013484694126355126
+    x: 587257.307601719
+    y: 4140941.4922890933
+    theta: -1.8233377208683252
+    kappa: -0.013487138937723875
     s: 14.141477379014992
-    dkappa: -0.0083342497113853536
+    dkappa: -0.0083353243616865677
     ddkappa: 0
   }
   v: 1.4280015198140348
@@ -789,12 +789,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.2717997221
-    y: 4140941.3552311016
-    theta: -1.825359759421989
-    kappa: -0.013947560631569815
+    x: 587257.27162148966
+    y: 4140941.3552731508
+    theta: -1.8253910242215503
+    kappa: -0.013949973008457645
     s: 14.283138754611571
-    dkappa: -0.0082121750519036953
+    dkappa: -0.0082132738977462212
     ddkappa: 0
   }
   v: 1.4052004349831346
@@ -803,12 +803,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.23640576052
-    y: 4140941.2204303714
-    theta: -1.8273795397865189
-    kappa: -0.014402939937750825
+    x: 587257.23622326646
+    y: 4140941.22047354
+    theta: -1.8274111339217176
+    kappa: -0.014405320404808826
     s: 14.422508655447011
-    dkappa: -0.0080920750379455536
+    dkappa: -0.0080931976879490847
     ddkappa: 0
   }
   v: 1.3821344067288435
@@ -817,12 +817,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.2016034089
-    y: 4140941.0878828359
-    theta: -1.8293655627984891
-    kappa: -0.014850707579442221
+    x: 587257.20141672448
+    y: 4140941.0879271049
+    theta: -1.8293974807655582
+    kappa: -0.014853056670043155
     s: 14.559548988577658
-    dkappa: -0.0079739824955589959
+    dkappa: -0.0079751285518370168
     ddkappa: 0
   }
   v: 1.3585683305989111
@@ -831,12 +831,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.16740855051
-    y: 4140940.957648987
-    theta: -1.8313169066429185
-    kappa: -0.015290659202325703
+    x: 587257.16721774894
+    y: 4140940.9576943368
+    theta: -1.8313491427827797
+    kappa: -0.015292977464162066
     s: 14.694197210886839
-    dkappa: -0.0078579513203781669
+    dkappa: -0.00785912037436189
     ddkappa: 0
   }
   v: 1.334248307585211
@@ -845,12 +845,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.13384375535
-    y: 4140940.8298147838
-    theta: -1.83323226956357
-    kappa: -0.015722504422259215
+    x: 587257.13364891242
+    y: 4140940.8298611948
+    theta: -1.8332648180004159
+    kappa: -0.015724792423371617
     s: 14.826364449629279
-    dkappa: -0.0077440580972171751
+    dkappa: -0.007745249725158488
     ddkappa: 0
   }
   v: 1.308901644123738
@@ -859,12 +859,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.10083582613
-    y: 4140940.704521385
-    theta: -1.8351320886890015
-    kappa: -0.016147025608795381
+    x: 587257.1006369174
+    y: 4140940.704568868
+    theta: -1.8351649611006158
+    kappa: -0.016149284714721442
     s: 14.955933622975504
-    dkappa: -0.0076665910481400106
+    dkappa: -0.0076678311242247424
     ddkappa: 0
   }
   v: 1.2822368520946106
@@ -873,12 +873,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.066654432
-    y: 4140940.5823905393
-    theta: -1.8373946216638213
-    kappa: -0.016583747964104167
+    x: 587257.06645139586
+    y: 4140940.5824391777
+    theta: -1.8374277608449712
+    kappa: -0.016585973093352333
     s: 15.08275756055626
-    dkappa: -0.00821280007381069
+    dkappa: -0.00821398774081283
     ddkappa: 0
   }
   v: 1.2539436488220685
@@ -887,12 +887,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.03326121089
-    y: 4140940.463075852
-    theta: -1.8396050486702371
-    kappa: -0.017010400141950707
+    x: 587257.03305414226
+    y: 4140940.4631256186
+    theta: -1.8396384484911426
+    kappa: -0.017012592077973371
     s: 15.206657124006922
-    dkappa: -0.00874641431986196
+    dkappa: -0.0087475507862581434
     ddkappa: 0
   }
   v: 1.2236929570744755
@@ -901,12 +901,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.00071356609
-    y: 4140940.3467824273
-    theta: -1.8417595444754602
-    kappa: -0.0174262487187495
+    x: 587257.00050256751
+    y: 4140940.3468332943
+    theta: -1.8417931983493865
+    kappa: -0.017428408302058881
     s: 15.327419327511908
-    dkappa: -0.0092665164927841787
+    dkappa: -0.009267603055065899
     ddkappa: 0
   }
   v: 1.1911369050643166
@@ -915,12 +915,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.96907853044
-    y: 4140940.2337497743
-    theta: -1.8438536494276361
-    kappa: -0.017830437244149976
+    x: 587256.96886371181
+    y: 4140940.2338017104
+    theta: -1.8438875502367937
+    kappa: -0.017832565381889487
     s: 15.444795458349093
-    dkappa: -0.0097720354293867363
+    dkappa: -0.0097730734868242391
     ddkappa: 0
   }
   v: 1.1559088264482003
@@ -929,12 +929,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.93843327241
-    y: 4140940.1242536171
-    theta: -1.8458822360885798
-    kappa: -0.018221979769070005
+    x: 587256.93821475317
+    y: 4140940.1243065894
+    theta: -1.8459163761061304
+    kappa: -0.018224077445089186
     s: 15.558499197434214
-    dkappa: -0.010261738002304021
+    dkappa: -0.010262729072486858
     ddkappa: 0
   }
   v: 1.117623260326857
@@ -943,12 +943,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.908865602
-    y: 4140940.018607704
-    theta: -1.8478394758911683
-    kappa: -0.018599754373729481
+    x: 587256.90864351252
+    y: 4140940.0186616755
+    theta: -1.8478738466993427
+    kappa: -0.018601822659162392
     s: 15.668204739865292
-    dkappa: -0.010734221025501409
+    dkappa: -0.01073516676065567
     ddkappa: 0
   }
   v: 1.07587595124514
@@ -957,12 +957,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.8804744787
-    y: 4140939.9171656184
-    theta: -1.8497188058054688
-    kappa: -0.018962496695683808
+    x: 587256.88024896092
+    y: 4140939.9172205497
+    theta: -1.8497533982098053
+    kappa: -0.018964536760030983
     s: 15.773544915467035
-    dkappa: -0.011187903159781213
+    dkappa: -0.011188805363863435
     ddkappa: 0
   }
   v: 1.0302438491920247
@@ -971,12 +971,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.85337051691
-    y: 4140939.8203225881
-    theta: -1.8515128949941397
-    kappa: -0.019308793457857475
+    x: 587256.85314172623
+    y: 4140939.8203784348
+    theta: -1.8515476989338324
+    kappa: -0.019310806580571365
     s: 15.874109309335259
-    dkappa: -0.01162101681828869
+    dkappa: -0.011621877464856418
     ddkappa: 0
   }
   v: 0.98028510960060966
@@ -985,12 +985,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.82727678015
-    y: 4140939.7286333847
-    theta: -1.8532885174347995
-    kappa: -0.01964053988823734
+    x: 587256.82704467233
+    y: 4140939.7286901767
+    theta: -1.8533235493001496
+    kappa: -0.019642528333768436
     s: 15.969442382381292
-    dkappa: -0.011924783809146151
+    dkappa: -0.011925568198102322
     ddkappa: 0
   }
   v: 0.92553909334811579
@@ -999,12 +999,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.8014478992
-    y: 4140939.6428377712
-    theta: -1.8552018456214219
-    kappa: -0.019963638566861657
+    x: 587256.80121273175
+    y: 4140939.6428954843
+    theta: -1.8552370390207034
+    kappa: -0.019965600062946603
     s: 16.059041591876394
-    dkappa: -0.011861657272323217
+    dkappa: -0.011862452392395095
     ddkappa: 0
   }
   v: 0.865526366755886
@@ -1013,12 +1013,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.77743088664
-    y: 4140939.5630606292
-    theta: -1.8569810106959281
-    kappa: -0.020264072216370391
+    x: 587256.77719287435
+    y: 4140939.5631191991
+    theta: -1.8570163543098748
+    kappa: -0.020266008653484213
     s: 16.142355511996168
-    dkappa: -0.011802958995006481
+    dkappa: -0.011803764093417697
     ddkappa: 0
   }
   v: 0.79974870158938627
@@ -1027,12 +1027,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.75539933634
-    y: 4140939.4898785828
-    theta: -1.8586131305720177
-    kappa: -0.020539669319073209
+    x: 587256.75515871425
+    y: 4140939.4899379387
+    theta: -1.8586486119903429
+    kappa: -0.020541582768815774
     s: 16.218781954364957
-    dkappa: -0.011749113245076029
+    dkappa: -0.011749927496927336
     ddkappa: 0
   }
   v: 0.72768907505820546
@@ -1041,12 +1041,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.735542017
-    y: 4140939.4239186649
-    theta: -1.8600842037481207
-    kappa: -0.020788068527894693
+    x: 587256.73529904289
+    y: 4140939.4239787287
+    theta: -1.8601198093758233
+    kappa: -0.020789961258824263
     s: 16.287666088600282
-    dkappa: -0.011700581378993442
+    dkappa: -0.011701403880957879
     ddkappa: 0
   }
   v: 0.648811669816054
@@ -1055,12 +1055,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.71806341456
-    y: 4140939.3658601139
-    theta: -1.8613790688917673
-    kappa: -0.021006711888974964
+    x: 587256.71781837
+    y: 4140939.3659208007
+    theta: -1.8614147838519695
+    kappa: -0.021008586383007032
     s: 16.348298562857234
-    dkappa: -0.011657863165959917
+    dkappa: -0.011658692929752898
     ddkappa: 0
   }
   v: 0.56256187396076562
@@ -1069,12 +1069,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.70318427356
-    y: 4140939.3164361743
-    theta: -1.8624813642441858
-    kappa: -0.021192838064270473
+    x: 587256.70293746633
+    y: 4140939.316497392
+    theta: -1.8625171722774576
+    kappa: -0.021194697033641904
     s: 16.3999136243729
-    dkappa: -0.011621498112074427
+    dkappa: -0.011622334057698705
     ddkappa: 0
   }
   v: 0.46836628103429589
@@ -1083,12 +1083,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.69114213879
-    y: 4140939.2764358995
-    theta: -1.8633734868389951
-    kappa: -0.021343475554154712
+    x: 587256.69089390524
+    y: 4140939.2764975475
+    theta: -1.8634093701991163
+    kappa: -0.021345321958953181
     s: 16.441687240010765
-    dkappa: -0.011592066784491843
+    dkappa: -0.011592907733257395
     ddkappa: 0
   }
   v: 0.365632690022724
@@ -1097,12 +1097,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.682191898
-    y: 4140939.2467059479
-    theta: -1.8640365515523976
-    kappa: -0.021455435920018924
+    x: 587256.68194260425
+    y: 4140939.2467679149
+    theta: -1.8640724908985091
+    kappa: -0.021457272986277653
     s: 16.472735216805123
-    dkappa: -0.011570192135581088
+    dkappa: -0.011571036802899885
     ddkappa: 0
   }
   v: 0.25375010535625075
@@ -1111,12 +1111,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.67660632264
-    y: 4140939.2281523813
-    theta: -1.8644503500366978
-    kappa: -0.021525307006872871
+    x: 587256.6763563673
+    y: 4140939.228214547
+    theta: -1.8644863243218093
+    kappa: -0.021527138245230656
     s: 16.4921113225055
-    dkappa: -0.01155654082708325
+    dkappa: -0.011557387815038955
     ddkappa: 0
   }
   v: 0.13208873690919942
@@ -1125,12 +1125,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.67467661039
-    y: 4140939.22174247
-    theta: -1.8645933096372818
-    kappa: -0.021549446165945556
+    x: 587256.67442642653
+    y: 4140939.2218047045
+    theta: -1.8646292959931288
+    kappa: -0.021551275390872121
     s: 16.498805406121065
-    dkappa: -0.01155182455426974
+    dkappa: -0.011552672343962272
     ddkappa: 0
   }
   v: 1.6875389974302379e-14
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_mission_complete_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_mission_complete_0.pb.txt
index 2d6f82cbd59724dcd468dd6912caa5263ad5cb57..369196dd7863f13a87fb7ff30114bd541b8c844d 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_mission_complete_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_mission_complete_0.pb.txt
@@ -13,1128 +13,1128 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.15529010837185e-24
-  a: 0.0046889957913127667
+  v: 4.1520589341040647e-24
+  a: 0.0046890227582941516
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 587610.56669302867
-    y: 4140980.447338135
-    theta: 1.3203319789098693
-    kappa: 7.6802807170447681e-11
-    s: 2.203959289907151e-05
-    dkappa: 7.7054398261395524e-11
+    x: 587610.56669302878
+    y: 4140980.4473381354
+    theta: 1.3203319789029764
+    kappa: 6.3222402855031363e-11
+    s: 2.2039733962575657e-05
+    dkappa: 6.3429507158506818e-11
     ddkappa: 0
   }
-  v: 0.0004270268095397981
-  a: 0.0038630631547759459
+  v: 0.00042702968523814747
+  a: 0.0038630933907195584
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 587610.56670808012
-    y: 4140980.4473969671
-    theta: 1.320331979010009
-    kappa: 2.8842247666845745e-10
-    s: 8.2766687871393838e-05
-    dkappa: 2.8936729272742916e-10
+    x: 587610.56670808024
+    y: 4140980.447396968
+    theta: 1.3203319789841239
+    kappa: 2.3742327162091028e-10
+    s: 8.2767270884284475e-05
+    dkappa: 2.3820102411214727e-10
     ddkappa: 0
   }
-  v: 0.00077488745197484333
-  a: 0.0031053751488287467
+  v: 0.000774893440221414
+  a: 0.003105406885470033
   relative_time: 0.2
 }
 trajectory_point {
   path_point {
-    x: 587610.566730842
-    y: 4140980.4474859382
-    theta: 1.3203319791614478
-    kappa: 6.0845018014839235e-10
-    s: 0.00017460291835555021
-    dkappa: 6.1044334485585171e-10
+    x: 587610.56673084234
+    y: 4140980.4474859391
+    theta: 1.3203319791068415
+    kappa: 5.0086361500870974e-10
+    s: 0.0001746042593739987
+    dkappa: 5.0250434686166033e-10
     ddkappa: 0
   }
-  v: 0.0010503172002474096
-  a: 0.0024141479711374059
+  v: 0.0010503263708643564
+  a: 0.0024141796361865715
   relative_time: 0.30000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587610.56675960089
-    y: 4140980.4475983488
-    theta: 1.3203319793527852
-    kappa: 1.0127930931036965e-09
-    s: 0.00029063452607268233
-    dkappa: 1.0161108067227437e-09
+    x: 587610.56675960147
+    y: 4140980.4475983512
+    theta: 1.3203319792618906
+    kappa: 8.3371086848626233e-10
+    s: 0.00029063694060025105
+    dkappa: 8.3644194324820056e-10
     ddkappa: 0
   }
-  v: 0.0012598729470663943
-  a: 0.0017875978193681585
+  v: 0.0012598852224456304
+  a: 0.0017876280365101715
   relative_time: 0.4
 }
 trajectory_point {
   path_point {
-    x: 587610.56679280323
-    y: 4140980.4477281291
-    theta: 1.3203319795736868
-    kappa: 1.4796122335434139e-09
-    s: 0.000424594523003153
-    dkappa: 1.4844591560704606e-09
+    x: 587610.56679280428
+    y: 4140980.4477281328
+    theta: 1.3203319794408972
+    kappa: 1.2179877303758912e-09
+    s: 0.00042459831222764531
+    dkappa: 1.2219776214479291e-09
     ddkappa: 0
   }
-  v: 0.0014099332049073189
-  a: 0.0012239408911872405
+  v: 0.0014099483796079925
+  a: 0.0012239684800818291
   relative_time: 0.5
 }
 trajectory_point {
   path_point {
-    x: 587610.56682905194
-    y: 4140980.4478698163
-    theta: 1.3203319798148554
-    kappa: 1.9892603006685657e-09
-    s: 0.00057084485336320815
-    dkappa: 1.9957767313555786e-09
+    x: 587610.56682905334
+    y: 4140980.4478698215
+    theta: 1.3203319796363271
+    kappa: 1.637520997951996e-09
+    s: 0.00057085029235326521
+    dkappa: 1.6428851984665989e-09
     ddkappa: 0
   }
-  v: 0.0015066981060123279
-  a: 0.00072139338426088686
+  v: 0.0015067158663582979
+  a: 0.00072141736054254051
   relative_time: 0.6
 }
 trajectory_point {
   path_point {
-    x: 587610.56686710089
-    y: 4140980.44801854
-    theta: 1.3203319800680011
-    kappa: 2.52421951354853e-09
-    s: 0.00072435855558163965
-    dkappa: 2.5324883667968409e-09
+    x: 587610.56686710264
+    y: 4140980.4480185467
+    theta: 1.320331979841463
+    kappa: 2.0778904035386438e-09
+    s: 0.00072436588344308554
+    dkappa: 2.0846971686341466e-09
     ddkappa: 0
   }
-  v: 0.0015561894023901895
-  a: 0.0002781714962553335
+  v: 0.0015562093460675022
+  a: 0.00027819107153330327
   relative_time: 0.7
 }
 trajectory_point {
   path_point {
-    x: 587610.566905851
-    y: 4140980.4481700049
-    theta: 1.3203319803258131
-    kappa: 3.0690394497974138e-09
-    s: 0.00088070192427644673
-    dkappa: 3.0790930274230987e-09
+    x: 587610.56690585334
+    y: 4140980.4481700137
+    theta: 1.3203319800503803
+    kappa: 2.5263774506071577e-09
+    s: 0.0008807113362683818
+    dkappa: 2.5346533720992806e-09
     ddkappa: 0
   }
-  v: 0.0015642504658162958
-  a: -0.00010750857516318366
+  v: 0.00156427212147066
+  a: -0.0001074939933048873
   relative_time: 0.79999999999999993
 }
 trajectory_point {
   path_point {
-    x: 587610.5669443463
-    y: 4140980.4483204731
-    theta: 1.3203319805819289
-    kappa: 3.6102748842504403e-09
-    s: 0.0010360166722315001
-    dkappa: 3.622101444121264e-09
+    x: 587610.56694434932
+    y: 4140980.4483204843
+    theta: 1.3203319802579232
+    kappa: 2.9719142498737016e-09
+    s: 0.0010360283138421387
+    dkappa: 2.9816496633240663e-09
     ddkappa: 0
   }
-  v: 0.0015365462878326622
-  a: -0.00043743063232842914
+  v: 0.001536569134666926
+  a: -0.00043742144033103413
   relative_time: 0.89999999999999991
 }
 trajectory_point {
   path_point {
-    x: 587610.56698176847
-    y: 4140980.448466748
-    theta: 1.3203319808309055
-    kappa: 4.1364236276403154e-09
-    s: 0.0011870020923732017
-    dkappa: 4.1499737486842458e-09
+    x: 587610.56698177208
+    y: 4140980.448466761
+    theta: 1.3203319804596814
+    kappa: 3.4050323553968738e-09
+    s: 0.001187016055355463
+    dkappa: 3.4161865795785197e-09
     ddkappa: 0
   }
-  v: 0.0014785634797479269
-  a: -0.00071337847757416833
+  v: 0.0014785869671195544
+  a: -0.00071337487590414014
   relative_time: 0.99999999999999989
 }
 trajectory_point {
   path_point {
-    x: 587610.56701743335
-    y: 4140980.448606153
-    theta: 1.3203319810681902
-    kappa: 4.6378643652736192e-09
-    s: 0.00133089721974715
-    dkappa: 4.6530571088589054e-09
+    x: 587610.56701743754
+    y: 4140980.4486061693
+    theta: 1.3203319806519649
+    kappa: 3.8178116006752955e-09
+    s: 0.0013309135401139913
+    dkappa: 3.8303180094351958e-09
     ddkappa: 0
   }
-  v: 0.0013956102726373526
-  a: -0.0009371359132341649
+  v: 0.0013956338396558989
+  a: -0.00093713790638320828
   relative_time: 1.0999999999999999
 }
 trajectory_point {
   path_point {
-    x: 587610.567050786
-    y: 4140980.44873652
-    theta: 1.3203319812900907
-    kappa: 5.106794495707168e-09
-    s: 0.0014654629934948
-    dkappa: 5.1235233633939909e-09
+    x: 587610.56705079076
+    y: 4140980.4487365382
+    theta: 1.3203319808317822
+    kappa: 4.2038289347451931e-09
+    s: 0.0014654816514742999
+    dkappa: 4.2175998612637705e-09
     ddkappa: 0
   }
-  v: 0.0012928165173428264
-  a: -0.0011104867416421825
+  v: 0.0012928396124674134
+  a: -0.0011104941381272441
   relative_time: 1.2
 }
 trajectory_point {
   path_point {
-    x: 587610.56708139624
-    y: 4140980.448856168
-    theta: 1.320331981493746
-    kappa: 5.5371679694244069e-09
-    s: 0.001588964418830127
-    dkappa: 5.5553066570880959e-09
+    x: 587610.56708140159
+    y: 4140980.448856188
+    theta: 1.3203319809968146
+    kappa: 4.5581072582779922e-09
+    s: 0.0015889853407803156
+    dkappa: 4.573038731725637e-09
     ddkappa: 0
   }
-  v: 0.0011751336844728567
-  a: -0.0012352147651319865
+  v: 0.0011751557851096522
+  a: -0.0012352271774952479
   relative_time: 1.3
 }
 trajectory_point {
   path_point {
-    x: 587610.56710895465
-    y: 4140980.4489638866
-    theta: 1.3203319816770966
-    kappa: 5.9246331275117747e-09
-    s: 0.0017001527290162876
-    dkappa: 5.9440410758375878e-09
+    x: 587610.56710896059
+    y: 4140980.4489639094
+    theta: 1.3203319811453933
+    kappa: 4.8770642596779067e-09
+    s: 0.0017001757912997266
+    dkappa: 4.8930405742684932e-09
     ddkappa: 0
   }
-  v: 0.0010473348644025767
-  a: -0.0013131037860373406
+  v: 0.0010473554965022658
+  a: -0.0013131206308462252
   relative_time: 1.4000000000000001
 }
 trajectory_point {
   path_point {
-    x: 587610.56713326776
-    y: 4140980.4490589206
-    theta: 1.3203319818388561
-    kappa: 6.2664705403350943e-09
-    s: 0.0017982475473422839
-    dkappa: 6.2869982816845708e-09
+    x: 587610.567133274
+    y: 4140980.4490589448
+    theta: 1.3203319812764756
+    kappa: 5.1584612511795269e-09
+    s: 0.0017982725821603907
+    dkappa: 5.1753593676209286e-09
     ddkappa: 0
   }
-  v: 0.000914014767273745
-  a: -0.0013459376066920084
+  v: 0.000914033524929006
+  a: -0.0013459581045391798
   relative_time: 1.5000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587610.56715425383
-    y: 4140980.4491409506
-    theta: 1.3203319819784805
-    kappa: 6.5615308462159337e-09
-    s: 0.0018829190490996228
-    dkappa: 6.5830251478648118e-09
+    x: 587610.5671542607
+    y: 4140980.4491409762
+    theta: 1.3203319813896206
+    kappa: 5.4013520049454023e-09
+    s: 0.0018829458522867451
+    dkappa: 5.4190457842870087e-09
     ddkappa: 0
   }
-  v: 0.00077958972299473918
-  a: -0.001335500029429758
+  v: 0.00077960628803772926
+  a: -0.0013355232049331138
   relative_time: 1.6000000000000003
 }
 trajectory_point {
   path_point {
-    x: 587610.56717193837
-    y: 4140980.4492100752
-    theta: 1.320331982096139
-    kappa: 6.8101725901080133e-09
-    s: 0.0019542701235589852
-    dkappa: 6.8324813938557173e-09
+    x: 587610.5671719457
+    y: 4140980.4492101027
+    theta: 1.3203319814849659
+    kappa: 5.6060315891636379e-09
+    s: 0.0019542984643362181
+    dkappa: 5.624395859040874e-09
     ddkappa: 0
   }
-  v: 0.00064829768124056617
-  a: -0.0012835748565843486
+  v: 0.000648311842840385
+  a: -0.0012835995383870293
   relative_time: 1.7000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587610.56718644989
-    y: 4140980.4492667969
-    theta: 1.320331982192686
-    kappa: 7.014200062273543e-09
-    s: 0.002012818535946878
-    dkappa: 7.0371772204242356e-09
+    x: 587610.56718645734
+    y: 4140980.4492668249
+    theta: 1.3203319815632031
+    kappa: 5.7739852041454805e-09
+    s: 0.0020128481686356382
+    dkappa: 5.79289965742132e-09
     ddkappa: 0
   }
-  v: 0.00052419821145284918
-  a: -0.0011919458904895474
+  v: 0.0005242098857130233
+  a: -0.0011919707112599326
   relative_time: 1.8000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587610.56719801482
-    y: 4140980.449312001
-    theta: 1.3203319822696298
-    kappa: 7.1768011369596381e-09
-    s: 0.002059479089422305
-    dkappa: 7.200310944674841e-09
+    x: 587610.56719802273
+    y: 4140980.4493120313
+    theta: 1.320331981625555
+    kappa: 5.9078370184229079e-09
+    s: 0.0020595097671176452
+    dkappa: 5.9271899442263952e-09
     ddkappa: 0
   }
-  v: 0.00041117250283984248
-  a: -0.001062396933479118
+  v: 0.00041118175239579881
+  a: -0.0010624203299108249
   relative_time: 1.9000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587610.56720695412
-    y: 4140980.4493469428
-    theta: 1.3203319823291042
-    kappa: 7.3024851110746636e-09
-    s: 0.002095545787053424
-    dkappa: 7.3264066350974466e-09
+    x: 587610.56720696215
+    y: 4140980.4493469731
+    theta: 1.3203319816737502
+    kappa: 6.0112990048462074e-09
+    s: 0.0020955772772570948
+    dkappa: 6.0309908520079685e-09
     ddkappa: 0
   }
-  v: 0.00031292336437641966
-  a: -0.00089671178788682969
+  v: 0.00031293041799295595
+  a: -0.00089673200069871335
   relative_time: 2.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587610.567213672
-    y: 4140980.4493732015
-    theta: 1.3203319823737996
-    kappa: 7.3969373440476115e-09
-    s: 0.0021226501187807264
-    dkappa: 7.42116827525517e-09
+    x: 587610.56721368013
+    y: 4140980.4493732327
+    theta: 1.3203319817099695
+    kappa: 6.0890512866489622e-09
+    s: 0.0021226822199440897
+    dkappa: 6.1089978351736929e-09
     ddkappa: 0
   }
-  v: 0.00023202292263189414
-  a: -0.00072510840284144317
+  v: 0.00023202815266873367
+  a: -0.00072512474753540018
   relative_time: 2.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587610.56721858808
-    y: 4140980.449392417
-    theta: 1.3203319824065074
-    kappa: 7.4660569546001164e-09
-    s: 0.0021424849156331588
-    dkappa: 7.49051430823773e-09
+    x: 587610.56721859635
+    y: 4140980.4493924486
+    theta: 1.3203319817364745
+    kappa: 6.1459499672689767e-09
+    s: 0.0021425174638933738
+    dkappa: 6.1660829048123833e-09
     ddkappa: 0
   }
-  v: 0.00016716364994990234
-  a: -0.00057564603354291245
+  v: 0.00016716741799160012
+  a: -0.00057565900920035626
   relative_time: 2.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587610.56722207321
-    y: 4140980.4494060394
-    theta: 1.3203319824296944
-    kappa: 7.5150565784864017e-09
-    s: 0.0021565460131691724
-    dkappa: 7.5396744453824574e-09
+    x: 587610.56722208159
+    y: 4140980.4494060716
+    theta: 1.3203319817552641
+    kappa: 6.186286044508425e-09
+    s: 0.00215657887838108
+    dkappa: 6.206551115201e-09
     ddkappa: 0
   }
-  v: 0.00011620415668373173
-  a: -0.00044687044143177637
+  v: 0.00011620677604570347
+  a: -0.00044688051435407786
   relative_time: 2.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587610.56722444668
-    y: 4140980.4494153168
-    theta: 1.3203319824454858
-    kappa: 7.5484279658406253e-09
-    s: 0.0021661223791753447
-    dkappa: 7.573155150925233e-09
+    x: 587610.56722445507
+    y: 4140980.4494153489
+    theta: 1.3203319817680608
+    kappa: 6.2137570892052489e-09
+    s: 0.0021661554602484547
+    dkappa: 6.234112149700879e-09
     ddkappa: 0
   }
-  v: 7.7148477042615914e-05
-  a: -0.00033732738794857355
+  v: 7.7150216049142421e-05
+  a: -0.00033733499165706116
   relative_time: 2.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587610.56722598057
-    y: 4140980.4494213122
-    theta: 1.3203319824556903
-    kappa: 7.56999265797667e-09
-    s: 0.0021723106560519742
-    dkappa: 7.5947904848076342e-09
+    x: 587610.567225989
+    y: 4140980.4494213443
+    theta: 1.3203319817763302
+    kappa: 6.2315089619474384e-09
+    s: 0.00217234387661525
+    dkappa: 6.2519221741278529e-09
     ddkappa: 0
   }
-  v: 4.8146069091734554e-05
-  a: -0.00024556263453384263
+  v: 4.8147154353965927e-05
+  a: -0.00024556816976980254
   relative_time: 2.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587610.56722690223
-    y: 4140980.449424915
-    theta: 1.3203319824618232
-    kappa: 7.5829526641879191e-09
-    s: 0.002176029703198675
-    dkappa: 7.6077929454840674e-09
+    x: 587610.56722691073
+    y: 4140980.4494249471
+    theta: 1.3203319817812997
+    kappa: 6.2421775297873422e-09
+    s: 0.002176063007593123
+    dkappa: 6.262625690122419e-09
     ddkappa: 0
   }
-  v: 2.7491814752213597e-05
-  a: -0.00017012194262812244
+  v: 2.7492434446173373e-05
+  a: -0.00017012577735279826
   relative_time: 2.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 587610.56722739933
-    y: 4140980.4494268578
-    theta: 1.32033198246513
-    kappa: 7.5899411385470284e-09
-    s: 0.0021780351393999695
-    dkappa: 7.6148043127289178e-09
+    x: 587610.56722740782
+    y: 4140980.4494268908
+    theta: 1.3203319817839796
+    kappa: 6.2479303829559518e-09
+    s: 0.0021780684889990249
+    dkappa: 6.2683973885198634e-09
     ddkappa: 0
   }
-  v: 1.3626019801125022e-05
-  a: -0.00010955107367195162
+  v: 1.3626326945714623e-05
+  a: -0.00010955354306654429
   relative_time: 2.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587610.56722762214
-    y: 4140980.449427729
-    theta: 1.3203319824666122
-    kappa: 7.5930730567057281e-09
-    s: 0.002178933885210886
-    dkappa: 7.61794649044369e-09
+    x: 587610.56722763053
+    y: 4140980.4494277611
+    theta: 1.3203319817851806
+    kappa: 6.2505085515772073e-09
+    s: 0.0021789672550686025
+    dkappa: 6.27098400272042e-09
     ddkappa: 0
   }
-  v: 5.1344138714870579e-06
-  a: -6.2395789105868567e-05
+  v: 5.13452960648964e-06
+  a: -6.2397195571537e-05
   relative_time: 2.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587610.5672276878
-    y: 4140980.4494279856
-    theta: 1.3203319824670487
-    kappa: 7.593995892694591e-09
-    s: 0.002179198705342552
-    dkappa: 7.6188723494641558e-09
+    x: 587610.5672276963
+    y: 4140980.4494280182
+    theta: 1.3203319817855346
+    kappa: 6.2512682223822835e-09
+    s: 0.0021792320811695881
+    dkappa: 6.2717461620594016e-09
     ddkappa: 0
   }
-  v: 7.4815045226372555e-07
-  a: -2.7201850370412459e-05
+  v: 7.4816731634910327e-07
+  a: -2.720246352827299e-05
   relative_time: 2.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587610.56722768373
-    y: 4140980.4494279698
-    theta: 1.3203319824670225
-    kappa: 7.5939402957228153e-09
-    s: 0.0021791827510477873
-    dkappa: 7.6188165703674917e-09
+    x: 587610.56722769223
+    y: 4140980.4494280024
+    theta: 1.320331981785513
+    kappa: 6.2512224554238884e-09
+    s: 0.0021792161265151975
+    dkappa: 6.2717002451773462e-09
     ddkappa: 0
   }
-  v: -6.5619311163429709e-07
-  a: -2.5150189061215944e-06
+  v: -6.562079029062912e-07
+  a: -2.5150755972481054e-06
   relative_time: 3.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587610.56722767174
-    y: 4140980.4494279227
-    theta: 1.3203319824669424
-    kappa: 7.593770766978015e-09
-    s: 0.002179134102506702
-    dkappa: 7.6186464862794229e-09
+    x: 587610.56722768024
+    y: 4140980.4494279553
+    theta: 1.3203319817854482
+    kappa: 6.251082900790563e-09
+    s: 0.0021791674768775237
+    dkappa: 6.2715602333901656e-09
     ddkappa: 0
   }
-  v: -5.6615619350704281e-08
-  a: 1.3118943846465291e-05
+  v: -5.6616895525020243e-08
+  a: 1.311923956104149e-05
   relative_time: 3.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587610.5672276906
-    y: 4140980.4494279968
-    theta: 1.3203319824670681
-    kappa: 7.5940363364259918e-09
-    s: 0.0021792103112122881
-    dkappa: 7.6189129256813729e-09
+    x: 587610.5672276991
+    y: 4140980.4494280289
+    theta: 1.32033198178555
+    kappa: 6.2513015153209692e-09
+    s: 0.0021792436873009327
+    dkappa: 6.2717795640592835e-09
     ddkappa: 0
   }
-  v: 1.7143079859174132e-06
-  a: 2.1154276446809354e-05
+  v: 1.7143466281942916e-06
+  a: 2.1154753286099196e-05
   relative_time: 3.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587610.56722776056
-    y: 4140980.4494282706
-    theta: 1.3203319824675341
-    kappa: 7.5950212396105161e-09
-    s: 0.0021794929423560143
-    dkappa: 7.6199010552175878e-09
+    x: 587610.56722776918
+    y: 4140980.4494283027
+    theta: 1.3203319817859276
+    kappa: 6.2521122793181906e-09
+    s: 0.0021795263248154577
+    dkappa: 6.2725929839617764e-09
     ddkappa: 0
   }
-  v: 3.9694266169183756e-06
-  a: 2.3045217454371967e-05
+  v: 3.9695160919039018e-06
+  a: 2.3045736917428955e-05
   relative_time: 3.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587610.56722788687
-    y: 4140980.4494287642
-    theta: 1.3203319824683737
-    kappa: 7.5967955944531248e-09
-    s: 0.0021800021172134238
-    dkappa: 7.6216812225023117e-09
+    x: 587610.56722789537
+    y: 4140980.4494287958
+    theta: 1.3203319817866079
+    kappa: 6.2535729132640293e-09
+    s: 0.0021800355111501954
+    dkappa: 6.2740584026605281e-09
     ddkappa: 0
   }
-  v: 6.1670130423477029e-06
-  a: 2.0246005428614612e-05
+  v: 6.1671520532060827e-06
+  a: 2.0246461794534167e-05
   relative_time: 3.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 587610.56722806254
-    y: 4140980.4494294506
-    theta: 1.3203319824695428
-    kappa: 7.5992660780528812e-09
-    s: 0.0021807110555297242
-    dkappa: 7.6241597989268909e-09
+    x: 587610.567228071
+    y: 4140980.4494294827
+    theta: 1.3203319817875554
+    kappa: 6.2556065945332951e-09
+    s: 0.0021807444654466995
+    dkappa: 6.2760987458743576e-09
     ddkappa: 0
   }
-  v: 7.9107638868461633e-06
-  a: 1.4210878928998553e-05
+  v: 7.910942203653501e-06
+  a: 1.4211199256919534e-05
   relative_time: 3.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587610.56722827314
-    y: 4140980.4494302734
-    theta: 1.3203319824709436
-    kappa: 7.6022266034861712e-09
-    s: 0.0021815606179053856
-    dkappa: 7.6271300224669479e-09
+    x: 587610.56722828164
+    y: 4140980.449430306
+    theta: 1.3203319817886907
+    kappa: 6.2580436741081049e-09
+    s: 0.0021815940469723768
+    dkappa: 6.2785438088481753e-09
     ddkappa: 0
   }
-  v: 8.9497996310007491e-06
-  a: 6.3940765149850557e-06
+  v: 8.9500013687494342e-06
+  a: 6.3942206440878056e-06
   relative_time: 3.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 587610.56722849945
-    y: 4140980.4494311581
-    theta: 1.3203319824724495
-    kappa: 7.6054089966064872e-09
-    s: 0.0021824738481817352
-    dkappa: 7.6303228404895164e-09
+    x: 587610.56722850807
+    y: 4140980.4494311907
+    theta: 1.3203319817899111
+    kappa: 6.2606633932921782e-09
+    s: 0.0021825072978338815
+    dkappa: 6.2811721097231172e-09
     ddkappa: 0
   }
-  v: 9.1786646113453272e-06
-  a: -1.7501632539648326e-06
+  v: 9.1788715079472289e-06
+  a: -1.7502027044565341e-06
   relative_time: 3.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 587610.5672287218
-    y: 4140980.4494320271
-    theta: 1.3203319824739281
-    kappa: 7.6085336728441939e-09
-    s: 0.0021833705158265486
-    dkappa: 7.6334577525601678e-09
+    x: 587610.5672287303
+    y: 4140980.4494320592
+    theta: 1.3203319817911092
+    kappa: 6.263235600425133e-09
+    s: 0.0021834039856905118
+    dkappa: 6.2837527429066963e-09
     ddkappa: 0
   }
-  v: 8.637327020358796e-06
-  a: -8.76760181838898e-06
+  v: 8.6375217146512765e-06
+  a: -8.7677994492105183e-06
   relative_time: 3.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 587610.56722892274
-    y: 4140980.4494328131
-    theta: 1.3203319824752657
-    kappa: 7.6113603140063269e-09
-    s: 0.0021841816583196478
-    dkappa: 7.6362936532501777e-09
+    x: 587610.56722893124
+    y: 4140980.4494328452
+    theta: 1.320331981792193
+    kappa: 6.2655624675967778e-09
+    s: 0.0021842151464676026
+    dkappa: 6.2860872324429373e-09
     ddkappa: 0
   }
-  v: 7.5111789064670367e-06
-  a: -1.3204000618826988e-05
+  v: 7.511348216214899e-06
+  a: -1.3204298250669988e-05
   relative_time: 3.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 587610.56722909189
-    y: 4140980.4494334739
-    theta: 1.3203319824763913
-    kappa: 7.613738545076366e-09
-    s: 0.0021848641235384953
-    dkappa: 7.6386796749436572e-09
+    x: 587610.56722910039
+    y: 4140980.4494335065
+    theta: 1.320331981793105
+    kappa: 6.2675202073614123e-09
+    s: 0.0021848976270699219
+    dkappa: 6.2880513853825271e-09
     ddkappa: 0
   }
-  v: 6.13103617404129e-06
-  a: -1.3605121095818126e-05
+  v: 6.131174373943227e-06
+  a: -1.3605427769330957e-05
   relative_time: 4.0000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587610.56722922786
-    y: 4140980.4494340052
-    theta: 1.3203319824772957
-    kappa: 7.6156499336180571e-09
-    s: 0.0021854126220489396
-    dkappa: 7.6405973248232661e-09
+    x: 587610.56722923636
+    y: 4140980.4494340373
+    theta: 1.3203319817938379
+    kappa: 6.2690936462929279e-09
+    s: 0.0021854461379440904
+    dkappa: 6.289629978594163e-09
     ddkappa: 0
   }
-  v: 4.87233012068828e-06
-  a: -1.1601212011757271e-05
+  v: 4.8724399480528258e-06
+  a: -1.1601473515085011e-05
   relative_time: 4.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587610.56722933508
-    y: 4140980.4494344243
-    theta: 1.3203319824780086
-    kappa: 7.6171564984388355e-09
-    s: 0.0021858449509773912
-    dkappa: 7.6421088248580163e-09
+    x: 587610.56722934358
+    y: 4140980.4494344569
+    theta: 1.3203319817944157
+    kappa: 6.2703338377506874e-09
+    s: 0.0021858784766176837
+    dkappa: 6.29087423267809e-09
     ddkappa: 0
   }
-  v: 3.8044952967097311e-06
-  a: -9.7862571770998277e-06
+  v: 3.8045810539719734e-06
+  a: -9.7864777694668561e-06
   relative_time: 4.2000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587610.56722941785
-    y: 4140980.4494347484
-    theta: 1.3203319824785598
-    kappa: 7.6183215115325032e-09
-    s: 0.0021861792670688539
-    dkappa: 7.6432776543084837e-09
+    x: 587610.56722942647
+    y: 4140980.4494347805
+    theta: 1.3203319817948624
+    kappa: 6.271292866706505e-09
+    s: 0.0021862128002449769
+    dkappa: 6.291836403226366e-09
     ddkappa: 0
   }
-  v: 2.9090680765340255e-06
-  a: -8.15162060447129e-06
+  v: 2.9091336499398222e-06
+  a: -8.1518043504378842e-06
   relative_time: 4.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587610.56722948048
-    y: 4140980.4494349929
-    theta: 1.3203319824789765
-    kappa: 7.6192019612247509e-09
-    s: 0.0021864319238857097
-    dkappa: 7.6441609881830538e-09
+    x: 587610.5672294891
+    y: 4140980.4494350259
+    theta: 1.3203319817952002
+    kappa: 6.27201764546959e-09
+    s: 0.0021864654627569789
+    dkappa: 6.2925635562238082e-09
     ddkappa: 0
   }
-  v: 2.1684484333269989e-06
-  a: -6.6886663064971435e-06
+  v: 2.1684973123993837e-06
+  a: -6.6888170759594923e-06
   relative_time: 4.4
 }
 trajectory_point {
   path_point {
-    x: 587610.56722952658
-    y: 4140980.4494351731
-    theta: 1.3203319824792825
-    kappa: 7.6198488531170419e-09
-    s: 0.0021866175581675943
-    dkappa: 7.64480999916764e-09
+    x: 587610.56722953508
+    y: 4140980.4494352052
+    theta: 1.3203319817954482
+    kappa: 6.2725501614210091e-09
+    s: 0.0021866511012232524
+    dkappa: 6.2930978165939828e-09
     ddkappa: 0
   }
-  v: 1.5658999389919384e-06
-  a: -5.3887582958028822e-06
+  v: 1.5659352359975292e-06
+  a: -5.3888797639930739e-06
   relative_time: 4.5
 }
 trajectory_point {
   path_point {
-    x: 587610.56722955918
-    y: 4140980.4494353007
-    theta: 1.3203319824794997
-    kappa: 7.6203075110304983e-09
-    s: 0.0021867491761912708
-    dkappa: 7.64527015955541e-09
+    x: 587610.56722956768
+    y: 4140980.4494353328
+    theta: 1.3203319817956241
+    kappa: 6.2729277247481639e-09
+    s: 0.0021867827222137358
+    dkappa: 6.2934766167452129e-09
     ddkappa: 0
   }
-  v: 1.0855497641695808e-06
-  a: -4.2432605850139957e-06
+  v: 1.0855742335849934e-06
+  a: -4.2433562325000276e-06
   relative_time: 4.6
 }
 trajectory_point {
   path_point {
-    x: 587610.5672295813
-    y: 4140980.4494353868
-    theta: 1.3203319824796464
-    kappa: 7.6206178779498e-09
-    s: 0.0021868382401305036
-    dkappa: 7.6455815431765053e-09
+    x: 587610.56722958968
+    y: 4140980.4494354189
+    theta: 1.3203319817957431
+    kappa: 6.2731832161792522e-09
+    s: 0.0021868717881605619
+    dkappa: 6.2937329451165706e-09
     ddkappa: 0
   }
-  v: 7.1238867823811447e-07
-  a: -3.2435371867559722e-06
+  v: 7.1240473621636819e-07
+  a: -3.2436102994417454e-06
   relative_time: 4.6999999999999993
 }
 trajectory_point {
   path_point {
-    x: 587610.56722959527
-    y: 4140980.4494354413
-    theta: 1.3203319824797397
-    kappa: 7.6208148169670474e-09
-    s: 0.0021868947544159312
-    dkappa: 7.645779127327753e-09
+    x: 587610.56722960377
+    y: 4140980.4494354739
+    theta: 1.3203319817958186
+    kappa: 6.2733453347177433e-09
+    s: 0.00218692830371988
+    dkappa: 6.2938955947238845e-09
     ddkappa: 0
   }
-  v: 4.3227104931317644e-07
-  a: -2.3809521136543016e-06
+  v: 4.3228079315010797e-07
+  a: -2.3810057827796233e-06
   relative_time: 4.7999999999999989
 }
 trajectory_point {
   path_point {
-    x: 587610.5672296033
-    y: 4140980.449435473
-    theta: 1.3203319824797934
-    kappa: 7.6209284122256645e-09
-    s: 0.0021869273520949408
-    dkappa: 7.6458930947023914e-09
+    x: 587610.5672296118
+    y: 4140980.4494355056
+    theta: 1.3203319817958621
+    kappa: 6.2734388453768514e-09
+    s: 0.002186960902133675
+    dkappa: 6.2939894117057363e-09
     ddkappa: 0
   }
-  v: 2.3191484424785996e-07
-  a: -1.6468693783344789e-06
+  v: 2.3192007184852703e-07
+  a: -1.6469065004750607e-06
   relative_time: 4.8999999999999986
 }
 trajectory_point {
   path_point {
-    x: 587610.56722960726
-    y: 4140980.4494354883
-    theta: 1.3203319824798199
-    kappa: 7.6209842698642775e-09
-    s: 0.0021869433811915417
-    dkappa: 7.64594913531979e-09
+    x: 587610.56722961576
+    y: 4140980.4494355209
+    theta: 1.3203319817958836
+    kappa: 6.27348482691399e-09
+    s: 0.0021869769315915884
+    dkappa: 6.2940355438694532e-09
     ddkappa: 0
   }
-  v: 9.8901628632704384e-08
-  a: -1.0326529934219888e-06
+  v: 9.8903857977798669e-08
+  a: -1.032676270489447e-06
   relative_time: 4.9999999999999982
 }
 trajectory_point {
   path_point {
-    x: 587610.56722960877
-    y: 4140980.4494354939
-    theta: 1.3203319824798292
-    kappa: 7.621003818960608e-09
-    s: 0.0021869489910662392
-    dkappa: 7.6459687484551643e-09
+    x: 587610.56722961715
+    y: 4140980.449435526
+    theta: 1.320331981795891
+    kappa: 6.2735009195652577e-09
+    s: 0.0021869825415927382
+    dkappa: 6.2940516892371195e-09
     ddkappa: 0
   }
-  v: 2.1676566795702807e-08
-  a: -5.2966697154232632e-07
+  v: 2.1677055407958626e-08
+  a: -5.2967891078418155e-07
   relative_time: 5.0999999999999979
 }
 trajectory_point {
   path_point {
-    x: 587610.56722960877
-    y: 4140980.4494354939
-    theta: 1.3203319824798296
-    kappa: 7.6210046124753451e-09
-    s: 0.0021869492187759081
-    dkappa: 7.6459695445693017e-09
+    x: 587610.56722961727
+    y: 4140980.4494355265
+    theta: 1.3203319817958914
+    kappa: 6.2735015727799e-09
+    s: 0.0021869827693075396
+    dkappa: 6.2940523445915657e-09
     ddkappa: 0
   }
-  v: -1.0451578197705587e-08
-  a: -1.2927532532097799e-07
+  v: -1.0451813787095787e-08
+  a: -1.2927823932065715e-07
   relative_time: 5.1999999999999975
 }
 trajectory_point {
   path_point {
-    x: 587610.56722960842
-    y: 4140980.449435493
-    theta: 1.3203319824798276
-    kappa: 7.6210006281960487e-09
-    s: 0.0021869480754336669
-    dkappa: 7.64596554723828e-09
+    x: 587610.56722961692
+    y: 4140980.4494355256
+    theta: 1.3203319817958898
+    kappa: 6.273498292954778e-09
+    s: 0.0021869816259395265
+    dkappa: 6.2940490540223741e-09
     ddkappa: 0
   }
-  v: -7.310444544621962e-09
-  a: 1.7715793261656634e-07
+  v: -7.3106093296107643e-09
+  a: 1.7716192593972506e-07
   relative_time: 5.2999999999999972
 }
 trajectory_point {
   path_point {
-    x: 587610.56722960854
-    y: 4140980.4494354934
-    theta: 1.3203319824798285
-    kappa: 7.6210025696810231e-09
-    s: 0.0021869486325687515
-    dkappa: 7.6459674950831829e-09
+    x: 587610.56722961715
+    y: 4140980.449435526
+    theta: 1.3203319817958905
+    kappa: 6.2734998911688385e-09
+    s: 0.0021869821830871692
+    dkappa: 6.2940506574718733e-09
     ddkappa: 0
   }
-  v: 2.2135928295297772e-08
-  a: 3.9826878964481339e-07
+  v: 2.2136427262033316e-08
+  a: 3.9827776703557749e-07
   relative_time: 5.3999999999999968
 }
 trajectory_point {
   path_point {
-    x: 587610.5672296097
-    y: 4140980.4494354981
-    theta: 1.3203319824798359
-    kappa: 7.6210181672032053e-09
-    s: 0.0021869531084863876
-    dkappa: 7.64598314369982e-09
+    x: 587610.5672296182
+    y: 4140980.44943553
+    theta: 1.3203319817958965
+    kappa: 6.2735127309175875e-09
+    s: 0.0021869866591056972
+    dkappa: 6.2940635392811434e-09
     ddkappa: 0
   }
-  v: 6.9787099599849731e-08
-  a: 5.42693233138275e-07
+  v: 6.9788672673309182e-08
+  a: 5.42705466005499e-07
   relative_time: 5.4999999999999964
 }
 trajectory_point {
   path_point {
-    x: 587610.56722961215
-    y: 4140980.4494355074
-    theta: 1.3203319824798521
-    kappa: 7.621052478694052e-09
-    s: 0.0021869629546276666
-    dkappa: 7.6460175675884531e-09
+    x: 587610.56722962065
+    y: 4140980.44943554
+    theta: 1.3203319817959098
+    kappa: 6.2735409758475534e-09
+    s: 0.0021869965054689185
+    dkappa: 6.29409187673601e-09
     ddkappa: 0
   }
-  v: 1.2840622738427783e-07
-  a: 6.1906725047145613e-07
+  v: 1.2840912179355537e-07
+  a: 6.1908120488810021e-07
   relative_time: 5.5999999999999961
 }
 trajectory_point {
   path_point {
-    x: 587610.567229616
-    y: 4140980.4494355228
-    theta: 1.3203319824798785
-    kappa: 7.6211081906874259e-09
-    s: 0.0021869789419294177
-    dkappa: 7.6460734620835045e-09
+    x: 587610.5672296246
+    y: 4140980.4494355554
+    theta: 1.3203319817959311
+    kappa: 6.27358683749076e-09
+    s: 0.0021870124931310402
+    dkappa: 6.294137888613045e-09
     ddkappa: 0
   }
-  v: 1.9162006840127884e-07
-  a: 6.3602682901887713e-07
+  v: 1.9162438771597032e-07
+  a: 6.3604116572197834e-07
   relative_time: 5.6999999999999957
 }
 trajectory_point {
   path_point {
-    x: 587610.56722962158
-    y: 4140980.4494355447
-    theta: 1.3203319824799153
-    kappa: 7.6211859192634853e-09
-    s: 0.0021870012471840831
-    dkappa: 7.6461514452832943e-09
+    x: 587610.56722963008
+    y: 4140980.4494355773
+    theta: 1.3203319817959609
+    kappa: 6.2736508229991977e-09
+    s: 0.0021870347988884892
+    dkappa: 6.2942020837255729e-09
     ddkappa: 0
   }
-  v: 2.5391897814100921e-07
-  a: 6.0220795615503793e-07
+  v: 2.5392470173761148e-07
+  a: 6.0222153054574237e-07
   relative_time: 5.7999999999999954
 }
 trajectory_point {
   path_point {
-    x: 587610.56722962868
-    y: 4140980.4494355717
-    theta: 1.320331982479962
-    kappa: 7.62128451099256e-09
-    s: 0.00218702953939959
-    dkappa: 7.64625035997974e-09
+    x: 587610.56722963718
+    y: 4140980.4494356047
+    theta: 1.3203319817959986
+    kappa: 6.2737319828792864e-09
+    s: 0.0021870630917417318
+    dkappa: 6.294283509469654e-09
     ddkappa: 0
   }
-  v: 3.1065691083106555e-07
-  a: 5.2624661925445543e-07
+  v: 3.1066391335939958e-07
+  a: 5.2625848139799791e-07
   relative_time: 5.899999999999995
 }
 trajectory_point {
   path_point {
-    x: 587610.567229637
-    y: 4140980.4494356043
-    theta: 1.3203319824800173
-    kappa: 7.6214013438790444e-09
-    s: 0.002187063066159226
-    dkappa: 7.6463675755880861e-09
+    x: 587610.56722964544
+    y: 4140980.4494356369
+    theta: 1.3203319817960435
+    kappa: 6.2738281587263562e-09
+    s: 0.0021870966192570952
+    dkappa: 6.2943800003701058e-09
     ddkappa: 0
   }
-  v: 3.5805141943648886e-07
-  a: 4.1677880569163296e-07
+  v: 3.5805949028611356e-07
+  a: 4.167882003173455e-07
   relative_time: 5.9999999999999947
 }
 trajectory_point {
   path_point {
-    x: 587610.56722964626
-    y: 4140980.4494356406
-    theta: 1.3203319824800794
-    kappa: 7.6215326283052821e-09
-    s: 0.0021871007399815105
-    dkappa: 7.6464992900766267e-09
+    x: 587610.56722965487
+    y: 4140980.4494356732
+    theta: 1.3203319817960939
+    kappa: 6.273936230959103e-09
+    s: 0.0021871342939285871
+    dkappa: 6.2944884266264817e-09
     ddkappa: 0
   }
-  v: 3.9318365565979843e-07
-  a: 2.8244050284108233e-07
+  v: 3.9319251842639391e-07
+  a: 2.8244686934239413e-07
   relative_time: 6.0999999999999943
 }
 trajectory_point {
   path_point {
-    x: 587610.56722965639
-    y: 4140980.44943568
-    theta: 1.320331982480146
-    kappa: 7.6216737079754577e-09
-    s: 0.0021871412246800724
-    dkappa: 7.6466408318964221e-09
+    x: 587610.56722966477
+    y: 4140980.4494357123
+    theta: 1.3203319817961479
+    kappa: 6.2740523665540676e-09
+    s: 0.0021871747795397158
+    dkappa: 6.2946049426590823e-09
     ddkappa: 0
   }
-  v: 4.1399836994093596e-07
-  a: 1.3186769807731705e-07
+  v: 4.1400770189273764e-07
+  a: 1.3187067051174604e-07
   relative_time: 6.199999999999994
 }
 trajectory_point {
   path_point {
-    x: 587610.56722966675
-    y: 4140980.4494357207
-    theta: 1.3203319824802151
-    kappa: 7.6218193608594668e-09
-    s: 0.0021871830217235193
-    dkappa: 7.6467869619110117e-09
+    x: 587610.56722967513
+    y: 4140980.4494357533
+    theta: 1.3203319817962038
+    kappa: 6.2741722667801023e-09
+    s: 0.0021872165775253115
+    dkappa: 6.294725235654954e-09
     ddkappa: 0
   }
-  v: 4.1930391145730192e-07
-  a: -2.6303621225156151e-08
+  v: 4.1931336300151009e-07
+  a: -2.6304214135989767e-08
   relative_time: 6.2999999999999936
 }
 trajectory_point {
   path_point {
-    x: 587610.56722967688
-    y: 4140980.4494357607
-    theta: 1.3203319824802835
-    kappa: 7.62196410013682e-09
-    s: 0.0021872245565953147
-    dkappa: 7.6469321753261474e-09
+    x: 587610.56722968549
+    y: 4140980.4494357929
+    theta: 1.3203319817962593
+    kappa: 6.2742914149328392e-09
+    s: 0.0021872581133333458
+    dkappa: 6.2948447741138811e-09
     ddkappa: 0
   }
-  v: 4.0877222812376322e-07
-  a: -1.8343746769182716e-07
+  v: 4.087814422729204e-07
+  a: -1.8344160256221954e-07
   relative_time: 6.3999999999999932
 }
 trajectory_point {
   path_point {
-    x: 587610.56722968677
-    y: 4140980.4494357989
-    theta: 1.320331982480349
-    kappa: 7.6221024751405143e-09
-    s: 0.00218726426515365
-    dkappa: 7.6470710036195e-09
+    x: 587610.56722969527
+    y: 4140980.449435832
+    theta: 1.3203319817963124
+    kappa: 6.2744053240691615e-09
+    s: 0.0021872978227867532
+    dkappa: 6.2949590563943945e-09
     ddkappa: 0
   }
-  v: 3.8293886659261255e-07
-  a: -3.3089785394818586e-07
+  v: 3.8294749843107148e-07
+  a: -3.3090531272832581e-07
   relative_time: 6.4999999999999929
 }
 trajectory_point {
   path_point {
-    x: 587610.56722969585
-    y: 4140980.4494358343
-    theta: 1.3203319824804092
-    kappa: 7.62222937230093e-09
-    s: 0.0021873006799913186
-    dkappa: 7.6471983164703845e-09
+    x: 587610.56722970435
+    y: 4140980.4494358669
+    theta: 1.320331981796361
+    kappa: 6.2745097847416742e-09
+    s: 0.00218733423844525
+    dkappa: 6.2950638592597666e-09
     ddkappa: 0
   }
-  v: 3.4320297225361663e-07
-  a: -4.6004879261972383e-07
+  v: 3.4321070840388547e-07
+  a: -4.600591625957182e-07
   relative_time: 6.5999999999999925
 }
 trajectory_point {
   path_point {
-    x: 587610.56722970377
-    y: 4140980.4494358655
-    theta: 1.3203319824804616
-    kappa: 7.62234031608971e-09
-    s: 0.0021873325167955904
-    dkappa: 7.6473096236894866e-09
+    x: 587610.56722971227
+    y: 4140980.4494358981
+    theta: 1.3203319817964034
+    kappa: 6.2746011127331677e-09
+    s: 0.0021873660759671562
+    dkappa: 6.29515548642401e-09
     ddkappa: 0
   }
-  v: 2.9182728923397813e-07
-  a: -5.6225429633193095e-07
+  v: 2.918338673231681e-07
+  a: -5.6226697012577585e-07
   relative_time: 6.6999999999999922
 }
 trajectory_point {
   path_point {
-    x: 587610.56722971029
-    y: 4140980.4494358907
-    theta: 1.3203319824805049
-    kappa: 7.6224317699636482e-09
-    s: 0.0021873587607080841
-    dkappa: 7.6474013771485676e-09
+    x: 587610.56722971878
+    y: 4140980.4494359237
+    theta: 1.3203319817964385
+    kappa: 6.2746763967910936e-09
+    s: 0.0021873923204712149
+    dkappa: 6.2952310170978773e-09
     ddkappa: 0
   }
-  v: 2.3193816039836107e-07
-  a: -6.2887837771028694e-07
+  v: 2.3194338852459345e-07
+  a: -6.2889255327990332e-07
   relative_time: 6.7999999999999918
 }
 trajectory_point {
   path_point {
-    x: 587610.56722971518
-    y: 4140980.4494359107
-    theta: 1.3203319824805377
-    kappa: 7.622501437308575e-09
-    s: 0.0021873787526846423
-    dkappa: 7.6474712727101952e-09
+    x: 587610.56722972379
+    y: 4140980.4494359423
+    theta: 1.3203319817964654
+    kappa: 6.2747337463620326e-09
+    s: 0.0021874123128984126
+    dkappa: 6.2952885545348635e-09
     ddkappa: 0
   }
-  v: 1.6752552734886962e-07
-  a: -6.5128504938031217e-07
+  v: 1.6752930354767009e-07
+  a: -6.5129973001950345e-07
   relative_time: 6.8999999999999915
 }
 trajectory_point {
   path_point {
-    x: 587610.56722971867
-    y: 4140980.4494359232
-    theta: 1.3203319824805602
-    kappa: 7.6225485623832419e-09
-    s: 0.0021873922758552039
-    dkappa: 7.647518552157462e-09
+    x: 587610.567229727
+    y: 4140980.4494359558
+    theta: 1.3203319817964834
+    kappa: 6.2747725393261592e-09
+    s: 0.0021874258363738007
+    dkappa: 6.2953274745772e-09
     ddkappa: 0
   }
-  v: 1.0344293042505576e-07
-  a: -6.2083832396745758e-07
+  v: 1.0344526213580374e-07
+  a: -6.2085231830595539e-07
   relative_time: 6.9999999999999911
 }
 trajectory_point {
   path_point {
-    x: 587610.56722972041
-    y: 4140980.4494359307
-    theta: 1.3203319824805724
-    kappa: 7.6225742312632083e-09
-    s: 0.00218739964188368
-    dkappa: 7.6475443051236977e-09
+    x: 587610.56722972891
+    y: 4140980.4494359633
+    theta: 1.3203319817964931
+    kappa: 6.2747936697317189e-09
+    s: 0.0021874332025683145
+    dkappa: 6.2953486742018619e-09
     ddkappa: 0
   }
-  v: 4.5407508703961612e-08
-  a: -5.2890221409724183e-07
+  v: 4.5408532236208322e-08
+  a: -5.2891413610066029e-07
   relative_time: 7.0999999999999908
 }
 trajectory_point {
   path_point {
-    x: 587610.567229721
-    y: 4140980.4494359326
-    theta: 1.3203319824805757
-    kappa: 7.62258167278473e-09
-    s: 0.0021874017773278243
-    dkappa: 7.6475517710221983e-09
+    x: 587610.5672297295
+    y: 4140980.4494359652
+    theta: 1.320331981796496
+    kappa: 6.2747997955294914e-09
+    s: 0.0021874353380605944
+    dkappa: 6.2953548200665572e-09
     ddkappa: 0
   }
-  v: 7.6232965252887031e-21
-  a: -3.6684073239514635e-07
+  v: 1.0164395367051604e-20
+  a: -3.6684900136501763e-07
   relative_time: 7.19999999999999
 }
 trajectory_point {
   path_point {
-    x: 587610.56722972053
-    y: 4140980.4494359316
-    theta: 1.3203319824805733
-    kappa: 7.6225765594886381e-09
-    s: 0.0021874003099991114
-    dkappa: 7.6475466409759412e-09
+    x: 587610.567229729
+    y: 4140980.4494359638
+    theta: 1.320331981796494
+    kappa: 6.2747955863072617e-09
+    s: 0.0021874338706988059
+    dkappa: 6.2953505970557337e-09
     ddkappa: 0
   }
-  v: -2.5335259134860028e-08
-  a: -1.2601789148664916e-07
+  v: -2.5335830217824847e-08
+  a: -1.260207320604201e-07
   relative_time: 7.29999999999999
 }
 trajectory_point {
   path_point {
-    x: 587610.56722971983
-    y: 4140980.4494359284
-    theta: 1.320331982480569
-    kappa: 7.6225673085642352e-09
-    s: 0.0021873976553226054
-    dkappa: 7.6475373597473051e-09
+    x: 587610.56722972845
+    y: 4140980.4494359614
+    theta: 1.3203319817964905
+    kappa: 6.2747879710242951e-09
+    s: 0.0021874312159624611
+    dkappa: 6.29534295682658e-09
     ddkappa: 0
   }
-  v: -2.2290333411281255e-08
-  a: 2.0220229600273445e-07
+  v: -2.2290835858487038e-08
+  a: 2.022068538517464e-07
   relative_time: 7.39999999999999
 }
 trajectory_point {
   path_point {
-    x: 587610.56722971972
-    y: 4140980.4494359279
-    theta: 1.3203319824805682
-    kappa: 7.6225653827931711e-09
-    s: 0.0021873971026968378
-    dkappa: 7.64753542766779e-09
+    x: 587610.56722972821
+    y: 4140980.4494359605
+    theta: 1.3203319817964898
+    kappa: 6.27478638574579e-09
+    s: 0.0021874306633242364
+    dkappa: 6.2953413663550108e-09
     ddkappa: 0
   }
-  v: 1.83063111975298e-08
-  a: 6.2645581744754e-07
+  v: 1.8306723840734656e-08
+  a: 6.2646993841006544e-07
   relative_time: 7.4999999999999893
 }
 trajectory_point {
   path_point {
-    x: 587610.56722972123
-    y: 4140980.4494359335
-    theta: 1.3203319824805777
-    kappa: 7.6225855914933342e-09
-    s: 0.0021874029018536792
-    dkappa: 7.6475557025677331e-09
+    x: 587610.56722972961
+    y: 4140980.4494359661
+    theta: 1.3203319817964976
+    kappa: 6.2748030213773753e-09
+    s: 0.002187436462611797
+    dkappa: 6.2953580564816916e-09
     ddkappa: 0
   }
-  v: 1.0648980745586946e-07
-  a: 1.1553786602222539e-06
+  v: 1.0649220784633918e-07
+  a: 1.1554047036531528e-06
   relative_time: 7.599999999999989
 }
 trajectory_point {
   path_point {
-    x: 587610.56722972542
-    y: 4140980.4494359507
-    theta: 1.3203319824806063
-    kappa: 7.62264639146274e-09
-    s: 0.0021874203492182132
-    dkappa: 7.6476167017060371e-09
+    x: 587610.567229734
+    y: 4140980.4494359829
+    theta: 1.3203319817965209
+    kappa: 6.274853071399549e-09
+    s: 0.0021874539103696124
+    dkappa: 6.29540827045801e-09
     ddkappa: 0
   }
-  v: 2.5315888686546153e-07
-  a: 1.7976068117014083e-06
+  v: 2.5316459332881328e-07
+  a: 1.7976473316196124e-06
   relative_time: 7.6999999999999886
 }
 trajectory_point {
   path_point {
-    x: 587610.56722973427
-    y: 4140980.4494359847
-    theta: 1.320331982480665
-    kappa: 7.6227701879234056e-09
-    s: 0.0021874558742686111
-    dkappa: 7.6477409036998831e-09
+    x: 587610.56722974277
+    y: 4140980.4494360173
+    theta: 1.3203319817965684
+    kappa: 6.2749549796021708e-09
+    s: 0.0021874894362207816
+    dkappa: 6.2955105124920966e-09
     ddkappa: 0
   }
-  v: 4.7007587966546319e-07
-  a: 2.5617762592594997e-06
+  v: 4.7008647566233923e-07
+  a: 2.56183400434806e-06
   relative_time: 7.7999999999999883
 }
 trajectory_point {
   path_point {
-    x: 587610.56722974952
-    y: 4140980.4494360443
-    theta: 1.320331982480766
-    kappa: 7.62298363546525e-09
-    s: 0.0021875171258960041
-    dkappa: 7.64795505045445e-09
+    x: 587610.56722975813
+    y: 4140980.4494360769
+    theta: 1.3203319817966501
+    kappa: 6.2751306878189195e-09
+    s: 0.00218755068922885
+    dkappa: 6.2956867962948116e-09
     ddkappa: 0
   }
-  v: 7.6986671483246164e-07
-  a: 3.45652299027103e-06
+  v: 7.6988406842510238e-07
+  a: 3.4566009038770641e-06
   relative_time: 7.8999999999999879
 }
 trajectory_point {
   path_point {
-    x: 587610.56722977315
-    y: 4140980.4494361375
-    theta: 1.3203319824809243
-    kappa: 7.6233179389899691e-09
-    s: 0.0021876130587643594
-    dkappa: 7.6482904490926383e-09
+    x: 587610.56722978177
+    y: 4140980.4494361696
+    theta: 1.3203319817967785
+    kappa: 6.2754058836617662e-09
+    s: 0.0021876466242596315
+    dkappa: 6.2959628936257523e-09
     ddkappa: 0
   }
-  v: 1.1660209200805859e-06
-  a: 4.4904829921105291e-06
+  v: 1.1660472033990598e-06
+  a: 4.490584212245281e-06
   relative_time: 7.9999999999999876
 }
 decision {
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_nudge_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_nudge_0.pb.txt
index e535304e319f48c2208058c0848e3aa1beec61a6..0b4075f6e36a5a47eb1532b7be8a4aa95eb114dc 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_nudge_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_nudge_0.pb.txt
@@ -19,12 +19,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587325.06228416925
-    y: 4141205.106355601
-    theta: -1.8235697695939759
-    kappa: -0.00019333653137903209
+    x: 587325.06228385423
+    y: 4141205.106355682
+    theta: -1.8235705034033844
+    kappa: -0.00019454801181965274
     s: 1.104209005910727
-    dkappa: -0.00017136167151717832
+    dkappa: -0.00017245719969775602
     ddkappa: 0
   }
   v: 11.073672499778571
@@ -33,12 +33,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.78444151592
-    y: 4141204.03103724
-    theta: -1.8236907475847943
-    kappa: -3.7596822851217553e-05
+    x: 587324.78443938016
+    y: 4141204.0310377916
+    theta: -1.8236934694326721
+    kappa: -3.9874512706646362e-05
     s: 2.214842212094831
-    dkappa: 9.3818720616120231e-05
+    dkappa: 9.2618776641130435e-05
     ddkappa: 0
   }
   v: 11.139466599422537
@@ -47,12 +47,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.50488175207
-    y: 4141202.949232562
-    theta: -1.8236522776024668
-    kappa: 9.7125915617237712e-05
+    x: 587324.50487502338
+    y: 4141202.9492343008
+    theta: -1.8236580943467247
+    kappa: 9.3929464469812818e-05
     s: 3.332185052409435
-    dkappa: 3.0937139939638936e-05
+    dkappa: 2.9054449171293093e-05
     ddkappa: 0
   }
   v: 11.207743992644858
@@ -61,12 +61,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.22370989551
-    y: 4141201.8606947325
-    theta: -1.823475436687261
-    kappa: 0.00021142788264686519
+    x: 587324.22369467281
+    y: 4141201.8606986655
+    theta: -1.8234853035165057
+    kappa: 0.00020746248577535289
     s: 4.4564502468654323
-    dkappa: 0.00013783354733484777
+    dkappa: 0.00013571256026173318
     ddkappa: 0
   }
   v: 11.277799968268411
@@ -75,12 +75,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.941027641
-    y: 4141200.7652482912
-    theta: -1.8231810104156332
-    kappa: 0.000306095731888662
+    x: 587323.94099904317
+    y: 4141200.7652556747
+    theta: -1.8231957260134168
+    kappa: 0.000301513716596094
     s: 5.5877822870845213
-    dkappa: 0.00020199727297707279
+    dkappa: 0.00019964633822672548
     ddkappa: 0
   }
   v: 11.34897466968644
@@ -89,12 +89,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.6569329598
-    y: 4141199.66278466
-    theta: -1.8227890453673112
-    kappa: 0.00038193937575725217
+    x: 587323.65688528109
+    y: 4141199.6627969607
+    theta: -1.8228092466842774
+    kappa: 0.00037689568211472373
     s: 6.7262619217562447
-    dkappa: 0.00013445490531898704
+    dkappa: 0.00013170044020680084
     ddkappa: 0
   }
   v: 11.420653094862557
@@ -103,12 +103,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.37151924451
-    y: 4141198.553257749
-    theta: -1.8223189668690167
-    kappa: 0.00043982206160912175
+    x: 587323.37144611857
+    y: 4141198.5532765957
+    theta: -1.8223451236371926
+    kappa: 0.0004344743023102566
     s: 7.8719106420950222
-    dkappa: 0.00034548684919513976
+    dkappa: 0.00034299766181078065
     ddkappa: 0
   }
   v: 11.49226509633074
@@ -117,12 +117,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.08489720966
-    y: 4141197.4366737022
-    theta: -1.8217881604921344
-    kappa: 0.0004800378215877432
+    x: 587323.08479152434
+    y: 4141197.4367009061
+    theta: -1.82182057212881
+    kappa: 0.00047455245180283954
     s: 9.0246951672971942
-    dkappa: 0.00011546703303762476
+    dkappa: 0.00011243888952317808
     ddkappa: 0
   }
   v: 11.563285381195335
@@ -131,12 +131,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587322.79718263156
-    y: 4141196.3130894424
-    theta: -1.8212155216102652
-    kappa: 0.00050289467084597517
+    x: 587322.79703663732
+    y: 4141196.3131269692
+    theta: -1.8212542859628686
+    kappa: 0.00049744877239364682
     s: 10.18453192999805
-    dkappa: 0.00033934736418819917
+    dkappa: 0.00033681870126726318
     ddkappa: 0
   }
   v: 11.633233511131056
@@ -145,12 +145,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587322.50839908654
-    y: 4141195.1826328868
-    theta: -1.8206224535425757
-    kappa: 0.0005107896543905837
+    x: 587322.50820553862
+    y: 4141195.1826825612
+    theta: -1.8206674632545574
+    kappa: 0.00050554970557612562
     s: 11.351291561728866
-    dkappa: 0.00022154407750863038
+    dkappa: 0.00021891111637940743
     ddkappa: 0
   }
   v: 11.701673902382984
@@ -159,12 +159,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587322.21860821953
-    y: 4141194.0454648472
-    theta: -1.8200251539379897
-    kappa: 0.00050471891691682667
+    x: 587322.218360036
+    y: 4141194.0455284463
+    theta: -1.8200761097166314
+    kappa: 0.0004998539245923188
     s: 12.524803378373957
-    dkappa: 0.00023228568218327519
+    dkappa: 0.00022991186244901706
     ddkappa: 0
   }
   v: 11.768215825766566
@@ -173,12 +173,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587321.92786366
-    y: 4141192.9017862072
-    theta: -1.8194389683349463
-    kappa: 0.00048568739726988664
+    x: 587321.9275541408
+    y: 4141192.9018653994
+    theta: -1.8194953739281128
+    kappa: 0.00048136878721688963
     s: 13.704859865627686
-    dkappa: 0.00030974557994554975
+    dkappa: 0.000307865713343787
     ddkappa: 0
   }
   v: 11.832513406667619
@@ -187,12 +187,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587321.63621047
-    y: 4141191.7518333849
-    theta: -1.8188782949381217
-    kappa: 0.00045472198167369405
+    x: 587321.63583352335
+    y: 4141191.75192968
+    theta: -1.8189394524515083
+    kappa: 0.00045112346790388637
     s: 14.891221164451524
-    dkappa: 9.1367670481690941e-05
+    dkappa: 8.9474475266551243e-05
     ddkappa: 0
   }
   v: 11.894265625042324
@@ -201,12 +201,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587321.3437356432
-    y: 4141190.5958609115
-    theta: -1.8183615645177347
-    kappa: 0.00041204068868725779
+    x: 587321.34328567493
+    y: 4141190.595975684
+    theta: -1.8184264657384441
+    kappa: 0.00040935400723601719
     s: 16.083619556531083
-    dkappa: 0.00030717835239444416
+    dkappa: 0.00030627015677305356
     ddkappa: 0
   }
   v: 11.953216315417233
@@ -215,12 +215,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587321.05043131893
-    y: 4141189.4341714005
-    theta: -1.8179009439205458
-    kappa: 0.00035935558295030874
+    x: 587321.04990424379
+    y: 4141189.4343056437
+    theta: -1.8179683744133079
+    kappa: 0.00035776769793367542
     s: 17.281763949733133
-    dkappa: 0.00011719909885861133
+    dkappa: 0.0001165459658265495
     ddkappa: 0
   }
   v: 12.009154166889262
@@ -229,12 +229,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587320.75628186716
-    y: 4141188.2670887662
-    theta: -1.8175050094183838
-    kappa: 0.00029832151159315206
+    x: 587320.75567524764
+    y: 4141188.2672430603
+    theta: -1.8175735866829166
+    kappa: 0.00029801274659976395
     s: 18.485344363562671
-    dkappa: 0.00012483981375220333
+    dkappa: 0.00012494683667431318
     ddkappa: 0
   }
   v: 12.061912723125694
@@ -243,12 +243,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587320.46129254228
-    y: 4141187.0949463914
-    theta: -1.8171832532248342
-    kappa: 0.00023013671403008184
+    x: 587320.46060560877
+    y: 4141187.0951209008
+    theta: -1.8172513769848766
+    kappa: 0.00023128899921532464
     s: 19.69403641461993
-    dkappa: 0.00017534171044935055
+    dkappa: 0.00017639412522059379
     ddkappa: 0
   }
   v: 12.111370382364186
@@ -257,12 +257,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587320.16546084918
-    y: 4141185.9180897507
-    theta: -1.816941367199161
-    kappa: 0.00016334621919351306
+    x: 587320.16469474591
+    y: 4141185.9182841643
+    theta: -1.8170072570602163
+    kappa: 0.00016602269913819663
     s: 20.907505802057432
-    dkappa: -7.0917159187680885e-05
+    dkappa: -6.9566145750747074e-05
     ddkappa: 0
   }
   v: 12.157450397412751
@@ -271,12 +271,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587319.86880723468
-    y: 4141184.7368640876
-    theta: -1.8167762326106356
-    kappa: 0.00011433673286039129
+    x: 587319.86796581582
+    y: 4141184.7370774187
+    theta: -1.8168379537597714
+    kappa: 0.0001183712713009976
     s: 22.125412793037018
-    dkappa: 0.00014396804851761924
+    dkappa: 0.00014644429401714925
     ddkappa: 0
   }
   v: 12.200120875649779
@@ -285,12 +285,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587319.57131901057
-    y: 4141183.5516238916
-    theta: -1.8166576722394074
-    kappa: 8.688262262513839e-05
+    x: 587319.57040787756
+    y: 4141183.5518547231
+    theta: -1.8167136802309922
+    kappa: 9.209413558199404e-05
     s: 23.347416708186884
-    dkappa: 1.3811359661055692e-05
+    dkappa: 1.6588669874205203e-05
     ddkappa: 0
   }
   v: 12.239394779024025
@@ -299,12 +299,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587319.273045172
-    y: 4141182.3627044475
-    theta: -1.8165329492339264
-    kappa: 0.00013406417646724038
+    x: 587319.27207145281
+    y: 4141182.3629509816
+    theta: -1.816581940571832
+    kappa: 0.000140274093121969
     s: 24.573180442051317
-    dkappa: 5.0440620364567806e-05
+    dkappa: 5.3645341558813431e-05
     ddkappa: 0
   }
   v: 12.275331011342645
@@ -313,12 +313,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587318.97412538179
-    y: 4141181.1704101027
-    theta: -1.8163121376187334
-    kappa: 0.00023393361604768373
+    x: 587318.97309760342
+    y: 4141181.1706701927
+    theta: -1.8163530122767526
+    kappa: 0.00024096176873292263
     s: 25.802374833595707
-    dkappa: 0.00023602835116580927
+    dkappa: 0.00023992780666946741
     ddkappa: 0
   }
   v: 12.308025183343146
@@ -327,12 +327,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587318.67479855148
-    y: 4141179.9750098139
-    theta: -1.8159703145183623
-    kappa: 0.00031945059866172207
+    x: 587318.67372721434
+    y: 4141179.9752808111
+    theta: -1.8160021315018016
+    kappa: 0.00032710184046946426
     s: 27.034680974740823
-    dkappa: 7.0823333797433266e-05
+    dkappa: 7.4514374499472284e-05
     ddkappa: 0
   }
   v: 12.337584481442956
@@ -341,12 +341,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587318.37528722663
-    y: 4141178.7767653437
-    theta: -1.8155277935760219
-    kappa: 0.00038803517167150308
+    x: 587318.37418405106
+    y: 4141178.7770443014
+    theta: -1.81554985608907
+    kappa: 0.00039611053765856326
     s: 28.269791044258881
-    dkappa: 0.0002475540973485649
+    dkappa: 0.0002517617342175955
     ddkappa: 0
   }
   v: 12.364123456253351
@@ -355,12 +355,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587318.07572370383
-    y: 4141177.5759489788
-    theta: -1.8150119349813423
-    kappa: 0.00044216221125207883
+    x: 587318.07460016757
+    y: 4141177.5762330173
+    theta: -1.8150238311502811
+    kappa: 0.00045047746808440773
     s: 29.507409044192912
-    dkappa: 0.0002549466787540974
+    dkappa: 0.00025919522503643284
     ddkappa: 0
   }
   v: 12.38776402257945
@@ -369,12 +369,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587317.77625126066
-    y: 4141176.3728174022
-    theta: -1.814438756627518
-    kappa: 0.00048281993609806744
+    x: 587317.775119225
+    y: 4141176.3731035572
+    theta: -1.8144402902374939
+    kappa: 0.00049118990067813456
     s: 30.747251536276156
-    dkappa: 0.00015852570198082481
+    dkappa: 0.00016249578366524554
     ddkappa: 0
   }
   v: 12.408635459420214
@@ -383,12 +383,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587317.47703091067
-    y: 4141175.1676093256
-    theta: -1.8138218336515841
-    kappa: 0.00051047388489832259
+    x: 587317.475902987
+    y: 4141175.1678944603
+    theta: -1.813813040118269
+    kappa: 0.00051870296535927365
     s: 31.989048378351438
-    dkappa: 0.00037475920615987179
+    dkappa: 0.00037910160324027844
     ddkappa: 0
   }
   v: 12.426874409968452
@@ -397,12 +397,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587317.17822175915
-    y: 4141173.9605496638
-    theta: -1.8131755808754551
-    kappa: 0.0005256560127465176
+    x: 587317.17711111577
+    y: 4141173.9608305213
+    theta: -1.8131567716974348
+    kappa: 0.00053353627154036467
     s: 33.232543460790552
-    dkappa: 0.00021738989130829509
+    dkappa: 0.00022113367682016766
     ddkappa: 0
   }
   v: 12.442624881610817
@@ -411,12 +411,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587316.87982382369
-    y: 4141172.7518875916
-    theta: -1.8125166887268627
-    kappa: 0.00053093992568569609
+    x: 587316.87874182314
+    y: 4141172.7521613804
+    theta: -1.8124883857622054
+    kappa: 0.0005382839390511737
     s: 34.47749544291365
-    dkappa: 0.00024599460439928293
+    dkappa: 0.00024949939295855424
     ddkappa: 0
   }
   v: 12.456038245927804
@@ -425,12 +425,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587316.58189635689
-    y: 4141171.5418415959
-    theta: -1.8118560710678357
-    kappa: 0.00052757340356700917
+    x: 587316.58085366269
+    y: 4141171.5421057083
+    theta: -1.8118190171341757
+    kappa: 0.0005341933334513919
     s: 35.723678489408641
-    dkappa: 0.00035106853478011262
+    dkappa: 0.00035442258293253272
     ddkappa: 0
   }
   v: 12.467273238693755
@@ -439,12 +439,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587316.284523463
-    y: 4141170.3306073537
-    theta: -1.8112039821231578
-    kappa: 0.000516448049862545
+    x: 587316.28353018884
+    y: 4141170.3308593351
+    theta: -1.8111591998274581
+    kappa: 0.00052214671880111653
     s: 36.97088300675054
-    dkappa: 0.00013789238478524249
+    dkappa: 0.0001402618430363466
     ddkappa: 0
   }
   v: 12.476495959876855
@@ -453,12 +453,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587315.98778936663
-    y: 4141169.1183631131
-    theta: -1.8105710066779823
-    kappa: 0.00049852696372991033
+    x: 587315.98685475381
+    y: 4141169.1186007373
+    theta: -1.8105198666203957
+    kappa: 0.000503093600840214
     s: 38.218916379620879
-    dkappa: 0.0002940107441705013
+    dkappa: 0.00029616883649245281
     ddkappa: 0
   }
   v: 12.483879873639133
@@ -467,12 +467,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587315.69162607659
-    y: 4141167.9053061251
-    theta: -1.8099625342635766
-    kappa: 0.000475802171399269
+    x: 587315.69075669965
+    y: 4141167.9055278245
+    theta: -1.8099065069417057
+    kappa: 0.00047904818730805273
     s: 39.4676037073271
-    dkappa: 0.00025454579356146216
+    dkappa: 0.000255934481115784
     ddkappa: 0
   }
   v: 12.489605808336464
@@ -481,12 +481,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587315.396036897
-    y: 4141166.6915970542
-    theta: -1.8093844015575946
-    kappa: 0.00044630603103303047
+    x: 587315.39523754525
+    y: 4141166.6918017021
+    theta: -1.8093251487009836
+    kappa: 0.000448133383808069
     s: 40.7167885402219
-    dkappa: 0.00012467739493273171
+    dkappa: 0.00012510290792428207
     ddkappa: 0
   }
   v: 12.493861956518566
@@ -495,12 +495,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587315.1010452332
-    y: 4141165.4773718864
-    theta: -1.8088509408564226
-    kappa: 0.00040374153770245282
+    x: 587315.10031882883
+    y: 4141165.4775588145
+    theta: -1.8087902048897191
+    kappa: 0.00040425214043497877
     s: 41.966333616122654
-    dkappa: 0.00030897349378018117
+    dkappa: 0.00030925762347394159
     ddkappa: 0
   }
   v: 12.496843874929002
@@ -509,12 +509,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587314.80664616043
-    y: 4141164.2627529576
-    theta: -1.8083835314813483
-    kappa: 0.00034705815991569692
+    x: 587314.80599356291
+    y: 4141164.2629219983
+    theta: -1.808322966188475
+    kappa: 0.00034640864959068591
     s: 43.216121596730787
-    dkappa: 0.00011519519157744149
+    dkappa: 0.0001145401487080718
     ddkappa: 0
   }
   v: 12.498754484505179
@@ -523,12 +523,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587314.512714021
-    y: 4141163.0478705019
-    theta: -1.8079914111599396
-    kappa: 0.00027890794267696042
+    x: 587314.5121342421
+    y: 4141163.0480219265
+    theta: -1.8079323198984079
+    kappa: 0.00027723915055813
     s: 44.466055804051152
-    dkappa: 9.9886059299144627e-05
+    dkappa: 9.88057296471937e-05
     ddkappa: 0
   }
   v: 12.49980407037835
@@ -537,12 +537,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587314.2191743562
-    y: 4141161.8328201631
-    theta: -1.8076876233770423
-    kappa: 0.00020037639835598044
+    x: 587314.218665019
+    y: 4141161.8329545725
+    theta: -1.8076311454726579
+    kappa: 0.00019782924048577765
     s: 45.716060956811425
-    dkappa: 0.00018007624272695585
+    dkappa: 0.00017886393476391417
     ddkappa: 0
   }
   v: 12.500210281873608
@@ -551,12 +551,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587313.92594261025
-    y: 4141160.6176771624
-    theta: -1.8074900664350757
-    kappa: 0.00011205376443310481
+    x: 587313.925499835
+    y: 4141160.6177955121
+    theta: -1.8074372264158141
+    kappa: 0.00010877816932227795
     s: 46.966083906881479
-    dkappa: -7.7060542896579008e-05
+    dkappa: -7.9102740989487735e-05
     ddkappa: 0
   }
   v: 12.500198132509897
@@ -565,12 +565,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587313.63289010932
-    y: 4141159.4025037587
-    theta: -1.8074152505619261
-    kappa: 1.4848759703137191e-05
+    x: 587313.63250863738
+    y: 4141159.4026073245
+    theta: -1.8073669110612718
+    kappa: 1.1000455047847149e-05
     s: 48.216094375692762
-    dkappa: 3.0130036673812788e-05
+    dkappa: 2.8205656677141204e-05
     ddkappa: 0
   }
   v: 12.5
@@ -579,12 +579,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587313.33985826652
-    y: 4141158.187346152
-    theta: -1.8074610825353012
-    kappa: -8.9014296054269531e-05
+    x: 587313.33953247289
+    y: 4141158.1874362929
+    theta: -1.8074178427319849
+    kappa: -9.3294328362566323e-05
     s: 49.466084643923985
-    dkappa: -3.6623840485880117e-05
+    dkappa: -3.8773143989058838e-05
     ddkappa: 0
   }
   v: 12.499813698192774
@@ -593,12 +593,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587313.04671385384
-    y: 4141156.9722323958
-    theta: -1.8076000717523768
-    kappa: -0.00012005371593079555
+    x: 587313.04643752682
+    y: 4141156.972310605
+    theta: -1.8075623670944589
+    kappa: -0.00012462555446500902
     s: 50.716058679915619
-    dkappa: -0.00014511285885222152
+    dkappa: -0.00014768013099755254
     ddkappa: 0
   }
   v: 12.499674344553263
@@ -607,12 +607,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587312.75338990428
-    y: 4141155.7571741426
-    theta: -1.807741144855175
-    kappa: -0.00010398863123809232
+    x: 587312.75315593462
+    y: 4141155.7572421264
+    theta: -1.8077092536026476
+    kappa: -0.00010870013248807784
     s: 51.966020880607211
-    dkappa: 4.8167698170382857e-05
+    dkappa: 4.6061435093353841e-05
     ddkappa: 0
   }
   v: 12.499576034729541
@@ -621,12 +621,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587312.45990300819
-    y: 4141154.5421634791
-    theta: -1.8078591001660851
-    kappa: -8.6373242063170148e-05
+    x: 587312.45970389689
+    y: 4141154.5422230442
+    theta: -1.8078331036945909
+    kappa: -9.1069735581721271e-05
     s: 53.215975068460835
-    dkappa: -7.3792407441977415e-05
+    dkappa: -7.6222696919768642e-05
     ddkappa: 0
   }
   v: 12.499513183523833
@@ -635,12 +635,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587312.16629273456
-    y: 4141153.3271874939
-    theta: -1.8079565160469064
-    kappa: -6.9823354277768132e-05
+    x: 587312.16612165654
+    y: 4141153.327240285
+    theta: -1.807936309220266
+    kappa: -7.4363830528233622e-05
     s: 54.4659245233765
-    dkappa: -6.08047893607575e-05
+    dkappa: -6.3108354882748547e-05
     ddkappa: 0
   }
   v: 12.499480524892531
@@ -649,12 +649,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587311.87258065294
-    y: 4141152.1122381361
-    theta: -1.8080352630491234
-    kappa: -5.5408210117993854e-05
+    x: 587311.87243099569
+    y: 4141152.1122857491
+    theta: -1.8080205764770472
+    kappa: -5.9651661561686646e-05
     s: 55.715872014607548
-    dkappa: 6.7147833828841842e-05
+    dkappa: 6.5346818609548292e-05
     ddkappa: 0
   }
   v: 12.499473111946177
@@ -663,12 +663,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587311.57877923606
-    y: 4141150.8973100418
-    theta: -1.8080973963875666
-    kappa: -4.45780208669943e-05
+    x: 587311.57864432747
+    y: 4141150.8973540878
+    theta: -1.8080877469083825
+    kappa: -4.8371946562225826e-05
     s: 56.9658198326761
-    dkappa: -0.0001151590004243748
+    dkappa: -0.00011721395500620483
     ddkappa: 0
   }
   v: 12.499486316949477
@@ -677,12 +677,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587311.28490605322
-    y: 4141149.6823970713
-    theta: -1.8081484923251065
-    kappa: -3.8705237287827533e-05
+    x: 587311.28477993
+    y: 4141149.682438992
+    theta: -1.8081431938386723
+    kappa: -4.1896408126275465e-05
     s: 58.215769821288433
-    dkappa: 1.2354566546974716e-05
+    dkappa: 1.0978143061950025e-05
     ddkappa: 0
   }
   v: 12.499515831321295
@@ -691,12 +691,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587310.99097569194
-    y: 4141148.46749423
-    theta: -1.8081960158347798
-    kappa: -3.8585591781210174e-05
+    x: 587310.990853751
+    y: 4141148.4675351391
+    theta: -1.8081942588757738
+    kappa: -4.1033002101609426e-05
     s: 59.465723409250451
-    dkappa: 1.2848593318798392e-05
+    dkappa: 1.1873166202996427e-05
     ddkappa: 0
   }
   v: 12.499557665634651
@@ -705,12 +705,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587310.69698798133
-    y: 4141147.2526004855
-    theta: -1.8082467205188977
-    kappa: -4.3579244201084556e-05
+    x: 587310.69686662336
+    y: 4141147.2526412541
+    theta: -1.8082475531619724
+    kappa: -4.520748825932236e-05
     s: 60.715681642383032
-    dkappa: -0.0001201841053820942
+    dkappa: -0.00012110057298048092
     ddkappa: 0
   }
   v: 12.499608149616723
@@ -719,12 +719,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587310.40292964
-    y: 4141146.0377183403
-    theta: -1.8083060461784368
-    kappa: -5.1602038023312145e-05
+    x: 587310.40280624072
+    y: 4141146.037759603
+    theta: -1.8083084296009662
+    kappa: -5.2473948171000913e-05
     s: 61.965645215437505
-    dkappa: 6.47087828913021e-05
+    dkappa: 6.4669175024696261e-05
     ddkappa: 0
   }
   v: 12.499663932148849
@@ -733,12 +733,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587310.10878759937
-    y: 4141144.8228505775
-    theta: -1.8083778926590082
-    kappa: -6.3204924821933634e-05
+    x: 587310.10866080318
+    y: 4141144.8228926631
+    theta: -1.8083809077619186
+    kappa: -6.3409858179163626e-05
     s: 63.215614504011029
-    dkappa: -6.23667529820288e-05
+    dkappa: -6.2422966645451041e-05
     ddkappa: 0
   }
   v: 12.499721981266525
@@ -747,12 +747,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587309.814551143
-    y: 4141143.6079997071
-    theta: -1.8084662578409938
-    kappa: -7.922710066181236e-05
+    x: 587309.81442067167
+    y: 4141143.6080426825
+    theta: -1.8084691559202093
+    kappa: -7.8847945020077463e-05
     s: 64.465589596462024
-    dkappa: -8.0157556581791179e-05
+    dkappa: -7.9956789884143991e-05
     ddkappa: 0
   }
   v: 12.499779584159405
@@ -761,12 +761,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587309.52019870165
-    y: 4141142.3931711344
-    theta: -1.8085768631387187
-    kappa: -0.00010066490860024221
+    x: 587309.5200650699
+    y: 4141142.3932148754
+    theta: -1.8085789920485513
+    kappa: -9.9784552189181335e-05
     s: 65.715570325825581
-    dkappa: 4.6523806194676482e-05
+    dkappa: 4.7329815346039308e-05
     ddkappa: 0
   }
   v: 12.499834347171305
@@ -775,12 +775,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587309.22568352742
-    y: 4141141.1783766057
-    theta: -1.8087202686306623
-    kappa: -0.00012927662398046707
+    x: 587309.22554811765
+    y: 4141141.1784207774
+    theta: -1.8087210274517518
+    kappa: -0.00012798614279894104
     s: 66.9655563017289
-    dkappa: -0.00016136187500034204
+    dkappa: -0.00016089034527703902
     ddkappa: 0
   }
   v: 12.49988419580019
@@ -789,12 +789,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587308.930963297
-    y: 4141139.9636270106
-    theta: -1.8089055646400853
-    kappa: -0.00016614562608595169
+    x: 587308.9308281194
+    y: 4141139.9636711255
+    theta: -1.8089044855089802
+    kappa: -0.00016453365881849506
     s: 68.215546942306645
-    dkappa: -6.6526434187629826e-05
+    dkappa: -6.5592627218081812e-05
     ddkappa: 0
   }
   v: 12.499927374698194
@@ -803,12 +803,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587308.63599710062
-    y: 4141138.7489330815
-    theta: -1.8091405978877935
-    kappa: -0.00021197402819543096
+    x: 587308.63586450624
+    y: 4141138.74897657
+    theta: -1.8091373461501092
+    kappa: -0.0002101234674038787
     s: 69.465541506116423
-    dkappa: -7.5467984216944355e-05
+    dkappa: -7.4394186399798963e-05
     ddkappa: 0
   }
   v: 12.499962447671603
@@ -817,12 +817,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587308.34072390466
-    y: 4141137.5343106021
-    theta: -1.8094361951405964
-    kappa: -0.00026789908656953495
+    x: 587308.34059656854
+    y: 4141137.5343528115
+    theta: -1.8094305327091775
+    kappa: -0.00026589282499451524
     s: 70.7155391240542
-    dkappa: -0.00025345848590183572
+    dkappa: -0.0002527202239392672
     ddkappa: 0
   }
   v: 12.499988297680861
@@ -831,12 +831,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587308.04501761938
-    y: 4141136.3197913421
-    theta: -1.8098142988555628
-    kappa: -0.00033668427457963357
+    x: 587308.04489876342
+    y: 4141136.3198314877
+    theta: -1.8098060792500712
+    kappa: -0.00033461411040876504
     s: 71.965538831269654
-    dkappa: -9.57459285515112e-05
+    dkappa: -9.4463223795833082e-05
     ddkappa: 0
   }
   v: 12.500004126840574
@@ -845,12 +845,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587307.74878641311
-    y: 4141135.1053989264
-    theta: -1.81028864979483
-    kappa: -0.00041970776234413916
+    x: 587307.74867921509
+    y: 4141135.1054362278
+    theta: -1.8102778490970854
+    kappa: -0.00041766161506668514
     s: 73.215539599081666
-    dkappa: -0.000249759394738104
+    dkappa: -0.00024880933844398861
     ddkappa: 0
   }
   v: 12.500009456419505
@@ -859,12 +859,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587307.4519381118
-    y: 4141133.89115722
-    theta: -1.8108722260810433
-    kappa: -0.00051834700468837659
+    x: 587307.45184553938
+    y: 4141133.8911909452
+    theta: -1.8108589237128376
+    kappa: -0.00051640776315197887
     s: 74.465540366893663
-    dkappa: -0.00032608568134296316
+    dkappa: -0.00032526454568364223
     ddkappa: 0
   }
   v: 12.500004126840574
@@ -873,12 +873,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587307.15434030467
-    y: 4141132.6771000866
-    theta: -1.8115842387960226
-    kappa: -0.00063499624814152358
+    x: 587307.15426518058
+    y: 4141132.677129535
+    theta: -1.8115686107524926
+    kappa: -0.00063324679830633593
     s: 75.715540074109128
-    dkappa: -0.00023148091491100302
+    dkappa: -0.00023035052682768113
     ddkappa: 0
   }
   v: 12.49998829768086
@@ -887,12 +887,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587306.855736931
-    y: 4141131.4632920539
-    theta: -1.8124042733412684
-    kappa: -0.00066494374013160735
+    x: 587306.85568209016
+    y: 4141131.4633165123
+    theta: -1.8123866364403922
+    kappa: -0.00066347783862967351
     s: 76.9655376920469
-    dkappa: -0.00040910932372187358
+    dkappa: -0.00040867492797186654
     ddkappa: 0
   }
   v: 12.499962447671601
@@ -901,12 +901,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587306.55612109636
-    y: 4141130.2497366965
-    theta: -1.8132390734253232
-    kappa: -0.00066985981714828076
+    x: 587306.55608873791
+    y: 4141130.2497556042
+    theta: -1.8132198401470783
+    kappa: -0.00066876358224919065
     s: 78.215532255856687
-    dkappa: -0.00032009212815996431
+    dkappa: -0.00031957324824821612
     ddkappa: 0
   }
   v: 12.499927374698192
@@ -915,12 +915,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587306.25553251663
-    y: 4141129.036425964
-    theta: -1.8140778231756469
-    kappa: -0.000671619781385688
+    x: 587306.25552421261
+    y: 4141129.0364389122
+    theta: -1.8140574871680812
+    kappa: -0.000670976108130982
     s: 79.465522896434422
-    dkappa: -0.00029306909645671324
+    dkappa: -0.00029271443870186039
     ddkappa: 0
   }
   v: 12.499884195800188
@@ -929,12 +929,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587305.95396862633
-    y: 4141127.8233620632
-    theta: -1.8149172269465625
-    kappa: -0.00067126528265138558
+    x: 587305.95398533961
+    y: 4141127.8233687924
+    theta: -1.8148963638107349
+    kappa: -0.000671113098366712
     s: 80.715508872337722
-    dkappa: -0.000438709626867841
+    dkappa: -0.00043904443650701834
     ddkappa: 0
   }
   v: 12.499834347171301
@@ -943,12 +943,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587305.65130368085
-    y: 4141126.61057782
-    theta: -1.8157558984959687
-    kappa: -0.00067067722538736638
+    x: 587305.65134568023
+    y: 4141126.610578239
+    theta: -1.8157351427505031
+    kappa: -0.00067096553130515323
     s: 81.9654896017013
-    dkappa: -0.00026305733470048437
+    dkappa: -0.00026305606130454543
     ddkappa: 0
   }
   v: 12.499779584159404
@@ -957,12 +957,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587305.34762468992
-    y: 4141125.3980529164
-    theta: -1.8165939629715007
-    kappa: -0.00067031948735579976
+    x: 587305.347691509
+    y: 4141125.3980471194
+    theta: -1.8165738256957276
+    kappa: -0.00067098748388888779
     s: 83.215464694152274
-    dkappa: -0.00034852824180043235
+    dkappa: -0.00034897562024814572
     ddkappa: 0
   }
   v: 12.499721981266521
@@ -971,12 +971,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587305.04297339125
-    y: 4141124.1857779324
-    theta: -1.8174317999075584
-    kappa: -0.00067034211228356893
+    x: 587305.04306405107
+    y: 4141124.1857661447
+    theta: -1.8174127008693179
+    kappa: -0.00067133141526210427
     s: 84.465433982725813
-    dkappa: -0.00038275966519090386
+    dkappa: -0.00038345657564935263
     ddkappa: 0
   }
   v: 12.499663932148845
@@ -985,12 +985,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587304.737349957
-    y: 4141122.9737535417
-    theta: -1.818269952579328
-    kappa: -0.00067092324058718681
+    x: 587304.73746304284
+    y: 4141122.9737360994
+    theta: -1.8182522435282402
+    kappa: -0.00067217546653422717
     s: 85.715397555780285
-    dkappa: -0.00022869638712249109
+    dkappa: -0.00022903835038724281
     ddkappa: 0
   }
   v: 12.499608149616719
@@ -999,12 +999,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587304.43062037928
-    y: 4141121.7620141231
-    theta: -1.8191094825178409
-    kappa: -0.00067241375802991755
+    x: 587304.43075384246
+    y: 4141121.7619915232
+    theta: -1.8190934762300792
+    kappa: -0.0006738625840184921
     s: 86.965355788912859
-    dkappa: -0.00040744503890742371
+    dkappa: -0.00040842519889647472
     ddkappa: 0
   }
   v: 12.499557665634647
@@ -1013,12 +1013,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587304.12288082426
-    y: 4141120.5505356155
-    theta: -1.8199515671987554
-    kappa: -0.00067503944482663137
+    x: 587304.12303247675
+    y: 4141120.5505083953
+    theta: -1.81993746860614
+    kappa: -0.00067662460854993529
     s: 88.21530937687487
-    dkappa: -0.00032877370302300708
+    dkappa: -0.00032955559553869018
     ddkappa: 0
   }
   v: 12.499515831321292
@@ -1027,12 +1027,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587303.81416200288
-    y: 4141119.3393100011
-    theta: -1.8207976218192128
-    kappa: -0.00067909915092816879
+    x: 587303.81432954234
+    y: 4141119.3392787324
+    theta: -1.820785559045069
+    kappa: -0.00068076226973524685
     s: 89.465259365487213
-    dkappa: -0.00029001177662259762
+    dkappa: -0.00029069822427866046
     ddkappa: 0
   }
   v: 12.499486316949474
@@ -1041,12 +1041,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587303.50445917156
-    y: 4141118.1283378392
-    theta: -1.821649523853506
-    kappa: -0.00068498064352376061
+    x: 587303.50464018923
+    y: 4141118.1283031236
+    theta: -1.8216395570103938
+    kappa: -0.00068666333450651234
     s: 90.715207183555762
-    dkappa: -0.000456564026485767
+    dkappa: -0.00045774908883788635
     ddkappa: 0
   }
   v: 12.499473111946173
@@ -1055,12 +1055,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587303.19361999712
-    y: 4141116.9176572021
-    theta: -1.8225112522560543
-    kappa: -0.00069355814104071409
+    x: 587303.19381171488
+    y: 4141116.9176197397
+    theta: -1.82250336216902
+    kappa: -0.000695193539979095
     s: 91.965154674786817
-    dkappa: -0.00028115907992106511
+    dkappa: -0.00028174882305386233
     ddkappa: 0
   }
   v: 12.499480524892528
@@ -1069,12 +1069,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587302.88174720388
-    y: 4141115.7072404064
-    theta: -1.8233854222354191
-    kappa: -0.00070522333442384446
+    x: 587302.88194719609
+    y: 4141115.7072008131
+    theta: -1.8233795130310755
+    kappa: -0.00070675171454001964
     s: 93.215104129702482
-    dkappa: -0.00036246939539150375
+    dkappa: -0.00036323105781050491
     ddkappa: 0
   }
   v: 12.499513183523831
@@ -1083,12 +1083,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587302.568848128
-    y: 4141114.4970836192
-    theta: -1.8242757646362859
-    kappa: -0.0007206232547339801
+    x: 587302.56905418169
+    y: 4141114.497042458
+    theta: -1.8242716728092874
+    kappa: -0.00072198622870902045
     s: 94.4650583175561
-    dkappa: -0.0004193460793517279
+    dkappa: -0.000420156893508572
     ddkappa: 0
   }
   v: 12.499576034729538
@@ -1097,12 +1097,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587302.25490690966
-    y: 4141113.2871884685
-    theta: -1.825186985798422
-    kappa: -0.0007405703444053846
+    x: 587302.25511703466
+    y: 4141113.2871462507
+    theta: -1.8251844798253347
+    kappa: -0.00074170952195534408
     s: 95.715020518247684
-    dkappa: -0.00025831756984088264
+    dkappa: -0.00025847610506098655
     ddkappa: 0
   }
   v: 12.499674344553261
@@ -1111,12 +1111,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587301.93971687206
-    y: 4141112.0776058221
-    theta: -1.8261303456080449
-    kappa: -0.00076711238684180969
+    x: 587301.939929183
+    y: 4141112.0775630353
+    theta: -1.826129068967798
+    kappa: -0.000767960332274723
     s: 96.964994554239325
-    dkappa: -0.00045566205492513553
+    dkappa: -0.0004562296103931206
     ddkappa: 0
   }
   v: 12.499813698192773
@@ -1125,12 +1125,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587301.623394863
-    y: 4141110.8683019625
-    theta: -1.8271102875922642
-    kappa: -0.00080080056621155838
+    x: 587301.6236081369
+    y: 4141110.8682589242
+    theta: -1.8271098489127562
+    kappa: -0.00080129809131165223
     s: 98.214984822470541
-    dkappa: -0.00040505138831258957
+    dkappa: -0.0004052108302854987
     ddkappa: 0
   }
   v: 12.499999999999998
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_rightturn_01_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_rightturn_01_0.pb.txt
index 3e61b7d8a189f5b32a8c8cdc8a47ea73abe27901..acf520a88b08a4a28ba7d015dda253da7fad631e 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_rightturn_01_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_rightturn_01_0.pb.txt
@@ -14,1127 +14,1127 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.4499998092651367
-  a: -0.020740223155960698
+  a: -0.02131327714622943
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 587695.64173868578
-    y: 4141446.7904223106
-    theta: -0.21716596100997837
-    kappa: -0.0081798528718663114
-    s: 0.5449597101101078
-    dkappa: -0.0048979333516837344
+    x: 587695.64173573651
+    y: 4141446.7904224489
+    theta: -0.21716742442743286
+    kappa: -0.0081827734790423882
+    s: 0.54495680013167769
+    dkappa: -0.004900838601738529
     ddkappa: 0
   }
-  v: 5.4497549592099741
-  a: 0.012919502933644036
+  v: 5.4496962973556551
+  a: 0.012318737789389411
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 587696.1738896966
-    y: 4141446.6724451357
-    theta: -0.22174006916302091
-    kappa: -0.00914067506505435
-    s: 1.0900346403967625
-    dkappa: -0.0083301317827331883
+    x: 587696.17387784214
+    y: 4141446.6724463324
+    theta: -0.22174339101915555
+    kappa: -0.009146447751171417
+    s: 1.0900228105279464
+    dkappa: -0.00833552655953241
     ddkappa: 0
   }
-  v: 5.4520247157402864
-  a: 0.029809405675862703
+  v: 5.4519044722908969
+  a: 0.029178216761474217
   relative_time: 0.2
 }
 trajectory_point {
   path_point {
-    x: 587696.70542562183
-    y: 4141446.5504309675
-    theta: -0.22716918663737454
-    kappa: -0.010257155796356011
-    s: 1.6353949773729979
-    dkappa: -0.0045373475810283025
+    x: 587696.70539816271
+    y: 4141446.5504322695
+    theta: -0.22717660449703075
+    kappa: -0.01026549996434771
+    s: 1.6353679147635931
+    dkappa: -0.0045425166820278638
     ddkappa: 0
   }
-  v: 5.4552093456580311
-  a: 0.031474467804625726
+  v: 5.4550244075784793
+  a: 0.030811701849449785
   relative_time: 0.30000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587697.23679361923
-    y: 4141446.4263788657
-    theta: -0.23300650423536196
-    kappa: -0.011438694943241243
-    s: 2.1810586591457133
-    dkappa: -0.00376369199080189
+    x: 587697.23674377007
+    y: 4141446.4263804704
+    theta: -0.23301867360101364
+    kappa: -0.011449471777238471
+    s: 2.1810097364745635
+    dkappa: -0.0037688838094957744
     ddkappa: 0
   }
-  v: 5.45786361403856
-  a: 0.019459672053863515
+  v: 5.4576108309339642
+  a: 0.018765735132740921
   relative_time: 0.4
 }
 trajectory_point {
   path_point {
-    x: 587697.76743768575
-    y: 4141446.2984449184
-    theta: -0.23963575425456746
-    kappa: -0.012746230172928124
-    s: 2.7269068252430126
-    dkappa: -0.0088785492366789936
+    x: 587697.76735776046
+    y: 4141446.2984446422
+    theta: -0.23965426319136415
+    kappa: -0.012759219498612881
+    s: 2.7268291047787572
+    dkappa: -0.0088860910443987265
     ddkappa: 0
   }
-  v: 5.4586967842306171
-  a: -0.0046899988424934994
+  v: 5.4583731242808522
+  a: -0.0054131413092275912
   relative_time: 0.5
 }
 trajectory_point {
   path_point {
-    x: 587698.29712553986
-    y: 4141446.1668898319
-    theta: -0.24698651985139214
-    kappa: -0.014156027388206868
-    s: 3.2726992664415442
-    dkappa: -0.0095338115797060918
+    x: 587698.29700783989
+    y: 4141446.16688703
+    theta: -0.24701206593920633
+    kappa: -0.014171007178370323
+    s: 3.27258551969682
+    dkappa: -0.0095437782805857
     ddkappa: 0
   }
-  v: 5.45657261785634
-  a: -0.039429562150514859
+  v: 5.4561753237505908
+  a: -0.040178385397030905
   relative_time: 0.6
 }
 trajectory_point {
   path_point {
-    x: 587698.82556032692
-    y: 4141446.0319543239
-    theta: -0.25502317682187714
-    kappa: -0.015662760802615291
-    s: 3.8180898745938388
-    dkappa: -0.0059220566653021087
+    x: 587698.8253963578
+    y: 4141446.0319461864
+    theta: -0.25505688051447978
+    kappa: -0.015679581489362698
+    s: 3.8179326175729407
+    dkappa: -0.0059316530721728634
     ddkappa: 0
   }
-  v: 5.450509374811257
-  a: -0.083214035136270237
+  v: 5.450036119682566
+  a: -0.083983455051244271
   relative_time: 0.7
 }
 trajectory_point {
   path_point {
-    x: 587699.35164904571
-    y: 4141445.891416478
-    theta: -0.26414504985012105
-    kappa: -0.017310413977057335
-    s: 4.3626420924556513
-    dkappa: -0.0084073075839911464
+    x: 587699.3514303125
+    y: 4141445.8914018748
+    theta: -0.26418739165844579
+    kappa: -0.017328759734624867
+    s: 4.3624336364956431
+    dkappa: -0.0084151968011704111
     ddkappa: 0
   }
-  v: 5.4396798132642923
-  a: -0.13449843506582917
+  v: 5.4391288566241087
+  a: -0.13528180819244293
   relative_time: 0.79999999999999993
 }
 trajectory_point {
   path_point {
-    x: 587699.87571782118
-    y: 4141445.7485157228
-    theta: -0.2737568354779395
-    kappa: -0.019020828006468975
-    s: 4.905844363513296
-    dkappa: -0.013729158786733371
+    x: 587699.875435246
+    y: 4141445.7484913627
+    theta: -0.27380868908066208
+    kappa: -0.019040631680471546
+    s: 4.9055768817185816
+    dkappa: -0.013738783609940066
     ddkappa: 0
   }
-  v: 5.4234111896577613
-  a: -0.19173777920526131
+  v: 5.4227815333304923
+  a: -0.1925269027412021
   relative_time: 0.89999999999999991
 }
 trajectory_point {
   path_point {
-    x: 587700.39553894009
-    y: 4141445.5976605411
-    theta: -0.28485641176065751
-    kappa: -0.020910681183381748
-    s: 5.447125581810992
-    dkappa: -0.011680034248361924
+    x: 587700.3951836
+    y: 4141445.5976257394
+    theta: -0.28491774460769476
+    kappa: -0.020931526001779319
+    s: 5.446791191081334
+    dkappa: -0.011693889421552525
     ddkappa: 0
   }
-  v: 5.4011852587073745
-  a: -0.2533870848206361
+  v: 5.40047680276493
+  a: -0.2541721966180967
   relative_time: 0.99999999999999989
 }
 trajectory_point {
   path_point {
-    x: 587700.91249042377
-    y: 4141445.4459789814
-    theta: -0.29617994137633175
-    kappa: -0.022825296554383465
-    s: 5.9858705417781968
-    dkappa: -0.0083080249602725982
+    x: 587700.91205288784
+    y: 4141445.44592955
+    theta: -0.29625164795878117
+    kappa: -0.022847210291229516
+    s: 5.9854614004301983
+    dkappa: -0.0083213827863978379
     ddkappa: 0
   }
-  v: 5.3726382734022344
-  a: -0.31790136917802314
+  v: 5.3718519720985825
+  a: -0.31867114774370248
   relative_time: 1.0999999999999999
 }
 trajectory_point {
   path_point {
-    x: 587701.42283501069
-    y: 4141445.28356612
-    theta: -0.3094392062345202
-    kappa: -0.024956070559507441
-    s: 6.5214353880569496
-    dkappa: -0.013801991592857263
+    x: 587701.4223066516
+    y: 4141445.2835034914
+    theta: -0.30952039844653256
+    kappa: -0.024978532790812483
+    s: 6.5209438090389868
+    dkappa: -0.01380984188280128
     ddkappa: 0
   }
-  v: 5.337560985004834
-  a: -0.38373564954349232
+  v: 5.3366990027105476
+  a: -0.38447721403859403
   relative_time: 1.2
 }
 trajectory_point {
   path_point {
-    x: 587701.9289298316
-    y: 4141445.1205343348
-    theta: -0.32289744951677612
-    kappa: -0.027102404709506409
-    s: 7.0531630653292048
-    dkappa: -0.018138048080546157
+    x: 587701.92830510577
+    y: 4141445.1204652046
+    theta: -0.32298709189768604
+    kappa: -0.027125266245865542
+    s: 7.0525816450298127
+    dkappa: -0.018155860182313941
     ddkappa: 0
   }
-  v: 5.2958986430510642
-  a: -0.44934494318311285
+  v: 5.2949645101878682
+  a: -0.4500438534233473
   relative_time: 1.3
 }
 trajectory_point {
   path_point {
-    x: 587702.42688649055
-    y: 4141444.94728119
-    theta: -0.33815693074343489
-    kappa: -0.029430989376745157
-    s: 7.5803987681441791
-    dkappa: -0.015039016291834907
+    x: 587702.42615002
+    y: 4141444.9471855285
+    theta: -0.33825750407477273
+    kappa: -0.029454203651468502
+    s: 7.579720530793896
+    dkappa: -0.015056142335034967
     ddkappa: 0
   }
-  v: 5.247750995350204
-  a: -0.51318426736295442
+  v: 5.24674976432553
+  a: -0.51382452381853683
   relative_time: 1.4000000000000001
 }
 trajectory_point {
   path_point {
-    x: 587702.918683201
-    y: 4141444.7721137227
-    theta: -0.35383618539673395
-    kappa: -0.031790274224078333
-    s: 8.1025053907456854
-    dkappa: -0.014019851283088691
+    x: 587702.91783578007
+    y: 4141444.772013783
+    theta: -0.35394397424787849
+    kappa: -0.031813421504012923
+    s: 8.1017239484123529
+    dkappa: -0.014024315837217312
     ddkappa: 0
   }
-  v: 5.19337228798493
-  a: -0.57370863934908645
+  v: 5.19231068912646
+  a: -0.574272683144738
   relative_time: 1.5000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587703.400856055
-    y: 4141444.5873137787
-    theta: -0.37117252755612495
-    kappa: -0.034294926193353469
-    s: 8.618878976899472
-    dkappa: -0.01959184204665898
+    x: 587703.39988050493
+    y: 4141444.5871836385
+    theta: -0.37129073707984156
+    kappa: -0.03431809227712844
+    s: 8.6179887050769857
+    dkappa: -0.019597053031729112
     ddkappa: 0
   }
-  v: 5.133171265311308
-  a: -0.62937307640757933
+  v: 5.1320578628015294
+  a: -0.629841789322526
   relative_time: 1.6000000000000003
 }
 trajectory_point {
   path_point {
-    x: 587703.87508596107
-    y: 4141444.3996380661
-    theta: -0.38907088026621472
-    kappa: -0.0368306295922692
-    s: 9.1289641697205681
-    dkappa: -0.022579298906469165
+    x: 587703.87398902036
+    y: 4141444.3995092148
+    theta: -0.38919439457571176
+    kappa: -0.036853342011443489
+    s: 9.1279603985110835
+    dkappa: -0.022600089449459539
     ddkappa: 0
   }
-  v: 5.0677111699588
-  a: -0.67863259580450142
+  v: 5.0665565177695493
+  a: -0.67898530027247628
   relative_time: 1.7000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587704.33828448574
-    y: 4141444.202753468
-    theta: -0.408504125423676
-    kappa: -0.039473156848566567
-    s: 9.6322696615006187
-    dkappa: -0.01978142949964818
+    x: 587704.33704632253
+    y: 4141444.2025915035
+    theta: -0.40863723413752445
+    kappa: -0.03949561241963076
+    s: 9.63114888239021
+    dkappa: -0.019801345115913346
     ddkappa: 0
   }
-  v: 4.9977097428302582
-  a: -0.71994221480592291
+  v: 4.9965265406572756
+  a: -0.72015667391516369
   relative_time: 1.8000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587704.79219975858
-    y: 4141444.0027529825
-    theta: -0.42851018507236355
-    kappa: -0.04212922166026227
-    s: 10.128383643535214
-    dkappa: -0.019688148486031766
+    x: 587704.790836038
+    y: 4141444.0026008957
+    theta: -0.42864627811054967
+    kappa: -0.042150978544532272
+    s: 10.12714373176301
+    dkappa: -0.019688394194826916
     ddkappa: 0
   }
-  v: 4.9240392231019294
-  a: -0.75175695067791359
+  v: 4.922842472299406
+  a: -0.75180936817116362
   relative_time: 1.9000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587705.23366694374
-    y: 4141443.7933677658
-    theta: -0.45000522707475815
-    kappa: -0.0448524676423078
-    s: 10.616989255951252
-    dkappa: -0.025174225351658645
+    x: 587705.23215381522
+    y: 4141443.7931798631
+    theta: -0.45014995863590063
+    kappa: -0.044873776809293955
+    s: 10.615629708471987
+    dkappa: -0.02517516469101954
     ddkappa: 0
   }
-  v: 4.8477263482234543
-  a: -0.77253182068654214
+  v: 4.8465335077385809
+  a: -0.77239684096105088
   relative_time: 2.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587705.66534167365
-    y: 4141443.5816805973
-    theta: -0.47191849515009054
-    kappa: -0.047558281387055396
-    s: 11.097880005331572
-    dkappa: -0.028323716428672888
+    x: 587705.66370746843
+    y: 4141443.581513186
+    theta: -0.47206384262014617
+    kappa: -0.04757885904642533
+    s: 11.096402164911083
+    dkappa: -0.028347552098793648
     ddkappa: 0
   }
-  v: 4.7699490017207262
-  a: -0.78092561250253156
+  v: 4.7687789676079628
+  a: -0.78061151315196664
   relative_time: 2.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587706.08287436957
-    y: 4141443.3592349528
-    theta: -0.49541127373206151
-    kappa: -0.050284910142039313
-    s: 11.570971557769093
-    dkappa: -0.025728398042159137
+    x: 587706.08108873584
+    y: 4141443.3590283073
+    theta: -0.49556440781456229
+    kappa: -0.050304954015102127
+    s: 11.56937853180945
+    dkappa: -0.025751019598755552
     ddkappa: 0
   }
-  v: 4.6919435930120681
-  a: -0.77724838564490517
+  v: 4.6908122287397616
+  a: -0.77679547639563684
   relative_time: 2.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587706.49148508918
-    y: 4141443.1367861019
-    theta: -0.518974996820504
-    kappa: -0.052965685975269641
-    s: 12.036299908928196
-    dkappa: -0.024532877994728992
+    x: 587706.48959137837
+    y: 4141443.1366096637
+    theta: -0.519126946620644
+    kappa: -0.052985233094982431
+    s: 12.034596196231234
+    dkappa: -0.024528552454938662
     ddkappa: 0
   }
-  v: 4.6148706277129152
-  a: -0.76243206515218465
+  v: 4.6137899638238284
+  a: -0.76187669478951459
   relative_time: 2.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587706.883676783
-    y: 4141442.9008063544
-    theta: -0.544329395260854
-    kappa: -0.055597368340222178
-    s: 12.494012445409606
-    dkappa: -0.029735996140449441
+    x: 587706.88163420721
+    y: 4141442.900586505
+    theta: -0.5444885061039263
+    kappa: -0.055616387861856881
+    s: 12.492203572689085
+    dkappa: -0.029732979996933813
     ddkappa: 0
   }
-  v: 4.53979741893485
-  a: -0.737408576062892
+  v: 4.53877604914027
+  a: -0.73678313243105353
   relative_time: 2.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587707.269556603
-    y: 4141442.668624429
-    theta: -0.56927295624934016
-    kappa: -0.0581866968069023
-    s: 12.944358625500021
-    dkappa: -0.034855376089046278
+    x: 587707.26737144624
+    y: 4141442.6683595264
+    theta: -0.569439475473155
+    kappa: -0.058205223316000206
+    s: 12.942450823503169
+    dkappa: -0.03485369988345044
     ddkappa: 0
   }
-  v: 4.4676980872856049
-  a: -0.703109843415549
+  v: 4.466741564559447
+  a: -0.7024427534177069
   relative_time: 2.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587707.635716258
-    y: 4141442.4187094569
-    theta: -0.596283456575183
-    kappa: -0.060610712697350896
-    s: 13.387680659921712
-    dkappa: -0.032273086868451567
+    x: 587707.63343999907
+    y: 4141442.4184797211
+    theta: -0.59644722806468353
+    kappa: -0.060629250999166037
+    s: 13.385680579160208
+    dkappa: -0.0322991422183163
     ddkappa: 0
   }
-  v: 4.3994535608690573
-  a: -0.66046779224867791
+  v: 4.3985647935419765
+  a: -0.65978352184692812
   relative_time: 2.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 587707.99634120555
-    y: 4141442.1723822537
-    theta: -0.62291203076068091
-    kappa: -0.062997876671974262
-    s: 13.824404192582151
-    dkappa: -0.029682112407838843
+    x: 587707.99392727588
+    y: 4141442.1721024988
+    theta: -0.623082945760015
+    kappa: -0.063015978979107365
+    s: 13.822318658672495
+    dkappa: -0.029706304961048702
     ddkappa: 0
   }
-  v: 4.3358515752852362
-  a: -0.61041434760080038
+  v: 4.3350312231387278
+  a: -0.60973340181617064
   relative_time: 2.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587708.34018029808
-    y: 4141441.9134011352
-    theta: -0.65078777553286493
-    kappa: -0.065161725190312381
-    s: 14.255028981323616
-    dkappa: -0.032492236378476369
+    x: 587708.33769609756
+    y: 4141441.9131617551
+    theta: -0.6509561773158441
+    kappa: -0.065180490459648716
+    s: 14.252864789936922
+    dkappa: -0.032487060875858224
     ddkappa: 0
   }
-  v: 4.2775866736303136
-  a: -0.55388143451043825
+  v: 4.2768335439908265
+  a: -0.55322035742288767
   relative_time: 2.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587708.67547687585
-    y: 4141441.6521001197
-    theta: -0.67889151255732771
-    kappa: -0.067231090775891678
-    s: 14.680119578672814
-    dkappa: -0.037149089416568012
+    x: 587708.67285918037
+    y: 4141441.6518066954
+    theta: -0.67906690678207982
+    kappa: -0.067249457676204938
+    s: 14.677883330094007
+    dkappa: -0.037146380110306479
     ddkappa: 0
   }
-  v: 4.2252602064966132
-  a: -0.49180097801611378
+  v: 4.2245716503296524
+  a: -0.49117235276453292
   relative_time: 2.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587708.99940831971
-    y: 4141441.3848493649
-    theta: -0.70741253462266307
-    kappa: -0.069118048427961654
-    s: 15.100296012590491
-    dkappa: -0.038622638779501159
+    x: 587708.99673993222
+    y: 4141441.3845968144
+    theta: -0.70758676931510633
+    kappa: -0.069137708590033445
+    s: 15.097993985886919
+    dkappa: -0.038653763287731083
     ddkappa: 0
   }
-  v: 4.1793803319726059
-  a: -0.42510490315634863
+  v: 4.1787526399768407
+  a: -0.42451735193855955
   relative_time: 3.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587709.30954975856
-    y: 4141441.107705418
-    theta: -0.73672942466472691
-    kappa: -0.0707634050280884
-    s: 15.51622446722104
-    dkappa: -0.035687312749026673
+    x: 587709.30675047007
+    y: 4141441.1073963172
+    theta: -0.73691050320261509
+    kappa: -0.070782620974107091
+    s: 15.513862534020507
+    dkappa: -0.0357153585606038
     ddkappa: 0
   }
-  v: 4.140362015642908
-  a: -0.35472513496966479
+  v: 4.13979081434428
+  a: -0.354183319042421
   relative_time: 3.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587709.61636454356
-    y: 4141440.8322123424
-    theta: -0.765831273007562
-    kappa: -0.072378527831854472
-    s: 15.928607963642134
-    dkappa: -0.033009546967847574
+    x: 587709.6135318965
+    y: 4141440.8319408516
+    theta: -0.76601339427842019
+    kappa: -0.072399522633467092
+    s: 15.926191541520312
+    dkappa: -0.0330030507447055
     ddkappa: 0
   }
-  v: 4.1085270305882853
-  a: -0.28159359849458421
+  v: 4.1080076784341131
+  a: -0.28109821817357084
   relative_time: 3.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587709.90127104428
-    y: 4141440.5379758487
-    theta: -0.79607465260473131
-    kappa: -0.073512570436826519
-    s: 16.338177040614323
-    dkappa: -0.037093144110694878
+    x: 587709.89830821916
+    y: 4141440.5376472846
+    theta: -0.79626334016945122
+    kappa: -0.0735329365543986
+    s: 16.33571108609161
+    dkappa: -0.037090400673622584
     ddkappa: 0
   }
-  v: 4.0841039573856532
-  a: -0.20664221876962874
+  v: 4.0836319408387407
+  a: -0.2061900134294623
   relative_time: 3.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587710.18474060472
-    y: 4141440.2452233504
-    theta: -0.82617507185436578
-    kappa: -0.074640893440836992
-    s: 16.745680435330659
-    dkappa: -0.041156145425799553
+    x: 587710.18165123311
+    y: 4141440.2448349278
+    theta: -0.82637086052993824
+    kappa: -0.07466064588594766
+    s: 16.743169476478432
+    dkappa: -0.041157178547991655
     ddkappa: 0
   }
-  v: 4.067228184108072
-  a: -0.13080292083332024
+  v: 4.0667995137408139
+  a: -0.13038666890754858
   relative_time: 3.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 587710.45365805714
-    y: 4141439.9411415537
-    theta: -0.85667852639977582
-    kappa: -0.075443544874315191
-    s: 17.151875764166306
-    dkappa: -0.040582141843794696
+    x: 587710.45053912851
+    y: 4141439.9407830006
+    theta: -0.85687725634148382
+    kappa: -0.07546479395932966
+    s: 17.149323972822575
+    dkappa: -0.040614812391065826
     ddkappa: 0
   }
-  v: 4.05794190632475
-  a: -0.05500762972418094
+  v: 4.057553512913243
+  a: -0.054616148705283307
   relative_time: 3.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587710.71361865185
-    y: 4141439.6297448613
-    theta: -0.88749243857963211
-    kappa: -0.076042273774029764
-    s: 17.557520203428158
-    dkappa: -0.037096643610995156
+    x: 587710.71037178719
+    y: 4141439.6293276083
+    theta: -0.88769780052639147
+    kappa: -0.0760625663536311
+    s: 17.554931507022651
+    dkappa: -0.037124138507544278
     ddkappa: 0
   }
-  v: 4.0561941271010458
-  a: 0.019811729519267685
+  v: 4.0558442577191878
+  a: 0.020193583079880062
   relative_time: 3.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 587710.96936861856
-    y: 4141439.3148450227
-    theta: -0.91836061681817338
-    kappa: -0.076549778362844267
-    s: 17.963361170104449
-    dkappa: -0.034888392916597684
+    x: 587710.96609462437
+    y: 4141439.314444432
+    theta: -0.91857143909286521
+    kappa: -0.076571133263395652
+    s: 17.9607394030931
+    dkappa: -0.034883187059187006
     ddkappa: 0
   }
-  v: 4.0618406569984629
-  a: 0.092723231858503175
+  v: 4.0615292711120663
+  a: 0.093114562350487984
   relative_time: 3.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 587711.20589062711
-    y: 4141438.9839133341
-    theta: -0.94958813779974194
-    kappa: -0.076640374108827944
-    s: 18.370127002614371
-    dkappa: -0.038517677280217766
+    x: 587711.20248736255
+    y: 4141438.9834561967
+    theta: -0.94980486061790637
+    kappa: -0.076660295565735448
+    s: 18.367476097523209
+    dkappa: -0.03851771740943747
     ddkappa: 0
   }
-  v: 4.0746441140746548
-  a: 0.16279495225500418
+  v: 4.0743732796355516
+  a: 0.16321882500908746
   relative_time: 3.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 587711.44335741131
-    y: 4141438.6516597564
-    theta: -0.98094242776605578
-    kappa: -0.076731331735134772
-    s: 18.778517641557691
-    dkappa: -0.042161458644596868
+    x: 587711.43982690421
+    y: 4141438.6511422824
+    theta: -0.98116565178913828
+    kappa: -0.07674981498221628
+    s: 18.775841859636152
+    dkappa: -0.042166804833138533
     ddkappa: 0
   }
-  v: 4.0942739238834225
-  a: 0.22909496567024845
+  v: 4.0940482134235685
+  a: 0.22957840695822507
   relative_time: 3.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 587711.662293284
-    y: 4141438.3044674266
-    theta: -1.012351455827849
-    kappa: -0.076490612692855828
-    s: 19.18919531046437
-    dkappa: -0.039968464138404977
+    x: 587711.65873034787
+    y: 4141438.3039558064
+    theta: -1.0125806498311347
+    kappa: -0.076508835267979469
+    s: 19.186499511948
+    dkappa: -0.039997226128811569
     ddkappa: 0
   }
-  v: 4.1203063194747136
-  a: 0.29069134706571342
+  v: 4.1201332062002995
+  a: 0.29126534410044691
   relative_time: 4.0000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587711.876385357
-    y: 4141437.9506109403
-    theta: -1.0439724410006268
-    kappa: -0.076141280963032312
-    s: 19.602777017446478
-    dkappa: -0.035874891767260747
+    x: 587711.87269518548
+    y: 4141437.9500390468
+    theta: -1.0442072697892673
+    kappa: -0.07615748348293902
+    s: 19.600066948386541
+    dkappa: -0.0358953399877447
     ddkappa: 0
   }
-  v: 4.1522979146893793
-  a: 0.34889623032823763
+  v: 4.1521872445209462
+  a: 0.34956789205375355
   relative_time: 4.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587712.082218133
-    y: 4141437.5881841704
-    theta: -1.075644173890649
-    kappa: -0.0756695649689879
-    s: 20.0198464453961
-    dkappa: -0.0345547156864235
+    x: 587712.07848587865
+    y: 4141437.5875959317
+    theta: -1.0758860570623057
+    kappa: -0.075684601931529275
+    s: 20.017128806867543
+    dkappa: -0.034549283019250969
     ddkappa: 0
   }
-  v: 4.1900359121095674
-  a: 0.40561151066695933
+  v: 4.1899965094879814
+  a: 0.40636218631500343
   relative_time: 4.2000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587712.27267179755
-    y: 4141437.2125872513
-    theta: -1.1073161789755612
-    kappa: -0.074987010918944441
-    s: 20.440970707714076
-    dkappa: -0.038070551846267578
+    x: 587712.26881491579
+    y: 4141437.21193593
+    theta: -1.1075627341281076
+    kappa: -0.074999661374884091
+    s: 20.438252991880049
+    dkappa: -0.038072790212940022
     ddkappa: 0
   }
-  v: 4.2333689872901417
-  a: 0.46078990502680839
+  v: 4.2334078685182392
+  a: 0.46160208260503077
   relative_time: 4.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587712.4643977799
-    y: 4141436.8325071731
-    theta: -1.1392996024574831
-    kappa: -0.074291617034098978
-    s: 20.866701548941457
-    dkappa: -0.041394160356325704
+    x: 587712.46048431227
+    y: 4141436.8318155049
+    theta: -1.1395534145530501
+    kappa: -0.074302277400415923
+    s: 20.8639918639334
+    dkappa: -0.041422410487187193
     ddkappa: 0
   }
-  v: 4.282141087480464
-  a: 0.5143841303527148
+  v: 4.28226357460064
+  a: 0.51524143664466981
   relative_time: 4.4
 }
 trajectory_point {
   path_point {
-    x: 587712.63063785364
-    y: 4141436.4349948363
-    theta: -1.1708992507828553
-    kappa: -0.073403615971434216
-    s: 21.297574871928958
-    dkappa: -0.036849871627997369
+    x: 587712.626604087
+    y: 4141436.4342348981
+    theta: -1.1711568597656095
+    kappa: -0.073411810583442288
+    s: 21.294881778114423
+    dkappa: -0.036866419669486936
     ddkappa: 0
   }
-  v: 4.3361914316243846
-  a: 0.56634690358960849
+  v: 4.3364012662961882
+  a: 0.56723410415475461
   relative_time: 4.5
 }
 trajectory_point {
   path_point {
-    x: 587712.799062474
-    y: 4141436.0322588221
-    theta: -1.2029215658342056
-    kappa: -0.072503945761508509
-    s: 21.7341102650064
-    dkappa: -0.032245866783780684
+    x: 587712.79491015954
+    y: 4141436.0314217443
+    theta: -1.2031837808164434
+    kappa: -0.072509624357099484
+    s: 21.73144262264465
+    dkappa: -0.032250467581521412
     ddkappa: 0
   }
-  v: 4.3953545103602485
-  a: 0.61663094168241939
+  v: 4.3956539677379673
+  a: 0.61753394085611957
   relative_time: 4.6
 }
 trajectory_point {
   path_point {
-    x: 587712.94762453868
-    y: 4141435.6154532116
-    theta: -1.2347195936701103
-    kappa: -0.071783351075420179
-    s: 22.176810529152171
-    dkappa: -0.034127704414113676
+    x: 587712.9433995299
+    y: 4141435.6145544127
+    theta: -1.2349866997114973
+    kappa: -0.071786408466326182
+    s: 22.174177357437536
+    dkappa: -0.0341254257100829
     ddkappa: 0
   }
-  v: 4.4594600860208962
-  a: 0.66518896157607732
+  v: 4.4598500886311481
+  a: 0.666094802469599
   relative_time: 4.6999999999999993
 }
 trajectory_point {
   path_point {
-    x: 587713.09098873253
-    y: 4141435.1895861691
-    theta: -1.2667938481917638
-    kappa: -0.071115995660465031
-    s: 22.626161205162653
-    dkappa: -0.038226123781492934
+    x: 587713.08664968354
+    y: 4141435.1886030822
+    theta: -1.2670647129860937
+    kappa: -0.071116861163995057
+    s: 22.62357155265564
+    dkappa: -0.03823577296398098
     ddkappa: 0
   }
-  v: 4.5283331926336565
-  a: 0.71197368021551244
+  v: 4.5288134242529852
+  a: 0.71287054471602684
   relative_time: 4.7999999999999989
 }
 trajectory_point {
   path_point {
-    x: 587713.21974577755
-    y: 4141434.7519483492
-    theta: -1.2990822126114812
-    kappa: -0.070452787484456519
-    s: 23.082630100821703
-    dkappa: -0.037464260358777431
+    x: 587713.21531160211
+    y: 4141434.7508824868
+    theta: -1.2993567759743945
+    kappa: -0.070451513241192226
+    s: 23.08009292726787
+    dkappa: -0.037475556176184179
     ddkappa: 0
   }
-  v: 4.6017941359203549
-  a: 0.75693781454565445
+  v: 4.6023631554528128
+  a: 0.75781502331623751
   relative_time: 4.8999999999999986
 }
 trajectory_point {
   path_point {
-    x: 587713.3363290932
-    y: 4141434.3027953627
-    theta: -1.33169582836504
-    kappa: -0.069791061809977711
-    s: 23.546666818070069
-    dkappa: -0.032514449679213193
+    x: 587713.33178563078
+    y: 4141434.3016378786
+    theta: -1.3319735933278121
+    kappa: -0.069787711411567144
+    s: 23.544190887606657
+    dkappa: -0.032509643007869383
     ddkappa: 0
   }
-  v: 4.6796584932973078
-  a: 0.80003408151143374
+  v: 4.6803138486520526
+  a: 0.800882093991065
   relative_time: 4.9999999999999982
 }
 trajectory_point {
   path_point {
-    x: 587713.44193947769
-    y: 4141433.8430007868
-    theta: -1.3644090776753723
-    kappa: -0.068789290814440962
-    s: 24.018702280174868
-    dkappa: -0.031026033356827541
+    x: 587713.43727448268
+    y: 4141433.8417437938
+    theta: -1.3646892689823715
+    kappa: -0.068783148782310941
+    s: 24.016296065925182
+    dkappa: -0.031021767478182971
     ddkappa: 0
   }
-  v: 4.7617371138753262
-  a: 0.84121519805777956
+  v: 4.7624754558442071
+  a: 0.84202561246134389
   relative_time: 5.0999999999999979
 }
 trajectory_point {
   path_point {
-    x: 587713.53054845089
-    y: 4141433.3707966059
-    theta: -1.3970809005216518
-    kappa: -0.067291663753834982
-    s: 24.499148258899016
-    dkappa: -0.03466981251624536
+    x: 587713.52577955683
+    y: 4141433.3694409593
+    theta: -1.3973632614978608
+    kappa: -0.067282606037872325
+    s: 24.496819858954588
+    dkappa: -0.034680917027454371
     ddkappa: 0
   }
-  v: 4.8478361184597132
-  a: 0.8804338811296224
+  v: 4.8486533145948636
+  a: 0.88119943444790816
   relative_time: 5.1999999999999975
 }
 trajectory_point {
   path_point {
-    x: 587713.60959727655
-    y: 4141432.8882194939
-    theta: -1.4296810878745929
-    kappa: -0.0655372565507068
-    s: 24.988396901670686
-    dkappa: -0.034973871314860891
+    x: 587713.60468036076
+    y: 4141432.8867532951
+    theta: -1.4299624438872878
+    kappa: -0.065524065689543956
+    s: 24.986153966461167
+    dkappa: -0.034973436783779566
     ddkappa: 0
   }
-  v: 4.9377568995502648
-  a: 0.91764284767189186
+  v: 4.9386481480416915
+  a: 0.9183574156715919
   relative_time: 5.2999999999999972
 }
 trajectory_point {
   path_point {
-    x: 587713.66923039523
-    y: 4141432.3933763532
-    theta: -1.4616877097794689
-    kappa: -0.063321396003783587
-    s: 25.486820258752754
-    dkappa: -0.028918173380217821
+    x: 587713.6642157993
+    y: 4141432.3918051184
+    theta: -1.4619694195379769
+    kappa: -0.06330457148959899
+    s: 25.484669929803594
+    dkappa: -0.028895844617550084
     ddkappa: 0
   }
-  v: 5.03129612134127
-  a: 0.95279481462951821
+  v: 5.0322560648944465
+  a: 0.95345341185322963
   relative_time: 5.3999999999999968
 }
 trajectory_point {
   path_point {
-    x: 587713.71859644284
-    y: 4141431.8880545828
-    theta: -1.4933782660072001
-    kappa: -0.060896661584448272
-    s: 25.994769810412251
-    dkappa: -0.026289563152227569
+    x: 587713.713411731
+    y: 4141431.8863675082
+    theta: -1.4936534979014031
+    kappa: -0.060874926068770534
+    s: 25.99271867049012
+    dkappa: -0.026283581393583843
     ddkappa: 0
   }
-  v: 5.1282457197215106
-  a: 0.985842498947431
+  v: 5.1292685594349638
+  a: 0.98644127871365539
   relative_time: 5.4999999999999964
 }
 trajectory_point {
   path_point {
-    x: 587713.74891489884
-    y: 4141431.3711367603
-    theta: -1.524094153496147
-    kappa: -0.05813282432682617
-    s: 26.512575994089808
-    dkappa: -0.02982343487719144
+    x: 587713.74364005961
+    y: 4141431.3693390526
+    theta: -1.5243672007905908
+    kappa: -0.058107016588572771
+    s: 26.510630028735783
+    dkappa: -0.029837200845073221
     ddkappa: 0
   }
-  v: 5.2283929022742619
-  a: 1.0167386175705606
+  v: 5.2294725115171641
+  a: 1.0172748719737035
   relative_time: 5.5999999999999961
 }
 trajectory_point {
   path_point {
-    x: 587713.76638708741
-    y: 4141430.843660939
-    theta: -1.5540677574764736
-    kappa: -0.055202159289214479
-    s: 27.040547731569113
-    dkappa: -0.02874700734987248
+    x: 587713.76092836
+    y: 4141430.8417466055
+    theta: -1.5543275824203795
+    kappa: -0.055171405701428589
+    s: 27.038712302019615
+    dkappa: -0.028725475593366218
     ddkappa: 0
   }
-  v: 5.3315201482772938
-  a: 1.0454358874438368
+  v: 5.3326501865670517
+  a: 1.0459080473542079
   relative_time: 5.6999999999999957
 }
 trajectory_point {
   path_point {
-    x: 587713.76767068286
-    y: 4141430.3052382446
-    theta: -1.583004543102599
-    kappa: -0.052075003296528324
-    s: 27.578971956146354
-    dkappa: -0.021892752623374982
+    x: 587713.76213123323
+    y: 4141430.3032084676
+    theta: -1.5832591238896829
+    kappa: -0.052040080722447417
+    s: 27.577251783641856
+    dkappa: -0.021844017484816815
     ddkappa: 0
   }
-  v: 5.4374052087028648
-  a: 1.0718870255121891
+  v: 5.4385792355827149
+  a: 1.0722946605760033
   relative_time: 5.7999999999999954
 }
 trajectory_point {
   path_point {
-    x: 587713.75217040174
-    y: 4141429.7564832596
-    theta: -1.6105568972742257
-    kappa: -0.048844218723172
-    s: 28.128113139799666
-    dkappa: -0.02140472651676378
+    x: 587713.74644404324
+    y: 4141429.7543385779
+    theta: -1.6107911527030443
+    kappa: -0.04880499734939743
+    s: 28.12651230128116
+    dkappa: -0.021404072608209024
     ddkappa: 0
   }
-  v: 5.5458211062177307
-  a: 1.096044748720548
+  v: 5.5470326951343223
+  a: 1.0963885673599236
   relative_time: 5.899999999999995
 }
 trajectory_point {
   path_point {
-    x: 587713.725217304
-    y: 4141429.1970324726
-    theta: -1.6373717336103126
-    kappa: -0.045521526347369057
-    s: 28.688212820358586
-    dkappa: -0.025217778123876676
+    x: 587713.7194219135
+    y: 4141429.1947682044
+    theta: -1.6375974357869327
+    kappa: -0.045478357649063571
+    s: 28.6867347555518
+    dkappa: -0.025242171580122104
     ddkappa: 0
   }
-  v: 5.6565361351831385
-  a: 1.1178617740138432
+  v: 5.65777898736413
+  a: 1.1181436234268027
   relative_time: 5.9999999999999947
 }
 trajectory_point {
   path_point {
-    x: 587713.67681611131
-    y: 4141428.6278940663
-    theta: -1.6619700417587304
-    kappa: -0.042190381735755275
-    s: 29.259489128673508
-    dkappa: -0.019957030634460911
+    x: 587713.6708423791
+    y: 4141428.6255170451
+    theta: -1.6621687673561647
+    kappa: -0.042144030470086517
+    s: 29.258136658560876
+    dkappa: -0.019906871603373871
     ddkappa: 0
   }
-  v: 5.7693138616548278
-  a: 1.1372908183370047
+  v: 5.7705819199864754
+  a: 1.1375136844974756
   relative_time: 6.0999999999999943
 }
 trajectory_point {
   path_point {
-    x: 587713.62063755514
-    y: 4141428.0480003189
-    theta: -1.686132314861275
-    kappa: -0.038821432152730088
-    s: 29.842136315785105
-    dkappa: -0.013686566100769217
+    x: 587713.6144877146
+    y: 4141428.0455138222
+    theta: -1.6863010261962204
+    kappa: -0.038772562156634931
+    s: 29.840911672465531
+    dkappa: -0.013674205771029952
     ddkappa: 0
   }
-  v: 5.8839131233830315
-  a: 1.1542845986349626
+  v: 5.8852006862877788
+  a: 1.154452606292776
   relative_time: 6.199999999999994
 }
 trajectory_point {
   path_point {
-    x: 587713.54260752024
-    y: 4141427.4589581722
-    theta: -1.7077485637671073
-    kappa: -0.0355347835485802
-    s: 30.436324280093821
-    dkappa: -0.01794602573149898
+    x: 587713.53642033786
+    y: 4141427.45634598
+    theta: -1.707902857512555
+    kappa: -0.035483115653694046
+    s: 30.435229148030153
+    dkappa: -0.017963755598545315
     ddkappa: 0
   }
-  v: 6.0000880298124768
-  a: 1.1687958318526463
+  v: 6.0013898651265452
+  a: 1.1689142445335381
   relative_time: 6.2999999999999936
 }
 trajectory_point {
   path_point {
-    x: 587713.4543436931
-    y: 4141426.8596335854
-    theta: -1.7283885830011039
-    kappa: -0.032278360536413045
-    s: 31.042198094529283
-    dkappa: -0.017196707257904224
+    x: 587713.44800649583
+    y: 4141426.8569114879
+    theta: -1.728507906332005
+    kappa: -0.032225506619170188
+    s: 31.041233663183583
+    dkappa: -0.017146176752303368
     ddkappa: 0
   }
-  v: 6.1175879620823821
-  a: 1.1807772349349863
+  v: 6.1188994209333618
+  a: 1.1808524549405963
   relative_time: 6.3999999999999932
 }
 trajectory_point {
   path_point {
-    x: 587713.352597758
-    y: 4141426.2503917166
-    theta: -1.7475404941477288
-    kappa: -0.029086751692776927
-    s: 31.659877533719772
-    dkappa: -0.0095470955440013633
+    x: 587713.34624194773
+    y: 4141426.2475394472
+    theta: -1.7476428150014984
+    kappa: -0.029031812694683887
+    s: 31.659044561576323
+    dkappa: -0.0094615145911057918
     ddkappa: 0
   }
-  v: 6.2361575730264587
-  a: 1.1901815248269123
+  v: 6.2374747037109017
+  a: 1.1902210932347848
   relative_time: 6.4999999999999929
 }
 trajectory_point {
   path_point {
-    x: 587713.23460284038
-    y: 4141425.6320082303
-    theta: -1.7645247765838112
-    kappa: -0.02603475124105302
-    s: 32.28945660116166
-    dkappa: -0.011978874070831765
+    x: 587713.22813342
+    y: 4141425.6290422715
+    theta: -1.7645889222996596
+    kappa: -0.025979924107930229
+    s: 32.288755491137749
+    dkappa: -0.0119952605789603
     ddkappa: 0
   }
-  v: 6.3555367871729143
-  a: 1.1969614184733541
+  v: 6.3568564490339172
+  a: 1.1969740151369375
   relative_time: 6.5999999999999925
 }
 trajectory_point {
   path_point {
-    x: 587713.10741304036
-    y: 4141425.0032293024
-    theta: -1.7805290984814111
-    kappa: -0.023032662788873563
-    s: 32.93100305638886
-    dkappa: -0.0138068541544759
+    x: 587713.10085120169
+    y: 4141425.0001478903
+    theta: -1.7805544370250512
+    kappa: -0.022978385298746313
+    s: 32.930433942633314
+    dkappa: -0.013755002579776656
     ddkappa: 0
   }
-  v: 6.4754608007444441
-  a: 1.2010696328192423
+  v: 6.4767807780492479
+  a: 1.201065076367889
   relative_time: 6.6999999999999922
 }
 trajectory_point {
   path_point {
-    x: 587712.96647473879
-    y: 4141424.3650519103
-    theta: -1.7945956631785414
-    kappa: -0.0201664103262872
-    s: 33.58455794214229
-    dkappa: -0.0059325632759830681
+    x: 587712.95993687422
+    y: 4141424.361830066
+    theta: -1.7946010429070527
+    kappa: -0.020111569556883833
+    s: 33.584120788221774
+    dkappa: -0.0058428787711482261
     ddkappa: 0
   }
-  v: 6.5956600816582407
-  a: 1.2024588848095059
+  v: 6.5969791974758145
+  a: 1.2024481326484735
   relative_time: 6.7999999999999918
 }
 trajectory_point {
   path_point {
-    x: 587712.81385623873
-    y: 4141423.71723531
-    theta: -1.8069007145513791
-    kappa: -0.01742796626133376
-    s: 34.250135111539308
-    dkappa: -0.0075182215521364263
+    x: 587712.80726961128
+    y: 4141423.7138889856
+    theta: -1.8068660510998311
+    kappa: -0.017374862502053924
+    s: 34.249829820012359
+    dkappa: -0.0075400578157043636
     ddkappa: 0
   }
-  v: 6.7158603695259877
-  a: 1.2010818913890753
+  v: 6.7171785996046243
+  a: 1.201077039699525
   relative_time: 6.8999999999999915
 }
 trajectory_point {
   path_point {
-    x: 587712.65251357923
-    y: 4141423.0591533771
-    theta: -1.8180196568690812
-    kappa: -0.014767038046579568
-    s: 34.927720755243158
-    dkappa: -0.009851594877005921
+    x: 587712.64590575115
+    y: 4141423.05567677
+    theta: -1.817945657514106
+    kappa: -0.014716004548880697
+    s: 34.927547288622016
+    dkappa: -0.0097959354076981951
     ddkappa: 0
   }
-  v: 6.8357826756538627
-  a: 1.1968913695028807
+  v: 6.8371012622987628
+  a: 1.1969056532418778
   relative_time: 6.9999999999999911
 }
 trajectory_point {
   path_point {
-    x: 587712.48050771
-    y: 4141422.3913987959
-    theta: -1.8273261510782608
-    kappa: -0.012242195627610655
-    s: 35.617272928632445
-    dkappa: -0.0017506579836685419
+    x: 587712.47396898328
+    y: 4141422.3877681093
+    theta: -1.827231831998317
+    kappa: -0.012192064318973399
+    s: 35.6172314417326
+    dkappa: -0.0016548211298563682
     ddkappa: 0
   }
-  v: 6.9551432830425348
-  a: 1.1898400360958516
+  v: 6.956464848993404
+  a: 1.1898878289963664
   relative_time: 7.0999999999999908
 }
 trajectory_point {
   path_point {
-    x: 587712.29928563093
-    y: 4141421.7137730247
-    theta: -1.8348648663358518
-    kappa: -0.0098523382385583731
-    s: 36.318721078970533
-    dkappa: -0.0046318766294633452
+    x: 587712.29277394852
+    y: 4141421.7099977252
+    theta: -1.8347327192967944
+    kappa: -0.009805342742584388
+    s: 36.318812062648092
+    dkappa: -0.0046666696599612315
     ddkappa: 0
   }
-  v: 7.0736537463871692
-  a: 1.1798806081129181
+  v: 7.0749824086958011
+  a: 1.1799774226838244
   relative_time: 7.19999999999999
 }
 trajectory_point {
   path_point {
-    x: 587712.11113630072
-    y: 4141421.0257982956
-    theta: -1.8411496196936408
-    kappa: -0.0075520776487308892
-    s: 37.031965572575061
-    dkappa: -0.0053302844427935269
+    x: 587712.10467956576
+    y: 4141421.0218696296
+    theta: -1.8409813053168544
+    kappa: -0.0075085731981546474
+    s: 37.0321900088518
+    dkappa: -0.0052674638508327593
     ddkappa: 0
   }
-  v: 7.191020892077419
-  a: 1.1669658024990102
+  v: 7.1923623759852937
+  a: 1.1671282900250868
   relative_time: 7.29999999999999
 }
 trajectory_point {
   path_point {
-    x: 587711.91625431285
-    y: 4141420.327573467
-    theta: -1.8460969960156834
-    kappa: -0.0053508378654516554
-    s: 37.756877221987345
-    dkappa: 0.0029981207113948792
+    x: 587711.90990945045
+    y: 4141420.3234733385
+    theta: -1.8459107729430573
+    kappa: -0.005309603067967841
+    s: 37.757236750563571
+    dkappa: 0.003103081303551262
     ddkappa: 0
   }
-  v: 7.3069468181974351
-  a: 1.1510483361990576
+  v: 7.3083085710133053
+  a: 1.151294286740987
   relative_time: 7.39999999999999
 }
 trajectory_point {
   path_point {
-    x: 587711.714513909
-    y: 4141419.6193259871
-    theta: -1.8490109266762147
-    kappa: -0.0033246862658420891
-    s: 38.493296813141832
-    dkappa: -0.0033249819027573236
+    x: 587711.70826945815
+    y: 4141419.6150541473
+    theta: -1.8487927620940035
+    kappa: -0.00328798102826247
+    s: 38.493793909297
+    dkappa: -0.0033805723145797512
     ddkappa: 0
   }
-  v: 7.4211288945258591
-  a: 1.1320809261579907
+  v: 7.42252019950334
+  a: 1.1324292685523598
   relative_time: 7.4999999999999893
 }
 trajectory_point {
   path_point {
-    x: 587711.50844211411
-    y: 4141418.9005454951
-    theta: -1.8507074966904939
-    kappa: -0.0014032970827340086
-    s: 39.241034632535573
-    dkappa: -0.0002706638446230065
+    x: 587711.50232305238
+    y: 4141418.8960909611
+    theta: -1.8504604433253209
+    kappa: -0.0013715399374352507
+    s: 39.24167279641663
+    dkappa: -0.0001970335200736301
     ddkappa: 0
   }
-  v: 7.5332597625358266
-  a: 1.1100162893207388
+  v: 7.5346918527509867
+  a: 1.1104870911800395
   relative_time: 7.599999999999989
 }
 trajectory_point {
   path_point {
-    x: 587711.298599423
-    y: 4141418.1713012634
-    theta: -1.8511628937320568
-    kappa: 0.00039794110810320484
-    s: 39.999869994397635
-    dkappa: 0.0033661791258388619
+    x: 587711.29262910353
+    y: 4141418.1666522464
+    theta: -1.8508901579282664
+    kappa: 0.00042426158386355852
+    s: 40.00065395169517
+    dkappa: 0.0033304464324053233
     ddkappa: 0
   }
-  v: 7.6430273353949651
-  a: 1.0848071426322323
+  v: 7.6445135076239188
+  a: 1.0854216103448595
   relative_time: 7.6999999999999886
 }
 trajectory_point {
   path_point {
-    x: 587711.08570657764
-    y: 4141417.4316492556
-    theta: -1.8503735290706411
-    kappa: 0.0020621250860499507
-    s: 40.769550767858604
-    dkappa: -0.0033886877880624834
+    x: 587711.07990607817
+    y: 4141417.4267932787
+    theta: -1.8500793321475384
+    kappa: 0.0020826597844539616
+    s: 40.770486681870722
+    dkappa: -0.0033448885985358705
     ddkappa: 0
   }
-  v: 7.750114797965395
-  a: 1.0564062030374013
+  v: 7.75167052656189
+  a: 1.057186681767655
   relative_time: 7.7999999999999883
 }
 trajectory_point {
   path_point {
-    x: 587710.87135164719
-    y: 4141416.6814294513
-    theta: -1.8480535594920195
-    kappa: 0.0034993344307550993
-    s: 41.549792904119982
-    dkappa: 0.00516170705977736
+    x: 587710.86571675015
+    y: 4141416.6763600144
+    theta: -1.8477537233281447
+    kappa: 0.0035163275761028695
+    s: 41.550888599203923
+    dkappa: 0.0052508399477901166
     ddkappa: 0
   }
-  v: 7.8542006068037313
-  a: 1.0247661874811753
+  v: 7.8558436575767416
+  a: 1.0257361611692595
   relative_time: 7.8999999999999879
 }
 trajectory_point {
   path_point {
-    x: 587710.65637878294
-    y: 4141415.9207359347
-    theta: -1.8447167931641146
-    kappa: 0.00474432375476754
-    s: 42.340279963623658
-    dkappa: 0.0018627699830652605
+    x: 587710.6509369
+    y: 4141415.9154358311
+    theta: -1.8444028916021813
+    kappa: 0.00475561716862777
+    s: 42.341545160035231
+    dkappa: 0.0017947074342516127
     ddkappa: 0
   }
-  v: 7.9549584901610793
-  a: 0.98983981290848422
+  v: 7.9567090342523965
+  a: 0.99102390427050757
   relative_time: 7.9999999999999876
 }
 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 1656bcd048afda6b3f55c8a492c127b9e46c67cb..13975c87613b8f8b34812bab9d43ad4feecd9211 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
@@ -14,1127 +14,1127 @@ trajectory_point {
     ddkappa: 0
   }
   v: 1.7111110687255859
-  a: -0.32737463279777168
+  a: -0.3273245796948433
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 587258.34714054433
-    y: 4140946.9115320086
-    theta: -1.793820940930698
-    kappa: 0.000688301711210813
-    s: 0.16951119460902248
-    dkappa: -0.00014715432964617617
+    x: 587258.34714058391
+    y: 4140946.9115317352
+    theta: -1.7938206459107797
+    kappa: 0.00068888257125219532
+    s: 0.16951145189359759
+    dkappa: -0.00014657133403951089
     ddkappa: 0
   }
-  v: 1.6794669798239654
-  a: -0.30612136657448813
+  v: 1.6794721931402097
+  a: -0.30606725563182025
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 587258.3103565526
-    y: 4140946.7492004214
-    theta: -1.7938033933871027
-    kappa: 0.00037627382166291164
-    s: 0.33595820154753
-    dkappa: -0.00029164860793937396
+    x: 587258.31035651057
+    y: 4140946.7491993485
+    theta: -1.793802808621793
+    kappa: 0.00037742402147460233
+    s: 0.33595925672359095
+    dkappa: -0.00029049362677754931
     ddkappa: 0
   }
-  v: 1.6497678352198846
-  a: -0.28843690607531736
+  v: 1.6497786373077796
+  a: -0.28837934031206458
   relative_time: 0.2
 }
 trajectory_point {
   path_point {
-    x: 587258.27421060693
-    y: 4140946.5896845981
-    theta: -1.7937861502188746
-    kappa: 6.96582911756256e-05
-    s: 0.49951805962437257
-    dkappa: -0.00043363652400495048
+    x: 587258.27421035455
+    y: 4140946.589682166
+    theta: -1.7937852806709036
+    kappa: 7.1366852963972681e-05
+    s: 0.49952048786479691
+    dkappa: -0.00043191998811011257
     ddkappa: 0
   }
-  v: 1.6216684065724296
-  a: -0.2740882066703107
+  v: 1.6216851136511257
+  a: -0.27402776779244309
   relative_time: 0.30000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587258.23867095448
-    y: 4140946.4328444107
-    theta: -1.7937691962785474
-    kappa: -0.00023181423048915153
-    s: 0.66033445003745839
-    dkappa: -0.00057324280889922143
+    x: 587258.23867035669
+    y: 4140946.4328400288
+    theta: -1.7937680466575239
+    kappa: -0.00022955784555593091
+    s: 0.6603388552640308
+    dkappa: -0.00057097467960409872
     ddkappa: 0
   }
-  v: 1.5948467700036819
-  a: -0.26284222372951949
+  v: 1.5948696411873911
+  a: -0.26277947212982267
   relative_time: 0.4
 }
 trajectory_point {
   path_point {
-    x: 587258.20371269935
-    y: 4140946.2785699894
-    theta: -1.7937525196896686
-    kappa: -0.00052835492886665848
-    s: 0.81852002682005387
-    dkappa: -0.00071056525869273837
+    x: 587258.203711616
+    y: 4140946.2785630478
+    theta: -1.7937510945066748
+    kappa: -0.00052556091658053988
+    s: 0.81852703609953825
+    dkappa: -0.00070775512974676814
     ddkappa: 0
   }
-  v: 1.5690043060987169
-  a: -0.25446591262299489
+  v: 1.5690335455280322
+  a: -0.25440138738107004
   relative_time: 0.5
 }
 trajectory_point {
   path_point {
-    x: 587258.16931728716
-    y: 4140946.1267794529
-    theta: -1.7937361116011139
-    kappa: -0.00082012119353252978
-    s: 0.9741587472870824
-    dkappa: -0.00084567675755356432
+    x: 587258.16931557446
+    y: 4140946.1267693229
+    theta: -1.7937344152192229
+    kappa: -0.00081679949497425288
+    s: 0.97416900544042606
+    dkappa: -0.00084233394919520325
     ddkappa: 0
   }
-  v: 1.5438656999056057
-  a: -0.24872622872078842
+  v: 1.5439014588788191
+  a: -0.24866044760305206
   relative_time: 0.6
 }
 trajectory_point {
   path_point {
-    x: 587258.1353605825
-    y: 4140945.9774419307
-    theta: -1.7939864500198082
-    kappa: -0.0011529501923609228
-    s: 1.1273082024814245
-    dkappa: -0.0010430443487939988
+    x: 587258.13535851124
+    y: 4140945.9774278756
+    theta: -1.7939840937122975
+    kappa: -0.0011491642629605568
+    s: 1.1273223669060937
+    dkappa: -0.0010396844618729742
     ddkappa: 0
   }
-  v: 1.5191789409354137
-  a: -0.2453901273929513
+  v: 1.519221320039835
+  a: -0.24532358685263544
   relative_time: 0.7
 }
 trajectory_point {
   path_point {
-    x: 587258.10192936414
-    y: 4140945.8305033091
-    theta: -1.794278222326396
-    kappa: -0.0014882413171605658
-    s: 1.2780019476202167
-    dkappa: -0.0012482332962350654
+    x: 587258.10192686052
+    y: 4140945.8304846645
+    theta: -1.7942751255634466
+    kappa: -0.0014840049875609646
+    s: 1.2780206833256647
+    dkappa: -0.0012449377518206825
     ddkappa: 0
   }
-  v: 1.4947153231622015
-  a: -0.24422456400953479
+  v: 1.4947643744054764
+  a: -0.24415773918668709
   relative_time: 0.79999999999999993
 }
 trajectory_point {
   path_point {
-    x: 587258.069040313
-    y: 4140945.6859476492
-    theta: -1.7945652629158728
-    kappa: -0.0018180948928400121
-    s: 1.4262518325411515
-    dkappa: -0.0014500946131541523
+    x: 587258.06903721951
+    y: 4140945.6859237663
+    theta: -1.7945614391381792
+    kappa: -0.0018134171160395429
+    s: 1.4262758073974178
+    dkappa: -0.00144686337839909
     ddkappa: 0
   }
-  v: 1.4702694450230243
-  a: -0.24499649394059037
+  v: 1.4703251739644532
+  a: -0.24492983866207382
   relative_time: 0.89999999999999991
 }
 trajectory_point {
   path_point {
-    x: 587258.03669509874
-    y: 4140945.5437822891
-    theta: -1.7948475571686171
-    kappa: -0.0021424941761846311
-    s: 1.5720503321487764
-    dkappa: -0.001648618053164365
+    x: 587258.036691258
+    y: 4140945.5437525203
+    theta: -1.7948430198514269
+    kappa: -0.002137383924452519
+    s: 1.572080212348218
+    dkappa: -0.0016454510900341557
     ddkappa: 0
   }
-  v: 1.4456592094179326
-  a: -0.24747287255616912
+  v: 1.4457215772997882
+  a: -0.24740681933566239
   relative_time: 0.99999999999999989
 }
 trajectory_point {
   path_point {
-    x: 587258.004899172
-    y: 4140945.4040311864
-    theta: -1.7951250574681996
-    kappa: -0.0024613845002745931
-    s: 1.7153728768607943
-    dkappa: -0.0018437701616141784
+    x: 587258.00489442807
+    y: 4140945.403994889
+    theta: -1.7951198201976741
+    kappa: -0.0024558508076329391
+    s: 1.7154093225929492
+    dkappa: -0.0018406674144229545
     ddkappa: 0
   }
-  v: 1.4207258237099714
-  a: -0.25142065522632256
+  v: 1.4207947495888185
+  a: -0.2513556152643196
   relative_time: 1.0999999999999999
 }
 trajectory_point {
   path_point {
-    x: 587257.97366124764
-    y: 4140945.2667326443
-    theta: -1.7953976877134625
-    kappa: -0.0027746784596898713
-    s: 1.8561801830543621
-    dkappa: -0.0020354974487902977
+    x: 587257.97365544655
+    y: 4140945.2666891878
+    theta: -1.7953917642518609
+    kappa: -0.0027687304577468114
+    s: 1.8562238443939441
+    dkappa: -0.0020324588329255831
     ddkappa: 0
   }
-  v: 1.3953337997251805
-  a: -0.25660679732110192
+  v: 1.3954091626031933
+  a: -0.25654316050491233
   relative_time: 1.2
 }
 trajectory_point {
   path_point {
-    x: 587257.94298938452
-    y: 4140945.1319378242
-    theta: -1.7956680355281671
-    kappa: -0.0030826065825837121
-    s: 1.9944205835123898
-    dkappa: -0.0022235168361388275
+    x: 587257.94298222766
+    y: 4140945.1318866233
+    theta: -1.7956615588391007
+    kappa: -0.0030762696986280373
+    s: 1.9944720965204159
+    dkappa: -0.0022205284439524597
     ddkappa: 0
   }
-  v: 1.3693709537525949
-  a: -0.26279825421055852
+  v: 1.3694525947088758
+  a: -0.26273638911430736
   relative_time: 1.3
 }
 trajectory_point {
   path_point {
-    x: 587257.91249596584
-    y: 4140944.9997988641
-    theta: -1.7962528697721682
-    kappa: -0.0034257654866406556
-    s: 2.1300323578698412
-    dkappa: -0.0023826631302690522
+    x: 587257.91248827451
+    y: 4140944.9997390942
+    theta: -1.7962453675348422
+    kappa: -0.0034190861611428465
+    s: 2.13009234090789
+    dkappa: -0.00237926674783578
     ddkappa: 0
   }
-  v: 1.3427484065442445
-  a: -0.26976198126474366
+  v: 1.3428361308661427
+  a: -0.26970223514937136
   relative_time: 1.4000000000000001
 }
 trajectory_point {
   path_point {
-    x: 587257.88260923012
-    y: 4140944.8702888796
-    theta: -1.796826069461434
-    kappa: -0.0037620970597899879
-    s: 2.2629460630600331
-    dkappa: -0.0025386431233069794
+    x: 587257.88260084286
+    y: 4140944.8702199645
+    theta: -1.7968175653701652
+    kappa: -0.0037550840413337963
+    s: 2.2630151133176342
+    dkappa: -0.0025348477640409111
     ddkappa: 0
   }
-  v: 1.3154005833151534
-  a: -0.27726493385370871
+  v: 1.3154941626295837
+  a: -0.27720763266697135
   relative_time: 1.5000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587257.85334600473
-    y: 4140944.7434807895
-    theta: -1.7973873114351258
-    kappa: -0.0040914119349416108
-    s: 2.3930868637609337
-    dkappa: -0.0026913689927591136
+    x: 587257.85333676473
+    y: 4140944.7434021784
+    theta: -1.7973778296673883
+    kappa: -0.00408407411113973
+    s: 2.3931655539960932
+    dkappa: -0.0026871838720922943
     ddkappa: 0
   }
-  v: 1.2872852137433417
-  a: -0.28507406734750484
+  v: 1.2873843881481015
+  a: -0.28501951572397405
   relative_time: 1.6000000000000003
 }
 trajectory_point {
   path_point {
-    x: 587257.82472380542
-    y: 4140944.6194504965
-    theta: -1.797936259373661
-    kappa: -0.0044135129972486982
-    s: 2.5203768628414638
-    dkappa: -0.0028407493229663555
+    x: 587257.82471356122
+    y: 4140944.6193616642
+    theta: -1.7979258246006991
+    kappa: -0.0044058593952572642
+    s: 2.5204657383343156
+    dkappa: -0.0028361838642184689
     ddkappa: 0
   }
-  v: 1.258383331969823
-  a: -0.29295633711618363
+  v: 1.2584878121649132
+  a: -0.29290481837724613
   relative_time: 1.7000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587257.79676031251
-    y: 4140944.498274616
-    theta: -1.7984725738485829
-    kappa: -0.0047282012811870689
-    s: 2.6447374318077945
-    dkappa: -0.0029866918399837229
+    x: 587257.79674891871
+    y: 4140944.4981750674
+    theta: -1.7984612112286935
+    kappa: -0.0047202410625062294
+    s: 2.6448370075273879
+    dkappa: -0.0029817556732997059
     ddkappa: 0
   }
-  v: 1.2286992765986071
-  a: -0.30067869852979606
+  v: 1.2288087460175479
+  a: -0.30063047468365461
   relative_time: 1.8000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587257.76947284664
-    y: 4140944.3800282031
-    theta: -1.7989959223722718
-    kappa: -0.0050352818676345727
-    s: 2.7660915412496476
-    dkappa: -0.0031291061464600793
+    x: 587257.76946016448
+    y: 4140944.3799174759
+    theta: -1.7989836575267608
+    kappa: -0.0050270243171951223
+    s: 2.7662022992338646
+    dkappa: -0.0031238091008156492
     ddkappa: 0
   }
-  v: 1.1982606906966975
-  a: -0.30800810695839376
+  v: 1.1983748076378489
+  a: -0.30796341870006627
   relative_time: 1.9000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587257.742877845
-    y: 4140944.2647824869
-    theta: -1.7995059894473771
-    kappa: -0.0053345697809504657
-    s: 2.884366091286596
-    dkappa: -0.0032679064565178609
+    x: 587257.74286374287
+    y: 4140944.2646601526
+    theta: -1.7994928484191879
+    kappa: -0.0053260242904865545
+    s: 2.8844884782352023
+    dkappa: -0.0032622585447929483
     ddkappa: 0
   }
-  v: 1.1671185217940936
-  a: -0.31471151777202794
+  v: 1.1672369215519722
+  a: -0.31467058448334773
   relative_time: 2.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587257.71693544416
-    y: 4140944.1526156249
-    theta: -1.8000292042844301
-    kappa: -0.0056285566644575253
-    s: 2.9994940482732408
-    dkappa: -0.0034075840465787777
+    x: 587257.7169191502
+    y: 4140944.1524814432
+    theta: -1.8000155470316537
+    kappa: -0.0056197722226754695
+    s: 2.99962847358716
+    dkappa: -0.0034016958769803752
     ddkappa: 0
   }
-  v: 1.1353390936972492
-  a: -0.32080265902147326
+  v: 1.1354613999512881
+  a: -0.32076540436576872
   relative_time: 2.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587257.69115456787
-    y: 4140944.0437051011
-    theta: -1.8008108202516295
-    kappa: -0.0059415422025901445
-    s: 3.1114143584264715
-    dkappa: -0.0035900600992679059
+    x: 587257.691137532
+    y: 4140944.0435583405
+    theta: -1.8007960948185053
+    kappa: -0.0059325338684807563
+    s: 3.1115611949180693
+    dkappa: -0.0035842786529369066
     ddkappa: 0
   }
-  v: 1.1029731802618306
-  a: -0.32643809210996994
+  v: 1.1030990415831561
+  a: -0.32640419711806484
   relative_time: 2.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587257.66612555168
-    y: 4140943.9379707868
-    theta: -1.8015696446572294
-    kappa: -0.0062453999938692833
-    s: 3.2200706830775707
-    dkappa: -0.0037672145156101722
+    x: 587257.66610763408
+    y: 4140943.9378111321
+    theta: -1.8015538870706722
+    kappa: -0.0062361762559769033
+    s: 3.2202302699139769
+    dkappa: -0.0037615378237804767
     ddkappa: 0
   }
-  v: 1.0700672920339951
-  a: -0.331599022436267
+  v: 1.0701963859748604
+  a: -0.3315682264548015
   relative_time: 2.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587257.64186028019
-    y: 4140943.8354628887
-    theta: -1.8023053160765499
-    kappa: -0.0065399857564974722
-    s: 3.3254114285854439
-    dkappa: -0.0039389631767896265
+    x: 587257.6418413471
+    y: 4140943.8352900548
+    theta: -1.8022885626840606
+    kappa: -0.0065305551358360563
+    s: 3.3255840738950875
+    dkappa: -0.0039333891802000569
     ddkappa: 0
   }
-  v: 1.0366698190200254
-  a: -0.3362666553991136
+  v: 1.0368018462822288
+  a: -0.33623875609054443
   relative_time: 2.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587257.61836950132
-    y: 4140943.7362268111
-    theta: -1.8030175076926276
-    kappa: -0.006825169003755233
-    s: 3.4273899342826297
-    dkappa: -0.0041052300067624982
+    x: 587257.61834942515
+    y: 4140943.7360405428
+    theta: -1.8029997951248282
+    kappa: -0.0068155400480653114
+    s: 3.4275759171786144
+    dkappa: -0.0040997565627688892
     ddkappa: 0
   }
-  v: 1.0028310306863291
-  a: -0.34042219639725885
+  v: 1.0029657092896322
+  a: -0.34039704973985913
   relative_time: 2.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587257.5956627843
-    y: 4140943.6403029789
-    theta: -1.8037059286009494
-    kappa: -0.00710083356959276
-    s: 3.5259646604213142
-    dkappa: -0.0042659472786863535
+    x: 587257.59564144257
+    y: 4140943.6401030449
+    theta: -1.8036872937281512
+    kappa: -0.0070910148455347313
+    s: 3.5261642324416362
+    dkappa: -0.004260572167567641
     ddkappa: 0
   }
-  v: 0.96860307595943873
-  a: -0.34404685082945197
+  v: 0.9687401354099856
+  a: -0.34402437111731127
   relative_time: 2.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 587257.57374847471
-    y: 4140943.5477266479
-    theta: -1.8043703251133671
-    kappa: -0.0073668781342216046
-    s: 3.6210993761193437
-    dkappa: -0.0044210559213492574
+    x: 587257.57372575044
+    y: 4140943.5475128447
+    theta: -1.8043508049961736
+    kappa: -0.0073568782175053495
+    s: 3.6213127620839494
+    dkappa: -0.0044157768518075357
     ddkappa: 0
   }
-  v: 0.934039983226012
-  a: -0.34712182409444192
+  v: 0.93417915868474655
+  a: -0.34710198393746639
   relative_time: 2.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587257.552633652
-    y: 4140943.4585277247
-    theta: -1.8050104820613222
-    kappa: -0.0076232167497063495
-    s: 3.712763347306236
-    dkappa: -0.0045705058255989311
+    x: 587257.552609434
+    y: 4140943.4582998757
+    theta: -1.8049901138952693
+    kappa: -0.0076130442131571788
+    s: 3.7129907455909232
+    dkappa: -0.0045653204394535025
     ddkappa: 0
   }
-  v: 0.899197660332832
-  a: -0.34962832159097795
+  v: 0.89933868678391671
+  a: -0.34961115191489012
   relative_time: 2.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587257.53232408653
-    y: 4140943.3727305853
-    theta: -1.8056262240985024
-    kappa: -0.00786977936555629
-    s: 3.8009315246691928
-    dkappa: -0.0047142561507719071
+    x: 587257.5322982691
+    y: 4140943.3724885397
+    theta: -1.8056050451527377
+    kappa: -0.0078594427651172243
+    s: 3.8011731068963552
+    dkappa: -0.0047091620268473189
     ddkappa: 0
   }
-  v: 0.86413389458680656
-  a: -0.35154754871780919
+  v: 0.8642765010060407
+  a: -0.35153313876414805
   relative_time: 2.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587257.51282419544
-    y: 4140943.2903538914
-    theta: -1.806217417003066
-    kappa: -0.00810651235431712
-    s: 3.8855847315991148
-    dkappa: -0.0048522756311226971
+    x: 587257.512796679
+    y: 4140943.2900975244
+    theta: -1.8061954645530705
+    kappa: -0.0080960202129874877
+    s: 3.8858406417453235
+    dkappa: -0.004847270288330748
     ddkappa: 0
   }
-  v: 0.82890835275496877
-  a: -0.3528607108736847
+  v: 0.829052256278207
+  a: -0.35284920819980575
   relative_time: 3.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587257.49413700006
-    y: 4140943.211410406
-    theta: -1.8067839689795631
-    kappa: -0.0083333790371626065
-    s: 3.9667098521366095
-    dkappa: -0.004984542882252938
+    x: 587257.49410769145
+    y: 4140943.2111396207
+    theta: -1.8067612802339124
+    kappa: -0.0083227398268729755
+    s: 3.9669802050570429
+    dkappa: -0.0049796237818686877
     ddkappa: 0
   }
-  v: 0.79358258106447654
-  a: -0.35354901345735362
+  v: 0.7937274811560473
+  a: -0.35354062393642888
   relative_time: 3.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587257.47578462074
-    y: 4140943.1360224285
-    theta: -1.8074965302832688
-    kappa: -0.008563905344074408
-    s: 4.0443000189180083
-    dkappa: -0.00509766351856637
+    x: 587257.47575176973
+    y: 4140943.1357375486
+    theta: -1.8074738411620515
+    kappa: -0.0085532148125089559
+    s: 4.0445848982877184
+    dkappa: -0.0050925561247197543
     ddkappa: 0
   }
-  v: 0.75822000520261312
-  a: -0.35359366186756508
+  v: 0.75836557782373681
+  a: -0.353588649688583
   relative_time: 3.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587257.45812359266
-    y: 4140943.0641044276
-    theta: -1.80822820521369
-    kappa: -0.00878802049536766
-    s: 4.1183548011213773
-    dkappa: -0.0052015860207101014
+    x: 587257.45808948018
+    y: 4140943.0638048481
+    theta: -1.8082047393211678
+    kappa: -0.0087772304183524417
+    s: 4.1186542567934
+    dkappa: -0.0051962969803455864
     ddkappa: 0
   }
-  v: 0.722885930316787
-  a: -0.35297586150306826
+  v: 0.72303182209399419
+  a: -0.35297454917083371
   relative_time: 3.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587257.44130422722
-    y: 4140942.9956137864
-    theta: -1.8089250146849489
-    kappa: -0.0090014551054223724
-    s: 4.1888803924125293
-    dkappa: -0.0053005559412753287
+    x: 587257.44126874465
+    y: 4140942.9952995209
+    theta: -1.8089008160053375
+    kappa: -0.0089905723665924288
+    s: 4.1891944371928354
+    dkappa: -0.0052950949018920074
     ddkappa: 0
   }
-  v: 0.6876475410145314
-  a: -0.35167681776261217
+  v: 0.68779336340808128
+  a: -0.35167958609774658
   relative_time: 3.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 587257.42532342265
-    y: 4140942.9305378734
-    theta: -1.8095870860472767
-    kappa: -0.00920424853515335
-    s: 4.2558897988910367
-    dkappa: -0.0053945915319720782
+    x: 587257.42528647138
+    y: 4140942.9302089745
+    theta: -1.8095621980042211
+    kappa: -0.0091932798693554474
+    s: 4.256218404730328
+    dkappa: -0.0053889680483597754
     ddkappa: 0
   }
-  v: 0.6525739013635049
-  a: -0.349677736044946
+  v: 0.6527192248358038
+  a: -0.34968502418388719
   relative_time: 3.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587257.41017640743
-    y: 4140942.86885726
-    theta: -1.8102146160326373
-    kappa: -0.0093964613310237831
-    s: 4.319403027036242
-    dkappa: -0.0054837208682781274
+    x: 587257.41013790085
+    y: 4140942.8685138253
+    theta: -1.8101890813580883
+    kappa: -0.0093854132974578348
+    s: 4.3197461206385865
+    dkappa: -0.0054779443772660415
     ddkappa: 0
   }
-  v: 0.6177359548914908
-  a: -0.34695982174881895
+  v: 0.61788030307551067
+  a: -0.34697212714382125
   relative_time: 3.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 587257.39585669641
-    y: 4140942.8105455358
-    theta: -1.8108078725870016
-    kappa: -0.0095781757938343121
-    s: 4.3794472716532766
-    dkappa: -0.0055679821131872868
+    x: 587257.39581655979
+    y: 4140942.81018771
+    theta: -1.8107817331820917
+    kappa: -0.00956705474706657
+    s: 4.3798047295015827
+    dkappa: -0.0055620619070630151
     ddkappa: 0
   }
-  v: 0.5832065245863981
-  a: -0.34350428027298
+  v: 0.58334936845409424
+  a: -0.34352215869211428
   relative_time: 3.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 587257.3823560446
-    y: 4140942.7555691269
-    theta: -1.8113671967034555
-    kappa: -0.00974949654751204
-    s: 4.4360571038190644
-    dkappa: -0.0056474237809576323
+    x: 587257.38231421821
+    y: 4140942.7551971106
+    theta: -1.8113404934913684
+    kappa: -0.0097383086063600882
+    s: 4.4364287466174064
+    dkappa: -0.0056413689795566536
     ddkappa: 0
   }
-  v: 0.54906031289626023
-  a: -0.3392923170161784
+  v: 0.54920106492699028
+  a: -0.33931638254333185
   relative_time: 3.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 587257.36966440314
-    y: 4140942.7038871148
-    theta: -1.8118930042563608
-    kappa: -0.00991055110789962
-    s: 4.489274658828343
-    dkappa: -0.0057221050008597861
+    x: 587257.36962084309
+    y: 4140942.7035011654
+    theta: -1.8118657770271922
+    kappa: -0.0098993021221890909
+    s: 4.4896602453611152
+    dkappa: -0.0057159245223253156
     ddkappa: 0
   }
-  v: 0.51537390172923614
-  a: -0.33430513737716322
+  v: 0.51551191007817809
+  a: -0.33433606241203967
   relative_time: 3.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 587257.35776987509
-    y: 4140942.6554510505
-    theta: -1.8123857878367604
-    kappa: -0.010061490451544268
-    s: 4.5391498241396695
-    dkappa: -0.0057920957809251611
+    x: 587257.35772455519
+    y: 4140942.6550514945
+    theta: -1.8123580750843635
+    kappa: -0.010050185966737381
+    s: 4.5395490445475932
+    dkappa: -0.0057857983111384535
     ddkappa: 0
   }
-  v: 0.48222575245360982
-  a: -0.32852394675468366
+  v: 0.48236029512018064
+  a: -0.3285624620128032
   relative_time: 4.0000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587257.34665864706
-    y: 4140942.6102046864
-    theta: -1.8128461194931931
-    kappa: -0.010202489861381728
-    s: 4.5857405188163263
-    dkappa: -0.0058574774000907711
+    x: 587257.346611561
+    y: 4140942.6097919215
+    theta: -1.8128179582509285
+    kappa: -0.010191135083371036
+    s: 4.5861529881062912
+    dkappa: -0.0058510713616664461
     ddkappa: 0
   }
-  v: 0.44969978879748257
-  a: -0.32182630835156784
+  v: 0.44983010047601873
+  a: -0.3218722222781038
   relative_time: 4.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587257.33631473547
-    y: 4140942.5680829347
-    theta: -1.8132746612984174
-    kappa: -0.010333752169027797
-    s: 4.6291137648826837
-    dkappa: -0.00591834391165424
+    x: 587257.33626589645
+    y: 4140942.5676574297
+    theta: -1.8132460870446134
+    kappa: -0.010322351952280016
+    s: 4.62953902484519
+    dkappa: -0.0059118374417879843
     ddkappa: 0
   }
-  v: 0.41789306971197
-  a: -0.31415130208824804
+  v: 0.4180184667838247
+  a: -0.31420349896546718
   relative_time: 4.2000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587257.32671985123
-    y: 4140942.5290113194
-    theta: -1.8136721709033208
-    kappa: -0.010455509461161833
-    s: 4.6693462511676458
-    dkappa: -0.0059748029345261808
+    x: 587257.32666928926
+    y: 4140942.5285736034
+    theta: -1.8136432175428994
+    kappa: -0.010444068321243264
+    s: 4.6697837807174558
+    dkappa: -0.0059682038731014315
     ddkappa: 0
   }
-  v: 0.38689965795933479
-  a: -0.30557240843987943
+  v: 0.3870195656037857
+  a: -0.30562982802702565
   relative_time: 4.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587257.31785354926
-    y: 4140942.4929065853
-    theta: -1.8140394953173939
-    kappa: -0.01056802117919331
-    s: 4.7065237053739679
-    dkappa: -0.0060269747720429652
+    x: 587257.31780131
+    y: 4140942.492457238
+    theta: -1.8140101951739391
+    kappa: -0.010556543307304805
+    s: 4.7069729311525128
+    dkappa: -0.006020290651817555
     ddkappa: 0
   }
-  v: 0.3568062682543241
-  a: -0.29616310788161715
+  v: 0.35692021490087555
+  a: -0.29622474541491151
   relative_time: 4.4
 }
 trajectory_point {
   path_point {
-    x: 587257.30969340436
-    y: 4140942.4596774154
-    theta: -1.8143775636350685
-    kappa: -0.010671571895490794
-    s: 4.7407401592735035
-    dkappa: -0.0060749913808010644
+    x: 587257.30963954586
+    y: 4140942.4592170557
+    theta: -1.8143479474465964
+    kappa: -0.010660061172750025
+    s: 4.7412004656965214
+    dkappa: -0.0060682294188218059
     ddkappa: 0
   }
-  v: 0.32769226726416956
-  a: -0.28599688088861636
+  v: 0.32779987904485497
+  a: -0.28606178708125718
   relative_time: 4.5
 }
 trajectory_point {
   path_point {
-    x: 587257.30221518606
-    y: 4140942.4292251393
-    theta: -1.8146873797639049
-    kappa: -0.010766469089610931
-    s: 4.7720972139024518
-    dkappa: -0.0061189953394913876
+    x: 587257.30215977761
+    y: 4140942.4287544172
+    theta: -1.8146574766823329
+    kappa: -0.01075492910108199
+    s: 4.7725679526528593
+    dkappa: -0.0061121624297366091
     ddkappa: 0
   }
-  v: 0.29962967360858717
-  a: -0.2751472079360322
+  v: 0.29973066881027122
+  a: -0.27521448897819484
   relative_time: 4.6
 }
 trajectory_point {
   path_point {
-    x: 587257.29539303423
-    y: 4140942.4014444505
-    theta: -1.8149700151546098
-    kappa: -0.01085304092452744
-    s: 4.80070330475661
-    dkappa: -0.0061591388177336306
+    x: 587257.29533615371
+    y: 4140942.40096404
+    theta: -1.8149398527489178
+    kappa: -0.010841474972997735
+    s: 4.801183803722596
+    dkappa: -0.0061522415249836435
     ddkappa: 0
   }
-  v: 0.27268315785977754
-  a: -0.26368756949901978
+  v: 0.2727773413764587
+  a: -0.26375638705785692
   relative_time: 4.6999999999999993
 }
 trajectory_point {
   path_point {
-    x: 587257.28919963364
-    y: 4140942.3762241183
-    theta: -1.8152266015328142
-    kappa: -0.010931634022860102
-    s: 4.8266729669866191
-    dkappa: -0.0061955825449106095
+    x: 587257.28914136614
+    y: 4140942.3757347064
+    theta: -1.8151962057958901
+    kappa: -0.010920045142364568
+    s: 4.8271625386449752
+    dkappa: -0.0061886270998461231
     ddkappa: 0
   }
-  v: 0.24691004254242582
-  a: -0.2516914460527343
+  v: 0.24699730032753817
+  a: -0.25176101727237565
   relative_time: 4.7999999999999989
 }
 trajectory_point {
   path_point {
-    x: 587257.28360638977
-    y: 4140942.3534477027
-    theta: -1.8154583236324964
-    kappa: -0.011002611243103734
-    s: 4.8501261005932133
-    dkappa: -0.006228494779002597
+    x: 587257.283546826
+    y: 4140942.352949983
+    theta: -1.8154277189916646
+    kappa: -0.010991002212196377
+    s: 4.8506240498378919
+    dkappa: -0.0062214870745310865
     ddkappa: 0
   }
-  v: 0.2223603021337015
-  a: -0.23923231807233086
+  v: 0.22244059565241742
+  a: -0.23930191557388347
   relative_time: 4.8999999999999986
 }
 trajectory_point {
   path_point {
-    x: 587257.27858360414
-    y: 4140942.3329942673
-    theta: -1.8156664119309134
-    kappa: -0.011066349455857185
-    s: 4.8711872356224664
-    dkappa: -0.006258050275421671
+    x: 587257.27852283826
+    y: 4140942.3324889317
+    theta: -1.8156356212621372
+    kappa: -0.011054722810629919
+    s: 4.8716928670383695
+    dkappa: -0.0062509958642316772
     ddkappa: 0
   }
-  v: 0.19907656306325855
-  a: -0.22638366603296467
+  v: 0.1991499237447909
+  a: -0.2264526179145126
   relative_time: 4.9999999999999982
 }
 trajectory_point {
   path_point {
-    x: 587257.274100649
-    y: 4140942.3147390913
-    theta: -1.8158521353848767
-    kappa: -0.011123237320052314
-    s: 4.8899847973610422
-    dkappa: -0.0062844292558460468
+    x: 587257.27403877757
+    y: 4140942.3142268267
+    theta: -1.8158211800306354
+    kappa: -0.011111595366901142
+    s: 4.8904974219430439
+    dkappa: -0.0062773333491894312
     ddkappa: 0
   }
-  v: 0.17709410371323558
-  a: -0.21321897040979082
+  v: 0.17716062740313993
+  a: -0.21328666024639537
   relative_time: 5.0999999999999979
 }
 trajectory_point {
   path_point {
-    x: 587257.27012614312
-    y: 4140942.2985543869
-    theta: -1.8160167941682031
-    kappa: -0.011173673059182997
-    s: 4.9066503715314447
-    dkappa: -0.0063078163770544232
+    x: 587257.27006326371
+    y: 4140942.2980358652
+    theta: -1.8159856939590362
+    kappa: -0.011162017887321454
+    s: 4.9071693128486338
+    dkappa: -0.0063006838447565525
     ddkappa: 0
   }
-  v: 0.15644085441825567
-  a: -0.19981171167796447
+  v: 0.15650069583073245
+  a: -0.19987757852166413
   relative_time: 5.1999999999999975
 }
 trajectory_point {
   path_point {
-    x: 587257.2666281265
-    y: 4140942.2843100084
-    theta: -1.8161617124101639
-    kappa: -0.011218062237534089
-    s: 4.9213179694872622
-    dkappa: -0.00632839969976032
+    x: 587257.26656433637
+    y: 4140942.2837858843
+    theta: -1.816130485689883
+    kappa: -0.011206395731254062
+    s: 4.9218425692924281
+    dkappa: -0.0063212350714582088
     ddkappa: 0
   }
-  v: 0.13713739746542608
-  a: -0.18623537031264073
+  v: 0.13719076463562335
+  a: -0.18629890869245122
   relative_time: 5.2999999999999972
 }
 trajectory_point {
   path_point {
-    x: 587257.26357423619
-    y: 4140942.2718741703
-    theta: -1.8162882309347597
-    kappa: -0.011256815536410426
-    s: 4.9341232934084189
-    dkappa: -0.006346369657446416
+    x: 587257.263509631
+    y: 4140942.2713450729
+    theta: -1.8162568945893229
+    kappa: -0.011245139387090234
+    s: 4.934652916692758
+    dkappa: -0.0063391771250548038
     ddkappa: 0
   }
-  v: 0.11919696709433902
-  a: -0.17256342678897482
+  v: 0.11924411583065397
+  a: -0.17262418671088889
   relative_time: 5.3999999999999968
 }
 trajectory_point {
   path_point {
-    x: 587257.260931881
-    y: 4140942.2611141568
-    theta: -1.8163977000006519
-    kappa: -0.011290346530365803
-    s: 4.9452030014964219
-    dkappa: -0.006361918025198891
+    x: 587257.2608665541
+    y: 4140942.2605806892
+    theta: -1.8163662694907019
+    kappa: -0.01127866224822564
+    s: 4.9457370409894787
+    dkappa: -0.0063547014466042709
     ddkappa: 0
   }
-  v: 0.10262544949707081
-  a: -0.15886936158212187
+  v: 0.1026666778334529
+  a: -0.15892694852910952
   relative_time: 5.4999999999999964
 }
 trajectory_point {
   path_point {
-    x: 587257.25866841723
-    y: 4140942.251897038
-    theta: -1.816491472041603
-    kappa: -0.011319069463431964
-    s: 4.95469397316961
-    dkappa: -0.0063752368885417639
+    x: 587257.25860245829
+    y: 4140942.251359771
+    theta: -1.8164599614386645
+    kappa: -0.011307378389036615
+    s: 4.9552318532844488
+    dkappa: -0.0063679997925243492
     ddkappa: 0
   }
-  v: 0.087421382818182514
-  a: -0.145226655167237
+  v: 0.087457025466435079
+  a: -0.14528073009924539
   relative_time: 5.5999999999999961
 }
 trajectory_point {
   path_point {
-    x: 587257.25675132347
-    y: 4140942.2440903829
-    theta: -1.8165708944072789
-    kappa: -0.011343397025347594
-    s: 4.9627325742584025
-    dkappa: -0.00638651761227124
+    x: 587257.256684818
+    y: 4140942.2435498526
+    theta: -1.816539316433617
+    kappa: -0.011331700340856492
+    s: 4.9632737544820067
+    dkappa: -0.0063792632046548734
     ddkappa: 0
   }
-  v: 0.073575957154719429
-  a: -0.13170878801947536
+  v: 0.0736063799568023
+  a: -0.13175906737342888
   relative_time: 5.6999999999999957
 }
 trajectory_point {
   path_point {
-    x: 587257.25514837611
-    y: 4140942.2375629735
-    theta: -1.8166373021042963
-    kappa: -0.0113637381277873
-    s: 4.9694539222005494
-    dkappa: -0.00639594980929004
+    x: 587257.25508140458
+    y: 4140942.2370196762
+    theta: -1.8166056681764331
+    kappa: -0.011352036867951882
+    s: 4.96999789992945
+    dkappa: -0.0063886809803200542
     ddkappa: 0
   }
-  v: 0.061073014556211558
-  a: -0.11838924061399211
+  v: 0.061098608936543342
+  a: -0.11843549630379227
   relative_time: 5.7999999999999954
 }
 trajectory_point {
   path_point {
-    x: 587257.25382782461
-    y: 4140942.2321855165
-    theta: -1.8166920105374069
-    kappa: -0.011380495680590593
-    s: 4.9749911512363747
-    dkappa: -0.0064037203094417473
+    x: 587257.2537604617
+    y: 4140942.231639911
+    theta: -1.8166603308132976
+    kappa: -0.011368790743498974
+    s: 4.9755374640575152
+    dkappa: -0.0063964396423907607
     ddkappa: 0
   }
-  v: 0.049889049024673282
-  a: -0.1053414934259424
+  v: 0.049910226442433536
+  a: -0.10538355284246786
   relative_time: 5.899999999999995
 }
 trajectory_point {
   path_point {
-    x: 587257.25275856641
-    y: 4140942.2278313576
-    theta: -1.8167363082507335
-    kappa: -0.011394064367990885
-    s: 4.9794746776040295
-    dkappa: -0.0064100121283451432
+    x: 587257.252690881
+    y: 4140942.22728386
+    theta: -1.8167045916806006
+    kappa: -0.011382356525559859
+    s: 4.9800229050208547
+    dkappa: -0.00640272190934681
     ddkappa: 0
   }
-  v: 0.03999320651460353
-  a: -0.092639026930481344
+  v: 0.040010392916035031
+  a: -0.09267677294158802
   relative_time: 5.9999999999999947
 }
 trajectory_point {
   path_point {
-    x: 587257.25188696908
-    y: 4140942.2243830739
-    theta: -1.8167780467100181
-    kappa: -0.011405253327023145
-    s: 4.9830314647347409
-    dkappa: -0.0064159167489727491
+    x: 587257.25181301788
+    y: 4140942.223835567
+    theta: -1.8167480025516076
+    kappa: -0.011393654101270691
+    s: 4.9835812293385189
+    dkappa: -0.0064088655442349441
     ddkappa: 0
   }
-  v: 0.031347284932985608
-  a: -0.080355321602764124
+  v: 0.031360915203696771
+  a: -0.080388692553285013
   relative_time: 6.0999999999999943
 }
 trajectory_point {
   path_point {
-    x: 587257.25120016071
-    y: 4140942.2217173032
-    theta: -1.8168138037398469
-    kappa: -0.01141413557269888
-    s: 4.9857842884480545
-    dkappa: -0.006420964749052546
+    x: 587257.25112602685
+    y: 4140942.2211686014
+    theta: -1.8167837361771528
+    kappa: -0.01140253589769478
+    s: 4.9863352565344288
+    dkappa: -0.0064139189272362038
     ddkappa: 0
   }
-  v: 0.023905734139287427
-  a: -0.068563857917945836
+  v: 0.023916246556554444
+  a: -0.0685928476296912
   relative_time: 6.199999999999994
 }
 trajectory_point {
   path_point {
-    x: 587257.25068453175
-    y: 4140942.2197159454
-    theta: -1.8168406487472661
-    kappa: -0.011420804019620298
-    s: 4.9878510021470914
-    dkappa: -0.0064247545925020929
+    x: 587257.25061025808
+    y: 4140942.2191663361
+    theta: -1.8168105637434622
+    kappa: -0.011409204039637728
+    s: 4.9884028837778613
+    dkappa: -0.0064177128303539452
     ddkappa: 0
   }
-  v: 0.017615655945461384
-  a: -0.057338116351181745
+  v: 0.017623486630530538
+  a: -0.057362774122938887
   relative_time: 6.2999999999999936
 }
 trajectory_point {
   path_point {
-    x: 587257.2503120898
-    y: 4140942.2182703535
-    theta: -1.8168600390650103
-    kappa: -0.011425620679363736
-    s: 4.9893438020137877
-    dkappa: -0.006427492019461095
+    x: 587257.25023771345
+    y: 4140942.2177200806
+    theta: -1.8168299415544609
+    kappa: -0.01141402050173526
+    s: 4.9898963505239236
+    dkappa: -0.0064204532025182812
     ddkappa: 0
   }
-  v: 0.012416804115944147
-  a: -0.046751577377626863
+  v: 0.012422381486334289
+  a: -0.0467720079851604
   relative_time: 6.3999999999999932
 }
 trajectory_point {
   path_point {
-    x: 587257.25005643757
-    y: 4140942.2172780666
-    theta: -1.816873349002232
-    kappa: -0.011428926939030241
-    s: 4.9903684922041514
-    dkappa: -0.0064293710486694328
+    x: 587257.24998198962
+    y: 4140942.2167273355
+    theta: -1.8168432429674857
+    kappa: -0.011417326640824149
+    s: 4.9909215031540342
+    dkappa: -0.0064223342619654763
     ddkappa: 0
   }
-  v: 0.008241584367657262
-  a: -0.0368777214724364
+  v: 0.0082453235894616728
+  a: -0.036894085168488022
   relative_time: 6.4999999999999929
 }
 trajectory_point {
   path_point {
-    x: 587257.24989295576
-    y: 4140942.2166435304
-    theta: -1.816881860298617
-    kappa: -0.011431041190328622
-    s: 4.9910237500435048
-    dkappa: -0.0064305726300163775
+    x: 587257.24981846125
+    y: 4140942.2160925032
+    theta: -1.8168517488506237
+    kappa: -0.011419440824392071
+    s: 4.9915770596163975
+    dkappa: -0.0064235371469218028
     ddkappa: 0
   }
-  v: 0.0050150543700063221
-  a: -0.027790029110765502
+  v: 0.005017351810195414
+  a: -0.027802541625054134
   relative_time: 6.5999999999999925
 }
 trajectory_point {
   path_point {
-    x: 587257.24979898671
-    y: 4140942.2162788
-    theta: -1.8168867525788435
-    kappa: -0.01143225645865854
-    s: 4.9914003912217346
-    dkappa: -0.0064312632970898133
+    x: 587257.24972446507
+    y: 4140942.2157276003
+    theta: -1.8168566380403923
+    kappa: -0.0114206560590275
+    s: 4.99195387406649
+    dkappa: -0.0064242285662874342
     ddkappa: 0
   }
-  v: 0.0026549237448816876
-  a: -0.019561980767769216
+  v: 0.0026561514236051464
+  a: -0.019570913306991067
   relative_time: 6.6999999999999922
 }
 trajectory_point {
   path_point {
-    x: 587257.24975401722
-    y: 4140942.2161042555
-    theta: -1.8168890938073328
-    kappa: -0.01143283803219358
-    s: 4.9915806349885417
-    dkappa: -0.0064315938197254709
+    x: 587257.24967948243
+    y: 4140942.2155529736
+    theta: -1.8168589777997082
+    kappa: -0.011421237618869529
+    s: 4.9921342015075307
+    dkappa: -0.0064245594503203043
     ddkappa: 0
   }
-  v: 0.001071554066658098
-  a: -0.012267056918602914
+  v: 0.0010720541095470826
+  a: -0.012272736166431042
   relative_time: 6.7999999999999918
 }
 trajectory_point {
   path_point {
-    x: 587257.24973986251
-    y: 4140942.2160493159
-    theta: -1.8168898307432348
-    kappa: -0.011433021090964342
-    s: 4.9916373693486884
-    dkappa: -0.006431697856556149
+    x: 587257.24966532341
+    y: 4140942.2154980069
+    theta: -1.8168597142760936
+    kappa: -0.011421420674057782
+    s: 4.9921909624309668
+    dkappa: -0.0064246636013199944
     ddkappa: 0
   }
-  v: 0.00016795886219489464
-  a: -0.0059787380384215827
+  v: 0.00016803795266445709
+  a: -0.0059815461555065563
   relative_time: 6.8999999999999915
 }
 trajectory_point {
   path_point {
-    x: 587257.24974084878
-    y: 4140942.2160531436
-    theta: -1.8168897793956034
-    kappa: -0.011433008335941497
-    s: 4.9916334162572475
-    dkappa: -0.00643169060756094
+    x: 587257.24966631
+    y: 4140942.2155018365
+    theta: -1.8168596629600682
+    kappa: -0.01142140791918224
+    s: 4.9921870074569457
+    dkappa: -0.0064246563443115965
     ddkappa: 0
   }
-  v: -0.00016019638916414669
-  a: -0.000770504602380262
+  v: -0.00016027255761302817
+  a: -0.00077087922634966333
   relative_time: 6.9999999999999911
 }
 trajectory_point {
   path_point {
-    x: 587257.24974399724
-    y: 4140942.216065364
-    theta: -1.8168896154787078
-    kappa: -0.011432967618118876
-    s: 4.9916207968148507
-    dkappa: -0.0064316674666144574
+    x: 587257.24966945953
+    y: 4140942.2155140629
+    theta: -1.8168594991436797
+    kappa: -0.011421367201733141
+    s: 4.9921743819748
+    dkappa: -0.0064246331777295983
     ddkappa: 0
   }
-  v: -2.4594255590926206e-05
-  a: 0.0032841629143656181
+  v: -2.4606527068815609e-05
+  a: 0.0032857286689070286
   relative_time: 7.0999999999999908
 }
 trajectory_point {
   path_point {
-    x: 587257.249739207
-    y: 4140942.2160467715
-    theta: -1.8168898648674376
-    kappa: -0.011433029567596552
-    s: 4.9916399964629345
-    dkappa: -0.0064317026740360553
+    x: 587257.24966466788
+    y: 4140942.2154954611
+    theta: -1.8168597483791258
+    kappa: -0.011421429150550808
+    s: 4.9921935907835211
+    dkappa: -0.0064246684241017517
     ddkappa: 0
   }
-  v: 0.00045573464722731272
-  a: 0.0061117840366610721
+  v: 0.00045595334330056358
+  a: 0.0061147415781314107
   relative_time: 7.19999999999999
 }
 trajectory_point {
   path_point {
-    x: 587257.24971943884
-    y: 4140942.2159700436
-    theta: -1.8168908940527593
-    kappa: -0.011433285222663908
-    s: 4.9917192301789948
-    dkappa: -0.0064318479691390651
+    x: 587257.24964489345
+    y: 4140942.2154186955
+    theta: -1.8168607769374261
+    kappa: -0.011421684804275549
+    s: 4.992272862732241
+    dkappa: -0.0064248138807329472
     ddkappa: 0
   }
-  v: 0.0011544116560877615
-  a: 0.0076388782893510587
+  v: 0.0011549707572852119
+  a: 0.007642623549191041
   relative_time: 7.29999999999999
 }
 trajectory_point {
   path_point {
-    x: 587257.24968089792
-    y: 4140942.2158204513
-    theta: -1.8168929005971839
-    kappa: -0.011433783658882715
-    s: 4.9918737076718269
-    dkappa: -0.0064321312427800077
+    x: 587257.24960634031
+    y: 4140942.2152690291
+    theta: -1.8168627822671102
+    kappa: -0.011422183239797511
+    s: 4.9924274153607122
+    dkappa: -0.0064250974703890977
     ddkappa: 0
   }
-  v: 0.0019377100602721575
-  a: 0.0077919651972802595
+  v: 0.0019386558234621432
+  a: 0.0077958386299537
   relative_time: 7.39999999999999
 }
 trajectory_point {
   path_point {
-    x: 587257.2496232175
-    y: 4140942.2155965711
-    theta: -1.8168959035902252
-    kappa: -0.01143452961817022
-    s: 4.9921048985767831
-    dkappa: -0.006432555189907832
+    x: 587257.24954864162
+    y: 4140942.2150450367
+    theta: -1.8168657834528914
+    kappa: -0.011422929200706528
+    s: 4.99265871953978
+    dkappa: -0.0064255218919809969
     ddkappa: 0
   }
-  v: 0.0026645551015466173
-  a: 0.0064975642852935778
+  v: 0.0026658650551950047
+  a: 0.0065008508682869448
   relative_time: 7.4999999999999893
 }
 trajectory_point {
   path_point {
-    x: 587257.24954964255
-    y: 4140942.2153109978
-    theta: -1.8168997341038271
-    kappa: -0.01143548113788221
-    s: 4.9923997976510117
-    dkappa: -0.0064330959621131277
+    x: 587257.24947504292
+    y: 4140942.2147593196
+    theta: -1.8168696116743106
+    kappa: -0.011423880725742021
+    s: 4.99295376411187
+    dkappa: -0.0064260632712482123
     ddkappa: 0
   }
-  v: 0.003186523974161859
-  a: 0.0036821950782358615
+  v: 0.0031881013706341332
+  a: 0.0036841243120585565
   relative_time: 7.599999999999989
 }
 trajectory_point {
   path_point {
-    x: 587257.24946721224
-    y: 4140942.2149910531
-    theta: -1.8169040256477567
-    kappa: -0.011436547179896093
-    s: 4.9927301899687144
-    dkappa: -0.0064337018201773627
+    x: 587257.24939258583
+    y: 4140942.2144392123
+    theta: -1.8168739006643486
+    kappa: -0.011424946777242821
+    s: 4.9932843205314565
+    dkappa: -0.0064266698114429418
     ddkappa: 0
   }
-  v: 0.0033478458248528686
-  a: -0.00072762289904804156
+  v: 0.0033495140927169431
+  a: -0.0007278769908638516
   relative_time: 7.6999999999999886
 }
 trajectory_point {
   path_point {
-    x: 587257.249386944
-    y: 4140942.2146795015
-    theta: -1.816908204624978
-    kappa: -0.011437585259693989
-    s: 4.9930519161163893
-    dkappa: -0.0064342917866221016
+    x: 587257.24931229115
+    y: 4140942.2141275005
+    theta: -1.8168780771680166
+    kappa: -0.011425984869597079
+    s: 4.993606207505553
+    dkappa: -0.0064272604440139052
     ddkappa: 0
   }
-  v: 0.0029854017528395116
-  a: -0.0068053701217132279
+  v: 0.0029868989491672604
+  a: -0.0068086889926124439
   relative_time: 7.7999999999999883
 }
 trajectory_point {
   path_point {
-    x: 587257.24932401674
-    y: 4140942.2144352561
-    theta: -1.8169114807870381
-    kappa: -0.011438399075445782
-    s: 4.99330413738808
-    dkappa: -0.0064347542982582306
+    x: 587257.24924934306
+    y: 4140942.21388313
+    theta: -1.8168813514009585
+    kappa: -0.011426798697692093
+    s: 4.9938585556341808
+    dkappa: -0.00642772347929021
     ddkappa: 0
   }
-  v: 0.0019287248098258103
-  a: -0.01462452706491485
+  v: 0.0019296980724957669
+  a: -0.014631847645319773
   relative_time: 7.8999999999999879
 }
 trajectory_point {
   path_point {
-    x: 587257.24929795391
-    y: 4140942.2143340958
-    theta: -1.8169128376895161
-    kappa: -0.011438736137092216
-    s: 4.9934086009806249
-    dkappa: -0.0064349458587351881
+    x: 587257.24922327138
+    y: 4140942.2137819175
+    theta: -1.8168827075081202
+    kappa: -0.011427135765364206
+    s: 4.9939630720508532
+    dkappa: -0.0064279152571652251
     ddkappa: 0
   }
-  v: 2.7755575615628914e-16
-  a: -0.024258574203808114
+  v: 2.2204460492503131e-16
+  a: -0.024270888901118004
   relative_time: 7.9999999999999876
 }
 decision {
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_stop_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_stop_0.pb.txt
index 01746a9c72d2cae3441e37298ed4d27b2cd84793..10aefbbc9eb5594162600f1846afce180fa651e2 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_stop_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_stop_0.pb.txt
@@ -19,12 +19,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.75791471277
-    y: 4141549.3113520094
-    theta: 1.2068363044276664
-    kappa: -0.012310363823529211
+    x: 587045.75791524281
+    y: 4141549.3113518059
+    theta: 1.2068346298577517
+    kappa: -0.012313623820875291
     s: 0.26636801586103515
-    dkappa: -0.0040713139307567989
+    dkappa: -0.0040745521019446848
     ddkappa: 0
   }
   v: 2.6146890758563712
@@ -33,12 +33,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.84987272
-    y: 4141549.5507680187
-    theta: 1.2034117301737124
-    kappa: -0.013411529350544972
+    x: 587045.84987376013
+    y: 4141549.5507676192
+    theta: 1.2034084431579919
+    kappa: -0.013417928193053746
     s: 0.52283692344667221
-    dkappa: -0.0079913244954040435
+    dkappa: -0.0079976804967279817
     ddkappa: 0
   }
   v: 2.5146890758563711
@@ -47,12 +47,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.93824518507
-    y: 4141549.780848939
-    theta: 1.2001206885453963
-    kappa: -0.014469759243086284
+    x: 587045.93824671558
+    y: 4141549.7808483513
+    theta: 1.2001158519616149
+    kappa: -0.01447917454379115
     s: 0.76930583103230954
-    dkappa: -0.011758489609834744
+    dkappa: -0.011767841873726747
     ddkappa: 0
   }
   v: 2.4146890758563715
@@ -61,12 +61,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.02303210786
-    y: 4141550.0015947707
-    theta: 1.196963245982015
-    kappa: -0.015485053501153151
+    x: 587046.0230341088
+    y: 4141550.0015940019
+    theta: 1.1969569228018087
+    kappa: -0.015497362873087506
     s: 1.0057747386179465
-    dkappa: -0.015372809274048894
+    dkappa: -0.015385036232940968
     ddkappa: 0
   }
   v: 2.3146890758563714
@@ -75,12 +75,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.10749291128
-    y: 4141550.2117244704
-    theta: 1.1930134332669167
-    kappa: -0.016361926393171456
+    x: 587046.10749793414
+    y: 4141550.2117224862
+    theta: 1.1930030727196583
+    kappa: -0.016376676765811556
     s: 1.2322436462035835
-    dkappa: -0.012833629938994147
+    dkappa: -0.012845559498015939
     ddkappa: 0
   }
   v: 2.2146890758563713
@@ -89,12 +89,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.18823649676
-    y: 4141550.4125708318
-    theta: 1.1892344062863534
-    kappa: -0.017199721131294685
+    x: 587046.18824441824
+    y: 4141550.4125676835
+    theta: 1.1892201762404355
+    kappa: -0.017216803461973774
     s: 1.4487125537892207
-    dkappa: -0.010384022571983981
+    dkappa: -0.010395655471660669
     ddkappa: 0
   }
   v: 2.1146890758563712
@@ -103,12 +103,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.2652500507
-    y: 4141550.604138894
-    theta: 1.1856299247874074
-    kappa: -0.017998813094619096
+    x: 587046.26526073692
+    y: 4141550.6041346341
+    theta: 1.1856120039044209
+    kappa: -0.01801811965617834
     s: 1.6551814613748577
-    dkappa: -0.0080475772934387
+    dkappa: -0.0080589272382486019
     ddkappa: 0
   }
   v: 2.0146890758563716
@@ -117,12 +117,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.338533573
-    y: 4141550.7864286555
-    theta: 1.1822000800603016
-    kappa: -0.018759202283144694
+    x: 587046.33854689018
+    y: 4141550.7864233372
+    theta: 1.1821786472825611
+    kappa: -0.018780625348425263
     s: 1.8516503689604951
-    dkappa: -0.005824294103358294
+    dkappa: -0.00583537479777973
     ddkappa: 0
   }
   v: 1.9146890758563715
@@ -131,12 +131,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.40850271261
-    y: 4141550.9592682305
-    theta: 1.1788594177263754
-    kappa: -0.019472616466239309
+    x: 587046.40851900878
+    y: 4141550.9592616968
+    theta: 1.1788344214875424
+    kappa: -0.019496016653579858
     s: 2.0381192765461322
-    dkappa: -0.0043054711274608786
+    dkappa: -0.0043165641419008784
     ddkappa: 0
   }
   v: 1.8146890758563714
@@ -145,12 +145,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.47754686011
-    y: 4141551.1216695271
-    theta: 1.1751161086476458
-    kappa: -0.020091502053437731
+    x: 587046.47756928368
+    y: 4141551.1216603881
+    theta: 1.1750861644671511
+    kappa: -0.020116557443293152
     s: 2.2145881841317689
-    dkappa: -0.006890233724255462
+    dkappa: -0.0069032010422047643
     ddkappa: 0
   }
   v: 1.7146890758563713
@@ -159,12 +159,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.54267846874
-    y: 4141551.2748679956
-    theta: 1.1715847539076878
-    kappa: -0.02067531712336115
+    x: 587046.54270667245
+    y: 4141551.2748563993
+    theta: 1.1715501415022105
+    kappa: -0.020701933920028975
     s: 2.3810570917174063
-    dkappa: -0.0093285250456192677
+    dkappa: -0.0093432604555341546
     ddkappa: 0
   }
   v: 1.6146890758563714
@@ -173,12 +173,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.60389753839
-    y: 4141551.4188636364
-    theta: 1.16826546328306
-    kappa: -0.021224061676009556
+    x: 587046.603931175
+    y: 4141551.41884973
+    theta: 1.1682264628052088
+    kappa: -0.021252146083787334
     s: 2.5375259993030435
-    dkappa: -0.011620345091552284
+    dkappa: -0.011636742381889035
     ddkappa: 0
   }
   v: 1.5146890758563714
@@ -187,12 +187,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.661204069
-    y: 4141551.5536564495
-    theta: 1.1651583158486603
-    kappa: -0.021737735711382954
+    x: 587046.66124279134
+    y: 4141551.5536403805
+    theta: 1.1651152077650837
+    kappa: -0.021767193934568226
     s: 2.6839949068886808
-    dkappa: -0.013765693862054517
+    dkappa: -0.013783646821269414
     ddkappa: 0
   }
   v: 1.4146890758563713
@@ -201,12 +201,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.7145980607
-    y: 4141551.6792464345
-    theta: 1.162263364446058
-    kappa: -0.022216339229481338
+    x: 587046.71464152145
+    y: 4141551.6792283515
+    theta: 1.1622164294332662
+    kappa: -0.022247077472371653
     s: 2.8204638144743179
-    dkappa: -0.015764571357125959
+    dkappa: -0.015783973773675287
     ddkappa: 0
   }
   v: 1.3146890758563712
@@ -215,12 +215,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.76407951349
-    y: 4141551.7956335917
-    theta: 1.1595806398736359
-    kappa: -0.022659872230304714
+    x: 587046.76412736555
+    y: 4141551.7956136419
+    theta: 1.1595301587304483
+    kappa: -0.022691796697197616
     s: 2.9469327220599553
-    dkappa: -0.01761697757676662
+    dkappa: -0.017637723239106652
     ddkappa: 0
   }
   v: 1.2146890758563711
@@ -229,12 +229,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.81044626457
-    y: 4141551.9024677933
-    theta: 1.15698302033403
-    kappa: -0.023056613252837685
+    x: 587046.81049927289
+    y: 4141551.9024455841
+    theta: 1.1569289427035458
+    kappa: -0.023089580006077252
     s: 3.0634016296455924
-    dkappa: -0.018343007988734769
+    dkappa: -0.018364500076025025
     ddkappa: 0
   }
   v: 1.114689075856371
@@ -243,12 +243,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.85435404687
-    y: 4141551.9994612727
-    theta: 1.1543658292686494
-    kappa: -0.02339692856923568
+    x: 587046.85441390611
+    y: 4141551.9994359612
+    theta: 1.1543078292912181
+    kappa: -0.02343075222965792
     s: 3.1698705372312292
-    dkappa: -0.017137290842455341
+    dkappa: -0.017158510115872119
     ddkappa: 0
   }
   v: 1.0146890758563709
@@ -257,12 +257,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.89413782873
-    y: 4141552.0873447219
-    theta: 1.1519943494724416
-    kappa: -0.023705280064071573
+    x: 587046.89420389559
+    y: 4141552.0873166011
+    theta: 1.151932795073189
+    kappa: -0.023739880147410977
     s: 3.2663394448168668
-    dkappa: -0.016044819635635713
+    dkappa: -0.01606579171898596
     ddkappa: 0
   }
   v: 0.91468907585637083
@@ -271,12 +271,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.92979761015
-    y: 4141552.1661181441
-    theta: 1.149868634990741
-    kappa: -0.023981667737345353
+    x: 587046.92986924108
+    y: 4141552.166087504
+    theta: 1.1498038943381448
+    kappa: -0.024016963759336409
     s: 3.3528083524025036
-    dkappa: -0.015065594368275891
+    dkappa: -0.015086344885366572
     ddkappa: 0
   }
   v: 0.81468907585637074
@@ -285,12 +285,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.961333391
-    y: 4141552.2357815374
-    theta: 1.1479887225219754
-    kappa: -0.024226091589057024
+    x: 587046.9614099426
+    y: 4141552.2357486691
+    theta: 1.1479211639497682
+    kappa: -0.024262003065434225
     s: 3.4292772599881407
-    dkappa: -0.014199615040375866
+    dkappa: -0.014220169615013935
     ddkappa: 0
   }
   v: 0.71468907585637087
@@ -299,12 +299,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.98874517146
-    y: 4141552.2963349018
-    theta: 1.146354635266861
-    kappa: -0.024438551619206588
+    x: 587046.98882600013
+    y: 4141552.2963000974
+    theta: 1.1462846272132512
+    kappa: -0.024474998065704419
     s: 3.495746167573778
-    dkappa: -0.013446881651935642
+    dkappa: -0.013467265907928055
     ddkappa: 0
   }
   v: 0.61468907585637078
@@ -313,12 +313,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.01203295146
-    y: 4141552.3477782384
-    theta: 1.1449663863346278
-    kappa: -0.02461904782779404
+    x: 587047.0121174138
+    y: 4141552.3477417883
+    theta: 1.1448942972968548
+    kappa: -0.024655948760146994
     s: 3.552215075159415
-    dkappa: -0.012807394202955225
+    dkappa: -0.012827633764108936
     ddkappa: 0
   }
   v: 0.51468907585637069
@@ -327,12 +327,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.03119673114
-    y: 4141552.3901115451
-    theta: 1.1438239817049776
-    kappa: -0.024767580214819385
+    x: 587047.03128418361
+    y: 4141552.3900737423
+    theta: 1.143750180207205
+    kappa: -0.024804855148761948
     s: 3.5986839827450519
-    dkappa: -0.012281152693434612
+    dkappa: -0.012301273183556578
     ddkappa: 0
   }
   v: 0.4146890758563706
@@ -341,12 +341,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.04623651016
-    y: 4141552.4233348244
-    theta: 1.1429274227449728
-    kappa: -0.024884148780282618
+    x: 587047.04632630944
+    y: 4141552.4232959589
+    theta: 1.1428522773175149
+    kappa: -0.024921717231549283
     s: 3.6351528903306889
-    dkappa: -0.011868157123373795
+    dkappa: -0.011888184166270974
     ddkappa: 0
   }
   v: 0.31468907585637051
@@ -355,12 +355,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.05715228885
-    y: 4141552.4474480748
-    theta: 1.142276708280404
-    kappa: -0.024968753524183748
+    x: 587047.05724379141
+    y: 4141552.4474084382
+    theta: 1.1422005874482821
+    kappa: -0.025006535008509002
     s: 3.6616217979163266
-    dkappa: -0.011568407492772773
+    dkappa: -0.011588366712252121
     ddkappa: 0
   }
   v: 0.21468907585637043
@@ -369,12 +369,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.06394406711
-    y: 4141552.4624512969
-    theta: 1.1418718362214282
-    kappa: -0.025021394446522766
+    x: 587047.0640366294
+    y: 4141552.46241118
+    theta: 1.1417951085002485
+    kappa: -0.025059308479641097
     s: 3.6780907055019636
-    dkappa: -0.011381903801631562
+    dkappa: -0.011401820821500035
     ddkappa: 0
   }
   v: 0.11468907585637034
@@ -383,12 +383,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.06661184493
-    y: 4141552.46834449
-    theta: 1.1417128047424232
-    kappa: -0.02504207154729967
+    x: 587047.0667048234
+    y: 4141552.4683041852
+    theta: 1.1416358386395671
+    kappa: -0.025080037644945572
     s: 3.6845596130876004
-    dkappa: -0.011308646049950154
+    dkappa: -0.011328546494014706
     ddkappa: 0
   }
   v: 0.014689075856370248
@@ -397,12 +397,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.06665052043
-    y: 4141552.4684299254
-    theta: 1.1417104992210898
-    kappa: -0.0250423713088676
+    x: 587047.066743505
+    y: 4141552.4683896173
+    theta: 1.1416335296623559
+    kappa: -0.025080338161307156
     s: 3.684653394604728
-    dkappa: -0.011307584012349046
+    dkappa: -0.01132748421610965
     ddkappa: 0
   }
   v: 0
@@ -411,12 +411,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.06665052043
-    y: 4141552.4684299254
-    theta: 1.1417104992210898
-    kappa: -0.0250423713088676
+    x: 587047.066743505
+    y: 4141552.4683896173
+    theta: 1.1416335296623559
+    kappa: -0.025080338161307156
     s: 3.684653394604728
-    dkappa: -0.011307584012349046
+    dkappa: -0.01132748421610965
     ddkappa: 0
   }
   v: 0