From 44c928d9ea2063a46bd1ce91dd5ebd68e790693e Mon Sep 17 00:00:00 2001
From: Jiangtao Hu <boris.hu@gmail.com>
Date: Sun, 24 Dec 2017 15:28:12 -0800
Subject: [PATCH] planning: adjust min_stop_distance_obstacle from 3 to 6.

---
 modules/planning/common/planning_gflags.cc    |    2 +-
 .../garage_test/result_stop_obstacle_0.pb.txt | 1703 ++++++++--------
 .../result_stop_over_line_0.pb.txt            |   14 +-
 .../result_rightturn_01_0.pb.txt              |  518 ++---
 .../result_slowdown_01_0.pb.txt               | 1705 ++++++++--------
 .../sunnyvale_loop_test/result_stop_0.pb.txt  | 1811 +++++++++--------
 6 files changed, 2937 insertions(+), 2816 deletions(-)

diff --git a/modules/planning/common/planning_gflags.cc b/modules/planning/common/planning_gflags.cc
index 3321c44704..601bf23d93 100644
--- a/modules/planning/common/planning_gflags.cc
+++ b/modules/planning/common/planning_gflags.cc
@@ -187,7 +187,7 @@ DEFINE_double(lateral_ignore_buffer, 3.0,
               "distance, ignore it");
 DEFINE_double(max_stop_distance_obstacle, 10.0,
               "max stop distance from in-lane obstacle (meters)");
-DEFINE_double(min_stop_distance_obstacle, 3.0,
+DEFINE_double(min_stop_distance_obstacle, 6.0,
               "min stop distance from in-lane obstacle (meters)");
 DEFINE_double(stop_distance_destination, 0.5,
               "stop distance from destination line");
diff --git a/modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt b/modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt
index dc4f5e3740..b00570e886 100644
--- a/modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt
@@ -1,7 +1,7 @@
 header {
   module_name: "planning"
 }
-total_path_length: 12.232274347627669
+total_path_length: 9.2456905196758132
 total_path_time: 7.9999999999999885
 is_replan: true
 gear: GEAR_DRIVE
@@ -9,1694 +9,1695 @@ trajectory_point {
   path_point {
     x: 586343.90215949644
     y: 4140690.0647428529
+    z: 0
     theta: 1.6602776953500005
-    kappa: 1.828426117139991e-17
-    s: 2.22769665127881e-19
+    kappa: 1.8289120868160188e-17
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
   v: 4.2611112594604492
-  a: 1.2202416949265955
+  a: 1.1976470743630585
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 586343.89577773237
-    y: 4140690.1499659764
-    theta: 1.6590958463838774
-    kappa: -0.0018643386783865118
-    s: 0.085461732193604423
+    x: 586343.89577818254
+    y: 4140690.1499599661
+    theta: 1.6590959297307577
+    kappa: -0.0018642072024183053
+    s: 0.085455705303686291
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2848359415254356
-  a: 1.1524359815788645
+  v: 4.2841583357476658
+  a: 1.1073662312950954
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 586343.88936154242
-    y: 4140690.2356488286
-    theta: 1.6579075913150461
-    kappa: -0.0037387343740810054
-    s: 0.17138448047286098
+    x: 586343.88936378737
+    y: 4140690.23561885
+    theta: 1.6579080070650849
+    kappa: -0.0037380785631037912
+    s: 0.17135441794036405
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3072170373277228
-  a: 1.0858810588489138
+  v: 4.3054180768069354
+  a: 1.0189106024295445
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 586343.88291291462
-    y: 4140690.3217648622
-    theta: 1.656713301211528
-    kappa: -0.0056226063228654736
-    s: 0.25774162246671617
+    x: 586343.88291895343
+    y: 4140690.3216842203
+    theta: 1.6567144195922374
+    kappa: -0.0056208422091019415
+    s: 0.25766075506865921
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3282794420912047
-  a: 1.0205648678908497
+  v: 4.3249268099739746
+  a: 0.93226249293303254
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 586343.8764338
-    y: 4140690.408288022
-    theta: 1.6555133404233497
-    kappa: -0.0075153845696431347
-    s: 0.34450703129682525
+    x: 586343.87644626852
+    y: 4140690.4081215137
+    theta: 1.6555156496919854
+    kappa: -0.0075117420397960519
+    s: 0.34434005685494545
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3480478098628632
-  a: 0.95647534985877813
+  v: 4.3427205086878278
+  a: 0.84740420797218774
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 586343.8699261127
-    y: 4140690.4951927448
-    theta: 1.6543080666397718
-    kappa: -0.0094165098632134278
-    s: 0.43165507075401383
+    x: 586343.86994826782
+    y: 4140690.4948968813
+    theta: 1.654312169969246
+    kappa: -0.0094100375725028468
+    s: 0.43135837939541066
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.366546553512757
-  a: 0.89360044590680532
+  v: 4.3588347924908772
+  a: 0.76431805271363706
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 586343.863391731
-    y: 4140690.5824539494
-    theta: 1.6530978309464941
-    kappa: -0.011325433551047044
-    s: 0.5191605904747395
+    x: 586343.86342743284
+    y: 4140690.5819771811
+    theta: 1.6531044433387487
+    kappa: -0.011315003788068101
+    s: 0.51868248763812363
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.38379984473403
-  a: 0.83192809718903726
+  v: 4.3733049270288333
+  a: 0.68298633232400829
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 586343.856832497
-    y: 4140690.6700470354
-    theta: 1.6518829778828643
-    kappa: -0.013241617474060935
-    s: 0.60699892111755382
+    x: 586343.856886193
+    y: 4140690.66932997
+    theta: 1.6518929231097743
+    kappa: -0.013225930976462276
+    s: 0.60627984830510084
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.399831614042907
-  a: 0.77144624485958
+  v: 4.3861658240507415
+  a: 0.60339135196992877
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 586343.85025021725
-    y: 4140690.757947878
-    theta: 1.6506638454991143
-    kappa: -0.015164533861393336
-    s: 0.69514586953956337
+    x: 586343.85032692587
+    y: 4140690.7569234991
+    theta: 1.6506780530710126
+    kappa: -0.015142124582376142
+    s: 0.69411862281437253
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4146655507786958
-  a: 0.71214283007253976
+  v: 4.3974520414089779
+  a: 0.52551541681802616
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 586343.84364666324
-    y: 4140690.8461328214
-    theta: 1.6494407654136474
-    kappa: -0.017093665225178786
-    s: 0.78357771397289189
+    x: 586343.84375195764
+    y: 4140690.8447267041
+    theta: 1.6494602675755738
+    kappa: -0.017062905050816427
+    s: 0.78216766020205042
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4283251031037851
-  a: 0.65400579398202252
+  v: 4.4071977830592539
+  a: 0.4493408320349277
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 586343.83702357148
-    y: 4140690.9345786739
-    theta: 1.6482140628703974
-    kappa: -0.019028504255323136
-    s: 0.87227119920114149
+    x: 586343.83716356347
+    y: 4140690.9327091994
+    theta: 1.6482399916261949
+    kappa: -0.018987607672701422
+    s: 0.87039649004439346
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4408334780036478
-  a: 0.59702307774213415
+  v: 4.4154368990606114
+  a: 0.37484990278726105
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 586343.83038264408
-    y: 4140691.0232627084
-    theta: 1.6469840567962857
-    kappa: -0.0209685537142786
-    s: 0.96120353173585471
+    x: 586343.83056396851
+    y: 4140691.0208412712
+    theta: 1.6470176409606718
+    kappa: -0.020915582430456641
+    s: 0.95877531537987493
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4522136412868356
-  a: 0.54118262250698113
+  v: 4.422202885575425
+  a: 0.30202493424165344
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586343.823725549
-    y: 4140691.1121626482
-    theta: 1.6457510598587866
-    kappa: -0.02291332633181873
-    s: 1.0503523749929762
+    x: 586343.82395534823
+    y: 4140691.109093871
+    theta: 1.6457936221375473
+    kappa: -0.022846193843610398
+    s: 1.0472750056312485
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4624883175849854
-  a: 0.48647236943066929
+  v: 4.4275288848694023
+  a: 0.23084823156473266
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 586343.81705392036
-    y: 4140691.201256671
-    theta: 1.6445153785236304
-    kappa: -0.024862344699813466
-    s: 1.1396958444693135
+    x: 586343.81733982847
+    y: 4140691.1974386061
+    theta: 1.6445683326220877
+    kappa: -0.024778820814389521
+    s: 1.1358670895276159
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4716799903528157
-  a: 0.4328802596673047
+  v: 4.431447685311583
+  a: 0.16130209992312583
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586343.810369359
-    y: 4140691.2905233996
-    theta: 1.6432773131126517
-    kappa: -0.026815141167004144
-    s: 1.229212502919
+    x: 586343.81071948679
+    y: 4140691.2858477356
+    theta: 1.643342160872572
+    kappa: -0.026712856473314898
+    s: 1.2245237480264928
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4798109018681247
-  a: 0.38039423437099329
+  v: 4.4339917213743405
+  a: 0.093368844483460611
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586343.80367343267
-    y: 4140691.379941897
-    theta: 1.6420371578618045
-    kappa: -0.028771257733778539
-    s: 1.3188813555299559
+    x: 586343.80409635219
+    y: 4140691.3742941613
+    theta: 1.6421154864269163
+    kappa: -0.028647708024797155
+    s: 1.3132178072358751
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4869030532317966
-  a: 0.32900223469584144
+  v: 4.4351930736333784
+  a: 0.02703077041236468
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 586343.79696767661
-    y: 4140691.4694916643
-    theta: 1.6407952009793576
-    kappa: -0.030730245946945831
-    s: 1.4086818451003493
+    x: 586343.79747240618
+    y: 4140691.4627514216
+    theta: 1.6408886799896578
+    kappa: -0.030582796592732279
+    s: 1.4019227313363072
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4929782043677937
-  a: 0.27869220179595511
+  v: 4.4350834687677363
+  a: -0.037729817123534959
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 586343.79025359335
-    y: 4140691.559152632
-    theta: 1.6395517247042821
-    kappa: -0.032691666794511666
-    s: 1.4985938472150586
+    x: 586343.79084958334
+    y: 4140691.5511936839
+    theta: 1.6396621035193166
+    kappa: -0.032517557066097234
+    s: 1.4906126155029467
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4980578740231634
-  a: 0.22945207682544022
+  v: 4.4336942795597825
+  a: -0.10093061295761041
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 586343.78353265382
-    y: 4140691.64890516
-    theta: 1.6383070053648465
-    kappa: -0.034655090600453173
-    s: 1.5885976654221341
+    x: 586343.78422977135
+    y: 4140691.6395957381
+    theta: 1.638436110316152
+    kappa: -0.034451437944545596
+    s: 1.5792621788276315
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5021633397680345
-  a: 0.18126980093840328
+  v: 4.4310565248952214
+  a: -0.16258931192323445
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586343.77680629736
-    y: 4140691.7387300283
-    theta: 1.6370613134374274
-    kappa: -0.036620096919493979
-    s: 1.6786740264092597
+    x: 586343.77761481213
+    y: 4140691.7279329887
+    theta: 1.6372110451103272
+    kappa: -0.036383901184003205
+    s: 1.6678467572409488
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5053156379956167
-  a: 0.13413331528894967
+  v: 4.4272008697630874
+  a: -0.2227236088537794
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586343.77136022039
-    y: 4140691.8286637692
-    theta: 1.6350115208964631
-    kappa: -0.038304005017724026
-    s: 1.7688040751802139
+    x: 586343.77150197187
+    y: 4140691.8162027961
+    theta: 1.6356773141661671
+    kappa: -0.038205524371790048
+    s: 1.7563422964342976
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5075355639222039
-  a: 0.088030561031186227
+  v: 4.422157625255748
+  a: -0.28135119858261781
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586343.77033459977
-    y: 4140691.9188232305
-    theta: 1.6301930584089686
-    kappa: -0.0390165466763834
-    s: 1.8589693702313321
+    x: 586343.770496624
+    y: 4140691.904580127
+    theta: 1.630954401133222
+    kappa: -0.038903981620017437
+    s: 1.8447253447819592
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.50884367158717
-  a: 0.042949479319218442
+  v: 4.4159567485689051
+  a: -0.33848977594312224
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 586343.76930878335
-    y: 4140692.0089999046
-    theta: 1.625371695503248
-    kappa: -0.039729224366284205
-    s: 1.949151878727968
+    x: 586343.76949281583
+    y: 4140691.9928221186
+    theta: 1.6262367886842835
+    kappa: -0.039601369273999425
+    s: 1.9329730462631611
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5092602738529717
-  a: -0.0011219886928472977
+  v: 4.4086278430015895
+  a: -0.39415703576866545
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586343.76828297158
-    y: 4140692.0991761633
-    theta: 1.6205485971527525
-    kappa: -0.040441898772303336
-    s: 2.0393339716809553
+    x: 586343.7684908004
+    y: 4140692.0809065066
+    theta: 1.6215258851699583
+    kappa: -0.040297511362294591
+    s: 2.0210631333841462
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.50880544240515
-  a: -0.044195901850905051
+  v: 4.4002001579561689
+  a: -0.44837067289261956
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586343.76725736051
-    y: 4140692.1893347772
-    theta: 1.6157249088383163
-    kappa: -0.041154433732634652
-    s: 2.129498419123069
+    x: 586343.76749082457
+    y: 4140692.1688116062
+    theta: 1.6168230677716535
+    kappa: -0.040992236508430022
+    s: 2.1089739201002358
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5074990077523234
-  a: -0.086284319000848875
+  v: 4.39070258893834
+  a: -0.50114838214835733
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586343.76623214176
-    y: 4140692.2794589126
-    theta: 1.6109017562804104
-    kappa: -0.041866696200670427
-    s: 2.2196283852854877
+    x: 586343.76649312838
+    y: 4140692.2565163067
+    theta: 1.6121296822554327
+    kappa: -0.041685377874967171
+    s: 2.1966842947379006
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5053605592261965
-  a: -0.12739929898857194
+  v: 4.3801636775571335
+  a: -0.55250785836925131
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586343.76520750229
-    y: 4140692.3695321232
-    theta: 1.6060802451847802
-    kappa: -0.042578556206882778
-    s: 2.3097074237742548
+    x: 586343.76549794548
+    y: 4140692.3440000638
+    theta: 1.6074470427520207
+    kappa: -0.042376773107567593
+    s: 2.2841737129168243
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5024094449815557
-  a: -0.16755290065996942
+  v: 4.368611611524913
+  a: -0.60246679638867384
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586343.76418362476
-    y: 4140692.459538349
-    theta: 1.6012614610022873
-    kappa: -0.043289886820705108
-    s: 2.3997194727467406
+    x: 586343.76450550335
+    y: 4140692.4312428972
+    theta: 1.6027764315637911
+    kappa: -0.043066264279058852
+    s: 2.3714221904719723
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4986647719962676
-  a: -0.20675718286093403
+  v: 4.356074224657374
+  a: -0.65104289103999746
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586343.76316068764
-    y: 4140692.5494619077
-    theta: 1.596446468703705
-    kappa: -0.044000564112413529
-    s: 2.4896488500881024
+    x: 586343.76351602282
+    y: 4140692.5182253756
+    theta: 1.5981190989994385
+    kappa: -0.043753697833500249
+    s: 2.4584102963756549
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.494145406071282
-  a: -0.24502420443736028
+  v: 4.3425789968735442
+  a: -0.698253837156595
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586343.762138865
-    y: 4140692.6392874951
-    theta: 1.5916363125701518
-    kappa: -0.044710467115008257
-    s: 2.5794802485877488
+    x: 586343.76252971892
+    y: 4140692.6049286146
+    theta: 1.5934762632369091
+    kappa: -0.04443892453024869
+    s: 2.5451191456595983
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.48886997183063
-  a: -0.28236602423514245
+  v: 4.328153054195786
+  a: -0.74411732957183863
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 586343.7611183268
-    y: 4140692.7290001726
-    theta: 1.5868320159997769
-    kappa: -0.045419477786095133
-    s: 2.6691987311157992
+    x: 586343.76154680015
+    y: 4140692.6913342709
+    theta: 1.5888491102150406
+    kappa: -0.0451217993880245
+    s: 2.6315303923370084
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4828568527214276
-  a: -0.31879470110017394
+  v: 4.3128231687497909
+  a: -0.788651063119101
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 586343.76009923872
-    y: 4140692.8185853711
-    theta: 1.5820345813312418
-    kappa: -0.046127480969766939
-    s: 2.7587897257995455
+    x: 586343.76056746917
+    y: 4140692.7774245311
+    theta: 1.5842387935542384
+    kappa: -0.0458021816289772
+    s: 2.7176262223246392
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4761241910138692
-  a: -0.35432229387834857
+  v: 4.2966157587645855
+  a: -0.83187273263175476
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 586343.75908176252
-    y: 4140692.9080288792
-    theta: 1.5772449896844833
-    kappa: -0.046834364358484933
-    s: 2.8482390211999156
+    x: 586343.75959192275
+    y: 4140692.8631821061
+    theta: 1.5796464345064014
+    kappa: -0.046479934622751365
+    s: 2.8033893463648569
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4686898878012338
-  a: -0.38896086141556063
+  v: 4.2795568885725288
+  a: -0.87380003294317254
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586343.7580660556
-    y: 4140692.9973168429
-    theta: 1.5724642008191634
-    kappa: -0.0475400184549602
-    s: 2.9375327614879319
+    x: 586343.75862035155
+    y: 4140692.9485902274
+    theta: 1.5750731219341978
+    kappa: -0.047154925830552412
+    s: 2.8888029929477095
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.460571602999881
-  a: -0.42272246255770396
+  v: 4.2616722686093107
+  a: -0.91445065888672672
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 586343.75705227186
-    y: 4140693.0864357566
-    theta: 1.5676931530111553
-    kappa: -0.048244336534035134
-    s: 3.0266574416211767
+    x: 586343.75765294058
+    y: 4140693.0336326328
+    theta: 1.570519912319678
+    kappa: -0.047827026749212395
+    s: 2.9738509012329919
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4517867553492536
-  a: -0.45561915615067239
+  v: 4.242987255413956
+  a: -0.95384230529578917
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586343.75604056078
-    y: 4140693.1753724637
-    theta: 1.5629327629473511
-    kappa: -0.048947214604564859
-    s: 3.1155999025202519
+    x: 586343.7566898691
+    y: 4140693.118293568
+    theta: 1.5659878298021161
+    kappa: -0.048496112855255888
+    s: 3.0585173139723132
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4423525224118761
-  a: -0.48766300104036
+  v: 4.2235268516288205
+  a: -0.99199266700373312
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586343.75503106823
-    y: 4140693.2641141461
-    theta: 1.5581839256390047
-    kappa: -0.049648551371298644
-    s: 3.2043473262452395
+    x: 586343.7557313107
+    y: 4140693.2025577724
+    theta: 1.5614778662448676
+    kappa: -0.049162063548965712
+    s: 3.1427869704311613
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4322858405733552
-  a: -0.51886605607266056
+  v: 4.203315705999592
+  a: -1.0289194388439313
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 586343.75402393623
-    y: 4140693.3526483225
-    theta: 1.553447514353772
-    kappa: -0.050348248196761342
-    s: 3.2928872311721649
+    x: 586343.75477743347
+    y: 4140693.2864104765
+    theta: 1.5569909813309404
+    kappa: -0.049824762098448827
+    s: 3.2266450993109737
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4216034050423794
-  a: -0.549240380093468
+  v: 4.1823781133752922
+  a: -1.0646403156497557
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 586343.75301930285
-    y: 4140693.4409628445
-    theta: 1.5487243805665447
-    kappa: -0.051046209063134812
-    s: 3.3812074671694581
+    x: 586343.7538283997
+    y: 4140693.3698373912
+    theta: 1.5525281026868898
+    kappa: -0.050484095583702086
+    s: 3.3100774116711995
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.41032166985072
-  a: -0.57879803194867629
+  v: 4.1607380147082749
+  a: -1.0991729922545785
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 586343.75201730267
-    y: 4140693.5290458892
-    theta: 1.5440153539291175
-    kappa: -0.05174234053413939
-    s: 3.4692962107744161
+    x: 586343.75288436678
+    y: 4140693.4528247039
+    theta: 1.5480901260345641
+    kappa: -0.051139954840678077
+    s: 3.3930700938513678
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3984568478532307
-  a: -0.60755107048417911
+  v: 4.1384189970542264
+  a: -1.1325351634917735
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 586343.75101806666
-    y: 4140693.6168859554
-    theta: 1.5393212422586748
-    kappa: -0.052436551716915282
-    s: 3.5571419603696635
+    x: 586343.75194548641
+    y: 4140693.53535907
+    theta: 1.5436779153701523
+    kappa: -0.051792234405350961
+    s: 3.4756098003931584
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3860249107278451
-  a: -0.63551155454587116
+  v: 4.1154442935721658
+  a: -1.1647445241947121
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 586343.75801888457
-    y: 4140693.7041788888
-    theta: 1.5349617184276183
-    kappa: -0.052415287123365767
-    s: 3.6447335313596128
+    x: 586343.751011905
+    y: 4140693.6174276071
+    theta: 1.5392923031699204
+    kappa: -0.052440832457782222
+    s: 3.5576836469624609
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3730415889755818
-  a: -0.66269154297964539
+  v: 4.0918367835244442
+  a: -1.1958187691967683
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 586343.76539886312
-    y: 4140693.7911930084
-    theta: 1.5306298106003144
-    kappa: -0.052358370085070889
-    s: 3.7320600513469295
+    x: 586343.75755793834
+    y: 4140693.698744073
+    theta: 1.5352322326744856
+    kappa: -0.052418842105296938
+    s: 3.6392792032714465
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3595223719205389
-  a: -0.68910309463139674
+  v: 4.0676189922767447
+  a: -1.2257755933313124
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 586343.77275554917
-    y: 4140693.8779324982
-    theta: 1.5263101127345491
-    kappa: -0.05230163268576616
-    s: 3.8191109553089917
+    x: 586343.7644121591
+    y: 4140693.7795592109
+    theta: 1.53120907576988
+    kappa: -0.052365979899363563
+    s: 3.720384486000635
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3454825077098995
-  a: -0.71475826834701794
+  v: 4.0428130912980853
+  a: -1.2546326914317183
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 586343.78008807567
-    y: 4140693.9643871314
-    theta: 1.5220033079538959
-    kappa: -0.052245081614249907
-    s: 3.9058759807743519
+    x: 586343.77122397115
+    y: 4140693.8598743272
+    theta: 1.5272095368912029
+    kappa: -0.052313444764037327
+    s: 3.8009879517209568
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3309370033139265
-  a: -0.7396691229724035
+  v: 4.0174408981608147
+  a: -1.2824077583313584
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 586343.78739560046
-    y: 4140694.0505469805
-    theta: 1.5177100623007913
-    kappa: -0.052188723365269724
-    s: 3.9923451629991979
+    x: 586343.77799243573
+    y: 4140693.9396783509
+    theta: 1.5232343175195997
+    kappa: -0.052261243940914345
+    s: 3.8810784898158248
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3159006245259661
-  a: -0.7638477173534477
+  v: 3.9915238765406138
+  a: -1.3091184888636047
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 586343.794677306
-    y: 4140694.1364024077
-    theta: 1.5134310247504261
-    kappa: -0.05213256424266631
-    s: 4.0785088301438153
+    x: 586343.78471664956
+    y: 4140694.0189606352
+    theta: 1.5192840938660759
+    kappa: -0.052209384394108632
+    s: 3.9606454154031989
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3003878959624453
-  a: -0.78730611033604392
+  v: 3.9650831362164971
+  a: -1.3347825778618303
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 586343.80193239951
-    y: 4140694.2219440616
-    theta: 1.5091668272379071
-    kappa: -0.052076610362517338
-    s: 4.16435759844905
+    x: 586343.79139574524
+    y: 4140694.0977109503
+    theta: 1.5153595170604521
+    kappa: -0.052157872814865246
+    s: 4.039678462257652
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2844131010628743
-  a: -0.81005636076608667
+  v: 3.9381394330708113
+  a: -1.359417720159408
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 586343.8091601117
-    y: 4140694.3071628753
-    theta: 1.5049180846986014
-    kappa: -0.052020867656281292
-    s: 4.2498823674127681
+    x: 586343.79802888993
+    y: 4140694.1759194769
+    theta: 1.5114612133588967
+    kappa: -0.052106715626173514
+    s: 4.1181677757324362
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2679902820898459
-  a: -0.83211052748946956
+  v: 3.9107131690892363
+  a: -1.3830416105897096
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 586343.81635969714
-    y: 4140694.392050059
-    theta: 1.5006853951215431
-    kappa: -0.051965341873941348
-    s: 4.33507431496632
+    x: 586343.80461528513
+    y: 4140694.2535767998
+    theta: 1.5075897843695096
+    kappa: -0.052055918987380254
+    s: 4.1961039056815528
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2511332401290343
-  a: -0.853480669352086
+  v: 3.8828243923607824
+  a: -1.4056719439861081
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 586343.82353043335
-    y: 4140694.476597094
-    theta: 1.4964693396157609
-    kappa: -0.051910038587149168
-    s: 4.4199248926509975
+    x: 586343.81115416554
+    y: 4140694.3306738976
+    theta: 1.5037458072954173
+    kappa: -0.052005488798802929
+    s: 4.2734777993818138
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2338555350891953
-  a: -0.87417884519983025
+  v: 3.8544927970777954
+  a: -1.4273264151819756
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586343.85892765818
-    y: 4140694.89395039
-    theta: 1.4756577413807732
-    kappa: -0.051637042507134988
-    s: 4.8387765819168207
+    x: 586343.84311134648
+    y: 4140694.7074671322
+    theta: 1.4849562285271345
+    kappa: -0.051759023569478119
+    s: 4.6516238066161328
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1416147781268151
-  a: -0.96801229382176057
+  v: 3.7068570663101834
+  a: -1.52158015732139
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586343.89350786991
-    y: 4140695.3016706402
-    theta: 1.4553396284290396
-    kappa: -0.051370347524703076
-    s: 5.2479606401806658
+    x: 586343.87378416874
+    y: 4140695.0691170515
+    theta: 1.4669259944425566
+    kappa: -0.051522463774418083
+    s: 5.0145721308855773
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0407527537744334
-  a: -1.0468554189559782
+  v: 3.5509015099470966
+  a: -1.5940915694534137
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586343.94356000726
-    y: 4140695.6967753842
-    theta: 1.435639809654617
-    kappa: -0.050536430596866092
-    s: 5.64668738006141
+    x: 586343.903111197
+    y: 4140695.4148992989
+    theta: 1.4497016341327102
+    kappa: -0.05129628322453815
+    s: 5.3615958148570169
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9326931265939846
-  a: -1.1122155763392247
+  v: 3.3886897682806945
+  a: -1.6470725057496123
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 586344.01382184518
-    y: 4140696.0779686738
-    theta: 1.4165734255658553
-    kappa: -0.0489674937509009
-    s: 6.03430194385544
+    x: 586343.95180328644
+    y: 4140695.7414978514
+    theta: 1.4334041263840613
+    kappa: -0.050352359342814526
+    s: 5.69216320595568
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.8187088255737294
-  a: -1.1656001217082395
+  v: 3.2220642961859824
+  a: -1.6827348203815513
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586344.08197241265
-    y: 4140696.4477076363
-    theta: 1.3980654620991186
-    kappa: -0.047445701271527807
-    s: 6.4102692299792867
+    x: 586344.00867637282
+    y: 4140696.0500528156
+    theta: 1.4179703890830877
+    kappa: -0.049082391414268227
+    s: 6.0059158378234407
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.699922044128253
-  a: -1.2085164107997635
+  v: 3.0526463631208074
+  a: -1.7032903675207953
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586344.147933689
-    y: 4140696.8055689828
-    theta: 1.3801505187841734
-    kappa: -0.045972795354019762
-    s: 6.7741588194122526
+    x: 586344.06246389891
+    y: 4140696.3418676131
+    theta: 1.4033641853462333
+    kappa: -0.047881323607760359
+    s: 6.3026463117770923
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.577304240098468
-  a: -1.2424717993505392
+  v: 2.8818360531258609
+  a: -1.7109510013389111
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 586344.2116438963
-    y: 4140697.1512175491
-    theta: 1.3628564352397516
-    kappa: -0.044550155485734592
-    s: 7.12562990213905
+    x: 586344.11315164447
+    y: 4140696.6168650948
+    theta: 1.3895967175042025
+    kappa: -0.04674947346138425
+    s: 6.5822761782666461
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.4516761357516121
-  a: -1.2689736430973051
+  v: 2.7108122648246775
+  a: -1.7079285760074636
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586344.275843549
-    y: 4140697.4837549315
-    theta: 1.3465509654969059
-    kappa: -0.04309217321399262
-    s: 7.4644162035924282
+    x: 586344.1607447539
+    y: 4140696.8750731703
+    theta: 1.3766718754567202
+    kappa: -0.045686726097976062
+    s: 6.8448338183336066
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.3237077177812488
-  a: -1.2895292977768031
+  v: 2.540532711423634
+  a: -1.6964349456980174
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586344.36168174539
-    y: 4140697.7981418977
-    theta: 1.3338572105730504
-    kappa: -0.040941123526512559
-    s: 7.7903109110958173
+    x: 586344.20526372723
+    y: 4140697.1166030597
+    theta: 1.3645876069594627
+    kappa: -0.0446926237537299
+    s: 7.09043232506926
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.193918237307269
-  a: -1.3056461191257727
+  v: 2.3717339207119537
+  a: -1.6786819645821394
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586344.44408161216
-    y: 4140698.0999357989
-    theta: 1.3216644487022151
-    kappa: -0.038876236152576044
-    s: 8.10315160030594
+    x: 586344.24674041092
+    y: 4140697.3416275391
+    theta: 1.3533372958635295
+    kappa: -0.04376645530681289
+    s: 7.3192473850729547
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.062676209875888
-  a: -1.3188314628809552
+  v: 2.2049312350617023
+  a: -1.656881486831393
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586344.52300848928
-    y: 4140698.3890096908
-    theta: 1.3099821232478186
-    kappa: -0.036898379654044924
-    s: 8.4028066797883643
+    x: 586344.29351262539
+    y: 4140697.5484688645
+    theta: 1.3439389059630664
+    kappa: -0.042649397588225155
+    s: 7.5314989092542213
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.9302612858208286
-  a: -1.3286793216720583
+  v: 2.0405698543029129
+  a: -1.6286606242751676
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 586344.59843674849
-    y: 4140698.6652697073
-    theta: 1.2988174551936462
-    kappa: -0.035008196257716756
-    s: 8.6891788489946329
+    x: 586344.3451304175
+    y: 4140697.7375217783
+    theta: 1.3363055613268451
+    kappa: -0.041355889098381858
+    s: 7.7274718676680614
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.7970956631641277
-  a: -1.3338676517836088
+  v: 1.8795207292196676
+  a: -1.5907911913289718
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586344.67035269481
-    y: 4140698.928665692
-    theta: 1.2881750886024168
-    kappa: -0.033206029149608469
-    s: 8.9622161135392826
+    x: 586344.39256010007
+    y: 4140697.9112355169
+    theta: 1.3292887772183657
+    kappa: -0.040167331925041273
+    s: 7.9075441805538951
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.6636386297127927
-  a: -1.3345297544345771
+  v: 1.7227022714484257
+  a: -1.544202096043555
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586344.73875455523
-    y: 4140699.1791911484
-    theta: 1.2780571747393379
-    kappa: -0.031491922764639163
-    s: 9.2219117413114535
+    x: 586344.43592418148
+    y: 4140698.0700587742
+    theta: 1.3228717307507225
+    kappa: -0.039080656090510146
+    s: 8.0721809647745317
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.5303361431519327
-  a: -1.3307989308439336
+  v: 1.5709400018205719
+  a: -1.4898222464696658
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 586344.80729176663
-    y: 4140699.4157709563
-    theta: 1.2695072355357149
-    kappa: -0.029772964077967996
-    s: 9.4683029294627055
+    x: 586344.47536569007
+    y: 4140698.2145154011
+    theta: 1.3170341727570929
+    kappa: -0.038092277392740147
+    s: 8.2219252447356723
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.3976208310447604
-  a: -1.3228084822306492
+  v: 1.4249665503624165
+  a: -1.4285805506580538
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 586344.881964478
-    y: 4140699.63665695
-    theta: 1.26423223515631
-    kappa: -0.027896393975255097
-    s: 9.70146947139482
+    x: 586344.51104572788
+    y: 4140698.3451954396
+    theta: 1.3117528745497713
+    kappa: -0.037198158717381379
+    s: 8.357388663305386
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.2659119908325915
-  a: -1.3106917098136939
+  v: 1.2854216562951934
+  a: -1.3614059166594679
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 586344.95244070119
-    y: 4140699.8451295029
-    theta: 1.2592528705523776
-    kappa: -0.026125284133860154
-    s: 9.9215324237476175
+    x: 586344.54314102419
+    y: 4140698.4627461722
+    theta: 1.3070020637777358
+    kappa: -0.036393871349835787
+    s: 8.4792421927336186
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.1356155898348446
-  a: -1.2945819148120385
+  v: 1.1528521680350634
+  a: -1.2892272525246571
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 586345.01877199358
-    y: 4140700.0413411204
-    theta: 1.2545658643904625
-    kappa: -0.024458338899884725
-    s: 10.128652773386767
+    x: 586344.57184148789
+    y: 4140698.5678631496
+    theta: 1.3027538487855128
+    kappa: -0.035674656287310605
+    s: 8.5882068455716762
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.0071242652490411
-  a: -1.2746123984446536
+  v: 1.027712043193111
+  a: -1.21297346630437
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 586345.08102227258
-    y: 4140700.225480875
-    theta: 1.2501669766406265
-    kappa: -0.02289395198147778
-    s: 10.323030104391592
+    x: 586344.5973477622
+    y: 4140698.6612812374
+    theta: 1.2989786316340592
+    kappa: -0.035035485550871784
+    s: 8.68504438559172
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8808173241508057
-  a: -1.2509164619305093
+  v: 0.9103623485753467
+  a: -1.1335734660493566
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 586345.13926739013
-    y: 4140700.3977731429
-    theta: 1.2460510483388716
-    kappa: -0.021430217177179488
-    s: 10.504901265042886
+    x: 586344.61986877653
+    y: 4140698.7437656512
+    theta: 1.2956455104983773
+    kappa: -0.0344711234974975
+    s: 8.7705480387062558
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7570607434938654
-  a: -1.2236274064885766
+  v: 0.80107126018270547
+  a: -1.0519561598103653
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 586345.19359470531
-    y: 4140700.5584763344
-    theta: 1.2422120444963913
-    kappa: -0.020064939104265034
-    s: 10.674539034810715
+    x: 586344.63961930072
+    y: 4140698.8161029983
+    theta: 1.2927226725265009
+    kappa: -0.033976188132131477
+    s: 8.8455332038876335
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6362071701100507
-  a: -1.1928785333378258
+  v: 0.70001406321104764
+  a: -0.96905045563814529
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 586345.24410265707
-    y: 4140700.7078816388
-    theta: 1.2386430959977719
-    kappa: -0.018795643927088334
-    s: 10.832250791342243
+    x: 586344.65681749838
+    y: 4140698.8790923138
+    theta: 1.2901777784559645
+    kappa: -0.033545212419736549
+    s: 8.91082816408753
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5185959207092949
-  a: -1.1588031436972273
+  v: 0.607273152051159
+  a: -0.88578526158344573
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586345.29090033786
-    y: 4140700.8463117578
-    theta: 1.2353365403808754
-    kappa: -0.017619590085425903
-    s: 10.978377177449525
+    x: 586344.67168247991
+    y: 4140698.9335361
+    theta: 1.2879783403652167
+    kappa: -0.033172705597348
+    s: 8.967264797156453
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4045529818796343
-  a: -1.1215345387857516
+  v: 0.52283803028874942
+  a: -0.80308948569701588
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 586345.33410706685
-    y: 4140700.9741196427
-    theta: 1.2322839614367287
-    kappa: -0.016533779022820622
-    s: 11.113290768097325
+    x: 586344.68443185568
+    y: 4140698.980231368
+    theta: 1.2860920939174671
+    kappa: -0.032853214486127075
+    s: 9.0156692867632238
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.2943910100872078
-  a: -1.0812060198223696
+  v: 0.44660531070445497
+  a: -0.72189203602960439
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 586345.37385196239
-    y: 4140701.0916872341
-    theta: 1.2294762276079028
-    kappa: -0.015534965914925489
-    s: 11.237394737390929
+    x: 586344.69527928974
+    y: 4140699.0199606731
+    theta: 1.2844873663606675
+    kappa: -0.032581384803414409
+    s: 9.0568528333144727
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1884093316762581
-  a: -1.0379508880260513
+  v: 0.37837871527383626
+  a: -0.64312182063196022
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586345.41027351574
-    y: 4140701.1994241951
-    theta: 1.2269035291984312
-    kappa: -0.014619670397847437
-    s: 11.351121525563951
+    x: 586344.704432053
+    y: 4140699.053483156
+    theta: 1.283133441404674
+    kappa: -0.032352022474783429
+    s: 9.0916023648741344
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0868939428691298
-  a: -0.99190244461576749
+  v: 0.31786907516737894
+  a: -0.56770774755483289
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 586345.44517104025
-    y: 4140701.2971818452
-    theta: 1.2255322062417096
-    kappa: -0.013800310603679328
-    s: 11.454931505966144
+    x: 586344.71208857652
+    y: 4140699.081525581
+    theta: 1.28200092192681
+    kappa: -0.032160154946093839
+    s: 9.1206712480829388
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.99011750976627066
-  a: -0.94319399081048871
+  v: 0.26469433075049387
+  a: -0.49657872484897125
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 586345.47793376283
-    y: 4140701.3856929522
-    theta: 1.224897630030342
-    kappa: -0.013065483540738868
-    s: 11.549311652051212
+    x: 586344.718436005
+    y: 4140699.1047733757
+    theta: 1.2810620912779223
+    kappa: -0.032001092495545023
+    s: 9.1447699990779032
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.89833936834623263
-  a: -0.89195882782918534
+  v: 0.21837953158351731
+  a: -0.43066366056512462
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586345.50760086975
-    y: 4140701.4658410065
-    theta: 1.2243230083739973
-    kappa: -0.012400087279842511
-    s: 11.63477420436462
+    x: 586344.72364774952
+    y: 4140699.1238616682
+    theta: 1.2802912737866392
+    kappa: -0.031870489545729547
+    s: 9.1645569944118215
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.81180552446566878
-  a: -0.838330256890828
+  v: 0.17835683642171007
+  a: -0.37089146275404183
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 586345.53435848642
-    y: 4140701.5381288407
-    theta: 1.2238047376831858
-    kappa: -0.011799947260542109
-    s: 11.7118553375314
+    x: 586344.72788104194
+    y: 4140699.1393663273
+    theta: 1.2796651949000977
+    kappa: -0.031764405975686541
+    s: 9.1806291819727619
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.73074865385933641
-  a: -0.78244157921438762
+  v: 0.14396551321525888
+  a: -0.31819103946647215
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586345.55839994666
-    y: 4140701.6030787621
-    theta: 1.2233390743663197
-    kappa: -0.011260727248226162
-    s: 11.781111991376898
+    x: 586344.73127489013
+    y: 4140699.1517964769
+    theta: 1.2791632816730321
+    kappa: -0.03167935833568674
+    s: 9.1935143216898538
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.65531480251623575
-  a: -0.72661833178221424
+  v: 0.11451406886515009
+  a: -0.27158049086854064
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 586345.57991839456
-    y: 4140701.6612125644
-    theta: 1.2229222782060123
-    kappa: -0.010778095261329267
-    s: 11.843100559471248
+    x: 586344.73395245941
+    y: 4140699.161603217
+    theta: 1.2787673090413774
+    kappa: -0.031612260180175586
+    s: 9.2036800253886568
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.58534889016995739
-  a: -0.67307800433814324
+  v: 0.089504170657476678
+  a: -0.22932727379283774
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 586345.599099696
-    y: 4140701.7130323797
-    theta: 1.2225507497285655
-    kappa: -0.010347882558339284
-    s: 11.898356469063593
+    x: 586344.736025085
+    y: 4140699.1691943184
+    theta: 1.2784608051862734
+    kappa: -0.031560321525438487
+    s: 9.2115489894540552
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.520624063303631
-  a: -0.62179182724337156
+  v: 0.068509938287493549
+  a: -0.1912343312898096
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586345.616121892
-    y: 4140701.7590191988
-    theta: 1.2222210408514829
-    kappa: -0.00996609590188871
-    s: 11.94739260589958
+    x: 586344.73759314383
+    y: 4140699.1749374173
+    theta: 1.2782289214807805
+    kappa: -0.031521026984344183
+    s: 9.2175023075252014
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.46091634536426696
-  a: -0.57273103085909627
+  v: 0.051125197145410875
+  a: -0.15710460640990243
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586345.63115529856
-    y: 4140701.7996331453
-    theta: 1.2219298529926996
-    kappa: -0.0096289153188016553
-    s: 11.990699601917749
+    x: 586344.7387465745
+    y: 4140699.1791619183
+    theta: 1.2780583549227924
+    kappa: -0.031492122759711541
+    s: 9.2218814410650225
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.40600463676275572
-  a: -0.52586684554651475
+  v: 0.036963478316394287
+  a: -0.12674104220356236
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586345.644362606
-    y: 4140701.8353137388
-    theta: 1.2216740351696145
-    kappa: -0.0093326918601407943
-    s: 12.028746122945924
+    x: 586344.73956539528
+    y: 4140699.1821608935
+    theta: 1.2779372707538388
+    kappa: -0.03147160363767653
+    s: 9.224990189929704
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.35567071487386803
-  a: -0.48117050166682357
+  v: 0.025658018580564743
+  a: -0.09994658172123555
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586345.65589897987
-    y: 4140701.8664801726
-    theta: 1.2214505820901069
-    kappa: -0.009073945361254377
-    s: 12.061979156397603
+    x: 586344.74012022431
+    y: 4140699.1841929844
+    theta: 1.2778552252383995
+    kappa: -0.031457699981059
+    s: 9.2270966629381945
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.3096992340362551
-  a: -0.43861322958122018
+  v: 0.0168617604129986
+  a: -0.076524168013368157
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 586345.66591216
-    y: 4140701.8935315767
-    theta: 1.2212566322373066
-    kappa: -0.00884936220182327
-    s: 12.090824298968341
+    x: 586344.74047279754
+    y: 4140699.1854843032
+    theta: 1.2778030885757996
+    kappa: -0.031448864722729605
+    s: 9.2284352484416967
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.26787772555244815
-  a: -0.39816625965090163
+  v: 0.010247351983727646
+  a: -0.056276744130406446
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 586345.67454256047
-    y: 4140701.9168472923
-    theta: 1.2210894659495266
-    kappa: -0.0086557930659078922
-    s: 12.11568604433214
+    x: 586344.74067648861
+    y: 4140699.1862303321
+    theta: 1.27777296791732
+    kappa: -0.031443760358976676
+    s: 9.2292085848931666
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.22999659768885902
-  a: -0.35980082223706489
+  v: 0.0055071471577389907
+  a: -0.039007253122796466
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586345.68192337
-    y: 4140701.9367871368
-    theta: 1.2209465034964919
-    kappa: -0.00849025070199524
-    s: 12.136948070837837
+    x: 586344.740776827
+    y: 4140699.1865978264
+    theta: 1.2777581304635954
+    kappa: -0.031441245942873106
+    s: 9.2295895314168028
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.19584913567577952
-  a: -0.32348814770090728
+  v: 0.0023532054949751175
+  a: -0.024518638040984402
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 586345.68818065093
-    y: 4140701.95369168
-    theta: 1.2208253031527543
-    kappa: -0.0083499076830458735
-    s: 12.154973529205494
+    x: 586344.740812018
+    y: 4140699.1867267159
+    theta: 1.2777529266207557
+    kappa: -0.03144036407764321
+    s: 9.2297231383775475
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.16523150170738196
-  a: -0.2891994664036257
+  v: 0.00051729225033400215
+  a: -0.012613841935416495
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 586345.69343344
-    y: 4140701.9678825089
-    theta: 1.2207235592689889
-    kappa: -0.0082320941665409465
-    s: 12.170105330222778
+    x: 586344.74081346125
+    y: 4140699.1867320016
+    theta: 1.2777527131975039
+    kappa: -0.03144032791002957
+    s: 9.2297286179505846
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.13794273494171885
-  a: -0.25690600870641728
+  v: 0
+  a: -0.0030958078565388246
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 586345.69779384765
-    y: 4140701.9796624975
-    theta: 1.2206391003417119
-    kappa: -0.0081342956545291233
-    s: 12.182666432441364
+    x: 586344.74080626993
+    y: 4140699.1867056629
+    theta: 1.2777537766290159
+    kappa: -0.031440508123659991
+    s: 9.2297013146908249
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.11378475150072304
-  a: -0.22657900497047914
+  v: 0
+  a: 0.00423252114520236
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 586345.701367158
-    y: 4140701.9893160798
-    theta: 1.2205698870818338
-    kappa: -0.00805415075367362
-    s: 12.192960129873311
+    x: 586344.74080978916
+    y: 4140699.1867185524
+    theta: 1.2777532562169298
+    kappa: -0.0314404199324143
+    s: 9.2297146761024109
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.09256234447020753
-  a: -0.19818968555700833
+  v: 0.00053096099845656322
+  a: 0.0095682020193608684
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 586345.70425192884
-    y: 4140701.9971095184
-    theta: 1.2205140104823635
-    kappa: -0.0079894489352992318
-    s: 12.201270339687452
+    x: 586344.74083811615
+    y: 4140699.1868223017
+    theta: 1.2777490673776768
+    kappa: -0.031439710073791295
+    s: 9.2298222232082079
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.074083183899865943
-  a: -0.17170928082720197
+  v: 0.0016789278743928836
+  a: 0.01310829171549073
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 586345.70654009143
-    y: 4140702.0032911724
-    theta: 1.2204696898854921
-    kappa: -0.0079381282954392833
-    s: 12.207861889905784
+    x: 586344.74090061872
+    y: 4140699.1870512203
+    theta: 1.2777398248938894
+    kappa: -0.031438143802275519
+    s: 9.2300595211193048
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.058157816803271915
-  a: -0.1471090211422571
+  v: 0.0030993348672721946
+  a: 0.015049847183145419
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 586345.70831705071
-    y: 4140702.0080917692
-    theta: 1.2204352710492357
-    kappa: -0.0078982733148826416
-    s: 12.212980807099854
+    x: 586344.74100245372
+    y: 4140699.1874241969
+    theta: 1.2777247661656517
+    kappa: -0.031435591882704263
+    s: 9.2304461496044983
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.044599667157879413
-  a: -0.12436013686337101
+  v: 0.0046421814017245638
+  a: 0.01558992537187919
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 586345.70966178412
-    y: 4140702.0117246741
-    theta: 1.2204092242137605
-    kappa: -0.0078681126192206748
-    s: 12.216854604087152
+    x: 586344.741145087
+    y: 4140699.187946599
+    theta: 1.2777036744599191
+    kappa: -0.031432017583634321
+    s: 9.2309876736598042
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.033225035905022661
-  a: -0.10343385835174067
+  v: 0.00617717259733583
+  a: 0.014925583231245571
   relative_time: 5.7999999999999963
 }
 trajectory_point {
   path_point {
-    x: 586345.71064694272
-    y: 4140702.0143861584
-    theta: 1.2203901421674856
-    kappa: -0.0078460167388943233
-    s: 12.21969256762749
+    x: 586344.74132681207
+    y: 4140699.1886121766
+    theta: 1.2776768021575942
+    kappa: -0.031427463670708949
+    s: 9.23167761407794
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.02385310094991621
-  a: -0.0843014159685629
+  v: 0.0075937192686465994
+  a: 0.013253877710798594
   relative_time: 5.8999999999999959
 }
 trajectory_point {
   path_point {
-    x: 586345.71133895032
-    y: 4140702.0162556721
-    theta: 1.2203767383130208
-    kappa: -0.0078304958692410175
-    s: 12.221686046119402
+    x: 586344.741543269
+    y: 4140699.1894049617
+    theta: 1.2776447939985416
+    kappa: -0.031422039400024714
+    s: 9.2324994180178237
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.016305917161655092
-  a: -0.0669340400750352
+  v: 0.00880093792515342
+  a: 0.01077186576009179
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 586345.71179810376
-    y: 4140702.0174961118
-    theta: 1.220367844732996
-    kappa: -0.0078201976305416945
-    s: 12.223008737296523
+    x: 586344.74178796355
+    y: 4140699.1903011682
+    theta: 1.2776086103253244
+    kappa: -0.031415907511498364
+    s: 9.2334284295740741
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.010408416373214352
-  a: -0.051302961032354426
+  v: 0.00972765077130799
+  a: 0.0076766043286792991
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 586345.71207867225
-    y: 4140702.01825409
-    theta: 1.2203624102558073
-    kappa: -0.0078139048280678573
-    s: 12.223816975923974
+    x: 586344.74205278652
+    y: 4140699.1912710965
+    theta: 1.2775694503266608
+    kappa: -0.031409271222233692
+    s: 9.2344338603465044
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0059884073814494895
-  a: -0.037379409201717846
+  v: 0.010322385706517594
+  a: 0.0041651503661148739
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
-    x: 586345.71222899784
-    y: 4140702.0186602063
-    theta: 1.2203594985213038
-    kappa: -0.0078105332121284421
-    s: 12.224250021494766
+    x: 586344.74232853192
+    y: 4140699.1922810287
+    theta: 1.2775286752816373
+    kappa: -0.031402361219888472
+    s: 9.235480760009608
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0028765759470960237
-  a: -0.025134614944322408
+  v: 0.010553376325144416
+  a: 0.00043456082195226858
   relative_time: 6.2999999999999945
 }
 trajectory_point {
   path_point {
-    x: 586345.712291595
-    y: 4140702.0188293178
-    theta: 1.2203582860464361
-    kappa: -0.0078091292381169642
-    s: 12.224430345926168
+    x: 586344.74260541715
+    y: 4140699.1932951352
+    theta: 1.2774877318055424
+    kappa: -0.031395422656041241
+    s: 9.2365319868820741
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00090648479477037781
-  a: -0.014539808621365176
+  v: 0.010408561916506584
+  a: -0.00331810735425471
   relative_time: 6.3999999999999941
 }
 trajectory_point {
   path_point {
-    x: 586345.7123032501
-    y: 4140702.0188608048
-    theta: 1.2203580602908717
-    kappa: -0.0078088678265583553
-    s: 12.224463921256111
+    x: 586344.742873601
+    y: 4140699.1942773731
+    theta: 1.27744807509794
+    kappa: -0.031388702139558265
+    s: 9.23755017849626
     dkappa: 0
     ddkappa: 0
   }
-  v: 0
-  a: -0.0055662205940433191
+  v: 0.00989558746487705
+  a: -0.0068957972129520314
   relative_time: 6.4999999999999938
 }
 trajectory_point {
   path_point {
-    x: 586345.71229512233
-    y: 4140702.0188388471
-    theta: 1.2203582177225918
-    kappa: -0.0078090501231561152
-    s: 12.224440507339565
+    x: 586344.74312370375
+    y: 4140699.1951933876
+    theta: 1.27741109219322
+    kappa: -0.031382434729960357
+    s: 9.2384997221677079
     dkappa: 0
     ddkappa: 0
   }
-  v: 0
-  a: 0.0018149187764459932
+  v: 0.0090418036494849165
+  a: -0.010101451804586053
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
-    x: 586345.71229284233
-    y: 4140702.0188326873
-    theta: 1.2203582618834738
-    kappa: -0.0078091012588391807
-    s: 12.224433939544936
+    x: 586344.74334732478
+    y: 4140699.1960124117
+    theta: 1.2773780252135027
+    kappa: -0.03137683093078978
+    s: 9.2393487255646214
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00022543473432351643
-  a: 0.0076323791289058121
+  v: 0.0078942668445139037
+  a: -0.012738014179602969
   relative_time: 6.6999999999999931
 }
 trajectory_point {
   path_point {
-    x: 586345.71231661306
-    y: 4140702.0188969057
-    theta: 1.2203578014548648
-    kappa: -0.0078085681098089728
-    s: 12.224502416450447
+    x: 586344.743537563
+    y: 4140699.1967091691
+    theta: 1.2773498946233823
+    kappa: -0.031372063682977111
+    s: 9.24007098727738
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0012154712338742759
-  a: 0.011914930102139077
+  v: 0.0065197391191033514
+  a: -0.01460842738844869
   relative_time: 6.7999999999999927
 }
 trajectory_point {
   path_point {
-    x: 586345.71238130913
-    y: 4140702.0190716875
-    theta: 1.2203565483231513
-    kappa: -0.0078071170575864258
-    s: 12.224688787540527
+    x: 586344.7436895353
+    y: 4140699.1972657759
+    theta: 1.2773274224856921
+    kappa: -0.031368255358208134
+    s: 9.2406479673880142
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0025582160967369989
-  a: 0.01469134133494851
+  v: 0.0050046882373483191
+  a: -0.015515634481569632
   relative_time: 6.8999999999999924
 }
 trajectory_point {
   path_point {
-    x: 586345.71249657683
-    y: 4140702.0193830924
-    theta: 1.2203543156453285
-    kappa: -0.0078045317490589043
-    s: 12.225020840902207
+    x: 586344.743800895
+    y: 4140699.1976736374
+    theta: 1.2773109557172713
+    kappa: -0.031365464752290684
+    s: 9.2410707580397222
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0041044938308097629
-  a: 0.015990382466137158
+  v: 0.0034552876582987749
+  a: -0.015262578509411987
   relative_time: 6.999999999999992
 }
 trajectory_point {
   path_point {
-    x: 586345.71266693366
-    y: 4140702.019843325
-    theta: 1.2203510159145683
-    kappa: -0.0078007108565272612
-    s: 12.225511590921496
+    x: 586344.74387235241
+    y: 4140699.197935353
+    theta: 1.2773003893436776
+    kappa: -0.031363674078521551
+    s: 9.2413420540063473
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0057080059078708847
-  a: 0.015840823134507964
+  v: 0.001997416535960206
+  a: -0.013652202522421835
   relative_time: 7.0999999999999917
 }
 trajectory_point {
   path_point {
-    x: 586345.712891869
-    y: 4140702.0204510055
-    theta: 1.2203466590257941
-    kappa: -0.0077956658377527945
-    s: 12.226159565979778
+    x: 586344.743908192
+    y: 4140699.1980666178
+    theta: 1.277295089751967
+    kappa: -0.03136277596105335
+    s: 9.2414781232618868
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0072253307635773631
-  a: 0.014271432978863649
+  v: 0.00077665971929400854
+  a: -0.010487449571045257
   relative_time: 7.1999999999999913
 }
 trajectory_point {
   path_point {
-    x: 586345.71316594316
-    y: 4140702.0211914391
-    theta: 1.220341350341255
-    kappa: -0.0077895186960043068
-    s: 12.226949096150191
+    x: 586344.7439167928
+    y: 4140699.1980981189
+    theta: 1.2772938179410993
+    kappa: -0.0313625604282614
+    s: 9.2415107775499763
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0085159237974694359
-  a: 0.011310981638007148
+  v: 0
+  a: -0.00557126270572839
   relative_time: 7.2999999999999909
 }
 trajectory_point {
   path_point {
-    x: 586345.71347888769
-    y: 4140702.022036883
-    theta: 1.2203352887561054
-    kappa: -0.0077824997401049825
-    s: 12.227850600894032
+    x: 586344.74391114712
+    y: 4140699.1980774412
+    theta: 1.2772946527702778
+    kappa: -0.031362701906110582
+    s: 9.2414893429533951
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0094421173729653564
-  a: 0.0069882387507417387
+  v: 0
+  a: 0.0012934150230823533
   relative_time: 7.3999999999999906
 }
 trajectory_point {
   path_point {
-    x: 586345.71381570515
-    y: 4140702.0229468225
-    theta: 1.2203287647639856
-    kappa: -0.0077749453444795136
-    s: 12.228820876757121
+    x: 586344.74390937923
+    y: 4140699.1980709657
+    theta: 1.2772949142066221
+    kappa: -0.031362746211522227
+    s: 9.2414826304635564
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0098691208173642842
-  a: 0.0013319739558699157
+  v: 0.00028850901726749613
+  a: 0.010303640564940948
   relative_time: 7.49999999999999
 }
 trajectory_point {
   path_point {
-    x: 586345.71415676887
-    y: 4140702.0238682339
-    theta: 1.2203221585226032
-    kappa: -0.0077672957092009814
-    s: 12.229803385066214
+    x: 586344.7439352643
+    y: 4140699.1981657716
+    theta: 1.2772910865750795
+    kappa: -0.031362097545741027
+    s: 9.24158090655
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00966502042184595
-  a: -0.0056290431078052684
+  v: 0.0018661718120062075
+  a: 0.021656470869401145
   relative_time: 7.59999999999999
 }
 trajectory_point {
   path_point {
-    x: 586345.714477923
-    y: 4140702.0247358568
-    theta: 1.2203159379193136
-    kappa: -0.0077600926200379556
-    s: 12.230728539625369
+    x: 586344.74401874852
+    y: 4140699.1984715369
+    theta: 1.2772787418154052
+    kappa: -0.031360005487701827
+    s: 9.2418978637299
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.008700779441469771
-  a: -0.013866042801480873
+  v: 0.004704458581552824
+  a: 0.035548962886016916
   relative_time: 7.6999999999999895
 }
 trajectory_point {
   path_point {
-    x: 586345.71475058247
-    y: 4140702.0254724687
-    theta: 1.2203106566366948
-    kappa: -0.0077539772085014006
-    s: 12.231513994412351
+    x: 586344.74419646652
+    y: 4140699.1991224387
+    theta: 1.2772524627535073
+    kappa: -0.031355551987396563
+    s: 9.2425725911375451
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.006850238095176174
-  a: -0.023350255486354179
+  v: 0.0090671883445999444
+  a: 0.052178173564342026
   relative_time: 7.7999999999999892
 }
 trajectory_point {
   path_point {
-    x: 586345.71494183247
-    y: 4140702.025989145
-    theta: 1.2203069522181165
-    kappa: -0.00774968771189173
-    s: 12.232064931275008
+    x: 586344.74451226182
+    y: 4140699.2002790561
+    theta: 1.2772057663974912
+    kappa: -0.031347638359241155
+    s: 9.2437715450938427
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0039901135657858288
-  a: -0.034052911523622026
+  v: 0.015237885814796497
+  a: 0.071741159853930217
   relative_time: 7.8999999999999888
 }
 trajectory_point {
   path_point {
-    x: 586345.71501452825
-    y: 4140702.0261855386
-    theta: 1.2203055441332993
-    kappa: -0.0077480572333457585
-    s: 12.232274347627669
+    x: 586344.74501770525
+    y: 4140699.2021302693
+    theta: 1.2771310272725307
+    kappa: -0.031334972275442331
+    s: 9.2456905196758132
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8873791418560608e-15
-  a: -0.0459452412744817
+  v: 0.02351978140074644
+  a: 0.094434978704335523
   relative_time: 7.9999999999999885
 }
 decision {
@@ -1705,10 +1706,10 @@ decision {
       reason_code: STOP_REASON_OBSTACLE
       reason: "stop by 1_0"
       stop_point {
-        x: 586346.12364781287
-        y: 4140705.9921340537
+        x: 586345.20425900875
+        y: 4140703.1364937108
       }
-      stop_heading: 1.2638695222186633
+      stop_heading: 1.2567355733668926
       change_lane_type: FORWARD
     }
   }
@@ -1719,12 +1720,12 @@ decision {
       object_decision {
         stop {
           reason_code: STOP_REASON_OBSTACLE
-          distance_s: -3
+          distance_s: -6
           stop_point {
-            x: 586346.12364781287
-            y: 4140705.9921340537
+            x: 586345.20425900875
+            y: 4140703.1364937108
           }
-          stop_heading: 1.2638695222186633
+          stop_heading: 1.2567355733668926
         }
       }
     }
diff --git a/modules/planning/testdata/garage_test/result_stop_over_line_0.pb.txt b/modules/planning/testdata/garage_test/result_stop_over_line_0.pb.txt
index f84d5af407..811a3d19bc 100644
--- a/modules/planning/testdata/garage_test/result_stop_over_line_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_stop_over_line_0.pb.txt
@@ -1061,10 +1061,10 @@ decision {
       reason_code: STOP_REASON_OBSTACLE
       reason: "stop by 2352_0"
       stop_point {
-        x: 586354.79522783868
-        y: 4140738.1639204472
+        x: 586354.05696787883
+        y: 4140735.25617709
       }
-      stop_heading: 1.3216449705945246
+      stop_heading: 1.3226448512227573
       change_lane_type: FORWARD
     }
   }
@@ -1075,12 +1075,12 @@ decision {
       object_decision {
         stop {
           reason_code: STOP_REASON_OBSTACLE
-          distance_s: -3
+          distance_s: -6
           stop_point {
-            x: 586354.79522783868
-            y: 4140738.1639204472
+            x: 586354.05696787883
+            y: 4140735.25617709
           }
-          stop_heading: 1.3216449705945246
+          stop_heading: 1.3226448512227573
         }
       }
     }
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 92ea21d229..d0f4b763ed 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
@@ -17,7 +17,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.4499998092651367
-  a: -0.44997068857863687
+  a: -0.44997068857863676
   relative_time: 0
 }
 trajectory_point {
@@ -31,7 +31,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.4412113344471651
-  a: -0.42901048982805029
+  a: -0.42901048982805007
   relative_time: 0.02
 }
 trajectory_point {
@@ -45,7 +45,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.4328340770357757
-  a: -0.40884718651329771
+  a: -0.40884718651329738
   relative_time: 0.04
 }
 trajectory_point {
@@ -59,7 +59,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.4248522048066814
-  a: -0.38947021019141947
+  a: -0.38947021019141903
   relative_time: 0.06
 }
 trajectory_point {
@@ -73,7 +73,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.4172500969044526
-  a: -0.37086899241945603
+  a: -0.37086899241945548
   relative_time: 0.08
 }
 trajectory_point {
@@ -87,7 +87,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.4100123438425234
-  a: -0.35303296475444779
+  a: -0.35303296475444718
   relative_time: 0.1
 }
 trajectory_point {
@@ -101,7 +101,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.4031237475031819
-  a: -0.33595155875343513
+  a: -0.33595155875343441
   relative_time: 0.12000000000000001
 }
 trajectory_point {
@@ -115,7 +115,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.3965693211375791
-  a: -0.31961420597345847
+  a: -0.31961420597345774
   relative_time: 0.14
 }
 trajectory_point {
@@ -129,7 +129,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.3903342893657227
-  a: -0.3040103379715583
+  a: -0.30401033797155752
   relative_time: 0.16
 }
 trajectory_point {
@@ -143,7 +143,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.3844040881764821
-  a: -0.28912938630477497
+  a: -0.28912938630477414
   relative_time: 0.18
 }
 trajectory_point {
@@ -157,7 +157,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.3787643649275845
-  a: -0.27496078253014888
+  a: -0.274960782530148
   relative_time: 0.19999999999999998
 }
 trajectory_point {
@@ -171,7 +171,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.3734009783456154
-  a: -0.26149395820472054
+  a: -0.26149395820471955
   relative_time: 0.21999999999999997
 }
 trajectory_point {
@@ -179,13 +179,13 @@ trajectory_point {
     x: 587696.37471525231
     y: 4141446.6194088776
     theta: -0.22613485655377008
-    kappa: -0.010331761509857176
-    s: 1.2972314266667664
+    kappa: -0.010331761509857177
+    s: 1.2972314266667666
     dkappa: 0
     ddkappa: 0
   }
   v: 5.3682999985260205
-  a: -0.24871834488553024
+  a: -0.24871834488552921
   relative_time: 0.23999999999999996
 }
 trajectory_point {
@@ -199,7 +199,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.3634477069331057
-  a: -0.23662337412961842
+  a: -0.2366233741296174
   relative_time: 0.25999999999999995
 }
 trajectory_point {
@@ -213,7 +213,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.3588305964000336
-  a: -0.22519847749402558
+  a: -0.22519847749402447
   relative_time: 0.27999999999999997
 }
 trajectory_point {
@@ -227,7 +227,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.3544353711288286
-  a: -0.21443308653579202
+  a: -0.21443308653579091
   relative_time: 0.3
 }
 trajectory_point {
@@ -241,7 +241,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.3502489466903729
-  a: -0.20431663281195822
+  a: -0.20431663281195708
   relative_time: 0.32
 }
 trajectory_point {
@@ -254,22 +254,22 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.3462584500244068
-  a: -0.19483854787956456
+  v: 5.3462584500244077
+  a: -0.19483854787956345
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
     x: 587697.001241224
     y: 4141446.4767161678
-    theta: -0.23275696140089983
+    theta: -0.23275696140089985
     kappa: -0.011857923781480775
-    s: 1.9398012693024658
+    s: 1.939801269302466
     dkappa: 0
     ddkappa: 0
   }
   v: 5.3424512194395337
-  a: -0.18598826329565155
+  a: -0.18598826329565038
   relative_time: 0.36000000000000004
 }
 trajectory_point {
@@ -283,7 +283,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.3388148046132109
-  a: -0.17775521061725946
+  a: -0.1777552106172583
   relative_time: 0.38000000000000006
 }
 trajectory_point {
@@ -297,7 +297,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.3353369665917594
-  a: -0.17012882140142876
+  a: -0.17012882140142765
   relative_time: 0.40000000000000008
 }
 trajectory_point {
@@ -311,7 +311,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.3320056777903568
-  a: -0.16309852720519996
+  a: -0.16309852720519885
   relative_time: 0.4200000000000001
 }
 trajectory_point {
@@ -320,12 +320,12 @@ trajectory_point {
     y: 4141446.3732748665
     theta: -0.23824188514488
     kappa: -0.012957148720873921
-    s: 2.3666360439033935
+    s: 2.366636043903394
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.32880912199304
-  a: -0.15665375958561339
+  v: 5.3288091219930411
+  a: -0.15665375958561217
   relative_time: 0.44000000000000011
 }
 trajectory_point {
@@ -339,7 +339,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.3257356943527077
-  a: -0.15078395009970946
+  a: -0.15078395009970824
   relative_time: 0.46000000000000013
 }
 trajectory_point {
@@ -353,7 +353,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.3227740013911138
-  a: -0.14547853030452851
+  a: -0.14547853030452734
   relative_time: 0.48000000000000015
 }
 trajectory_point {
@@ -366,8 +366,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.3199128609988744
-  a: -0.1407269317571111
+  v: 5.3199128609988753
+  a: -0.14072693175710999
   relative_time: 0.50000000000000011
 }
 trajectory_point {
@@ -380,8 +380,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.3171413024354637
-  a: -0.13651858601449757
+  v: 5.3171413024354646
+  a: -0.13651858601449646
   relative_time: 0.52000000000000013
 }
 trajectory_point {
@@ -395,21 +395,21 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.314448566329216
-  a: -0.13284292463372832
+  a: -0.13284292463372727
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587698.03421920829
-    y: 4141446.2165988404
+    x: 587698.03421920817
+    y: 4141446.21659884
     theta: -0.24669253030687402
-    kappa: -0.014620595813509347
-    s: 3.0050417259027924
+    kappa: -0.01462059581350935
+    s: 3.0050417259027933
     dkappa: 0
     ddkappa: 0
   }
   v: 5.3118241046773225
-  a: -0.12968937917184387
+  a: -0.12968937917184276
   relative_time: 0.56000000000000016
 }
 trajectory_point {
@@ -423,7 +423,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.309257580845836
-  a: -0.12704738118588443
+  a: -0.12704738118588343
   relative_time: 0.58000000000000018
 }
 trajectory_point {
@@ -436,8 +436,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.3067388695696671
-  a: -0.12490636223289053
+  v: 5.306738869569668
+  a: -0.12490636223288965
   relative_time: 0.6000000000000002
 }
 trajectory_point {
@@ -450,8 +450,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.3042580569525866
-  a: -0.12325575386990267
+  v: 5.3042580569525875
+  a: -0.12325575386990173
   relative_time: 0.62000000000000022
 }
 trajectory_point {
@@ -465,7 +465,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.3018054404672244
-  a: -0.12208498765396114
+  a: -0.12208498765396025
   relative_time: 0.64000000000000024
 }
 trajectory_point {
@@ -479,21 +479,21 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.299371528955068
-  a: -0.12138349514210639
+  a: -0.12138349514210561
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587698.65126897406
+    x: 587698.65126897418
     y: 4141446.0603865609
     theta: -0.2551181426627343
-    kappa: -0.016279119245819516
-    s: 3.6415577819911573
+    kappa: -0.016279119245819519
+    s: 3.6415577819911578
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2969470426264653
-  a: -0.12114070789137887
+  v: 5.2969470426264662
+  a: -0.12114070789137815
   relative_time: 0.68000000000000027
 }
 trajectory_point {
@@ -502,12 +502,12 @@ trajectory_point {
     y: 4141446.0343932221
     theta: -0.25652005799163208
     kappa: -0.016555093466301841
-    s: 3.747472488398885
+    s: 3.7474724883988859
     dkappa: 0
     ddkappa: 0
   }
   v: 5.2945229130606251
-  a: -0.12134605745881905
+  a: -0.12134605745881832
   relative_time: 0.70000000000000029
 }
 trajectory_point {
@@ -521,7 +521,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.2920902832056118
-  a: -0.12198897540146708
+  a: -0.12198897540146642
   relative_time: 0.72000000000000031
 }
 trajectory_point {
@@ -534,8 +534,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2896405073783512
-  a: -0.12305889327636371
+  v: 5.2896405073783521
+  a: -0.12305889327636305
   relative_time: 0.74000000000000032
 }
 trajectory_point {
@@ -544,12 +544,12 @@ trajectory_point {
     y: 4141445.9540674253
     theta: -0.26129705088497474
     kappa: -0.017409879468413637
-    s: 4.064924091510937
+    s: 4.0649240915109379
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2871651512646283
-  a: -0.12454524264054906
+  v: 5.2871651512646292
+  a: -0.1245452426405485
   relative_time: 0.76000000000000034
 }
 trajectory_point {
@@ -557,27 +557,27 @@ trajectory_point {
     x: 587699.16256463423
     y: 4141445.9246170842
     theta: -0.26353061895553531
-    kappa: -0.017725395871813737
-    s: 4.1706423660223928
+    kappa: -0.017725395871813744
+    s: 4.1706423660223937
     dkappa: 0
     ddkappa: 0
   }
   v: 5.2846559919190872
-  a: -0.12643745505106374
+  a: -0.1264374550510633
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
     x: 587699.26404944679
     y: 4141445.8951808354
-    theta: -0.26576329077069377
-    kappa: -0.018040761294752794
-    s: 4.276310052375341
+    theta: -0.26576329077069383
+    kappa: -0.018040761294752797
+    s: 4.2763100523753419
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.28210501776523
-  a: -0.12872496206494816
+  v: 5.2821050177652307
+  a: -0.12872496206494771
   relative_time: 0.80000000000000038
 }
 trajectory_point {
@@ -590,8 +590,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.27950442859542
-  a: -0.13139719523924259
+  v: 5.2795044285954207
+  a: -0.1313971952392422
   relative_time: 0.8200000000000004
 }
 trajectory_point {
@@ -605,21 +605,21 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.2768466355708776
-  a: -0.13444358613098767
+  a: -0.13444358613098728
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
     x: 587699.56820285157
     y: 4141445.8069594046
-    theta: -0.27245549476196557
-    kappa: -0.01898592209922496
-    s: 4.5929996707529588
+    theta: -0.27245549476196551
+    kappa: -0.018985922099224964
+    s: 4.59299967075296
     dkappa: 0
     ddkappa: 0
   }
   v: 5.2741242612216839
-  a: -0.13785356629722345
+  a: -0.13785356629722323
   relative_time: 0.86000000000000043
 }
 trajectory_point {
@@ -627,22 +627,22 @@ trajectory_point {
     x: 587699.66948307958
     y: 4141445.7775824973
     theta: -0.27468413199609754
-    kappa: -0.019300651772775571
-    s: 4.6984543401988814
+    kappa: -0.019300651772775575
+    s: 4.6984543401988823
     dkappa: 0
     ddkappa: 0
   }
   v: 5.2713301394467784
-  a: -0.14161656729499067
+  a: -0.1416165672949905
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 587699.77070889226
-    y: 4141445.7482213718
+    x: 587699.77070889238
+    y: 4141445.7482213723
     theta: -0.27691163288071652
     kappa: -0.01961521235028562
-    s: 4.8038523516028064
+    s: 4.8038523516028073
     dkappa: 0
     ddkappa: 0
   }
@@ -654,27 +654,27 @@ trajectory_point {
   path_point {
     x: 587699.87187871314
     y: 4141445.7188764885
-    theta: -0.27913794053288904
-    kappa: -0.019929598931712929
-    s: 4.90919206313566
+    theta: -0.279137940532889
+    kappa: -0.019929598931712932
+    s: 4.9091920631356611
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2654990460598876
-  a: -0.1501593580132809
+  v: 5.2654990460598885
+  a: -0.15015935801328084
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
     x: 587699.97299083741
     y: 4141445.68954834
-    theta: -0.28136299533258563
-    kappa: -0.020243806220812
-    s: 5.0144717002147932
+    theta: -0.28136299533258569
+    kappa: -0.020243806220812004
+    s: 5.014471700214794
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2624487990900777
+  v: 5.2624487990900786
   a: -0.15491801084788459
   relative_time: 0.9400000000000005
 }
@@ -682,14 +682,14 @@ trajectory_point {
   path_point {
     x: 587700.07404343761
     y: 4141445.6602374567
-    theta: -0.28358673501678655
+    theta: -0.28358673501678661
     kappa: -0.020557828537750625
-    s: 5.1196893597313506
+    s: 5.1196893597313515
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2593002539789078
-  a: -0.15998741074218131
+  v: 5.2593002539789087
+  a: -0.15998741074218142
   relative_time: 0.96000000000000052
 }
 trajectory_point {
@@ -702,8 +702,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2560473014696125
-  a: -0.16535698925321152
+  v: 5.2560473014696134
+  a: -0.16535698925321157
   relative_time: 0.98000000000000054
 }
 trajectory_point {
@@ -717,7 +717,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.2526840436742877
-  a: -0.17101617793801538
+  a: -0.1710161779380156
   relative_time: 1.0000000000000004
 }
 trajectory_point {
@@ -730,8 +730,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2340215451506786
-  a: -0.20328637846505695
+  v: 5.2340215451506795
+  a: -0.20328637846505757
   relative_time: 1.1000000000000005
 }
 trajectory_point {
@@ -740,26 +740,26 @@ trajectory_point {
     y: 4141445.2928608316
     theta: -0.310879216339083
     kappa: -0.024529089521531017
-    s: 6.3766173193460478
+    s: 6.3766173193460487
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2118382683845956
-  a: -0.24121156689250633
+  v: 5.2118382683845965
+  a: -0.24121156689250731
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
     x: 587701.767864046
     y: 4141445.1250489582
-    theta: -0.32284522595013715
-    kappa: -0.026374115134505431
-    s: 6.8965262054151255
+    theta: -0.3228452259501372
+    kappa: -0.026374115134505438
+    s: 6.8965262054151264
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.1856347673544949
-  a: -0.28347068785041485
+  v: 5.1856347673544958
+  a: -0.2834706878504159
   relative_time: 1.3000000000000007
 }
 trajectory_point {
@@ -772,8 +772,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.1550437015758286
-  a: -0.32874268596883471
+  v: 5.1550437015758295
+  a: -0.32874268596883616
   relative_time: 1.4000000000000008
 }
 trajectory_point {
@@ -787,7 +787,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.1198298361010428
-  a: -0.37570650587781723
+  a: -0.37570650587781879
   relative_time: 1.5000000000000009
 }
 trajectory_point {
@@ -801,7 +801,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.0798900415195778
-  a: -0.42304109220741443
+  a: -0.42304109220741609
   relative_time: 1.600000000000001
 }
 trajectory_point {
@@ -815,21 +815,21 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.0352532939578705
-  a: -0.46942538958767815
+  a: -0.46942538958767954
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
     x: 587704.14811416157
     y: 4141444.2196517102
-    theta: -0.39709537241085596
-    kappa: -0.036544216315704592
-    s: 9.4443054796378743
+    theta: -0.397095372410856
+    kappa: -0.0365442163157046
+    s: 9.444305479637876
     dkappa: 0
     ddkappa: 0
   }
   v: 4.9860806750793518
-  a: -0.51353834264865983
+  a: -0.5135383426486605
   relative_time: 1.8000000000000012
 }
 trajectory_point {
@@ -842,22 +842,22 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.9326653720844469
-  a: -0.55405889602041114
+  v: 4.932665372084446
+  a: -0.55405889602041192
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
     x: 587705.04609378288
-    y: 4141443.8132358952
-    theta: -0.43719491130683152
-    kappa: -0.041212069117411661
-    s: 10.430711160891535
+    y: 4141443.8132358948
+    theta: -0.43719491130683164
+    kappa: -0.041212069117411668
+    s: 10.430711160891537
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8754326777105756
-  a: -0.58966599433298406
+  v: 4.8754326777105739
+  a: -0.58966599433298472
   relative_time: 2.0000000000000013
 }
 trajectory_point {
@@ -865,27 +865,27 @@ trajectory_point {
     x: 587705.47876611492
     y: 4141443.5951076667
     theta: -0.46089229668671583
-    kappa: -0.043739479084077632
-    s: 10.915257619926296
+    kappa: -0.043739479084077638
+    s: 10.915257619926297
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8150712584589135
-  a: -0.61519739020155217
+  v: 4.8150712584589117
+  a: -0.6151973902015524
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
     x: 587705.905955271
     y: 4141443.3797437367
-    theta: -0.484292690254961
-    kappa: -0.046234859655075192
-    s: 11.393663511953392
+    theta: -0.48429269025496108
+    kappa: -0.046234859655075206
+    s: 11.393663511953394
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7528494754443456
-  a: -0.62704051284770368
+  v: 4.7528494754443438
+  a: -0.62704051284770357
   relative_time: 2.2000000000000015
 }
 trajectory_point {
@@ -898,22 +898,22 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6900859830121311
-  a: -0.62619882181111208
+  v: 4.6900859830121293
+  a: -0.62619882181111164
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
     x: 587706.725996887
     y: 4141442.9265193753
-    theta: -0.53092267042578878
-    kappa: -0.051387583334273089
-    s: 12.331703373350445
+    theta: -0.53092267042578889
+    kappa: -0.051387583334273096
+    s: 12.331703373350447
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6279990895535592
-  a: -0.61367577663145045
+  v: 4.6279990895535574
+  a: -0.61367577663144979
   relative_time: 2.4000000000000017
 }
 trajectory_point {
@@ -926,22 +926,22 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5677067575059533
-  a: -0.59047483684839219
+  v: 4.5677067575059525
+  a: -0.5904748368483913
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
     x: 587707.49715839454
     y: 4141442.4365814077
-    theta: -0.57766359899591857
-    kappa: -0.056813570385423728
-    s: 13.245338575610285
+    theta: -0.57766359899591868
+    kappa: -0.056813570385423742
+    s: 13.245338575610287
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5102266033526712
-  a: -0.55759946200161048
+  v: 4.5102266033526694
+  a: -0.55759946200160948
   relative_time: 2.6000000000000019
 }
 trajectory_point {
@@ -954,8 +954,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4564758976231
-  a: -0.51605311163077872
+  v: 4.4564758976230987
+  a: -0.51605311163077761
   relative_time: 2.700000000000002
 }
 trajectory_point {
@@ -968,8 +968,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4072715648926613
-  a: -0.46683924527557
+  v: 4.4072715648926595
+  a: -0.46683924527556891
   relative_time: 2.800000000000002
 }
 trajectory_point {
@@ -982,8 +982,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3633301837828089
-  a: -0.410961322475658
+  v: 4.363330183782808
+  a: -0.41096132247565675
   relative_time: 2.9000000000000021
 }
 trajectory_point {
@@ -996,8 +996,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.32526798696103
-  a: -0.34942280277071547
+  v: 4.3252679869610295
+  a: -0.34942280277071425
   relative_time: 3.0000000000000022
 }
 trajectory_point {
@@ -1010,22 +1010,22 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2936008611408445
-  a: -0.28322714570041596
+  v: 4.2936008611408436
+  a: -0.28322714570041491
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587709.575965581
+    x: 587709.57596558111
     y: 4141440.842910808
     theta: -0.74666789724299787
-    kappa: -0.072150646919410283
-    s: 15.868594837574431
+    kappa: -0.072150646919410269
+    s: 15.868594837574429
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2687443470818032
-  a: -0.21337781080443285
+  v: 4.2687443470818023
+  a: -0.2133778108044318
   relative_time: 3.2000000000000024
 }
 trajectory_point {
@@ -1038,8 +1038,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.251013639589492
-  a: -0.14087825762243955
+  v: 4.2510136395894911
+  a: -0.14087825762243816
   relative_time: 3.3000000000000025
 }
 trajectory_point {
@@ -1052,8 +1052,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2406235875155271
-  a: -0.066731945694108674
+  v: 4.2406235875155263
+  a: -0.066731945694107675
   relative_time: 3.4000000000000026
 }
 trajectory_point {
@@ -1067,7 +1067,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 4.2376886937575584
-  a: 0.00805766544088582
+  a: 0.0080576654408868187
   relative_time: 3.5000000000000027
 }
 trajectory_point {
@@ -1080,8 +1080,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2422231152592706
-  a: 0.082487116242870862
+  v: 4.24222311525927
+  a: 0.08248711624287175
   relative_time: 3.6000000000000028
 }
 trajectory_point {
@@ -1094,8 +1094,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2541406630103769
-  a: 0.15555294717217327
+  v: 4.254140663010376
+  a: 0.15555294717217416
   relative_time: 3.7000000000000028
 }
 trajectory_point {
@@ -1109,7 +1109,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 4.2732548020466261
-  a: 0.22625169868911976
+  a: 0.22625169868912043
   relative_time: 3.8000000000000029
 }
 trajectory_point {
@@ -1122,8 +1122,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2992786514498
-  a: 0.29357991125403682
+  v: 4.2992786514497991
+  a: 0.2935799112540376
   relative_time: 3.900000000000003
 }
 trajectory_point {
@@ -1136,8 +1136,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.33182498434771
-  a: 0.356534125327252
+  v: 4.331824984347711
+  a: 0.35653412532725165
   relative_time: 4.0000000000000027
 }
 trajectory_point {
@@ -1150,22 +1150,22 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.370464249878613
-  a: 0.4158951176791354
+  v: 4.3704642498786139
+  a: 0.415895117679135
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587712.16527010384
+    x: 587712.16527010372
     y: 4141437.4561588806
-    theta: -1.0865007154479518
-    kappa: -0.080993787878319345
-    s: 20.152257759417274
+    theta: -1.086500715447952
+    kappa: -0.080993787878319332
+    s: 20.152257759417278
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4149335009866553
-  a: 0.47314090312456669
+  v: 4.4149335009866562
+  a: 0.47314090312456625
   relative_time: 4.200000000000002
 }
 trajectory_point {
@@ -1178,8 +1178,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4650233368575467
-  a: 0.528313879190634
+  v: 4.4650233368575476
+  a: 0.52831387919063355
   relative_time: 4.3000000000000016
 }
 trajectory_point {
@@ -1192,8 +1192,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5205285964297062
-  a: 0.58145644340442526
+  v: 4.5205285964297071
+  a: 0.58145644340442493
   relative_time: 4.4000000000000012
 }
 trajectory_point {
@@ -1207,21 +1207,21 @@ trajectory_point {
     ddkappa: 0
   }
   v: 4.5812483583942587
-  a: 0.63261099329302883
+  a: 0.63261099329302839
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
     x: 587712.881943927
     y: 4141435.7966682254
-    theta: -1.2236576661716421
-    kappa: -0.076022399371374241
-    s: 21.961859863863875
+    theta: -1.2236576661716423
+    kappa: -0.076022399371374227
+    s: 21.961859863863879
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6469859411950409
-  a: 0.6818199263835325
+  v: 4.6469859411950418
+  a: 0.68181992638353206
   relative_time: 4.6000000000000005
 }
 trajectory_point {
@@ -1234,8 +1234,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7175489030285975
-  a: 0.72912564020302451
+  v: 4.7175489030285984
+  a: 0.72912564020302417
   relative_time: 4.7
 }
 trajectory_point {
@@ -1249,7 +1249,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 4.7927490418441812
-  a: 0.77457053227859285
+  a: 0.77457053227859241
   relative_time: 4.8
 }
 trajectory_point {
@@ -1262,8 +1262,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8724023953437543
-  a: 0.81819700013732521
+  v: 4.8724023953437552
+  a: 0.81819700013732488
   relative_time: 4.8999999999999995
 }
 trajectory_point {
@@ -1277,7 +1277,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 4.9563292409819875
-  a: 0.86004744130631017
+  a: 0.86004744130630983
   relative_time: 4.9999999999999991
 }
 trajectory_point {
@@ -1291,7 +1291,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.0443540959662609
-  a: 0.90016425331263572
+  a: 0.90016425331263539
   relative_time: 5.0999999999999988
 }
 trajectory_point {
@@ -1305,7 +1305,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.1363057172566622
-  a: 0.93858983368338977
+  a: 0.93858983368338944
   relative_time: 5.1999999999999984
 }
 trajectory_point {
@@ -1318,8 +1318,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2320171015659875
-  a: 0.97536657994566023
+  v: 5.2320171015659884
+  a: 0.97536657994566
   relative_time: 5.299999999999998
 }
 trajectory_point {
@@ -1333,7 +1333,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.3313254853597467
-  a: 1.0105368896265354
+  a: 1.0105368896265352
   relative_time: 5.3999999999999977
 }
 trajectory_point {
@@ -1347,7 +1347,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.4340723448561512
-  a: 1.0441431602531035
+  a: 1.0441431602531033
   relative_time: 5.4999999999999973
 }
 trajectory_point {
@@ -1368,9 +1368,9 @@ trajectory_point {
   path_point {
     x: 587713.720155767
     y: 4141430.2676681858
-    theta: -1.5762797644534119
-    kappa: -0.04569397911213044
-    s: 27.582039191646896
+    theta: -1.5762797644534121
+    kappa: -0.045693979112130412
+    s: 27.5820391916469
     dkappa: 0
     ddkappa: 0
   }
@@ -1389,7 +1389,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.7614221360340245
-  a: 1.1360017130778439
+  a: 1.1360017130778441
   relative_time: 5.7999999999999963
 }
 trajectory_point {
@@ -1403,7 +1403,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.8764224555773392
-  a: 1.1637758027580636
+  a: 1.1637758027580631
   relative_time: 5.8999999999999959
 }
 trajectory_point {
@@ -1411,41 +1411,41 @@ trajectory_point {
     x: 587713.63250906253
     y: 4141428.5245517544
     theta: -1.6476754082505372
-    kappa: -0.036587513670223976
-    s: 29.32792422315903
+    kappa: -0.036587513670223955
+    s: 29.327924223159034
     dkappa: 0
     ddkappa: 0
   }
   v: 5.9941322282050074
-  a: 1.1901978410194156
+  a: 1.1901978410194158
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
     x: 587713.58364551188
     y: 4141427.9214400039
-    theta: -1.6698750890473353
-    kappa: -0.033644431101795387
-    s: 29.933330826603733
+    theta: -1.6698750890473355
+    kappa: -0.033644431101795373
+    s: 29.933330826603736
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.1144183686514957
+  v: 6.1144183686514966
   a: 1.2153102253889891
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 587713.505260699
+    x: 587713.50526069908
     y: 4141427.3088761242
-    theta: -1.6891518561922796
-    kappa: -0.031011930409219339
-    s: 30.550889476255357
+    theta: -1.6891518561922798
+    kappa: -0.031011930409219322
+    s: 30.55088947625536
     dkappa: 0
     ddkappa: 0
   }
   v: 6.2371520314039817
-  a: 1.2391553533938715
+  a: 1.2391553533938717
   relative_time: 6.1999999999999948
 }
 trajectory_point {
@@ -1458,7 +1458,7 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.362208610702349
+  v: 6.36220861070235
   a: 1.2617756225611512
   relative_time: 6.2999999999999945
 }
@@ -1473,7 +1473,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 6.4894677405391938
-  a: 1.2832134304179159
+  a: 1.2832134304179164
   relative_time: 6.3999999999999941
 }
 trajectory_point {
@@ -1487,7 +1487,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 6.6188132946598168
-  a: 1.303511174491254
+  a: 1.3035111744912544
   relative_time: 6.4999999999999938
 }
 trajectory_point {
@@ -1495,27 +1495,27 @@ trajectory_point {
     x: 587713.1025272829
     y: 4141424.7448523538
     theta: -1.7548033750523035
-    kappa: -0.021112460197877894
-    s: 33.147233084183448
+    kappa: -0.021112460197877866
+    s: 33.147233084183455
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.7501333865622319
-  a: 1.3227112523082536
+  v: 6.7501333865622328
+  a: 1.322711252308254
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
     x: 587712.97711830016
-    y: 4141424.074891652
-    theta: -1.7684657796399694
-    kappa: -0.018834627194035364
-    s: 33.828890651901034
+    y: 4141424.0748916524
+    theta: -1.7684657796399696
+    kappa: -0.018834627194035346
+    s: 33.828890651901041
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8833203694971585
-  a: 1.3408560613960026
+  v: 6.8833203694971594
+  a: 1.340856061396003
   relative_time: 6.6999999999999931
 }
 trajectory_point {
@@ -1529,7 +1529,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 7.0182708364680266
-  a: 1.3579879992815889
+  a: 1.3579879992815893
   relative_time: 6.7999999999999927
 }
 trajectory_point {
@@ -1542,8 +1542,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.1548856202309725
-  a: 1.3741494634921008
+  v: 7.1548856202309734
+  a: 1.374149463492101
   relative_time: 6.8999999999999924
 }
 trajectory_point {
@@ -1556,8 +1556,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.2930697932948449
-  a: 1.389382851554626
+  v: 7.2930697932948467
+  a: 1.3893828515546265
   relative_time: 6.999999999999992
 }
 trajectory_point {
@@ -1570,8 +1570,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.4327326679212007
-  a: 1.4037305609962529
+  v: 7.4327326679212016
+  a: 1.4037305609962536
   relative_time: 7.0999999999999917
 }
 trajectory_point {
@@ -1584,22 +1584,22 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.5737877961243019
-  a: 1.4172349893440697
+  v: 7.5737877961243028
+  a: 1.4172349893440703
   relative_time: 7.1999999999999913
 }
 trajectory_point {
   path_point {
     x: 587711.95634528028
-    y: 4141419.8190004067
+    y: 4141419.8190004062
     theta: -1.8285330315214277
-    kappa: -0.0082709874899339758
-    s: 38.206064758402555
+    kappa: -0.0082709874899339619
+    s: 38.206064758402562
     dkappa: 0
     ddkappa: 0
   }
   v: 7.7161529696711231
-  a: 1.4299385341251645
+  a: 1.4299385341251647
   relative_time: 7.2999999999999909
 }
 trajectory_point {
@@ -1612,8 +1612,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8597502200813452
-  a: 1.4418835928666243
+  v: 7.8597502200813469
+  a: 1.4418835928666249
   relative_time: 7.3999999999999906
 }
 trajectory_point {
@@ -1626,8 +1626,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0045058186273614
-  a: 1.4531125630955386
+  v: 8.0045058186273632
+  a: 1.453112563095539
   relative_time: 7.49999999999999
 }
 trajectory_point {
@@ -1640,8 +1640,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.15035027633427
-  a: 1.4636678423389948
+  v: 8.1503502763342723
+  a: 1.4636678423389953
   relative_time: 7.59999999999999
 }
 trajectory_point {
@@ -1654,8 +1654,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.2972183439798819
-  a: 1.4735918281240807
+  v: 8.2972183439798837
+  a: 1.4735918281240812
   relative_time: 7.6999999999999895
 }
 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 35db79b551..f6bc886abc 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_slowdown_01_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_slowdown_01_0.pb.txt
@@ -1,7 +1,7 @@
 header {
   module_name: "planning"
 }
-total_path_length: 12.344350787549768
+total_path_length: 9.3671722018507761
 total_path_time: 7.9999999999999885
 is_replan: true
 gear: GEAR_DRIVE
@@ -9,1695 +9,1694 @@ trajectory_point {
   path_point {
     x: 587258.38443173422
     y: 4140947.0761697497
-    z: 0
     theta: -1.7938388115138242
-    kappa: 0.0010060738435035696
-    s: 0
+    kappa: 0.0010060738435035683
+    s: 2.6026273150514534e-16
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7111110687255859
-  a: -0.55372820002608236
+  v: 1.7111110687255853
+  a: -0.55771118380669049
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 587258.3767171636
-    y: 4140947.0429402106
-    theta: -1.7939823665130317
-    kappa: 0.00083360197975538739
-    s: 0.034113294881241756
+    x: 587258.3767174
+    y: 4140947.0429412285
+    theta: -1.7939823621144158
+    kappa: 0.00083360726439923568
+    s: 0.03411224962893207
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7003088748663093
-  a: -0.52659145563245
+  v: 1.7001919721036631
+  a: -0.53428770807773762
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 587258.36905023211
-    y: 4140947.00991587
-    theta: -1.79412503519632
-    kappa: 0.000662195164834421
-    s: 0.068015933230361952
+    x: 587258.36905140057
+    y: 4140947.0099209035
+    theta: -1.7941250134534126
+    kappa: 0.00066222128745888336
+    s: 0.068010766423602917
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6900434205265722
-  a: -0.50005320729617286
+  v: 1.6897360101965044
+  a: -0.511396763244488
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 587258.361428539
-    y: 4140946.9770863885
-    theta: -1.79426686222699
-    kappa: 0.000491799728276772
-    s: 0.10171853055485028
+    x: 587258.36143166525
+    y: 4140946.9770998545
+    theta: -1.7942668040545702
+    kappa: 0.00049186961840601436
+    s: 0.1017047069517633
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6803027982317855
-  a: -0.47410721036144571
+  v: 1.6797325893313471
+  a: -0.489032654791727
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 587258.35384973721
-    y: 4140946.9444416561
-    theta: -1.7944078912718364
-    kappa: 0.00032236319735336509
-    s: 0.13523146546163586
+    x: 587258.35385617067
+    y: 4140946.9444693672
+    theta: -1.7944077715552289
+    kappa: 0.0003225070284074167
+    s: 0.13520301704661145
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6710752254004761
-  a: -0.4487472201724636
+  v: 1.6701712297257334
+  a: -0.46718968820423995
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 587258.34631153266
-    y: 4140946.9119717907
-    theta: -1.7945481650116324
-    kappa: 0.00015383428444111882
-    s: 0.16856488215494947
+    x: 587258.34632294124
+    y: 4140946.9120209324
+    theta: -1.7945479527158736
+    kappa: 0.00015408934255690704
+    s: 0.16851443408459962
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6623490443442872
-  a: -0.42396699207342126
+  v: 1.6610415654875104
+  a: -0.44586216896681224
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 587258.3388116837
-    y: 4140946.8796671373
-    theta: -1.7946877251516133
-    kappa: -1.3837125605882516e-05
-    s: 0.20172869293418585
+    x: 587258.33883004764
+    y: 4140946.8797462378
+    theta: -1.794687383429695
+    kappa: -1.3426571640925327e-05
+    s: 0.20164748926323989
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6541127222679783
-  a: -0.39976028140851361
+  v: 1.6523333446148287
+  a: -0.42504440256422915
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 587258.33134800068
-    y: 4140946.8475182643
-    theta: -1.7948266124319621
-    kappa: -0.00018069998808521932
-    s: 0.23473258069176628
+    x: 587258.33137560636
+    y: 4140946.8476371733
+    theta: -1.7948260987321529
+    kappa: -0.0001800828157869763
+    s: 0.23461050987891047
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6463548512694246
-  a: -0.37612084352193564
+  v: 1.6440364289961438
+  a: -0.40473069448127597
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 587258.323918345
-    y: 4140946.8155159606
-    theta: -1.7949648666382967
-    kappa: -0.00034680211107496477
-    s: 0.26758600141100047
+    x: 587258.32395778026
+    y: 4140946.8156858231
+    theta: -1.7949641328105392
+    kappa: -0.00034592047210388333
+    s: 0.26741162160466175
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6390641483396173
-  a: -0.35304243375788213
+  v: 1.6361407944102149
+  a: -0.384915350202738
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 587258.31652062922
-    y: 4140946.7836512332
-    theta: -1.795102526612159
-    kappa: -0.00051219016806251371
-    s: 0.30029818666394925
+    x: 587258.31657477655
+    y: 4140946.7838844657
+    theta: -1.7951015190135433
+    kappa: -0.00051097961495228154
+    s: 0.30005875076802224
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6322294553626644
-  a: -0.33051880746054807
+  v: 1.6286365305261061
+  a: -0.36559267521340066
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 587258.30915281572
-    y: 4140946.7519153059
-    theta: -1.7952396302615035
-    kappa: -0.00067690971057341354
-    s: 0.33287814610928684
+    x: 587258.30922484747
+    y: 4140946.7522255736
+    theta: -1.7952382898608175
+    kappa: -0.00067529932234706178
+    s: 0.33255962662880495
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6258397391157895
-  a: -0.30854371997412816
+  v: 1.6215138409031857
+  a: -0.34675697499804925
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 587258.30181291653
-    y: 4140946.720299616
-    theta: -1.7953762145711885
-    kappa: -0.00084100518080019093
-    s: 0.3653346699901629
+    x: 587258.301906289
+    y: 4140946.7207018062
+    theta: -1.7953744770525448
+    kappa: -0.00083891768747362534
+    s: 0.36492178365691308
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.619884091269332
-  a: -0.28711092664281745
+  v: 1.6147630429911257
+  a: -0.32840255504146904
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587258.294498993
-    y: 4140946.6887958134
-    theta: -1.7955123156134678
-    kappa: -0.0010045199242311813
-    s: 0.39767633163206517
+    x: 587258.29461744067
+    y: 4140946.6893060114
+    theta: -1.7955101114790062
+    kappa: -0.0010018718302041429
+    s: 0.39715256381014652
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6143517283867481
-  a: -0.26621418281081083
+  v: 1.6083745681299031
+  a: -0.31052372082844537
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 587258.28720915469
-    y: 4140946.657395754
-    theta: -1.7956479685584825
-    kappa: -0.0011674962022793613
-    s: 0.42991148994068173
+    x: 587258.2873566855
+    y: 4140946.6580312252
+    theta: -1.7956452232301492
+    kappa: -0.0011641979086138084
+    s: 0.42925911881200762
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6092319919246096
-  a: -0.24584724382230305
+  v: 1.6023389615497989
+  a: -0.29311477784376372
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587258.2799415593
-    y: 4140946.6260915035
-    theta: -1.7957832076847566
-    kappa: -0.0013299752049111713
-    s: 0.4620482918997631
+    x: 587258.28012244846
+    y: 4140946.626870661
+    theta: -1.7957798416051585
+    kappa: -0.0013259311304971006
+    s: 0.46124841242950754
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6045143482326045
-  a: -0.22600386502148911
+  v: 1.5966468823713984
+  a: -0.2761700315722091
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587258.27269441169
-    y: 4140946.59487533
-    theta: -1.7959180663896892
-    kappa: -0.0014919970632753512
-    s: 0.49409467506898486
+    x: 587258.27291319671
+    y: 4140946.5958177187
+    theta: -1.7959139951220249
+    kappa: -0.0014871057648840331
+    s: 0.49312722275097204
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6001883885535375
-  a: -0.20667780175256384
+  v: 1.5912891036055912
+  a: -0.25968378749856713
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 587258.26546596352
-    y: 4140946.5637397012
-    theta: -1.796052577200052
-    kappa: -0.0016536008623317651
-    s: 0.52605837008180967
+    x: 587258.26572743885
+    y: 4140946.5648659733
+    theta: -1.7960477115271181
+    kappa: -0.0016477551535564164
+    s: 0.52490214446384753
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5962438290233281
-  a: -0.18786280935972216
+  v: 1.586256512153571
+  a: -0.24365035110762295
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 587258.258254513
-    y: 4140946.5326772882
-    theta: -1.7961867717824833
-    kappa: -0.0018148246534802316
-    s: 0.55794690314334994
+    x: 587258.25856372458
+    y: 4140946.5340091782
+    theta: -1.7961810178047577
+    kappa: -0.0018079117225641124
+    s: 0.55657959113250777
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5926705106710133
-  a: -0.1695526431871589
+  v: 1.5815401088068364
+  a: -0.22806402788416197
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 587258.25105840364
-    y: 4140946.5016809553
-    theta: -1.7963206809539871
-    kappa: -0.0019757054671893544
-    s: 0.58976759852822991
+    x: 587258.25142064388
+    y: 4140946.5032412591
+    theta: -1.7963139401867858
+    kappa: -0.0019676069937412926
+    s: 0.58816579747605935
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5894583994187452
-  a: -0.151741058579069
+  v: 1.5771310082471894
+  a: -0.21291912331296958
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587258.24387602427
-    y: 4140946.4707437623
-    theta: -1.7964543346924291
-    kappa: -0.0021362793256253488
-    s: 0.621527581078448
+    x: 587258.24429682689
+    y: 4140946.4725563163
+    theta: -1.7964465041621407
+    kappa: -0.00212687159622269
+    s: 0.61966682164614773
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5865975860817927
-  a: -0.13442181087964739
+  v: 1.5730204390467373
+  a: -0.19820994287883092
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587258.23670580809
-    y: 4140946.43985896
-    theta: -1.7965877621470354
-    kappa: -0.0022965812552808726
-    s: 0.653233778701239
+    x: 587258.23719094263
+    y: 4140946.4419486159
+    theta: -1.7965787344864306
+    kappa: -0.0022857352779598649
+    s: 0.651088547504764
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5840782863685403
-  a: -0.11758865543308888
+  v: 1.5691997436678906
+  a: -0.18393079206653146
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587258.22954623238
-    y: 4140946.4090199918
-    theta: -1.7967209916488922
-    kappa: -0.0024566452996038542
-    s: 0.68489292486693687
+    x: 587258.23010169971
+    y: 4140946.4114125967
+    theta: -1.796710655191508
+    kappa: -0.0024442269172374525
+    s: 0.68243668690205
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5818908408804888
-  a: -0.10123534758358838
+  v: 1.5656603784633651
+  a: -0.17007597636085653
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 587258.22239581787
-    y: 4140946.3782204827
-    theta: -1.7968540507214439
-    kappa: -0.0026165045316263237
-    s: 0.71651156110683667
+    x: 587258.22302784468
+    y: 4140946.3809428588
+    theta: -1.7968422895950451
+    kappa: -0.0026023745341894242
+    s: 0.71371678195410526
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5800257151122552
-  a: -0.085355642675340737
+  v: 1.5623939136761802
+  a: -0.15663980124659133
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587258.215253128
-    y: 4140946.3474542471
-    theta: -1.7969869660909943
-    kappa: -0.0027761910665932378
-    s: 0.74809603951105685
+    x: 587258.21596816229
+    y: 4140946.3505341676
+    theta: -1.7969736603101092
+    kappa: -0.0027602053023153424
+    s: 0.74493420732079263
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5784734994515721
-  a: -0.0699432960525409
+  v: 1.5593920334396603
+  a: -0.14361657220852136
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587258.20811676839
-    y: 4140946.3167152791
-    theta: -1.7971197636972072
-    kappa: -0.0029357360745913136
-    s: 0.77965252522640172
+    x: 587258.20892147417
+    y: 4140946.3201814475
+    theta: -1.7971047892547385
+    kappa: -0.0029177455599966214
+    s: 0.77609417248354473
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5772249091792891
-  a: -0.0549920630593837
+  v: 1.5566465357774328
+  a: -0.13100059473143183
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587258.20098538685
-    y: 4140946.285997753
-    theta: -1.7972524687036069
-    kappa: -0.0030951697931778516
-    s: 0.81118699895422353
+    x: 587258.20188663923
+    y: 4140946.2898797835
+    theta: -1.7972356976615194
+    kappa: -0.0030750208220127772
+    s: 0.80720172402316925
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5762707844693713
-  a: -0.040495699040064115
+  v: 1.5541493326034312
+  a: -0.11878617430010807
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587258.19385767181
-    y: 4140946.25529602
-    theta: -1.7973851055080803
-    kappa: -0.0032545215400095755
-    s: 0.84270525944828545
+    x: 587258.19486255234
+    y: 4140946.2596244151
+    theta: -1.7973664060871628
+    kappa: -0.0032320557910576896
+    s: 0.83826174789765606
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5756020903888996
-  a: -0.026447959338776972
+  v: 1.5518924497218916
+  a: -0.1069676163993355
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587258.18673235259
-    y: 4140946.2246046057
-    theta: -1.7975176977533787
-    kappa: -0.0034138197254714479
-    s: 0.874212926012623
+    x: 587258.18784814444
+    y: 4140946.2294107387
+    theta: -1.7974969344220826
+    kappa: -0.0033888743692558573
+    s: 0.86927897171998258
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.575209916898072
-  a: -0.012842599299717095
+  v: 1.5498680268273555
+  a: -0.095539226513899289
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587258.17960819835
-    y: 4140946.1939182095
-    theta: -1.7976502683376214
-    kappa: -0.0035730918653055104
-    s: 0.90571544099940737
+    x: 587258.1808423819
+    y: 4140946.1992343003
+    theta: -1.7976273018999724
+    kappa: -0.0035454996696786555
+    s: 0.90025796703592031
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5750854788502016
-  a: 0.0003266257329206379
+  v: 1.5480683175046681
+  a: -0.084495310128584911
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587258.17248401779
-    y: 4140946.1632317007
-    theta: -1.797782839424797
-    kappa: -0.0037323645932397032
-    s: 0.93721807230680643
+    x: 587258.1738442654
+    y: 4140946.1690907963
+    theta: -1.7977575271073849
+    kappa: -0.0037019540278605905
+    s: 0.93120315160184053
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5752201159917181
-  a: 0.013065960414941125
+  v: 1.5464856892289793
+  a: -0.073830172728177546
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587258.16535865841
-    y: 4140946.1325401152
-    theta: -1.7979154324552682
-    kappa: -0.0038916636736167059
-    s: 0.96872591587684831
+    x: 587258.16685283033
+    y: 4140946.138976072
+    theta: -1.7978876279933105
+    kappa: -0.0038582590133155537
+    s: 0.96211879166252023
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5756052929621671
-  a: 0.02538164940214982
+  v: 1.5451126233657431
+  a: -0.0635381197974626
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587258.15823100635
-    y: 4140946.101838653
-    theta: -1.7980480681562752
-    kappa: -0.0040510140140227538
-    s: 1.0002438981932831
+    x: 587258.15986714559
+    y: 4140946.1088861162
+    theta: -1.798017621878756
+    kappa: -0.004014435441053091
+    s: 0.99300900422894944
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5762325992942108
-  a: 0.037279937350351511
+  v: 1.5439417151707178
+  a: -0.053613456821225314
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587258.151099985
-    y: 4140946.071122678
-    theta: -1.7981807665524392
-    kappa: -0.0042104396779164745
-    s: 1.0317767787794452
+    x: 587258.15288631339
+    y: 4140946.0788170621
+    theta: -1.7981475254663239
+    kappa: -0.0041705033830946436
+    s: 1.0238777593561352
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5770937494136266
-  a: 0.048767068915351541
+  v: 1.5429656737899657
+  a: -0.04405048928425126
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587258.14396455535
-    y: 4140946.0403877152
-    theta: -1.7983135469762686
-    kappa: -0.0043699638972577179
-    s: 1.0633291526961159
+    x: 587258.14590946841
+    y: 4140946.048765183
+    theta: -1.7982773548497939
+    kappa: -0.0043264821799898125
+    s: 1.05472888242091
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5781805826393087
-  a: 0.05984928875295481
+  v: 1.542177322259854
+  a: -0.034843522671325511
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587258.136823715
-    y: 4140946.0096294456
-    theta: -1.7984464280786618
-    kappa: -0.00452960908513638
-    s: 1.0949054530393854
+    x: 587258.13893577817
+    y: 4140946.0187268914
+    theta: -1.7984071255237002
+    kappa: -0.0044823904523326186
+    s: 1.085566056399736
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5794850631832675
-  a: 0.07053284151896666
+  v: 1.5415695975070538
+  a: -0.025986862467233585
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587258.12967649719
-    y: 4140945.9788437067
-    theta: -1.7985794278394143
-    kappa: -0.0046893968484012375
-    s: 1.1265099534385152
+    x: 587258.13196444116
+    y: 4140945.9886987368
+    theta: -1.7985368523929151
+    kappa: -0.0046382461122777482
+    s: 1.1163928241465118
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5809992801506287
-  a: 0.0808239718691921
+  v: 1.5411355503485404
+  a: -0.017474814156760554
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587258.12252197112
-    y: 4140945.9480264881
-    theta: -1.7987125635777224
-    kappa: -0.0048493480002887706
-    s: 1.1581467705538009
+    x: 587258.12499468739
+    y: 4140945.9586774022
+    theta: -1.798666549782227
+    kappa: -0.0047940663750568246
+    s: 1.1472125906703792
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5827154475396352
-  a: 0.090728924459436255
+  v: 1.5408683454915939
+  a: -0.0093016832246919368
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 587258.11535924068
-    y: 4140945.9171739304
-    theta: -1.7988458519626904
-    kappa: -0.0050094825730519982
-    s: 1.1898198665744335
+    x: 587258.11802577763
+    y: 4140945.9286597027
+    theta: -1.798796231445924
+    kappa: -0.0049498677704946535
+    s: 1.1780286254135277
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5846259042416451
-  a: 0.10025394394550424
+  v: 1.5407612615337982
+  a: -0.0014617751558131387
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 587258.10818744428
-    y: 4140945.8862823229
-    theta: -1.7989793090238371
-    kappa: -0.0051698198305893022
-    s: 1.2215330517163625
+    x: 587258.11105700256
+    y: 4140945.8986425828
+    theta: -1.7989259105773734
+    kappa: -0.00510566615452548
+    s: 1.2088440645290024
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5867231140411329
-  a: 0.10940527498320118
+  v: 1.5408076909630417
+  a: 0.0060506045650907669
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 587258.10100575408
-    y: 4140945.8553480986
-    theta: -1.7991129501616006
-    kappa: -0.005330378281073263
-    s: 1.2532899867201586
+    x: 587258.10408768256
+    y: 4140945.8686231161
+    theta: -1.799055599818604
+    kappa: -0.0052614767207092546
+    s: 1.2396619131585087
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5889996656156893
-  a: 0.11818916222833209
+  v: 1.5410011401575168
+  a: 0.013241150453234263
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 587258.09381337545
-    y: 4140945.8243678347
-    theta: -1.7992467901578457
-    kappa: -0.0054911756895794786
-    s: 1.2850941853488749
+    x: 587258.0971171672
+    y: 4140945.8385985009
+    theta: -1.7991853112698883
+    kappa: -0.0054173140117478773
+    s: 1.270485047710219
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5914482725360213
-  a: 0.12661185033670219
+  v: 1.5413352293857208
+  a: 0.020115557023832165
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 587258.08660954633
-    y: 4140945.7933382494
-    theta: -1.799380843186372
-    kappa: -0.0056522290907154056
-    s: 1.3169490168859097
+    x: 587258.0901448346
+    y: 4140945.8085660576
+    theta: -1.7993150564993243
+    kappa: -0.0055731919310014685
+    s: 1.3013162181365789
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5940617732659512
-  a: 0.13467958396411661
+  v: 1.5418036928064545
+  a: 0.026679518792099066
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 587258.079393537
-    y: 4140945.7622561995
-    theta: -1.7995151228234196
-    kappa: -0.0058135548012491749
-    s: 1.348857708632869
+    x: 587258.0831700908
+    y: 4140945.7785232291
+    theta: -1.799444846552418
+    kappa: -0.00572912375400461
+    s: 1.3321580502121124
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5968331311624182
-  a: 0.14239860776638036
+  v: 1.5424003784688238
+  a: 0.032938730273249561
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 587258.07216464914
-    y: 4140945.7311186772
-    theta: -1.7996496420581771
-    kappa: -0.0059751684327384348
-    s: 1.3808233484074295
+    x: 587258.0761923698
+    y: 4140945.7484675758
+    theta: -1.7995746919616662
+    kappa: -0.0058851221399826145
+    s: 1.36301304781123
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5997554344754774
-  a: 0.14977516639929866
+  v: 1.5431192483122385
+  a: 0.038898885982498466
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 587258.06492221542
-    y: 4140945.6999228075
-    theta: -1.7997844133032896
-    kappa: -0.0061370849041591695
-    s: 1.4128488870411993
+    x: 587258.06921113224
+    y: 4140945.7183967759
+    theta: -1.7997046027561392
+    kappa: -0.0060411991433677774
+    s: 1.3938835951860318
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6028218963482999
-  a: 0.15681550451867665
+  v: 1.5439543781664127
+  a: 0.044565680435060484
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 587258.05766559893
-    y: 4140945.6686658477
-    theta: -1.7999194484053667
-    kappa: -0.0062993184545345332
-    s: 1.4449371408775813
+    x: 587258.06222586543
+    y: 4140945.6883086204
+    theta: -1.7998345884710638
+    kappa: -0.0061973662253156309
+    s: 1.4247719592441159
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6060258548171729
-  a: 0.16352586678031922
+  v: 1.5448999577513645
+  a: 0.0499448081461501
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 587258.05039419257
-    y: 4140945.6373451827
-    theta: -1.8000547586554896
-    kappa: -0.0064618826555636749
-    s: 1.4770907942696352
+    x: 587258.05523608287
+    y: 4140945.6582010146
+    theta: -1.7999646581574071
+    kappa: -0.0063536342652212056
+    s: 1.4556802918263845
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6093607728114998
-  a: 0.16991249784003171
+  v: 1.5459502906774167
+  a: 0.055041963630982127
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 587258.04310741869
-    y: 4140945.605958323
-    theta: -1.800190354799722
-    kappa: -0.0066247904242505767
-    s: 1.5093124020779405
+    x: 587258.04824132344
+    y: 4140945.6280719703
+    theta: -1.8000948203914595
+    kappa: -0.0065100135722352855
+    s: 1.4866106319848487
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6128202381538
-  a: 0.17598164235361924
+  v: 1.5470997944451967
+  a: 0.059862841404771272
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 587258.03580472816
-    y: 4140945.5745029049
-    theta: -1.8003262470496155
-    kappa: -0.0067880540355328691
-    s: 1.5416043921684572
+    x: 587258.041241151
+    y: 4140945.5979196103
+    theta: -1.8002250832844189
+    kappa: -0.0066665138967806608
+    s: 1.5175649082604346
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.616397963559709
-  a: 0.18173954497688682
+  v: 1.5483430004456356
+  a: 0.064413135982732128
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 587258.0284856
-    y: 4140945.5429766839
-    theta: -1.8004624450927216
-    kappa: -0.0069516851349106738
-    s: 1.5739690679103906
+    x: 587258.03423515381
+    y: 4140945.5677421615
+    theta: -1.800355454491974
+    kappa: -0.0068231444420683938
+    s: 1.548544940960791
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6200877866379786
-  a: 0.18719245036563958
+  v: 1.5496745539599686
+  a: 0.068698541880079289
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 587258.02114954113
-    y: 4140945.5113775358
-    theta: -1.8005989581030981
-    kappa: -0.00711569475107542
-    s: 1.6064086106740505
+    x: 587258.02722294419
+    y: 4140945.5375379534
+    theta: -1.8004859412238894
+    kappa: -0.0069799138756140629
+    s: 1.5795524444380937
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6238836698904766
-  a: 0.19234660317568264
+  v: 1.5510892141597359
+  a: 0.072724753612027682
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587257.98419987818
-    y: 4140945.3522215243
-    theta: -1.8012865341357982
-    kappa: -0.007941765002527924
-    s: 1.7697974525872162
+    x: 587257.99205675349
+    y: 4140945.3860640237
+    theta: -1.801140330083526
+    kappa: -0.0077661116826218072
+    s: 1.73505489592548
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6442514188498889
-  a: 0.2138546414984317
+  v: 1.5592336587652831
+  a: 0.089167207823298789
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587257.9467678233
-    y: 4140945.1909876708
-    theta: -1.8019830835926509
-    kappa: -0.00877861991282543
-    s: 1.9353193974852856
+    x: 587257.95668997522
+    y: 4140945.233726088
+    theta: -1.801798448883118
+    kappa: -0.0085567939519652653
+    s: 1.891444331427055
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6664368609869302
-  a: 0.22883056372420563
+  v: 1.5687351331626367
+  a: 0.0999839851817983
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587257.90693495912
-    y: 4140945.0279844557
-    theta: -1.803641664397279
-    kappa: -0.0094738383235880089
-    s: 2.1031248550754955
+    x: 587257.92014239146
+    y: 4140945.0806489415
+    theta: -1.8029437444500902
+    kappa: -0.0092748288010958517
+    s: 2.0488295118950726
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6898258137906856
-  a: 0.23805495182864378
+  v: 1.5790666603866115
+  a: 0.10588690008936041
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587257.86553793843
-    y: 4140944.862914308
-    theta: -1.8058292746748112
-    kappa: -0.010097608423133302
-    s: 2.2733067197238714
+    x: 587257.8816014654
+    y: 4140944.9269674388
+    theta: -1.8049803919127629
+    kappa: -0.0098555632786140081
+    s: 2.2072700596572763
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7138821529478026
-  a: 0.24230838778738528
+  v: 1.5897724449122053
+  a: 0.10758776694781902
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587257.82355234784
-    y: 4140944.6954972451
-    theta: -1.8080480775489918
-    kappa: -0.010730247088580983
-    s: 2.4459081762749788
+    x: 587257.84279953793
+    y: 4140944.7722451957
+    theta: -1.807030915856835
+    kappa: -0.010440230525758043
+    s: 2.3667835765609184
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.738147812342494
-  a: 0.24237145357606982
+  v: 1.6004678726545998
+  a: 0.10579840015900932
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587257.78097787593
-    y: 4140944.5257320246
-    theta: -1.810298070991103
-    kappa: -0.011371759015722908
-    s: 2.6209305058716845
+    x: 587257.80374081735
+    y: 4140944.6164989932
+    theta: -1.8090950781675115
+    kappa: -0.011028767130789546
+    s: 2.5273527621167795
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.762242784056536
-  a: 0.2390247311703364
+  v: 1.6108395109691596
+  a: 0.10123061412476497
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 587257.73782250541
-    y: 4140944.3536504768
-    theta: -1.8125788127898239
-    kappa: -0.012022023920881125
-    s: 2.7983408917749117
+    x: 587257.76443627046
+    y: 4140944.4597725617
+    theta: -1.8111722828312675
+    kappa: -0.011621007844159549
+    s: 2.6889325316431854
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7858651183692686
-  a: 0.23304880254582427
+  v: 1.6206451086514326
+  a: 0.094596223246920408
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587257.6941006144
-    y: 4140944.1793099353
-    theta: -1.8148895208229188
-    kappa: -0.012680825151694199
-    s: 2.9780802251833949
+    x: 587257.72490189131
+    y: 4140944.3021296756
+    theta: -1.8132616674328155
+    kappa: -0.01221671166874682
+    s: 2.8514571344100257
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8087909237575963
-  a: 0.22522424967817312
+  v: 1.6297135959371514
+  a: 0.08660704192731028
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587257.64983107836
-    y: 4140944.0027856654
-    theta: -1.8172291733963701
-    kappa: -0.013347878297903539
-    s: 3.160070911053436
+    x: 587257.68515696935
+    y: 4140944.1436472526
+    theta: -1.8153621947013376
+    kappa: -0.012815587950096165
+    s: 3.0148472717827719
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8308743668959866
-  a: 0.21633165454302172
+  v: 1.6379450845022305
+  a: 0.0779748845677688
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587257.60503537022
-    y: 4140943.82416329
-    theta: -1.8195966096337055
-    kappa: -0.014022859802139749
-    s: 3.344224673918665
+    x: 587257.64522235841
+    y: 4140943.9844084475
+    theta: -1.8174727440907468
+    kappa: -0.01341732246665673
+    s: 3.1790172153664984
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8520476726564714
-  a: 0.20715159911600944
+  v: 1.6453108674627686
+  a: 0.069411565570129868
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587257.55973593332
-    y: 4140943.6435323032
-    theta: -1.8219906155765826
-    kappa: -0.014705431488009159
-    s: 3.5304492498355278
+    x: 587257.60511907062
+    y: 4140943.824497045
+    theta: -1.8195921861363804
+    kappa: -0.014021598604275617
+    s: 3.343880583997771
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8722760868216346
-  a: 0.1970894309357894
+  v: 1.6517994000550953
+  a: 0.059989657989065442
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587257.51395745063
-    y: 4140943.4609911256
-    theta: -1.8244098571908358
-    kappa: -0.01539522143263568
-    s: 3.7186431678797156
+    x: 587257.56487027952
+    y: 4140943.66400545
+    theta: -1.8217192767284256
+    kappa: -0.014628067184406984
+    s: 3.5093421120393455
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8914034715622909
-  a: 0.18516464431541321
+  v: 1.6572393122770446
+  a: 0.048483146901882782
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587257.46772888862
-    y: 4140943.2766552623
-    theta: -1.8268527724927006
-    kappa: -0.016091793171446094
-    s: 3.9086873483534714
+    x: 587257.52450392151
+    y: 4140943.5030450579
+    theta: -1.8238525197438245
+    kappa: -0.015236307263365032
+    s: 3.6752869543240472
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9092536551412866
-  a: 0.1715790413921186
+  v: 1.6614353403997941
+  a: 0.0351559647443704
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587257.41748632642
-    y: 4140943.0916202511
-    theta: -1.8300187329441329
-    kappa: -0.016668642904198779
-    s: 4.1004461033960107
+    x: 587257.48405236157
+    y: 4140943.341744924
+    theta: -1.8259901848558731
+    kappa: -0.015845831164138582
+    s: 3.8415820589739971
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.925670646035192
-  a: 0.15653442430314324
+  v: 1.6642186139381006
+  a: 0.02027204395231675
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 587257.36444123555
-    y: 4140942.9057169948
-    theta: -1.8336772365672873
-    kappa: -0.017166379124342119
-    s: 4.29376915500489
+    x: 587257.44283059647
+    y: 4140943.1804424636
+    theta: -1.8282708977768067
+    kappa: -0.016430830865089681
+    s: 4.0080788067249715
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9405186329343005
-  a: 0.14023259518572478
+  v: 1.6654466556502989
+  a: 0.0040953169615103591
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587257.31101160788
-    y: 4140942.7184660803
-    theta: -1.8373626130365071
-    kappa: -0.017667723557007315
-    s: 4.4884936530573851
+    x: 587257.39713525958
+    y: 4140943.0202973532
+    theta: -1.8314222924068986
+    kappa: -0.016859602386753991
+    s: 4.1746156502507574
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.95368198474263
-  a: 0.12287535617710092
+  v: 1.6650033815383039
+  a: -0.013110283792260272
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 587257.2572450228
-    y: 4140942.5300342562
-    theta: -1.8410714866381948
-    kappa: -0.018172229749683307
-    s: 4.6844461933318557
+    x: 587257.35147607012
+    y: 4140942.8602789263
+    theta: -1.8345714966988007
+    kappa: -0.017288034725310257
+    s: 4.3410207534875163
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9650652505779218
-  a: 0.1046645094145093
+  v: 1.6627991008476077
+  a: -0.031080825873206708
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 587257.20319140237
-    y: 4140942.3405964794
-    theta: -1.8448003153626447
-    kappa: -0.018679429272578353
-    s: 4.8814448355291242
+    x: 587257.30590227689
+    y: 4140942.7005597809
+    theta: -1.8377150504646265
+    kappa: -0.01771566577123659
+    s: 4.5071146309581325
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9745931597716408
-  a: 0.085801857035187612
+  v: 1.6587705160672828
+  a: -0.049552376845540425
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 587257.14890245674
-    y: 4140942.1503339764
-    theta: -1.8485454299560551
-    kappa: -0.019188836914287695
-    s: 5.0793011212938453
+    x: 587257.26046450238
+    y: 4140942.5413173307
+    theta: -1.8408493983244969
+    kappa: -0.018142020515862771
+    s: 4.6727127870965806
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9822106218689763
-  a: 0.066489201176373525
+  v: 1.6528807229299798
+  a: -0.068261004273472925
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 587257.09443113091
-    y: 4140941.9594322983
-    theta: -1.8523030730663919
-    kappa: -0.019699955877461223
-    s: 5.2778220922358781
+    x: 587257.21521402011
+    y: 4140942.38273127
+    theta: -1.8439709403300271
+    kappa: -0.018566617846666095
+    s: 4.8376283555722779
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9878827266288406
-  a: 0.046928343975304737
+  v: 1.6451192104119279
+  a: -0.086942775721215726
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587257.039831051
-    y: 4140941.7680793856
-    theta: -1.8560694384557066
-    kappa: -0.020212282974471165
-    s: 5.4768123079516595
+    x: 587257.1702020301
+    y: 4140942.2249810356
+    theta: -1.8470760826080048
+    kappa: -0.018988977342567218
+    s: 5.001674738614442
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.99159474402387
-  a: 0.027321087569218888
+  v: 1.6355018607329359
+  a: -0.10533375875298029
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587256.98515597044
-    y: 4140941.5764636239
-    theta: -1.8598407102498722
-    kappa: -0.020725313823079763
-    s: 5.6760758640455773
+    x: 587257.12547893391
+    y: 4140942.068243267
+    theta: -1.8501612879978337
+    kappa: -0.019408626069226012
+    s: 5.1646682463364542
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9933521242404248
-  a: 0.0078692340953536721
+  v: 1.6240709493563907
+  a: -0.12317002093297819
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587256.93045921612
-    y: 4140941.3847719026
-    theta: -1.8636131021956504
-    kappa: -0.021238548042106922
-    s: 5.8754184101513385
+    x: 587257.08109361166
+    y: 4140941.9126892686
+    theta: -1.8532231266569998
+    kappa: -0.01982510537433739
+    s: 5.3264307360602086
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9931804976785894
-  a: -0.011225414309053244
+  v: 1.6108951449892586
+  a: -0.14018762982542093
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587256.871772385
-    y: 4140941.1944279708
-    theta: -1.8677830890596159
-    kappa: -0.021673109357608772
-    s: 6.07464916795335
+    x: 587257.03709269664
+    y: 4140941.7584824753
+    theta: -1.8562583266100912
+    kappa: -0.020237977682927177
+    s: 5.4867922516404786
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9911256749521709
-  a: -0.029761055506764167
+  v: 1.5960695095820847
+  a: -0.15612265299452002
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 587256.80910533806
-    y: 4140941.0056225406
-    theta: -1.8723522097006648
-    kappa: -0.022027722611364986
-    s: 6.2735829492080786
+    x: 587256.99351985217
+    y: 4140941.605775909
+    theta: -1.8592638242186892
+    kappa: -0.02064683329264794
+    s: 5.6455936627892687
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.987253646888701
-  a: -0.047535887360541484
+  v: 1.5797154983289925
+  a: -0.17071115800448694
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587256.74658778333
-    y: 4140940.8172675055
-    theta: -1.8769110970411635
-    kappa: -0.022381489934915254
-    s: 6.4720421737654368
+    x: 587256.95041504642
+    y: 4140941.454709644
+    theta: -1.8622368145517534
+    kappa: -0.021051297169074848
+    s: 5.8026893044001771
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.981650584529435
-  a: -0.064348107733147364
+  v: 1.5619809596676846
+  a: -0.18368921241953315
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587256.68427262886
-    y: 4140940.6295222687
-    theta: -1.8814556989085216
-    kappa: -0.022734111938550315
-    s: 6.6698588875901459
+    x: 587256.90781382914
+    y: 4140941.3054082687
+    theta: -1.8651748016387686
+    kappa: -0.021451035741001493
+    s: 5.9579496158727485
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9744228391293517
-  a: -0.079995914487344333
+  v: 1.5430401352794427
+  a: -0.19479288380387036
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587256.62220911472
-    y: 4140940.4425351825
-    theta: -1.8859822326143416
-    kappa: -0.023085309989040747
-    s: 6.8668767807831106
+    x: 587256.86023824732
+    y: 4140941.1596775241
+    theta: -1.8686239963693236
+    kappa: -0.021738377442224156
+    s: 6.111263780436837
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9656969421571542
-  a: -0.094277505485894586
+  v: 1.5230936600891269
+  a: -0.20375823972170978
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587256.56044217665
-    y: 4140940.2564416314
-    theta: -1.8904872329364797
-    kappa: -0.023434829806899943
-    s: 7.0629532056027937
+    x: 587256.81258328352
+    y: 4140941.0161010139
+    theta: -1.8720986097390884
+    kappa: -0.022008042003827246
+    s: 6.26254236447696
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.955619605295269
-  a: -0.10699107859156048
+  v: 1.5023685622651768
+  a: -0.21032134773726316
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 587256.4990118118
-    y: 4140940.0713621192
-    theta: -1.8949675998902873
-    kappa: -0.023782445063647059
-    s: 7.2579611944865858
+    x: 587256.76559016283
+    y: 4140940.8745185267
+    theta: -1.8755253518666297
+    kappa: -0.022273961402169879
+    s: 6.4117199568566585
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9443577204398454
-  a: -0.11793483166710581
+  v: 1.4811182632196103
+  a: -0.2142182754147422
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587256.43795279309
-    y: 4140939.8874014122
-    theta: -1.8994206205726483
-    kappa: -0.024127958986862187
-    s: 7.4517903604674913
+    x: 587256.71927187638
+    y: 4140940.7349692034
+    theta: -1.878903180426247
+    kappa: -0.022536062123842827
+    s: 6.5587553174352511
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9320534906659643
-  a: -0.12826127598246681
+  v: 1.4595229460658634
+  a: -0.21817399848462335
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587256.37729799142
-    y: 4140939.7046585428
-    theta: -1.9038437279591913
-    kappa: -0.024471185573000608
-    s: 7.644336357788303
+    x: 587256.67364160065
+    y: 4140940.5974927424
+    theta: -1.8822310462424166
+    kappa: -0.022794269608454841
+    s: 6.70360661872771
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9186832292325822
-  a: -0.13925480062092438
+  v: 1.4373906914711507
+  a: -0.22491288310055205
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 587256.31708203675
-    y: 4140939.5232378477
-    theta: -1.9082342346753796
-    kappa: -0.024811928860785953
-    s: 7.8354892545238171
+    x: 587256.62872049434
+    y: 4140940.4621528909
+    theta: -1.885507324558124
+    kappa: -0.023048464124139877
+    s: 6.84620669259354
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9041804273782506
-  a: -0.15091142016525175
+  v: 1.4144564256209196
+  a: -0.23417008246149237
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587256.25454064657
-    y: 4140939.3442574544
-    theta: -1.9127431842005689
-    kappa: -0.025120056831952304
-    s: 8.0251324877997732
+    x: 587256.58453764964
+    y: 4140940.3290372994
+    theta: -1.8887298186079897
+    kappa: -0.023298481048326051
+    s: 6.9864631877448042
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.888478974883244
-  a: -0.16322714919822215
+  v: 1.39048155938072
+  a: -0.24568074976640858
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587256.18653640046
-    y: 4140939.1689767116
-    theta: -1.9175434027324454
-    kappa: -0.02536141715072731
-    s: 8.213142903647018
+    x: 587256.54112925718
+    y: 4140940.1982550044
+    theta: -1.8918958205826386
+    kappa: -0.023544115588813426
+    s: 7.1242612182141185
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8715131600695591
-  a: -0.17619800230260876
+  v: 1.3652539882962067
+  a: -0.25918003821426466
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587256.1191696669
-    y: 4140938.9953391552
-    theta: -1.9222994876495316
-    kappa: -0.025600514812808665
-    s: 8.3993907968556929
+    x: 587256.49853777234
+    y: 4140940.0699339164
+    theta: -1.8950021727193744
+    kappa: -0.023785127504851823
+    s: 7.2594660118226706
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.853217669800916
-  a: -0.18981999406118477
+  v: 1.3385880925931373
+  a: -0.27440310100402487
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587256.05248971633
-    y: 4140938.8234717795
-    theta: -1.9270077234032805
-    kappa: -0.025837174948729783
-    s: 8.5837399508293917
+    x: 587256.45681108022
+    y: 4140939.9442183068
+    theta: -1.8980453284729832
+    kappa: -0.024021245828218629
+    s: 7.3919255586482322
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8335275894827567
-  a: -0.20408913905672338
+  v: 1.310324737177373
+  a: -0.29108509133465321
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 587255.9865481595
-    y: 4140938.6535076117
-    theta: -1.9316642403004469
-    kappa: -0.026071214381127368
-    s: 8.7660476774393388
+    x: 587256.416001662
+    y: 4140939.8212662907
+    theta: -1.9010214137227306
+    kappa: -0.024252173584296589
+    s: 7.5214732594931633
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.812378403062247
-  a: -0.21900145187199777
+  v: 1.2803312716348789
+  a: -0.3089611624051139
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 587255.92139893374
-    y: 4140938.4855856756
-    theta: -1.9362650167973574
-    kappa: -0.026302441675904634
-    s: 8.9461648568785677
+    x: 587256.37616576068
+    y: 4140939.7012473196
+    theta: -1.9039262879742165
+    kappa: -0.024477592513151604
+    s: 7.6479305743524293
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7897059930282737
-  a: -0.23455294708978122
+  v: 1.2485015302317237
+  a: -0.327766467414371
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587255.85709828848
-    y: 4140938.3198509528
-    theta: -1.9408058817950025
-    kappa: -0.026530657193394477
-    s: 9.1239359775160747
+    x: 587256.337362546
+    y: 4140939.5843396615
+    theta: -1.9067556055178141
+    kappa: -0.024697167790610527
+    s: 7.7711096708816028
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7654466404114482
-  a: -0.25073963929284687
+  v: 1.2147558319140797
+  a: -0.3472361595613887
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 587255.79370477051
-    y: 4140938.156454348
-    theta: -1.94528251691772
-    kappa: -0.026755653139522703
-    s: 9.2991991757510135
+    x: 587256.29965328076
+    y: 4140939.4707278935
+    theta: -1.9095048765091143
+    kappa: -0.024910552749339007
+    s: 7.8908160728648813
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7395370247841027
-  a: -0.26755754306396795
+  v: 1.1790409803082222
+  a: -0.36710539204513115
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 587255.73127920972
-    y: 4140937.9955526497
-    theta: -1.9496904587575017
-    kappa: -0.026977213616971238
-    s: 9.4717862758668545
+    x: 587256.26115303463
+    y: 4140939.3613008652
+    theta: -1.91227649038547
+    kappa: -0.025096588178902951
+    s: 8.0068513086830961
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7119142242602936
-  a: -0.28500267298591753
+  v: 1.1413302637205309
+  a: -0.38710931806456245
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 587255.66988470475
-    y: 4140937.8373084944
-    theta: -1.9540251010653267
-    kappa: -0.027195114676341323
-    s: 9.6415228298855578
+    x: 587256.22058270068
+    y: 4140939.2567309495
+    theta: -1.9151400515458257
+    kappa: -0.025240580198059585
+    s: 8.1190155597817188
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6825157154957993
-  a: -0.30307104364146908
+  v: 1.1016234551374882
+  a: -0.40698309081864659
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 587255.6095866086
-    y: 4140937.6818903303
-    theta: -1.9582816968704058
-    kappa: -0.027409124367316736
-    s: 9.80822815742175
+    x: 587256.18148432462
+    y: 4140939.1559550012
+    theta: -1.9179000515831317
+    kappa: -0.025379347952203306
+    s: 8.22711030913888
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6512793736881211
-  a: -0.32175866961339555
+  v: 1.059946812225681
+  a: -0.42646186350634785
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 587255.54950291268
-    y: 4140937.5298709846
-    theta: -1.9624857985724098
-    kappa: -0.027614792128368926
-    s: 9.971715385536891
+    x: 587256.14392828208
+    y: 4140939.0591544122
+    theta: -1.9205514452299846
+    kappa: -0.025512641663905274
+    s: 8.3309409897333637
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6181434725764823
-  a: -0.34106156548447025
+  v: 1.0163530773317995
+  a: -0.4452807893266304
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 587255.48353552422
-    y: 4140937.3840194764
-    theta: -1.9668311232824172
-    kappa: -0.027784520807544354
-    s: 10.131791488593453
+    x: 587256.10798256227
+    y: 4140938.9665044257
+    theta: -1.9230893645699416
+    kappa: -0.025640220026589191
+    s: 8.4303196330126386
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5830466844418298
-  a: -0.36097574583746644
+  v: 0.97092147748263724
+  a: -0.46317502147845818
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 587255.41905592592
-    y: 4140937.2414574181
-    theta: -1.9710792540597777
-    kappa: -0.027950421524578172
-    s: 10.288257328109086
+    x: 587256.0737118097
+    y: 4140938.8781716628
+    theta: -1.925509186647308
+    kappa: -0.025761853604529483
+    s: 8.5250675173608563
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5459280801068327
-  a: -0.38149722525515728
+  v: 0.92375772438509129
+  a: -0.47987971316079547
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 587255.35614868556
-    y: 4140937.1023717867
-    theta: -1.9752244149449303
-    kappa: -0.0281122766937621
-    s: 10.440907692610796
+    x: 587256.041176366
+    y: 4140938.7943116529
+    theta: -1.9278066009342498
+    kappa: -0.025877328232849578
+    s: 8.6150178165668549
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.506727128935883
-  a: -0.40262201832031586
+  v: 0.87499401442616265
+  a: -0.49513001757260644
   relative_time: 5.7999999999999963
 }
 trajectory_point {
   path_point {
-    x: 587255.294900857
-    y: 4140936.9669550546
-    theta: -1.9792606856426636
-    kappa: -0.028269862332453717
-    s: 10.589531337489111
+    x: 587256.01043131249
+    y: 4140938.7150663687
+    theta: -1.9299776766531362
+    kappa: -0.025986448417520139
+    s: 8.7000182482921886
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.465383698835095
-  a: -0.42434613961571543
+  v: 0.82478902867295578
+  a: -0.508661087912855
   relative_time: 5.8999999999999959
 }
 trajectory_point {
   path_point {
-    x: 587255.235401964
-    y: 4140936.835405156
-    theta: -1.9831820033060312
-    kappa: -0.028422948103333897
-    s: 10.733911024852256
+    x: 587255.981525512
+    y: 4140938.640561753
+    theta: -1.93201892995966
+    kappa: -0.02608904073535729
+    s: 8.77993372253912
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4218380562523063
-  a: -0.44666560372412933
+  v: 0.77332793287267854
+  a: -0.52020807738050556
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 587255.17774398392
-    y: 4140936.7079254491
-    theta: -1.9869821641352945
-    kappa: -0.028571297356664114
-    s: 10.873823563380332
+    x: 587255.95450065052
+    y: 4140938.5709052528
+    theta: -1.9339273909993768
+    kappa: -0.026184957234020881
+    s: 8.8546489901186423
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3760308661770766
-  a: -0.4695764252283306
+  v: 0.72082237745264288
+  a: -0.52950613917452194
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 587255.12202133075
-    y: 4140936.5847246796
-    theta: -1.9906548247728593
-    kappa: -0.02871466717254386
-    s: 11.009039848179475
+    x: 587255.92939027981
+    y: 4140938.5061833458
+    theta: -1.9357006708565716
+    kappa: -0.026274078832012715
+    s: 8.9240712911184819
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3279031921406885
-  a: -0.49307461871109254
+  v: 0.66751049752026348
+  a: -0.53629042649386849
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
-    x: 587255.06833083916
-    y: 4140936.4660169464
-    theta: -1.9941935034766956
-    kappa: -0.028852808403168031
-    s: 11.139324900636039
+    x: 587255.906218859
+    y: 4140938.4464590778
+    theta: -1.937337028419531
+    kappa: -0.026356318718674793
+    s: 8.9881330033711127
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.2773964962161473
-  a: -0.51715619875518826
+  v: 0.61365691286305957
+  a: -0.54029609253750932
   relative_time: 6.2999999999999945
 }
 trajectory_point {
   path_point {
-    x: 587255.01677174808
-    y: 4140936.3520216616
-    theta: -1.997591581056581
-    kappa: -0.028985465715084251
-    s: 11.264437908270764
+    x: 587255.88500079711
+    y: 4140938.3917695861
+    theta: -1.9388354371901695
+    kappa: -0.026431625754187553
+    s: 9.0467942909217633
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.2244526390181809
-  a: -0.54181717994339107
+  v: 0.55955272794865341
+  a: -0.54125829050440843
   relative_time: 6.3999999999999941
 }
 trajectory_point {
   path_point {
-    x: 587254.96744568413
-    y: 4140936.24296352
-    theta: -2.0008423015597541
-    kappa: -0.029112377631450304
-    s: 11.384132264592951
+    x: 587255.86573949456
+    y: 4140938.342123636
+    theta: -1.9401956520683226
+    kappa: -0.026499987869568125
+    s: 9.1000457524964382
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.16901387970324
-  a: -0.56705357685847413
+  v: 0.50551553192477083
+  a: -0.53891217359353016
   relative_time: 6.4999999999999938
 }
 trajectory_point {
   path_point {
-    x: 587254.92045664508
-    y: 4140936.1390724555
-    theta: -2.0039387726951778
-    kappa: -0.029233276574291471
-    s: 11.498155608954628
+    x: 587255.84842638555
+    y: 4140938.2974991482
+    theta: -1.9414182761417871
+    kappa: -0.026561435466668545
+    s: 9.1479110699699078
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.111022875969498
-  a: -0.59286140408321064
+  v: 0.45188939861924143
+  a: -0.53299289500383851
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
-    x: 587254.87591098377
-    y: 4140936.0405836105
-    theta: -2.0068739659886834
-    kappa: -0.029347888906757935
-    s: 11.606249866404729
+    x: 587255.8330399798
+    y: 4140938.257840733
+    theta: -1.942504827512278
+    kappa: -0.02661604481817402
+    s: 9.19044965683374
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0504226840568511
-  a: -0.6192366762003737
+  v: 0.39904488653999853
+  a: -0.52323560793429735
   relative_time: 6.6999999999999931
 }
 trajectory_point {
   path_point {
-    x: 587254.83391739137
-    y: 4140935.9477372975
-    theta: -2.0096407166647339
-    kappa: -0.029455934975382129
-    s: 11.708151287543263
+    x: 587255.8195449051
+    y: 4140938.2230572179
+    theta: -1.9434578061849517
+    kappa: -0.026663941467601163
+    s: 9.2277593066642964
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.987156758746917
-  a: -0.64617540779273663
+  v: 0.34737903887507865
+  a: -0.50937546558387137
   relative_time: 6.7999999999999927
 }
 trajectory_point {
   path_point {
-    x: 587254.794586881
-    y: 4140935.8607789623
-    theta: -2.01223172325455
-    kappa: -0.029557129152336131
-    s: 11.803590488375491
+    x: 587255.80789094872
+    y: 4140938.1930191796
+    theta: -1.9442807610450903
+    kappa: -0.026705303629296229
+    s: 9.2599788415907476
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.92116895336303828
-  a: -0.67367361344307253
+  v: 0.297315383492622
+  a: -0.49114762115152422
   relative_time: 6.8999999999999924
 }
 trajectory_point {
   path_point {
-    x: 587254.75803277141
-    y: 4140935.7799591492
-    theta: -2.0146395469348093
-    kappa: -0.029651179877689022
-    s: 11.892292490166083
+    x: 587255.79801209993
+    y: 4140938.1675564763
+    theta: -1.9449783569401953
+    kappa: -0.026740365588433374
+    s: 9.2872907607630868
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.85240351977027817
-  a: -0.70172730773415481
+  v: 0.24930393294087266
+  a: -0.46828722783622029
   relative_time: 6.999999999999992
 }
 trajectory_point {
   path_point {
-    x: 587254.72247635608
-    y: 4140935.70645846
-    theta: -2.0171087994159724
-    kappa: -0.029738589828901461
-    s: 11.973976759293318
+    x: 587255.78982559172
+    y: 4140938.1464557759
+    theta: -1.9455564418793545
+    kappa: -0.026769421101012859
+    s: 9.3099238888201352
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.78080510837542372
-  a: -0.73033250524875659
+  v: 0.20382118444817821
+  a: -0.44052943883692353
   relative_time: 7.0999999999999917
 }
 trajectory_point {
   path_point {
-    x: 587254.68788298417
-    y: 4140935.6406120053
-    theta: -2.0196525496064912
-    kappa: -0.029819120135374175
-    s: 12.048357247103224
+    x: 587255.783230943
+    y: 4140938.1294580884
+    theta: -1.946022114354711
+    kappa: -0.026792826793859342
+    s: 9.32815602435755
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.70631876812698424
-  a: -0.759485220569651
+  v: 0.16137011992298947
+  a: -0.40760940735259821
   relative_time: 7.1999999999999913
 }
 trajectory_point {
   path_point {
-    x: 587254.65682208736
-    y: 4140935.58148941
-    theta: -2.0219368794975967
-    kappa: -0.029891427152790292
-    s: 12.115142429763777
+    x: 587255.77810900076
+    y: 4140938.1162562976
+    theta: -1.9463837907825949
+    kappa: -0.026811005564620072
+    s: 9.3423165883958461
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.62888994651519181
-  a: -0.78918146827961111
+  v: 0.12248020595386092
+  a: -0.36926228658220828
   relative_time: 7.2999999999999909
 }
 trajectory_point {
   path_point {
-    x: 587254.62943177717
-    y: 4140935.5293535609
-    theta: -2.0239515027960477
-    kappa: -0.029955189369251473
-    s: 12.174035348119057
+    x: 587255.77432098193
+    y: 4140938.1064926903
+    theta: -1.9466512730548369
+    kappa: -0.026824449981763164
+    s: 9.3527892728483923
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.54846448957200133
-  a: -0.81941726296141026
+  v: 0.087707393809451431
+  a: -0.32522322972471807
   relative_time: 7.3999999999999906
 }
 trajectory_point {
   path_point {
-    x: 587254.605852675
-    y: 4140935.4844721225
-    theta: -2.0256859708411539
-    kappa: -0.030010079431289305
-    s: 12.224733647543429
+    x: 587255.77170751512
+    y: 4140938.0997564872
+    theta: -1.9468358161844328
+    kappa: -0.026833725684575822
+    s: 9.36001468898943
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.46498864187109024
-  a: -0.85018861919782174
+  v: 0.057634119438522244
+  a: -0.27522738997909169
   relative_time: 7.49999999999999
 }
 trajectory_point {
   path_point {
-    x: 587254.58622789255
-    y: 4140935.4471175
-    theta: -2.0271296717042175
-    kappa: -0.030055764187014673
-    s: 12.266929617795707
+    x: 587255.77008768334
+    y: 4140938.0955813751
+    theta: -1.9469501960245224
+    kappa: -0.026839474783162585
+    s: 9.3644930159220845
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.37840904652785806
-  a: -0.88149155157161874
+  v: 0.032869303469939139
+  a: -0.21900992054429311
   relative_time: 7.59999999999999
 }
 trajectory_point {
   path_point {
-    x: 587254.57070301415
-    y: 4140935.4175668042
-    theta: -2.0282718294033506
-    kappa: -0.030091904729267087
-    s: 12.300310232873336
+    x: 587255.7692580655
+    y: 4140938.0934430379
+    theta: -1.9470087770358671
+    kappa: -0.026842419258443575
+    s: 9.3667866490463716
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.28867274519942837
-  a: -0.91332207466557425
+  v: 0.014048351212670496
+  a: -0.15630597461928658
   relative_time: 7.6999999999999895
 }
 trajectory_point {
   path_point {
-    x: 587254.55942607764
-    y: 4140935.3961018175
-    theta: -2.0291015033735187
-    kappa: -0.030118156438764011
-    s: 12.324557190866557
+    x: 587255.76899177872
+    y: 4140938.0927566844
+    theta: -1.9470275800758843
+    kappa: -0.026843364362152729
+    s: 9.3675228485272086
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.19572717808464601
-  a: -0.94567620306246181
+  v: 0.0018331526557899443
+  a: -0.08685070540303623
   relative_time: 7.7999999999999892
 }
 trajectory_point {
   path_point {
-    x: 587254.55254755635
-    y: 4140935.383008955
-    theta: -2.0296075883547409
-    kappa: -0.030134169027250204
-    s: 12.339346953812582
+    x: 587255.76903752016
+    y: 4140938.0928745829
+    theta: -1.9470243501816824
+    kappa: -0.02684320201683605
+    s: 9.3673963877624278
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.099520183924079728
-  a: -0.97854995134505429
+  v: 0
+  a: -0.010379266094506132
   relative_time: 7.8999999999999888
 }
 trajectory_point {
   path_point {
-    x: 587254.55022034
-    y: 4140935.3785792352
-    theta: -2.0297788148864213
-    kappa: -0.030139586580647054
-    s: 12.344350787549768
+    x: 587255.76911860926
+    y: 4140938.09308359
+    theta: -1.9470186243200278
+    kappa: -0.026842914215849827
+    s: 9.3671722018507761
     dkappa: 0
     ddkappa: 0
   }
-  v: 9.2148511043560622e-14
-  a: -1.0119393340961254
+  v: 5.8386510800344173e-15
+  a: 0.073373190107339586
   relative_time: 7.9999999999999885
 }
 decision {
@@ -1706,10 +1705,10 @@ decision {
       reason_code: STOP_REASON_OBSTACLE
       reason: "stop by 867_0"
       stop_point {
-        x: 587252.94799968787
-        y: 4140931.8336123093
+        x: 587254.452385788
+        y: 4140934.42768598
       }
-      stop_heading: -2.1420620124130152
+      stop_heading: -2.0447991987911851
       change_lane_type: FORWARD
     }
   }
@@ -1720,12 +1719,12 @@ decision {
       object_decision {
         stop {
           reason_code: STOP_REASON_OBSTACLE
-          distance_s: -3
+          distance_s: -6
           stop_point {
-            x: 587252.94799968787
-            y: 4140931.8336123093
+            x: 587254.452385788
+            y: 4140934.42768598
           }
-          stop_heading: -2.1420620124130152
+          stop_heading: -2.0447991987911851
         }
       }
     }
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 823524556c..e339b063f7 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
@@ -1,7 +1,7 @@
 header {
   module_name: "planning"
 }
-total_path_length: 2.9900088448548212
+total_path_length: 0
 total_path_time: 7.9999999999999885
 is_replan: true
 gear: GEAR_DRIVE
@@ -9,1694 +9,1815 @@ trajectory_point {
   path_point {
     x: 587045.66209810227
     y: 4141549.06192431
+    z: 0
     theta: 1.2103929796890078
     kappa: -0.011166695847059904
-    s: 3.4327995420950649e-23
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.7111110687255859
-  a: -0.946324410876753
+  v: 0
+  a: 0
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 587045.68193636963
-    y: 4141549.1121817119
-    theta: 1.2095298969760315
-    kappa: -0.011428362362098576
-    s: 0.054031132824821838
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.69191174609583
-  a: -0.9734648349690106
+  v: 0
+  a: 0
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 587045.7016316792
-    y: 4141549.162076951
-    theta: 1.2086730113741315
-    kappa: -0.011688143264095282
-    s: 0.10767290819322019
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.6721781708396479
-  a: -0.999750932137899
+  v: 0
+  a: 0
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 587045.72118017019
-    y: 4141549.2116002459
-    theta: 1.2078224925834364
-    kappa: -0.011945987631621954
-    s: 0.16091481141460379
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.6519273539783947
-  a: -1.0251902540946847
+  v: 0
+  a: 0
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 587045.74057810626
-    y: 4141549.2607421316
-    theta: 1.2069785048504376
-    kappa: -0.012201846183584207
-    s: 0.21374666650846622
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.6311761554991997
-  a: -1.0497903525506345
+  v: 0
+  a: 0
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 587045.75982187479
-    y: 4141549.309493457
-    theta: 1.2061412070140074
-    kappa: -0.01245567126459253
-    s: 0.26615863318370131
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.6099412843549663
-  a: -1.0735587792170147
+  v: 0
+  a: 0
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 587045.77890798461
-    y: 4141549.3578453753
-    theta: 1.2053107525516418
-    kappa: -0.012707416830333445
-    s: 0.318141203817919
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.5882392984643743
-  a: -1.0965030858050921
+  v: 0
+  a: 0
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 587045.79783306608
-    y: 4141549.405789352
-    theta: 1.2044872896259273
-    kappa: -0.012957038432940698
-    s: 0.36968520043676018
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.5660866047118764
-  a: -1.1186308240261333
+  v: 0
+  a: 0
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 587045.81659386912
-    y: 4141549.4533171528
-    theta: 1.203670961131226
-    kappa: -0.013204493206366415
-    s: 0.42078177169321268
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.5434994589477009
-  a: -1.1399495455914048
+  v: 0
+  a: 0
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 587045.83518726286
-    y: 4141549.500420846
-    theta: 1.2028619047405682
-    kappa: -0.013449739851752296
-    s: 0.47142238984692675
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.5204939659878502
-  a: -1.1604668022121731
+  v: 0
+  a: 0
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 587045.85361023364
-    y: 4141549.5470927972
-    theta: 1.2020602529527524
-    kappa: -0.013692738622800776
-    s: 0.52159884774353049
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.4970860796141019
-  a: -1.180190145599705
+  v: 0
+  a: 0
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 587045.87185988517
-    y: 4141549.5933256694
-    theta: 1.2012661331396406
-    kappa: -0.013933451311146211
-    s: 0.571303255793945
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.4732916025740086
-  a: -1.1991271274652671
+  v: 0
+  a: 0
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587045.88993343571
-    y: 4141549.6391124148
-    theta: 1.2004796675936447
-    kappa: -0.014171841231726042
-    s: 0.62052803895370046
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.4491261865808962
-  a: -1.2172852995201258
+  v: 0
+  a: 0
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 587045.90782821854
-    y: 4141549.6844462776
-    theta: 1.199700973575397
-    kappa: -0.014407873208151976
-    s: 0.66926593370225118
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.4246053323138663
-  a: -1.2346722134755479
+  v: 0
+  a: 0
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587045.92554168
-    y: 4141549.7293207888
-    theta: 1.1989301633615954
-    kappa: -0.014641513558081163
-    s: 0.71750998502229135
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.3997443894177954
-  a: -1.2512954210427998
+  v: 0
+  a: 0
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587045.94307137863
-    y: 4141549.7737297635
-    theta: 1.1981673442930199
-    kappa: -0.014872730078587366
-    s: 0.76525354337907026
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.3745585565033336
-  a: -1.2671624739331484
+  v: 0
+  a: 0
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 587045.960414984
-    y: 4141549.8176672976
-    theta: 1.1974126188227066
-    kappa: -0.015101492031532139
-    s: 0.81249026169970817
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.349062881146907
-  a: -1.2822809238578599
+  v: 0
+  a: 0
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 587045.97757027578
-    y: 4141549.8611277658
-    theta: 1.1966660845642791
-    kappa: -0.015327770128936003
-    s: 0.85921409235251145
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.323272259890715
-  a: -1.2966583225282011
+  v: 0
+  a: 0
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 587045.99453514209
-    y: 4141549.9041058188
-    theta: 1.1959278343404249
-    kappa: -0.015551536518349616
-    s: 0.90541928412628814
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.297201438242733
-  a: -1.3103022216554385
+  v: 0
+  a: 0
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587046.01130757935
-    y: 4141549.9465963808
-    theta: 1.1951979562315087
-    kappa: -0.015772764768224951
-    s: 0.95110037920966373
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.27086501067671
-  a: -1.3232201729508388
+  v: 0
+  a: 0
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587046.02788568987
-    y: 4141549.9885946428
-    theta: 1.1944765336243182
-    kappa: -0.015991429853286474
-    s: 0.99625221017039622
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.24427742063217
-  a: -1.3354197281256688
+  v: 0
+  a: 0
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587046.04426768213
-    y: 4141550.030096069
-    theta: 1.1937636452609337
-    kappa: -0.016207508139902318
-    s: 1.0408698969346923
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.2174529605144118
-  a: -1.3469084388911947
+  v: 0
+  a: 0
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 587046.06045186869
-    y: 4141550.0710963816
-    theta: 1.1930593652877113
-    kappa: -0.016420977371455443
-    s: 1.0849488437665216
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.1904057716945089
-  a: -1.3576938569586829
+  v: 0
+  a: 0
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587046.07643666549
-    y: 4141550.1115915691
-    theta: 1.1923637633043753
-    kappa: -0.016631816653714847
-    s: 1.1284847362469339
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.1631498445093094
-  a: -1.3677835340394009
+  v: 0
+  a: 0
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587046.09222059045
-    y: 4141550.1515778769
-    theta: 1.1916769044132127
-    kappa: -0.016840006440206695
-    s: 1.1714735382533727
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.1356990182614362
-  a: -1.3771850218446144
+  v: 0
+  a: 0
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587046.107802263
-    y: 4141550.1910518073
-    theta: 1.1909988492683572
-    kappa: -0.017045528517585527
-    s: 1.2139114889389926
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.1080669812192863
-  a: -1.3859058720855906
+  v: 0
+  a: 0
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587046.123180402
-    y: 4141550.2300101141
-    theta: 1.1903296541251631
-    kappa: -0.017248365991005432
-    s: 1.2557950997119733
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.0802672706170324
-  a: -1.3939536364735954
+  v: 0
+  a: 0
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587046.13835382543
-    y: 4141550.2684498038
-    theta: 1.1896693708896557
-    kappa: -0.017448503269491197
-    s: 1.297121151214836
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.0523132726546205
-  a: -1.4013358667198961
+  v: 0
+  a: 0
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587046.15332144918
-    y: 4141550.3063681293
-    theta: 1.1890180471680538
-    kappa: -0.017645926051309511
-    s: 1.3378866903037583
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.0242182224977725
-  a: -1.408060114535759
+  v: 0
+  a: 0
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587046.16808228544
-    y: 4141550.3437625887
-    theta: 1.1883757263163575
-    kappa: -0.017840621309340124
-    s: 1.3780890270278907
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9959952042779847
-  a: -1.4141339316324506
+  v: 0
+  a: 0
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587046.18263544212
-    y: 4141550.3806309216
-    theta: 1.1877424474899945
-    kappa: -0.018032577276447019
-    s: 1.41772573160867
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9676571510925274
-  a: -1.4195648697212377
+  v: 0
+  a: 0
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587046.19698012155
-    y: 4141550.416971107
-    theta: 1.1871182456935163
-    kappa: -0.018221783430849611
-    s: 1.4567946314191376
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.939216845004446
-  a: -1.4243604805133867
+  v: 0
+  a: 0
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587046.2111156194
-    y: 4141550.4527813615
-    theta: 1.1865031518303411
-    kappa: -0.018408230481493884
-    s: 1.4952938079632523
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9106869170425607
-  a: -1.4285283157201645
+  v: 0
+  a: 0
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587046.22504132334
-    y: 4141550.4880601331
-    theta: 1.185897192752535
-    kappa: -0.018591910353423606
-    s: 1.5332215938552083
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8820798472014668
-  a: -1.4320759270528371
+  v: 0
+  a: 0
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587046.23875671229
-    y: 4141550.5228061015
-    theta: 1.1853003913106268
-    kappa: -0.018772816173151477
-    s: 1.5705765697987488
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8534079644415331
-  a: -1.4350108662226719
+  v: 0
+  a: 0
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587046.252261355
-    y: 4141550.5570181748
-    theta: 1.184712766403448
-    kappa: -0.0189509422540303
-    s: 1.6073575615664815
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8246834466889037
-  a: -1.4373406849409347
+  v: 0
+  a: 0
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587046.26555490948
-    y: 4141550.5906954864
-    theta: 1.184134333027997
-    kappa: -0.019126284081624187
-    s: 1.6435636369791962
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7959183208354976
-  a: -1.4390729349188927
+  v: 0
+  a: 0
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 587046.27863712108
-    y: 4141550.6238373914
-    theta: 1.183565102329317
-    kappa: -0.019298838299079708
-    s: 1.6791941028851773
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7671244627390081
-  a: -1.4402151678678121
+  v: 0
+  a: 0
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 587046.29150782188
-    y: 4141550.6564434636
-    theta: 1.1830050816503854
-    kappa: -0.019468602692497065
-    s: 1.7142485021395222
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7383135972229034
-  a: -1.4407749354989596
+  v: 0
+  a: 0
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 587046.30416692968
-    y: 4141550.6885134955
-    theta: 1.1824542745820104
-    kappa: -0.019635576176301288
-    s: 1.7487266105834545
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7094972980764263
-  a: -1.4407597895236022
+  v: 0
+  a: 0
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 587046.31661444646
-    y: 4141550.7200474925
-    theta: 1.1819126810127263
-    kappa: -0.019799758778613386
-    s: 1.7826284340236402
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6806869880545943
-  a: -1.4401772816530058
+  v: 0
+  a: 0
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 587046.32885045791
-    y: 4141550.7510456708
-    theta: 1.1813802971786873
-    kappa: -0.019961151626621536
-    s: 1.8159542052115043
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6518939388781992
-  a: -1.4390349635984374
+  v: 0
+  a: 0
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 587046.34087513166
-    y: 4141550.7815084551
-    theta: 1.1808571157135532
-    kappa: -0.020119756931952251
-    s: 1.8487043808225447
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6231292712338081
-  a: -1.4373403870711636
+  v: 0
+  a: 0
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 587046.35268871638
-    y: 4141550.8114364757
-    theta: 1.1803431256983619
-    kappa: -0.020275577976041573
-    s: 1.8808796384356488
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5944039547737623
-  a: -1.4351011037824508
+  v: 0
+  a: 0
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 587046.36429154081
-    y: 4141550.8408305654
-    theta: 1.1798383127113887
-    kappa: -0.020428619095506213
-    s: 1.9124808735124081
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.565728808116178
-  a: -1.4323246654435657
+  v: 0
+  a: 0
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 587046.37568401289
-    y: 4141550.8696917575
-    theta: 1.1793426588779838
-    kappa: -0.020578885667514767
-    s: 1.943509196376435
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5371144988449457
-  a: -1.4290186237657752
+  v: 0
+  a: 0
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 587046.38686661771
-    y: 4141550.8980212812
-    theta: 1.1788561429203883
-    kappa: -0.020726384095158847
-    s: 1.9739659291926761
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5085715435097311
-  a: -1.4251905304603452
+  v: 0
+  a: 0
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 587046.39783991757
-    y: 4141550.9258205611
-    theta: 1.1783787402075245
-    kappa: -0.020871121792824306
-    s: 2.0038526029467305
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4801103076259743
-  a: -1.420847937238543
+  v: 0
+  a: 0
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 587046.40945263975
-    y: 4141550.9527359135
-    theta: 1.1777476803231257
-    kappa: -0.020974215091625066
-    s: 2.033170954424163
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4517410056748898
-  a: -1.4159983958116347
+  v: 0
+  a: 0
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 587046.42111077125
-    y: 4141550.9790182966
-    theta: 1.1770770312067309
-    kappa: -0.021062946548748032
-    s: 2.06192292318982
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4234737011034677
-  a: -1.4106494578908872
+  v: 0
+  a: 0
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 587046.43254011765
-    y: 4141551.0047849016
-    theta: 1.1764195237333595
-    kappa: -0.021149936694226337
-    s: 2.0901106485671472
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3953183063244716
-  a: -1.4048086751875668
+  v: 0
+  a: 0
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587046.48629094823
-    y: 4141551.1259621177
-    theta: 1.1733271037061745
-    kappa: -0.021559040801441883
-    s: 2.22267410344497
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.2565563413668539
-  a: -1.3684913998267052
+  v: 0
+  a: 0
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587046.534496695
-    y: 4141551.2346383585
-    theta: 1.1705534019547212
-    kappa: -0.021925940601942046
-    s: 2.3415619471269205
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1219921384549414
-  a: -1.3210107615915145
+  v: 0
+  a: 0
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587046.5773495601
-    y: 4141551.3312469297
-    theta: 1.1680874929585947
-    kappa: -0.022252098968168349
-    s: 2.4472481993587611
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.992694835680751
-  a: -1.2633107243903188
+  v: 0
+  a: 0
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587046.61508318211
-    y: 4141551.4163145507
-    theta: 1.1659160282657481
-    kappa: -0.022539294153456225
-    s: 2.5403090738759131
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.86963917474546681
-  a: -1.1963352521314428
+  v: 0
+  a: 0
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587046.64796880924
-    y: 4141551.49045273
-    theta: 1.1640234654227115
-    kappa: -0.022789590660362818
-    s: 2.6214135387643749
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.75370550095944067
-  a: -1.12102830872321
+  v: 0
+  a: 0
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587046.6763114715
-    y: 4141551.554349137
-    theta: 1.1623922956169606
-    kappa: -0.023005310108994707
-    s: 2.6913138768216394
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.64567976324219245
-  a: -1.0383338580739456
+  v: 0
+  a: 0
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 587046.70044615306
-    y: 4141551.6087589664
-    theta: 1.1610032699886264
-    kappa: -0.023189002105335657
-    s: 2.7508362459176081
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.5462535141224083
-  a: -0.94919586409197265
+  v: 0
+  a: 0
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587046.72073396516
-    y: 4141551.6544963177
-    theta: 1.1598356246718609
-    kappa: -0.023343415109574377
-    s: 2.8008712393555104
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.45602390973794371
-  a: -0.85455829068561584
+  v: 0
+  a: 0
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587046.737558318
-    y: 4141551.6924255593
-    theta: 1.1588673046959865
-    kappa: -0.023471467304432281
-    s: 2.8423644462328204
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.37549370983582
-  a: -0.7553651017632006
+  v: 0
+  a: 0
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587046.75132109364
-    y: 4141551.7234527045
-    theta: 1.1580751869164598
-    kappa: -0.023576217463491221
-    s: 2.8763070118021714
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.30507127777222781
-  a: -0.65256026123304944
+  v: 0
+  a: 0
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587046.76243654871
-    y: 4141551.7485116646
-    theta: 1.1574354327958694
-    kappa: -0.023660818544329047
-    s: 2.9037206000904736
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.24484794568379509
-  a: -0.55370303570022628
+  v: 0
+  a: 0
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587046.77130334708
-    y: 4141551.7685011965
-    theta: 1.156925099336767
-    kappa: -0.02372830482460387
-    s: 2.9255884188493066
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.19398110373350219
-  a: -0.46533281712781188
+  v: 0
+  a: 0
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587046.77828000346
-    y: 4141551.7842295449
-    theta: 1.1565235532234905
-    kappa: -0.02378140500448
-    s: 2.9427946584711071
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.15145134362804061
-  a: -0.38686375746115753
+  v: 0
+  a: 0
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587046.78368488629
-    y: 4141551.7964144479
-    theta: 1.1562124711846562
-    kappa: -0.023822542224435
-    s: 2.9561244977592529
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.11629784187956693
-  a: -0.31771000864561427
+  v: 0
+  a: 0
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 587046.78779859282
-    y: 4141551.8056884911
-    theta: 1.1559757035567044
-    kappa: -0.023853852145117805
-    s: 2.9662699624086124
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.087618359805702317
-  a: -0.25728572262653343
+  v: 0
+  a: 0
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587046.79086632491
-    y: 4141551.8126044627
-    theta: 1.1557991377209316
-    kappa: -0.023877201027206807
-    s: 2.9738357834860882
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.064569243529533027
-  a: -0.20500505134926611
+  v: 0
+  a: 0
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 587046.79310026416
-    y: 4141551.8176407106
-    theta: 1.155670561467977
-    kappa: -0.023894203811267972
-    s: 2.9793452559111655
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.046365423979610243
-  a: -0.16028214675916341
+  v: 0
+  a: 0
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 587046.79468194791
-    y: 4141551.8212064989
-    theta: 1.1555795263218918
-    kappa: -0.023906242197612934
-    s: 2.9832460969364574
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.032280416889949815
-  a: -0.12253116080157656
+  v: 0
+  a: 0
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 587046.79576464347
-    y: 4141551.8236473547
-    theta: 1.1555172108423215
-    kappa: -0.023914482726157083
-    s: 2.985916304628252
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.021646322800032702
-  a: -0.0911662454218568
+  v: 0
+  a: 0
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 587046.79647572536
-    y: 4141551.8252504352
-    theta: 1.1554762839151571
-    kappa: -0.023919894856277692
-    s: 2.9876700163470589
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.01385382705480448
-  a: -0.06560155256535527
+  v: 0
+  a: 0
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587046.796919049
-    y: 4141551.8262498751
-    theta: 1.15545076803725
-    kappa: -0.02392326904667199
-    s: 2.9887633672281546
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0083521998046759416
-  a: -0.045251234177423161
+  v: 0
+  a: 0
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587046.79717732745
-    y: 4141551.8268321445
-    theta: 1.1554359025981189
-    kappa: -0.023925234835215296
-    s: 2.9894003486621314
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0046492960055225518
-  a: -0.029529442203411582
+  v: 0
+  a: 0
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587046.79731450614
-    y: 4141551.8271414032
-    theta: 1.155428007160147
-    kappa: -0.023926278918819092
-    s: 2.9897386667754411
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0023115554186846077
-  a: -0.017850328588671638
+  v: 0
+  a: 0
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587046.79737813864
-    y: 4141551.8272848586
-    theta: 1.155424344738025
-    kappa: -0.023926763233289135
-    s: 2.9898956009109439
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00096400261096740714
-  a: -0.0096280452785547643
+  v: 0
+  a: 0
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 587046.79740176187
-    y: 4141551.8273381153
-    theta: 1.1554229850778168
-    kappa: -0.023926943033183566
-    s: 2.9899538621084529
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00029024695464108241
-  a: -0.0042767442184119542
+  v: 0
+  a: 0
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587046.79740727216
-    y: 4141551.8273505373
-    theta: 1.1554226679358475
-    kappa: -0.023926984971670992
-    s: 2.9899674515852817
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.2482627440766354e-05
-  a: -0.001210577353594422
+  v: 0
+  a: 0
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587046.79740729951
-    y: 4141551.8273505992
-    theta: 1.1554226663575091
-    kappa: -0.023926985180388605
-    s: 2.98996751921679
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
   v: 0
-  a: 0.00015630337054706178
+  a: 0
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587046.7974075845
-    y: 4141551.8273512414
-    theta: 1.1554226499560374
-    kappa: -0.02392698734930028
-    s: 2.989968222016933
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.6628698682695795e-05
-  a: 0.00040974600866061639
+  v: 0
+  a: 0
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587046.79740935261
-    y: 4141551.8273552274
-    theta: 1.1554225481912761
-    kappa: -0.023927000806554667
-    s: 2.9899725826188024
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.5851479919155622e-05
-  a: 0.00013559861539524931
+  v: 0
+  a: 0
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 587046.79741169023
-    y: 4141551.8273604978
-    theta: 1.1554224136484437
-    kappa: -0.0239270185983433
-    s: 2.9899783477551778
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.5690355870517222e-05
-  a: -8.0290754599822019e-05
+  v: 0
+  a: 0
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587046.79741379106
-    y: 4141551.8273652345
-    theta: 1.1554222927251667
-    kappa: -0.023927034589097645
-    s: 2.9899835292960244
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8077619742603889e-05
-  a: -7.2060897469869022e-05
+  v: 0
+  a: 0
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587046.79741559969
-    y: 4141551.8273693118
-    theta: 1.1554221886300864
-    kappa: -0.023927048354510743
-    s: 2.9899879897516528
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1259126645974631e-05
-  a: -6.4402532256981529e-05
+  v: 0
+  a: 0
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 587046.79741714709
-    y: 4141551.8273727996
-    theta: 1.1554220995755531
-    kappa: -0.023927060130979286
-    s: 2.9899918057225241
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.5178735904070619e-05
-  a: -5.7295488658204524e-05
+  v: 0
+  a: 0
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587046.79741846165
-    y: 4141551.827375764
-    theta: 1.1554220239025808
-    kappa: -0.023927070137885639
-    s: 2.9899950482958828
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.9782323870628582e-05
-  a: -5.071959637058299e-05
+  v: 0
+  a: 0
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587046.79741957062
-    y: 4141551.8273782637
-    theta: 1.1554219600761406
-    kappa: -0.023927078578220302
-    s: 2.9899977832474605
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.5017783929680713e-05
-  a: -4.4654685091161891e-05
+  v: 0
+  a: 0
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587046.79742049845
-    y: 4141551.8273803554
-    theta: 1.1554219066804523
-    kappa: -0.023927085639204392
-    s: 2.9900000712431778
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.0835026495554729e-05
-  a: -3.90805845169862e-05
+  v: 0
+  a: 0
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587046.79742126749
-    y: 4141551.8273820886
-    theta: 1.155421862414278
-    kappa: -0.023927091492912105
-    s: 2.99000196804085
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7185979012873824e-05
-  a: -3.3977124345100883e-05
+  v: 0
+  a: 0
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 587046.79742189869
-    y: 4141551.8273835122
-    theta: 1.1554218260862146
-    kappa: -0.023927096296893213
-    s: 2.990003524691887
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.402458595655673e-05
-  a: -2.9324134272550929e-05
+  v: 0
+  a: 0
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 587046.79742241092
-    y: 4141551.8273846665
-    theta: 1.1554217966099865
-    kappa: -0.02392710019479552
-    s: 2.9900047877429978
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1306808831817652e-05
-  a: -2.5101443996381279e-05
+  v: 0
+  a: 0
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587046.79742282117
-    y: 4141551.8273855923
-    theta: 1.1554217729997389
-    kappa: -0.023927103316987365
-    s: 2.9900057994378941
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.99062617416632e-06
-  a: -2.1288883213636931e-05
+  v: 0
+  a: 0
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 587046.7974231448
-    y: 4141551.8273863215
-    theta: 1.15542175436533
-    kappa: -0.023927105781180055
-    s: 2.9900065979189927
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0360335494079426e-06
-  a: -1.7866281621362835e-05
+  v: 0
+  a: 0
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 587046.797423396
-    y: 4141551.8273868877
-    theta: 1.1554217399076241
-    kappa: -0.023927107693050387
-    s: 2.9900072174291172
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.405043553643251e-06
-  a: -1.4813468916603992e-05
+  v: 0
+  a: 0
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 587046.79742358706
-    y: 4141551.827387318
-    theta: 1.1554217289137838
-    kappa: -0.023927109146863093
-    s: 2.9900076885132041
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0616858132684778e-06
-  a: -1.2110274796405353e-05
+  v: 0
+  a: 0
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 587046.79742372886
-    y: 4141551.8273876379
-    theta: 1.1554217207525643
-    kappa: -0.023927110226093319
-    s: 2.9900080382200027
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.972006984975361e-06
-  a: -9.7365289578118656e-06
+  v: 0
+  a: 0
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 587046.79742383107
-    y: 4141551.8273878689
-    theta: 1.1554217148696047
-    kappa: -0.023927111004049123
-    s: 2.9900082903037797
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.1040707557511143e-06
-  a: -7.67206109786856e-06
+  v: 0
+  a: 0
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 587046.79742390208
-    y: 4141551.8273880291
-    theta: 1.1554217107827209
-    kappa: -0.023927111544493916
-    s: 2.9900084654260231
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4279578428784836e-06
-  a: -5.8967009136203458e-06
+  v: 0
+  a: 0
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 587046.79742394912
-    y: 4141551.8273881348
-    theta: 1.1554217080771991
-    kappa: -0.023927111902268969
-    s: 2.9900085813571429
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 9.1576599393570107e-07
-  a: -4.3902781021122493e-06
+  v: 0
+  a: 0
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 587046.79742397822
-    y: 4141551.8273882004
-    theta: 1.1554217064010888
-    kappa: -0.023927112123915871
-    s: 2.9900086531781755
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4160998679653924e-07
-  a: -3.1326223603891948e-06
+  v: 0
+  a: 0
   relative_time: 5.7999999999999963
 }
 trajectory_point {
   path_point {
-    x: 587046.79742399463
-    y: 4141551.8273882372
-    theta: 1.1554217054604941
-    kappa: -0.023927112248299021
-    s: 2.9900086934824874
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.816216296302097e-07
-  a: -2.1035633854961844e-06
+  v: 0
+  a: 0
   relative_time: 5.8999999999999959
 }
 trajectory_point {
   path_point {
-    x: 587046.79742400232
-    y: 4141551.827388254
-    theta: 1.1554217050148683
-    kappa: -0.02392711230722807
-    s: 2.9900087125774766
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1394976090146424e-07
-  a: -1.2829308744781802e-06
+  v: 0
+  a: 0
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 587046.79742400476
-    y: 4141551.8273882605
-    theta: 1.1554217048723052
-    kappa: -0.023927112326080441
-    s: 2.9900087186862767
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8760249370568024e-08
-  a: -6.50554524380158e-07
+  v: 0
+  a: 0
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 587046.79742400441
-    y: 4141551.8273882596
-    theta: 1.155421704884833
-    kappa: -0.023927112324423766
-    s: 2.9900087181494603
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
   v: 0
-  a: -1.862640322470799e-07
+  a: 0
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
-    x: 587046.79742400348
-    y: 4141551.8273882577
-    theta: 1.1554217049437066
-    kappa: -0.023927112316638407
-    s: 2.990008715626741
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
   v: 0
-  a: 1.3011090487609172e-07
+  a: 0
   relative_time: 6.2999999999999945
 }
 trajectory_point {
   path_point {
-    x: 587046.797424003
-    y: 4141551.8273882559
-    theta: 1.1554217049747
-    kappa: -0.023927112312539862
-    s: 2.9900087142986758
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4118975701066386e-20
-  a: 3.1874058994436765e-07
+  v: 0
+  a: 0
   relative_time: 6.3999999999999941
 }
 trajectory_point {
   path_point {
-    x: 587046.7974240036
-    y: 4141551.8273882573
-    theta: 1.1554217049334001
-    kappa: -0.023927112318001317
-    s: 2.9900087160683708
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.6739210773052907e-08
-  a: 3.9979532591275858e-07
+  v: 0
+  a: 0
   relative_time: 6.4999999999999938
 }
 trajectory_point {
   path_point {
-    x: 587046.79742400593
-    y: 4141551.8273882628
-    theta: 1.1554217048004984
-    kappa: -0.023927112335576074
-    s: 2.9900087217631821
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7045576977722254e-08
-  a: 3.9344541573631585e-07
+  v: 0
+  a: 0
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
-    x: 587046.79742400977
-    y: 4141551.8273882712
-    theta: 1.1554217045770847
-    kappa: -0.023927112365120042
-    s: 2.9900087313364185
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1318714914730615e-07
-  a: 3.198611623700772e-07
+  v: 0
+  a: 0
   relative_time: 6.6999999999999931
 }
 trajectory_point {
   path_point {
-    x: 587046.797424015
-    y: 4141551.8273882829
-    theta: 1.1554217042799393
-    kappa: -0.023927112404414211
-    s: 2.9900087440690468
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3944900811059508e-07
-  a: 1.99212868769067e-07
+  v: 0
+  a: 0
   relative_time: 6.7999999999999927
 }
 trajectory_point {
   path_point {
-    x: 587046.797424021
-    y: 4141551.8273882968
-    theta: 1.155421703936826
-    kappa: -0.023927112449787125
-    s: 2.9900087587713924
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5213326499182508e-07
-  a: 5.1670837888336535e-08
+  v: 0
+  a: 0
   relative_time: 6.8999999999999924
 }
 trajectory_point {
   path_point {
-    x: 587046.79742402723
-    y: 4141551.8273883108
-    theta: 1.1554217035817849
-    kappa: -0.02392711249673736
-    s: 2.9900087739848433
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4955906121076586e-07
-  a: -1.0259462731715771e-07
+  v: 0
+  a: 0
   relative_time: 6.999999999999992
 }
 trajectory_point {
   path_point {
-    x: 587046.79742403282
-    y: 4141551.8273883234
-    theta: 1.155421703250425
-    kappa: -0.023927112540556
-    s: 2.990008788183554
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3206256848271394e-07
-  a: -2.4341322389233727e-07
+  v: 0
+  a: 0
   relative_time: 7.0999999999999917
 }
 trajectory_point {
   path_point {
-    x: 587046.79742403771
-    y: 4141551.8273883346
-    theta: 1.1554217029752174
-    kappa: -0.023927112576949121
-    s: 2.9900087999761462
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0199698881847234e-07
-  a: -3.5061464888213718e-07
+  v: 0
+  a: 0
   relative_time: 7.1999999999999913
 }
 trajectory_point {
   path_point {
-    x: 587046.797424041
-    y: 4141551.8273883415
-    theta: 1.155421702780788
-    kappa: -0.023927112602660252
-    s: 2.9900088083074152
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.373255452428968e-08
-  a: -4.0402859933161462e-07
+  v: 0
+  a: 0
   relative_time: 7.2999999999999909
 }
 trajectory_point {
   path_point {
-    x: 587046.79742404283
-    y: 4141551.8273883462
-    theta: 1.1554217026792097
-    kappa: -0.023927112616092858
-    s: 2.9900088126600295
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.3656528202029504e-08
-  a: -3.8348477228567747e-07
+  v: 0
+  a: 0
   relative_time: 7.3999999999999906
 }
 trajectory_point {
   path_point {
-    x: 587046.79742404306
-    y: 4141551.8273883462
-    theta: 1.1554217026652958
-    kappa: -0.023927112617932809
-    s: 2.9900088132562366
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
   v: 0
-  a: -2.6881286478935577e-07
+  a: 0
   relative_time: 7.49999999999999
 }
 trajectory_point {
   path_point {
-    x: 587046.79742404236
-    y: 4141551.8273883453
-    theta: 1.1554217027118927
-    kappa: -0.023927112611770877
-    s: 2.9900088112595631
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
   v: 0
-  a: -3.9842573887503236e-08
+  a: 0
   relative_time: 7.59999999999999
 }
 trajectory_point {
   path_point {
-    x: 587046.79742404143
-    y: 4141551.8273883425
-    theta: 1.1554217027651728
-    kappa: -0.023927112604725184
-    s: 2.9900088089765213
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
   v: 0
-  a: 3.2359640337475526e-07
+  a: 0
   relative_time: 7.6999999999999895
 }
 trajectory_point {
   path_point {
-    x: 587046.79742404178
-    y: 4141551.8273883434
-    theta: 1.1554217027399267
-    kappa: -0.02392711260806369
-    s: 2.9900088100583098
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3577815009793778e-08
-  a: 8.4167436995253885e-07
+  v: 0
+  a: 0
   relative_time: 7.7999999999999892
 }
 trajectory_point {
   path_point {
-    x: 587046.79742404574
-    y: 4141551.8273883527
-    theta: 1.155421702514857
-    kappa: -0.023927112637826674
-    s: 2.9900088197025156
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6084882791619451e-07
-  a: 1.5345616288008989e-06
+  v: 0
+  a: 0
   relative_time: 7.8999999999999888
 }
 trajectory_point {
   path_point {
-    x: 587046.79742405587
-    y: 4141551.8273883751
-    theta: 1.1554217019278694
-    kappa: -0.023927112715449211
-    s: 2.9900088448548212
+    x: 587045.66209810227
+    y: 4141549.06192431
+    z: 0
+    theta: 1.2103929796890078
+    kappa: -0.011166695847059904
+    s: 0
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.5698946061079389e-07
-  a: 2.4224284828747378e-06
+  v: 0
+  a: 0
   relative_time: 7.9999999999999885
 }
 decision {
@@ -1705,10 +1826,10 @@ decision {
       reason_code: STOP_REASON_OBSTACLE
       reason: "stop by 401_0"
       stop_point {
-        x: 587049.08822406118
-        y: 4141555.0813091695
+        x: 587047.77858322009
+        y: 4141552.3828042015
       }
-      stop_heading: 1.0883513444795476
+      stop_heading: 1.1486946741855291
       change_lane_type: FORWARD
     }
   }
@@ -1719,12 +1840,12 @@ decision {
       object_decision {
         stop {
           reason_code: STOP_REASON_OBSTACLE
-          distance_s: -3
+          distance_s: -6
           stop_point {
-            x: 587049.08822406118
-            y: 4141555.0813091695
+            x: 587047.77858322009
+            y: 4141552.3828042015
           }
-          stop_heading: 1.0883513444795476
+          stop_heading: 1.1486946741855291
         }
       }
     }
-- 
GitLab