From 58a5d0aa02c9971237ea109420a0570978681c86 Mon Sep 17 00:00:00 2001
From: Dong Li <lidong.main@gmail.com>
Date: Fri, 1 Sep 2017 16:04:40 -0700
Subject: [PATCH] planning: reduce look_backward_boundary and enable reference
 line smooth

---
 modules/planning/common/planning_gflags.cc    |    2 +-
 modules/planning/conf/planning.conf           |    1 -
 .../tasks/st_graph/st_boundary_mapper.cc      |    8 +-
 .../garage_test/result_follow_0.pb.txt        | 1280 +++++-----
 .../garage_test/result_stop_dest_0.pb.txt     | 2260 ++++++++---------
 .../garage_test/result_stop_obstacle_0.pb.txt | 2258 ++++++++--------
 .../result_cruise_0.pb.txt                    | 2244 ++++++++--------
 .../result_cruise_0.pb.txt                    | 2250 ++++++++--------
 scripts/plot_planning.sh                      |   26 +
 9 files changed, 5178 insertions(+), 5151 deletions(-)
 mode change 100755 => 100644 modules/planning/testdata/garage_test/result_follow_0.pb.txt
 mode change 100755 => 100644 modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt
 mode change 100755 => 100644 modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt
 mode change 100755 => 100644 modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
 mode change 100755 => 100644 modules/planning/testdata/yongfengchanyeyuan_test/result_cruise_0.pb.txt
 create mode 100755 scripts/plot_planning.sh

diff --git a/modules/planning/common/planning_gflags.cc b/modules/planning/common/planning_gflags.cc
index 9d1cd1d75d..2efcec71b1 100644
--- a/modules/planning/common/planning_gflags.cc
+++ b/modules/planning/common/planning_gflags.cc
@@ -33,7 +33,7 @@ DEFINE_double(trajectory_resolution, 0.01,
               "output trajectory.");
 
 DEFINE_double(
-    look_backward_distance, 60,
+    look_backward_distance, 10,
     "look backward this distance when creating reference line from routing");
 
 DEFINE_double(
diff --git a/modules/planning/conf/planning.conf b/modules/planning/conf/planning.conf
index f57eed9574..3117b86c11 100644
--- a/modules/planning/conf/planning.conf
+++ b/modules/planning/conf/planning.conf
@@ -1,4 +1,3 @@
 --flagfile=modules/common/data/global_flagfile.txt
 --adapter_config_path=modules/planning/conf/adapter.conf
 --nouse_ros_time
---noenable_smooth_reference_line
diff --git a/modules/planning/tasks/st_graph/st_boundary_mapper.cc b/modules/planning/tasks/st_graph/st_boundary_mapper.cc
index 616ecff02b..f4ee042cc1 100644
--- a/modules/planning/tasks/st_graph/st_boundary_mapper.cc
+++ b/modules/planning/tasks/st_graph/st_boundary_mapper.cc
@@ -186,8 +186,9 @@ bool StBoundaryMapper::MapStopDecision(const PathObstacle& stop_obstacle,
   if (!path_data_.GetPathPointWithRefS(
           stop_obstacle.perception_sl_boundary().start_s(), &obstacle_point)) {
     AERROR << "Fail to get path point from reference s. The sl boundary of "
-              "stop obstacle is: "
-           << stop_obstacle.perception_sl_boundary().DebugString();
+              "stop obstacle "
+           << stop_obstacle.Id()
+           << " is: " << stop_obstacle.perception_sl_boundary().DebugString();
     return false;
   }
 
@@ -195,7 +196,8 @@ bool StBoundaryMapper::MapStopDecision(const PathObstacle& stop_obstacle,
       obstacle_point.s() + stop_decision.stop().distance_s() -
       vehicle_param_.front_edge_to_center() - FLAGS_decision_valid_stop_range;
   if (st_stop_s < 0.0) {
-    AERROR << "obstacle st stop_s " << st_stop_s << " is less than 0.";
+    AERROR << "obstacle " << stop_obstacle.Id() << " st stop_s " << st_stop_s
+           << " is less than 0.";
     return false;
   }
 
diff --git a/modules/planning/testdata/garage_test/result_follow_0.pb.txt b/modules/planning/testdata/garage_test/result_follow_0.pb.txt
old mode 100755
new mode 100644
index 60dc214636..105a8881cb
--- a/modules/planning/testdata/garage_test/result_follow_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_follow_0.pb.txt
@@ -19,10 +19,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586344.89334502735
-    y: 4140700.4310728456
-    theta: 1.3188187104184559
-    kappa: -0.024302693656397166
+    x: 586344.89327744534
+    y: 4140700.431090368
+    theta: 1.3191821041375069
+    kappa: -0.023329566684786394
     s: 0.28012780139567195
     dkappa: 0
     ddkappa: 0
@@ -33,10 +33,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586344.96371463616
-    y: 4140700.7023397242
-    theta: 1.3121334743590687
-    kappa: -0.02255443517172143
+    x: 586344.96357944363
+    y: 4140700.7023747768
+    theta: 1.3128604093807135
+    kappa: -0.020607771980906991
     s: 0.56037341026330767
     dkappa: 0
     ddkappa: 0
@@ -47,10 +47,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.03642037953
-    y: 4140700.9728977322
-    theta: 1.3060908372242348
-    kappa: -0.020987551225330827
+    x: 586345.035727492
+    y: 4140700.973082643
+    theta: 1.3074214800903488
+    kappa: -0.018136555186814032
     s: 0.84053704408195573
     dkappa: 0
     ddkappa: 0
@@ -61,10 +61,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.11059890012
-    y: 4140701.2427802626
-    theta: 1.3004733617015638
-    kappa: -0.01954064526643258
+    x: 586345.10959046171
+    y: 4140701.2430517054
+    theta: 1.3028299954826219
+    kappa: -0.015906616409726421
     s: 1.1204281533483078
     dkappa: 0
     ddkappa: 0
@@ -75,10 +75,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.18530465907
-    y: 4140701.5120429276
-    theta: 1.2950374544456114
-    kappa: -0.018146608900748411
+    x: 586345.1833791117
+    y: 4140701.5125703448
+    theta: 1.2982706511122317
+    kappa: -0.013687390692223475
     s: 1.3998654215767008
     dkappa: 0
     ddkappa: 0
@@ -89,10 +89,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.26248929719
-    y: 4140701.7799576474
-    theta: 1.2903179836715792
-    kappa: -0.016962169533526895
+    x: 586345.25954601017
+    y: 4140701.7807761785
+    theta: 1.2951007896040512
+    kappa: -0.011870334627784212
     s: 1.6786767652991148
     dkappa: 0
     ddkappa: 0
@@ -103,10 +103,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.33945557533
-    y: 4140702.0471144198
-    theta: 1.2856118610773692
-    kappa: -0.015781081020319113
+    x: 586345.33549742762
+    y: 4140702.0482232403
+    theta: 1.2919398809296525
+    kappa: -0.010058419130076516
     s: 1.956699334065173
     dkappa: 0
     ddkappa: 0
@@ -117,10 +117,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.41839891765
-    y: 4140702.3127075755
-    theta: 1.2814889428740144
-    kappa: -0.014772364616110681
+    x: 586345.41206566512
+    y: 4140702.3145124237
+    theta: 1.2893686225643792
+    kappa: -0.0084289370478793542
     s: 2.2337795104421452
     dkappa: 0
     ddkappa: 0
@@ -131,10 +131,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.49770799093
-    y: 4140702.5770604219
-    theta: 1.2775533111985893
-    kappa: -0.013818414751843353
+    x: 586345.48917534016
+    y: 4140702.5795151684
+    theta: 1.287364823375807
+    kappa: -0.0069755824446433648
     s: 2.509772910014942
     dkappa: 0
     ddkappa: 0
@@ -145,10 +145,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.57737342222
-    y: 4140702.8400272583
-    theta: 1.2738116813645681
-    kappa: -0.012924907558477878
+    x: 586345.5659436211
+    y: 4140702.8433446446
+    theta: 1.2853698996723506
+    kappa: -0.0055286623973764522
     s: 2.7845443813861195
     dkappa: 0
     ddkappa: 0
@@ -159,10 +159,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.65822255146
-    y: 4140703.1012243042
-    theta: 1.270481235814096
-    kappa: -0.012160896329509089
+    x: 586345.64332735771
+    y: 4140703.1055892054
+    theta: 1.2842572326610548
+    kappa: -0.0043915855414266795
     s: 3.0579680061758792
     dkappa: 0
     ddkappa: 0
@@ -173,10 +173,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.73863863072
-    y: 4140703.3610223071
-    theta: 1.2671686337147663
-    kappa: -0.011400977354276368
+    x: 586345.72032100847
+    y: 4140703.3664219594
+    theta: 1.2831719861176953
+    kappa: -0.0032680463292708312
     s: 3.3299270990220635
     dkappa: 0
     ddkappa: 0
@@ -187,10 +187,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.82018083939
-    y: 4140703.6188195487
-    theta: 1.2642463221333811
-    kappa: -0.010766723352434884
+    x: 586345.79706774978
+    y: 4140703.6256883563
+    theta: 1.2823724161622911
+    kappa: -0.0022638861383613147
     s: 3.60031420758016
     dkappa: 0
     ddkappa: 0
@@ -201,10 +201,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.90158278483
-    y: 4140703.87491035
-    theta: 1.2614268203268595
-    kappa: -0.010164090112738607
+    x: 586345.87362476578
+    y: 4140703.8832690204
+    theta: 1.2819787666220055
+    kappa: -0.0014279340041993939
     s: 3.8690311125233023
     dkappa: 0
     ddkappa: 0
@@ -215,10 +215,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.9829327825
-    y: 4140704.12916989
-    theta: 1.2587294269647824
-    kappa: -0.0095982460396336344
+    x: 586345.94968059147
+    y: 4140704.1391633986
+    theta: 1.2815876941724884
+    kappa: -0.00059745453865065856
     s: 4.1359888275422643
     dkappa: 0
     ddkappa: 0
@@ -229,10 +229,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.06500497344
-    y: 4140704.381265346
-    theta: 1.2563269576488434
-    kappa: -0.0091238845300053179
+    x: 586346.02528185374
+    y: 4140704.3932744195
+    theta: 1.2815931667434777
+    kappa: 3.5017111397885091e-05
     s: 4.4011075993454671
     dkappa: 0
     ddkappa: 0
@@ -243,10 +243,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.14648605627
-    y: 4140704.6315451353
-    theta: 1.2539417910290656
-    kappa: -0.0086529395117004634
+    x: 586346.10035559058
+    y: 4140704.6455502096
+    theta: 1.2816950171974006
+    kappa: 0.00061586039657467932
     s: 4.6643169076589732
     dkappa: 0
     ddkappa: 0
@@ -257,10 +257,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.22830810852
-    y: 4140704.8796383757
-    theta: 1.2517629281781901
-    kappa: -0.008248449346121985
+    x: 586346.17483998043
+    y: 4140704.8959452068
+    theta: 1.2818442242262131
+    kappa: 0.0011621675951435646
     s: 4.9255554652264912
     dkappa: 0
     ddkappa: 0
@@ -271,10 +271,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.30994847487
-    y: 4140705.1256620679
-    theta: 1.2496904582077593
-    kappa: -0.0078769905746551641
+    x: 586346.24859000428
+    y: 4140705.1444482058
+    theta: 1.282270710433006
+    kappa: 0.0015295715740880165
     s: 5.1847712178093719
     dkappa: 0
     ddkappa: 0
@@ -285,10 +285,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.39096520562
-    y: 4140705.3697162424
-    theta: 1.2476393275623254
-    kappa: -0.0075100029240613283
+    x: 586346.3217523325
+    y: 4140705.3909709454
+    theta: 1.282693798107899
+    kappa: 0.0018940478016153479
     s: 5.4419213441866114
     dkappa: 0
     ddkappa: 0
@@ -299,10 +299,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.47253274336
-    y: 4140705.6113724122
-    theta: 1.245821767808216
-    kappa: -0.0072140115412470092
+    x: 586346.39416466479
+    y: 4140705.6355264378
+    theta: 1.2831962300484703
+    kappa: 0.002180423553843412
     s: 5.69697225615485
     dkappa: 0
     ddkappa: 0
@@ -313,10 +313,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.55342114484
-    y: 4140705.8510165382
-    theta: 1.2440193372227339
-    kappa: -0.0069204846009080882
+    x: 586346.46577655163
+    y: 4140705.8781042155
+    theta: 1.2838015576471906
+    kappa: 0.00236725939761133
     s: 5.9498995985283694
     dkappa: 0
     ddkappa: 0
@@ -327,10 +327,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.63396747946
-    y: 4140706.0885179662
-    theta: 1.242290350339891
-    kappa: -0.0066443347687370775
+    x: 586346.53678290581
+    y: 4140706.1186308148
+    theta: 1.284401766720525
+    kappa: 0.0025525154031683594
     s: 6.2006882491390991
     dkappa: 0
     ddkappa: 0
@@ -341,10 +341,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.71462236368
-    y: 4140706.3237171657
-    theta: 1.2407118610806382
-    kappa: -0.0064052820369570182
+    x: 586346.6068979993
+    y: 4140706.3571841675
+    theta: 1.2850479702576973
+    kappa: 0.0026431872320126491
     s: 6.4493323188366087
     dkappa: 0
     ddkappa: 0
@@ -355,10 +355,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.79458267579
-    y: 4140706.5568909091
-    theta: 1.2391469645093021
-    kappa: -0.0061682879449259723
+    x: 586346.67626408662
+    y: 4140706.593725902
+    theta: 1.2857147431845481
+    kappa: 0.002685540852651813
     s: 6.6958351514881143
     dkappa: 0
     ddkappa: 0
@@ -369,10 +369,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.8743268206
-    y: 4140706.7878873562
-    theta: 1.2376679264642441
-    kappa: -0.0059486194222422737
+    x: 586346.7450311674
+    y: 4140706.8282249952
+    theta: 1.2863757580912234
+    kappa: 0.0027275287311858461
     s: 6.9402093239784755
     dkappa: 0
     ddkappa: 0
@@ -383,10 +383,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.95384835545
-    y: 4140707.0167318033
-    theta: 1.2362725395690672
-    kappa: -0.0057458200774747181
+    x: 586346.81284530344
+    y: 4140707.0608075978
+    theta: 1.2870219746889993
+    kappa: 0.0026787204366014216
     s: 7.1824766462101941
     dkappa: 0
     ddkappa: 0
@@ -397,10 +397,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.0326885297
-    y: 4140707.2436154541
-    theta: 1.2348891090950989
-    kappa: -0.0055447583684899516
+    x: 586346.879999019
+    y: 4140707.2914205692
+    theta: 1.2876606488809315
+    kappa: 0.0026103957660406386
     s: 7.4226681611034193
     dkappa: 0
     ddkappa: 0
@@ -411,10 +411,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.11139575078
-    y: 4140707.4683892466
-    theta: 1.2335984779208196
-    kappa: -0.0053542744693492947
+    x: 586346.94658363238
+    y: 4140707.5200791829
+    theta: 1.2882939103639419
+    kappa: 0.0025426501209813634
     s: 7.6608241445959422
     dkappa: 0
     ddkappa: 0
@@ -425,10 +425,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.189756879
-    y: 4140707.6911801444
-    theta: 1.2323656066899067
-    kappa: -0.0051705234393278586
+    x: 586347.01225181972
+    y: 4140707.7469357997
+    theta: 1.2888521236292809
+    kappa: 0.0024013787090322435
     s: 7.8969941056431958
     dkappa: 0
     ddkappa: 0
@@ -439,10 +439,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.26747853623
-    y: 4140707.9121529423
-    theta: 1.2311427969877577
-    kappa: -0.00498827192165056
+    x: 586347.077346509
+    y: 4140707.9719520546
+    theta: 1.2893985168331072
+    kappa: 0.0022535450284614051
     s: 8.13123678621826
     dkappa: 0
     ddkappa: 0
@@ -453,10 +453,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.34510787542
-    y: 4140708.1311862133
-    theta: 1.2300216704094478
-    kappa: -0.0048077490283839853
+    x: 586347.14192450792
+    y: 4140708.1951822387
+    theta: 1.2899405728963751
+    kappa: 0.002106884780383551
     s: 8.363620161311859
     dkappa: 0
     ddkappa: 0
@@ -467,10 +467,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.42238565232
-    y: 4140708.3484535469
-    theta: 1.2289518770275589
-    kappa: -0.0046287415598154844
+    x: 586347.20571108384
+    y: 4140708.4167859703
+    theta: 1.2903661408534113
+    kappa: 0.00191129643540536
     s: 8.5942214389323617
     dkappa: 0
     ddkappa: 0
@@ -481,10 +481,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.49909519055
-    y: 4140708.5641232715
-    theta: 1.2278899504679843
-    kappa: -0.0044510503682914475
+    x: 586347.26900282246
+    y: 4140708.6367676491
+    theta: 1.2907787940432971
+    kappa: 0.0017127860091666986
     s: 8.8231270601057723
     dkappa: 0
     ddkappa: 0
@@ -495,10 +495,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.575689675
-    y: 4140708.7781350305
-    theta: 1.2269148776318854
-    kappa: -0.0042652726832280406
+    x: 586347.33185217064
+    y: 4140708.8552117217
+    theta: 1.2911885628545443
+    kappa: 0.0015156631119334447
     s: 9.0504326988757544
     dkappa: 0
     ddkappa: 0
@@ -509,10 +509,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.652001228
-    y: 4140708.9906602222
-    theta: 1.2259878743354824
-    kappa: -0.0040758245646840153
+    x: 586347.39409393608
+    y: 4140709.0722747697
+    theta: 1.291467532725191
+    kappa: 0.0012951995573201283
     s: 9.2762432623035984
     dkappa: 0
     ddkappa: 0
@@ -523,10 +523,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.72784610069
-    y: 4140709.2018857258
-    theta: 1.2250665402667948
-    kappa: -0.0038875350083590063
+    x: 586347.45592466765
+    y: 4140709.2880191058
+    theta: 1.2917247451258989
+    kappa: 0.0010722278920711971
     s: 9.5006728904682536
     dkappa: 0
     ddkappa: 0
@@ -537,10 +537,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.80357714638
-    y: 4140709.41181546
-    theta: 1.2242261306514359
-    kappa: -0.0036852565653410431
+    x: 586347.51740893873
+    y: 4140709.502554547
+    theta: 1.2919805162599747
+    kappa: 0.00085050561948372136
     s: 9.7238449564663032
     dkappa: 0
     ddkappa: 0
@@ -551,10 +551,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.87917148182
-    y: 4140709.6205986375
-    theta: 1.2234496166624578
-    kappa: -0.00347215099177851
+    x: 586347.5784948
+    y: 4140709.7160338992
+    theta: 1.2921177903812853
+    kappa: 0.000627233878095405
     s: 9.94589206641198
     dkappa: 0
     ddkappa: 0
@@ -565,10 +565,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.954431122
-    y: 4140709.8284574253
-    theta: 1.2226765406879589
-    kappa: -0.0032599889463495973
+    x: 586347.63928034215
+    y: 4140709.928576617
+    theta: 1.2922149020068845
+    kappa: 0.00040405058951496043
     s: 10.166956059437155
     dkappa: 0
     ddkappa: 0
@@ -579,10 +579,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.02958330768
-    y: 4140710.0354465637
-    theta: 1.2219747708610365
-    kappa: -0.0030332809644847084
+    x: 586347.69983104232
+    y: 4140710.1402981859
+    theta: 1.2923116384461193
+    kappa: 0.00018172955875125514
     s: 10.387165982441235
     dkappa: 0
     ddkappa: 0
@@ -593,10 +593,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.10472852143
-    y: 4140710.2415794968
-    theta: 1.2213756352212035
-    kappa: -0.0027848988472768282
+    x: 586347.76015172875
+    y: 4140710.3512460561
+    theta: 1.2923214098796116
+    kappa: -2.7768832650381704e-05
     s: 10.606568783901761
     dkappa: 0
     ddkappa: 0
@@ -607,10 +607,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.1796059527
-    y: 4140710.4469778687
-    theta: 1.2207786345395704
-    kappa: -0.0025374018478072644
+    x: 586347.82025167125
+    y: 4140710.5614438625
+    theta: 1.2922690962998904
+    kappa: -0.00022791795345726944
     s: 10.825189736356439
     dkappa: 0
     ddkappa: 0
@@ -621,10 +621,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.25429331511
-    y: 4140710.6516396967
-    theta: 1.2202222607839026
-    kappa: -0.0022800056462706168
+    x: 586347.8801435011
+    y: 4140710.7709138016
+    theta: 1.2922169638702243
+    kappa: -0.00042737400325948351
     s: 11.043053654043492
     dkappa: 0
     ddkappa: 0
@@ -635,10 +635,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.32899251324
-    y: 4140710.855517122
-    theta: 1.2198142809958399
-    kappa: -0.0019826021366323807
+    x: 586347.93986373639
+    y: 4140710.9796707579
+    theta: 1.2921176347277525
+    kappa: -0.0006092912617188279
     s: 11.260184892901663
     dkappa: 0
     ddkappa: 0
@@ -649,10 +649,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.403447871
-    y: 4140711.0587290311
-    theta: 1.2194076329207453
-    kappa: -0.001686169440841415
+    x: 586347.99943763379
+    y: 4140711.1877323617
+    theta: 1.2919414185436742
+    kappa: -0.000763121649321543
     s: 11.476607350570214
     dkappa: 0
     ddkappa: 0
@@ -663,10 +663,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.47766745207
-    y: 4140711.2612974318
-    theta: 1.219002272651537
-    kappa: -0.0013906754541579761
+    x: 586348.05882287933
+    y: 4140711.3951351
+    theta: 1.2917657603782653
+    kappa: -0.00091646490447258339
     s: 11.692344466388924
     dkappa: 0
     ddkappa: 0
@@ -677,10 +677,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.55185383873
-    y: 4140711.4631724646
-    theta: 1.2188169137766607
-    kappa: -0.0010339210006758033
+    x: 586348.11804708652
+    y: 4140711.6018949617
+    theta: 1.2915785437820619
+    kappa: -0.0010596248805429234
     s: 11.907419221398092
     dkappa: 0
     ddkappa: 0
@@ -691,10 +691,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.62582515134
-    y: 4140711.6644448638
-    theta: 1.2186384295539279
-    kappa: -0.00067643090164562947
+    x: 586348.17722215783
+    y: 4140711.8080032938
+    theta: 1.29131967948963
+    kappa: -0.0011443906375509536
     s: 12.121854138338538
     dkappa: 0
     ddkappa: 0
@@ -705,10 +705,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.69958335708
-    y: 4140711.8651374094
-    theta: 1.2184604595369459
-    kappa: -0.00031997070930409358
+    x: 586348.23622674961
+    y: 4140712.0135178408
+    theta: 1.2910615609522673
+    kappa: -0.0012289121896975155
     s: 12.335671281651592
     dkappa: 0
     ddkappa: 0
@@ -719,10 +719,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.77314497437
-    y: 4140712.0652670497
-    theta: 1.2184797408793631
-    kappa: 8.7783141113065456e-05
+    x: 586348.29506682383
+    y: 4140712.2184593696
+    theta: 1.2908041621215873
+    kappa: -0.0013131980778536437
     s: 12.548892257479118
     dkappa: 0
     ddkappa: 0
@@ -733,10 +733,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.84651209274
-    y: 4140712.2648555478
-    theta: 1.2185832719273963
-    kappa: 0.00051684047161862194
+    x: 586348.35391034628
+    y: 4140712.4228015784
+    theta: 1.2905234426606884
+    kappa: -0.001313469905748269
     s: 12.76153821366348
     dkappa: 0
     ddkappa: 0
@@ -747,10 +747,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.91968795611
-    y: 4140712.4639237542
-    theta: 1.2186865330883332
-    kappa: 0.00094477932614980473
+    x: 586348.41262358124
+    y: 4140712.6266044583
+    theta: 1.2902400322906269
+    kappa: -0.0013017994839358703
     s: 12.973629839747574
     dkappa: 0
     ddkappa: 0
@@ -761,10 +761,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.99261482211
-    y: 4140712.6625144193
-    theta: 1.2189185995611196
-    kappa: 0.001406849036088699
+    x: 586348.471188962
+    y: 4140712.8298941129
+    theta: 1.2899573356279341
+    kappa: -0.0012901584511147489
     s: 13.185187366974812
     dkappa: 0
     ddkappa: 0
@@ -775,10 +775,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.06526529067
-    y: 4140712.8606586652
-    theta: 1.2193477247174196
-    kappa: 0.0019217060851421169
+    x: 586348.52971427806
+    y: 4140713.0326600173
+    theta: 1.2897050465573099
+    kappa: -0.0012044158767968167
     s: 13.396230568289122
     dkappa: 0
     ddkappa: 0
@@ -789,10 +789,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.13774535421
-    y: 4140713.058338155
-    theta: 1.2197758433839367
-    kappa: 0.0024353555139578309
+    x: 586348.58817697712
+    y: 4140713.2349287821
+    theta: 1.2894750351370994
+    kappa: -0.0010647843085076474
     s: 13.606778758334952
     dkappa: 0
     ddkappa: 0
@@ -803,10 +803,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.21003796556
-    y: 4140713.2555791968
-    theta: 1.2202250099196206
-    kappa: 0.002947222017553919
+    x: 586348.64650746272
+    y: 4140713.4367401153
+    theta: 1.2892455438859489
+    kappa: -0.00092546851713731218
     s: 13.81685079345727
     dkappa: 0
     ddkappa: 0
@@ -817,10 +817,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.281806143
-    y: 4140713.4525245558
-    theta: 1.2210163749322214
-    kappa: 0.003448288536870243
+    x: 586348.7047398102
+    y: 4140713.6381033114
+    theta: 1.2890479602114113
+    kappa: -0.0007749516153595493
     s: 14.026465071701558
     dkappa: 0
     ddkappa: 0
@@ -831,10 +831,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.35342373489
-    y: 4140713.6490566796
-    theta: 1.2218060800956156
-    kappa: 0.0039483037077702081
+    x: 586348.76293484436
+    y: 4140713.8390194457
+    theta: 1.288942813375862
+    kappa: -0.00059104241279017765
     s: 14.235639532813824
     dkappa: 0
     ddkappa: 0
@@ -845,10 +845,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.42489672662
-    y: 4140713.8451919933
-    theta: 1.2225941904224602
-    kappa: 0.0044473093182829295
+    x: 586348.82101237937
+    y: 4140714.0395299192
+    theta: 1.2888378788370654
+    kappa: -0.00040750453384570635
     s: 14.44439165824059
     dkappa: 0
     ddkappa: 0
@@ -859,10 +859,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.49577358854
-    y: 4140714.0411122907
-    theta: 1.223604184484441
-    kappa: 0.0048778532988265795
+    x: 586348.87897715112
+    y: 4140714.2396510816
+    theta: 1.2887331480402158
+    kappa: -0.00022432301152978067
     s: 14.652738471128895
     dkappa: 0
     ddkappa: 0
@@ -873,10 +873,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.56632163865
-    y: 4140714.236738285
-    theta: 1.2247083160332239
-    kappa: 0.0052785866563654065
+    x: 586348.936861438
+    y: 4140714.4393908279
+    theta: 1.2887314868935089
+    kappa: -7.8145076508083158e-05
     s: 14.860696536326301
     dkappa: 0
     ddkappa: 0
@@ -887,10 +887,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.63674327324
-    y: 4140714.4320137356
-    theta: 1.2258104690590053
-    kappa: 0.0056786019377085446
+    x: 586348.99464483629
+    y: 4140714.6387718376
+    theta: 1.2887403676067972
+    kappa: 6.401512803758027e-05
     s: 15.068281960380887
     dkappa: 0
     ddkappa: 0
@@ -901,10 +901,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.70679183421
-    y: 4140714.6270436454
-    theta: 1.2269887995736479
-    kappa: 0.0060508517854412965
+    x: 586349.05232886237
+    y: 4140714.8378099641
+    theta: 1.2887492330475703
+    kappa: 0.00020593085403618127
     s: 15.275510391541248
     dkappa: 0
     ddkappa: 0
@@ -915,10 +915,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.77617654088
-    y: 4140714.8219483383
-    theta: 1.2283350925420706
-    kappa: 0.0063635630906487091
+    x: 586349.10989822564
+    y: 4140715.0365254516
+    theta: 1.2888044213145726
+    kappa: 0.00032793507289911373
     s: 15.482397019756501
     dkappa: 0
     ddkappa: 0
@@ -929,10 +929,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.845451556
-    y: 4140715.0165449027
-    theta: 1.2296792590063725
-    kappa: 0.0066757800241677018
+    x: 586349.16735987726
+    y: 4140715.234931624
+    theta: 1.2888991665306884
+    kappa: 0.00043291130525102
     s: 15.688956576676285
     dkappa: 0
     ddkappa: 0
@@ -943,10 +943,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.91462166642
-    y: 4140715.210846785
-    theta: 1.2310213869939808
-    kappa: 0.006987524160316505
+    x: 586349.22473451332
+    y: 4140715.4330373448
+    theta: 1.288993768272177
+    kappa: 0.000537728569650647
     s: 15.895203335650747
     dkappa: 0
     ddkappa: 0
@@ -957,10 +957,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.98283641168
-    y: 4140715.4051690763
-    theta: 1.2325342908402952
-    kappa: 0.0072276970634816556
+    x: 586349.28201301454
+    y: 4140715.6308596358
+    theta: 1.2890975748503319
+    kappa: 0.000637426935372737
     s: 16.101151111730562
     dkappa: 0
     ddkappa: 0
@@ -971,10 +971,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.05087162554
-    y: 4140715.5992518673
-    theta: 1.2340622562496151
-    kappa: 0.0074604735093552112
+    x: 586349.33913979633
+    y: 4140715.8284284864
+    theta: 1.2892533293274784
+    kappa: 0.00070929077488151963
     s: 16.306813261666921
     dkappa: 0
     ddkappa: 0
@@ -985,10 +985,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.11881661823
-    y: 4140715.7930772854
-    theta: 1.2355881940095113
-    kappa: 0.0076929412713858929
+    x: 586349.39619082271
+    y: 4140716.0257353419
+    theta: 1.289408877263412
+    kappa: 0.00078105931606207112
     s: 16.512202683911532
     dkappa: 0
     ddkappa: 0
@@ -999,10 +999,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.18614419457
-    y: 4140715.9868416414
-    theta: 1.2371802081495495
-    kappa: 0.0078901652013268174
+    x: 586349.45316954888
+    y: 4140716.2227921523
+    theta: 1.2895642280719266
+    kappa: 0.00085273690583531885
     s: 16.717331818616628
     dkappa: 0
     ddkappa: 0
@@ -1013,10 +1013,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.25283032516
-    y: 4140716.1805659663
-    theta: 1.2388419840878704
-    kappa: 0.0080503174681971646
+    x: 586349.5099904344
+    y: 4140716.4196360963
+    theta: 1.2897491452859409
+    kappa: 0.00090268647229825854
     s: 16.922212647634947
     dkappa: 0
     ddkappa: 0
@@ -1027,10 +1027,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.31943938613
-    y: 4140716.3740664027
-    theta: 1.2405018416160503
-    kappa: 0.00821028464603006
+    x: 586349.56671590672
+    y: 4140716.6162611372
+    theta: 1.2899438077789285
+    kappa: 0.00094533486630241949
     s: 17.126856694519759
     dkappa: 0
     ddkappa: 0
@@ -1041,10 +1041,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.38592047268
-    y: 4140716.567372
-    theta: 1.2421641752142336
-    kappa: 0.0083669976012959533
+    x: 586349.62337881245
+    y: 4140716.8126693061
+    theta: 1.2901382555638037
+    kappa: 0.00098793622027504129
     s: 17.331275024524849
     dkappa: 0
     ddkappa: 0
@@ -1055,10 +1055,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.45126437466
-    y: 4140716.7608380932
-    theta: 1.2439092256829984
-    kappa: 0.0084632832362272828
+    x: 586349.67993611144
+    y: 4140717.0088840411
+    theta: 1.2903393871939268
+    kappa: 0.0010221288678948198
     s: 17.535478244604519
     dkappa: 0
     ddkappa: 0
@@ -1069,10 +1069,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.51654269034
-    y: 4140716.9541100031
-    theta: 1.2456525317893341
-    kappa: 0.008559472228088694
+    x: 586349.736344324
+    y: 4140717.2049284321
+    theta: 1.2905541476709235
+    kappa: 0.0010394941667728138
     s: 17.739476503413588
     dkappa: 0
     ddkappa: 0
@@ -1083,10 +1083,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.58175852033
-    y: 4140717.14719691
-    theta: 1.2473941657418106
-    kappa: 0.0086555691460638486
+    x: 586349.79269854154
+    y: 4140717.4007851654
+    theta: 1.2907687025827408
+    kappa: 0.0010568428432647862
     s: 17.9432794913074
     dkappa: 0
     ddkappa: 0
@@ -1097,10 +1097,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.64621981536
-    y: 4140717.3403399689
-    theta: 1.2491622713847412
-    kappa: 0.0087179653093512888
+    x: 586349.84900131694
+    y: 4140717.596463114
+    theta: 1.2909830616287368
+    kappa: 0.0010741756832470979
     s: 18.146896440341813
     dkappa: 0
     ddkappa: 0
@@ -1111,10 +1111,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.71015704714
-    y: 4140717.5334713636
-    theta: 1.2509477461947947
-    kappa: 0.00875767556696222
+    x: 586349.90512299049
+    y: 4140717.7920086603
+    theta: 1.2912012051683452
+    kappa: 0.0010720991185657767
     s: 18.350336124273202
     dkappa: 0
     ddkappa: 0
@@ -1125,10 +1125,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.7740411812
-    y: 4140717.7264423687
-    theta: 1.2527317397346971
-    kappa: 0.0087973528465727854
+    x: 586349.96118398081
+    y: 4140717.987395857
+    theta: 1.2914195907600108
+    kappa: 0.0010679574533477023
     s: 18.55360685855846
     dkappa: 0
     ddkappa: 0
@@ -1139,10 +1139,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.83770834107
-    y: 4140717.9193149786
-    theta: 1.2545157165734595
-    kappa: 0.0088301900704573673
+    x: 586350.01720055589
+    y: 4140718.1826282553
+    theta: 1.2916378033282487
+    kappa: 0.0010638190694320241
     s: 18.756716548260847
     dkappa: 0
     ddkappa: 0
@@ -1153,10 +1153,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.90026797552
-    y: 4140718.1123890094
-    theta: 1.256307305548271
-    kappa: 0.0088196419474276672
+    x: 586350.07310791011
+    y: 4140718.3777324622
+    theta: 1.2918515187435364
+    kappa: 0.0010514083795863684
     s: 18.959672921059319
     dkappa: 0
     ddkappa: 0
@@ -1167,10 +1167,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.96278268239
-    y: 4140718.3053243831
-    theta: 1.2580976147623284
-    kappa: 0.0088091013995817268
+    x: 586350.12889805145
+    y: 4140718.5727186343
+    theta: 1.2920600928859109
+    kappa: 0.0010294793060269148
     s: 19.162483539791882
     dkappa: 0
     ddkappa: 0
@@ -1181,10 +1181,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.02525473444
-    y: 4140718.4981281133
-    theta: 1.2598866978161432
-    kappa: 0.0087985680437577332
+    x: 586350.18465012615
+    y: 4140718.7675717631
+    theta: 1.2922685247187489
+    kappa: 0.0010075651951032387
     s: 19.365155776872722
     dkappa: 0
     ddkappa: 0
@@ -1195,10 +1195,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.08690135193
-    y: 4140718.6910587321
-    theta: 1.2616593757315655
-    kappa: 0.00875982315814252
+    x: 586350.24036381766
+    y: 4140718.9622994089
+    theta: 1.2924764705239149
+    kappa: 0.00098542217891528961
     s: 19.567696814292187
     dkappa: 0
     ddkappa: 0
@@ -1209,10 +1209,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.148114529
-    y: 4140718.883997893
-    theta: 1.2634232954244671
-    kappa: 0.0087068800129005315
+    x: 586350.29591207614
+    y: 4140719.1569451485
+    theta: 1.2926641824661687
+    kappa: 0.00094937115359864136
     s: 19.770113643616803
     dkappa: 0
     ddkappa: 0
@@ -1223,10 +1223,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.20929220063
-    y: 4140719.0768251438
-    theta: 1.265186192805037
-    kappa: 0.0086539675760405523
+    x: 586350.35142811516
+    y: 4140719.3514779881
+    theta: 1.2928517855400565
+    kappa: 0.000913341038801234
     s: 19.972413065989269
     dkappa: 0
     ddkappa: 0
@@ -1237,10 +1237,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.2702213662
-    y: 4140719.2696142932
-    theta: 1.2669388016261689
-    kappa: 0.00859444445051013
+    x: 586350.406913749
+    y: 4140719.5459042848
+    theta: 1.2930392858628412
+    kappa: 0.00087733065713483228
     s: 20.174601692128459
     dkappa: 0
     ddkappa: 0
@@ -1251,10 +1251,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.3301828485
-    y: 4140719.46259788
-    theta: 1.2686499529939244
-    kappa: 0.0085060392771025874
+    x: 586350.46230517956
+    y: 4140719.7402489013
+    theta: 1.293210359973733
+    kappa: 0.00083540621038486753
     s: 20.37668594232942
     dkappa: 0
     ddkappa: 0
@@ -1265,10 +1265,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.39011520951
-    y: 4140719.6554877404
-    theta: 1.2703602788601218
-    kappa: 0.0084176770393521484
+    x: 586350.5176186813
+    y: 4140719.934513676
+    theta: 1.2933686414273389
+    kappa: 0.00078888459455949984
     s: 20.578672046463375
     dkappa: 0
     ddkappa: 0
@@ -1279,10 +1279,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.450020241
-    y: 4140719.8482896429
-    theta: 1.2720698203568357
-    kappa: 0.0083293550952004
+    x: 586350.5729069598
+    y: 4140720.1286898656
+    theta: 1.2935268507051809
+    kappa: 0.00074238419281116613
     s: 20.780566043977711
     dkappa: 0
     ddkappa: 0
@@ -1293,10 +1293,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.50909149623
-    y: 4140720.0412576413
-    theta: 1.2737270811929942
-    kappa: 0.0082204156601204861
+    x: 586350.62816201115
+    y: 4140720.3227858273
+    theta: 1.2936814323079076
+    kappa: 0.00069519944783339978
     s: 20.982373783895998
     dkappa: 0
     ddkappa: 0
@@ -1307,10 +1307,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.5677976266
-    y: 4140720.2342535732
-    theta: 1.2753619056549577
-    kappa: 0.00810279096897024
+    x: 586350.6833098731
+    y: 4140720.5168284839
+    theta: 1.2938044042480836
+    kappa: 0.00064179312636486788
     s: 21.184100924817983
     dkappa: 0
     ddkappa: 0
@@ -1321,10 +1321,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.62648189254
-    y: 4140720.4271776257
-    theta: 1.2769961216019152
-    kappa: 0.0079852100857057545
+    x: 586350.73843719577
+    y: 4140720.7107988722
+    theta: 1.2939273303912011
+    kappa: 0.000588406695431283
     s: 21.385752934919573
     dkappa: 0
     ddkappa: 0
@@ -1335,10 +1335,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.68491735321
-    y: 4140720.62010366
-    theta: 1.2786102839922784
-    kappa: 0.0078629451569841389
+    x: 586350.79354542214
+    y: 4140720.9047020683
+    theta: 1.2940502139508379
+    kappa: 0.00053503875777234687
     s: 21.587335091952863
     dkappa: 0
     ddkappa: 0
@@ -1349,10 +1349,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.74249895464
-    y: 4140720.8132192167
-    theta: 1.2801527318449872
-    kappa: 0.0077234504073513555
+    x: 586350.84859162907
+    y: 4140721.0985555514
+    theta: 1.2941484817118851
+    kappa: 0.00047955673444539866
     s: 21.78885248324611
     dkappa: 0
     ddkappa: 0
@@ -1363,10 +1363,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.80006344919
-    y: 4140721.0062774005
-    theta: 1.2816947253419746
-    kappa: 0.0075839971002371557
+    x: 586350.90359608945
+    y: 4140721.2923586546
+    theta: 1.2942326375961666
+    kappa: 0.00042286995883604846
     s: 21.990310005703751
     dkappa: 0
     ddkappa: 0
@@ -1377,10 +1377,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.85761218157
-    y: 4140721.1992827221
-    theta: 1.2832362931742358
-    kappa: 0.0074445819777126318
+    x: 586350.95858548873
+    y: 4140721.4861086905
+    theta: 1.294316770437312
+    kappa: 0.00036619870499030093
     s: 22.1917123658064
     dkappa: 0
     ddkappa: 0
@@ -1391,10 +1391,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.91442965856
-    y: 4140721.3924512332
-    theta: 1.2847029492755078
-    kappa: 0.0072943336837883264
+    x: 586351.01355388039
+    y: 4140721.679812042
+    theta: 1.2943942785877254
+    kappa: 0.00030957895851782774
     s: 22.393064079610831
     dkappa: 0
     ddkappa: 0
@@ -1405,10 +1405,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.970960423
-    y: 4140721.5856561344
-    theta: 1.2861408243962227
-    kappa: 0.007139970865621275
+    x: 586351.06847362127
+    y: 4140721.8734810464
+    theta: 1.2944386521038349
+    kappa: 0.00025315907842442161
     s: 22.59436947275001
     dkappa: 0
     ddkappa: 0
@@ -1419,10 +1419,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.027479341
-    y: 4140721.7788205482
-    theta: 1.2875783983305897
-    kappa: 0.0069856403956481371
+    x: 586351.12338185334
+    y: 4140722.0671094661
+    theta: 1.2944830163211527
+    kappa: 0.00019675102165248281
     s: 22.795632680433066
     dkappa: 0
     ddkappa: 0
@@ -1433,10 +1433,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.08376840246
-    y: 4140721.9720117887
-    theta: 1.2889896722689564
-    kappa: 0.00682937108086251
+    x: 586351.17827965249
+    y: 4140722.2607010957
+    theta: 1.294527372109048
+    kappa: 0.00014035368259669172
     s: 22.996857647445296
     dkappa: 0
     ddkappa: 0
@@ -1447,10 +1447,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.13931721565
-    y: 4140722.1653817417
-    theta: 1.2903139427699075
-    kappa: 0.00666656611385043
+    x: 586351.23315638758
+    y: 4140722.4542628508
+    theta: 1.2945451580413856
+    kappa: 8.6611248762513763e-05
     s: 23.198048128148184
     dkappa: 0
     ddkappa: 0
@@ -1461,10 +1461,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.19485749118
-    y: 4140722.3587219752
-    theta: 1.2916380121306683
-    kappa: 0.0065037861694720546
+    x: 586351.28801954864
+    y: 4140722.6477963151
+    theta: 1.2945512282196359
+    kappa: 3.4043532749054865e-05
     s: 23.399207686479379
     dkappa: 0
     ddkappa: 0
@@ -1475,10 +1475,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.25039016036
-    y: 4140722.55203573
-    theta: 1.2929618979136133
-    kappa: 0.0063410285178526656
+    x: 586351.342875196
+    y: 4140722.841303274
+    theta: 1.2945572975665736
+    kappa: -1.8516984100969067e-05
     s: 23.600339695952705
     dkappa: 0
     ddkappa: 0
@@ -1489,10 +1489,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.30528649222
-    y: 4140722.7455054279
-    theta: 1.2942007254373713
-    kappa: 0.0061773663415051975
+    x: 586351.39772590133
+    y: 4140723.0347863077
+    theta: 1.2945558279645182
+    kappa: -6.9381013920972534e-05
     s: 23.801447339658161
     dkappa: 0
     ddkappa: 0
@@ -1503,10 +1503,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.35995209566
-    y: 4140722.9390186314
-    theta: 1.2954091002351005
-    kappa: 0.0060133914231507409
+    x: 586351.45257682714
+    y: 4140723.2282470642
+    theta: 1.2945275834364094
+    kappa: -0.00011423647935686785
     s: 24.002533610261924
     dkappa: 0
     ddkappa: 0
@@ -1517,10 +1517,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.41461265064
-    y: 4140723.1325139636
-    theta: 1.2966173634717721
-    kappa: 0.0058494316483221811
+    x: 586351.50742268737
+    y: 4140723.4216899537
+    theta: 1.2944993415167358
+    kappa: -0.00015908780226956979
     s: 24.203601310006334
     dkappa: 0
     ddkappa: 0
@@ -1531,10 +1531,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.46908289136
-    y: 4140723.3260460775
-    theta: 1.2977982343262959
-    kappa: 0.005686924057047015
+    x: 586351.56226419436
+    y: 4140723.6151174894
+    theta: 1.2944711018386867
+    kappa: -0.00020393556526640896
     s: 24.404653050709911
     dkappa: 0
     ddkappa: 0
@@ -1545,10 +1545,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.52296945709
-    y: 4140723.5197277674
-    theta: 1.2988939063208356
-    kappa: 0.0055289157810258418
+    x: 586351.61712329451
+    y: 4140723.8085259637
+    theta: 1.294422702063361
+    kappa: -0.00023917154369107457
     s: 24.60569125376735
     dkappa: 0
     ddkappa: 0
@@ -1559,10 +1559,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.57685299218
-    y: 4140723.7133985646
-    theta: 1.2999895180186816
-    kappa: 0.0053709163916154726
+    x: 586351.67198663147
+    y: 4140724.0019214842
+    theta: 1.2943673696359543
+    kappa: -0.0002711002938751894
     s: 24.806718150149518
     dkappa: 0
     ddkappa: 0
@@ -1573,10 +1573,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.63073404378
-    y: 4140723.907060435
-    theta: 1.301085077910674
-    kappa: 0.0052129242850246123
+    x: 586351.72684743954
+    y: 4140724.19530809
+    theta: 1.2943120397590411
+    kappa: -0.00030302757233637169
     s: 25.007735780403454
     dkappa: 0
     ddkappa: 0
@@ -1587,10 +1587,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.684077961
-    y: 4140724.1008629883
-    theta: 1.3020998885685666
-    kappa: 0.005064515391214248
+    x: 586351.78171785118
+    y: 4140724.3886842625
+    theta: 1.2942522452583773
+    kappa: -0.00033049490881381237
     s: 25.208745994652372
     dkappa: 0
     ddkappa: 0
@@ -1601,10 +1601,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.737241285
-    y: 4140724.2947094562
-    theta: 1.3030876663469024
-    kappa: 0.00491931544384264
+    x: 586351.83661851066
+    y: 4140724.5820458652
+    theta: 1.2941802293025511
+    kappa: -0.000345759913553573
     s: 25.409750452595659
     dkappa: 0
     ddkappa: 0
@@ -1615,10 +1615,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.79040347517
-    y: 4140724.488551789
-    theta: 1.3040754230520089
-    kappa: 0.0047741185933005605
+    x: 586351.89151799923
+    y: 4140724.775403345
+    theta: 1.2941082148823322
+    kappa: -0.00036102459272078168
     s: 25.610750623508878
     dkappa: 0
     ddkappa: 0
@@ -1629,10 +1629,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.84340067382
-    y: 4140724.6824359624
-    theta: 1.3050385672721276
-    kappa: 0.0046340729305710494
+    x: 586351.94641666627
+    y: 4140724.9687579307
+    theta: 1.2940362015402798
+    kappa: -0.00037628904343605963
     s: 25.811747786243764
     dkappa: 0
     ddkappa: 0
@@ -1643,10 +1643,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.89592207258
-    y: 4140724.8764477931
-    theta: 1.3059305054784218
-    kappa: 0.004508933335998647
+    x: 586352.00135473954
+    y: 4140725.1620993265
+    theta: 1.2939611631074213
+    kappa: -0.00037561338831889278
     s: 26.012743029228226
     dkappa: 0
     ddkappa: 0
@@ -1657,10 +1657,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.9484432044
-    y: 4140725.0704586371
-    theta: 1.3068224398443604
-    kappa: 0.0043837943775652958
+    x: 586352.05630309635
+    y: 4140725.3554367386
+    theta: 1.2938853246405067
+    kappa: -0.00037072111026958173
     s: 26.213737250466345
     dkappa: 0
     ddkappa: 0
@@ -1671,10 +1671,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.00096425414
-    y: 4140725.2644691784
-    theta: 1.3077143720907549
-    kappa: 0.0042586556147316823
+    x: 586352.11125136737
+    y: 4140725.5487738494
+    theta: 1.2938094862921778
+    kappa: -0.00036582883986719631
     s: 26.414731157538377
     dkappa: 0
     ddkappa: 0
@@ -1685,10 +1685,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.05305171746
-    y: 4140725.4585966272
-    theta: 1.3085471300186533
-    kappa: 0.0041544909105433077
+    x: 586352.16621477425
+    y: 4140725.7421068666
+    theta: 1.2937372499884934
+    kappa: -0.000353517334723916
     s: 26.615725267600752
     dkappa: 0
     ddkappa: 0
@@ -1699,10 +1699,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.105006519
-    y: 4140725.6527603981
-    theta: 1.3093617672880828
-    kappa: 0.0040567499183928532
+    x: 586352.22121131024
+    y: 4140725.9354310818
+    theta: 1.2936728879891295
+    kappa: -0.00032498677434695617
     s: 26.816719971899165
     dkappa: 0
     ddkappa: 0
@@ -1713,10 +1713,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.15696160658
-    y: 4140725.8469252377
-    theta: 1.3101764090207713
-    kappa: 0.0039590083880606253
+    x: 586352.27620814927
+    y: 4140726.1287563611
+    theta: 1.2936085256351488
+    kappa: -0.00029645605687493066
     s: 27.017715782915282
     dkappa: 0
     ddkappa: 0
@@ -1727,10 +1727,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.20876967383
-    y: 4140726.0411308985
-    theta: 1.3109752678816409
-    kappa: 0.0038707568485951997
+    x: 586352.33120543708
+    y: 4140726.3220832194
+    theta: 1.2935441627560913
+    kappa: -0.00026792510650948008
     s: 27.218713234640326
     dkappa: 0
     ddkappa: 0
@@ -1741,10 +1741,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.26019099192
-    y: 4140726.2354416959
-    theta: 1.3117326628755062
-    kappa: 0.0038074344293046072
+    x: 586352.386232191
+    y: 4140726.5154039222
+    theta: 1.2935042418134917
+    kappa: -0.00021675481665438327
     s: 27.419712831463855
     dkappa: 0
     ddkappa: 0
@@ -1755,10 +1755,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.31161298021
-    y: 4140726.4297550255
-    theta: 1.3124900681485319
-    kappa: 0.0037441111846514513
+    x: 586352.44126528827
+    y: 4140726.7087255437
+    theta: 1.29346908217654
+    kappa: -0.00016117340245240343
     s: 27.620715048173743
     dkappa: 0
     ddkappa: 0
@@ -1769,10 +1769,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.36303575267
-    y: 4140726.6240713187
-    theta: 1.3132474845092506
-    kappa: 0.0036807869743853664
+    x: 586352.49629922479
+    y: 4140726.9020501128
+    theta: 1.2934339220034492
+    kappa: -0.00010559114069227212
     s: 27.821720329956186
     dkappa: 0
     ddkappa: 0
@@ -1783,10 +1783,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.4140605646
-    y: 4140726.81849597
-    theta: 1.3139729125223472
-    kappa: 0.0036205873778739402
+    x: 586352.5513374795
+    y: 4140727.0953770718
+    theta: 1.2934142855098676
+    kappa: -4.8186465897897778e-05
     s: 28.022729092395714
     dkappa: 0
     ddkappa: 0
@@ -1797,10 +1797,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.46497869468
-    y: 4140727.012952704
-    theta: 1.3146897169753684
-    kappa: 0.0035612303583182
+    x: 586352.60638277093
+    y: 4140727.2887060475
+    theta: 1.2934222279085326
+    kappa: 1.2455163687544146e-05
     s: 28.223741721475172
     dkappa: 0
     ddkappa: 0
@@ -1811,10 +1811,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.51589789439
-    y: 4140727.2074135225
-    theta: 1.315406536455084
-    kappa: 0.0035018720917465531
+    x: 586352.66142921906
+    y: 4140727.4820390851
+    theta: 1.2934301704740574
+    kappa: 7.3098067276924453e-05
     s: 28.424758573575726
     dkappa: 0
     ddkappa: 0
@@ -1825,10 +1825,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.56667907047
-    y: 4140727.4019150087
-    theta: 1.3161089477643586
-    kappa: 0.003435994514043565
+    x: 586352.716476913
+    y: 4140727.6753764991
+    theta: 1.2934381132193533
+    kappa: 0.00013374234345330847
     s: 28.625779975476874
     dkappa: 0
     ddkappa: 0
@@ -1839,10 +1839,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.61713786039
-    y: 4140727.5965055097
-    theta: 1.3167778384390234
-    kappa: 0.0033549598545062143
+    x: 586352.77150959556
+    y: 4140727.8687232248
+    theta: 1.2934792378241813
+    kappa: 0.00018233676117562477
     s: 28.826806224356428
     dkappa: 0
     ddkappa: 0
@@ -1853,10 +1853,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.66759793425
-    y: 4140727.7911009621
-    theta: 1.31744674639624
-    kappa: 0.0032739231332670283
+    x: 586352.826541526
+    y: 4140728.0620754827
+    theta: 1.2935247343412328
+    kappa: 0.00022934495119509684
     s: 29.027837587790529
     dkappa: 0
     ddkappa: 0
@@ -1867,10 +1867,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.71805935155
-    y: 4140727.9857015954
-    theta: 1.3181156718292475
-    kappa: 0.0031928842543973417
+    x: 586352.88157492178
+    y: 4140728.2554328893
+    theta: 1.2935702320696172
+    kappa: 0.00027635439282375272
     s: 29.228874303753638
     dkappa: 0
     ddkappa: 0
@@ -1881,10 +1881,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.768169463
-    y: 4140728.1803986519
-    theta: 1.3187331377625859
-    kappa: 0.00309693563017066
+    x: 586352.93659330113
+    y: 4140728.4488003491
+    theta: 1.2936272903495198
+    kappa: 0.00031822403933390244
     s: 29.429916580618542
     dkappa: 0
     ddkappa: 0
@@ -1895,10 +1895,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.81820126506
-    y: 4140728.3751218496
-    theta: 1.3193389834428551
-    kappa: 0.002997613924114048
+    x: 586352.99158918951
+    y: 4140728.6421801755
+    theta: 1.2937011677993637
+    kappa: 0.00035261513491999729
     s: 29.630964597156353
     dkappa: 0
     ddkappa: 0
@@ -1909,10 +1909,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.86823453254
-    y: 4140728.5698507512
-    theta: 1.3199448468359607
-    kappa: 0.0028982893088525734
+    x: 586353.04658668861
+    y: 4140728.8355656662
+    theta: 1.2937750474133987
+    kappa: 0.00038700723784624457
     s: 29.8320185025365
     dkappa: 0
     ddkappa: 0
@@ -1923,10 +1923,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.918138804
-    y: 4140728.76461886
-    theta: 1.3205252130774279
-    kappa: 0.0027940604753778982
+    x: 586353.10158583138
+    y: 4140729.0289569357
+    theta: 1.2938489292347544
+    kappa: 0.00042140036856579908
     s: 30.033078416326738
     dkappa: 0
     ddkappa: 0
@@ -1937,10 +1937,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.96778209787
-    y: 4140728.9594600378
-    theta: 1.3210542723757781
-    kappa: 0.0026799693169364497
+    x: 586353.15653368854
+    y: 4140729.2223691228
+    theta: 1.2939413774820048
+    kappa: 0.00044497590324830559
     s: 30.234144428493146
     dkappa: 0
     ddkappa: 0
@@ -1951,10 +1951,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.01742691232
-    y: 4140729.1543071838
-    theta: 1.3215833480005816
-    kappa: 0.0025658746638325312
+    x: 586353.2114793316
+    y: 4140729.4157883413
+    theta: 1.2940351947898014
+    kappa: 0.00046775596444535375
     s: 30.435216599400128
     dkappa: 0
     ddkappa: 0
@@ -1965,10 +1965,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.06707325508
-    y: 4140729.3491603276
-    theta: 1.3221124397365824
-    kappa: 0.0024517764986104079
+    x: 586353.266426666
+    y: 4140729.6092135143
+    theta: 1.2941290149852092
+    kappa: 0.00049053672686954488
     s: 30.63629495981041
     dkappa: 0
     ddkappa: 0
@@ -1979,10 +1979,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.11644380155
-    y: 4140729.5440898826
-    theta: 1.32257132425509
-    kappa: 0.0023280406079504838
+    x: 586353.32134489226
+    y: 4140729.8026533858
+    theta: 1.294228380119107
+    kappa: 0.00050848933306263714
     s: 30.837379510885036
     dkappa: 0
     ddkappa: 0
@@ -1993,10 +1993,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.1657669486
-    y: 4140729.7390378295
-    theta: 1.3230178375173551
-    kappa: 0.0022026017151098283
+    x: 586353.3762280466
+    y: 4140729.9961096197
+    theta: 1.2943343618599674
+    kappa: 0.00052068001312149113
     s: 31.038470224183381
     dkappa: 0
     ddkappa: 0
@@ -2007,10 +2007,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.21509159275
-    y: 4140729.9339916944
-    theta: 1.3234643643141446
-    kappa: 0.0020771590145261623
+    x: 586353.43111286685
+    y: 4140730.1895717252
+    theta: 1.2944403468186128
+    kappa: 0.0005328710632328522
     s: 31.23956704166314
     dkappa: 0
     ddkappa: 0
@@ -2021,10 +2021,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.26431759109
-    y: 4140730.1289766496
-    theta: 1.3238776941385713
-    kappa: 0.0019487987304486777
+    x: 586353.48599932913
+    y: 4140730.38303962
+    theta: 1.2945463349464703
+    kappa: 0.00054506247808349762
     s: 31.440669875680328
     dkappa: 0
     ddkappa: 0
@@ -2035,10 +2035,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.31337423436
-    y: 4140730.3240104117
-    theta: 1.3242343824552685
-    kappa: 0.001815463938688091
+    x: 586353.540815462
+    y: 4140730.5765335835
+    theta: 1.2946573429383694
+    kappa: 0.00054786048446181046
     s: 31.641778608989291
     dkappa: 0
     ddkappa: 0
@@ -2049,10 +2049,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.36243228114
-    y: 4140730.5190497525
-    theta: 1.3245910810081405
-    kappa: 0.0016821253330653813
+    x: 586353.59563175379
+    y: 4140730.7700334815
+    theta: 1.2947684523628145
+    kappa: 0.000550474590040915
     s: 31.842893094742688
     dkappa: 0
     ddkappa: 0
@@ -2063,10 +2063,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.41149168788
-    y: 4140730.714094501
-    theta: 1.3249477893934667
-    kappa: 0.0015487830305659746
+    x: 586353.65044956538
+    y: 4140730.9635387445
+    theta: 1.2948795648671536
+    kappa: 0.0005530887680973485
     s: 32.044013156491516
     dkappa: 0
     ddkappa: 0
@@ -2077,10 +2077,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.46036149934
-    y: 4140730.9091923637
-    theta: 1.3252178028411732
-    kappa: 0.0014118379354372569
+    x: 586353.70523106051
+    y: 4140731.1570598688
+    theta: 1.2949900562407948
+    kappa: 0.00055136435024671156
     s: 32.245138588185071
     dkappa: 0
     ddkappa: 0
@@ -2091,10 +2091,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.50920907094
-    y: 4140731.1043011006
-    theta: 1.3254771557873888
-    kappa: 0.0012744465233929627
+    x: 586353.75997707224
+    y: 4140731.3505963739
+    theta: 1.295099941178443
+    kappa: 0.00054540436492376068
     s: 32.446269154171006
     dkappa: 0
     ddkappa: 0
@@ -2105,10 +2105,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.55805782508
-    y: 4140731.2994145616
-    theta: 1.3257365150068234
-    kappa: 0.0011370517853298364
+    x: 586353.81472440937
+    y: 4140731.544137564
+    theta: 1.2952098287768377
+    kappa: 0.00053944423531942316
     s: 32.647404589195261
     dkappa: 0
     ddkappa: 0
@@ -2119,10 +2119,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.60684934119
-    y: 4140731.4945470444
-    theta: 1.3259571275577411
-    kappa: 0.0010014583391605768
+    x: 586353.86947072262
+    y: 4140731.7376837973
+    theta: 1.2953195026419779
+    kappa: 0.00053324592491955111
     s: 32.848544598402128
     dkappa: 0
     ddkappa: 0
@@ -2133,10 +2133,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.655558831
-    y: 4140731.6897044135
-    theta: 1.3261225818793634
-    kappa: 0.00086843055258120649
+    x: 586353.92414858728
+    y: 4140731.931253795
+    theta: 1.2954225452119426
+    kappa: 0.0005197446209065093
     s: 33.04968885733421
     dkappa: 0
     ddkappa: 0
@@ -2147,10 +2147,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.70426926389
-    y: 4140731.8848655615
-    theta: 1.3262880394083769
-    kappa: 0.00073540018958307253
+    x: 586353.978827511
+    y: 4140732.1248275419
+    theta: 1.2955255897791307
+    kappa: 0.00050624305540664691
     s: 33.250837011932425
     dkappa: 0
     ddkappa: 0
@@ -2161,10 +2161,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.75298054749
-    y: 4140732.0800301176
-    theta: 1.3264534998201365
-    kappa: 0.00060236750390216742
+    x: 586354.03350738925
+    y: 4140732.318404668
+    theta: 1.2956286361448113
+    kappa: 0.00049274125417222818
     s: 33.45198867853604
     dkappa: 0
     ddkappa: 0
@@ -2175,10 +2175,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.80161669524
-    y: 4140732.2752166037
-    theta: 1.3265361351651332
-    kappa: 0.00047902769812989437
+    x: 586354.0881515448
+    y: 4140732.5119950986
+    theta: 1.2957251169494448
+    kappa: 0.00047559646514604446
     s: 33.653143443882612
     dkappa: 0
     ddkappa: 0
@@ -2189,10 +2189,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.85024829069
-    y: 4140732.4704069616
-    theta: 1.3266131021536243
-    kappa: 0.00035634986989893896
+    x: 586354.14276738442
+    y: 4140732.7055962812
+    theta: 1.295816383887481
+    kappa: 0.0004555586237673519
     s: 33.854300865108044
     dkappa: 0
     ddkappa: 0
@@ -2203,10 +2203,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.8988804142
-    y: 4140732.6655994388
-    theta: 1.3266900699773538
-    kappa: 0.00023367071009208038
+    x: 586354.197383817
+    y: 4140732.8991995659
+    theta: 1.2959076518164248
+    kappa: 0.00043552056489290557
     s: 34.055460469746556
     dkappa: 0
     ddkappa: 0
@@ -2217,10 +2217,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.947501641
-    y: 4140732.8607963631
-    theta: 1.3267286496655317
-    kappa: 0.00011670910074908679
+    x: 586354.25199602
+    y: 4140733.0928057902
+    theta: 1.2959975862835951
+    kappa: 0.00041502197793894121
     s: 34.2566217557307
     dkappa: 0
     ddkappa: 0
@@ -2231,10 +2231,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.99610988458
-    y: 4140733.055997706
-    theta: 1.3267221904616364
-    kappa: 6.4560643943703907e-06
+    x: 586354.30655700422
+    y: 4140733.28642765
+    theta: 1.296072849257949
+    kappa: 0.00038946084292099364
     s: 34.457784191391326
     dkappa: 0
     ddkappa: 0
@@ -2245,10 +2245,10 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586355.04471827031
-    y: 4140733.2511996194
-    theta: 1.3267157312388476
-    kappa: -0.0001037972944535241
+    x: 586354.36111814808
+    y: 4140733.4800500763
+    theta: 1.2961481124530065
+    kappa: 0.00036389963313601973
     s: 34.658947215457637
     dkappa: 0
     ddkappa: 0
diff --git a/modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt b/modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt
old mode 100755
new mode 100644
index 70497c91dd..08cf9bc6c6
--- a/modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt
@@ -7,2254 +7,2254 @@ trajectory_point {
   path_point {
     x: 586361.375698
     y: 4140761.72259
+    z: 0
     theta: 1.2866508336700004
-    kappa: 0.0057515850184131972
-    s: 1.3877787807814457e-17
+    kappa: 0.0057515850184131946
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4250001907348633
-  a: -0.0767818497206998
+  v: 4.4250001907348606
+  a: -0.076781849720700013
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 586361.43761951046
-    y: 4140761.9348909445
-    theta: 1.2869556317262403
-    kappa: -0.0016105832906754968
-    s: 0.221146929856944
+    x: 586361.43761979812
+    y: 4140761.9348908989
+    theta: 1.2869461996934721
+    kappa: -0.0016457768594910194
+    s: 0.22114696638026682
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4207354800191068
-  a: -0.093764672575422855
+  v: 4.4207376699249918
+  a: -0.093677195821501982
   relative_time: 0.05
 }
 trajectory_point {
   path_point {
-    x: 586361.49958888872
-    y: 4140762.1469337652
-    theta: 1.2858607864975047
-    kappa: -0.007446357891593168
-    s: 0.44205950041536712
+    x: 586361.49959353916
+    y: 4140762.146932709
+    theta: 1.2858365621607781
+    kappa: -0.0075224534937758268
+    s: 0.44205979200454609
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4156279141107335
-  a: -0.11049605742539245
+  v: 4.4156366498379658
+  a: -0.11032182080698455
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 586361.561839472
-    y: 4140762.3586062221
-    theta: 1.2836754749843113
-    kappa: -0.011879304824213607
-    s: 0.66269588321314443
+    x: 586361.56185137073
+    y: 4140762.3586037448
+    theta: 1.2836312379370614
+    kappa: -0.011993478800886118
+    s: 0.66269686531037353
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.40969006490998
-  a: -0.12697600427060859
+  v: 4.4097096665295483
+  a: -0.12671572467714773
   relative_time: 0.15000000000000002
 }
 trajectory_point {
   path_point {
-    x: 586361.62454483914
-    y: 4140762.5698131365
-    theta: 1.2806846155716498
-    kappa: -0.015032408648735205
-    s: 0.88301487838316284
+    x: 586361.62456757517
+    y: 4140762.5698088231
+    theta: 1.2806140888210946
+    kappa: -0.015175736841233373
+    s: 0.88301720153807384
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4029345043170851
-  a: -0.14320451311107127
+  v: 4.4029692560555063
+  a: -0.14285890743199148
   relative_time: 0.2
 }
 trajectory_point {
   path_point {
-    x: 586361.68782528723
-    y: 4140762.7804745724
-    theta: 1.2771462135609628
-    kappa: -0.017028486850386472
-    s: 1.1029759146533211
+    x: 586361.68786317436
+    y: 4140762.7804680006
+    theta: 1.2770440635022533
+    kappa: -0.017188516558140715
+    s: 1.10298044273076
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3953738042322854
-  a: -0.1591815839467805
+  v: 4.3954279544716046
+  a: -0.15875136907151588
   relative_time: 0.25
 }
 trajectory_point {
   path_point {
-    x: 586361.75175244338
-    y: 4140762.9905247837
-    theta: 1.2732918668699851
-    kappa: -0.017990536801769902
-    s: 1.32253904934653
+    x: 586361.75181054242
+    y: 4140762.9905155124
+    theta: 1.2731549012092298
+    kappa: -0.018153287742387388
+    s: 1.3225468577343327
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.38702053655582
-  a: -0.17490721677773627
+  v: 4.38709829783361
+  a: -0.17439310959572085
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 586361.81635420548
-    y: 4140763.1999111883
-    theta: 1.2693275459465954
-    kappa: -0.018041522376664506
-    s: 1.5416649683807127
+    x: 586361.81643851788
+    y: 4140763.1998986751
+    theta: 1.2691545337032635
+    kappa: -0.018192678666150687
+    s: 1.5416773421974821
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.377887273187925
-  a: -0.19038141160393857
+  v: 4.3779928221972879
+  a: -0.18978412900460642
   relative_time: 0.35
 }
 trajectory_point {
   path_point {
-    x: 586361.88164144254
-    y: 4140763.408586551
-    theta: 1.2654382037429248
-    kappa: -0.017286909954262866
-    s: 1.7603149862688026
+    x: 586361.88180141046
+    y: 4140763.4085567021
+    theta: 1.2652457055781001
+    kappa: -0.017375376625637609
+    s: 1.7603334185716855
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.367986586028838
-  a: -0.20560416842538742
+  v: 4.3681240636184047
+  a: -0.20492442729817262
   relative_time: 0.39999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586361.94765803835
-    y: 4140763.6164930947
-    theta: 1.2618413461846265
-    kappa: -0.0157898508575292
-    s: 1.978451046118747
+    x: 586361.94785843487
+    y: 4140763.6164584504
+    theta: 1.2616329905082646
+    kappa: -0.0158425001059192
+    s: 1.9784772361112093
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3573310469787989
-  a: -0.22057548724208287
+  v: 4.3575045581527263
+  a: -0.21981400447641938
   relative_time: 0.44999999999999996
 }
 trajectory_point {
   path_point {
-    x: 586362.014191526
-    y: 4140763.823655542
-    theta: 1.2586370377809397
-    kappa: -0.01379190022575371
-    s: 2.1960357196335036
+    x: 586362.01443410537
+    y: 4140763.8236174369
+    theta: 1.2584199294530474
+    kappa: -0.013800223079484985
+    s: 2.1960715708731073
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3459332279380432
-  a: -0.23529536805402479
+  v: 4.3461468418560179
+  a: -0.23445286053934677
   relative_time: 0.49999999999999994
 }
 trajectory_point {
   path_point {
-    x: 586362.08113798732
-    y: 4140764.0300666187
-    theta: 1.2559112189714523
-    kappa: -0.011409278829047721
-    s: 2.4130322071110433
+    x: 586362.08142373571
+    y: 4140764.0300268633
+    theta: 1.2556934535570741
+    kappa: -0.01136627090694527
+    s: 2.4130798257172232
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3338057008068089
-  a: -0.2497638108612133
+  v: 4.334063450784047
+  a: -0.24884099548695479
   relative_time: 0.54999999999999993
 }
 trajectory_point {
   path_point {
-    x: 586362.14838197676
-    y: 4140764.2357242675
-    theta: 1.2537261430737654
-    kappa: -0.0087552133762177718
-    s: 2.6294043374443477
+    x: 586362.14871109254
+    y: 4140764.2356851459
+    theta: 1.2535168148235312
+    kappa: -0.0086549555474615611
+    s: 2.6294660303061872
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3209610374853336
-  a: -0.26398081566364839
+  v: 4.3212669209925778
+  a: -0.26297840931924343
   relative_time: 0.6
 }
 trajectory_point {
   path_point {
-    x: 586362.21580106218
-    y: 4140764.4406301053
-    theta: 1.252121421240282
-    kappa: -0.005939719685083179
-    s: 2.8451165681214103
+    x: 586362.21617278806
+    y: 4140764.440594431
+    theta: 1.2519309927068052
+    kappa: -0.0057768615876599991
+    s: 2.8451948411054189
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3074118098738561
-  a: -0.27794638246132997
+  v: 4.307769788537378
+  a: -0.27686510203621262
   relative_time: 0.65
 }
 trajectory_point {
   path_point {
-    x: 586362.28326946741
-    y: 4140764.6447881307
-    theta: 1.251115342369211
-    kappa: -0.0030695089476889589
-    s: 3.0601339852252374
+    x: 586362.28368197836
+    y: 4140764.6447592904
+    theta: 1.2509560043844192
+    kappa: -0.0028387609253455034
+    s: 3.0602315413831263
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2931705898726138
-  a: -0.29166051125425813
+  v: 4.2935845894742117
+  a: -0.29050107363786243
   relative_time: 0.70000000000000007
 }
 trajectory_point {
   path_point {
-    x: 586362.35066095751
-    y: 4140764.8482036418
-    theta: 1.25072251941195
-    kappa: -0.00025290043191363921
-    s: 3.2744223034338469
+    x: 586362.35110920633
+    y: 4140764.8481863271
+    theta: 1.2506688056422302
+    kappa: 3.1892204390648556e-05
+    s: 3.2745420412103043
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2782499493818431
-  a: -0.30512320204243282
+  v: 4.2787238598588466
+  a: -0.30388632412419286
   relative_time: 0.75000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586362.41782397137
-    y: 4140765.0508913216
-    theta: 1.2510003991525227
-    kappa: 0.0023497571659514297
-    s: 3.4879478660202672
+    x: 586362.41828261013
+    y: 4140765.050897189
+    theta: 1.251007238716298
+    kappa: 0.0026953944132338307
+    s: 3.4880928774607374
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2626624603017831
-  a: -0.3183344548258541
+  v: 4.2632001357470477
+  a: -0.31702085349520387
   relative_time: 0.80000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586362.48462062061
-    y: 4140765.252862019
-    theta: 1.2517778089960689
-    kappa: 0.0046477746543047991
-    s: 3.7006776448525409
+    x: 586362.48508118885
+    y: 4140765.2528973185
+    theta: 1.2518569903277541
+    kappa: 0.0050596047414659777
+    s: 3.7008512138109979
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.24642069453267
-  a: -0.33129426960452185
+  v: 4.2470259531945818
+  a: -0.3299046617508955
   relative_time: 0.8500000000000002
 }
 trajectory_point {
   path_point {
-    x: 586362.55095991842
-    y: 4140765.4541115244
-    theta: 1.2529782033108083
-    kappa: 0.0065464797262323165
-    s: 3.9125792403937214
+    x: 586362.55141035665
+    y: 4140765.4541838719
+    theta: 1.2531441007061648
+    kappa: 0.0070310909400828521
+    s: 3.9127848407404469
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2295372239747433
-  a: -0.34400264637843614
+  v: 4.2302138482572138
+  a: -0.34253774889126776
   relative_time: 0.90000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586362.616764935
-    y: 4140765.6546314368
-    theta: 1.2545102797043572
-    kappa: 0.0079542157115686223
-    s: 4.1236208817018731
+    x: 586362.61719143519
+    y: 4140765.6547491997
+    theta: 1.2547803947092024
+    kappa: 0.0085200710313026538
+    s: 4.1238621755312339
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.212024620528239
-  a: -0.35645958514759707
+  v: 4.2127763569907106
+  a: -0.35492011491632058
   relative_time: 0.95000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586362.68197708752
-    y: 4140765.854407792
-    theta: 1.2562695550443943
-    kappa: 0.0087832267467258217
-    s: 4.3337714264300722
+    x: 586362.68236250675
+    y: 4140765.8545805854
+    theta: 1.256665087190733
+    kappa: 0.0094406750509552738
+    s: 4.3340522622682949
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1938954560933963
-  a: -0.36866508591200442
+  v: 4.1947260154508372
+  a: -0.367051759826054
   relative_time: 1.0000000000000002
 }
 trajectory_point {
   path_point {
-    x: 586362.746559305
-    y: 4140766.0534200589
-    theta: 1.2581400554919357
-    kappa: 0.0089504569098159212
-    s: 4.5430003608264107
+    x: 586362.74682140886
+    y: 4140766.0536786346
+    theta: 1.2586764913834463
+    kappa: 0.0095916039839624313
+    s: 4.543324771839357
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1751623025704507
-  a: -0.38061914867165841
+  v: 4.17607535969336
+  a: -0.37893268362046806
   relative_time: 1.0500000000000003
 }
 trajectory_point {
   path_point {
-    x: 586362.81040541211
-    y: 4140766.2516703205
-    theta: 1.2599255429696592
-    kappa: 0.0081426835244783752
-    s: 4.7512777997339866
+    x: 586362.81056687667
+    y: 4140766.2520114109
+    theta: 1.2606034444755145
+    kappa: 0.00886071805833048
+    s: 4.7516500019349337
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1558377318596422
-  a: -0.39232177342655894
+  v: 4.1568369257740452
+  a: -0.39056288629956265
   relative_time: 1.1000000000000003
 }
 trajectory_point {
   path_point {
-    x: 586362.87364185508
-    y: 4140766.4490862302
-    theta: 1.2614311679911805
-    kappa: 0.00644522951264287
-    s: 4.9585744865909147
+    x: 586362.87367700762
+    y: 4140766.4495224813
+    theta: 1.26227499765904
+    kappa: 0.0072534910764648595
+    s: 4.9589988770483275
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1359343158612081
-  a: -0.403772960176706
+  v: 4.1370232497486592
+  a: -0.40194236786333792
   relative_time: 1.1500000000000004
 }
 trajectory_point {
   path_point {
-    x: 586362.93632933451
-    y: 4140766.6456179367
-    theta: 1.2625443353228534
-    kappa: 0.0043976641287530073
-    s: 5.164861793430318
+    x: 586362.9362058288
+    y: 4140766.6461642808
+    theta: 1.2635695986105604
+    kappa: 0.00515946874064883
+    s: 5.1653429484756286
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1154646264753847
-  a: -0.41497270892209959
+  v: 4.1166468676729666
+  a: -0.41307112831179377
   relative_time: 1.2000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586362.99853083165
-    y: 4140766.8412157409
-    theta: 1.2632542630994368
-    kappa: 0.0024336939461320683
-    s: 5.3701117208803346
+    x: 586362.99821556825
+    y: 4140766.8418874955
+    theta: 1.2644352946522506
+    kappa: 0.0030379530962305495
+    s: 5.3706543943157161
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.09444123560241
-  a: -0.42592101966273976
+  v: 4.0957203156027351
+  a: -0.42394916764493024
   relative_time: 1.2500000000000004
 }
 trajectory_point {
   path_point {
-    x: 586363.06030699
-    y: 4140767.0358314971
-    theta: 1.2636109987211432
-    kappa: 0.00084102638778492378
-    s: 5.5742968981641114
+    x: 586363.05976918107
+    y: 4140767.0366434432
+    theta: 1.2648977005097624
+    kappa: 0.0013228523871405345
+    s: 5.574906019470256
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0728767151425229
-  a: -0.43661789239862642
+  v: 4.07425612959373
+  a: -0.43457648586274727
   relative_time: 1.3000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586363.12170748564
-    y: 4140767.2294213311
-    theta: 1.2636493811747664
-    kappa: -0.00036116884700716003
-    s: 5.77739058309981
+    x: 586363.12093464192
+    y: 4140767.2303826995
+    theta: 1.2649987560202127
+    kappa: 4.1535178891515358e-05
+    s: 5.7780712556437059
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.05078363699596
-  a: -0.44706332712975966
+  v: 4.0522668457017152
+  a: -0.44495308296524494
   relative_time: 1.3500000000000005
 }
 trajectory_point {
   path_point {
-    x: 586363.18279093993
-    y: 4140767.4219392473
-    theta: 1.2634683540362177
-    kappa: -0.0012190353894899616
-    s: 5.9793666621006007
+    x: 586363.18177232286
+    y: 4140767.4230589909
+    theta: 1.2648943272898314
+    kappa: -0.0009339340633987604
+    s: 5.9801241613433067
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.02817457306296
-  a: -0.45725732385613949
+  v: 4.02976499998246
+  a: -0.45507895895242323
   relative_time: 1.4000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586363.24357598682
-    y: 4140767.6133525851
-    theta: 1.263157423111658
-    kappa: -0.0018108875068986361
-    s: 6.1801996501746688
+    x: 586363.24230124848
+    y: 4140767.6146397563
+    theta: 1.2646356672063861
+    kappa: -0.0016325708077214308
+    s: 6.1810394218790936
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0050620952437592
-  a: -0.46719988257776585
+  v: 4.0067631284917287
+  a: -0.46495411382428209
   relative_time: 1.4500000000000006
 }
 trajectory_point {
   path_point {
-    x: 586363.30407044129
-    y: 4140767.8036327651
-    theta: 1.2627603970413699
-    kappa: -0.0021632981668453228
-    s: 6.3798646909252108
+    x: 586363.30253399955
+    y: 4140767.8050951222
+    theta: 1.2642682854276457
+    kappa: -0.0020820087465580413
+    s: 6.3807923493638858
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9814587754385955
-  a: -0.47689100329463868
+  v: 3.9832737672852869
+  a: -0.47457854758082157
   relative_time: 1.5000000000000007
 }
 trajectory_point {
   path_point {
-    x: 586363.36427711963
-    y: 4140767.99275345
-    theta: 1.2623160265908353
-    kappa: -0.0023014594941331346
-    s: 6.5783375565504327
+    x: 586363.36248878087
+    y: 4140767.9943940719
+    theta: 1.2638305743484248
+    kappa: -0.0022828365511995863
+    s: 6.5793588827132909
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9573771855477076
-  a: -0.48633068600675805
+  v: 3.9593094524189016
+  a: -0.48395226022204163
   relative_time: 1.5500000000000007
 }
 trajectory_point {
   path_point {
-    x: 586363.42421312176
-    y: 4140768.1806843826
-    theta: 1.2618668128179316
-    kappa: -0.0022108971957606541
-    s: 6.7755946478435565
+    x: 586363.42217588122
+    y: 4140768.1825087117
+    theta: 1.2633795960171617
+    kappa: -0.0022566546672080942
+    s: 6.7767155876457066
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9328298974713323
-  a: -0.49551893071412412
+  v: 3.9348827199483383
+  a: -0.49307525174794231
   relative_time: 1.6000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586363.48385589884
-    y: 4140768.36740859
-    theta: 1.2614611036670964
-    kappa: -0.0019517646864469515
-    s: 6.971612994192812
+    x: 586363.481565252
+    y: 4140768.3694246025
+    theta: 1.262954005588514
+    kappa: -0.0020804381133541403
+    s: 6.9728396566823188
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9078294831097073
-  a: -0.50445573741673655
+  v: 3.9100061059293627
+  a: -0.50194752215852356
   relative_time: 1.6500000000000008
 }
 trajectory_point {
   path_point {
-    x: 586363.543177873
-    y: 4140768.5529113971
-    theta: 1.2611177107993861
-    kappa: -0.001569377983302739
-    s: 7.1663702535814444
+    x: 586363.54064308666
+    y: 4140768.5551228342
+    theta: 1.2625732754757719
+    kappa: -0.0017766515279921944
+    s: 7.1677089091470991
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.882388514363071
-  a: -0.51314110611459574
+  v: 3.8846921464177413
+  a: -0.51056907145378549
   relative_time: 1.7000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586363.60216132
-    y: 4140768.73717566
-    theta: 1.2608515535443223
-    kappa: -0.0010840827804101314
-    s: 7.3598447125877069
+    x: 586363.59939500724
+    y: 4140768.7395853391
+    theta: 1.2622538953285585
+    kappa: -0.0013661226981706639
+    s: 7.36130179116681
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.8565195631316604
-  a: -0.52157503680770123
+  v: 3.8589533774692395
+  a: -0.518939899633728
   relative_time: 1.7500000000000009
 }
 trajectory_point {
   path_point {
-    x: 586363.66079033318
-    y: 4140768.9201843166
-    theta: 1.2606886862580413
-    kappa: -0.00050759016364078813
-    s: 7.5520152863848677
+    x: 586363.65781604324
+    y: 4140768.92279171
+    theta: 1.2620550614648622
+    kappa: -0.00084638134978688912
+    s: 7.5535973756710044
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.8302352013157135
-  a: -0.52975752949605337
+  v: 3.8328023351396237
+  a: -0.52706000669835107
   relative_time: 1.8000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586363.71903380309
-    y: 4140769.1019258434
-    theta: 1.260669122786781
-    kappa: 0.00013822289653533557
-    s: 7.7428615187412078
+    x: 586363.7158580086
+    y: 4140769.1047359915
+    theta: 1.2619531300804927
+    kappa: -0.0002841948785838182
+    s: 7.7445753623920179
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.8035480008154678
-  a: -0.53768858417965215
+  v: 3.8062515554846597
+  a: -0.53492939264765482
   relative_time: 1.850000000000001
 }
 trajectory_point {
   path_point {
-    x: 586363.7768582612
-    y: 4140769.2823901442
-    theta: 1.2607621986483184
-    kappa: 0.00080931469377382159
-    s: 7.932363582020014
+    x: 586363.77350188291
+    y: 4140769.2854035995
+    theta: 1.2619488567336332
+    kappa: 0.00030368595771210444
+    s: 7.9342160778649768
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.7764705335311608
-  a: -0.54536820085849735
+  v: 3.7793135745601134
+  a: -0.542548057481639
   relative_time: 1.900000000000001
 }
 trajectory_point {
   path_point {
-    x: 586363.83424405567
-    y: 4140769.461563352
-    theta: 1.2609672847629256
-    kappa: 0.0014912595467580082
-    s: 8.1205022771795932
+    x: 586363.83073047374
+    y: 4140769.4647800275
+    theta: 1.2620427090458051
+    kappa: 0.00090257082151900354
+    s: 8.1225004754278
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.7490153713630305
-  a: -0.5527963795325892
+  v: 3.7520009284217508
+  a: -0.54991600120030393
   relative_time: 1.9500000000000011
 }
 trajectory_point {
   path_point {
-    x: 586363.89116312528
-    y: 4140769.6394349514
-    theta: 1.2613003162852261
-    kappa: 0.0021675736729082234
-    s: 8.3072590337732581
+    x: 586363.88750061858
+    y: 4140769.6428597048
+    theta: 1.2622852733910277
+    kappa: 0.0014774884799292445
+    s: 8.3094101352211869
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.7211950862113135
-  a: -0.5599731202019288
+  v: 3.7243261531253387
+  a: -0.55703322380365083
   relative_time: 2.0000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586363.94756781717
-    y: 4140769.8160013352
-    theta: 1.2617778537769659
-    kappa: 0.00280131013194529
-    s: 8.49261592824933
+    x: 586363.94379971409
+    y: 4140769.8196279779
+    theta: 1.2626141263170743
+    kappa: 0.00200838627329868
+    s: 8.4949272824303428
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.6930232991023062
-  a: -0.566860367720095
+  v: 3.696302829308487
+  a: -0.563861935701574
   relative_time: 2.0500000000000007
 }
 trajectory_point {
   path_point {
-    x: 586364.00345705147
-    y: 4140769.9912446947
-    theta: 1.2623487267401607
-    kappa: 0.0033834794610454952
-    s: 8.6765557193016516
+    x: 586363.99961612886
+    y: 4140769.9950705944
+    theta: 1.2630205465625839
+    kappa: 0.0024854281830086877
+    s: 8.6790348220417073
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.6645128490831311
-  a: -0.57351963660862759
+  v: 3.6679437388720664
+  a: -0.57046388858639108
   relative_time: 2.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586364.05882064137
-    y: 4140770.1651507714
-    theta: 1.2630034255061562
-    kappa: 0.003905520681645026
-    s: 8.8590617587579974
+    x: 586364.05492330436
+    y: 4140770.1691786642
+    theta: 1.2635082569863916
+    kappa: 0.0028829848310917288
+    s: 8.8617162491730674
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.635675135085271
-  a: -0.57995092686752658
+  v: 3.639260219766733
+  a: -0.57683908245810211
   relative_time: 2.1500000000000004
 }
 trajectory_point {
   path_point {
-    x: 586364.11360827542
-    y: 4140770.3377186307
-    theta: 1.2637521220227168
-    kappa: 0.0043239237850781565
-    s: 9.0401179683927229
+    x: 586364.10968184355
+    y: 4140770.3419479094
+    theta: 1.2640647194251642
+    kappa: 0.00315127548869881
+    s: 9.0429556258397419
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.6065215560402062
-  a: -0.586154238496792
+  v: 3.6102636099431415
+  a: -0.58298751731670706
   relative_time: 2.2
 }
 trajectory_point {
   path_point {
-    x: 586364.16780248564
-    y: 4140770.5089373831
-    theta: 1.2645635822640835
-    kappa: 0.0046203533467384164
-    s: 9.2197088399267546
+    x: 586364.16391748365
+    y: 4140770.51335398
+    theta: 1.2646459141277873
+    kappa: 0.003312924205724755
+    s: 9.2227375809545862
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.5770635108794191
-  a: -0.59212957149642387
+  v: 3.5809652473519478
+  a: -0.588909193162206
   relative_time: 2.25
 }
 trajectory_point {
   path_point {
-    x: 586364.22143420379
-    y: 4140770.6787814684
-    theta: 1.2654023998616377
-    kappa: 0.0048092154327693485
-    s: 9.3978194350275928
+    x: 586364.21762715385
+    y: 4140770.6833823016
+    theta: 1.2652429614619
+    kappa: 0.0033651508793456548
+    s: 9.4010473103279857
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.5473123985343911
-  a: -0.597876925866422
+  v: 3.5513764699438064
+  a: -0.594604109994599
   relative_time: 2.3
 }
 trajectory_point {
   path_point {
-    x: 586364.27449059684
-    y: 4140770.8472398105
-    theta: 1.2662579460843377
-    kappa: 0.0048734723230263672
-    s: 9.5744353853093145
+    x: 586364.27075315814
+    y: 4140770.8520360738
+    theta: 1.2658245779249546
+    kappa: 0.0032067919437216118
+    s: 9.5778705766678609
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.5172796179366044
-  a: -0.60339630160678681
+  v: 3.5215086156693731
+  a: -0.60007226781388578
   relative_time: 2.3499999999999996
 }
 trajectory_point {
   path_point {
-    x: 586364.32689549669
-    y: 4140771.0143217137
-    theta: 1.2670995866778425
-    kappa: 0.0047258438039158787
-    s: 9.7495428923325651
+    x: 586364.32334741927
+    y: 4140771.0192845124
+    theta: 1.2663569707805555
+    kappa: 0.0028902308140435662
+    s: 9.7531937095796621
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.48697656801754
-  a: -0.60868769871751782
+  v: 3.4913730224793031
+  a: -0.60531366662006669
   relative_time: 2.3999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586364.37874062336
-    y: 4140771.17998437
-    theta: 1.2678954753717646
-    kappa: 0.0044382184177551385
-    s: 9.92312872760457
+    x: 586364.37542038411
+    y: 4140771.1851105797
+    theta: 1.2668347482862126
+    kappa: 0.0024318908714855507
+    s: 9.9270036055663731
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.45641464770868
-  a: -0.6137511171986153
+  v: 3.4609810283242517
+  a: -0.61032830641314151
   relative_time: 2.4499999999999993
 }
 trajectory_point {
   path_point {
-    x: 586364.43002896116
-    y: 4140771.344213557
-    theta: 1.2686420247490462
-    kappa: 0.00402397316498146
-    s: 10.095180232579127
+    x: 586364.42694811244
+    y: 4140771.3495086096
+    theta: 1.2671903275363912
+    kappa: 0.0017411158627099852
+    s: 10.099287728028514
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.4256052559415058
-  a: -0.61858655705007926
+  v: 3.4303439711548744
+  a: -0.61511618719311023
   relative_time: 2.4999999999999991
 }
 trajectory_point {
   path_point {
-    x: 586364.4806880313
-    y: 4140771.5070190942
-    theta: 1.2692794117891866
-    kappa: 0.0036342225717344821
-    s: 10.265685318656605
+    x: 586364.4779724984
+    y: 4140771.512452885
+    theta: 1.2674214352037503
+    kappa: 0.0010894848169336704
+    s: 10.270034107264133
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.3945597916474997
-  a: -0.62319401827190957
+  v: 3.3994731889218262
+  a: -0.61967730895997286
   relative_time: 2.5499999999999989
 }
 trajectory_point {
   path_point {
-    x: 586364.53081052424
-    y: 4140771.6683599437
-    theta: 1.2698673275158356
-    kappa: 0.0033410324720150705
-    s: 10.434632467183953
+    x: 586364.52850931371
+    y: 4140771.6739265178
+    theta: 1.2675694739973764
+    kappa: 0.00049787435704484
+    s: 10.439231340468812
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.3632896537581423
-  a: -0.62757350086410624
+  v: 3.3683800195757621
+  a: -0.62401167171372951
   relative_time: 2.5999999999999988
 }
 trajectory_point {
   path_point {
-    x: 586364.58040113281
-    y: 4140771.8282231763
-    theta: 1.2704128742347149
-    kappa: 0.0031335123267220311
-    s: 10.602010729454687
+    x: 586364.57856035628
+    y: 4140771.8339175833
+    theta: 1.2676089692171062
+    kappa: 7.7615861154332977e-05
+    s: 10.606868591735669
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.3318062412049163
-  a: -0.63172500482666938
+  v: 3.3370758010673383
+  a: -0.62811927545438018
   relative_time: 2.6499999999999986
 }
 trajectory_point {
   path_point {
-    x: 586364.62939760764
-    y: 4140771.9866171568
-    theta: 1.2709236464142448
-    kappa: 0.0030707940975838463
-    s: 10.767809726708903
+    x: 586364.62814343558
+    y: 4140771.99240975
+    theta: 1.2675923767037161
+    kappa: -0.00021934453205137025
+    s: 10.772935592055349
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.3001209529193027
-  a: -0.635648530159599
+  v: 3.3055718713472095
+  a: -0.63200012018192475
   relative_time: 2.6999999999999984
 }
 trajectory_point {
   path_point {
-    x: 586364.67786408775
-    y: 4140772.1435116939
-    theta: 1.2714287139314042
-    kappa: 0.0030778075428358155
-    s: 10.932019650133268
+    x: 586364.67725741677
+    y: 4140772.1493932065
+    theta: 1.2675436845193342
+    kappa: -0.00043530913518849396
+    s: 10.937422639316036
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.2682451878327838
-  a: -0.63934407686289485
+  v: 3.2738795683660311
+  a: -0.63565420589636323
   relative_time: 2.7499999999999982
 }
 trajectory_point {
   path_point {
-    x: 586364.72580529889
-    y: 4140772.2988956575
-    theta: 1.2719281482652587
-    kappa: 0.0031460217123977429
-    s: 11.094631260861023
+    x: 586364.72591086337
+    y: 4140772.3048557006
+    theta: 1.2674612825386438
+    kappa: -0.00051892819835253632
+    s: 11.100320598303444
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.2361903448768405
-  a: -0.64281164493655729
+  v: 3.2420102300744587
+  a: -0.63908153259769573
   relative_time: 2.799999999999998
 }
 trajectory_point {
   path_point {
-    x: 586364.773146918
-    y: 4140772.4527828284
-    theta: 1.2724558552294367
-    kappa: 0.0033247782077683117
-    s: 11.255635889971984
+    x: 586364.77409914683
+    y: 4140772.4587897025
+    theta: 1.2673768342649638
+    kappa: -0.00053001890354094014
+    s: 11.261620900700819
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.2039678229829556
-  a: -0.64605123438058609
+  v: 3.2099751944231469
+  a: -0.64228210028592225
   relative_time: 2.8499999999999979
 }
 trajectory_point {
   path_point {
-    x: 586364.81995863828
-    y: 4140772.6051432127
-    theta: 1.2730018653238917
-    kappa: 0.0035427896183693177
-    s: 11.415025438492544
+    x: 586364.82181654114
+    y: 4140772.6111886185
+    theta: 1.2672911362582797
+    kappa: -0.00048848617380314874
+    s: 11.421315545088941
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.1715890210826094
-  a: -0.64906284519498125
+  v: 3.1777857993627516
+  a: -0.64525590896104268
   relative_time: 2.8999999999999977
 }
 trajectory_point {
   path_point {
-    x: 586364.86621966783
-    y: 4140772.7559753191
-    theta: 1.2735745886741645
-    kappa: 0.0038135247699413906
-    s: 11.572792377395661
+    x: 586364.86906997638
+    y: 4140772.762042488
+    theta: 1.2672298052219977
+    kappa: -0.0003489084145917561
+    s: 11.579397096946119
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.1390653381072848
-  a: -0.65184647737974288
+  v: 3.1454533828439279
+  a: -0.648002958623057
   relative_time: 2.9499999999999975
 }
 trajectory_point {
   path_point {
-    x: 586364.91188132786
-    y: 4140772.9052866967
-    theta: 1.2742057921497798
-    kappa: 0.0041449148284948842
-    s: 11.72892974760088
+    x: 586364.9158429564
+    y: 4140772.9113492789
+    theta: 1.2671864678796576
+    kappa: -0.00018140046490551284
+    s: 11.735858688648202
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.1064081729884627
-  a: -0.65440213093487087
+  v: 3.1129892828173311
+  a: -0.65052324927196525
   relative_time: 2.9999999999999973
 }
 trajectory_point {
   path_point {
-    x: 586364.95700550487
-    y: 4140773.0530516803
-    theta: 1.2748630401729331
-    kappa: 0.0044986160195933413
-    s: 11.883431159974307
+    x: 586364.96213564533
+    y: 4140773.0591023392
+    theta: 1.2671713972817622
+    kappa: 3.0539894948988825e-05
+    s: 11.890694019468565
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.0736289246576254
-  a: -0.65672980586036533
+  v: 3.0804048372336168
+  a: -0.65281678090776762
   relative_time: 3.0499999999999972
 }
 trajectory_point {
   path_point {
-    x: 586365.00152051344
-    y: 4140773.1992860204
-    theta: 1.2755874139546211
-    kappa: 0.00489631386461982
-    s: 12.036290795328632
+    x: 586365.00793794682
+    y: 4140773.205298821
+    theta: 1.2672009708718943
+    kappa: 0.00027806880642410471
+    s: 12.043897355578121
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.0407389920462538
-  a: -0.65882950215622615
+  v: 3.0477113840434398
+  a: -0.65488355353046379
   relative_time: 3.099999999999997
 }
 trajectory_point {
   path_point {
-    x: 586365.04545480956
-    y: 4140773.3439754173
-    theta: 1.2763614262394967
-    kappa: 0.0053136323456171395
-    s: 12.187503404423113
+    x: 586365.053248506
+    y: 4140773.3499337356
+    theta: 1.2672508134799489
+    kappa: 0.00053811694995794049
+    s: 12.195463530045313
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.00774977408583
-  a: -0.66070121982245344
+  v: 3.0149202611974557
+  a: -0.656723567140054
   relative_time: 3.1499999999999968
 }
 trajectory_point {
   path_point {
-    x: 586365.0888277234
-    y: 4140773.4871091112
-    theta: 1.2771732946838197
-    kappa: 0.0057456650800298452
-    s: 12.337064307963587
+    x: 586365.0980505764
+    y: 4140773.4930075067
+    theta: 1.2673639795971032
+    kappa: 0.000820971864231643
+    s: 12.345387942836114
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.9746726697078358
-  a: -0.66234495885904709
+  v: 2.98204280664632
+  a: -0.65833682173653818
   relative_time: 3.1999999999999966
 }
 trajectory_point {
   path_point {
-    x: 586365.13153661392
-    y: 4140773.6287137028
-    theta: 1.2780744596320379
-    kappa: 0.0061968896624692961
-    s: 12.484969396602462
+    x: 586365.142347508
+    y: 4140773.6345148566
+    theta: 1.2675029174612131
+    kappa: 0.0011028644644314541
+    s: 12.493666560814034
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.9415190778437528
-  a: -0.66376071926600722
+  v: 2.9490903583406873
+  a: -0.65972331731991629
   relative_time: 3.2499999999999964
 }
 trajectory_point {
   path_point {
-    x: 586365.173703433
-    y: 4140773.76874858
-    theta: 1.2789979273071106
-    kappa: 0.0066483387076275921
-    s: 12.631215130938717
+    x: 586365.18612986407
+    y: 4140773.7744551064
+    theta: 1.2676846568266007
+    kappa: 0.001385139200920349
+    s: 12.640295917740119
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.9083003974250627
-  a: -0.6649485010433337
+  v: 2.9160742542312135
+  a: -0.66088305389018842
   relative_time: 3.2999999999999963
 }
 trajectory_point {
   path_point {
-    x: 586365.21520467964
-    y: 4140773.9072477105
-    theta: 1.2800042234421543
-    kappa: 0.0071040279014599082
-    s: 12.775798541517915
+    x: 586365.22938505025
+    y: 4140773.9128291495
+    theta: 1.2679092077123104
+    kappa: 0.001653689912366797
+    s: 12.785273114272936
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.8750280273832471
-  a: -0.66590830419102665
+  v: 2.8830058322685539
+  a: -0.66181603144735435
   relative_time: 3.3499999999999961
 }
 trajectory_point {
   path_point {
-    x: 586365.25611818477
-    y: 4140774.0441849874
-    theta: 1.2810472931465344
-    kappa: 0.0075509627123668752
-    s: 12.918717228832181
+    x: 586365.27212823287
+    y: 4140774.0496297986
+    theta: 1.2681555439308794
+    kappa: 0.0019134533036671494
+    s: 12.928595817968596
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.8417133666497874
-  a: -0.666640128709086
+  v: 2.8498964304033634
+  a: -0.6625222499914144
   relative_time: 3.3999999999999959
 }
 trajectory_point {
   path_point {
-    x: 586365.29640269757
-    y: 4140774.17957082
-    theta: 1.2821450454892491
-    kappa: 0.00798783754569738
-    s: 13.059969363320223
+    x: 586365.31431909627
+    y: 4140774.1848677834
+    theta: 1.2684526199586763
+    kappa: 0.0021379996367534189
+    s: 13.070262263280739
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.8083678141561657
-  a: -0.66714397459751174
+  v: 2.8167573865862976
+  a: -0.66300170952236837
   relative_time: 3.4499999999999957
 }
 trajectory_point {
   path_point {
-    x: 586365.3360457893
-    y: 4140774.3134072544
-    theta: 1.2832936401204849
-    kappa: 0.0083943588566534125
-    s: 13.199553685367318
+    x: 586365.356002286
+    y: 4140774.3185279039
+    theta: 1.2687572042344586
+    kappa: 0.00235109225941595
+    s: 13.210271251560538
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.7750027688338639
-  a: -0.66741984185630387
+  v: 2.7836000387680118
+  a: -0.66325441004021624
   relative_time: 3.4999999999999956
 }
 trajectory_point {
   path_point {
-    x: 586365.375121465
-    y: 4140774.4456716292
-    theta: 1.2844645122669229
-    kappa: 0.0087818024850519361
-    s: 13.337469505305322
+    x: 586365.3971198973
+    y: 4140774.4506275384
+    theta: 1.2691022516319339
+    kappa: 0.00250968388574989
+    s: 13.348622151056698
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.741629629614363
-  a: -0.66746773048546248
+  v: 2.7504357248991615
+  a: -0.66328035154495812
   relative_time: 3.5499999999999954
 }
 trajectory_point {
   path_point {
-    x: 586365.41347265011
-    y: 4140774.5764098037
-    theta: 1.2856978739185998
-    kappa: 0.0091174590986097346
-    s: 13.473716703412659
+    x: 586365.43772411789
+    y: 4140774.5811503432
+    theta: 1.2694503981431291
+    kappa: 0.0026483010667668169
+    s: 13.485314896915455
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.7082597954291447
-  a: -0.66728764048498745
+  v: 2.7172757829304013
+  a: -0.66307953403659392
   relative_time: 3.5999999999999952
 }
 trajectory_point {
   path_point {
-    x: 586365.45129895583
-    y: 4140774.7055635303
-    theta: 1.2869329810338539
-    kappa: 0.0094386553583535281
-    s: 13.608295729914332
+    x: 586365.47777053108
+    y: 4140774.7101106527
+    theta: 1.2698173032566629
+    kappa: 0.0027278263361644618
+    s: 13.620349991180582
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.6749046652096915
-  a: -0.66687957185487878
+  v: 2.6841315508123875
+  a: -0.66265195751512362
   relative_time: 3.649999999999995
 }
 trajectory_point {
   path_point {
-    x: 586365.48838662356
-    y: 4140774.8331960808
-    theta: 1.2882167886639864
-    kappa: 0.0096909951618871747
-    s: 13.741207604981915
+    x: 586365.51729422354
+    y: 4140774.8374986555
+    theta: 1.2701827494704663
+    kappa: 0.0027733271213746915
+    s: 13.753728502793383
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.6415756378874846
-  a: -0.66624352459513658
+  v: 2.6510143664957742
+  a: -0.66199762198054735
   relative_time: 3.6999999999999948
 }
 trajectory_point {
   path_point {
-    x: 586365.52497951209
-    y: 4140774.959237935
-    theta: 1.2894903415147674
-    kappa: 0.0099329009475348732
-    s: 13.872453918733557
+    x: 586365.55627065338
+    y: 4140774.9633236742
+    theta: 1.270549162968005
+    kappa: 0.0027584965803926212
+    s: 13.885452067592693
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.6082841123940055
-  a: -0.66537949870576085
+  v: 2.6179355679312177
+  a: -0.66111652743286509
   relative_time: 3.7499999999999947
 }
 trajectory_point {
   path_point {
-    x: 586365.56079160213
-    y: 4140775.0837739753
-    theta: 1.290800947047396
-    kappa: 0.010094562906680765
-    s: 14.002036831233983
+    x: 586365.59472132486
+    y: 4140775.0875813188
+    theta: 1.270903434210682
+    kappa: 0.0026965103209224627
+    s: 14.015522888314882
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.5750414876607355
-  a: -0.66428749418675148
+  v: 2.584906493069373
+  a: -0.66000867387207673
   relative_time: 3.7999999999999945
 }
 trajectory_point {
   path_point {
-    x: 586365.59611491417
-    y: 4140775.2067225627
-    theta: 1.2920972427604724
-    kappa: 0.010244755385059688
-    s: 14.129959072494492
+    x: 586365.632635026
+    y: 4140775.2102779569
+    theta: 1.2712430988039072
+    kappa: 0.002571856002084782
+    s: 14.143943734593851
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.5418591626191578
-  a: -0.66296751103810858
+  v: 2.5519384798608957
+  a: -0.65867406129818229
   relative_time: 3.8499999999999943
 }
 trajectory_point {
   path_point {
-    x: 586365.63072431518
-    y: 4140775.3281515818
-    theta: 1.2933981068157492
-    kappa: 0.010312263764001753
-    s: 14.256223942472953
+    x: 586365.67002863926
+    y: 4140775.3314118339
+    theta: 1.2715576569539759
+    kappa: 0.0023924329621510011
+    s: 14.270717942961038
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.5087485362007524
-  a: -0.661419549259832
+  v: 2.51904286625644
+  a: -0.65711268971118186
   relative_time: 3.8999999999999941
 }
 trajectory_point {
   path_point {
-    x: 586365.6648461828
-    y: 4140775.4480001531
-    theta: 1.2946827811495563
-    kappa: 0.010368387567033125
-    s: 14.38083531107381
+    x: 586365.70689618913
+    y: 4140775.4509888776
+    theta: 1.2718427624081592
+    kappa: 0.0021464034253941273
+    s: 14.395849416845408
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.4757210073370017
-  a: -0.65964360885192186
+  v: 2.4862309902066628
+  a: -0.65532455911107546
   relative_time: 3.949999999999994
 }
 trajectory_point {
   path_point {
-    x: 586365.6982152157
-    y: 4140775.5663481
-    theta: 1.2959578858786764
-    kappa: 0.010331552690074623
-    s: 14.503797618148086
+    x: 586365.74325627461
+    y: 4140775.5690080123
+    theta: 1.2720909469611592
+    kappa: 0.0018443718772764852
+    s: 14.519342626573458
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.4427879749593879
-  a: -0.65763968981437815
+  v: 2.4535141896622181
+  a: -0.65330966949786284
   relative_time: 3.9999999999999938
 }
 trajectory_point {
   path_point {
-    x: 586365.73112570262
-    y: 4140775.6831172104
-    theta: 1.2972147653208226
-    kappa: 0.010290238050575324
-    s: 14.625115913998689
+    x: 586365.77910129924
+    y: 4140775.68547689
+    theta: 1.2722900449544783
+    kappa: 0.0014645984897338933
+    s: 14.641202649713707
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.4099631571891931
-  a: -0.65532391486893948
+  v: 2.420906111134
+  a: -0.650984647156211
   relative_time: 4.0499999999999936
 }
 trajectory_point {
   path_point {
-    x: 586365.76332981838
-    y: 4140775.7983830022
-    theta: 1.2984298136255563
-    kappa: 0.010144447528528387
-    s: 14.744795936444772
+    x: 586365.81445618463
+    y: 4140775.8003938566
+    theta: 1.2724486644008142
+    kappa: 0.0010385394855175552
+    s: 14.761435247266661
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.3772584940849559
-  a: -0.65283350317597155
+  v: 2.3884186073940725
+  a: -0.64848667796615989
   relative_time: 4.0999999999999934
 }
 trajectory_point {
   path_point {
-    x: 586365.79509477457
-    y: 4140775.9120769426
-    theta: 1.2996282926464655
-    kappa: 0.010000645116393071
-    s: 14.862843911515572
+    x: 586365.84931155317
+    y: 4140775.9137683315
+    theta: 1.2725297186016471
+    kappa: 0.0005157993672359264
+    s: 14.880046664155293
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.344682717484055
-  a: -0.65016845473547236
+  v: 2.3560603257848576
+  a: -0.6458157619277104
   relative_time: 4.1499999999999932
 }
 trajectory_point {
   path_point {
-    x: 586365.8261430508
-    y: 4140776.0242830813
-    theta: 1.3007658278563845
-    kappa: 0.0097547177750010681
-    s: 14.979266501832189
+    x: 586365.88369211054
+    y: 4140776.0255997004
+    theta: 1.2725818903679116
+    kappa: -2.6665151190832779e-05
+    s: 14.997043577669698
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.3122445592238678
-  a: -0.64732876954744178
+  v: 2.3238399136487748
+  a: -0.64297189904086249
   relative_time: 4.1999999999999931
 }
 trajectory_point {
   path_point {
-    x: 586365.85673858016
-    y: 4140776.1349354284
-    theta: 1.3018841828169263
-    kappa: 0.0095043173630524128
-    s: 15.094070806607593
+    x: 586365.91759870667
+    y: 4140776.13589514
+    theta: 1.2725122049561781
+    kappa: -0.00067872085492186307
+    s: 15.112433097467095
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.27995275114177
-  a: -0.64431444761188
+  v: 2.2917660183282447
+  a: -0.63995508930561629
   relative_time: 4.2499999999999929
 }
 trajectory_point {
   path_point {
-    x: 586365.88672234688
-    y: 4140776.2440855205
-    theta: 1.3029423731304555
-    kappa: 0.0093122435872773426
-    s: 15.207264361646624
+    x: 586365.9510351948
+    y: 4140776.2446613563
+    theta: 1.2724434857182392
+    kappa: -0.0013217359419386429
+    s: 15.226222765571821
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.2478160250751387
-  a: -0.64112548892878685
+  v: 2.2598472871656869
+  a: -0.63676533272197156
   relative_time: 4.2999999999999927
 }
 trajectory_point {
   path_point {
-    x: 586365.9162209311
-    y: 4140776.3517067786
-    theta: 1.3039708068902338
-    kappa: 0.0091410981721683961
-    s: 15.318855139345988
+    x: 586365.98403901979
+    y: 4140776.3518951745
+    theta: 1.2722244938199452
+    kappa: -0.0018674493873842659
+    s: 15.338420556375336
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.2158431128613505
-  a: -0.63776189349816248
+  v: 2.2280923675035211
+  a: -0.63340262928992852
   relative_time: 4.3499999999999925
 }
 trajectory_point {
   path_point {
-    x: 586365.94517177239
-    y: 4140776.4578248337
-    theta: 1.3049724520966814
-    kappa: 0.0090381373341820448
-    s: 15.428851548694261
+    x: 586366.01657803822
+    y: 4140776.4576152773
+    theta: 1.2720043560218905
+    kappa: -0.0024029873155862664
+    s: 15.449034876636219
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.1840427463377816
-  a: -0.63422366132000685
+  v: 2.1965099066841676
+  a: -0.62986697900948718
   relative_time: 4.3999999999999924
 }
 trajectory_point {
   path_point {
-    x: 586365.973579962
-    y: 4140776.5624474655
-    theta: 1.3059476553929541
-    kappa: 0.00900191418997555
-    s: 15.537262435271892
+    x: 586366.04869948025
+    y: 4140776.56181636
+    theta: 1.2717106470377992
+    kappa: -0.0028150300005037928
+    s: 15.558074565480172
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.1524236573418087
-  a: -0.63051079239431984
+  v: 2.1651085520500457
+  a: -0.62615838188064732
   relative_time: 4.4499999999999922
 }
 trajectory_point {
   path_point {
-    x: 586366.00153571588
-    y: 4140776.6655595866
-    theta: 1.306909472474026
-    kappa: 0.0089852636154445272
-    s: 15.644097081251191
+    x: 586366.08038033207
+    y: 4140776.6645152159
+    theta: 1.2713866220169541
+    kappa: -0.0031689921650821578
+    s: 15.665548894400018
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.1209945777108086
-  a: -0.62662328672110157
+  v: 2.1338969509435763
+  a: -0.62227683790340915
   relative_time: 4.499999999999992
 }
 trajectory_point {
   path_point {
-    x: 586366.02889736148
-    y: 4140776.7672095653
-    theta: 1.3078608958936762
-    kappa: 0.0090579067806185257
-    s: 15.749365205396344
+    x: 586366.11163097026
+    y: 4140776.7657187786
+    theta: 1.2710449913352511
+    kappa: -0.0034656655153077051
+    s: 15.771467567255698
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.0897642392821578
-  a: -0.622561144300352
+  v: 2.1028837507071785
+  a: -0.61822234707777257
   relative_time: 4.5499999999999918
 }
 trajectory_point {
   path_point {
-    x: 586366.055854471
-    y: 4140776.8673566724
-    theta: 1.3087982527617512
-    kappa: 0.00912947593228599
-    s: 15.853076963063401
+    x: 586366.142483939
+    y: 4140776.8654276012
+    theta: 1.2706624849981092
+    kappa: -0.0036507105598571794
+    s: 15.875840720274276
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.0587413738932323
-  a: -0.61832436513207123
+  v: 2.0720775986832725
+  a: -0.61399490940373758
   relative_time: 4.5999999999999917
 }
 trajectory_point {
   path_point {
-    x: 586366.0822340434
-    y: 4140776.9660582235
-    theta: 1.309743899674378
-    kappa: 0.0092658076344973375
-    s: 15.955242946200284
+    x: 586366.17288317229
+    y: 4140776.9636700689
+    theta: 1.2702856039562194
+    kappa: -0.0038330342616771113
+    s: 15.978678922049939
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.0279347133814096
-  a: -0.61391294921625916
+  v: 2.0414871422142782
+  a: -0.60959452488130417
   relative_time: 4.6499999999999915
 }
 trajectory_point {
   path_point {
-    x: 586366.10816451407
-    y: 4140777.0632912247
-    theta: 1.3106820326528483
-    kappa: 0.00941987733074811
-    s: 16.055874183346784
+    x: 586366.20291380025
+    y: 4140777.06043131
+    theta: 1.2698913168104295
+    kappa: -0.0038820306528768844
+    s: 16.079993173543993
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9973529895840656
-  a: -0.60932689655291583
+  v: 2.0111210286426155
+  a: -0.60502119351047245
   relative_time: 4.6999999999999913
 }
 trajectory_point {
   path_point {
-    x: 586366.13364027464
-    y: 4140777.1590688908
-    theta: 1.3116185435411241
-    kappa: 0.0095891340073473164
-    s: 16.154982139634555
+    x: 586366.2325074001
+    y: 4140777.1557445051
+    theta: 1.2694997435182696
+    kappa: -0.0039122713554129215
+    s: 16.179794908084862
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9670049343385771
-  a: -0.60456620714204112
+  v: 1.9809879053107045
+  a: -0.60027491529124233
   relative_time: 4.7499999999999911
 }
 trajectory_point {
   path_point {
-    x: 586366.15856011165
-    y: 4140777.2534303954
-    theta: 1.3125746363035105
-    kappa: 0.0098029700815750638
-    s: 16.252578716787127
+    x: 586366.26168731949
+    y: 4140777.249614805
+    theta: 1.2691175458033084
+    kappa: -0.0038945110939917686
+    s: 16.278095991368097
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9368992794823208
-  a: -0.59963088098363515
+  v: 1.951096419560965
+  a: -0.59535569022361379
   relative_time: 4.7999999999999909
 }
 trajectory_point {
   path_point {
-    x: 586366.18309719092
-    y: 4140777.3463425492
-    theta: 1.3135160439352247
-    kappa: 0.010013521727051661
-    s: 16.348676253119898
+    x: 586366.29048647266
+    y: 4140777.3420448513
+    theta: 1.268747936697024
+    kappa: -0.0037842926698040185
+    s: 16.374908721456368
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9070447568526727
-  a: -0.59452091807769791
+  v: 1.9214552187358167
+  a: -0.59026351830758683
   relative_time: 4.8499999999999908
 }
 trajectory_point {
   path_point {
-    x: 586366.20710405335
-    y: 4140777.437857308
-    theta: 1.31448153613536
-    kappa: 0.010251352502700005
-    s: 16.443287523540128
+    x: 586366.31884666835
+    y: 4140777.4330660757
+    theta: 1.2683839611870538
+    kappa: -0.0036757541984047612
+    s: 16.470245828779461
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.87745009828701
-  a: -0.58923631842422941
+  v: 1.8920729501776798
+  a: -0.58499839954316157
   relative_time: 4.8999999999999906
 }
 trajectory_point {
   path_point {
-    x: 586366.2306639679
-    y: 4140777.5279664509
-    theta: 1.3154507584293371
-    kappa: 0.010500304714461884
-    s: 16.536425739546953
+    x: 586366.34682881157
+    y: 4140777.52267326
+    theta: 1.268056065587291
+    kappa: -0.0034774480168189093
+    s: 16.564120476134292
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8481240356227089
-  a: -0.58377708202322953
+  v: 1.8629582612289739
+  a: -0.5795603339303379
   relative_time: 4.94999999999999
 }
 trajectory_point {
   path_point {
-    x: 586366.25385471631
-    y: 4140777.6166636515
-    theta: 1.3164047931555822
-    kappa: 0.010745356030894245
-    s: 16.628104549231377
+    x: 586366.37440398091
+    y: 4140777.6108896718
+    theta: 1.2677465571719202
+    kappa: -0.003242247975252069
+    s: 16.656546258684891
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8190753006971461
-  a: -0.5781432088746985
+  v: 1.8341197992321194
+  a: -0.57394932146911581
   relative_time: 4.99999999999999
 }
 trajectory_point {
   path_point {
-    x: 586366.2764414472
-    y: 4140777.7040245095
-    theta: 1.3174064724668824
-    kappa: 0.01101230062806945
-    s: 16.718338037276268
+    x: 586366.40155106771
+    y: 4140777.6977365939
+    theta: 1.2674418536490228
+    kappa: -0.003010699228561585
+    s: 16.747537203962409
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7903126253476982
-  a: -0.57233469897863609
+  v: 1.8055662115295359
+  a: -0.56816536215949531
   relative_time: 5.04999999999999
 }
 trajectory_point {
   path_point {
-    x: 586366.29866105912
-    y: 4140777.7900024503
-    theta: 1.3183946276169454
-    kappa: 0.011275981359417438
-    s: 16.807140724956369
+    x: 586366.42833444476
+    y: 4140777.7832090282
+    theta: 1.2672105791492403
+    kappa: -0.0026680323559842654
+    s: 16.837107771865121
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7618447414117415
-  a: -0.56635155233504242
+  v: 1.7773061454636434
+  a: -0.56220845600147651
   relative_time: 5.09999999999999
 }
 trajectory_point {
   path_point {
-    x: 586366.32049608219
-    y: 4140777.8746173782
-    theta: 1.3193757824313084
-    kappa: 0.011536884045838232
-    s: 16.89452757013829
+    x: 586366.45470145834
+    y: 4140777.8673390611
+    theta: 1.2669873979567978
+    kappa: -0.0023232837936395011
+    s: 16.925272854658424
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.733680380726653
-  a: -0.56019376894391748
+  v: 1.7493482483768619
+  a: -0.55607860299505929
   relative_time: 5.14999999999999
 }
 trajectory_point {
   path_point {
-    x: 586366.34178591357
-    y: 4140777.9579264624
-    theta: 1.3203975895776643
-    kappa: 0.011802787363666405
-    s: 16.980513967280505
+    x: 586366.48065514979
+    y: 4140777.9501417964
+    theta: 1.2667736061408708
+    kappa: -0.0019771590994256271
+    s: 17.012047776974832
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7058282751298088
-  a: -0.55386134880526128
+  v: 1.721701167611611
+  a: -0.54977580314024366
   relative_time: 5.1999999999999895
 }
 trajectory_point {
   path_point {
-    x: 586366.36273292045
-    y: 4140778.0398940425
-    theta: 1.3214029435186121
-    kappa: 0.012064408908288264
-    s: 17.065115747433364
+    x: 586366.50623679673
+    y: 4140778.0316207996
+    theta: 1.2666576816172319
+    kappa: -0.0015268788838537905
+    s: 17.097448295813983
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6782971564585858
-  a: -0.5473542919190737
+  v: 1.694373550510311
+  a: -0.54330005643702961
   relative_time: 5.2499999999999893
 }
 trajectory_point {
   path_point {
-    x: 586366.38329631847
-    y: 4140778.1205472504
-    theta: 1.3224029252923724
-    kappa: 0.012320879268967999
-    s: 17.14834917823908
+    x: 586366.53141159192
+    y: 4140778.1118039563
+    theta: 1.26654360076168
+    kappa: -0.0010837599472856615
+    s: 17.181490600542634
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6510957565503603
-  a: -0.54067259828535486
+  v: 1.6673740444153817
+  a: -0.53665136288541715
   relative_time: 5.2999999999999892
 }
 trajectory_point {
   path_point {
-    x: 586366.40330533462
-    y: 4140778.1999466596
-    theta: 1.3234402398856375
-    kappa: 0.012568660271048793
-    s: 17.230230963931739
+    x: 586366.5561849965
+    y: 4140778.1907069744
+    theta: 1.2664361511867968
+    kappa: -0.00064351007273214639
+    s: 17.264191312894667
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6242328072425094
-  a: -0.53381626790410475
+  v: 1.6407112966692428
+  a: -0.52982972248540638
   relative_time: 5.349999999999989
 }
 trajectory_point {
   path_point {
-    x: 586366.422988245
-    y: 4140778.2780520208
-    theta: 1.3244606492001232
-    kappa: 0.012812402953657893
-    s: 17.310778245337293
+    x: 586366.58057333017
+    y: 4140778.268342603
+    theta: 1.2664471820875944
+    kappa: -0.00010825158283384994
+    s: 17.345567486971081
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.597717040372409
-  a: -0.52678530077532337
+  v: 1.6143939546143145
+  a: -0.52283513523699721
   relative_time: 5.3999999999999888
 }
 trajectory_point {
   path_point {
-    x: 586366.442337773
-    y: 4140778.3548832787
-    theta: 1.3254672545974109
-    kappa: 0.013051220858703893
-    s: 17.390008599873564
+    x: 586366.60456994211
+    y: 4140778.34473126
+    theta: 1.2664580358117323
+    kappa: 0.00041840966610734573
+    s: 17.425636609239994
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5715571877774361
-  a: -0.51957969689901073
+  v: 1.5884306655930165
+  a: -0.51566760114018961
   relative_time: 5.4499999999999886
 }
 trajectory_point {
   path_point {
-    x: 586366.46115500259
-    y: 4140778.4305088138
-    theta: 1.3265109010639018
-    kappa: 0.013268658996037318
-    s: 17.467940041550246
+    x: 586366.62818020256
+    y: 4140778.419890041
+    theta: 1.2664687147881852
+    kappa: 0.00093659153670664057
+    s: 17.504416598536647
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.545761981294967
-  a: -0.51219945627516683
+  v: 1.562830076947769
+  a: -0.50832712019498372
   relative_time: 5.4999999999999885
 }
 trajectory_point {
   path_point {
-    x: 586366.47966305341
-    y: 4140778.5048917732
-    theta: 1.3275374010277305
-    kappa: 0.013482524488975578
-    s: 17.544591020968891
+    x: 586366.65139293682
+    y: 4140778.4938416881
+    theta: 1.2666034852948378
+    kappa: 0.0015349371689570802
+    s: 17.58192580606341
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5203401527623783
-  a: -0.50464457890379155
+  v: 1.5376008360209916
+  a: -0.5008136924013793
   relative_time: 5.5499999999999883
 }
 trajectory_point {
   path_point {
-    x: 586366.49786648573
-    y: 4140778.5780504867
-    theta: 1.3285470052512376
-    kappa: 0.01369287003507016
-    s: 17.619980425322936
+    x: 586366.67422817007
+    y: 4140778.5665996033
+    theta: 1.2667550685465749
+    kappa: 0.0021371455577993883
+    s: 17.65818301538976
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4953004340170468
-  a: -0.496915064784885
+  v: 1.5127515901551045
+  a: -0.49312731775937657
   relative_time: 5.5999999999999881
 }
 trajectory_point {
   path_point {
-    x: 586366.51556324819
-    y: 4140778.6500547281
-    theta: 1.3295774817434198
-    kappa: 0.013875610217385195
-    s: 17.694127578397673
+    x: 586366.69669424649
+    y: 4140778.638181305
+    theta: 1.2669042012856992
+    kappa: 0.002729618580930889
+    s: 17.733207442452308
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.470651556896349
-  a: -0.48901091391844725
+  v: 1.4882909866925274
+  a: -0.48526799626897549
   relative_time: 5.6499999999999879
 }
 trajectory_point {
   path_point {
-    x: 586366.53290489875
-    y: 4140778.7208874431
-    theta: 1.3306024632013529
-    kappa: 0.014047941120616341
-    s: 17.767052240570273
+    x: 586366.71876882261
+    y: 4140778.7086143894
+    theta: 1.2671336518583791
+    kappa: 0.0033612437386079373
+    s: 17.807018735554777
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.446402253237661
-  a: -0.48093212630447812
+  v: 1.4642276729756802
+  a: -0.477235727930176
   relative_time: 5.6999999999999877
 }
 trajectory_point {
   path_point {
-    x: 586366.549960641
-    y: 4140778.7905523535
-    theta: 1.3316105459062613
-    kappa: 0.0142174308396271
-    s: 17.838774608809764
+    x: 586366.74046096532
+    y: 4140778.7779170554
+    theta: 1.267434509670867
+    kappa: 0.0040269072124788866
+    s: 17.879636975368012
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4225612548783602
-  a: -0.47267870194297779
+  v: 1.4405702963469833
+  a: -0.46903051274297808
   relative_time: 5.7499999999999876
 }
 trajectory_point {
   path_point {
-    x: 586366.56666983361
-    y: 4140778.8590854327
-    theta: 1.3326123908292988
-    kappa: 0.01437210262619195
-    s: 17.909315316677052
+    x: 586366.76180285309
+    y: 4140778.8461007182
+    theta: 1.2677305096537976
+    kappa: 0.0046818224608500825
+    s: 17.951082674929985
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3991372936558226
-  a: -0.46425064083394613
+  v: 1.4173275041488562
+  a: -0.46065235070738175
   relative_time: 5.7999999999999874
 }
 trajectory_point {
   path_point {
-    x: 586366.58294281515
-    y: 4140778.9265301568
-    theta: 1.3336232786077291
-    kappa: 0.014494637694869177
-    s: 17.978695434324912
+    x: 586366.78280074243
+    y: 4140778.913185365
+    theta: 1.2680217385329144
+    kappa: 0.0053261814821938787
+    s: 18.021376779645781
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3761391014074245
-  a: -0.45564794297738315
+  v: 1.3945079437237187
+  a: -0.45210124182338718
   relative_time: 5.8499999999999872
 }
 trajectory_point {
   path_point {
-    x: 586366.59894862666
-    y: 4140778.9928675722
-    theta: 1.3346175706851493
-    kappa: 0.014615160981546839
-    s: 18.046936468497986
+    x: 586366.80337266077
+    y: 4140778.9792189877
+    theta: 1.2684776178468926
+    kappa: 0.00604583169919292
+    s: 18.090540667287613
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.353575409970543
-  a: -0.4468706083732889
+  v: 1.3721202624139908
+  a: -0.44337718609099408
   relative_time: 5.899999999999987
 }
 trajectory_point {
   path_point {
-    x: 586366.614692415
-    y: 4140779.0581190116
-    theta: 1.3355955848119565
-    kappa: 0.014733711241250679
-    s: 18.114060362532783
+    x: 586366.82361366635
+    y: 4140779.0441947542
+    theta: 1.2689285512330821
+    kappa: 0.0067551425421310124
+    s: 18.15859614799481
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3314549511825549
-  a: -0.43791863702166345
+  v: 1.3501731075620929
+  a: -0.43448018351020262
   relative_time: 5.9499999999999869
 }
 trajectory_point {
   path_point {
-    x: 586366.630030288
-    y: 4140779.1223419062
-    theta: 1.3365715871206609
-    kappa: 0.014819494507531433
-    s: 18.180089496357681
+    x: 586366.84353162569
+    y: 4140779.1081335079
+    theta: 1.26937228770544
+    kappa: 0.00745313279489593
+    s: 18.225565464273824
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3097864568808357
-  a: -0.42879202892250656
+  v: 1.3286751265104442
+  a: -0.42541023408101275
   relative_time: 5.9999999999999867
 }
 trajectory_point {
   path_point {
-    x: 586366.64501951914
-    y: 4140779.1855460834
-    theta: 1.3375410625253861
-    kappa: 0.014883282366282998
-    s: 18.245046751593392
+    x: 586366.86310307344
+    y: 4140779.17106634
+    theta: 1.2698526877908936
+    kappa: 0.0081592831745110773
+    s: 18.291471353412241
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.2885823388652287
-  a: -0.4193616708245731
+  v: 1.3076384867696709
+  a: -0.41604450489465161
   relative_time: 6.0499999999999865
 }
 trajectory_point {
   path_point {
-    x: 586366.65976682841
-    y: 4140779.2477301639
-    theta: 1.3384948914900225
-    kappa: 0.014946040708338229
-    s: 18.30895561642814
+    x: 586366.88226907456
+    y: 4140779.2330359723
+    theta: 1.2704651606546129
+    kappa: 0.0089157732960794354
+    s: 18.356337144851747
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.2678513918860908
-  a: -0.40986518746381045
+  v: 1.287071761086505
+  a: -0.40661367177672114
   relative_time: 6.0999999999999863
 }
 trajectory_point {
   path_point {
-    x: 586366.67427769466
-    y: 4140779.3089172482
-    theta: 1.3394334266249102
-    kappa: 0.01500779284750315
-    s: 18.371839832070329
+    x: 586366.9011347224
+    y: 4140779.29403447
+    theta: 1.2710680356960755
+    kappa: 0.0096604083583414647
+    s: 18.420186415675129
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.2475969222065615
-  a: -0.4003025788402208
+  v: 1.2669782046575251
+  a: -0.3971177347272179
   relative_time: 6.1499999999999861
 }
 trajectory_point {
   path_point {
-    x: 586366.68841062288
-    y: 4140779.3691651803
-    theta: 1.340362288356876
-    kappa: 0.015028899079033521
-    s: 18.433723305041511
+    x: 586366.91970703192
+    y: 4140779.3540845127
+    theta: 1.2716615366274218
+    kappa: 0.01039346522458723
+    s: 18.483042905725011
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.2278222360897826
-  a: -0.39067384495380408
+  v: 1.2473610726793096
+  a: -0.38755669374614204
   relative_time: 6.199999999999986
 }
 trajectory_point {
   path_point {
-    x: 586366.70224862685
-    y: 4140779.42847916
-    theta: 1.3412790656129989
-    kappa: 0.015030281215822259
-    s: 18.494630107176402
+    x: 586366.93795241776
+    y: 4140779.4132214473
+    theta: 1.2722933087214767
+    kappa: 0.011133177450257277
+    s: 18.544930517603849
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.2085306397988951
-  a: -0.38097898580456035
+  v: 1.2282236203484371
+  a: -0.37793054883349347
   relative_time: 6.2499999999999858
 }
 trajectory_point {
   path_point {
-    x: 586366.71587023814
-    y: 4140779.4868656136
-    theta: 1.3421815073010974
-    kappa: 0.015031641739365945
-    s: 18.554584475622882
+    x: 586366.95579935657
+    y: 4140779.4714924647
+    theta: 1.2730553161440088
+    kappa: 0.011914559945414911
+    s: 18.605873316673922
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1897254395970405
-  a: -0.37121800139248956
+  v: 1.2095691028614866
+  a: -0.36823929998927224
   relative_time: 6.2999999999999856
 }
 trajectory_point {
   path_point {
-    x: 586366.72928100114
-    y: 4140779.544348306
-    theta: 1.3430699792668037
-    kappa: 0.015032981203421112
-    s: 18.613610812841976
+    x: 586366.97337670438
+    y: 4140779.5288832574
+    theta: 1.2738058136155042
+    kappa: 0.012684139097594537
+    s: 18.665895531057338
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.17140994174736
-  a: -0.36139089171759176
+  v: 1.1914007754150364
+  a: -0.35848294721347829
   relative_time: 6.3499999999999854
 }
 trajectory_point {
   path_point {
-    x: 586366.74238321115
-    y: 4140779.6009750506
-    theta: 1.3439411521893458
-    kappa: 0.015002643964281803
-    s: 18.671733686607872
+    x: 586366.99069160433
+    y: 4140779.5854171463
+    theta: 1.2745451052522656
+    kappa: 0.01344222763624878
+    s: 18.725021551636043
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1535874525129952
-  a: -0.35149765677986688
+  v: 1.173721893205665
+  a: -0.34866149050611167
   relative_time: 6.3999999999999853
 }
 trajectory_point {
   path_point {
-    x: 586366.75517584849
-    y: 4140779.65677147
-    theta: 1.344795163595512
-    kappa: 0.014938595796070138
-    s: 18.728977830007917
+    x: 586367.00774960546
+    y: 4140779.6411181069
+    theta: 1.2752751243731466
+    kappa: 0.014189689770742527
+    s: 18.7832759320518
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1362612781570873
-  a: -0.341538296579315
+  v: 1.1565357114299515
+  a: -0.33877492986717239
   relative_time: 6.4499999999999851
 }
 trajectory_point {
   path_point {
-    x: 586366.76777767576
-    y: 4140779.7117356509
-    theta: 1.345636437766069
-    kappa: 0.014875502946081321
-    s: 18.785368141442607
+    x: 586367.02436067746
+    y: 4140779.6960697984
+    theta: 1.2761917481573215
+    kappa: 0.014992876027762232
+    s: 18.840683388706207
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1194347249427776
-  a: -0.33151281111593611
+  v: 1.1398454852844742
+  a: -0.32882326529666039
   relative_time: 6.4999999999999849
 }
 trajectory_point {
   path_point {
-    x: 586366.78019429394
-    y: 4140779.7658920228
-    theta: 1.3464653474170949
-    kappa: 0.014813337371534847
-    s: 18.840929684625607
+    x: 586367.04073388746
+    y: 4140779.7502346104
+    theta: 1.2770952481827524
+    kappa: 0.0157845610798202
+    s: 18.897268800760692
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1031110991332076
-  a: -0.32142120038973016
+  v: 1.1236544699658118
+  a: -0.31880649679457573
   relative_time: 6.5499999999999847
 }
 trajectory_point {
   path_point {
-    x: 586366.79243117885
-    y: 4140779.8192652147
-    theta: 1.3472822480038285
-    kappa: 0.014752011786199871
-    s: 18.895687688583727
+    x: 586367.05687648163
+    y: 4140779.8036365137
+    theta: 1.2779860227726423
+    kappa: 0.016565095287795907
+    s: 18.95305721013651
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0872937069915185
-  a: -0.31126346440069713
+  v: 1.107965920670543
+  a: -0.30872462436091841
   relative_time: 6.5999999999999845
 }
 trajectory_point {
   path_point {
-    x: 586366.80429822847
-    y: 4140779.8719244814
-    theta: 1.3480636181348857
-    kappa: 0.014620312609898014
-    s: 18.94966754765694
+    x: 586367.07279575313
+    y: 4140779.856299635
+    theta: 1.2788644729727117
+    kappa: 0.017334831289737403
+    s: 19.008073821514746
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0719858547808516
-  a: -0.30103960314883715
+  v: 1.0927830925952462
+  a: -0.29857764799568837
   relative_time: 6.6499999999999844
 }
 trajectory_point {
   path_point {
-    x: 586366.815999828
-    y: 4140779.9238495734
-    theta: 1.3488340956790783
-    kappa: 0.014490449578721816
-    s: 19.00289482149838
+    x: 586367.08838688978
+    y: 4140779.9082818856
+    theta: 1.2798262776972185
+    kappa: 0.018120766352554343
+    s: 19.06234400233631
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0571908487643487
-  a: -0.29074961663415011
+  v: 1.0781092409365005
+  a: -0.28836556769888566
   relative_time: 6.6999999999999842
 }
 trajectory_point {
   path_point {
-    x: 586366.82754163328
-    y: 4140779.9750655885
-    theta: 1.3495940520635654
-    kappa: 0.014362359929319769
-    s: 19.055395235074332
+    x: 586367.10365336575
+    y: 4140779.959608878
+    theta: 1.2808751736379711
+    kappa: 0.018924187313186805
+    s: 19.115893282801949
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0429119952051507
-  a: -0.280393504856636
+  v: 1.063947620890884
+  a: -0.27808838347051024
   relative_time: 6.749999999999984
 }
 trajectory_point {
   path_point {
-    x: 586366.83892933559
-    y: 4140780.0255977819
-    theta: 1.3503438611832246
-    kappa: 0.014235980495012013
-    s: 19.107194678664236
+    x: 586367.11872164346
+    y: 4140780.0102695143
+    theta: 1.2819104541939514
+    kappa: 0.019717177803259024
+    s: 19.168747355872227
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0291526003663989
-  a: -0.26997126781629488
+  v: 1.0503014876549757
+  a: -0.26774609531056215
   relative_time: 6.7999999999999838
 }
 trajectory_point {
   path_point {
-    x: 586366.85013161087
-    y: 4140780.0754798558
-    theta: 1.3510750793194932
-    kappa: 0.014094730431163823
-    s: 19.158319207860696
+    x: 586367.13359909446
+    y: 4140780.0602885764
+    theta: 1.2829326235608738
+    kappa: 0.020500125746334967
+    s: 19.220932077267552
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0159159705112351
-  a: -0.25948290551312669
+  v: 1.0371740964253542
+  a: -0.2573387032190414
   relative_time: 6.8499999999999837
 }
 trajectory_point {
   path_point {
-    x: 586366.86105861538
-    y: 4140780.1247587618
-    theta: 1.3517653286092302
-    kappa: 0.013895925389152445
-    s: 19.20879504356947
+    x: 586367.14829313615
+    y: 4140780.1096910043
+    theta: 1.2839421892884944
+    kappa: 0.021273421507928308
+    s: 19.272473465468146
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0032054119028002
-  a: -0.24892841794713153
+  v: 1.0245687023985979
+  a: -0.24686620719594798
   relative_time: 6.8999999999999835
 }
 trajectory_point {
   path_point {
-    x: 586366.87185090291
-    y: 4140780.1734301168
-    theta: 1.3524470687684036
-    kappa: 0.013699571377836738
-    s: 19.258648572009474
+    x: 586367.16263126151
+    y: 4140780.1585548855
+    theta: 1.285074736958711
+    kappa: 0.022069262785367451
+    s: 19.323397701714068
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.99102423080423574
-  a: -0.23830780511830929
+  v: 1.0124885607712857
+  a: -0.23632860724128185
   relative_time: 6.9499999999999833
 }
 trajectory_point {
   path_point {
-    x: 586366.88251422124
-    y: 4140780.2215198432
-    theta: 1.3531206622329943
-    kappa: 0.013505563820870922
-    s: 19.307906344712777
+    x: 586367.17671676411
+    y: 4140780.2068772679
+    theta: 1.2862589024133406
+    kappa: 0.022871117852949409
+    s: 19.373731130005208
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.979375733478683
-  a: -0.22762106702666005
+  v: 1.0009369267399961
+  a: -0.22572590335504303
   relative_time: 6.9999999999999831
 }
 trajectory_point {
   path_point {
-    x: 586366.89305435412
-    y: 4140780.2690540245
-    theta: 1.3537864737450149
-    kappa: 0.01331379749080379
-    s: 19.356595078524613
+    x: 586367.19064435037
+    y: 4140780.2546578953
+    theta: 1.2874297946064359
+    kappa: 0.023663983114188509
+    s: 19.423500257101278
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.96826322618928362
-  a: -0.21686820367218376
+  v: 0.9899170555013076
+  a: -0.21505809553723151
   relative_time: 7.0499999999999829
 }
 trajectory_point {
   path_point {
-    x: 586366.90345754509
-    y: 4140780.3160632108
-    theta: 1.3544379654551539
-    kappa: 0.013114469997213354
-    s: 19.40474165560336
+    x: 586367.20442148368
+    y: 4140780.3019223716
+    theta: 1.2885880377284882
+    kappa: 0.024448283437608995
+    s: 19.472731752521824
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.95769001519917862
-  a: -0.20604921505488044
+  v: 0.97943220225179917
+  a: -0.20432518378784736
   relative_time: 7.0999999999999828
 }
 trajectory_point {
   path_point {
-    x: 586366.913607143
-    y: 4140780.3626007484
-    theta: 1.3550322972712474
-    kappa: 0.01284679391995207
-    s: 19.452373123420571
+    x: 586367.218055673
+    y: 4140780.3486964577
+    theta: 1.2897342600038773
+    kappa: 0.025224446284640011
+    s: 19.521452448546221
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.94765940677150939
-  a: -0.19516410117475008
+  v: 0.96948562218804912
+  a: -0.1935271681068905
   relative_time: 7.1499999999999826
 }
 trajectory_point {
   path_point {
-    x: 586366.923652777
-    y: 4140780.4086615946
-    theta: 1.3556205418850695
-    kappa: 0.012581859690066397
-    s: 19.499516694760938
+    x: 586367.23138241319
+    y: 4140780.3950556191
+    theta: 1.2909833263250146
+    kappa: 0.02601330148188094
+    s: 19.569689340213667
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.93817470716941753
-  a: -0.18421286203179271
+  v: 0.96008057050663631
+  a: -0.18266404849436094
   relative_time: 7.1999999999999824
 }
 trajectory_point {
   path_point {
-    x: 586366.933600281
-    y: 4140780.4542724988
-    theta: 1.3562030404928935
-    kappa: 0.012319513449987505
-    s: 19.546199747722323
+    x: 586367.2444562153
+    y: 4140780.4410124198
+    theta: 1.2923047426585457
+    kappa: 0.026809719805224022
+    s: 19.617469585323196
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.929239222656044
-  a: -0.17319549762600828
+  v: 0.95122030240413924
+  a: -0.17173582495025874
   relative_time: 7.2499999999999822
 }
 trajectory_point {
   path_point {
-    x: 586366.94345552416
-    y: 4140780.4994603717
-    theta: 1.3567801363779375
-    kappa: 0.012059600413130957
-    s: 19.59244982571574
+    x: 586367.25741254352
+    y: 4140780.486556279
+    theta: 1.2936142903671817
+    kappa: 0.027598981968284739
+    s: 19.664820504433671
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.92085625949453043
-  a: -0.16211200795739683
+  v: 0.94290807307713687
+  a: -0.1607424974745838
   relative_time: 7.2999999999999821
 }
 trajectory_point {
   path_point {
-    x: 586366.95322441065
-    y: 4140780.5442522862
-    theta: 1.3573521749074904
-    kappa: 0.011801964863896711
-    s: 19.638294637465361
+    x: 586367.27025891829
+    y: 4140780.531713631
+    theta: 1.2949127249728223
+    kappa: 0.028381546072911264
+    s: 19.711769580863781
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.91302912394801816
-  a: -0.15096239302595832
+  v: 0.93514713772220726
+  a: -0.1496840660673362
   relative_time: 7.3499999999999819
 }
 trajectory_point {
   path_point {
-    x: 586366.962826299
-    y: 4140780.5886942381
-    theta: 1.3578650185268251
-    kappa: 0.011485064605837678
-    s: 19.683762057008511
+    x: 586367.28300290392
+    y: 4140780.5765110669
+    theta: 1.2962008067264672
+    kappa: 0.029157872933891053
+    s: 19.758344460692037
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.90576112227964845
-  a: -0.1397466528316928
+  v: 0.92794075153592936
+  a: -0.13856053072851593
   relative_time: 7.3999999999999817
 }
 trajectory_point {
   path_point {
-    x: 586366.972322292
-    y: 4140780.6328016757
-    theta: 1.3583537100009277
-    kappa: 0.011147827509200649
-    s: 19.728880123695678
+    x: 586367.29558191821
+    y: 4140780.6209950196
+    theta: 1.2975206513360265
+    kappa: 0.029933033174398133
+    s: 19.804572952756793
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.89905556075256277
-  a: -0.12846478737460021
+  v: 0.92129216971488193
+  a: -0.12737189145812294
   relative_time: 7.4499999999999815
 }
 trajectory_point {
   path_point {
-    x: 586366.98175069294
-    y: 4140780.6765951589
-    theta: 1.3588389232374329
-    kappa: 0.010812990849726083
-    s: 19.773677042190506
+    x: 586367.30781441461
+    y: 4140780.6652454557
+    theta: 1.2989845055193394
+    kappa: 0.030719911400217639
+    s: 19.850483028656221
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.89291574562990217
-  a: -0.11711679665468067
+  v: 0.91520464745564334
+  a: -0.11611814825615729
   relative_time: 7.4999999999999813
 }
 trajectory_point {
   path_point {
-    x: 586366.99111747276
-    y: 4140780.7201024219
-    theta: 1.35932096529155
-    kappa: 0.010480342574697429
-    s: 19.818181182469793
+    x: 586367.319969567
+    y: 4140780.7092161039
+    theta: 1.3004391135674007
+    kappa: 0.031501814325320322
+    s: 19.896102822748329
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.88734498317480837
-  a: -0.105702680671934
+  v: 0.90968143995479234
+  a: -0.10479930112261898
   relative_time: 7.5499999999999812
 }
 trajectory_point {
   path_point {
-    x: 586367.00042863761
-    y: 4140780.7633513613
-    theta: 1.3598001450180071
-    kappa: 0.010149669395757175
-    s: 19.8624210798235
+    x: 586367.33205491479
+    y: 4140780.7529342379
+    theta: 1.3018853714696335
+    kappa: 0.03227922694968581
+    s: 19.94146063215095
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.88234657965042262
-  a: -0.094222439426360383
+  v: 0.90472580240890776
+  a: -0.093415350057507951
   relative_time: 7.599999999999981
 }
 trajectory_point {
   path_point {
-    x: 586367.00967912772
-    y: 4140780.8063724074
-    theta: 1.3602681958926408
-    kappa: 0.0098139981177585139
-    s: 19.906425434854732
+    x: 586367.34407804115
+    y: 4140780.796427289
+    theta: 1.30332418062478
+    kappa: 0.03305263706292462
+    s: 19.986584916741744
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.87792384131988621
-  a: -0.082676072917959642
+  v: 0.9003409900145678
+  a: -0.081966295060824257
   relative_time: 7.6499999999999808
 }
 trajectory_point {
   path_point {
-    x: 586367.018776899
-    y: 4140780.8492147606
-    theta: 1.3606496129424059
-    kappa: 0.009413369708366242
-    s: 19.950223113479769
+    x: 586367.35604657244
+    y: 4140780.8397228448
+    theta: 1.3047564478003466
+    kappa: 0.033822535244278196
+    s: 20.031504299158208
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.87408007444634062
-  a: -0.071063581146731947
+  v: 0.89653025796835151
+  a: -0.0704521361325679
   relative_time: 7.6999999999999806
 }
 trajectory_point {
   path_point {
-    x: 586367.02783776913
-    y: 4140780.8918833425
-    theta: 1.3610294831456804
-    kappa: 0.00901436626419678
-    s: 19.993843146928036
+    x: 586367.367725292
+    y: 4140780.882914654
+    theta: 1.3063079064932552
+    kappa: 0.03455187373510326
+    s: 20.076247564797658
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.87081858529292688
-  a: -0.059384964112677185
+  v: 0.89329686146683729
+  a: -0.058872873272738824
   relative_time: 7.7499999999999805
 }
 trajectory_point {
   path_point {
-    x: 586367.03686780331
-    y: 4140780.934406715
-    theta: 1.3614080606526988
-    kappa: 0.0086167207171135834
-    s: 20.037314731742118
+    x: 586367.37923204876
+    y: 4140780.9260006892
+    theta: 1.3079229056423614
+    kappa: 0.035258171580444142
+    s: 20.120843661817247
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.86814268012278661
-  a: -0.047640221815795414
+  v: 0.89064405570660388
+  a: -0.047228506481337085
   relative_time: 7.79999999999998
 }
 trajectory_point {
   path_point {
-    x: 586367.0458731004
-    y: 4140780.9768135976
-    theta: 1.361785601055812
-    kappa: 0.008220164486818976
-    s: 20.080667229777756
+    x: 586367.390708344
+    y: 4140780.9689726643
+    theta: 1.3095336394032682
+    kappa: 0.035962599667964307
+    s: 20.165321701133948
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.86605566519906119
-  a: -0.035829354256086521
+  v: 0.88857509588422978
+  a: -0.035519035758362627
   relative_time: 7.84999999999998
 }
 trajectory_point {
   path_point {
-    x: 586367.054859794
-    y: 4140781.0191328744
-    theta: 1.3621623613887379
-    kappa: 0.00782442748085416
-    s: 20.123930168203849
+    x: 586367.40216173127
+    y: 4140781.011858861
+    theta: 1.311141159492863
+    kappa: 0.036665621624219491
+    s: 20.209710956424576
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.86456084678489176
-  a: -0.023952361433550728
+  v: 0.88709323719629385
+  a: -0.023744461103815506
   relative_time: 7.89999999999998
 }
 trajectory_point {
   path_point {
-    x: 586367.06381322909
-    y: 4140781.0613979935
-    theta: 1.3625125445802777
-    kappa: 0.0074711895604020681
-    s: 20.167133239502455
+    x: 586367.41359980544
+    y: 4140781.0546877203
+    theta: 1.312746523626273
+    kappa: 0.037367703653500096
+    s: 20.254040864125763
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.86366153114341981
-  a: -0.012009243348187759
+  v: 0.88620173483937448
+  a: -0.011904782517695778
   relative_time: 7.94999999999998
 }
 trajectory_point {
   path_point {
-    x: 586367.072712132
-    y: 4140781.1036439734
-    theta: 1.3628020299526185
-    kappa: 0.0072155335555205494
-    s: 20.210306301468787
+    x: 586367.42503020389
+    y: 4140781.097487838
+    theta: 1.3143507954616125
+    kappa: 0.0380693145378316
+    s: 20.298341023433974
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.86336102453778674
-  a: 2.1649348980190553e-15
+  v: 0.8859038440100504
+  a: -3.219646771412954e-15
   relative_time: 7.99999999999998
 }
 decision {
@@ -2263,10 +2263,10 @@ decision {
       reason_code: STOP_REASON_DESTINATION
       reason: "stop by DEST"
       stop_point {
-        x: 586367.858633872
-        y: 4140785.4811067292
+        x: 586367.98188070732
+        y: 4140785.4701649384
       }
-      stop_heading: 1.4474826038586581
+      stop_heading: 1.604801449298078
     }
   }
   object_decision {
@@ -2286,10 +2286,10 @@ decision {
           reason_code: STOP_REASON_DESTINATION
           distance_s: 0
           stop_point {
-            x: 586367.858633872
-            y: 4140785.4811067292
+            x: 586367.98188070732
+            y: 4140785.4701649384
           }
-          stop_heading: 1.4474826038586581
+          stop_heading: 1.604801449298078
         }
       }
     }
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
old mode 100755
new mode 100644
index e35139a294..ee792774d7
--- a/modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt
@@ -9,2252 +9,2252 @@ trajectory_point {
     y: 4140690.05199
     z: 0
     theta: 1.6602776953500005
-    kappa: 1.6464083883232229e-18
+    kappa: 2.9137807886363263e-17
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2611112594604483
-  a: 1.2925492504799998
+  v: 4.2611112594604492
+  a: 1.29254925048
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 586343.8867738226
-    y: 4140690.2657320681
-    theta: 1.66005584599002
-    kappa: -0.00074164311467416182
-    s: 0.21459546728395695
+    x: 586343.88722572755
+    y: 4140690.2657199576
+    theta: 1.6580632344479016
+    kappa: -0.0064567967112670694
+    s: 0.21454361297806723
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3212032844961525
-  a: 1.1120515622780682
+  v: 4.3181066913863626
+  a: 0.98936105737476665
   relative_time: 0.05
 }
 trajectory_point {
   path_point {
-    x: 586343.86740684975
-    y: 4140690.4822443542
-    theta: 1.6598311213228487
-    kappa: -0.0014928983437214618
-    s: 0.43197221323777157
+    x: 586343.86834301869
+    y: 4140690.481916544
+    theta: 1.655823204264701
+    kappa: -0.012988110443455906
+    s: 0.43156324488809417
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3724083968212453
-  a: 0.93707274205556068
+  v: 4.3602566682797486
+  a: 0.69873104917887885
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 586343.84783105552
-    y: 4140690.7010911512
-    theta: 1.6596039736049824
-    kappa: -0.0022522538677303065
-    s: 0.65169279081088771
+    x: 586343.84930809191
+    y: 4140690.69985594
+    theta: 1.6535651267126468
+    kappa: -0.019572074560045987
+    s: 0.65033232070959113
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.415002539834699
-  a: 0.76761278981247738
+  v: 4.3881890993860733
+  a: 0.42065922589233684
   relative_time: 0.15000000000000002
 }
 trajectory_point {
   path_point {
-    x: 586343.8283793641
-    y: 4140690.9218766424
-    theta: 1.6588009704857221
-    kappa: -0.0034480848650147314
-    s: 0.87333355012269709
+    x: 586343.83279311529
+    y: 4140690.9190562111
+    theta: 1.6476390035680117
+    kappa: -0.025091585763743502
+    s: 0.87015566088434149
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4492616569354837
-  a: 0.60367170554881833
+  v: 4.4025318939508047
+  a: 0.15514558751514063
   relative_time: 0.2
 }
 trajectory_point {
   path_point {
-    x: 586343.80887862749
-    y: 4140691.1441740319
-    theta: 1.6578301497422645
-    kappa: -0.0047736636785871
-    s: 1.0964846384625409
+    x: 586343.81655266171
+    y: 4140691.13867036
+    theta: 1.6412765618122567
+    kappa: -0.030493364867281818
+    s: 1.0903694813164027
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4754616915225709
-  a: 0.44524948926458352
+  v: 4.40391296121941
+  a: -0.0978098659527098
   relative_time: 0.25
 }
 trajectory_point {
   path_point {
-    x: 586343.78928051668
-    y: 4140691.3675814327
-    theta: 1.6568544817802453
-    kappa: -0.0061058615830109214
-    s: 1.3207500002897086
+    x: 586343.80033004854
+    y: 4140691.3580432595
+    theta: 1.6349212965771558
+    kappa: -0.035889210030753058
+    s: 1.3103413933721049
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4938785869949323
-  a: 0.29234614095977296
+  v: 4.392960210437356
+  a: -0.33820713451121454
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 586343.7703170802
-    y: 4140691.5917777773
-    theta: 1.6551466462348083
-    kappa: -0.0076647519636638085
-    s: 1.545747377233438
+    x: 586343.78865381284
+    y: 4140691.576851707
+    theta: 1.6259443423367592
+    kappa: -0.040137313370218378
+    s: 1.5294704038800517
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5047882867515385
-  a: 0.14496166063438642
+  v: 4.3703015508501108
+  a: -0.56604621816037359
   relative_time: 0.35
 }
 trajectory_point {
   path_point {
-    x: 586343.75161755446
-    y: 4140691.8163615623
-    theta: 1.6531286975834358
-    kappa: -0.0093199027092314
-    s: 1.771108308092916
+    x: 586343.77794723108
+    y: 4140691.7943048016
+    theta: 1.6164963988343914
+    kappa: -0.0441332371781138
+    s: 1.7471869151311206
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5084667341913613
-  a: 0.0030960482884243579
+  v: 4.3365648917031407
+  a: -0.781327116900187
   relative_time: 0.39999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586343.732917291
-    y: 4140692.040954208
-    theta: 1.6511106725492526
-    kappa: -0.010975118746061045
-    s: 1.9964781288372775
+    x: 586343.76733657834
+    y: 4140692.009809555
+    theta: 1.607133571428361
+    kappa: -0.048093358214156723
+    s: 1.9629527248784615
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5051898727133715
-  a: -0.13325069607811368
+  v: 4.2923781422419145
+  a: -0.98404983073065422
   relative_time: 0.44999999999999996
 }
 trajectory_point {
   path_point {
-    x: 586343.71543634124
-    y: 4140692.2653106241
-    theta: 1.6482948989914148
-    kappa: -0.012690017702122185
-    s: 2.2215159726056055
+    x: 586343.76193576271
+    y: 4140692.2230247464
+    theta: 1.5963156883692777
+    kappa: -0.051086252062153434
+    s: 2.1762610263374982
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4952336457165405
-  a: -0.2640785724652277
+  v: 4.2383692117118983
+  a: -1.1742143596517765
   relative_time: 0.49999999999999994
 }
 trajectory_point {
   path_point {
-    x: 586343.69854357338
-    y: 4140692.4890526165
-    theta: 1.6451265396424835
-    kappa: -0.014427884947446096
-    s: 2.4458947697069338
+    x: 586343.7586506604
+    y: 4140692.4333744776
+    theta: 1.585017630850136
+    kappa: -0.053668083747384125
+    s: 2.3866364081859288
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4788739965998392
-  a: -0.38938758087291747
+  v: 4.1751660093585592
+  a: -1.3518207036635528
   relative_time: 0.54999999999999993
 }
 trajectory_point {
   path_point {
-    x: 586343.68172400841
-    y: 4140692.7118250486
-    theta: 1.6419719217155888
-    kappa: -0.016158221348988464
-    s: 2.6693012476202416
+    x: 586343.75541829038
+    y: 4140692.6403476852
+    theta: 1.5739011953617001
+    kappa: -0.056208471995077294
+    s: 2.5936348545637236
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4563868687622392
-  a: -0.509177721301183
+  v: 4.1033964444273652
+  a: -1.5168688627659836
   relative_time: 0.6
 }
 trajectory_point {
   path_point {
-    x: 586343.66666905757
-    y: 4140692.9334459389
-    theta: 1.638062458492046
-    kappa: -0.01782868073465434
-    s: 2.8914359309944588
+    x: 586343.75574389077
+    y: 4140692.8435185961
+    theta: 1.562396678890978
+    kappa: -0.05821638130650026
+    s: 2.7968437450731263
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4280482056027113
-  a: -0.62344899375002427
+  v: 4.0236884261637833
+  a: -1.6693588369590677
   relative_time: 0.65
 }
 trajectory_point {
   path_point {
-    x: 586343.65251730685
-    y: 4140693.1535687083
-    theta: 1.6338109253418618
-    kappa: -0.01946351733415802
-    s: 3.1120131416484642
+    x: 586343.76035376452
+    y: 4140693.0425033146
+    theta: 1.5503975972760602
+    kappa: -0.059587078724519488
+    s: 2.9958818547786534
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.394133950520227
-  a: -0.73220139821944152
+  v: 3.9366698638132807
+  a: -1.8092906262428068
   relative_time: 0.70000000000000007
 }
 trajectory_point {
   path_point {
-    x: 586343.63848292362
-    y: 4140693.3718658937
-    theta: 1.6295946750682728
-    kappa: -0.021084795441265125
-    s: 3.3307609985710824
+    x: 586343.76485893782
+    y: 4140693.2369686356
+    theta: 1.5386701187044682
+    kappa: -0.060926644472107189
+    s: 3.1903993542070972
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3549200469137572
-  a: -0.83543493470943431
+  v: 3.8429686666213247
+  a: -1.9366642306172004
   relative_time: 0.75000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586343.62658164953
-    y: 4140693.588193309
-    theta: 1.6247965415982635
-    kappa: -0.022588754559120953
-    s: 3.5474214179210897
+    x: 586343.769252035
+    y: 4140693.42659621
+    theta: 1.5272366116393659
+    kappa: -0.062232885619089073
+    s: 3.38007780934752
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3106824381822744
-  a: -0.93314960322000329
+  v: 3.7432127438333826
+  a: -2.0514796500822481
   relative_time: 0.80000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586343.61606441461
-    y: 4140693.8022638052
-    theta: 1.6196589679291862
-    kappa: -0.024012543668399684
-    s: 3.7617501130272095
+    x: 586343.78093264636
+    y: 4140693.6107680378
+    theta: 1.5155886051342364
+    kappa: -0.062713545696204714
+    s: 3.56463018165126
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2616970677247474
-  a: -1.025345403751148
+  v: 3.6380300046949219
+  a: -2.15373688463795
   relative_time: 0.8500000000000002
 }
 trajectory_point {
   path_point {
-    x: 586343.60567290906
-    y: 4140694.0137751745
-    theta: 1.6145828232787585
-    kappa: -0.025419311948879562
-    s: 3.9735165943881134
+    x: 586343.79278013716
+    y: 4140693.789546553
+    theta: 1.5042415317346605
+    kappa: -0.063126036950962028
+    s: 3.7438008280319286
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.20823987894015
-  a: -1.1120223363028681
+  v: 3.52804835845141
+  a: -2.2434359342843062
   relative_time: 0.90000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586343.59706703085
-    y: 4140694.2225774946
-    theta: 1.6091830358376564
-    kappa: -0.026714761022330388
-    s: 4.1825041696724234
+    x: 586343.80425693817
+    y: 4140693.9627313637
+    theta: 1.4932495771452343
+    kappa: -0.063525621989721739
+    s: 3.9173655008654089
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1505868152274514
-  a: -1.1931804008751643
+  v: 3.4138957143483135
+  a: -2.3205767990213166
   relative_time: 0.95000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586343.590574871
-    y: 4140694.4284809446
-    theta: 1.6033886923868772
-    kappa: -0.0278796310540797
-    s: 4.3885099437187067
+    x: 586343.815550474
+    y: 4140694.1301123938
+    theta: 1.4826262927271086
+    kappa: -0.063893456416518113
+    s: 4.0851313479898579
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0890138199856239
-  a: -1.2688195974680359
+  v: 3.2961999816311005
+  a: -2.3851594788489816
   relative_time: 1.0000000000000002
 }
 trajectory_point {
   path_point {
-    x: 586343.5841826403
-    y: 4140694.6312150708
-    theta: 1.5976834660110264
-    kappa: -0.029026571076326849
-    s: 4.5913448185354824
+    x: 586343.83326229593
+    y: 4140694.290945637
+    theta: 1.472340298658422
+    kappa: -0.063621367043263613
+    s: 4.2469369127057082
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0237968366136379
-  a: -1.338939926081484
+  v: 3.175589069545238
+  a: -2.4371839737673007
   relative_time: 1.0500000000000003
 }
 trajectory_point {
   path_point {
-    x: 586343.578446541
-    y: 4140694.8306164886
-    theta: 1.5919772192528234
-    kappa: -0.030125583233416228
-    s: 4.7908334933012178
+    x: 586343.85030744679
+    y: 4140694.4457251346
+    theta: 1.4624391731857942
+    kappa: -0.063359519084069421
+    s: 4.4026521337756623
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9552118085104655
-  a: -1.4035413867155071
+  v: 3.0526908873361926
+  a: -2.4766502837762734
   relative_time: 1.1000000000000003
 }
 trajectory_point {
   path_point {
-    x: 586343.57620476582
-    y: 4140695.0265846374
-    theta: 1.5857830778048418
-    kappa: -0.031026522366251107
-    s: 4.9868144643643264
+    x: 586343.86667512637
+    y: 4140694.5943528139
+    theta: 1.4529312823663632
+    kappa: -0.063108078453556177
+    s: 4.5521783454246982
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.8835346790750775
-  a: -1.4626239793701072
+  v: 2.9281333442494324
+  a: -2.5035584088759011
   relative_time: 1.1500000000000004
 }
 trajectory_point {
   path_point {
-    x: 586343.57400480378
-    y: 4140695.218897616
-    theta: 1.579704194884916
-    kappa: -0.031910657305986509
-    s: 5.1791400252431714
+    x: 586343.882357971
+    y: 4140694.7367618084
+    theta: 1.4438225516468222
+    kappa: -0.062867158272290932
+    s: 4.695448277340069
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.8090413917064447
-  a: -1.5161877040452818
+  v: 2.8025443495304252
+  a: -2.5179083490661829
   relative_time: 1.2000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586343.5718481869
-    y: 4140695.4074215223
-    theta: 1.5737452546916304
-    kappa: -0.032777372461534726
-    s: 5.3676762666260665
+    x: 586343.90002522327
+    y: 4140694.8725630045
+    theta: 1.4352672754873848
+    kappa: -0.062424874471699233
+    s: 4.8324260546712949
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.7320078898035396
-  a: -1.564232560741033
+  v: 2.6765518124246359
+  a: -2.5197001043471188
   relative_time: 1.2500000000000004
 }
 trajectory_point {
   path_point {
-    x: 586343.5722315989
-    y: 4140695.5920370803
-    theta: 1.5675983398771363
-    kappa: -0.033508623102198952
-    s: 5.55230307637127
+    x: 586343.91997224686
+    y: 4140695.00171283
+    theta: 1.4272790219979361
+    kappa: -0.061757777557301587
+    s: 4.9631071980301771
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.652710116765332
-  a: -1.6067585494573591
+  v: 2.5507836421775352
+  a: -2.5089336747187092
   relative_time: 1.3000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586343.57417774654
-    y: 4140695.7726376583
-    theta: 1.5613883201596672
-    kappa: -0.034149993948901086
-    s: 5.7329141395069945
+    x: 586343.93896226771
+    y: 4140695.1246664063
+    theta: 1.4196728986592659
+    kappa: -0.061122686097987815
+    s: 5.0875186234907845
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.5714240159907935
-  a: -1.6437656701942622
+  v: 2.4258677480345883
+  a: -2.4856090601809537
   relative_time: 1.3500000000000005
 }
 trajectory_point {
   path_point {
-    x: 586343.57607962622
-    y: 4140695.94913021
-    theta: 1.5553194343075543
-    kappa: -0.034776775871878281
-    s: 5.9094169382313941
+    x: 586343.95700418658
+    y: 4140695.241481361
+    theta: 1.4124463207505342
+    kappa: -0.060519302426895127
+    s: 5.2057186425894626
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.4884255308788963
-  a: -1.6752539229517407
+  v: 2.3024320392412614
+  a: -2.4497262607338524
   relative_time: 1.4000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586343.5779363896
-    y: 4140696.12143602
-    theta: 1.5493948287525521
-    kappa: -0.035388689325030462
-    s: 6.0817327519125781
+    x: 586343.974111696
+    y: 4140695.35224635
+    theta: 1.4055945315836191
+    kappa: -0.059947168610610641
+    s: 5.31779696232483
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.40399060482861
-  a: -1.7012233077297949
+  v: 2.1811044250430252
+  a: -2.4012852763774051
   relative_time: 1.4500000000000006
 }
 trajectory_point {
   path_point {
-    x: 586343.58288583928
-    y: 4140696.2894188203
-    theta: 1.5433422507864107
-    kappa: -0.035854248670573165
-    s: 6.2497966570885986
+    x: 586343.990303281
+    y: 4140695.4570810576
+    theta: 1.3991106386719339
+    kappa: -0.059405666449171404
+    s: 5.4238746851577746
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.3183951812389068
-  a: -1.7216738245284249
+  v: 2.0625128146853435
+  a: -2.3402861071116123
   relative_time: 1.5000000000000007
 }
 trajectory_point {
   path_point {
-    x: 586343.58856434154
-    y: 4140696.4530812083
-    theta: 1.5373693197723441
-    kappa: -0.036272098679013834
-    s: 6.4135575274674608
+    x: 586344.008103882
+    y: 4140695.5556976418
+    theta: 1.3932681214394853
+    kappa: -0.058710966873159333
+    s: 5.5241043090114648
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.2319152035087582
-  a: -1.7366054733476302
+  v: 1.9472851174136858
+  a: -2.2667287529364741
   relative_time: 1.5500000000000007
 }
 trajectory_point {
   path_point {
-    x: 586343.59409233928
-    y: 4140696.6124058431
-    theta: 1.5315545805179129
-    kappa: -0.036678873873940872
-    s: 6.5729780339271171
+    x: 586344.02632021648
+    y: 4140695.6484919488
+    theta: 1.3879158595799106
+    kappa: -0.057951499341501511
+    s: 5.6186697272713362
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.1448266150371342
-  a: -1.7460182541874116
+  v: 1.8360492424735186
+  a: -2.1806132138519891
   relative_time: 1.6000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586343.59946901631
-    y: 4140696.7673692056
-    theta: 1.525899277916251
-    kappa: -0.037074514211545513
-    s: 6.7280346445154668
+    x: 586344.043486915
+    y: 4140695.7359393919
+    theta: 1.3828714326873326
+    kappa: -0.057235792789571013
+    s: 5.7077862287850989
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.057405359223007
-  a: -1.7499121670477693
+  v: 1.7294330991103091
+  a: -2.0819394898581591
   relative_time: 1.6500000000000008
 }
 trajectory_point {
   path_point {
-    x: 586343.60675222985
-    y: 4140696.9178635362
-    theta: 1.5202946060344193
-    kappa: -0.03738426026877785
-    s: 6.87871762445036
+    x: 586344.05965149647
+    y: 4140695.8182820352
+    theta: 1.3781212105555793
+    kappa: -0.05656186606265555
+    s: 5.7917004978627409
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.9699273794653482
-  a: -1.7482872119287023
+  v: 1.6280645965695251
+  a: -1.9707075809549837
   relative_time: 1.7000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586343.61559965264
-    y: 4140697.0639092051
-    theta: 1.5147577620709745
-    kappa: -0.03762055768628815
-    s: 7.025031036119592
+    x: 586344.074867528
+    y: 4140695.89579275
+    theta: 1.3736496946169443
+    kappa: -0.055927485864883081
+    s: 5.8706906142765165
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.8826686191631268
-  a: -1.741143388830211
+  v: 1.5325716440966324
+  a: -1.8469174871424623
   relative_time: 1.7500000000000009
 }
 trajectory_point {
   path_point {
-    x: 586343.624183932
-    y: 4140697.2056111293
-    theta: 1.5093853386890435
-    kappa: -0.037849827056829231
-    s: 7.1669927390809116
+    x: 586344.08919462468
+    y: 4140695.9687752146
+    theta: 1.369439536693744
+    kappa: -0.055330166759221672
+    s: 5.94506605326096
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.7959050217153161
-  a: -1.7284806977522955
+  v: 1.4435821509371007
+  a: -1.7105692084205955
   relative_time: 1.8000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586343.63250698219
-    y: 4140697.3430009065
-    theta: 1.504176457303203
-    kappa: -0.038072119506288443
-    s: 7.3046343900620139
+    x: 586344.10269844893
+    y: 4140696.0375639172
+    theta: 1.3654715557473565
+    kappa: -0.054767171167479686
+    s: 6.015167685512874
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.7099125305208878
-  a: -1.7102991386949562
+  v: 1.3617240263363959
+  a: -1.5616627447893823
   relative_time: 1.850000000000001
 }
 trajectory_point {
   path_point {
-    x: 586343.64057155186
-    y: 4140697.4761239085
-    theta: 1.4991296624987409
-    kappa: -0.038287508443101186
-    s: 7.43800144296054
+    x: 586344.11545071134
+    y: 4140696.1025241511
+    theta: 1.3617247526128147
+    kappa: -0.054235509370305572
+    s: 6.0813677771913381
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.6249670889788108
-  a: -1.6865987116581922
+  v: 1.287625179539984
+  a: -1.4001980962488236
   relative_time: 1.900000000000001
 }
 trajectory_point {
   path_point {
-    x: 586343.65062319278
-    y: 4140697.6048740149
-    theta: 1.4941741536733892
-    kappa: -0.038419414482656747
-    s: 7.567153148844084
+    x: 586344.12752917
+    y: 4140696.164052017
+    theta: 1.3581763227960242
+    kappa: -0.053731939507188055
+    s: 6.1440699899177007
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.5413446404880591
-  a: -1.6573794166420044
+  v: 1.2219135197933353
+  a: -1.2261752627989195
   relative_time: 1.9500000000000011
 }
 trajectory_point {
   path_point {
-    x: 586343.66144214408
-    y: 4140697.7294143783
-    theta: 1.489343890800443
-    kappa: -0.0385098223335156
-    s: 7.6921625559501843
+    x: 586344.14084635791
+    y: 4140696.2221801206
+    theta: 1.3551250342283947
+    kappa: -0.053133169598577064
+    s: 6.2037093807755923
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.4593211284476029
-  a: -1.6226412536463914
+  v: 1.1652169563419155
+  a: -1.0395942444396735
   relative_time: 2.0000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586343.67190926813
-    y: 4140697.8499047444
-    theta: 1.48467047888124
-    kappa: -0.038597290161251879
-    s: 7.8131067127053448
+    x: 586344.15388454753
+    y: 4140696.2776429886
+    theta: 1.3522657577224739
+    kappa: -0.052540451305054026
+    s: 6.2606841480684716
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.3785746818938867
-  a: -1.6070964374403034
+  v: 1.1140408670547473
+  a: -1.007538442659994
   relative_time: 2.0500000000000007
 }
 trajectory_point {
   path_point {
-    x: 586343.68202868931
-    y: 4140697.9663925944
-    theta: 1.4801522927882156
-    kappa: -0.038681852433594538
-    s: 7.9300332785807326
+    x: 586344.16634629527
+    y: 4140696.3306537429
+    theta: 1.3495327113689675
+    kappa: -0.051973938159221605
+    s: 6.3151399578601843
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.2986235018097689
-  a: -1.590830594862451
+  v: 1.0644542005146231
+  a: -0.976017334557914
   relative_time: 2.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586343.691803927
-    y: 4140698.0789184393
-    theta: 1.47578793820395
-    kappa: -0.038763538559527751
-    s: 8.04298291818279
+    x: 586344.17824963434
+    y: 4140696.3812890947
+    theta: 1.3469220028479365
+    kappa: -0.051432810362320974
+    s: 6.3671556129209863
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.2195036395138383
-  a: -1.5738437259128346
+  v: 1.0164302220376629
+  a: -0.945030920133433
   relative_time: 2.1500000000000004
 }
 trajectory_point {
   path_point {
-    x: 586343.70123865653
-    y: 4140698.1875245874
-    theta: 1.4715759155416275
-    kappa: -0.038842379251666462
-    s: 8.1519980986838956
+    x: 586344.18961229234
+    y: 4140696.4296244555
+    theta: 1.344429794814332
+    kappa: -0.05091626202186883
+    s: 6.416808579286938
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.1412511463246822
-  a: -1.5561358305914537
+  v: 0.96994219693998673
+  a: -0.91457919938655152
   relative_time: 2.2
 }
 trajectory_point {
   path_point {
-    x: 586343.71274979808
-    y: 4140698.2920134612
-    theta: 1.467493765408199
-    kappa: -0.038840024505084961
-    s: 8.2571230898223487
+    x: 586344.2004516907
+    y: 4140696.4757339335
+    theta: 1.3420523066505532
+    kappa: -0.050423501151657349
+    s: 6.4641749862599074
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.0639020735608895
-  a: -1.5377069088983084
+  v: 0.9249633905377147
+  a: -0.8846621723172694
   relative_time: 2.25
 }
 trajectory_point {
   path_point {
-    x: 586343.72416913952
-    y: 4140698.3926485153
-    theta: 1.463557786375419
-    kappa: -0.03882706508436596
-    s: 8.3584039639023828
+    x: 586344.210784945
+    y: 4140696.5196903367
+    theta: 1.3397858160147322
+    kappa: -0.049953749671754186
+    s: 6.5093296264075677
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9874924725410481
-  a: -1.5185569608333989
+  v: 0.88146706814696674
+  a: -0.85527983892558668
   relative_time: 2.3
 }
 trajectory_point {
   path_point {
-    x: 586343.73516045755
-    y: 4140698.4895115346
-    theta: 1.459769165768197
-    kappa: -0.038814591412808547
-    s: 8.45588859579416
+    x: 586344.22062886506
+    y: 4140696.5615651719
+    theta: 1.3376266602041855
+    kappa: -0.049506243408502529
+    s: 6.5523459555633981
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9120583945837462
-  a: -1.498685986396725
+  v: 0.839426495083863
+  a: -0.82643219921150324
   relative_time: 2.3499999999999996
 }
 trajectory_point {
   path_point {
-    x: 586343.74572935328
-    y: 4140698.5826518778
-    theta: 1.4561260950097044
-    kappa: -0.038802597133923394
-    s: 8.5496266629337754
+    x: 586344.22999995481
+    y: 4140696.6014286438
+    theta: 1.335571237352023
+    kappa: -0.049080232094521045
+    s: 6.5932960928266837
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8376358910075723
-  a: -1.4780939855882869
+  v: 0.79881493666452341
+  a: -0.79811925317501919
   relative_time: 2.3999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586343.75588163093
-    y: 4140698.6721206987
-    theta: 1.4526266709509985
-    kappa: -0.038791075660573389
-    s: 8.6396696453232469
+    x: 586344.2389144121
+    y: 4140696.6393496576
+    theta: 1.3336160074735306
+    kappa: -0.048674979368703908
+    s: 6.6322508205625148
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7642610131311141
-  a: -1.4567809584080846
+  v: 0.75960565820506809
+  a: -0.77034100081613444
   relative_time: 2.4499999999999993
 }
 trajectory_point {
   path_point {
-    x: 586343.76562329824
-    y: 4140698.75797094
-    theta: 1.4492688986728508
-    kappa: -0.038780020174973617
-    s: 8.7260708255305239
+    x: 586344.247388129
+    y: 4140696.6753958147
+    theta: 1.3317574933776266
+    kappa: -0.048289762776220761
+    s: 6.6692795844017905
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6919698122729603
-  a: -1.4347469048561179
+  v: 0.721771925021617
+  a: -0.74309744213484907
   relative_time: 2.4999999999999991
 }
 trajectory_point {
   path_point {
-    x: 586343.77496056585
-    y: 4140698.8402573345
-    theta: 1.4460506941080236
-    kappa: -0.038769423628691381
-    s: 8.80888528868949
+    x: 586344.25543669157
+    y: 4140696.7096334179
+    theta: 1.3299922814574558
+    kappa: -0.047923873768516791
+    s: 6.7044504932412119
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.620798339751699
-  a: -1.4119918249323868
+  v: 0.68528700243029017
+  a: -0.71638857713116311
   relative_time: 2.5499999999999989
 }
 trajectory_point {
   path_point {
-    x: 586343.7848902914
-    y: 4140698.9189107763
-    theta: 1.4429830895306488
-    kappa: -0.038728369963777882
-    s: 8.8881699224999515
+    x: 586344.26307538012
+    y: 4140696.7421274674
+    theta: 1.3283170223730116
+    kappa: -0.047576617703312646
+    s: 6.7378303192432893
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.550782646885918
-  a: -1.3885157186368915
+  v: 0.65012415574720761
+  a: -0.69021440580507631
   relative_time: 2.5999999999999988
 }
 trajectory_point {
   path_point {
-    x: 586343.795420732
-    y: 4140698.9939893736
-    theta: 1.4400635086714271
-    kappa: -0.03865680049446199
-    s: 8.9639834172276487
+    x: 586344.27031916892
+    y: 4140696.7729416611
+    theta: 1.3267284316375711
+    kappa: -0.047247313844604462
+    s: 6.7694844978363387
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4819587849942062
-  a: -1.3643185859696318
+  v: 0.61625665028848942
+  a: -0.66457492815658914
   relative_time: 2.6499999999999986
 }
 trajectory_point {
   path_point {
-    x: 586343.80547743628
-    y: 4140699.0656903861
-    theta: 1.4372751305422988
-    kappa: -0.038588450744050921
-    s: 9.03638626570425
+    x: 586344.27718272654
+    y: 4140696.8021383979
+    theta: 1.325223290118708
+    kappa: -0.046935295362663922
+    s: 6.7994771277144794
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4143628053951516
-  a: -1.3394004269306077
+  v: 0.58365775137025566
+  a: -0.63947014418570114
   relative_time: 2.6999999999999984
 }
 trajectory_point {
   path_point {
-    x: 586343.815069057
-    y: 4140699.1340755057
-    theta: 1.434615615775602
-    kappa: -0.038523261904431914
-    s: 9.1054407633273513
+    x: 586344.28368041536
+    y: 4140696.8297787732
+    theta: 1.3237984444636655
+    kappa: -0.046639909334038158
+    s: 6.82787097083764
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.348030759407342
-  a: -1.3137612415198194
+  v: 0.5523007243086262
+  a: -0.61490005389241253
   relative_time: 2.7499999999999982
 }
 trajectory_point {
   path_point {
-    x: 586343.8242044975
-    y: 4140699.1992082065
-    theta: 1.4320825431627477
-    kappa: -0.038461173465833567
-    s: 9.17121100806048
+    x: 586344.29012261343
+    y: 4140696.8558478737
+    theta: 1.3225255520345558
+    kappa: -0.046343685113450987
+    s: 6.854727452431554
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.2829986983493662
-  a: -1.2874010297372669
+  v: 0.52215883441972122
+  a: -0.59086465727672333
   relative_time: 2.799999999999998
 }
 trajectory_point {
   path_point {
-    x: 586343.83289291093
-    y: 4140699.2611537529
-    theta: 1.4296734113211389
-    kappa: -0.0384021232168259
-    s: 9.2337629004330939
+    x: 586344.29672223877
+    y: 4140696.8803539788
+    theta: 1.3214517677600766
+    kappa: -0.046034684500359004
+    s: 6.88010666098776
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.2193026735398118
-  a: -1.26031979158295
+  v: 0.49320534701966068
+  a: -0.56736395433863329
   relative_time: 2.8499999999999979
 }
 trajectory_point {
   path_point {
-    x: 586343.84114370146
-    y: 4140699.3199791908
-    theta: 1.4273856402188903
-    kappa: -0.03834604724432026
-    s: 9.2931641435405776
+    x: 586344.30295299087
+    y: 4140696.9034903622
+    theta: 1.3204379726411277
+    kappa: -0.045742954872859246
+    s: 6.9040673482636024
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1569787362972677
-  a: -1.2325175270568689
+  v: 0.46541352742456477
+  a: -0.54439794507814276
   relative_time: 2.8999999999999977
 }
 trajectory_point {
   path_point {
-    x: 586343.84896652342
-    y: 4140699.3757533524
-    theta: 1.4252165725625463
-    kappa: -0.038292879933569411
-    s: 9.3494842430442464
+    x: 586344.30882980011
+    y: 4140696.9253124655
+    theta: 1.3194817440364774
+    kappa: -0.045467797183252272
+    s: 6.9266669292822343
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0960629379403215
-  a: -1.2039942361590235
+  v: 0.43875664095055344
+  a: -0.52196662949525152
   relative_time: 2.9499999999999975
 }
 trajectory_point {
   path_point {
-    x: 586343.8563712812
-    y: 4140699.4285468543
-    theta: 1.4231634750534068
-    kappa: -0.038242553968167468
-    s: 9.4027945071713468
+    x: 586344.31436724914
+    y: 4140696.9458744368
+    theta: 1.3185807147182991
+    kappa: -0.045208528659038294
+    s: 6.9479614823326115
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0365913297875617
-  a: -1.1747499188894135
+  v: 0.41320795291374646
+  a: -0.50007000758995979
   relative_time: 2.9999999999999973
 }
 trajectory_point {
   path_point {
-    x: 586343.86336813017
-    y: 4140699.4784320984
-    theta: 1.4212235395184196
-    kappa: -0.038195000330049939
-    s: 9.45316804671505
+    x: 586344.31957957265
+    y: 4140696.9652291336
+    theta: 1.3177325730447478
+    kappa: -0.044964482802917122
+    s: 6.9680057489694978
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.97859996315757658
-  a: -1.1447845752480394
+  v: 0.38874072863026421
+  a: -0.47870807936226722
   relative_time: 3.0499999999999972
 }
 trajectory_point {
   path_point {
-    x: 586343.86996747565
-    y: 4140699.525483272
-    theta: 1.4193938839218758
-    kappa: -0.038150148299493716
-    s: 9.50067977503446
+    x: 586344.32448065851
+    y: 4140696.9834281253
+    theta: 1.3169350631093162
+    kappa: -0.044735009392788204
+    s: 6.9868531340134616
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.9221248893689542
-  a: -1.1140982052349009
+  v: 0.36532823341622656
+  a: -0.45788084481217406
   relative_time: 3.099999999999997
 }
 trajectory_point {
   path_point {
-    x: 586343.87640738732
-    y: 4140699.5697413608
-    theta: 1.4176798659124137
-    kappa: -0.0381011509105623
-    s: 9.54540640805461
+    x: 586344.3290840463
+    y: 4140697.0005216869
+    theta: 1.3161859848697284
+    kappa: -0.044519474481750587
+    s: 7.00455570555088
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.867202159740283
-  a: -1.0826908088499985
+  v: 0.34294373258775379
+  a: -0.43758830393968029
   relative_time: 3.1499999999999968
 }
 trajectory_point {
   path_point {
-    x: 586343.88334876171
-    y: 4140699.611184122
-    theta: 1.4161021366697144
-    kappa: -0.038028571205589119
-    s: 9.5874264642664642
+    x: 586344.33340292831
+    y: 4140697.0165588059
+    theta: 1.3154831942588565
+    kappa: -0.044317260398102978
+    s: 7.0211641949339318
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.81386782559015081
-  a: -1.0505623860933313
+  v: 0.32156049146096533
+  a: -0.41783045674478592
   relative_time: 3.1999999999999966
 }
 trajectory_point {
   path_point {
-    x: 586343.88985629973
-    y: 4140699.6500367075
-    theta: 1.4146229656769269
-    kappa: -0.037960527736042658
-    s: 9.626820264726911
+    x: 586344.337450149
+    y: 4140697.0315871765
+    theta: 1.3148246032798785
+    kappa: -0.044127765745343661
+    s: 7.0367279967806056
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.76215793823714617
-  a: -1.0177129369648998
+  v: 0.3011517753519819
+  a: -0.39860730322749083
   relative_time: 3.2499999999999964
 }
 trajectory_point {
   path_point {
-    x: 586343.89594356774
-    y: 4140699.6863801149
-    theta: 1.4132392847073034
-    kappa: -0.03789687865279532
-    s: 9.6636699330587721
+    x: 586344.34123820555
+    y: 4140697.0456532044
+    theta: 1.3142081800876642
+    kappa: -0.043950405402170573
+    s: 7.0512951689746952
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.71210854899985754
-  a: -0.9841424614647043
+  v: 0.28169084957692325
+  a: -0.379918843387795
   relative_time: 3.2999999999999963
 }
 trajectory_point {
   path_point {
-    x: 586343.90162442951
-    y: 4140699.7202971154
-    theta: 1.4119479546303111
-    kappa: -0.037837478993213332
-    s: 9.6980593954508016
+    x: 586344.34477924742
+    y: 4140697.0588020026
+    theta: 1.3136319490581583
+    kappa: -0.043784610522481261
+    s: 7.0649124326657988
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.66375570919687221
-  a: -0.94985095959274435
+  v: 0.26315097945190924
+  a: -0.36176507722569862
   relative_time: 3.3499999999999961
 }
 trajectory_point {
   path_point {
-    x: 586343.90691304684
-    y: 4140699.7518722611
-    theta: 1.410745765943199
-    kappa: -0.03778218068115681
-    s: 9.7300743806576744
+    x: 586344.34808507608
+    y: 4140697.0710773943
+    theta: 1.3130939908473289
+    kappa: -0.043629828535372873
+    s: 7.0776251722693226
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.6171354701467795
-  a: -0.9148384313490201
+  v: 0.24550543029305993
+  a: -0.34414600474120161
   relative_time: 3.3999999999999959
 }
 trajectory_point {
   path_point {
-    x: 586343.91182387911
-    y: 4140699.78119188
-    theta: 1.4096294392354611
-    kappa: -0.037730832526979688
-    s: 9.7598024200000033
+    x: 586344.35116714612
+    y: 4140697.0825219117
+    theta: 1.3125924424410771
+    kappa: -0.04348552314514223
+    s: 7.0894774354664767
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.5722838831681667
-  a: -0.87910487673353155
+  v: 0.22872746741649586
+  a: -0.32706162593430388
   relative_time: 3.4499999999999957
 }
 trajectory_point {
   path_point {
-    x: 586343.91637168359
-    y: 4140699.8083440792
-    theta: 1.4085956255917635
-    kappa: -0.037683280227529772
-    s: 9.7873328473643255
+    x: 586344.35403656377
+    y: 4140697.093176798
+    theta: 1.3121254971973377
+    kappa: -0.043351174331285741
+    s: 7.1005119332042783
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.52923699957962267
-  a: -0.84265029574627881
+  v: 0.21279035613833652
+  a: -0.31051194080500555
   relative_time: 3.4999999999999956
 }
 trajectory_point {
   path_point {
-    x: 586343.92057151534
-    y: 4140699.8334187418
-    theta: 1.4076409069387743
-    kappa: -0.037639366366148716
-    s: 9.81275679920311
+    x: 586344.35670408816
+    y: 4140697.1030820012
+    theta: 1.31169140488146
+    kappa: -0.04322627834849941
+    s: 7.1107700396955522
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.48803087069973516
-  a: -0.80547468838726155
+  v: 0.197667361774702
+  a: -0.29449694935330661
   relative_time: 3.5499999999999954
 }
 trajectory_point {
   path_point {
-    x: 586343.92443872709
-    y: 4140699.8565075318
-    theta: 1.4067617963411785
-    kappa: -0.037598930412672024
-    s: 9.8361672145347541
+    x: 586344.35918013076
+    y: 4140697.1122761839
+    theta: 1.3112884716958177
+    kappa: -0.043110347726678926
+    s: 7.120291792418926
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.44870154784709282
-  a: -0.7675780546564801
+  v: 0.18333174964171273
+  a: -0.27901665157920696
   relative_time: 3.5999999999999952
 }
 trajectory_point {
   path_point {
-    x: 586343.92798896949
-    y: 4140699.8777038883
-    theta: 1.4059547382519768
-    kappa: -0.037561808723429055
-    s: 9.8576588349435834
+    x: 586344.36147475522
+    y: 4140697.1207967135
+    theta: 1.3109150603044888
+    kappa: -0.043002911270919535
+    s: 7.1291158921188362
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.41128508234028383
-  a: -0.72896039455393424
+  v: 0.16975678505548819
+  a: -0.26407104748270671
   relative_time: 3.649999999999995
 }
 trajectory_point {
   path_point {
-    x: 586343.93123819074
-    y: 4140699.8971030293
-    theta: 1.4052161087219532
-    kappa: -0.037527834541243019
-    s: 9.8773282045798556
+    x: 586344.36359767767
+    y: 4140697.1286796704
+    theta: 1.3105695898537344
+    kappa: -0.042903514061516171
+    s: 7.1372797028055226
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.37581752549789549
-  a: -0.6896217080796242
+  v: 0.1569157333321487
+  a: -0.24966013706380574
   relative_time: 3.6999999999999948
 }
 trajectory_point {
   path_point {
-    x: 586343.93420263682
-    y: 4140699.9148019496
-    theta: 1.4045422155729661
-    kappa: -0.037496837995430965
-    s: 9.8952736701597566
+    x: 586344.36555826664
+    y: 4140697.1359598413
+    theta: 1.3102505359889118
+    kappa: -0.042811717453963311
+    s: 7.1448192517550346
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.34233492863851778
-  a: -0.64956199523355
+  v: 0.14478185978781433
+  a: -0.23578392032250417
   relative_time: 3.7499999999999947
 }
 trajectory_point {
   path_point {
-    x: 586343.93689885177
-    y: 4140699.930899424
-    theta: 1.4039292985394614
-    kappa: -0.037468646101803817
-    s: 9.9115953809654
+    x: 586344.36736554315
+    y: 4140697.1426707236
+    theta: 1.3099564308683769
+    kappa: -0.042727099078955125
+    s: 7.151769229509223
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.31087334308073711
-  a: -0.60878125601571109
+  v: 0.13332842973860504
+  a: -0.222442397258802
   relative_time: 3.7999999999999945
 }
 trajectory_point {
   path_point {
-    x: 586343.93934367714
-    y: 4140699.9454960022
-    theta: 1.4033735293823433
-    kappa: -0.037443082762666335
-    s: 9.9263952888448319
+    x: 586344.3690281806
+    y: 4140697.1488445234
+    theta: 1.3096858631748465
+    kappa: -0.042649252842385368
+    s: 7.1581629898757484
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.28146882014314256
-  a: -0.56727949042610826
+  v: 0.12252870850064101
+  a: -0.2096355678726991
   relative_time: 3.8499999999999943
 }
 trajectory_point {
   path_point {
-    x: 586343.94155425241
-    y: 4140699.9586940142
-    theta: 1.4028710119790531
-    kappa: -0.037419968766817149
-    s: 9.939777148212027
+    x: 586344.3705545048
+    y: 4140697.1545121577
+    theta: 1.3094374781246423
+    kappa: -0.042577788925347429
+    s: 7.1640325499280761
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.25415741114432233
-  a: -0.5250566984647409
+  v: 0.11235596139004178
+  a: -0.19736343216419561
   relative_time: 3.8999999999999941
 }
 trajectory_point {
   path_point {
-    x: 586343.94354801462
-    y: 4140699.9705975661
-    theta: 1.4024177823934154
-    kappa: -0.037399121789548705
-    s: 9.9518465160468867
+    x: 586344.37195249356
+    y: 4140697.1597032486
+    theta: 1.3092099774751575
+    kappa: -0.042512333784134278
+    s: 7.169408590005478
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.22897516740286461
-  a: -0.48211288013160924
+  v: 0.10278345372292796
+  a: -0.1856259901332914
   relative_time: 3.949999999999994
 }
 trajectory_point {
   path_point {
-    x: 586343.94534269872
-    y: 4140699.9813125432
-    theta: 1.4020098089285185
-    kappa: -0.037380356392647342
-    s: 9.962710751895246
+    x: 586344.37322977767
+    y: 4140697.1644461332
+    theta: 1.3090021195308581
+    kappa: -0.042452530150238588
+    s: 7.17432045371303
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.20595814023735803
-  a: -0.43844803542671329
+  v: 0.093784450815419529
+  a: -0.17442324177998658
   relative_time: 3.9999999999999938
 }
 trajectory_point {
   path_point {
-    x: 586343.94695534592
-    y: 4140699.9909406872
-    theta: 1.4016432175663185
-    kappa: -0.037363494392100217
-    s: 9.9724730154797623
+    x: 586344.3743935891
+    y: 4140697.1687676637
+    theta: 1.3088127274449006
+    kappa: -0.042398039417466817
+    s: 7.1787959518602893
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.1847786940294566
-  a: -0.40889312908933095
+  v: 0.085320957456000213
+  a: -0.16416535605265858
   relative_time: 4.0499999999999936
 }
 trajectory_point {
   path_point {
-    x: 586343.94839909475
-    y: 4140699.9995604409
-    theta: 1.4013150214202803
-    kappa: -0.037348398409502111
-    s: 9.9812128420963049
+    x: 586344.37545066024
+    y: 4140697.1726928409
+    theta: 1.3086407055965739
+    kappa: -0.042348546352862615
+    s: 7.1828609755380963
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.16505249570842473
-  a: -0.380318119951945
+  v: 0.077363028864567765
+  a: -0.15420065106050238
   relative_time: 4.0999999999999934
 }
 trajectory_point {
   path_point {
-    x: 586343.94968574611
-    y: 4140700.0072422614
-    theta: 1.4010225376914394
-    kappa: -0.037334945053434758
-    s: 9.9890016692677186
+    x: 586344.37640746962
+    y: 4140697.17624572
+    theta: 1.3084849997992549
+    kappa: -0.042303747647129991
+    s: 7.1865404365089329
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.14673055041426233
-  a: -0.35272300801455331
+  v: 0.069896006004363839
+  a: -0.14452912680351776
   relative_time: 4.1499999999999932
 }
 trajectory_point {
   path_point {
-    x: 586343.95082669635
-    y: 4140700.0140541866
-    theta: 1.4007631755847518
-    kappa: -0.037323015163831283
-    s: 9.995908484773846
+    x: 586344.3772703046
+    y: 4140697.1794496477
+    theta: 1.308344586876657
+    kappa: -0.042263348914914255
+    s: 7.1898585135834407
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.12976386328696965
-  a: -0.32610779327715589
+  v: 0.062905229838629839
+  a: -0.13515078328170466
   relative_time: 4.1999999999999931
 }
 trajectory_point {
   path_point {
-    x: 586343.951832937
-    y: 4140700.0200618426
-    theta: 1.4005344363160541
-    kappa: -0.037312493811976259
-    s: 10.001999826651529
+    x: 586344.37804526172
+    y: 4140697.1823272631
+    theta: 1.3082184746646135
+    kappa: -0.042227064694802037
+    s: 7.1928386526204244
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.11410343946654694
-  a: -0.30047247573975272
+  v: 0.056376041330607217
+  a: -0.12606562049506315
   relative_time: 4.2499999999999929
 }
 trajectory_point {
   path_point {
-    x: 586343.95271505462
-    y: 4140700.0253284359
-    theta: 1.4003339131157881
-    kappa: -0.03730327030050562
-    s: 10.007339783194615
+    x: 586344.37873824756
+    y: 4140697.184900498
+    theta: 1.3081057020124511
+    kappa: -0.042194618449321236
+    s: 7.1955035665268507
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.099700284092994557
-  a: -0.2758170554023438
+  v: 0.050293781443537376
+  a: -0.11727363844359318
   relative_time: 4.2999999999999927
 }
 trajectory_point {
   path_point {
-    x: 586343.95348323183
-    y: 4140700.0299147582
-    theta: 1.4001592912303535
-    kappa: -0.037295238163406762
-    s: 10.011989992953943
+    x: 586344.37935497786
+    y: 4140697.1871905755
+    theta: 1.3080053387840314
+    kappa: -0.042165742564941072
+    s: 7.1978752352578477
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.086505402306312762
-  a: -0.25214153226492919
+  v: 0.044643791140661755
+  a: -0.10877483712729474
   relative_time: 4.3499999999999925
 }
 trajectory_point {
   path_point {
-    x: 586343.95414724585
-    y: 4140700.033879186
-    theta: 1.4000083479217755
-    kappa: -0.037288295166018455
-    s: 10.016009644737361
+    x: 586344.37990097748
+    y: 4140697.1892180126
+    theta: 1.3079164858585248
+    kappa: -0.042140178352072025
+    s: 7.1999749058167071
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.074469799246501822
-  a: -0.22944590632750889
+  v: 0.039411411385221751
+  a: -0.10056921654616789
   relative_time: 4.3999999999999924
 }
 trajectory_point {
   path_point {
-    x: 586343.95471646963
-    y: 4140700.037277678
-    theta: 1.3998789524662354
-    kappa: -0.037282343305030906
-    s: 10.019455477609709
+    x: 586344.380381581
+    y: 4140697.1910026171
+    theta: 1.3078382751309761
+    kappa: -0.0421176760450659
+    s: 7.2018230922548812
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.06354448005356203
-  a: -0.20773017759008286
+  v: 0.034581983140458808
+  a: -0.092656776700212592
   relative_time: 4.4499999999999922
 }
 trajectory_point {
   path_point {
-    x: 586343.95519987133
-    y: 4140700.0401637782
-    theta: 1.3997690661518929
-    kappa: -0.037277288808485728
-    s: 10.022381780892831
+    x: 586344.38080193242
+    y: 4140697.19256349
+    theta: 1.3077698695127049
+    kappa: -0.042097994802215793
+    s: 7.2034395756719851
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.053680449867493707
-  a: -0.18699434605265103
+  v: 0.030140847369614324
+  a: -0.085037517589428832
   relative_time: 4.499999999999992
 }
 trajectory_point {
   path_point {
-    x: 586343.95560601435
-    y: 4140700.0425886139
-    theta: 1.3996767422763239
-    kappa: -0.037273042135775938
-    s: 10.024840394165572
+    x: 586344.38116698479
+    y: 4140697.1939190235
+    theta: 1.3077104629315808
+    kappa: -0.0420809027057561
+    s: 7.2048434042157954
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.04482871382829709
-  a: -0.16723841171521348
+  v: 0.026073345035929757
+  a: -0.077711439213816638
   relative_time: 4.5499999999999918
 }
 trajectory_point {
   path_point {
-    x: 586343.95594305766
-    y: 4140700.0446008956
-    theta: 1.3996001261438209
-    kappa: -0.037269517977645973
-    s: 10.026880707263775
+    x: 586344.381481501
+    y: 4140697.1950869029
+    theta: 1.3076592803322016
+    kappa: -0.0420661767618625
+    s: 7.2060528930822514
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0369402770759725
-  a: -0.14846237457777023
+  v: 0.022364817102646511
+  a: -0.070678541573375983
   relative_time: 4.5999999999999917
 }
 trajectory_point {
   path_point {
-    x: 586343.95621875522
-    y: 4140700.04624692
-    theta: 1.3995374550627266
-    kappa: -0.037266635256191685
-    s: 10.028549660280284
+    x: 586344.38175005326
+    y: 4140697.1960841059
+    theta: 1.3076155776760017
+    kappa: -0.042053602900651965
+    s: 7.2070856245154538
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.029966144750520229
-  a: -0.13066623464032129
+  v: 0.019000604533006005
+  a: -0.0639388246681069
   relative_time: 4.6499999999999915
 }
 trajectory_point {
   path_point {
-    x: 586343.95644045668
-    y: 4140700.0475705648
-    theta: 1.3994870583429251
-    kappa: -0.037264317124860336
-    s: 10.029891743564942
+    x: 586344.38197702274
+    y: 4140697.196926902
+    theta: 1.3075786419413105
+    kappa: -0.042042975976182756
+    s: 7.207958447807667
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.023857321991940544
-  a: -0.11384999190286654
+  v: 0.015966048290249654
+  a: -0.057492288498009358
   relative_time: 4.6999999999999913
 }
 trajectory_point {
   path_point {
-    x: 586343.95661510644
-    y: 4140700.048613294
-    theta: 1.3994473572935653
-    kappa: -0.037262490968450586
-    s: 10.030948997724593
+    x: 586344.38216660055
+    y: 4140697.1976308534
+    theta: 1.307547791123373
+    kappa: -0.042034099766454495
+    s: 7.2086874792993143
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.01856481394023371
-  a: -0.0980136463654061
+  v: 0.013246489337618891
+  a: -0.051338933063083375
   relative_time: 4.7499999999999911
 }
 trajectory_point {
   path_point {
-    x: 586343.956749245
-    y: 4140700.0494141537
-    theta: 1.399416865221063
-    kappa: -0.037261088403112534
-    s: 10.031761013623079
+    x: 586344.382322787
+    y: 4140697.1982108131
+    theta: 1.3075223742343498
+    kappa: -0.042026786973408004
+    s: 7.2092881023789843
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.014039625735400074
-  a: -0.083157198027939916
+  v: 0.01082726863835512
+  a: -0.045478758363328958
   relative_time: 4.7999999999999909
 }
 trajectory_point {
   path_point {
-    x: 586343.9568490074
-    y: 4140700.050009775
-    theta: 1.3993941874273956
-    kappa: -0.037260045276347672
-    s: 10.032364932381247
+    x: 586344.38244939165
+    y: 4140697.1986809294
+    theta: 1.3075017713032988
+    kappa: -0.04202085922292545
+    s: 7.2097749674834271
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.010232762517439847
-  a: -0.069280646890467978
+  v: 0.0086937271556998125
+  a: -0.03991176439874608
   relative_time: 4.8499999999999908
 }
 trajectory_point {
   path_point {
-    x: 586343.95692012471
-    y: 4140700.0504343738
-    theta: 1.399378021208689
-    kappa: -0.037259301667008915
-    s: 10.032795445376939
+    x: 586344.38255003374
+    y: 4140697.19905464
+    theta: 1.3074853933761528
+    kappa: -0.042016147064830311
+    s: 7.2101619920975519
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0070952294263533777
-  a: -0.056383992952990347
+  v: 0.0068312058528943453
+  a: -0.034637951169334741
   relative_time: 4.8999999999999906
 }
 trajectory_point {
   path_point {
-    x: 586343.95696792286
-    y: 4140700.0507197473
-    theta: 1.3993671558540832
-    kappa: -0.037258801885300577
-    s: 10.033084794244997
+    x: 586344.3826281419
+    y: 4140697.1993446751
+    theta: 1.30747268251569
+    kappa: -0.042012489972887333
+    s: 7.210462360754434
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0045780316021409306
-  a: -0.044467236215506967
+  v: 0.0052250456931801637
+  a: -0.029657318675094996
   relative_time: 4.94999999999999
 }
 trajectory_point {
   path_point {
-    x: 586343.95699732308
-    y: 4140700.0508952788
-    theta: 1.3993604726448494
-    kappa: -0.037258494472778396
-    s: 10.033262770877267
+    x: 586344.38268695376
+    y: 4140697.1995630586
+    theta: 1.3074631118015083
+    kappa: -0.042009736344802553
+    s: 7.2106885250353088
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0026321741848027991
-  a: -0.033530376678017837
+  v: 0.003860587639798671
+  a: -0.024969866916026789
   relative_time: 4.99999999999999
 }
 trajectory_point {
   path_point {
-    x: 586343.95701284241
-    y: 4140700.0509879347
-    theta: 1.399356944853732
-    kappa: -0.037258332202349517
-    s: 10.033356717422592
+    x: 586344.38272951683
+    y: 4140697.1997211059
+    theta: 1.3074561853299984
+    kappa: -0.042007743502223326
+    s: 7.2108522035695737
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0012086623143392761
-  a: -0.023573414340523013
+  v: 0.0027231726559913128
+  a: -0.020575595892130122
   relative_time: 5.04999999999999
 }
 trajectory_point {
   path_point {
-    x: 586343.95701859239
-    y: 4140700.0510222651
-    theta: 1.3993556377444798
-    kappa: -0.0372582720782725
-    s: 10.033391526286815
+    x: 586344.38275868772
+    y: 4140697.1998294257
+    theta: 1.3074514382143223
+    kappa: -0.0420063776907383
+    s: 7.2109643820347884
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00025850113075057135
-  a: -0.014596349203022441
+  v: 0.0017981417049994647
+  a: -0.016474505603405021
   relative_time: 5.09999999999999
 }
 trajectory_point {
   path_point {
-    x: 586343.95701828087
-    y: 4140700.0510204053
-    theta: 1.3993557085715358
-    kappa: -0.037258275336157325
-    s: 10.033389640132782
+    x: 586344.38277713279
+    y: 4140697.1998979161
+    theta: 1.3074484365843941
+    kappa: -0.0420055140798774
+    s: 7.2110353131566747
     dkappa: 0
     ddkappa: 0
   }
-  v: -0.00026730422596288328
-  a: -0.0065991812655161186
+  v: 0.0010708357500645999
+  a: -0.012666596049851486
   relative_time: 5.14999999999999
 }
 trajectory_point {
   path_point {
-    x: 586343.9570152103
-    y: 4140700.0510020722
-    theta: 1.3993564065798516
-    kappa: -0.037258307442965363
-    s: 10.033371051880334
+    x: 586344.38278732728
+    y: 4140697.1999357715
+    theta: 1.3074467775868666
+    kappa: -0.042005036763111861
+    s: 7.2110745167091164
     dkappa: 0
     ddkappa: 0
   }
-  v: -0.00041774861580096134
-  a: 0.00041808947199589719
+  v: 0.00052659575442813578
+  a: -0.00915186723146949
   relative_time: 5.1999999999999895
 }
 trajectory_point {
   path_point {
-    x: 586343.95701227849
-    y: 4140700.0509845689
-    theta: 1.3993570730047982
-    kappa: -0.037258338097009423
-    s: 10.033353304706315
+    x: 586344.38279155619
+    y: 4140697.1999514746
+    theta: 1.3074460893851187
+    kappa: -0.042004838757854213
+    s: 7.21109077951416
     dkappa: 0
     ddkappa: 0
   }
-  v: -0.00024182689876320329
-  a: 0.0064554630095136623
+  v: 0.0001507626813314622
+  a: -0.0059303191482590334
   relative_time: 5.2499999999999893
 }
 trajectory_point {
   path_point {
-    x: 586343.95701197907
-    y: 4140700.0509827817
-    theta: 1.3993571410721426
-    kappa: -0.0372583412279537
-    s: 10.033351492044572
+    x: 586344.382791914
+    y: 4140697.1999528031
+    theta: 1.3074460311592502
+    kappa: -0.042004822005458284
+    s: 7.2110921554420129
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00021146606515057287
-  a: 0.011512939347037066
+  v: -7.1322505983961726e-05
+  a: -0.0030019518002201984
   relative_time: 5.2999999999999892
 }
 trajectory_point {
   path_point {
-    x: 586343.95701640064
-    y: 4140700.0510091791
-    theta: 1.3993561359980706
-    kappa: -0.037258294996813823
-    s: 10.033378257585946
+    x: 586344.38279030437
+    y: 4140697.1999468263
+    theta: 1.307446293106074
+    kappa: -0.042004897371219194
+    s: 7.2110859654110451
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00089313541594063239
-  a: 0.015590518484566274
+  v: -0.00015431884427676013
+  a: -0.00036676518735287433
   relative_time: 5.349999999999989
 }
 trajectory_point {
   path_point {
-    x: 586343.95702722692
-    y: 4140700.0510738166
-    theta: 1.3993536749892281
-    kappa: -0.037258181795956823
-    s: 10.033443795278281
+    x: 586344.38278844045
+    y: 4140697.1999399047
+    theta: 1.3074465964391164
+    kappa: -0.042004984644373355
+    s: 7.2110787973877892
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0017541862936072128
-  a: 0.018688200422101231
+  v: -0.00011288537030543222
+  a: 0.0019752406903429109
   relative_time: 5.3999999999999888
 }
 trajectory_point {
   path_point {
-    x: 586343.9570457373
-    y: 4140700.0511843311
-    theta: 1.3993494672427704
-    kappa: -0.037257988249101148
-    s: 10.033555849326421
+    x: 586344.38278784463
+    y: 4140697.1999376928
+    theta: 1.3074466933886146
+    kappa: -0.042005012538098463
+    s: 7.21107650638694
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0027456238381508291
-  a: 0.020805985159641882
+  v: 3.8318879171342357e-05
+  a: 0.00402406583286713
   relative_time: 5.4499999999999886
 }
 trajectory_point {
   path_point {
-    x: 586343.95707280643
-    y: 4140700.0513459444
-    theta: 1.399343313946402
-    kappa: -0.037257705211316654
-    s: 10.033719714192211
+    x: 586344.38278984907
+    y: 4140697.1999451355
+    theta: 1.3074463672015177
+    kappa: -0.042004918689513546
+    s: 7.2110842144713523
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0038184531895714968
-  a: 0.021943872697188338
+  v: 0.00028463486739506438
+  a: 0.00577971024021981
   relative_time: 5.4999999999999885
 }
 trajectory_point {
   path_point {
-    x: 586343.95710890426
-    y: 4140700.0515614627
-    theta: 1.3993351082783951
-    kappa: -0.037257327769024615
-    s: 10.033938234594492
+    x: 586344.382795595
+    y: 4140697.1999664712
+    theta: 1.3074454321414875
+    kappa: -0.042004649659678907
+    s: 7.2111063107520454
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0049236794878697032
-  a: 0.022101863034740432
+  v: 0.00061140355760713749
+  a: 0.0072421739124008955
   relative_time: 5.5499999999999883
 }
 trajectory_point {
   path_point {
-    x: 586343.957154096
-    y: 4140700.0518312752
-    theta: 1.3993248354075818
-    kappa: -0.037256855239997708
-    s: 10.034211805509113
+    x: 586344.38280603313
+    y: 4140697.200005231
+    theta: 1.3074437334888998
+    kappa: -0.04200416093359613
+    s: 7.2111464513882
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0060123078730456858
-  a: 0.021279956172298331
+  v: 0.0010039659130489514
+  a: 0.0084114568494104425
   relative_time: 5.5999999999999881
 }
 trajectory_point {
   path_point {
-    x: 586343.95720804215
-    y: 4140700.0521533554
-    theta: 1.3993125724933162
-    kappa: -0.037256291173360032
-    s: 10.034538372168912
+    x: 586344.38282192382
+    y: 4140697.2000642368
+    theta: 1.3074411475408465
+    kappa: -0.042003416920208106
+    s: 7.2112075595871588
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0070353434850996266
-  a: 0.019478152109861924
+  v: 0.0014476628969619931
+  a: 0.00928755905124845
   relative_time: 5.6499999999999879
 }
 trajectory_point {
   path_point {
-    x: 586343.95726999873
-    y: 4140700.0525232605
-    theta: 1.3992984886854016
-    kappa: -0.037255643349587093
-    s: 10.034913430063737
+    x: 586344.38284383644
+    y: 4140697.2001456041
+    theta: 1.307437581611137
+    kappa: -0.042002390952399049
+    s: 7.2112918256044249
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0079437914640319851
-  a: 0.016696450847431266
+  v: 0.0019278354725876384
+  a: 0.0098704805179148924
   relative_time: 5.6999999999999877
 }
 trajectory_point {
   path_point {
-    x: 586343.95733881684
-    y: 4140700.0529341316
-    theta: 1.3992828451239914
-    kappa: -0.037254923780505815
-    s: 10.03533002494043
+    x: 586344.38287214993
+    y: 4140697.2002507392
+    theta: 1.3074329740302977
+    kappa: -0.042001065286994425
+    s: 7.2114007067436665
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00868865694984286
-  a: 0.012934852385006301
+  v: 0.0024298246031672632
+  a: 0.010160221249409795
   relative_time: 5.7499999999999876
 }
 trajectory_point {
   path_point {
-    x: 586343.957412943
-    y: 4140700.0533766947
-    theta: 1.3992659949394632
-    kappa: -0.03725414870929452
-    s: 10.035778752802836
+    x: 586344.38290705276
+    y: 4140697.2003803421
+    theta: 1.3074272941455736
+    kappa: -0.041999431104761009
+    s: 7.2115349273567118
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0092209450825326555
-  a: 0.00819335672258703
+  v: 0.00293897125194241
+  a: 0.010156781245733132
   relative_time: 5.7999999999999874
 }
 trajectory_point {
   path_point {
-    x: 586343.95749041927
-    y: 4140700.0538392588
-    theta: 1.3992483832522804
-    kappa: -0.037253338610482967
-    s: 10.036247759911797
+    x: 586344.3829485426
+    y: 4140697.2005344052
+    theta: 1.3074205423209273
+    kappa: -0.041997488510406904
+    s: 7.2116944788435511
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0094916610021017478
-  a: 0.002471963860173565
+  v: 0.0034406163821544267
+  a: 0.0098601605068849019
   relative_time: 5.8499999999999872
 }
 trajectory_point {
   path_point {
-    x: 586343.95756888262
-    y: 4140700.054307716
-    theta: 1.3992305471728508
-    kappa: -0.037252518189952295
-    s: 10.036722742785161
+    x: 586344.38299642666
+    y: 4140697.2007122114
+    theta: 1.3074127499370378
+    kappa: -0.041995246532581451
+    s: 7.2118786196523388
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0094518098485500968
-  a: -0.0042293262022342071
+  v: 0.0039201009570446754
+  a: 0.0092703590328651331
   relative_time: 5.899999999999987
 }
 trajectory_point {
   path_point {
-    x: 586343.9576455655
-    y: 4140700.0547655439
-    theta: 1.3992131158014007
-    kappa: -0.037251716384935088
-    s: 10.037186948197768
+    x: 586344.3830503216
+    y: 4140697.2009123368
+    theta: 1.3074039793912982
+    kappa: -0.041992723123875336
+    s: 7.2120858752793877
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0090523967618783563
-  a: -0.01191051346463623
+  v: 0.004362765939854657
+  a: 0.0083873768236737978
   relative_time: 5.9499999999999869
 }
 trajectory_point {
   path_point {
-    x: 586343.957717296
-    y: 4140700.0551938033
-    theta: 1.3991968102278858
-    kappa: -0.037250966364015328
-    s: 10.037621173181464
+    x: 586344.38310965314
+    y: 4140697.2011326505
+    theta: 1.3073943240978128
+    kappa: -0.041989945160820524
+    s: 7.2123140382691764
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0082444268820865141
-  a: -0.020571597927032503
+  v: 0.0047539522938259277
+  a: 0.007211213879310896
   relative_time: 5.9999999999999867
 }
 trajectory_point {
   path_point {
-    x: 586343.95778125222
-    y: 4140700.0555756469
-    theta: 1.3991822718832347
-    kappa: -0.037250297631658186
-    s: 10.038008336111488
+    x: 586344.383173832
+    y: 4140697.2013709624
+    theta: 1.3073838800253283
+    kappa: -0.041986940254955779
+    s: 7.2125608407977211
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0072551001384975637
-  a: -0.019010419580605519
+  v: 0.0051199247493714161
+  a: 0.007424322047512609
   relative_time: 6.0499999999999865
 }
 trajectory_point {
   path_point {
-    x: 586343.95783735567
-    y: 4140700.0559106064
-    theta: 1.3991695186100097
-    kappa: -0.037249711008624661
-    s: 10.038347961807858
+    x: 586344.383242836
+    y: 4140697.2016271926
+    theta: 1.3073726506835592
+    kappa: -0.041983709416061665
+    s: 7.2128261999285161
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0063424901476170871
-  a: -0.01750292781870133
+  v: 0.0054960482690769039
+  a: 0.0076172564457053695
   relative_time: 6.0999999999999863
 }
 trajectory_point {
   path_point {
-    x: 586343.9578862288
-    y: 4140700.0562023991
-    theta: 1.3991584088880031
-    kappa: -0.037249199985326407
-    s: 10.038643818999976
+    x: 586344.38331679115
+    y: 4140697.2019018065
+    theta: 1.3073606156612905
+    kappa: -0.041980246771531164
+    s: 7.2131105979975567
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0055039125802186723
-  a: -0.016049122641322997
+  v: 0.0058813141644417921
+  a: 0.0077900170738886424
   relative_time: 6.1499999999999861
 }
 trajectory_point {
   path_point {
-    x: 586343.9579284722
-    y: 4140700.0564546091
-    theta: 1.3991488062371367
-    kappa: -0.037248758284002259
-    s: 10.038899542200785
+    x: 586344.38339580933
+    y: 4140697.2021952216
+    theta: 1.3073477566815748
+    kappa: -0.041976547062813757
+    s: 7.2134144669064133
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0047366831070760288
-  a: -0.014649004048470518
+  v: 0.0062747137469656069
+  a: 0.0079426039320624284
   relative_time: 6.199999999999986
 }
 trajectory_point {
   path_point {
-    x: 586343.95796466409
-    y: 4140700.056670689
-    theta: 1.3991405792174347
-    kappa: -0.037248379858718234
-    s: 10.039118631706767
+    x: 586344.38347999007
+    y: 4140697.202507806
+    theta: 1.3073340576017303
+    kappa: -0.041972605645415453
+    s: 7.2137381881222309
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0040381173989628618
-  a: -0.01330257204014389
+  v: 0.0066752383281478743
+  a: 0.0080750170202267286
   relative_time: 6.2499999999999858
 }
 trajectory_point {
   path_point {
-    x: 586343.95799536037
-    y: 4140700.0568539575
-    theta: 1.3991336014289983
-    kappa: -0.03724805889536751
-    s: 10.039304453597945
+    x: 586344.38356941915
+    y: 4140697.2028398793
+    theta: 1.3073195044133434
+    kappa: -0.041968418488898762
+    s: 7.2140820926777307
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.003405531126652879
-  a: -0.01200982661634312
+  v: 0.00708187921948812
+  a: 0.00818725633838154
   relative_time: 6.2999999999999856
 }
 trajectory_point {
   path_point {
-    x: 586343.95802109491
-    y: 4140700.0570076033
-    theta: 1.3991277515119853
-    kappa: -0.037247789811670477
-    s: 10.039460239737876
+    x: 586344.38366416981
+    y: 4140697.2031917125
+    theta: 1.3073040852422677
+    kappa: -0.041963982176882714
+    s: 7.2144464611712076
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00283623996091979
-  a: -0.0107707677770682
+  v: 0.0074936277324858672
+  a: 0.008279321886526865
   relative_time: 6.3499999999999854
 }
 trajectory_point {
   path_point {
-    x: 586343.9580423797
-    y: 4140700.0571346814
-    theta: 1.3991229131465899
-    kappa: -0.03724756725717468
-    s: 10.039589087773658
+    x: 586344.38376430166
+    y: 4140697.2035635286
+    theta: 1.3072877903486242
+    kappa: -0.041959293907042838
+    s: 7.2148315237665317
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0023275595725372987
-  a: -0.0095853955223191359
+  v: 0.0079094751786406448
+  a: 0.0083512136646627013
   relative_time: 6.3999999999999853
 }
 trajectory_point {
   path_point {
-    x: 586343.95805970381
-    y: 4140700.0572381141
-    theta: 1.3991189750530268
-    kappa: -0.037247386113254857
-    s: 10.039693961135931
+    x: 586344.38386986172
+    y: 4140697.2039554995
+    theta: 1.3072706121268023
+    kappa: -0.04195435149111118
+    s: 7.2152374601931486
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0018768056322791158
-  a: -0.0084537098520959253
+  v: 0.008328412869451976
+  a: 0.0084029316727890525
   relative_time: 6.4499999999999851
 }
 trajectory_point {
   path_point {
-    x: 586343.958073535
-    y: 4140700.0573206916
-    theta: 1.3991158309915179
-    kappa: -0.037247241493112933
-    s: 10.039777689038868
+    x: 586344.38398088329
+    y: 4140697.2043677517
+    theta: 1.3072525451054589
+    kappa: -0.0419491533548763
+    s: 7.2156643997460792
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0014812938109189456
-  a: -0.00737571076639857
+  v: 0.00874943211641939
+  a: 0.0084344759109059152
   relative_time: 6.4999999999999849
 }
 trajectory_point {
   path_point {
-    x: 586343.95808431832
-    y: 4140700.0573850721
-    theta: 1.399113379762279
-    kappa: -0.037247128741777991
-    s: 10.039842966480185
+    x: 586344.38409738708
+    y: 4140697.20480036
+    theta: 1.3072335859475201
+    kappa: -0.041943698538183249
+    s: 7.216112421285918
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0011383397792304973
-  a: -0.0063513982652270654
+  v: 0.0091715242310424078
+  a: 0.0084458463790132929
   relative_time: 6.5499999999999847
 }
 trajectory_point {
   path_point {
-    x: 586343.95809247682
-    y: 4140700.0574337812
-    theta: 1.3991115252055115
-    kappa: -0.037247043436106318
-    s: 10.039892354241132
+    x: 586344.38421938056
+    y: 4140697.2052533529
+    theta: 1.30721373345018
+    kappa: -0.041937986694933628
+    s: 7.2165815532388358
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00084525920798747718
-  a: -0.0053807723485814161
+  v: 0.0095936805248205553
+  a: 0.00843704307711118
   relative_time: 6.5999999999999845
 }
 trajectory_point {
   path_point {
-    x: 586343.95809841133
-    y: 4140700.0574692125
-    theta: 1.3991101762013933
-    kappa: -0.037246981384781372
-    s: 10.039928278886503
+    x: 586344.38434685767
+    y: 4140697.2057267083
+    theta: 1.3071929885449014
+    kappa: -0.041932018093085506
+    s: 7.2170717735965777
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00059936776796359285
-  a: -0.0044638330164616206
+  v: 0.010014892309253363
+  a: 0.0084080660051995826
   relative_time: 6.6499999999999844
 }
 trajectory_point {
   path_point {
-    x: 586343.95810250042
-    y: 4140700.0574936261
-    theta: 1.3991092466700719
-    kappa: -0.03724693862831379
-    s: 10.039953032764629
+    x: 586344.38447979989
+    y: 4140697.2062203572
+    theta: 1.3071713542974155
+    kappa: -0.041925793614653484
+    s: 7.2175830099164644
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00039798112993255121
-  a: -0.0036005802688676772
+  v: 0.010434150895840353
+  a: 0.0083589151632784964
   relative_time: 6.6999999999999842
 }
 trajectory_point {
   path_point {
-    x: 586343.95810510078
-    y: 4140700.0575091513
-    theta: 1.3991086555716605
-    kappa: -0.037246911439041394
-    s: 10.039968774007376
+    x: 586344.38461817522
+    y: 4140697.2067341795
+    theta: 1.3071488359077219
+    kappa: -0.041919314755708689
+    s: 7.21811513932139
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00023841496466806347
-  a: -0.0027910141057995927
+  v: 0.010850447596081052
+  a: 0.0082895905513479251
   relative_time: 6.749999999999984
 }
 trajectory_point {
   path_point {
-    x: 586343.95810654655
-    y: 4140700.0575177837
-    theta: 1.3991083269062328
-    kappa: -0.03724689632112918
-    s: 10.039977526530153
+    x: 586344.38476193836
+    y: 4140697.20726801
+    theta: 1.3071254407100885
+    kappa: -0.04191258362637873
+    s: 7.2186679884998251
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00011798494294383045
-  a: -0.0020351345272573602
+  v: 0.011262773721474984
+  a: 0.0082000921694078653
   relative_time: 6.7999999999999838
 }
 trajectory_point {
   path_point {
-    x: 586343.95810715016
-    y: 4140700.057521387
-    theta: 1.3991081897138204
-    kappa: -0.037246890010569328
-    s: 10.039981180031909
+    x: 586344.38491103146
+    y: 4140697.2078216309
+    theta: 1.3071011781730515
+    kappa: -0.041905602950847748
+    s: 7.2192413337058143
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.4006735533560761e-05
-  a: -0.0013329415332409764
+  v: 0.011670120583521678
+  a: 0.0080904200174583187
   relative_time: 6.8499999999999837
 }
 trajectory_point {
   path_point {
-    x: 586343.95810720127
-    y: 4140700.0575216925
-    theta: 1.3991081780744112
-    kappa: -0.037246889475181212
-    s: 10.039981489995126
+    x: 586344.385065383
+    y: 4140697.2083947775
+    theta: 1.3070760598994149
+    kappa: -0.041898376067356365
+    s: 7.219834900758979
     dkappa: 0
     ddkappa: 0
   }
-  v: -1.6203986789031788e-05
-  a: -0.00068443512375045146
+  v: 0.012071479493720658
+  a: 0.0079605740954992836
   relative_time: 6.8999999999999835
 }
 trajectory_point {
   path_point {
-    x: 586343.95810696809
-    y: 4140700.0575202992
-    theta: 1.3991082311079481
-    kappa: -0.037246891914611363
-    s: 10.039980077685827
+    x: 586344.38522490859
+    y: 4140697.2089871373
+    theta: 1.3070500996262495
+    kappa: -0.041890906928201743
+    s: 7.2204483650445139
     dkappa: 0
     ddkappa: 0
   }
-  v: -3.5331553250247255e-05
-  a: -8.9615298785775122e-05
+  v: 0.012465841763571446
+  a: 0.0078105544035307634
   relative_time: 6.9499999999999833
 }
 trajectory_point {
   path_point {
-    x: 586343.95810669591
-    y: 4140700.057518675
-    theta: 1.3991082929743268
-    kappa: -0.037246894760333514
-    s: 10.039978430153578
+    x: 586344.38538951077
+    y: 4140697.2095983475
+    theta: 1.3070233132248934
+    kappa: -0.041883200099737561
+    s: 7.2210813515131891
     dkappa: 0
     ddkappa: 0
   }
-  v: -2.6060293076375293e-05
-  a: 0.00045151794165304224
+  v: 0.012852198704573572
+  a: 0.0076403609415527556
   relative_time: 6.9999999999999831
 }
 trajectory_point {
   path_point {
-    x: 586343.95810660836
-    y: 4140700.0575181521
-    theta: 1.3991083128733981
-    kappa: -0.037246895675648567
-    s: 10.039977900231477
+    x: 586344.385559079
+    y: 4140697.210227997
+    theta: 1.3069957187009504
+    kappa: -0.041875260762373981
+    s: 7.2217334346813491
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.9254645062944449e-06
-  a: 0.00093896459756600409
+  v: 0.01322954162822656
+  a: 0.00744999370956526
   relative_time: 7.0499999999999829
 }
 trajectory_point {
   path_point {
-    x: 586343.95810690662
-    y: 4140700.0575199332
-    theta: 1.3991082450449654
-    kappa: -0.037246892555684592
-    s: 10.039979706536165
+    x: 586344.38573348918
+    y: 4140697.2108756276
+    theta: 1.3069673361942902
+    kappa: -0.041867094710577694
+    s: 7.2224041386309139
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.69413902714619e-05
-  a: 0.0013727246689531139
+  v: 0.013596861846029937
+  a: 0.0072394527075682769
   relative_time: 7.0999999999999828
 }
 trajectory_point {
   path_point {
-    x: 586343.95810777019
-    y: 4140700.0575250885
-    theta: 1.3991080487687872
-    kappa: -0.037246883527396878
-    s: 10.039984933467819
+    x: 586344.38591260486
+    y: 4140697.2115407297
+    theta: 1.3069381879790458
+    kappa: -0.041858708352871879
+    s: 7.22309293700938
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00014530315499284088
-  a: 0.0017527981558143717
+  v: 0.013953150669483227
+  a: 0.0070087379355618078
   relative_time: 7.1499999999999826
 }
 trajectory_point {
   path_point {
-    x: 586343.95810935553
-    y: 4140700.0575345545
-    theta: 1.3991076883645759
-    kappa: -0.037246866949567851
-    s: 10.039994531210159
+    x: 586344.38609627564
+    y: 4140697.2122227466
+    theta: 1.3069082984636144
+    kappa: -0.041850108711836267
+    s: 7.2237992530298154
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00024132642944413307
-  a: 0.0020791850581497774
+  v: 0.014297399410085957
+  a: 0.00675784939354585
   relative_time: 7.1999999999999824
 }
 trajectory_point {
   path_point {
-    x: 586343.95811179793
-    y: 4140700.0575491358
-    theta: 1.3991071331919995
-    kappa: -0.037246841412807152
-    s: 10.040009315730439
+    x: 586344.38628433878
+    y: 4140697.2129210732
+    theta: 1.3068776941906552
+    kappa: -0.041841303424107079
+    s: 7.2245224594708652
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.000352326884399054
-  a: 0.0023518853759593311
+  v: 0.014628599379337652
+  a: 0.0064867870815204067
   relative_time: 7.2499999999999822
 }
 trajectory_point {
   path_point {
-    x: 586343.95811520959
-    y: 4140700.0575695052
-    theta: 1.3991063576506819
-    kappa: -0.037246805739551581
-    s: 10.040029968779454
+    x: 586344.38647661777
+    y: 4140697.2136350549
+    theta: 1.3068464038370875
+    kappa: -0.041832300740377019
+    s: 7.2252618786767515
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00047562019063130106
-  a: 0.0025708991092430258
+  v: 0.014945741888737837
+  a: 0.0061955509994854755
   relative_time: 7.2999999999999821
 }
 trajectory_point {
   path_point {
-    x: 586343.95811968122
-    y: 4140700.057596202
-    theta: 1.3991053411802028
-    kappa: -0.037246758984065133
-    s: 10.040057037891536
+    x: 586344.38667292346
+    y: 4140697.2143639885
+    theta: 1.30681445821409
+    kappa: -0.041823109525395349
+    s: 7.2260167825572665
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00060852201891458887
-  a: 0.0027362262580008651
+  v: 0.015247818249786039
+  a: 0.0058841411474410566
   relative_time: 7.3499999999999819
 }
 trajectory_point {
   path_point {
-    x: 586343.958125281
-    y: 4140700.0576296356
-    theta: 1.3991040682600988
-    kappa: -0.037246700432438974
-    s: 10.040090936384559
+    x: 586344.38687305327
+    y: 4140697.2151071215
+    theta: 1.3067818902670987
+    kappa: -0.0418137392579678
+    s: 7.2267863925877824
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00074834804002262348
-  a: 0.0028478668222328522
+  v: 0.015533819773981782
+  a: 0.0055525575253871518
   relative_time: 7.3999999999999817
 }
 trajectory_point {
   path_point {
-    x: 586343.958132055
-    y: 4140700.0576700792
-    theta: 1.3991025284098626
-    kappa: -0.037246629602591452
-    s: 10.040131943359933
+    x: 586344.38707679184
+    y: 4140697.2158636553
+    theta: 1.3067487350758051
+    kappa: -0.041804200030956661
+    s: 7.2275698798092423
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.00089241392472911088
-  a: 0.0029058208019389839
+  v: 0.015802737772824593
+  a: 0.0052008001333237585
   relative_time: 7.4499999999999815
 }
 trajectory_point {
   path_point {
-    x: 586343.95814002724
-    y: 4140700.0577176763
-    theta: 1.3991007161889446
-    kappa: -0.0372465462442681
-    s: 10.040180203702604
+    x: 586344.3872839103
+    y: 4140697.2166327396
+    theta: 1.3067150298541543
+    kappa: -0.041794502551280671
+    s: 7.2283663648281671
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0010380353438077571
-  a: 0.002910088197119267
+  v: 0.016053563557813996
+  a: 0.0048288689712508784
   relative_time: 7.4999999999999813
 }
 trajectory_point {
   path_point {
-    x: 586343.95814919937
-    y: 4140700.0577724376
-    theta: 1.3990986311967504
-    kappa: -0.037246450339041622
-    s: 10.040235728081065
+    x: 586344.38749416685
+    y: 4140697.2174134762
+    theta: 1.3066808139503425
+    kappa: -0.041784658139915137
+    s: 7.2291749178166516
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0011825279680322707
-  a: 0.0028606690077736946
+  v: 0.016285288440449523
+  a: 0.0044367640391685115
   relative_time: 7.5499999999999812
 }
 trajectory_point {
   path_point {
-    x: 586343.958159551
-    y: 4140700.0578342415
-    theta: 1.3990962780726421
-    kappa: -0.037246342100311913
-    s: 10.04029839294734
+    x: 586344.38770730677
+    y: 4140697.2182049192
+    theta: 1.3066461288468152
+    kappa: -0.041774678731891848
+    s: 7.2299945585123648
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0013232074681763639
-  a: 0.00275756323390227
+  v: 0.016496903732230693
+  a: 0.0040244853370766578
   relative_time: 7.599999999999981
 }
 trajectory_point {
   path_point {
-    x: 586343.95817103982
-    y: 4140700.0579028339
-    theta: 1.3990936664959366
-    kappa: -0.037246221973306057
-    s: 10.040367940536994
+    x: 586344.38792306185
+    y: 4140697.2190060737
+    theta: 1.3066110181602633
+    kappa: -0.0417645768762991
+    s: 7.2308242562185523
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.001457389515013733
-  a: 0.0026007708755049867
+  v: 0.016687400744657029
+  a: 0.0035920328649753164
   relative_time: 7.6499999999999808
 }
 trajectory_point {
   path_point {
-    x: 586343.9581836008
-    y: 4140700.0579778273
-    theta: 1.3990908111859048
-    kappa: -0.0372460906350783
-    s: 10.040443978869133
+    x: 586344.388141151
+    y: 4140697.219815895
+    theta: 1.3065755276416222
+    kappa: -0.041754365736281732
+    s: 7.2316629298040329
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0015823897793180962
-  a: 0.0023902919325818513
+  v: 0.016855770789228064
+  a: 0.0031394066228644883
   relative_time: 7.6999999999999806
 }
 trajectory_point {
   path_point {
-    x: 586343.95819714689
-    y: 4140700.0580587038
-    theta: 1.3990877319017709
-    kappa: -0.037245948994510072
-    s: 10.040525981746397
+    x: 586344.38836128009
+    y: 4140697.2206332907
+    theta: 1.3065397051760681
+    kappa: -0.041744059089041045
+    s: 7.2325094477032019
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0016955239318631491
-  a: 0.0021261264051328672
+  v: 0.017001005177443317
+  a: 0.0026666066107441724
   relative_time: 7.7499999999999805
 }
 trajectory_point {
   path_point {
-    x: 586343.95821156935
-    y: 4140700.0581448111
-    theta: 1.3990844534427096
-    kappa: -0.037245798192309991
-    s: 10.040613288754969
+    x: 586344.38858314161
+    y: 4140697.22145712
+    theta: 1.3065036007830149
+    kappa: -0.041733671325834873
+    s: 7.23336262791603
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0017941076434226117
-  a: 0.0018082742931580208
+  v: 0.017122095220802321
+  a: 0.00217363282861437
   relative_time: 7.79999999999998
 }
 trajectory_point {
   path_point {
-    x: 586343.95822673663
-    y: 4140700.0582353664
-    theta: 1.3990810056478462
-    kappa: -0.037245639601013854
-    s: 10.040705105264569
+    x: 586344.38880641514
+    y: 4140697.2222861913
+    theta: 1.3064672666161115
+    kappa: -0.0417232174519776
+    s: 7.2342212380080593
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0018754565847701916
-  a: 0.0014367355966573223
+  v: 0.0172180322308046
+  a: 0.0016604852764750795
   relative_time: 7.84999999999998
 }
 trajectory_point {
   path_point {
-    x: 586343.95824249554
-    y: 4140700.0583294528
-    theta: 1.3990774233962537
-    kappa: -0.037245474824984637
-    s: 10.040800502428455
+    x: 586344.389030767
+    y: 4140697.2231192673
+    theta: 1.3064307569632385
+    kappa: -0.041712713086840048
+    s: 7.2350839951104122
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0019368864266795828
-  a: 0.0010115103156307786
+  v: 0.01728780751894967
+  a: 0.0011271639543263025
   relative_time: 7.89999999999998
 }
 trajectory_point {
   path_point {
-    x: 586343.95825867029
-    y: 4140700.0584260225
-    theta: 1.3990737466069521
-    kappa: -0.037245305700412491
-    s: 10.040898417183426
+    x: 586344.38925585058
+    y: 4140697.2239550604
+    theta: 1.3063941282465057
+    kappa: -0.041702174463849606
+    s: 7.2359495659197819
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0019757128399245051
-  a: 0.00053259845007837259
+  v: 0.01733041239673707
+  a: 0.00057366886216803862
   relative_time: 7.94999999999998
 }
 trajectory_point {
   path_point {
-    x: 586343.9582750632
-    y: 4140700.0585238943
-    theta: 1.3990700202389057
-    kappa: -0.037245134295314758
-    s: 10.040997652249816
+    x: 586344.38948130608
+    y: 4140697.2247922346
+    theta: 1.3063574390222479
+    kappa: -0.041691618430490143
+    s: 7.2368165666984385
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0019892514952786541
-  a: 1.1102230246251565e-16
+  v: 0.017344838175666316
+  a: 2.8883145875013838e-16
   relative_time: 7.99999999999998
 }
 decision {
@@ -2263,10 +2263,10 @@ decision {
       reason_code: STOP_REASON_OBSTACLE
       reason: "stop by 1_0"
       stop_point {
-        x: 586344.64474474289
-        y: 4140704.4574318649
+        x: 586346.35873687046
+        y: 4140703.8363721515
       }
-      stop_heading: 1.2658351870002598
+      stop_heading: 1.2622196382208788
     }
   }
   object_decision {
@@ -2278,10 +2278,10 @@ decision {
           reason_code: STOP_REASON_OBSTACLE
           distance_s: -5
           stop_point {
-            x: 586344.64474474289
-            y: 4140704.4574318649
+            x: 586346.35873687046
+            y: 4140703.8363721515
           }
-          stop_heading: 1.2658351870002598
+          stop_heading: 1.2622196382208788
         }
       }
     }
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
old mode 100755
new mode 100644
index 2298b0a5f6..7a92fd28e9
--- a/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
@@ -7,9 +7,9 @@ trajectory_point {
   path_point {
     x: 587503.356047
     y: 4140719.15699
-    z: 0
     theta: 2.35595493774
-    kappa: 4.6545264168434369e-19
+    kappa: 2.2465409107588389e-19
+    s: 5.6378512969246231e-18
     dkappa: 0
     ddkappa: 0
   }
@@ -19,2242 +19,2242 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587503.16398135643
-    y: 4140719.3492156197
-    theta: 2.3557145679957241
-    kappa: -0.000821403238142752
-    s: 0.27173498174081689
+    x: 587503.16395525972
+    y: 4140719.349189491
+    theta: 2.3559396440984339
+    kappa: -6.8754267356828733e-05
+    s: 0.2717349463597693
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4472408993819483
-  a: 0.50417684681944319
+  v: 5.4472387925364707
+  a: 0.50409385439028154
   relative_time: 0.05
 }
 trajectory_point {
   path_point {
-    x: 587502.97102485993
-    y: 4140719.5423328346
-    theta: 2.3554730833493935
-    kappa: -0.0016466163688333283
-    s: 0.54473034453912539
+    x: 587502.97097269073
+    y: 4140719.5422803103
+    theta: 2.3559242795323589
+    kappa: -0.00013782738373280696
+    s: 0.54473006789769485
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4726349373685554
-  a: 0.51153582499923345
+  v: 5.472626766264634
+  a: 0.51137752848061391
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 587502.77731923037
-    y: 4140719.736509115
-    theta: 2.3548279154278147
-    kappa: -0.0023861293023645052
-    s: 0.819004485840375
+    x: 587502.77709148568
+    y: 4140719.7362803989
+    theta: 2.3558570783479866
+    kappa: -0.00025333786263326491
+    s: 0.81900357379900235
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4983895971173107
-  a: 0.51860171730537075
+  v: 5.4983717887589663
+  a: 0.51837580503699709
   relative_time: 0.15000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587502.58271665883
-    y: 4140719.9316218672
-    theta: 2.3541311257067838
-    kappa: -0.0031183307355851183
-    s: 1.094575070375331
+    x: 587502.58229502314
+    y: 4140719.9311978803
+    theta: 2.3557815147289851
+    kappa: -0.00037656082214036708
+    s: 1.0945729597550826
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.5244902243345315
-  a: 0.52537452373785509
+  v: 5.5244595901427678
+  a: 0.5250886840594311
   relative_time: 0.2
 }
 trajectory_point {
   path_point {
-    x: 587502.38746876572
-    y: 4140720.1279454483
-    theta: 2.3531114474061958
-    kappa: -0.0037739414463032438
-    s: 1.3714590301600744
+    x: 587502.3866135804
+    y: 4140720.1270867269
+    theta: 2.35563710386677
+    kappa: -0.00056284580819388049
+    s: 1.371455007963492
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.5509221647265354
-  a: 0.53185424429668648
+  v: 5.5508759005393431
+  a: 0.53151616554791592
   relative_time: 0.25
 }
 trajectory_point {
   path_point {
-    x: 587502.1913526397
-    y: 4140720.3252807874
-    theta: 2.3520083481783747
-    kappa: -0.0044130230179105917
-    s: 1.6496725644960026
+    x: 587502.19000750012
+    y: 4140720.323930033
+    theta: 2.3554689934398914
+    kappa: -0.00077101164951360972
+    s: 1.6496657871279514
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.577670763999639
-  a: 0.538040878981865
+  v: 5.5776064500719942
+  a: 0.53765824950245167
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 587501.99472008692
-    y: 4140720.5239979257
-    theta: 2.3506587537499284
-    kappa: -0.0049856709505000808
-    s: 1.9292311399698285
+    x: 587501.99251469155
+    y: 4140720.5217873277
+    theta: 2.3552009488118655
+    kappa: -0.0010448666375790364
+    s: 1.9292206524583475
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.6047213678601606
-  a: 0.54393442779339063
+  v: 5.604636968864023
+  a: 0.54351493592303823
   relative_time: 0.35
 }
 trajectory_point {
   path_point {
-    x: 587501.7972900616
-    y: 4140720.7238391237
-    theta: 2.3492138007072554
-    kappa: -0.0055355066697939668
-    s: 2.2101494904535808
+    x: 587501.79409600981
+    y: 4140720.7206402216
+    theta: 2.3548781914251293
+    kappa: -0.0013549060615322516
+    s: 2.2101342456707318
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.6320593220144177
-  a: 0.54953489073126327
+  v: 5.6319531870387323
+  a: 0.54908622480967562
   relative_time: 0.39999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587501.59933037846
-    y: 4140720.9250868084
-    theta: 2.3475616861620376
-    kappa: -0.0060266514652338255
-    s: 2.4924416171046042
+    x: 587501.59480462223
+    y: 4140720.9205613448
+    theta: 2.354434360250103
+    kappa: -0.0017163321125824996
+    s: 2.49242049498732
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.659669972168726
-  a: 0.554842267795483
+  v: 5.659540834719424
+  a: 0.55437211616236381
   relative_time: 0.44999999999999996
 }
 trajectory_point {
   path_point {
-    x: 587501.400606487
-    y: 4140721.1275286209
-    theta: 2.3458057416591096
-    kappa: -0.0064908504231492694
-    s: 2.7761207883655592
+    x: 587501.39460783324
+    y: 4140721.1215367597
+    theta: 2.3538982653484681
+    kappa: -0.0021171291896201969
+    s: 2.7760926151364953
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.6875386640294048
-  a: 0.55985655898604969
+  v: 5.687385642029402
+  a: 0.55937260998110294
   relative_time: 0.49999999999999994
 }
 trajectory_point {
   path_point {
-    x: 587501.20139220753
-    y: 4140721.3314484619
-    theta: 2.3438816970836829
-    kappa: -0.0069053979808416334
-    s: 3.0611995399644227
+    x: 587501.19357031677
+    y: 4140721.3236482679
+    theta: 2.3532278032861043
+    kappa: -0.0025589095932782129
+    s: 3.0611631073528036
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.7156507433027706
-  a: 0.56457776430296358
+  v: 5.715473339091969
+  a: 0.56408770626589289
   relative_time: 0.54999999999999993
 }
 trajectory_point {
   path_point {
-    x: 587501.00146804517
-    y: 4140721.5366481859
-    theta: 2.3418581963123177
-    kappa: -0.00729270188927485
-    s: 3.3476896749144864
+    x: 587500.99167866888
+    y: 4140721.5268991636
+    theta: 2.3524275346741237
+    kappa: -0.0030403327432515334
+    s: 3.347643759376957
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.74399155569514
-  a: 0.56900588374622452
+  v: 5.743789656030426
+  a: 0.56851740501673365
   relative_time: 0.6
 }
 trajectory_point {
   path_point {
-    x: 587500.80109367415
-    y: 4140721.7433930072
-    theta: 2.3396963123837193
-    kappa: -0.0076360620408088206
-    s: 3.6356022635143592
+    x: 587500.78896809113
+    y: 4140721.7313394812
+    theta: 2.3514818651769556
+    kappa: -0.0035507528731619723
+    s: 3.6355456454558333
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.7725464469128323
-  a: 0.5731409173158325
+  v: 5.772320322968076
+  a: 0.57266170623362533
   relative_time: 0.65
 }
 trajectory_point {
   path_point {
-    x: 587500.60005513334
-    y: 4140721.9514897494
-    theta: 2.3374448794193872
-    kappa: -0.0079529743356599476
-    s: 3.9249476433479642
+    x: 587500.58545535465
+    y: 4140721.9370004386
+    theta: 2.3503721255044185
+    kappa: -0.0040936941767015112
+    s: 3.9248791263424732
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.8013007626621631
-  a: 0.57698286501178753
+  v: 5.8010510700282225
+  a: 0.57652060991656784
   relative_time: 0.70000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587500.398623716
-    y: 4140722.1612097379
-    theta: 2.335081336503408
-    kappa: -0.008228387197423026
-    s: 4.2157354192845418
+    x: 587500.38116977073
+    y: 4140722.1439236142
+    theta: 2.3491068907320676
+    kappa: -0.0046582506214053942
+    s: 4.2156538492960864
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.83023984864945
-  a: 0.58053172683408971
+  v: 5.8299676273341676
+  a: 0.58009411606556116
   relative_time: 0.75000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587500.196564979
-    y: 4140722.372339854
-    theta: 2.3326434021431863
-    kappa: -0.0084784501085384077
-    s: 4.5079744634786483
+    x: 587500.17615909851
+    y: 4140722.3521686997
+    theta: 2.3476524809739896
+    kappa: -0.0052486813208023324
+    s: 4.5078787480820424
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.8593490505810113
-  a: 0.58378750278273883
+  v: 5.8590557250092132
+  a: 0.5833822246806053
   relative_time: 0.80000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587499.99418618751
-    y: 4140722.585180359
-    theta: 2.3301173601047465
-    kappa: -0.0086882884413081347
-    s: 4.8016729153701538
+    x: 587499.970436427
+    y: 4140722.5617583711
+    theta: 2.3460297906610803
+    kappa: -0.0058532596851201546
+    s: 4.8015620429718817
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.8886137141631627
-  a: 0.58675019285773511
+  v: 5.8883010931766622
+  a: 0.58638493576170037
   relative_time: 0.8500000000000002
 }
 trajectory_point {
   path_point {
-    x: 587499.79119803652
-    y: 4140722.7994660214
-    theta: 2.3275344563570219
-    kappa: -0.00887501448495152
-    s: 5.0968381816842463
+    x: 587499.7640649993
+    y: 4140722.7727652024
+    theta: 2.3442033223370671
+    kappa: -0.0064744672176708911
+    s: 5.0967112407433044
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.9180191851022235
-  a: 0.58941979705907843
+  v: 5.9176894619598173
+  a: 0.58910224930884614
   relative_time: 0.90000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587499.58793045441
-    y: 4140723.0155130723
-    theta: 2.3248766978598163
-    kappa: -0.00902017901947539
-    s: 5.3934769364314308
+    x: 587499.557059265
+    y: 4140722.9852101025
+    theta: 2.3421948378056703
+    kappa: -0.0071015667375099118
+    s: 5.39333313468018
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.94755080910451
-  a: 0.59179631538676891
+  v: 5.9472065614819813
+  a: 0.59153416532204284
   relative_time: 0.95000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587499.38402085076
-    y: 4140723.2329879571
-    theta: 2.3221742883234073
-    kappa: -0.0091445435855803513
-    s: 5.6915951209075226
+    x: 587499.34946265514
+    y: 4140723.1991438707
+    theta: 2.3399834413598994
+    kappa: -0.007734907761236272
+    s: 5.69143380457254
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.9771939318763394
-  a: 0.59387974784080633
+  v: 5.9768381218664555
+  a: 0.59368068380129047
   relative_time: 1.0000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587499.17992040841
-    y: 4140723.452314998
-    theta: 2.3194175345938004
-    kappa: -0.0092257741086248478
-    s: 5.9911979436936615
+    x: 587499.14135994622
+    y: 4140723.4146521455
+    theta: 2.3375692711468021
+    kappa: -0.0083655033972433925
+    s: 5.9910186167165858
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.0069338991240295
-  a: 0.5956700944211909
+  v: 6.0065698732365442
+  a: 0.5955418047465888
   relative_time: 1.0500000000000003
 }
 trajectory_point {
   path_point {
-    x: 587498.97513782594
-    y: 4140723.6730420887
-    theta: 2.3166305928656139
-    kappa: -0.0092897773554292337
-    s: 6.2922898806562966
+    x: 587498.93272151961
+    y: 4140723.6317127915
+    theta: 2.3349618701624282
+    kappa: -0.0089936585243490432
+    s: 6.2920922239146782
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.036756056553898
-  a: 0.59716735512792241
+  v: 6.0363875457155487
+  a: 0.59711752815793806
   relative_time: 1.1000000000000003
 }
 trajectory_point {
   path_point {
-    x: 587498.770269456
-    y: 4140723.8957213569
-    theta: 2.3138123958225409
-    kappa: -0.0093080770156783559
-    s: 6.5948746749471931
+    x: 587498.723726694
+    y: 4140723.8504978642
+    theta: 2.3321347553373326
+    kappa: -0.009606584329977217
+    s: 6.5946585654753456
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.0666457498722615
-  a: 0.59837152996100107
+  v: 6.0662768694267726
+  a: 0.59840785403533814
   relative_time: 1.1500000000000004
 }
 trajectory_point {
   path_point {
-    x: 587498.56465237949
-    y: 4140724.1197451656
-    theta: 2.3109753047262545
-    kappa: -0.0093133925560632911
-    s: 6.8989553370034358
+    x: 587498.5141986236
+    y: 4140724.070842755
+    theta: 2.3291411340479531
+    kappa: -0.010209011475250123
+    s: 6.8987208672132807
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.0965883247854373
-  a: 0.59928261892042689
+  v: 6.0962235744935169
+  a: 0.599412782378789
   relative_time: 1.2000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587498.35910277022
-    y: 4140724.3458589064
-    theta: 2.308140760702361
-    kappa: -0.00927068547315961
-    s: 7.2045341445474236
+    x: 587498.30451833981
+    y: 4140724.2931056647
+    theta: 2.3259110927581714
+    kappa: -0.010780288671391712
+    s: 7.2042816414493442
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.1265691269997431
-  a: 0.59990062200619965
+  v: 6.1262133910390855
+  a: 0.60013231318829086
   relative_time: 1.2500000000000004
 }
 trajectory_point {
   path_point {
-    x: 587498.15271801676
-    y: 4140724.5732408254
-    theta: 2.3052949569731997
-    kappa: -0.0092200475388751636
-    s: 7.5116126425868694
+    x: 587498.09426931641
+    y: 4140724.5168953738
+    theta: 2.3225491583660776
+    kappa: -0.011336513809909474
+    s: 7.5113426870105586
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.1565735022214962
-  a: 0.60022553921831956
+  v: 6.15623204918678
+  a: 0.60056644646384338
   relative_time: 1.3000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587497.94650215539
-    y: 4140724.8027967373
-    theta: 2.3024786772024739
-    kappa: -0.0091160706496794149
-    s: 7.8201916434148027
+    x: 587497.88414719771
+    y: 4140724.7428569887
+    theta: 2.318937081878941
+    kappa: -0.011839123501592075
+    s: 7.8199050892301134
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.1865867961570142
-  a: 0.60025737055678652
+  v: 6.1862652790599038
+  a: 0.600715182205447
   relative_time: 1.3500000000000005
 }
 trajectory_point {
   path_point {
-    x: 587497.7394236722
-    y: 4140725.0335946255
-    theta: 2.2996562948226593
-    kappa: -0.009005304312960127
-    s: 8.1302712266095725
+    x: 587497.67337445053
+    y: 4140724.970265463
+    theta: 2.3152321315013484
+    kappa: -0.012326033273375642
+    s: 8.1299692199473625
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.2165943545126137
-  a: 0.59999611602160052
+  v: 6.2162988107817592
+  a: 0.60057852041310122
   relative_time: 1.4000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587497.5324605098
-    y: 4140725.2665067157
-    theta: 2.2968907549274382
-    kappa: -0.0088440852665881871
-    s: 8.44185073903484
+    x: 587497.46301421721
+    y: 4140725.200093885
+    theta: 2.311285686133874
+    kappa: -0.012730738433330044
+    s: 8.4415347375078227
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.2465815229946138
-  a: 0.59944177561276157
+  v: 6.2463183744756483
+  a: 0.60015646108680631
   relative_time: 1.4500000000000006
 }
 trajectory_point {
   path_point {
-    x: 587497.32474839746
-    y: 4140725.5007568737
-    theta: 2.2941344761781908
-    kappa: -0.0086711503509348566
-    s: 8.7549287948395786
+    x: 587497.25186565064
+    y: 4140725.4312355011
+    theta: 2.30728607064705
+    kappa: -0.013123954465061644
+    s: 8.754600586763182
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.2765336473093294
-  a: 0.59859434933026967
+  v: 6.276309700264874
+  a: 0.59944900422656233
   relative_time: 1.5000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587497.11698408
-    y: 4140725.7369582546
-    theta: 2.2914511154491062
-    kappa: -0.00845563185405313
-    s: 9.06950327545809
+    x: 587497.04146681493
+    y: 4140725.6650796789
+    theta: 2.3030723926178442
+    kappa: -0.013398723273209907
+    s: 9.0691649990712886
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.30643607316308
-  a: 0.59745383717412492
+  v: 6.3062585182727391
+  a: 0.59845614983236917
   relative_time: 1.5500000000000007
 }
 trajectory_point {
   path_point {
-    x: 587496.90864790545
-    y: 4140725.9746439783
-    theta: 2.2888059959106544
-    kappa: -0.00822108407364193
-    s: 9.3855713296099772
+    x: 587496.83037575218
+    y: 4140725.900311328
+    theta: 2.2988190467124263
+    kappa: -0.013650532918980879
+    s: 9.385225492296156
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.3362741462621806
-  a: 0.59602023914432722
+  v: 6.3361505586225455
+  a: 0.59717789790422682
   relative_time: 1.6000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587496.70008409151
-    y: 4140726.2141105663
-    theta: 2.286241167562554
-    kappa: -0.0079526625935562832
-    s: 9.7031293733001682
+    x: 587496.61986434681
+    y: 4140726.1380613996
+    theta: 2.2944452064417109
+    kappa: -0.013779484840626223
+    s: 9.7027788708079648
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.3660332123129511
-  a: 0.59429355524087657
+  v: 6.3659715514375961
+  a: 0.5956142484421354
   relative_time: 1.6500000000000008
 }
 trajectory_point {
   path_point {
-    x: 587496.49111848371
-    y: 4140726.4551950563
-    theta: 2.2837541665540186
-    kappa: -0.0076571925330050482
-    s: 10.022173089818905
+    x: 587496.40900450072
+    y: 4140726.377487184
+    theta: 2.2900422232195696
+    kappa: -0.01385153303986221
+    s: 10.021821225483059
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.3956986170217078
-  a: 0.592273785463773
+  v: 6.3957072268411927
+  a: 0.59376520144609479
   relative_time: 1.7000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587496.28171916306
-    y: 4140726.697863095
-    theta: 2.2813395080314662
-    kappa: -0.0073362615822070163
-    s: 10.342697429741746
+    x: 587496.19844514458
+    y: 4140726.6191525068
+    theta: 2.2856013486251374
+    kappa: -0.013808515312083526
+    s: 10.342347933703948
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.4252557060947684
-  a: 0.58996092981301651
+  v: 6.4253433149566392
+  a: 0.591630756916105
   relative_time: 1.7500000000000009
 }
 trajectory_point {
   path_point {
-    x: 587496.072067391
-    y: 4140726.9422587217
-    theta: 2.2790538386797881
-    kappa: -0.0069794984192694948
-    s: 10.66469661092956
+    x: 587495.98793198518
+    y: 4140726.8628125465
+    theta: 2.2811829887998751
+    kappa: -0.013660232215200647
+    s: 10.664353659359309
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.4546898252384484
-  a: 0.587354988288607
+  v: 6.4548655459072375
+  a: 0.589210914852166
   relative_time: 1.8000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587495.86177225877
-    y: 4140727.1880374551
-    theta: 2.2768193439339548
-    kappa: -0.0066060021681012063
-    s: 10.988164118528541
+    x: 587495.777406351
+    y: 4140727.108408602
+    theta: 2.2767845210021784
+    kappa: -0.013413151547248682
+    s: 10.987832352843981
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.483986320159068
-  a: 0.58445596089054463
+  v: 6.48425964981629
+  a: 0.586505675254278
   relative_time: 1.850000000000001
 }
 trajectory_point {
   path_point {
-    x: 587495.65136078128
-    y: 4140727.4356368175
-    theta: 2.2747808439632089
-    kappa: -0.0061879408614988968
-    s: 11.313092704970188
+    x: 587495.56730326032
+    y: 4140727.3562887944
+    theta: 2.2725173513961905
+    kappa: -0.01300008833881991
+    s: 11.312777251058968
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5131305365629419
-  a: 0.58126384761882932
+  v: 6.5135113568071
+  a: 0.58351503812244065
   relative_time: 1.900000000000001
 }
 trajectory_point {
   path_point {
-    x: 587495.44019401947
-    y: 4140727.6845012349
-    theta: 2.272782768354829
-    kappa: -0.0057645744418238845
-    s: 11.639474389971328
+    x: 587495.3568022924
+    y: 4140727.6057455447
+    theta: 2.2682908991444424
+    kappa: -0.012519872666882783
+    s: 11.639180877411443
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5421078201563905
-  a: 0.577778648473461
+  v: 6.5426063970029684
+  a: 0.58023900345665425
   relative_time: 1.9500000000000011
 }
 trajectory_point {
   path_point {
-    x: 587495.22886653873
-    y: 4140727.9351217216
-    theta: 2.2710025956593105
-    kappa: -0.0053592099984555
-    s: 11.967300460534092
+    x: 587495.147047298
+    y: 4140727.8577194139
+    theta: 2.26437017233466
+    kappa: -0.01181028063175267
+    s: 11.967035041814738
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5709035166457284
-  a: 0.5740003634544405
+  v: 6.5715305005271993
+  a: 0.57667757125691854
   relative_time: 2.0000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587495.0168526934
-    y: 4140728.1870399294
-    theta: 2.2692824224940336
-    kappa: -0.0049763805696442179
-    s: 12.296561553040204
+    x: 587494.93675645324
+    y: 4140728.1111221025
+    theta: 2.2605356026631558
+    kappa: -0.011038303624923326
+    s: 12.296330919042063
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5995083385550677
-  a: 0.57017895820961728
+  v: 6.6002745216987675
+  a: 0.57306954720573389
   relative_time: 2.0500000000000007
 }
 trajectory_point {
   path_point {
-    x: 587494.80444615392
-    y: 4140728.4404901974
-    theta: 2.2677039458108288
-    kappa: -0.0046453763099196417
-    s: 12.627248075998454
+    x: 587494.726965535
+    y: 4140728.3667959841
+    theta: 2.2571149626541418
+    kappa: -0.010033317045485927
+    s: 12.627059452976903
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.6279200569957366
-  a: 0.5662762247076375
+  v: 6.6288360824077621
+  a: 0.56937915275394835
   relative_time: 2.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587494.59149289143
-    y: 4140728.6953285616
-    theta: 2.2662131398790417
-    kappa: -0.0043509061044585831
-    s: 12.959350272575083
+    x: 587494.5168247628
+    y: 4140728.6240215553
+    theta: 2.2539287433974908
+    kappa: -0.0089892049157054357
+    s: 12.959211417633128
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.656134605554878
-  a: 0.56229216294850093
+  v: 6.6572110641341524
+  a: 0.56560638790156192
   relative_time: 2.1500000000000004
 }
 trajectory_point {
   path_point {
-    x: 587494.37794033077
-    y: 4140728.9514980945
-    theta: 2.264794692281586
-    kappa: -0.0040868074871817841
-    s: 13.2928581826157
+    x: 587494.30664940388
+    y: 4140728.8830438913
+    theta: 2.2511163432755636
+    kappa: -0.0078865352060250426
+    s: 13.292777381098606
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.6841479178196339
-  a: 0.55822677293220768
+  v: 6.6853953483579085
+  a: 0.56175125264857473
   relative_time: 2.2
 }
 trajectory_point {
   path_point {
-    x: 587494.1639707638
-    y: 4140729.2091359245
-    theta: 2.2634779776573306
-    kappa: -0.0038705865923700051
-    s: 13.627761642645257
+    x: 587494.09631662921
+    y: 4140729.1437443914
+    theta: 2.2486774459094181
+    kappa: -0.0068701005050347923
+    s: 13.627747705535205
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.7119559273771463
-  a: 0.55408005465875787
+  v: 6.7133848165589995
+  a: 0.55781374699498687
   relative_time: 2.25
 }
 trajectory_point {
   path_point {
-    x: 587493.94926985633
-    y: 4140729.4679670404
-    theta: 2.2621893689471095
-    kappa: -0.003669086965141068
-    s: 13.964050285868076
+    x: 587493.88551844214
+    y: 4140729.4058616003
+    theta: 2.24644569089981
+    kappa: -0.0059006818927713782
+    s: 13.964112547178791
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.7395545678145572
-  a: 0.54985200812815127
+  v: 6.7411753502173966
+  a: 0.553793870940798
   relative_time: 2.3
 }
 trajectory_point {
   path_point {
-    x: 587493.73422441736
-    y: 4140729.7282974725
-    theta: 2.2609940120753627
-    kappa: -0.0035182210886292569
-    s: 14.301713542167828
+    x: 587493.674668965
+    y: 4140729.6697118813
+    theta: 2.2446845340980013
+    kappa: -0.0050542362424328777
+    s: 14.301861856339229
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.7669397727190086
-  a: 0.54554263334038811
+  v: 6.7687628308130687
+  a: 0.5496916244860085
   relative_time: 2.3499999999999996
 }
 trajectory_point {
   path_point {
-    x: 587493.518482635
-    y: 4140729.9898213716
-    theta: 2.2598181674520572
-    kappa: -0.0033815779074614469
-    s: 14.640740638107545
+    x: 587493.46316774248
+    y: 4140729.9348003394
+    theta: 2.2430459920728505
+    kappa: -0.0042406714356121927
+    s: 14.640985377400384
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.7941074756776434
-  a: 0.54115193029546815
+  v: 6.7961431398259871
+  a: 0.5455070076306181
   relative_time: 2.3999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587493.302245375
-    y: 4140730.2526900908
-    theta: 2.2586892711212552
-    kappa: -0.0032758577394814657
-    s: 14.981120596929612
+    x: 587493.25138132949
+    y: 4140730.2014050977
+    theta: 2.2417759612054504
+    kappa: -0.0035329372973391575
+    s: 14.981472648820112
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8210536102776027
-  a: 0.53667989899339164
+  v: 6.8233121587361207
+  a: 0.54124002037462693
   relative_time: 2.4499999999999993
 }
 trajectory_point {
   path_point {
-    x: 587493.08545360551
-    y: 4140730.5168395024
-    theta: 2.2575861232967647
-    kappa: -0.0031925490939649637
-    s: 15.322842238555776
+    x: 587493.03897427057
+    y: 4140730.4692445481
+    theta: 2.2406701229042354
+    kappa: -0.0028766663653917238
+    s: 15.323313003130279
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8477741101060294
-  a: 0.53212653943415833
+  v: 6.85026576902344
+  a: 0.536890662718035
   relative_time: 2.4999999999999991
 }
 trajectory_point {
   path_point {
-    x: 587492.86801447684
-    y: 4140730.7821786432
-    theta: 2.2564986136869587
-    kappa: -0.0031239845932064208
-    s: 15.66589417958714
+    x: 587492.82600636268
+    y: 4140730.7383522261
+    theta: 2.2397691323595836
+    kappa: -0.0022849067661344508
+    s: 15.666495566936741
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8742649087500656
-  a: 0.52749185161776846
+  v: 6.8769998521679137
+  a: 0.53245893466084215
   relative_time: 2.5499999999999989
 }
 trajectory_point {
   path_point {
-    x: 587492.65017021657
-    y: 4140731.0488900784
-    theta: 2.255434393806893
-    kappa: -0.003081821945139381
-    s: 16.010264833304159
+    x: 587492.61246659665
+    y: 4140731.0087046195
+    theta: 2.2390999144718831
+    kappa: -0.001774906867019655
+    s: 16.011009260919352
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9005219397968531
-  a: 0.52277583554422191
+  v: 6.903510289649514
+  a: 0.52794483620304855
   relative_time: 2.5999999999999988
 }
 trajectory_point {
   path_point {
-    x: 587492.43164522608
-    y: 4140731.3167328574
-    theta: 2.2543725796296767
-    kappa: -0.0030468937885756585
-    s: 16.355942409666653
+    x: 587492.39820726716
+    y: 4140731.2801705911
+    theta: 2.2385215094746
+    kappa: -0.0012967820428537815
+    s: 16.356842799831973
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.926541136833535
-  a: 0.51797849121351869
+  v: 6.9297929629482091
+  a: 0.52334836734465418
   relative_time: 2.6499999999999986
 }
 trajectory_point {
   path_point {
-    x: 587492.21268545708
-    y: 4140731.5858918047
-    theta: 2.2533198034641257
-    kappa: -0.0030290216957631769
-    s: 16.702914915313794
+    x: 587492.18333965284
+    y: 4140731.5528231207
+    theta: 2.2381832781151538
+    kappa: -0.00091517046096401184
+    s: 16.703984692502456
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9523184334472523
-  a: 0.51309981862565879
+  v: 6.9558437535439692
+  a: 0.51866952808565892
   relative_time: 2.6999999999999984
 }
 trajectory_point {
   path_point {
-    x: 587491.99316906114
-    y: 4140731.8562511271
-    theta: 2.2522669495288903
-    kappa: -0.0030177297176342188
-    s: 17.051170153564115
+    x: 587491.96770761814
+    y: 4140731.8265241324
+    theta: 2.23791222775423
+    kappa: -0.00056427138992645948
+    s: 17.052423241832653
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9778497632251479
-  a: 0.50813981778064221
+  v: 6.9816585429167644
+  a: 0.513908318426063
   relative_time: 2.7499999999999982
 }
 trajectory_point {
   path_point {
-    x: 587491.77309034893
-    y: 4140732.1277902317
-    theta: 2.2512138509952488
-    kappa: -0.0030126632711428569
-    s: 17.400695724415495
+    x: 587491.75135182019
+    y: 4140732.1012905012
+    theta: 2.2377680779677345
+    kappa: -0.000272133682972333
+    s: 17.402146544798413
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0031310597543639
-  a: 0.503098488678469
+  v: 7.0072332125465646
+  a: 0.50906473836586619
   relative_time: 2.799999999999998
 }
 trajectory_point {
   path_point {
-    x: 587491.55261857854
-    y: 4140732.4006291442
-    theta: 2.25015756953145
-    kappa: -0.00301134322013941
-    s: 17.751479024545187
+    x: 587491.53422889113
+    y: 4140732.3770726407
+    theta: 2.2377233893604158
+    kappa: -3.5310697082980042e-05
+    s: 17.753142492449591
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0281582566220422
-  a: 0.49797583131913903
+  v: 7.032563643913341
+  a: 0.5041387879050685
   relative_time: 2.8499999999999979
 }
 trajectory_point {
   path_point {
-    x: 587491.33152400621
-    y: 4140732.674565278
-    theta: 2.249097742936164
-    kappa: -0.00301002924947725
-    s: 18.103507247309793
+    x: 587491.31633631489
+    y: 4140732.6538528972
+    theta: 2.2377279760217736
+    kappa: 0.00017448789547944943
+    s: 18.105398769910028
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0529272874153248
-  a: 0.49277184570265242
+  v: 7.0576457184970618
+  a: 0.49913046704367015
   relative_time: 2.8999999999999977
 }
 trajectory_point {
   path_point {
-    x: 587491.11005098
-    y: 4140732.9497786891
-    theta: 2.2480346232009696
-    kappa: -0.003005365509648061
-    s: 18.456767382745262
+    x: 587491.09764512884
+    y: 4140732.931592531
+    theta: 2.2378334662041852
+    kappa: 0.00031212416621304994
+    s: 18.458902856377577
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0774340857213547
-  a: 0.4874865318290092
+  v: 7.082475317777698
+  a: 0.49403977578167091
   relative_time: 2.9499999999999975
 }
 trajectory_point {
   path_point {
-    x: 587490.88806575921
-    y: 4140733.2261438631
-    theta: 2.2469708942001887
-    kappa: -0.0029940400728621524
-    s: 18.811246217566922
+    x: 587490.87816833879
+    y: 4140733.2102855793
+    theta: 2.2379603015359191
+    kappa: 0.00042871343196467394
+    s: 18.813642025124075
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.1016745851272729
-  a: 0.48211988969820929
+  v: 7.1070483232352188
+  a: 0.48886671411907084
   relative_time: 2.9999999999999973
 }
 trajectory_point {
   path_point {
-    x: 587490.66555958556
-    y: 4140733.5036365557
-    theta: 2.2459063872615057
-    kappa: -0.0029765061666342965
-    s: 19.16693033516944
+    x: 587490.65789347258
+    y: 4140733.489905756
+    theta: 2.2381283943014441
+    kappa: 0.00050380028364034959
+    s: 19.169603343495375
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.1256447192202232
-  a: 0.47667191931025266
+  v: 7.1313606163495944
+  a: 0.48361128205587
   relative_time: 3.0499999999999972
 }
 trajectory_point {
   path_point {
-    x: 587490.44274009194
-    y: 4140733.7824048307
-    theta: 2.2448540739839804
-    kappa: -0.0029396486355112716
-    s: 19.523806115626844
+    x: 587490.43680987088
+    y: 4140733.7704279171
+    theta: 2.238316453726759
+    kappa: 0.00054110041566424107
+    s: 19.526773672911315
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.149340421587346
-  a: 0.4711426206651394
+  v: 7.1554080786007956
+  a: 0.4782734795920684
   relative_time: 3.099999999999997
 }
 trajectory_point {
   path_point {
-    x: 587490.219359108
-    y: 4140734.062232038
-    theta: 2.2438078598742415
-    kappa: -0.0028927279633793219
-    s: 19.881859735692526
+    x: 587490.21495448577
+    y: 4140734.0518641877
+    theta: 2.2385138358866281
+    kappa: 0.00055921671311204356
+    s: 19.885139668865733
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.1727576258157839
-  a: 0.46553199376286947
+  v: 7.1791865914687909
+  a: 0.47285330672766596
   relative_time: 3.1499999999999968
 }
 trajectory_point {
   path_point {
-    x: 587489.99561360362
-    y: 4140734.3432571492
-    theta: 2.2427819946249583
-    kappa: -0.0028223780127561004
-    s: 20.241077168799229
+    x: 587489.99228067382
+    y: 4140734.3341605188
+    theta: 2.238710121916188
+    kappa: 0.00053492587151369856
+    s: 20.244687780926469
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.19589226549268
-  a: 0.45984003860344291
+  v: 7.202692036433552
+  a: 0.46735076346266269
   relative_time: 3.1999999999999966
 }
 trajectory_point {
   path_point {
-    x: 587489.77142859111
-    y: 4140734.6254014126
-    theta: 2.2417839096236007
-    kappa: -0.002728080423144901
-    s: 20.601444185059052
+    x: 587489.7688481994
+    y: 4140734.6173464991
+    theta: 2.2388967404132614
+    kappa: 0.00049600431908985087
+    s: 20.605404252735362
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.2187402742051754
-  a: 0.45406675518685963
+  v: 7.2259202949750474
+  a: 0.46176584979705865
   relative_time: 3.2499999999999964
 }
 trajectory_point {
   path_point {
-    x: 587489.54671817773
-    y: 4140734.9085781178
-    theta: 2.240802987415083
-    kappa: -0.0026179776239385597
-    s: 20.962946351263454
+    x: 587489.54465095163
+    y: 4140734.9013995221
+    theta: 2.2390693482251955
+    kappa: 0.00043633097485908768
+    s: 20.96727512200825
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.2412975855404129
-  a: 0.44821214351311967
+  v: 7.2488672485732479
+  a: 0.45609856573085383
   relative_time: 3.2999999999999963
 }
 trajectory_point {
   path_point {
-    x: 587489.32173836627
-    y: 4140735.1929709576
-    theta: 2.239895654836368
-    kappa: -0.0024612261894075673
-    s: 21.325569030883255
+    x: 587489.31969001959
+    y: 4140735.1863023746
+    theta: 2.2392078102177235
+    kappa: 0.00035578547627652571
+    s: 21.330286220534965
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.2635601330855346
-  a: 0.44227620358222308
+  v: 7.271528778708122
+  a: 0.45034891126404814
   relative_time: 3.3499999999999961
 }
 trajectory_point {
   path_point {
-    x: 587489.09622835554
-    y: 4140735.4783540145
-    theta: 2.23902605811623
-    kappa: -0.0022914752688129748
-    s: 21.689297384068624
+    x: 587489.094008037
+    y: 4140735.4720703983
+    theta: 2.2393277942404315
+    kappa: 0.00026676418220150791
+    s: 21.694423174179338
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.2855238504276825
-  a: 0.43625893539416982
+  v: 7.2939007668596423
+  a: 0.44451688639664166
   relative_time: 3.3999999999999959
 }
 trajectory_point {
   path_point {
-    x: 587488.870295307
-    y: 4140735.7647927483
-    theta: 2.2382206311899995
-    kappa: -0.0021012034053137525
-    s: 22.05411636764909
+    x: 587488.86759882956
+    y: 4140735.7586801318
+    theta: 2.2394021438170757
+    kappa: 0.00016456407043223016
+    s: 22.059671402879204
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.3071846711539994
-  a: 0.43016033894895989
+  v: 7.3159790945077763
+  a: 0.43860249112863442
   relative_time: 3.4499999999999957
 }
 trajectory_point {
   path_point {
-    x: 587488.643958014
-    y: 4140736.0522820242
-    theta: 2.2375034024525742
-    kappa: -0.0018889794217148094
-    s: 22.420010735133545
+    x: 587488.64049883746
+    y: 4140736.0461415132
+    theta: 2.239441714433823
+    kappa: 5.8919219074364711e-05
+    s: 22.426016120646388
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.3285385288516256
-  a: 0.42398041424659327
+  v: 7.337759643132495
+  a: 0.43260572546002635
   relative_time: 3.4999999999999956
 }
 trajectory_point {
   path_point {
-    x: 587488.41709326592
-    y: 4140736.340705025
-    theta: 2.236834791985733
-    kappa: -0.0016639862187926881
-    s: 22.786965036710225
+    x: 587488.41271988791
+    y: 4140736.33444478
+    theta: 2.2394544983000864
+    kappa: -4.9449404727973873e-05
+    s: 22.793442335566724
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.3495813571077058
-  a: 0.41771916128707
+  v: 7.3592382942137693
+  a: 0.42652658939081745
   relative_time: 3.5499999999999954
 }
 trajectory_point {
   path_point {
-    x: 587488.18982179812
-    y: 4140736.6301365998
-    theta: 2.236278761071874
-    kappa: -0.0014104043605935638
-    s: 23.154963619246736
+    x: 587488.18428768672
+    y: 4140736.6235908717
+    theta: 2.2394057234546914
+    kappa: -0.00015344837544995757
+    s: 23.161934849800041
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.37030908950938
-  a: 0.41137658007039
+  v: 7.3804109292315667
+  a: 0.42036508292100783
   relative_time: 3.5999999999999952
 }
 trajectory_point {
   path_point {
-    x: 587487.962042793
-    y: 4140736.9204771537
-    theta: 2.2358078711083418
-    kappa: -0.0011586703650970759
-    s: 23.523990626290036
+    x: 587487.95521831617
+    y: 4140736.9135728613
+    theta: 2.2393325858012725
+    kappa: -0.00025561559915440279
+    s: 23.531478259580155
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.3907176596437916
-  a: 0.40495267059655338
+  v: 7.40127342966586
+  a: 0.41412120605059732
   relative_time: 3.649999999999995
 }
 trajectory_point {
   path_point {
-    x: 587487.733733464
-    y: 4140737.2116883951
-    theta: 2.2353999265850413
-    kappa: -0.00090814006396254217
-    s: 23.894029998066443
+    x: 587487.72553078306
+    y: 4140737.204385872
+    theta: 2.239222173729932
+    kappa: -0.00035476843896895032
+    s: 23.902056955214896
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.410803001098083
-  a: 0.39844743286556006
+  v: 7.4218216769966165
+  a: 0.40779495877958605
   relative_time: 3.6999999999999948
 }
 trajectory_point {
   path_point {
-    x: 587487.50496602734
-    y: 4140737.5038061487
-    theta: 2.2351288018989677
-    kappa: -0.00069194674907874113
-    s: 24.265065471481627
+    x: 587487.49526187335
+    y: 4140737.4960387908
+    theta: 2.239067455761643
+    kappa: -0.00044560112289417965
+    s: 24.273655121086087
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.4305610474593955
-  a: 0.39186086687741012
+  v: 7.4420515527038082
+  a: 0.40138634110797394
   relative_time: 3.7499999999999947
 }
 trajectory_point {
   path_point {
-    x: 587487.27563593315
-    y: 4140737.7967275465
-    theta: 2.234906209114591
-    kappa: -0.00048902921628400306
-    s: 24.637080580120617
+    x: 587487.264403199
+    y: 4140737.7885045721
+    theta: 2.2388914296301272
+    kappa: -0.00052719437165479888
+    s: 24.646256735649548
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.4499877323148711
-  a: 0.3851929726321035
+  v: 7.4619589382674043
+  a: 0.39489535303576107
   relative_time: 3.7999999999999945
 }
 trajectory_point {
   path_point {
-    x: 587487.04576906911
-    y: 4140738.0904516834
-    theta: 2.234751445893195
-    kappa: -0.00030487355570093546
-    s: 25.0100586542478
+    x: 587487.03300248215
+    y: 4140738.0818002024
+    theta: 2.2386808436078844
+    kappa: -0.00058191664372451817
+    s: 25.0198455714351
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.4690789892516527
-  a: 0.37844375012964016
+  v: 7.4815397151673739
+  a: 0.38832199456294736
   relative_time: 3.8499999999999943
 }
 trajectory_point {
   path_point {
-    x: 587486.81535966636
-    y: 4140738.3849525983
-    theta: 2.234678841370954
-    kappa: -0.00014596817281775832
-    s: 25.383982820806921
+    x: 587486.80106636579
+    y: 4140738.3759099534
+    theta: 2.238455470244904
+    kappa: -0.00061600083872448148
+    s: 25.394405195046556
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.4878307518568823
-  a: 0.37161319937002024
+  v: 7.5007897648836881
+  a: 0.38166626568953288
   relative_time: 3.8999999999999941
 }
 trajectory_point {
   path_point {
-    x: 587486.58438754408
-    y: 4140738.6801928179
-    theta: 2.2346442011303069
-    kappa: 6.2055590183925088e-07
-    s: 25.758836003421081
+    x: 587486.568584829
+    y: 4140738.6708047255
+    theta: 2.2382210711712127
+    kappa: -0.00063638775446305212
+    s: 25.769918967161736
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.5062389537177019
-  a: 0.36470132035324365
+  v: 7.5197049688963169
+  a: 0.37492816641551752
   relative_time: 3.949999999999994
 }
 trajectory_point {
   path_point {
-    x: 587486.35285896726
-    y: 4140738.9761553025
-    theta: 2.2346688161298314
-    kappa: 0.00012688804365960246
-    s: 26.134600922392735
+    x: 587486.33562460449
+    y: 4140738.9665156417
+    theta: 2.2379815510489403
+    kappa: -0.0006316295515038313
+    s: 26.146370042532453
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.5242995284212535
-  a: 0.35770811307931027
+  v: 7.53828120868523
+  a: 0.36810769674090138
   relative_time: 3.9999999999999938
 }
 trajectory_point {
   path_point {
-    x: 587486.12075814675
-    y: 4140739.2728056666
-    theta: 2.2347422199810087
-    kappa: 0.00023430778334748226
-    s: 26.511260223899342
+    x: 587486.1021510188
+    y: 4140739.2629937916
+    theta: 2.2377463939746165
+    kappa: -0.00061542337376289512
+    s: 26.523741495366281
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.5420164369782814
-  a: 0.3509767292874974
+  v: 7.5565221518485259
+  a: 0.3615373584090858
   relative_time: 4.0499999999999936
 }
 trajectory_point {
   path_point {
-    x: 587485.88809943339
-    y: 4140739.5701336386
-    theta: 2.2348434275816862
-    kappa: 0.00033046583939846388
-    s: 26.888796977854277
+    x: 587485.868171569
+    y: 4140739.5602240451
+    theta: 2.2375153701108608
+    kappa: -0.00058832487360230669
+    s: 26.902016800756904
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.5593980513585723
-  a: 0.34429634600982434
+  v: 7.5744356773879531
+  a: 0.3550109917861522
   relative_time: 4.0999999999999934
 }
 trajectory_point {
   path_point {
-    x: 587485.654864504
-    y: 4140739.8681036704
-    theta: 2.2349875772213035
-    kappa: 0.00040663459668419008
-    s: 27.267194483299345
+    x: 587485.63374142931
+    y: 4140739.8582290281
+    theta: 2.2373041863070053
+    kappa: -0.000544976930364415
+    s: 27.281179642787766
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.5764469215878334
-  a: 0.33766696324629086
+  v: 7.5920239838889554
+  a: 0.34852859687209986
   relative_time: 4.1499999999999932
 }
 trajectory_point {
   path_point {
-    x: 587485.42106362362
-    y: 4140740.1667027338
-    theta: 2.2351551110798988
-    kappa: 0.00046961174831851988
-    s: 27.646436166777637
+    x: 587485.39881402918
+    y: 4140740.1569514507
+    theta: 2.2371054542764912
+    kappa: -0.00049603779093883348
+    s: 27.66121381547158
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.5931655976917707
-  a: 0.33108858099689681
+  v: 7.6092892699369772
+  a: 0.34209017366692884
   relative_time: 4.1999999999999931
 }
 trajectory_point {
   path_point {
-    x: 587485.18671736866
-    y: 4140740.4659260493
-    theta: 2.2353394323432414
-    kappa: 0.000522895219884335
-    s: 28.026505582333527
+    x: 587485.16341127153
+    y: 4140740.4563880591
+    theta: 2.2369235225442154
+    kappa: -0.00044011502294374866
+    s: 28.042103222750335
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.6095566296960913
-  a: 0.32456119926164234
+  v: 7.6262337341174629
+  a: 0.33569572217063914
   relative_time: 4.2499999999999929
 }
 trajectory_point {
   path_point {
-    x: 587484.95178743009
-    y: 4140740.7657228643
-    theta: 2.2355482989377777
-    kappa: 0.00055795485025205761
-    s: 28.407386411512679
+    x: 587484.92755622067
+    y: 4140740.756536623
+    theta: 2.2367720765248547
+    kappa: -0.00037802911972480244
+    s: 28.423831878495289
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.6256225676265039
-  a: 0.31808481804052735
+  v: 7.6428595750158559
+  a: 0.32934524238323071
   relative_time: 4.2999999999999927
 }
 trajectory_point {
   path_point {
-    x: 587484.71631101542
-    y: 4140741.066101728
-    theta: 2.2357661904231776
-    kappa: 0.00058350534472908686
-    s: 28.789062463362036
+    x: 587484.69122208422
+    y: 4140741.0573558854
+    theta: 2.23663754438747
+    kappa: -0.00031422111824948524
+    s: 28.806383906506973
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.6413659615087131
-  a: 0.31165943733355189
+  v: 7.6591689912176006
+  a: 0.32303873430470359
   relative_time: 4.3499999999999925
 }
 trajectory_point {
   path_point {
-    x: 587484.48030411324
-    y: 4140741.3670547656
-    theta: 2.2359921650612433
-    kappa: 0.00060058607564780341
-    s: 29.171517674429836
+    x: 587484.45442858478
+    y: 4140741.3588412874
+    theta: 2.2365280573390622
+    kappa: -0.000248904498693096
+    s: 29.189743540515195
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.6567893613684275
-  a: 0.30528505714071591
+  v: 7.6751641813081406
+  a: 0.31677619793505779
   relative_time: 4.3999999999999924
 }
 trajectory_point {
   path_point {
-    x: 587484.24372251751
-    y: 4140741.6685270146
-    theta: 2.2362236838250258
-    kappa: 0.00060252233449523455
-    s: 29.554736108765596
+    x: 587484.21717970772
+    y: 4140741.6609760411
+    theta: 2.2364484253043728
+    kappa: -0.00018545855130363938
+    s: 29.573895124179025
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.6718953172313533
-  a: 0.29896167746201946
+  v: 7.690847343872921
+  a: 0.31055763327429331
   relative_time: 4.4499999999999922
 }
 trajectory_point {
   path_point {
-    x: 587484.00663022511
-    y: 4140741.97054856
-    theta: 2.2364542855770586
-    kappa: 0.0005983227345223701
-    s: 29.93870195792012
+    x: 587483.97946646367
+    y: 4140741.9637333173
+    theta: 2.2363858543943995
+    kappa: -0.00012438162779570308
+    s: 29.958823111086812
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.6866863791231985
-  a: 0.29268929829746254
+  v: 7.7062206774973845
+  a: 0.30438304032241004
   relative_time: 4.499999999999992
 }
 trajectory_point {
   path_point {
-    x: 587483.76903025724
-    y: 4140742.2731018374
-    theta: 2.2366837377725846
-    kappa: 0.00058716824037880725
-    s: 30.323399540945491
+    x: 587483.74130115006
+    y: 4140742.2671031454
+    theta: 2.2363490622436268
+    kappa: -6.85063559704605e-05
+    s: 30.344512064756177
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7011650970696692
-  a: 0.2864679196470451
+  v: 7.7212863807669763
+  a: 0.29825241907940814
   relative_time: 4.5499999999999918
 }
 trajectory_point {
   path_point {
-    x: 587483.53089175094
-    y: 4140742.5761427917
-    theta: 2.2369043928175936
-    kappa: 0.00056579395483226152
-    s: 30.70881330439509
+    x: 587483.502683022
+    y: 4140742.5710654524
+    theta: 2.2363343089541639
+    kappa: -2.1397039873677814e-05
+    s: 30.730946658634018
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7153340210964716
-  a: 0.28029754151076725
+  v: 7.7360466522671407
+  a: 0.29216576954528756
   relative_time: 4.5999999999999917
 }
 trajectory_point {
   path_point {
-    x: 587483.29227037565
-    y: 4140742.8796955151
-    theta: 2.2371181351377305
-    kappa: 0.0005397946794756072
-    s: 31.094927822323569
+    x: 587483.26361612487
+    y: 4140742.87560407
+    theta: 2.2363318404367214
+    kappa: 1.9050724791905254e-05
+    s: 31.11811167609649
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7291957012293144
-  a: 0.27417816388862887
+  v: 7.7505036905833204
+  a: 0.28612309172004824
   relative_time: 4.6499999999999915
 }
 trajectory_point {
   path_point {
-    x: 587483.05316508934
-    y: 4140743.1837397255
-    theta: 2.2373230071239121
-    kappa: 0.00050832045101772353
-    s: 31.481727796286879
+    x: 587483.02410659054
+    y: 4140743.1807046053
+    theta: 2.2363457714517954
+    kappa: 4.69834793868222e-05
+    s: 31.505992010449035
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7427526874939039
-  a: 0.26810978678062997
+  v: 7.76465969430096
+  a: 0.28012438560369024
   relative_time: 4.6999999999999913
 }
 trajectory_point {
   path_point {
-    x: 587482.81356580474
-    y: 4140743.4882481545
-    theta: 2.2375097771036332
-    kappa: 0.00047098926440763251
-    s: 31.869198055342249
+    x: 587482.78415707848
+    y: 4140743.4863500809
+    theta: 2.2363670890386205
+    kappa: 5.9165340415288846e-05
+    s: 31.894572664926365
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7560075299159461
-  a: 0.26209241018677065
+  v: 7.7785168620055032
+  a: 0.2741696511962135
   relative_time: 4.7499999999999911
 }
 trajectory_point {
   path_point {
-    x: 587482.57352228346
-    y: 4140743.793240794
-    theta: 2.2376857684707971
-    kappa: 0.00043134703462979643
-    s: 32.257323556048192
+    x: 587482.54377921554
+    y: 4140743.7925306973
+    theta: 2.2363905287238408
+    kappa: 5.9657794478447177e-05
+    s: 32.283838752692454
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.76896277852115
-  a: 0.25612603410705076
+  v: 7.7920773922823958
+  a: 0.26825888849761809
   relative_time: 4.7999999999999909
 }
 trajectory_point {
   path_point {
-    x: 587482.33303533553
-    y: 4140744.098699294
-    theta: 2.2378479168543146
-    kappa: 0.00038910582518073071
-    s: 32.64608938246451
+    x: 587482.30297909421
+    y: 4140744.099232452
+    theta: 2.2364108774993881
+    kappa: 3.9170178624561512e-05
+    s: 32.673775496840562
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7816209833352206
-  a: 0.25021065854147045
+  v: 7.80534348371708
+  a: 0.26239209750790393
   relative_time: 4.8499999999999908
 }
 trajectory_point {
   path_point {
-    x: 587482.092101451
-    y: 4140744.4046020512
-    theta: 2.2379881721749761
-    kappa: 0.00034504365540909788
-    s: 33.035480746152288
+    x: 587482.06177028245
+    y: 4140744.4064473482
+    theta: 2.23641661974807
+    kappa: -3.7154201039108236e-06
+    s: 33.064368230393214
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.793984694383866
-  a: 0.24434628349002965
+  v: 7.818317334895001
+  a: 0.25656927822707115
   relative_time: 4.8999999999999906
 }
 trajectory_point {
   path_point {
-    x: 587481.85075847467
-    y: 4140744.7109601973
-    theta: 2.2381151291196639
-    kappa: 0.00030045097127963643
-    s: 33.4254829861739
+    x: 587481.82016557874
+    y: 4140744.7141669281
+    theta: 2.2364076460094786
+    kappa: -6.3843323227559279e-05
+    s: 33.455602396302204
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8060564616927923
-  a: 0.23853290895272833
+  v: 7.8310011444016014
+  a: 0.25079043065511963
   relative_time: 4.94999999999999
 }
 trajectory_point {
   path_point {
-    x: 587481.60900849733
-    y: 4140745.0177568747
-    theta: 2.2382249634387112
-    kappa: 0.00025540840345197446
-    s: 33.816081569093
+    x: 587481.57818076422
+    y: 4140745.0223852131
+    theta: 2.2363644503423692
+    kappa: -0.00015485044639827893
+    s: 33.847463547448605
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8178388352877075
-  a: 0.23277053492956656
+  v: 7.8433971108223268
+  a: 0.24505555479204943
   relative_time: 4.99999999999999
 }
 trajectory_point {
   path_point {
-    x: 587481.36686023837
-    y: 4140745.3249806035
-    theta: 2.2383127898290036
-    kappa: 0.00021250632014722462
-    s: 34.207262088974524
+    x: 587481.33584155724
+    y: 4140745.3311041654
+    theta: 2.2362755134557788
+    kappa: -0.00027628141931934804
+    s: 34.239937346642762
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8293343651943177
-  a: 0.22705916142054428
+  v: 7.8555074327426215
+  a: 0.2393646506378605
   relative_time: 5.04999999999999
 }
 trajectory_point {
   path_point {
-    x: 587481.12434281223
-    y: 4140745.6326361457
-    theta: 2.2383889824874479
-    kappa: 0.00017153219516155162
-    s: 34.599010267384713
+    x: 587481.0931574984
+    y: 4140745.6403131825
+    theta: 2.2361478163113189
+    kappa: -0.00042107131844249012
+    s: 34.633009566624281
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8405456014383308
-  a: 0.22139878842566152
+  v: 7.8673343087479282
+  a: 0.23371771819255288
   relative_time: 5.09999999999999
 }
 trajectory_point {
   path_point {
-    x: 587480.88146240835
-    y: 4140745.9407103616
-    theta: 2.2384511680704664
-    kappa: 0.00013322856206977228
-    s: 34.991311953391076
+    x: 587480.85016853455
+    y: 4140745.9500256651
+    theta: 2.2359460318077224
+    kappa: -0.00060562493741788184
+    s: 35.026666090062058
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.851475094045453
-  a: 0.21578941594491827
+  v: 7.8788799374236911
+  a: 0.22811475745612655
   relative_time: 5.14999999999999
 }
 trajectory_point {
   path_point {
-    x: 587480.6382276935
-    y: 4140746.249192222
-    theta: 2.2384879440835341
-    kappa: 6.4136339021546578e-05
-    s: 35.384153123562406
+    x: 587480.60691562528
+    y: 4140746.2602559063
+    theta: 2.2356495260433897
+    kappa: -0.00084867894229375893
+    s: 35.420892909554247
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8621253930413912
-  a: 0.21023104397831455
+  v: 7.8901465173553555
+  a: 0.22255576842858155
   relative_time: 5.1999999999999895
 }
 trajectory_point {
   path_point {
-    x: 587480.39466420072
-    y: 4140746.5580841638
-    theta: 2.2384963233426052
-    kappa: -3.2507693726615729e-05
-    s: 35.777519881968786
+    x: 587480.36339768209
+    y: 4140746.5709853549
+    theta: 2.2352794180362467
+    kappa: -0.0011072119375137965
+    s: 35.815676127628279
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8724990484518536
-  a: 0.20472367252585033
+  v: 7.9011362471283642
+  a: 0.21704075110991783
   relative_time: 5.2499999999999893
 }
 trajectory_point {
   path_point {
-    x: 587480.15078259434
-    y: 4140746.8673770614
-    theta: 2.2384663752377456
-    kappa: -0.00016511492069376133
-    s: 36.171398460181592
+    x: 587480.11969270452
+    y: 4140746.8822575752
+    theta: 2.2347874942968495
+    kappa: -0.001337611349320023
+    s: 36.211001956740859
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8825986103025452
-  a: 0.19926730158752565
+  v: 7.9118513253281613
+  a: 0.21156970550013543
   relative_time: 5.2999999999999892
 }
 trajectory_point {
   path_point {
-    x: 587479.90662310715
-    y: 4140747.1770852646
-    theta: 2.2383523004607313
-    kappa: -0.00034661999525718368
-    s: 36.565775217273469
+    x: 587479.87584776746
+    y: 4140747.1940918583
+    theta: 2.2342264541258379
+    kappa: -0.0014616441799887297
+    s: 36.606856719277964
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8924266286191749
-  a: 0.19386193116334044
+  v: 7.9222939505401921
+  a: 0.20614263159923432
   relative_time: 5.349999999999989
 }
 trajectory_point {
   path_point {
-    x: 587479.66220141866
-    y: 4140747.4872039175
-    theta: 2.2381778246535187
-    kappa: -0.0005538410226900675
-    s: 36.960636639818368
+    x: 587479.63182310387
+    y: 4140747.5064397696
+    theta: 2.2336372918161667
+    kappa: -0.0015160091362119068
+    s: 37.003226847554842
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.901985653427448
-  a: 0.18850756125329476
+  v: 7.9324663213498994
+  a: 0.20075952940721448
   relative_time: 5.3999999999999888
 }
 trajectory_point {
   path_point {
-    x: 587479.41753535217
-    y: 4140747.7977300212
-    theta: 2.2379325999894726
-    kappa: -0.00078935696856199185
-    s: 37.3559693418915
+    x: 587479.38769758085
+    y: 4140747.8193457336
+    theta: 2.2330377866092528
+    kappa: -0.0014773791690125399
+    s: 37.400098883816007
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9112782347530732
-  a: 0.18320419185738859
+  v: 7.9423706363427282
+  a: 0.19542039892407598
   relative_time: 5.4499999999999886
 }
 trajectory_point {
   path_point {
-    x: 587479.172722959
-    y: 4140748.10872389
-    theta: 2.237545272003989
-    kappa: -0.0010663999807087826
-    s: 37.751760065069391
+    x: 587479.14347854443
+    y: 4140748.13279826
+    theta: 2.2324787294134394
+    kappa: -0.0013607054705387439
+    s: 37.797459480235254
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.920306922621756
-  a: 0.17795182297562198
+  v: 7.9520090941041222
+  a: 0.19012524014981874
   relative_time: 5.4999999999999885
 }
 trajectory_point {
   path_point {
-    x: 587478.9277499232
-    y: 4140748.4201574335
-    theta: 2.2370656091792367
-    kappa: -0.0013645222014083205
-    s: 38.147995678429822
+    x: 587478.89909585193
+    y: 4140748.4467257988
+    theta: 2.23196224897521
+    kappa: -0.0011983467690362634
+    s: 38.195295398915647
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.929074267059204
-  a: 0.17275045460799485
+  v: 7.9613838932195247
+  a: 0.18487405308444282
   relative_time: 5.5499999999999883
 }
 trajectory_point {
   path_point {
-    x: 587478.68263365945
-    y: 4140748.7320277961
-    theta: 2.2364884255240063
-    kappa: -0.0016842174345418558
-    s: 38.544663178551879
+    x: 587478.65459023276
+    y: 4140748.7611433384
+    theta: 2.2315180883943837
+    kappa: -0.0009817595570410587
+    s: 38.593593511889523
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9375828180911245
-  a: 0.16760008675450722
+  v: 7.970497232274381
+  a: 0.17966683772794823
   relative_time: 5.5999999999999881
 }
 trajectory_point {
   path_point {
-    x: 587478.43756083434
-    y: 4140749.0444649979
-    theta: 2.2357182099623074
-    kappa: -0.0020369985032796219
-    s: 38.94174968951593
+    x: 587478.40995410655
+    y: 4140749.0760283321
+    theta: 2.2311952749912232
+    kappa: -0.00071995340325707482
+    s: 38.9923408011185
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9458351257432236
-  a: 0.16250071941515912
+  v: 7.9793513098541338
+  a: 0.17450359408033489
   relative_time: 5.6499999999999879
 }
 trajectory_point {
   path_point {
-    x: 587478.192446155
-    y: 4140749.35738549
-    theta: 2.2348354317304886
-    kappa: -0.00240488052806548
-    s: 39.339242462903634
+    x: 587478.16511581547
+    y: 4140749.3913087156
+    theta: 2.2309570791852771
+    kappa: -0.000438453320552796
+    s: 39.391524358493442
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9538337400412091
-  a: 0.15745235258995052
+  v: 7.9879483245442291
+  a: 0.16938432214160287
   relative_time: 5.6999999999999877
 }
 trajectory_point {
   path_point {
-    x: 587477.94730409142
-    y: 4140749.6707845214
-    theta: 2.2338385817921824
-    kappa: -0.00278733031835817
-    s: 39.737128877797915
+    x: 587477.920081665
+    y: 4140749.7069731597
+    theta: 2.2308266235272391
+    kappa: -0.00013845664624332338
+    s: 39.791131385834518
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9615812110107882
-  a: 0.15245498627888146
+  v: 7.9962904749301087
+  a: 0.16430902191175217
   relative_time: 5.7499999999999876
 }
 trajectory_point {
   path_point {
-    x: 587477.702418238
-    y: 4140749.9848672664
-    theta: 2.2326192692602653
-    kappa: -0.0031832789500739781
-    s: 40.135396440783
+    x: 587477.67481359851
+    y: 4140750.0229760446
+    theta: 2.2308529827247696
+    kappa: 0.00016534341414295298
+    s: 40.191149194891139
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.969080088677666
-  a: 0.1475086204819519
+  v: 8.00437995959722
+  a: 0.15927769339078274
   relative_time: 5.7999999999999874
 }
 trajectory_point {
   path_point {
-    x: 587477.45766262955
-    y: 4140750.2995186141
-    theta: 2.2312689940990857
-    kappa: -0.0035639041019185774
-    s: 40.53403278594439
+    x: 587477.42927658861
+    y: 4140750.3392742551
+    theta: 2.2309734175356297
+    kappa: 0.00046089775864403457
+    s: 40.591565207342015
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9763329230675524
-  a: 0.14261325519916185
+  v: 8.0122189771310026
+  a: 0.15429033657869462
   relative_time: 5.8499999999999872
 }
 trajectory_point {
   path_point {
-    x: 587477.21303152142
-    y: 4140750.6147183524
-    theta: 2.2297980542335307
-    kappa: -0.0039292914299668049
-    s: 40.933025674868894
+    x: 587477.18346530839
+    y: 4140750.6558478726
+    theta: 2.2311967560900565
+    kappa: 0.00074420843583682572
+    s: 40.9923669547951
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9833422642061516
-  a: 0.1377688904305113
+  v: 8.0198097261169021
+  a: 0.14934695147548779
   relative_time: 5.899999999999987
 }
 trajectory_point {
   path_point {
-    x: 587476.96883243928
-    y: 4140750.9306884538
-    theta: 2.2281421350280559
-    kappa: -0.0042622412220272472
-    s: 41.332362996644576
+    x: 587476.93729199457
+    y: 4140750.9726130511
+    theta: 2.2315663388500706
+    kappa: 0.00098411838877335745
+    s: 41.393542078787661
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9901106621191724
-  a: 0.13297552617600028
+  v: 8.0271544051403652
+  a: 0.14444753808116229
   relative_time: 5.9499999999999869
 }
 trajectory_point {
   path_point {
-    x: 587476.72493682336
-    y: 4140751.24731223
-    theta: 2.2263702573400193
-    kappa: -0.0045695855869699077
-    s: 41.732032767860815
+    x: 587476.6907918863
+    y: 4140751.28958157
+    theta: 2.2320020220618795
+    kappa: 0.0011882742782542022
+    s: 41.79507833078619
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.99664066683232
-  a: 0.12823316243562879
+  v: 8.0342552127868334
+  a: 0.13959209639571804
   relative_time: 5.9999999999999867
 }
 trajectory_point {
   path_point {
-    x: 587476.48129104346
-    y: 4140751.5645329463
-    theta: 2.2245035401559177
-    kappa: -0.00485438289425784
-    s: 42.132023300644939
+    x: 587476.44396752131
+    y: 4140751.6067402069
+    theta: 2.2325012773129882
+    kappa: 0.0013547218753657375
+    s: 42.196963726169535
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0029449198284119
-  a: 0.12394619788258388
+  v: 8.0411235966099035
+  a: 0.1351513839632292
   relative_time: 6.0499999999999865
 }
 trajectory_point {
   path_point {
-    x: 587476.23820402112
-    y: 4140751.8825724367
-    theta: 2.2225030112106192
-    kappa: -0.005101041696671014
-    s: 42.532323710474373
+    x: 587476.19668884878
+    y: 4140751.9239732893
+    theta: 2.2330726813473389
+    kappa: 0.0014293592212965838
+    s: 42.599187010172081
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0090362106680022
-  a: 0.11971467617529583
+  v: 8.0477711639267628
+  a: 0.13075943614728394
   relative_time: 6.0999999999999863
 }
 trajectory_point {
   path_point {
-    x: 587475.99562617636
-    y: 4140752.201376135
-    theta: 2.2204077951339993
-    kappa: -0.0053120613053066418
-    s: 42.932923418544831
+    x: 587475.94905670255
+    y: 4140752.241345231
+    theta: 2.2336505347676341
+    kappa: 0.0014384786760226823
+    s: 43.001737202924289
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.01491731149337
-  a: 0.11553859731375898
+  v: 8.0542003529682411
+  a: 0.12641625294788203
   relative_time: 6.1499999999999861
 }
 trajectory_point {
   path_point {
-    x: 587475.75340948137
-    y: 4140752.5208169273
-    theta: 2.2182481670927912
-    kappa: -0.0054948894861800027
-    s: 43.333811984659171
+    x: 587475.7010857003
+    y: 4140752.5588536276
+    theta: 2.2342267498750124
+    kappa: 0.0013826938899847176
+    s: 43.404603446468158
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0205909944468061
-  a: 0.11141796129797338
+  v: 8.06041360196516
+  a: 0.12212183436502348
   relative_time: 6.199999999999986
 }
 trajectory_point {
   path_point {
-    x: 587475.51179041457
-    y: 4140752.841059471
-    theta: 2.2160130968968179
-    kappa: -0.0056364159649609181
-    s: 43.734979107227353
+    x: 587475.45269855089
+    y: 4140752.8764242963
+    theta: 2.2347478172113786
+    kappa: 0.0012054397588868505
+    s: 43.807775004757239
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0260600316705979
-  a: 0.10735276812793898
+  v: 8.0664133491483518
+  a: 0.11787618039870831
   relative_time: 6.2499999999999858
 }
 trajectory_point {
   path_point {
-    x: 587475.27083303255
-    y: 4140753.1621353491
-    theta: 2.213725944414763
-    kappa: -0.0057316834028954509
-    s: 44.13641462326644
+    x: 587475.20399137423
+    y: 4140753.1941187177
+    theta: 2.2351705322816229
+    kappa: 0.00092400690517126759
+    s: 44.211241263656611
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.03132719530703
-  a: 0.10334301780365579
+  v: 8.07220203274864
+  a: 0.11367929104893651
   relative_time: 6.2999999999999856
 }
 trajectory_point {
   path_point {
-    x: 587475.03030335379
-    y: 4140753.4838542389
-    theta: 2.2114135115164486
-    kappa: -0.0057933903217304746
-    s: 44.538108508400633
+    x: 587474.95501182065
+    y: 4140753.5119608459
+    theta: 2.2354917343790666
+    kappa: 0.0005517158241664208
+    s: 44.6149917309429
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0363952574983912
-  a: 0.099388710325123836
+  v: 8.0777820909968536
+  a: 0.10953116631570807
   relative_time: 6.3499999999999854
 }
 trajectory_point {
   path_point {
-    x: 587474.79036260373
-    y: 4140753.8063230035
-    theta: 2.2090793016846249
-    kappa: -0.0058119134591608041
-    s: 44.940050876861228
+    x: 587474.7057695738
+    y: 4140753.8299450306
+    theta: 2.2356121568691538
+    kappa: 3.5947394174088552e-05
+    s: 45.019016036304272
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0412669903869709
-  a: 0.095489845692343089
+  v: 8.08315596212382
+  a: 0.10543180619902301
   relative_time: 6.3999999999999853
 }
 trajectory_point {
   path_point {
-    x: 587474.55125231948
-    y: 4140754.1297046491
-    theta: 2.2067500611915745
-    kappa: -0.0057708118906576447
-    s: 45.342231981486648
+    x: 587474.45638069371
+    y: 4140754.1481491923
+    theta: 2.235469413891471
+    kappa: -0.00062482308384030887
+    s: 45.423303931340435
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0459451661150556
-  a: 0.091646423905313579
+  v: 8.0883260843603662
+  a: 0.10138121069888131
   relative_time: 6.4499999999999851
 }
 trajectory_point {
   path_point {
-    x: 587474.312597529
-    y: 4140754.4537068927
-    theta: 2.2044416059639351
-    kappa: -0.0056906906693188312
-    s: 45.744642213722429
+    x: 587474.20690444554
+    y: 4140754.4666068759
+    theta: 2.2350932386155922
+    kappa: -0.0014000911990067763
+    s: 45.827845289562646
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0504325568249318
-  a: 0.087858444964035276
+  v: 8.0932948959373174
+  a: 0.097379379815283
   relative_time: 6.4999999999999849
 }
 trajectory_point {
   path_point {
-    x: 587474.07444557443
-    y: 4140754.7783520445
-    theta: 2.2021620941227313
-    kappa: -0.0055678227735025841
-    s: 46.147272103621212
+    x: 587473.95743373968
+    y: 4140754.7853781334
+    theta: 2.2343692741232521
+    kappa: -0.0023320906683364415
+    s: 46.232630106393685
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0547319346588875
-  a: 0.084125908868508181
+  v: 8.0980648350855038
+  a: 0.093426313548228057
   relative_time: 6.5499999999999847
 }
 trajectory_point {
   path_point {
-    x: 587473.83716931031
-    y: 4140755.1038979646
-    theta: 2.1999722879434565
-    kappa: -0.005373114969907374
-    s: 46.550112319842754
+    x: 587473.70821340708
+    y: 4140755.1046414673
+    theta: 2.2331313640647292
+    kappa: -0.0034628948641786412
+    s: 46.637648499167895
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.058846071759211
-  a: 0.080448815618732322
+  v: 8.10263834003575
+  a: 0.089522011897716469
   relative_time: 6.5999999999999845
 }
 trajectory_point {
   path_point {
-    x: 587473.60032420128
-    y: 4140755.4300058307
-    theta: 2.1978406042351382
-    kappa: -0.0052035072449395266
-    s: 46.953153669653943
+    x: 587473.45923574246
+    y: 4140755.4243775094
+    theta: 2.23149527141217
+    kappa: -0.0047382518264048584
+    s: 47.042890707131136
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.06277774026819
-  a: 0.076827165214707671
+  v: 8.1070178490188844
+  a: 0.08566647486374826
   relative_time: 6.6499999999999844
 }
 trajectory_point {
   path_point {
-    x: 587473.36391548335
-    y: 4140755.7566677984
-    theta: 2.1957645381464155
-    kappa: -0.0050681489364108783
-    s: 47.356387098928757
+    x: 587473.2106032772
+    y: 4140755.7446536035
+    theta: 2.2293905609936013
+    kappa: -0.0061799447949445578
+    s: 47.448347091440837
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0665297123281121
-  a: 0.073260957656434256
+  v: 8.1112058002657328
+  a: 0.081859702446323415
   relative_time: 6.6999999999999842
 }
 trajectory_point {
   path_point {
-    x: 587473.12835748983
-    y: 4140756.0841695229
-    theta: 2.1937298110290784
-    kappa: -0.005050241110882904
-    s: 47.7598036921483
+    x: 587472.96297457768
+    y: 4140756.0659646448
+    theta: 2.2264812134487544
+    kappa: -0.0075672560435383089
+    s: 47.854008135165955
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0701047600812625
-  a: 0.069750192943912048
+  v: 8.1152046320071243
+  a: 0.078101694645441949
   relative_time: 6.749999999999984
 }
 trajectory_point {
   path_point {
-    x: 587472.89327245357
-    y: 4140756.4122249614
-    theta: 2.1916814935621916
-    kappa: -0.0050963943794956976
-    s: 48.1633946724008
+    x: 587472.7160613531
+    y: 4140756.3880706169
+    theta: 2.2231369020916159
+    kappa: -0.0089033500558445711
+    s: 48.259864443286972
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0735056556699316
-  a: 0.066294871077141049
+  v: 8.1190167824738868
+  a: 0.074392451461103848
   relative_time: 6.7999999999999838
 }
 trajectory_point {
   path_point {
-    x: 587472.65862147359
-    y: 4140756.7407949367
-    theta: 2.1896121223736484
-    kappa: -0.0051986461790985219
-    s: 48.567151401381587
+    x: 587472.46991077566
+    y: 4140756.7109946953
+    theta: 2.2193634306210881
+    kappa: -0.010185631525888438
+    s: 48.665906742695938
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0767351712364057
-  a: 0.062894992056121285
+  v: 8.1226446898968447
+  a: 0.070731972893309111
   relative_time: 6.8499999999999837
 }
 trajectory_point {
   path_point {
-    x: 587472.42471829825
-    y: 4140757.0700908615
-    theta: 2.1874748443432397
-    kappa: -0.0053769322724082774
-    s: 48.97106537939311
+    x: 587472.22547770233
+    y: 4140757.03544263
+    theta: 2.2148977982068958
+    kappa: -0.011372819399002827
+    s: 49.072125882196438
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.079796078922973
-  a: 0.059550555880852729
+  v: 8.126090792506826
+  a: 0.067120258942057767
   relative_time: 6.8999999999999835
 }
 trajectory_point {
   path_point {
-    x: 587472.19145406492
-    y: 4140757.4000215465
-    theta: 2.1852486746003814
-    kappa: -0.00560531576213379
-    s: 49.375128245344925
+    x: 587471.98232226376
+    y: 4140757.3610556927
+    theta: 2.2100240090530243
+    kappa: -0.012498533501697606
+    s: 49.478512832503583
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.08269115087192
-  a: 0.056261562551335409
+  v: 8.1293575285346584
+  a: 0.063557309607349774
   relative_time: 6.9499999999999833
 }
 trajectory_point {
   path_point {
-    x: 587471.95870869409
-    y: 4140757.7304904796
-    theta: 2.182942964403749
-    kappa: -0.0058687408606915483
-    s: 49.779331776753708
+    x: 587471.74038062186
+    y: 4140757.6877702638
+    theta: 2.2047864771402272
+    kappa: -0.013568038399280471
+    s: 49.885058686244044
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0854231592255363
-  a: 0.053028012067569283
+  v: 8.13244733621117
+  a: 0.060043124889185159
   relative_time: 6.9999999999999831
 }
 trajectory_point {
   path_point {
-    x: 587471.7267521905
-    y: 4140758.0616760356
-    theta: 2.1805137261680914
-    kappa: -0.0061725499839405239
-    s: 50.183667889743255
+    x: 587471.50047495251
+    y: 4140758.016170756
+    theta: 2.1990429934433879
+    kappa: -0.014553562787585923
+    s: 50.291754657956027
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0879948761261069
-  a: 0.049849904429554392
+  v: 8.1353626537671868
+  a: 0.056577704787563909
   relative_time: 7.0499999999999829
 }
 trajectory_point {
   path_point {
-    x: 587471.4956964039
-    y: 4140758.3936419543
-    theta: 2.1779320494429921
-    kappa: -0.0065054372248407911
-    s: 50.58812863904447
+    x: 587471.26258202526
+    y: 4140758.346202658
+    theta: 2.1928989125774607
+    kappa: -0.015462536817053785
+    s: 50.69859208408927
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.09040907371592
-  a: 0.046727239637290724
+  v: 8.138105919433535
+  a: 0.053161049302486024
   relative_time: 7.0999999999999828
 }
 trajectory_point {
   path_point {
-    x: 587471.26527388173
-    y: 4140758.7261895584
-    theta: 2.175239413534618
-    kappa: -0.0068521909305926856
-    s: 50.992706217995369
+    x: 587471.02628958551
+    y: 4140758.6775451084
+    theta: 2.1864639043543868
+    kappa: -0.016312665844638551
+    s: 51.105562423005061
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0926685241372649
-  a: 0.043660017690778263
+  v: 8.1406795714410443
+  a: 0.049793158433951518
   relative_time: 7.1499999999999826
 }
 trajectory_point {
   path_point {
-    x: 587471.03563076654
-    y: 4140759.0594093022
-    theta: 2.1724129097918783
-    kappa: -0.0072110743775620151
-    s: 51.39739295854109
+    x: 587470.79216477589
+    y: 4140759.0105793043
+    theta: 2.1796789797516092
+    kappa: -0.017087137868435853
+    s: 51.512657254976233
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0947759995324269
-  a: 0.040648238590017025
+  v: 8.14308604802054
+  a: 0.046474032181960376
   relative_time: 7.1999999999999824
 }
 trajectory_point {
   path_point {
-    x: 587470.807200781
-    y: 4140759.3935845825
-    theta: 2.1693944882949676
-    kappa: -0.0075707960108022734
-    s: 51.802181331233882
+    x: 587470.56079792173
+    y: 4140759.34567318
+    theta: 2.1725378811748421
+    kappa: -0.017770764287673387
+    s: 51.91986828218716
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0967342720436957
-  a: 0.037691902335007008
+  v: 8.1453277874028487
+  a: 0.043203670546512613
   relative_time: 7.2499999999999822
 }
 trajectory_point {
   path_point {
-    x: 587470.57954612048
-    y: 4140759.7284016218
-    theta: 2.1662606309719168
-    kappa: -0.0079229172459755436
-    s: 52.207063945233095
+    x: 587470.33134971
+    y: 4140759.6822110824
+    theta: 2.1651837333483259
+    kappa: -0.018392653656405897
+    s: 52.327187328733743
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0985461138133576
-  a: 0.0347910089257482
+  v: 8.1474072278188
+  a: 0.039982073527608214
   relative_time: 7.2999999999999821
 }
 trajectory_point {
   path_point {
-    x: 587470.35268409282
-    y: 4140760.0638624374
-    theta: 2.1630118665388327
-    kappa: -0.0082655360065482034
-    s: 52.61203354830522
+    x: 587470.10402408324
+    y: 4140760.0203135973
+    theta: 2.1576109597507349
+    kappa: -0.018947362903519435
+    s: 52.734606340623444
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1002142969837
-  a: 0.031945558362240625
+  v: 8.14932680749922
+  a: 0.03680924112524718
   relative_time: 7.3499999999999819
 }
 trajectory_point {
   path_point {
-    x: 587470.12742275419
-    y: 4140760.4004963767
-    theta: 2.1595728275873962
-    kappa: -0.0085737075709031545
-    s: 53.017083026823848
+    x: 587469.88021395239
+    y: 4140760.360862324
+    theta: 2.1497578357525859
+    kappa: -0.019394649232806253
+    s: 53.142117385775251
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.10174159369701
-  a: 0.02915555064448426
+  v: 8.1510889646749334
+  a: 0.033685173339429525
   relative_time: 7.3999999999999817
 }
 trajectory_point {
   path_point {
-    x: 587469.90308421373
-    y: 4140760.7378320051
-    theta: 2.1560395037240356
-    kappa: -0.0088522979005377748
-    s: 53.422205405769674
+    x: 587469.65858317842
+    y: 4140760.7029266981
+    theta: 2.1417734776430839
+    kappa: -0.01977650690711167
+    s: 53.5497126540197
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1031307760955773
-  a: 0.026420985772479116
+  v: 8.1526961375767719
+  a: 0.030609870170155234
   relative_time: 7.4499999999999815
 }
 trajectory_point {
   path_point {
-    x: 587469.67966450914
-    y: 4140761.0758569068
-    theta: 2.1524178493780934
-    kappa: -0.0090997211997389353
-    s: 53.82739384873053
+    x: 587469.439150552
+    y: 4140761.0465020807
+    theta: 2.1336660667887206
+    kappa: -0.020092588323854955
+    s: 53.957384457098875
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1043846163216884
-  a: 0.023741863746225195
+  v: 8.154150764435558
+  a: 0.027583331617424336
   relative_time: 7.4999999999999813
 }
 trajectory_point {
   path_point {
-    x: 587469.45778921468
-    y: 4140761.41496919
-    theta: 2.1486808635260082
-    kappa: -0.0092850353058826548
-    s: 54.23264165790134
+    x: 587469.2233903564
+    y: 4140761.3924790667
+    theta: 2.1254132791404166
+    kappa: -0.020299273278069647
+    s: 54.365125228666386
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.10550588651763
-  a: 0.021118184565722481
+  v: 8.155455283482123
+  a: 0.024605557681236789
   relative_time: 7.5499999999999812
 }
 trajectory_point {
   path_point {
-    x: 587469.23712649266
-    y: 4140761.7549331319
-    theta: 2.1448872039575386
-    kappa: -0.0094109927258028067
-    s: 54.637942274084153
+    x: 587469.01025273756
+    y: 4140761.740141985
+    theta: 2.11710491548175
+    kappa: -0.0204278956205251
+    s: 54.7729275242874
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.10649735882569
-  a: 0.018549948230970989
+  v: 8.1566121329472914
+  a: 0.021676548361592607
   relative_time: 7.599999999999981
 }
 trajectory_point {
   path_point {
-    x: 587469.01747347636
-    y: 4140762.0956057846
-    theta: 2.141059664131749
-    kappa: -0.0094839076757460526
-    s: 55.043289276688142
+    x: 587468.79943150759
+    y: 4140762.0892802794
+    theta: 2.1087618406377593
+    kappa: -0.020487072904352685
+    s: 55.180784021438605
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1073618053881553
-  a: 0.016037154741970733
+  v: 8.157623751061891
+  a: 0.018796303658491803
   relative_time: 7.6499999999999808
 }
 trajectory_point {
   path_point {
-    x: 587468.79926501983
-    y: 4140762.4372544
-    theta: 2.1372112012838942
-    kappa: -0.009474540492993103
-    s: 55.448676383729577
+    x: 587468.59196201654
+    y: 4140762.4404806122
+    theta: 2.1004077271216071
+    kappa: -0.020444919617854787
+    s: 55.588687519508255
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1081019983473155
-  a: 0.013579804098721671
+  v: 8.15849257605675
+  a: 0.015964823571934364
   relative_time: 7.6999999999999806
 }
 trajectory_point {
   path_point {
-    x: 587468.5825429
-    y: 4140762.7798866713
-    theta: 2.1333945725769068
-    kappa: -0.0093681043733113822
-    s: 55.854097451831862
+    x: 587468.38766144775
+    y: 4140762.79357371
+    theta: 2.092100330623917
+    kappa: -0.020304097808652895
+    s: 55.996630939796127
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.108720709845457
-  a: 0.011177896301223844
+  v: 8.1592210461626937
+  a: 0.013182108101920303
   relative_time: 7.7499999999999805
 }
 trajectory_point {
   path_point {
-    x: 587468.36685024539
-    y: 4140763.1232007006
-    theta: 2.1296266686380165
-    kappa: -0.0091866554541344
-    s: 56.259546476225481
+    x: 587468.18571223924
+    y: 4140763.1480537858
+    theta: 2.0838522597156151
+    kappa: -0.020088724292512385
+    s: 56.404607325513552
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1092207120248663
-  a: 0.00883143134947724
+  v: 8.1598115996105509
+  a: 0.010448157248449608
   relative_time: 7.79999999999998
 }
 trajectory_point {
   path_point {
-    x: 587468.152377946
-    y: 4140763.4673060006
-    theta: 2.1259377176076977
-    kappa: -0.0089105992330097035
-    s: 56.665017590748064
+    x: 587467.98663736461
+    y: 4140763.5041931509
+    theta: 2.0756997543207327
+    kappa: -0.019781250174852986
+    s: 56.812609841783392
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1096047770278332
-  a: 0.0065404092434818428
+  v: 8.1602666746311474
+  a: 0.0077629710115222628
   relative_time: 7.84999999999998
 }
 trajectory_point {
   path_point {
-    x: 587467.93948131718
-    y: 4140763.8124069874
-    theta: 2.1224336601288987
-    kappa: -0.00849171775580992
-    s: 57.070505067844358
+    x: 587467.79115129856
+    y: 4140763.8623338453
+    theta: 2.0677412316032222
+    kappa: -0.01935045830232187
+    s: 57.220631775640058
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1098756769966442
-  a: 0.004304829983237668
+  v: 8.1605887094553111
+  a: 0.005126549391138352
   relative_time: 7.89999999999998
 }
 trajectory_point {
   path_point {
-    x: 587467.72751206183
-    y: 4140764.1580902711
-    theta: 2.119089055164054
-    kappa: -0.0079757530878760037
-    s: 57.4760033185662
+    x: 587467.59795061231
+    y: 4140764.2217230108
+    theta: 2.0599436237211344
+    kappa: -0.018838657633040275
+    s: 57.628666536029492
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1100361840735875
-  a: 0.0021246935687447288
+  v: 8.16078014231387
+  a: 0.0025388923872977642
   relative_time: 7.94999999999998
 }
 trajectory_point {
   path_point {
-    x: 587467.51645968889
-    y: 4140764.5043416894
-    theta: 2.1159148601903324
-    kappa: -0.0073607156304284962
-    s: 57.881506892572553
+    x: 587467.40703689132
+    y: 4140764.5823467826
+    theta: 2.0523172620210688
+    kappa: -0.018245221893343114
+    s: 58.036707653809188
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1100890704009476
-  a: 2.9976021664879227e-15
+  v: 8.16084341143765
+  a: 5.2735593669694936e-16
   relative_time: 7.99999999999998
 }
 decision {
diff --git a/modules/planning/testdata/yongfengchanyeyuan_test/result_cruise_0.pb.txt b/modules/planning/testdata/yongfengchanyeyuan_test/result_cruise_0.pb.txt
old mode 100755
new mode 100644
index 220654dd10..339df5292d
--- a/modules/planning/testdata/yongfengchanyeyuan_test/result_cruise_0.pb.txt
+++ b/modules/planning/testdata/yongfengchanyeyuan_test/result_cruise_0.pb.txt
@@ -7,2254 +7,2254 @@ trajectory_point {
   path_point {
     x: 435707.283186
     y: 4435948.39609
-    theta: 1.9388221429050951
-    kappa: 4.3370369538631396e-16
-    s: 1.2032042029375134e-13
+    theta: 1.9388221429050949
+    kappa: 3.5748800236964343e-16
+    s: 1.2031348139984743e-13
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.3083900042902883e-14
-  a: 3.30221960886945e-13
+  v: 7.3080430595950929e-14
+  a: 3.3020808309913718e-13
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 435707.28317848727
-    y: 4435948.3961094655
-    theta: 1.9388221703511377
-    kappa: 7.5200098838387781e-08
-    s: 2.0865088438277495e-05
+    x: 435707.28317849478
+    y: 4435948.39610945
+    theta: 1.9388221640100367
+    kappa: 6.1888535826255176e-08
+    s: 2.0847097173548114e-05
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0012499035858338798
-  a: 0.049836006382971886
+  v: 0.0012488255873231
+  a: 0.049793004632345608
   relative_time: 0.05
 }
 trajectory_point {
   path_point {
-    x: 435707.28312618606
-    y: 4435948.3962449785
-    theta: 1.9388223614202107
-    kappa: 5.9871502896422321e-07
-    s: 0.00016612002140475215
+    x: 435707.28312624648
+    y: 4435948.3962448472
+    theta: 1.9388223109346381
+    kappa: 4.9273305339954534e-07
+    s: 0.0001659766822359906
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.0049675869336573012
-  a: 0.098711190482835234
+  v: 0.0049632985775751454
+  a: 0.098625896120411255
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 435707.282985101
-    y: 4435948.39661053
-    theta: 1.9388228768389744
-    kappa: 2.0109237863189373e-06
-    s: 0.0005579527592694029
+    x: 435707.28298530396
+    y: 4435948.3966100914
+    theta: 1.9388227072710862
+    kappa: 1.6549576509246759e-06
+    s: 0.00055747098402780522
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.011105008929404433
-  a: 0.1466255522999203
+  v: 0.011095413313631732
+  a: 0.14649867446452713
   relative_time: 0.15000000000000002
 }
 trajectory_point {
   path_point {
-    x: 435707.28271210124
-    y: 4435948.3973178715
-    theta: 1.9388238741744137
-    kappa: 4.74354809054431e-06
-    s: 0.001316149206574942
+    x: 435707.2827125804
+    y: 4435948.3973168377
+    theta: 1.9388234741815977
+    kappa: 3.9038607343346848e-06
+    s: 0.0013150119484092811
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.019614128458936359
-  a: 0.193579091834227
+  v: 0.019597164138295362
+  a: 0.19341133966469323
   relative_time: 0.2
 }
 trajectory_point {
   path_point {
-    x: 435707.28226492112
-    y: 4435948.3984765159
-    theta: 1.9388255078338386
-    kappa: 9.219652385416179e-06
-    s: 0.002558093212157136
+    x: 435707.282265853
+    y: 4435948.3984745033
+    theta: 1.9388247303984196
+    kappa: 7.5876150176483306e-06
+    s: 0.0025558812383808336
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.030446904408114157
-  a: 0.23957180908575537
+  v: 0.030420545394368555
+  a: 0.23936389172090958
   relative_time: 0.25
 }
 trajectory_point {
   path_point {
-    x: 435707.28160215972
-    y: 4435948.40019373
-    theta: 1.9388279290648842
-    kappa: 1.5853643838844434e-05
-    s: 0.0043987665691448061
+    x: 435707.28160376317
+    y: 4435948.4001902686
+    theta: 1.9388265922238288
+    kappa: 1.30472675229701e-05
+    s: 0.0043949602340830046
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.043555295662798921
-  a: 0.28460370405450547
+  v: 0.043517551424653821
+  a: 0.28435633063317611
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 435707.28068328113
-    y: 4435948.4025745438
-    theta: 1.9388312859555126
-    kappa: 2.5051272342873027e-05
-    s: 0.0069507490149598276
+    x: 435707.28068581637
+    y: 4435948.40256907
+    theta: 1.9388291735301333
+    kappa: 2.0616739580490187e-05
+    s: 0.0069447300327964579
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.05889126110885172
-  a: 0.32867477674047724
+  v: 0.058840176571953666
+  a: 0.32838865640149295
   relative_time: 0.35
 }
 trajectory_point {
   path_point {
-    x: 435707.27946861414
-    y: 4435948.4057217455
-    theta: 1.938835723434013
-    kappa: 3.7209630513679952e-05
-    s: 0.010324218231317128
+    x: 435707.27947238227
+    y: 4435948.40571361
+    theta: 1.9388325857596722
+    kappa: 3.0622826828484524e-05
+    s: 0.010315271448941989
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.076406759632133656
-  a: 0.37178502714367068
+  v: 0.076340415179070614
+  a: 0.37146086902586006
   relative_time: 0.39999999999999997
 }
 trajectory_point {
   path_point {
-    x: 435707.27791935269
-    y: 4435948.4097358808
-    theta: 1.9388413832690041
-    kappa: 5.2717153691577284e-05
-    s: 0.014626949844224696
+    x: 435707.27792469459
+    y: 4435948.4097243464
+    theta: 1.9388369379248167
+    kappa: 4.3385199213314755e-05
+    s: 0.014614265014080514
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.09605375011850581
-  a: 0.41393445526408584
+  v: 0.095970261588807171
+  a: 0.41357296850627734
   relative_time: 0.44999999999999996
 }
 trajectory_point {
   path_point {
-    x: 435707.27599755547
-    y: 4435948.4147152584
-    theta: 1.9388484040694336
-    kappa: 7.19536199410111e-05
-    s: 0.019964317423983562
+    x: 435707.2760048511
+    y: 4435948.4146995023
+    theta: 1.93884233660797
+    kappa: 5.9216400989428243e-05
+    s: 0.019946990976913069
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.11778419145382925
-  a: 0.45512306110172263
+  v: 0.11768171014396585
+  a: 0.45472495484274489
   relative_time: 0.49999999999999994
 }
 trajectory_point {
   path_point {
-    x: 435707.27366614592
-    y: 4435948.42075594
-    theta: 1.9388569212845805
-    kappa: 9.529015005056162e-05
-    s: 0.02643929248518782
+    x: 435707.27367581404
+    y: 4435948.4207350565
+    theta: 1.9388488859615682
+    kappa: 7.8421850719358077e-05
+    s: 0.026416329303280837
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.14155004252396511
-  a: 0.49535084465658114
+  v: 0.14142675518734915
+  a: 0.49491682803526271
   relative_time: 0.54999999999999993
 }
 trajectory_point {
   path_point {
-    x: 435707.27088891267
-    y: 4435948.42795175
-    theta: 1.9388670672040564
-    kappa: 0.00012308920753294303
-    s: 0.03415244448672463
+    x: 435707.27090140939
+    y: 4435948.4279247541
+    theta: 1.9388566877080808
+    kappa: 0.00010129984127372312
+    s: 0.034122759676165113
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.1673032622147744
-  a: 0.53461780592866126
+  v: 0.16715739106175967
+  a: 0.5341485880838307
   relative_time: 0.6
 }
 trajectory_point {
   path_point {
-    x: 435707.2676305091
-    y: 4435948.4363942724
-    theta: 1.9388789709578049
-    kappa: 0.00015570459862500365
-    s: 0.043201940831774178
+    x: 435707.26764632756
+    y: 4435948.4363600966
+    theta: 1.9388658411400108
+    kappa: 0.00012814153983122789
+    s: 0.043164361495687309
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.1949958094121183
-  a: 0.57292394491796317
+  v: 0.19482561210999982
+  a: 0.57242023498844907
   relative_time: 0.65
 }
 trajectory_point {
   path_point {
-    x: 435707.26385645353
-    y: 4435948.4461728493
-    theta: 1.9388927585161033
-    kappa: 0.00019348147228772581
-    s: 0.053683546867809739
+    x: 435707.26387612277
+    y: 4435948.4461303484
+    theta: 1.9388764431198946
+    kappa: 0.00015923098787866259
+    s: 0.053636813879108979
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.22457964300185781
-  a: 0.6102692616244868
+  v: 0.22438341267487208
+  a: 0.60973176874911761
   relative_time: 0.70000000000000007
 }
 trajectory_point {
   path_point {
-    x: 435707.25953312905
-    y: 4435948.4573745821
-    theta: 1.9389085526895624
-    kappa: 0.00023675632020622587
-    s: 0.06569062588659759
+    x: 435707.25955721375
+    y: 4435948.4573225332
+    theta: 1.9388885880803035
+    kappa: 0.00019484510121090323
+    s: 0.065633395660831789
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.25600672186985407
-  a: 0.646653756048232
+  v: 0.255782787099179
+  a: 0.64608318936583642
   relative_time: 0.75000000000000011
 }
 trajectory_point {
   path_point {
-    x: 435707.25462778378
-    y: 4435948.470084331
-    theta: 1.9389264731291262
-    kappa: 0.0002858569767897543
-    s: 0.079314139124197131
+    x: 435707.254656883
+    y: 4435948.470021436
+    theta: 1.9389023680238417
+    kappa: 0.00023525366993091152
+    s: 0.079244985392397549
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.28922900490196812
-  a: 0.682077428189199
+  v: 0.28897572972572327
+  a: 0.68147449683860539
   relative_time: 0.80000000000000016
 }
 trajectory_point {
   path_point {
-    x: 435707.2491085307
-    y: 4435948.4843847137
-    theta: 1.938946636326071
-    kappa: 0.00034110261917169561
-    s: 0.094642645760960736
+    x: 435707.24914327753
+    y: 4435948.4843096025
+    theta: 1.9389178725231471
+    kappa: 0.00028071935844973481
+    s: 0.094560061342488122
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.32419845098406108
-  a: 0.71654027804738762
+  v: 0.3239142348973072
+  a: 0.71590569116742464
   relative_time: 0.8500000000000002
 }
 trajectory_point {
   path_point {
-    x: 435707.24294434773
-    y: 4435948.5003561107
-    theta: 1.9389691556120037
-    kappa: 0.00040280376720956831
-    s: 0.11176230292153393
+    x: 435707.24298540794
+    y: 4435948.5002673389
+    theta: 1.9389351887208903
+    kappa: 0.00033149770548650644
+    s: 0.11166470149692567
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.360867019001994
-  a: 0.750042305622798
+  v: 0.36055029695673335
+  a: 0.74937677235229416
   relative_time: 0.90000000000000024
 }
 trajectory_point {
   path_point {
-    x: 435707.23610507743
-    y: 4435948.5180766582
-    theta: 1.9389941411588589
-    kappa: 0.000471262283485025
-    s: 0.1307568656748552
+    x: 435707.23615314951
+    y: 4435948.5179727124
+    theta: 1.938954401329773
+    kappa: 0.000387837124068445
+    s: 0.13064258355867225
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.399186667841628
-  a: 0.78258351091542988
+  v: 0.3988359102468042
+  a: 0.78188774039321385
   relative_time: 0.95000000000000029
 }
 trajectory_point {
   path_point {
-    x: 435707.22856142768
-    y: 4435948.5376222525
-    theta: 1.9390216999788958
-    kappa: 0.00054677137330385217
-    s: 0.15170768703415607
+    x: 435707.22861724143
+    y: 4435948.53750155
+    theta: 1.9389755926325265
+    kappa: 0.00044997890153085509
+    s: 0.15157498494783017
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.43910935638882403
-  a: 0.81416389392528365
+  v: 0.43872306911032222
+  a: 0.81343859529018381
   relative_time: 1.0000000000000002
 }
 trajectory_point {
   path_point {
-    x: 435707.22028497094
-    y: 4435948.5590665489
-    theta: 1.9390519359246929
-    kappa: 0.000629615584695971
-    s: 0.17469371795696131
+    x: 435707.220349287
+    y: 4435948.5589274419
+    theta: 1.9389988424819098
+    kappa: 0.00051815719951712731
+    s: 0.17454078280164198
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.48058704352944337
-  a: 0.84478345465235893
+  v: 0.48016376789008997
+  a: 0.844029337043204
   relative_time: 1.0500000000000003
 }
 trajectory_point {
   path_point {
-    x: 435707.21124814468
-    y: 4435948.5824809633
-    theta: 1.9390849496891429
-    kappa: 0.000720070808415436
-    s: 0.19979150734508852
+    x: 435707.21132175351
+    y: 4435948.5823217342
+    theta: 1.9390242283007071
+    kappa: 0.00059259905397873725
+    s: 0.19961645397449007
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.523571688149347
-  a: 0.874442193096656
+  v: 0.52311000092891013
+  a: 0.87365996565227433
   relative_time: 1.1000000000000003
 }
 trajectory_point {
   path_point {
-    x: 435707.20142425125
-    y: 4435948.6079346687
-    theta: 1.9391208388054442
-    kappa: 0.00081840427794043608
-    s: 0.22707520204464851
+    x: 435707.20150797267
+    y: 4435948.6077535376
+    theta: 1.9390518250817235
+    kappa: 0.00067352437517524731
+    s: 0.22687607503789728
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.568015249134396
-  a: 0.90314010925817478
+  v: 0.56751376256958508
+  a: 0.90233048111739511
   relative_time: 1.1500000000000004
 }
 trajectory_point {
   path_point {
-    x: 435707.19078745786
-    y: 4435948.6354945982
-    theta: 1.939159697647092
-    kappa: 0.00092487456947329394
-    s: 0.25661654684604496
+    x: 435707.19088214019
+    y: 4435948.6352897231
+    theta: 1.9390817053877811
+    kappa: 0.00076114594767430445
+    s: 0.2563913222805263
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.61386968537045139
-  a: 0.93087720313691513
+  v: 0.61332704715491715
+  a: 0.93004088343856606
   relative_time: 1.2000000000000004
 }
 trajectory_point {
   path_point {
-    x: 435707.17931279662
-    y: 4435948.6652254425
-    theta: 1.9392016174278675
-    kappa: 0.0010397316019404668
-    s: 0.2884848844839748
+    x: 435707.17941931577
+    y: 4435948.6649949206
+    theta: 1.9391139393517152
+    kappa: 0.00085566943035164167
+    s: 0.28823147170818
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.66108695574337462
-  a: 0.95765347473287721
+  v: 0.66050184902770925
+  a: 0.95679117261578717
   relative_time: 1.2500000000000004
 }
 trajectory_point {
   path_point {
-    x: 435707.16697616456
-    y: 4435948.6971896524
-    theta: 1.9392466862018258
-    kappa: 0.0011632166369925454
-    s: 0.32274715563742795
+    x: 435707.16709542356
+    y: 4435948.6969315223
+    theta: 1.9391485946763667
+    kappa: 0.00095729335639107855
+    s: 0.32246339904380156
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.70961901913902636
-  a: 0.98346892404606112
+  v: 0.70899016253076352
+  a: 0.98258134864905866
   relative_time: 1.3000000000000005
 }
 trajectory_point {
   path_point {
-    x: 435707.15375432372
-    y: 4435948.7314474378
-    theta: 1.93929498886328
-    kappa: 0.0012955622790042547
-    s: 0.35946789892968734
+    x: 435707.15388725162
+    y: 4435948.7311596805
+    theta: 1.9391857366345764
+    kappa: 0.0010662091332845193
+    s: 0.35915157972747419
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.75941783444326783
-  a: 1.0083235510764665
+  v: 0.75874398200688264
+  a: 1.00741141153838
   relative_time: 1.3500000000000005
 }
 trajectory_point {
   path_point {
-    x: 435707.13962490077
-    y: 4435948.7680567689
-    theta: 1.9393466071467846
-    kappa: 0.0014369924750744546
-    s: 0.3987092509283291
+    x: 435707.139772452
+    y: 4435948.7677373076
+    theta: 1.9392254280691772
+    kappa: 0.0011826010428319545
+    s: 0.3983580889164211
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.81043536054196008
-  a: 1.0322173558240937
+  v: 0.80971530179886919
+  a: 1.0312813612837521
   relative_time: 1.4000000000000006
 }
 trajectory_point {
   path_point {
-    x: 435707.1245663875
-    y: 4435948.8070733733
-    theta: 1.9394016196271162
-    kappa: 0.0015877225150261372
-    s: 0.44053094614522204
+    x: 435707.12472954119
+    y: 4435948.8067200771
+    theta: 1.939267729392985
+    kappa: 0.00130664624114146
+    s: 0.44014260148500567
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.86262355632096421
-  a: 1.0551503382889427
+  v: 0.86185611624952552
+  a: 1.0541911978851741
   relative_time: 1.4500000000000006
 }
 trajectory_point {
   path_point {
-    x: 435707.10855814052
-    y: 4435948.8485507369
-    theta: 1.9394601017192519
-    kappa: 0.00174795903140643
-    s: 0.4849903170365284
+    x: 435707.10873789963
+    y: 4435948.8481614236
+    theta: 1.9393126985887892
+    kappa: 0.0014385147586291973
+    s: 0.48456239202473156
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.91593438066614141
-  a: 1.077122498471013
+  v: 0.91511841970165408
+  a: 1.0761409213426465
   relative_time: 1.5000000000000007
 }
 trajectory_point {
   path_point {
-    x: 435707.09158038138
-    y: 4435948.8925401065
-    theta: 1.9395221256783446
-    kappa: 0.0019178999994865953
-    s: 0.53214229400270352
+    x: 435707.091777772
+    y: 4435948.8921125419
+    theta: 1.9393603912093411
+    kappa: 0.0015783695000194152
+    s: 0.53167233484424259
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.97031979246335276
-  a: 1.0981338363703053
+  v: 0.96945420649805769
+  a: 1.0971305316561692
   relative_time: 1.5500000000000007
 }
 trajectory_point {
   path_point {
-    x: 435707.07361419639
-    y: 4435948.9390904866
-    theta: 1.9395877605996961
-    kappa: 0.002097734737262027
-    s: 0.58203940538849541
+    x: 435707.07383026689
+    y: 4435948.9386223871
+    theta: 1.9394108603773421
+    kappa: 0.0017263662443444451
+    s: 0.5815249039693221
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0257317505984591
-  a: 1.1181843519868191
+  v: 1.0248154709815389
+  a: 1.1171600288257419
   relative_time: 1.6000000000000008
 }
 trajectory_point {
   path_point {
-    x: 435707.0546415369
-    y: 4435948.9882486425
-    theta: 1.9396570724187276
-    kappa: 0.0022876439054522546
-    s: 0.6347317774829454
+    x: 435707.05487735732
+    y: 4435948.9877376761
+    theta: 1.9394641567854296
+    kappa: 0.0018826536449447074
+    s: 0.63417017314289448
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.0821222139573219
-  a: 1.1372740453205548
+  v: 1.0811542074948994
+  a: 1.136229412851365
   relative_time: 1.6500000000000008
 }
 trajectory_point {
   path_point {
-    x: 435707.0346452191
-    y: 4435949.0400590971
-    theta: 1.9397301239109466
-    kappa: 0.0024877995075009425
-    s: 0.69026713451938793
+    x: 435707.03490188025
+    y: 4435949.0395028852
+    theta: 1.9395203286961626
+    kappa: 0.0020473732294687065
+    s: 0.68965581582502355
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.1394431414258017
-  a: 1.1554029163715118
+  v: 1.1384224103809426
+  a: 1.1543386837330383
   relative_time: 1.7000000000000008
 }
 trajectory_point {
   path_point {
-    x: 435707.01352743746
-    y: 4435949.0945326006
-    theta: 1.9399178408078914
-    kappa: 0.0026504884065659509
-    s: 0.74869079867545041
+    x: 435707.01381363953
+    y: 4435949.0939316759
+    theta: 1.9396988285700254
+    kappa: 0.0022271456625461506
+    s: 0.7480271051929136
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.19764649188976
-  a: 1.1725709651396907
+  v: 1.196572073982471
+  a: 1.1714878414707617
   relative_time: 1.7500000000000009
 }
 trajectory_point {
   path_point {
-    x: 435706.99133736413
-    y: 4435949.151734204
-    theta: 1.9401323531057193
-    kappa: 0.0028138355411041192
-    s: 0.81004569007305294
+    x: 435706.99166492122
+    y: 4435949.1510902345
+    theta: 1.9398902735453771
+    kappa: 0.0024161540954378705
+    s: 0.809326914140909
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.256684224235058
-  a: 1.1887781916250915
+  v: 1.2555551926422868
+  a: 1.1876768860645357
   relative_time: 1.8000000000000009
 }
 trajectory_point {
   path_point {
-    x: 435706.96807250695
-    y: 4435949.2117063859
-    theta: 1.9403572554620732
-    kappa: 0.0029850944503931089
-    s: 0.87437232677840959
+    x: 435706.9684434529
+    y: 4435949.2110172091
+    theta: 1.9400909910140793
+    kappa: 0.0026143169542736616
+    s: 0.873595715280494
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3165082973475566
-  a: 1.2040245958277136
+  v: 1.3153237607031922
+  a: 1.2029058175143597
   relative_time: 1.850000000000001
 }
 trajectory_point {
   path_point {
-    x: 435706.94371908042
-    y: 4435949.2744846828
-    theta: 1.9405926811275194
-    kappa: 0.00316436661193462
-    s: 0.94170882480202656
+    x: 435706.94413547823
+    y: 4435949.2737480989
+    theta: 1.940301099870241
+    kappa: 0.0028217516291611493
+    s: 0.94087158094029311
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.3770706701131163
-  a: 1.2183101777475576
+  v: 1.3758297725079902
+  a: 1.2171746358202338
   relative_time: 1.900000000000001
 }
 trajectory_point {
   path_point {
-    x: 435706.91826416831
-    y: 4435949.3401023913
-    theta: 1.9408387549482664
-    kappa: 0.0033517471081588655
-    s: 1.0120908980987031
+    x: 435706.91872810846
+    y: 4435949.3393161651
+    theta: 1.9405207115072705
+    kappa: 0.0030385681093088443
+    s: 1.0111901831660712
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.4383233014175993
-  a: 1.2316349373846234
+  v: 1.437025222399483
+  a: 1.2304833409821585
   relative_time: 1.9500000000000011
 }
 trajectory_point {
   path_point {
-    x: 435706.89169572259
-    y: 4435949.4085905682
-    theta: 1.9410955933652414
-    kappa: 0.0035473246264257048
-    s: 1.0855518585679584
+    x: 435706.89220932173
+    y: 4435949.4077524329
+    theta: 1.9407499298172175
+    kappa: 0.0032648689830274522
+    s: 1.0845847937211583
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.500218150147129
-  a: 1.2439988747389796
+  v: 1.4988621047207369
+  a: 1.242831933000202
   relative_time: 2.0000000000000009
 }
 trajectory_point {
   path_point {
-    x: 435706.8640025233
-    y: 4435949.4799781367
-    theta: 1.9413633048082835
-    kappa: 0.0037511817599468083
-    s: 1.162122729083543
+    x: 435706.864567923
+    y: 4435949.4790857937
+    theta: 1.9409888515430214
+    kappa: 0.003500749786186582
+    s: 1.1610863970990244
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.5627148236476047
-  a: 1.2557772553004833
+  v: 1.5613000608673577
+  a: 1.254595589812868
   relative_time: 2.0500000000000007
 }
 trajectory_point {
   path_point {
-    x: 435706.83517393033
-    y: 4435949.5542925252
-    theta: 1.9416419920925907
-    kappa: 0.0039633968334309745
-    s: 1.241832929224904
+    x: 435706.83579329553
+    y: 4435949.5533436462
+    theta: 1.9412375684187249
+    kappa: 0.0037463011159224614
+    s: 1.2407243760476081
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6257867946792195
-  a: 1.2670107759845466
+  v: 1.6243125913968457
+  a: 1.2658149083148853
   relative_time: 2.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 435706.80519978673
-    y: 4435949.631559914
-    theta: 1.94193175335292
-    kappa: 0.0041840446153022324
-    s: 1.324710542793752
+    x: 435706.80587530509
+    y: 4435949.630552141
+    theta: 1.9414961680034697
+    kappa: 0.0040016094547969654
+    s: 1.3235267788631639
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.6894068202481016
-  a: 1.2776994367911698
+  v: 1.6878724793936684
+  a: 1.2764898885062537
   relative_time: 2.1500000000000004
 }
 trajectory_point {
   path_point {
-    x: 435706.77402813279
-    y: 4435949.7117887381
-    theta: 1.9422560722332314
-    kappa: 0.0044003843310797
-    s: 1.4107822914421035
+    x: 435706.774738406
+    y: 4435949.710710519
+    theta: 1.9418129881082178
+    kappa: 0.00424930411286588
+    s: 1.409520292996171
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7535476573603788
-  a: 1.2878432377203528
+  v: 1.7519525079422933
+  a: 1.2866205303869735
   relative_time: 2.2
 }
 trajectory_point {
   path_point {
-    x: 435706.74148331303
-    y: 4435949.7949377662
-    theta: 1.9427071071066484
-    kappa: 0.0045620529957278591
-    s: 1.5000735346722813
+    x: 435706.74226620165
+    y: 4435949.7938006595
+    theta: 1.942266634293264
+    kappa: 0.0044610614348161715
+    s: 1.4987302450513305
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.818182063022179
-  a: 1.2974421787720958
+  v: 1.8165254601271879
+  a: 1.2962068339570445
   relative_time: 2.25
 }
 trajectory_point {
   path_point {
-    x: 435706.70775630721
-    y: 4435949.8811071711
-    theta: 1.9431745262869689
-    kappa: 0.0047295942520308209
-    s: 1.5926082698369148
+    x: 435706.70861449814
+    y: 4435949.8799089119
+    theta: 1.9427367589386424
+    kappa: 0.0046805104905891506
+    s: 1.5911806007875682
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8832827942396304
-  a: 1.3064962599463983
+  v: 1.8815641190328196
+  a: 1.3052487992164667
   relative_time: 2.3
 }
 trajectory_point {
   path_point {
-    x: 435706.67283886543
-    y: 4435949.9703180306
-    theta: 1.9436584439525479
-    kappa: 0.004903049082764727
-    s: 1.68840913213894
+    x: 435706.67377506761
+    y: 4435949.96905633
+    theta: 1.9432234768301928
+    kappa: 0.0049077049373806893
+    s: 1.6868939651180319
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9488226080188611
-  a: 1.3150054812432608
+  v: 1.9470412677436566
+  a: 1.3137464261652403
   relative_time: 2.3499999999999996
 }
 trajectory_point {
   path_point {
-    x: 435706.63672323385
-    y: 4435950.0625901548
-    theta: 1.9441589673578858
-    kappa: 0.0050824560044289147
-    s: 1.7874973946315988
+    x: 435706.6377401771
+    y: 4435950.0612627007
+    theta: 1.9437268957895253
+    kappa: 0.0051426952021522464
+    s: 1.7858915821100942
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.0147742613659987
-  a: 1.3229698426626828
+  v: 2.0129296893441659
+  a: 1.3216997148033651
   relative_time: 2.3999999999999995
 }
 trajectory_point {
   path_point {
-    x: 435706.59940215549
-    y: 4435950.1579420846
-    theta: 1.9446761968291506
-    kappa: 0.005267851067245915
-    s: 1.8898929682184398
+    x: 435706.60050258925
+    y: 4435950.1565465434
+    theta: 1.9442471166694659
+    kappa: 0.0053855284816308659
+    s: 1.8881933349853501
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.0811105112871715
-  a: 1.3303893442046648
+  v: 2.0792021669188152
+  a: 1.3291086651308412
   relative_time: 2.4499999999999993
 }
 trajectory_point {
   path_point {
-    x: 435706.56086886971
-    y: 4435950.2563910941
-    theta: 1.9452102257595092
-    kappa: 0.0054592678551614558
-    s: 1.9956144016533179
+    x: 435706.56205556187
+    y: 4435950.25492511
+    theta: 1.9447842333493055
+    kappa: 0.0056362487423091776
+    s: 1.9938177461196185
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.1478041147885074
-  a: 1.3372639858692068
+  v: 2.145831483552072
+  a: 1.3359732771476684
   relative_time: 2.4999999999999991
 }
 trajectory_point {
   path_point {
-    x: 435706.52107713156
-    y: 4435950.3579374468
-    theta: 1.9457720842713573
-    kappa: 0.0056483518167814266
-    s: 2.1046788815403947
+    x: 435706.52230912982
+    y: 4435950.3563814713
+    theta: 1.9453669728123957
+    kappa: 0.0058728912659606215
+    s: 2.102781977042941
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.2148278288761345
-  a: 1.3435937676563083
+  v: 2.2127904223284038
+  a: 1.3422935508538469
   relative_time: 2.5499999999999989
 }
 trajectory_point {
   path_point {
-    x: 435706.47969289933
-    y: 4435950.4624666553
-    theta: 1.9464517098523983
-    kappa: 0.0057662784931137205
-    s: 2.2171022323341374
+    x: 435706.48100232007
+    y: 4435950.4608300058
+    theta: 1.9460827282458311
+    kappa: 0.0060284083756547
+    s: 2.2151018284395838
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.2821544105561804
-  a: 1.3493786895659696
+  v: 2.2800517663322788
+  a: 1.3480694862493767
   relative_time: 2.5999999999999988
 }
 trajectory_point {
   path_point {
-    x: 435706.43706690718
-    y: 4435950.5701323263
-    theta: 1.9471517293359513
-    kappa: 0.0058877436333948105
-    s: 2.332898916339321
+    x: 435706.4384561351
+    y: 4435950.56841243
+    theta: 1.9468199607578869
+    kappa: 0.0061885916421560595
+    s: 2.3307917401480349
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.3497566168347737
-  a: 1.3546187515981909
+  v: 2.3475882986481635
+  a: 1.3533010833342578
   relative_time: 2.6499999999999986
 }
 trajectory_point {
   path_point {
-    x: 435706.39319433289
-    y: 4435950.6809466425
-    theta: 1.9478722213156496
-    kappa: 0.0060127609790559654
-    s: 2.452082033711025
+    x: 435706.394665765
+    y: 4435950.679140904
+    theta: 1.9475787529865554
+    kappa: 0.0063534591745266246
+    s: 2.4498647911610072
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.4176072047180419
-  a: 1.3593139537529719
+  v: 2.4153728023605265
+  a: 1.3579883421084902
   relative_time: 2.6999999999999984
 }
 trajectory_point {
   path_point {
-    x: 435706.34807085554
-    y: 4435950.7949205171
-    theta: 1.9486132560285676
-    kappa: 0.0061413428426990834
-    s: 2.5746633224546378
+    x: 435706.34962690028
+    y: 4435950.7930263253
+    theta: 1.9483591787550876
+    kappa: 0.0065230271976128469
+    s: 2.5723326996254361
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.4856789312121128
-  a: 1.3634642960303127
+  v: 2.4833780605538349
+  a: 1.3621312625720738
   relative_time: 2.7499999999999982
 }
 trajectory_point {
   path_point {
-    x: 435706.30169265572
-    y: 4435950.9120635977
-    theta: 1.9493748953453904
-    kappa: 0.0062735001080966822
-    s: 2.7006531584258511
+    x: 435706.30333573185
+    y: 4435950.9100783262
+    theta: 1.9491613030605153
+    kappa: 0.006697310052045706
+    s: 2.69820582284248
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.5539445533231149
-  a: 1.3670697784302133
+  v: 2.5515768563125558
+  a: 1.3657298447250086
   relative_time: 2.799999999999998
 }
 trajectory_point {
   path_point {
-    x: 435706.25389875827
-    y: 4435951.0323214438
-    theta: 1.9501760841445837
-    kappa: 0.0063834531933256861
-    s: 2.8300605553306659
+    x: 435706.25557184644
+    y: 4435951.03021882
+    theta: 1.9500183059586751
+    kappa: 0.0068341643463643827
+    s: 2.8274931572675222
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.6223768280571766
-  a: 1.3701304009526736
+  v: 2.6199419727211577
+  a: 1.3687840885672946
   relative_time: 2.8499999999999979
 }
 trajectory_point {
   path_point {
-    x: 435706.20445637387
-    y: 4435951.1556095323
-    theta: 1.9510444280936179
-    kappa: 0.0064335919033032974
-    s: 2.9628931647253882
+    x: 435706.20618578681
+    y: 4435951.1533965133
+    theta: 1.9509526141208604
+    kappa: 0.0069051253879176318
+    s: 2.9602023385101672
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.6909485124204249
-  a: 1.3726461635976939
+  v: 2.6884461928641077
+  a: 1.3712939940989322
   relative_time: 2.8999999999999977
 }
 trajectory_point {
   path_point {
-    x: 435706.15373673057
-    y: 4435951.2820825558
-    theta: 1.9519352060239135
-    kappa: 0.0064850258605787022
-    s: 3.099157276016629
+    x: 435706.15552399459
+    y: 4435951.2797561111
+    theta: 1.9519110592955422
+    kappa: 0.0069779194833388342
+    s: 3.0963396413342457
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.7596323634189881
-  a: 1.3746170663652739
+  v: 2.7570622998258729
+  a: 1.3732595613199206
   relative_time: 2.9499999999999975
 }
 trajectory_point {
   path_point {
-    x: 435706.10173799447
-    y: 4435951.4117450891
-    theta: 1.9528484488166229
-    kappa: 0.0065377569249836587
-    s: 3.2388578164613095
+    x: 435706.1035846413
+    y: 4435951.409302176
+    theta: 1.9528936744122483
+    kappa: 0.0070525492601533771
+    s: 3.2359099796578086
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.828401138058994
-  a: 1.3760431092554135
+  v: 2.8257630766909214
+  a: 1.3746807902302605
   relative_time: 2.9999999999999973
 }
 trajectory_point {
   path_point {
-    x: 435706.04845883825
-    y: 4435951.54460044
-    theta: 1.9537841782298337
-    kappa: 0.006591786442195843
-    s: 3.3819983511666543
+    x: 435706.05036640458
+    y: 4435951.5420380048
+    theta: 1.9539004825473332
+    kappa: 0.0071290166182276246
+    s: 3.3789169065531324
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.8972275933465719
-  a: 1.3769242922681131
+  v: 2.8945213065437212
+  a: 1.3755576808299517
   relative_time: 3.0499999999999972
 }
 trajectory_point {
   path_point {
-    x: 435705.993732455
-    y: 4435951.6805836428
-    theta: 1.9547445386821587
-    kappa: 0.0066245418510478852
-    s: 3.5285810830901942
+    x: 435705.99564271572
+    y: 4435951.6778744673
+    theta: 1.9549371663029325
+    kappa: 0.0071720503481861927
+    s: 3.5253626142467169
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.9660844862878477
-  a: 1.3772606154033726
+  v: 2.9633097724687385
+  a: 1.3758902331189939
   relative_time: 3.099999999999997
 }
 trajectory_point {
   path_point {
-    x: 435705.93724979245
-    y: 4435951.8195708375
-    theta: 1.9557335044050095
-    kappa: 0.00659403656874185
-    s: 3.678606853039768
+    x: 435705.93917082954
+    y: 4435951.8167144293
+    theta: 1.9560098247554905
+    kappa: 0.0071437719449338952
+    s: 3.675247934119283
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.0349445738889509
-  a: 1.3770520786611917
+  v: 3.0321012575504422
+  a: 1.3756784470973877
   relative_time: 3.1499999999999968
 }
 trajectory_point {
   path_point {
-    x: 435705.87947106932
-    y: 4435951.961747257
-    theta: 1.9567451652243613
-    kappa: 0.0065628313070700741
-    s: 3.8320751396735204
+    x: 435705.88140320947
+    y: 4435951.9587400416
+    theta: 1.9571070976558025
+    kappa: 0.0071148447005016527
+    s: 3.8285723367057782
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.1037806131560091
-  a: 1.3762986820415706
+  v: 3.1008685448732995
+  a: 1.3749223227651326
   relative_time: 3.1999999999999966
 }
 trajectory_point {
   path_point {
-    x: 435705.82039699436
-    y: 4435952.1071111551
-    theta: 1.9577795067276995
-    kappa: 0.0065309264490097056
-    s: 3.9889840594999022
+    x: 435705.82234056771
+    y: 4435952.1039495524
+    theta: 1.9582289689179815
+    kappa: 0.007085268971528608
+    s: 3.9853339316953722
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.17256536109515
-  a: 1.3750004255445094
+  v: 3.1695844175217776
+  a: 1.3736218601222288
   relative_time: 3.2499999999999964
 }
 trajectory_point {
   path_point {
-    x: 435705.76002878987
-    y: 4435952.2556595244
-    theta: 1.9588365052385195
-    kappa: 0.0064983226545087119
-    s: 4.1493303668776713
+    x: 435705.7619841292
+    y: 4435952.25233995
+    theta: 1.959375412353703
+    kappa: 0.0070550453714005
+    s: 4.1455294679314569
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.2412715747125018
-  a: 1.373157309170008
+  v: 3.2382216585803443
+  a: 1.371777059168676
   relative_time: 3.2999999999999963
 }
 trajectory_point {
   path_point {
-    x: 435705.6978449715
-    y: 4435952.4071740909
-    theta: 1.9598764040187888
-    kappa: 0.0064036162769545192
-    s: 4.3131094540158905
+    x: 435705.6997323627
+    y: 4435952.4036598625
+    theta: 1.9604992472718246
+    kappa: 0.006943776784357778
+    s: 4.309154333411648
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.3098720110141935
-  a: 1.3707693329180664
+  v: 3.3067530511334677
+  a: 1.3693879199044747
   relative_time: 3.3499999999999961
 }
 trajectory_point {
   path_point {
-    x: 435705.63419572823
-    y: 4435952.5617916379
-    theta: 1.96092558656721
-    kappa: 0.006287642669129046
-    s: 4.4803153509739309
+    x: 435705.63603557838
+    y: 4435952.5580872828
+    theta: 1.9616354546128254
+    kappa: 0.0068111775276827775
+    s: 4.4762025552877862
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.3783394270063507
-  a: 1.3678364967886847
+  v: 3.3751513782656142
+  a: 1.3664544423296245
   relative_time: 3.3999999999999959
 }
 trajectory_point {
   path_point {
-    x: 435705.56924481341
-    y: 4435952.7195712216
-    theta: 1.9619962264245498
-    kappa: 0.0061692973197778039
-    s: 4.6509407256614681
+    x: 435705.57103623811
+    y: 4435952.7156726383
+    theta: 1.9627948975114948
+    kappa: 0.0066758667062512995
+    s: 4.6466667998659341
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.4466465796951038
-  a: 1.3643588007818626
+  v: 3.4433894230612516
+  a: 1.3629766264441259
   relative_time: 3.4499999999999957
 }
 trajectory_point {
   path_point {
-    x: 435705.50299553649
-    y: 4435952.8805048037
-    theta: 1.9630882665556428
-    kappa: 0.0060485862592047513
-    s: 4.8249768838384854
+    x: 435705.50473765715
+    y: 4435952.8764078924
+    theta: 1.9639775136729725
+    kappa: 0.0065378512216004618
+    s: 4.820538372606376
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.5147662260865795
-  a: 1.3603362448976002
+  v: 3.511439968604849
+  a: 1.3589544722479783
   relative_time: 3.4999999999999956
 }
 trajectory_point {
   path_point {
-    x: 435705.43500284757
-    y: 4435953.0443972023
-    theta: 1.9641284172939895
-    kappa: 0.0058790523037533151
-    s: 5.0024137691152717
+    x: 435705.43663639238
+    y: 4435953.0400731666
+    theta: 1.965094055914087
+    kappa: 0.0063363825339110245
+    s: 4.9978072181236239
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.5826711231869059
-  a: 1.3557688291358978
+  v: 3.5792757979808716
+  a: 1.354387979741182
   relative_time: 3.5499999999999954
 }
 trajectory_point {
   path_point {
-    x: 435705.36547096114
-    y: 4435953.2113206256
-    theta: 1.9651492160778079
-    kappa: 0.0056813920407189642
-    s: 5.183239962952424
+    x: 435705.36699518305
+    y: 4435953.2067651674
+    theta: 1.9661898758244345
+    kappa: 0.0061024165789601336
+    s: 5.17846192018641
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.6503340280022112
-  a: 1.3506565534967552
+  v: 3.6468696942737884
+  a: 1.349277148923737
   relative_time: 3.5999999999999952
 }
 trajectory_point {
   path_point {
-    x: 435705.294640721
-    y: 4435953.3813609732
-    theta: 1.9661890766293966
-    kappa: 0.0054800409109301059
-    s: 5.3674426846608432
+    x: 435705.29605367361
+    y: 4435953.3765695449
+    theta: 1.9673061569559305
+    kappa: 0.0058640821478502482
+    s: 5.3624897017176911
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.7177276975386238
-  a: 1.3449994179801723
+  v: 3.7141944405680674
+  a: 1.343621979795643
   relative_time: 3.649999999999995
 }
 trajectory_point {
   path_point {
-    x: 435705.22251756542
-    y: 4435953.55450519
-    theta: 1.9672479168330521
-    kappa: 0.0052750143738573866
-    s: 5.5550077914017386
+    x: 435705.22381731408
+    y: 4435953.549473254
+    theta: 1.968442810731126
+    kappa: 0.0056213975506091514
+    s: 5.5498764247946459
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.7848248888022709
-  a: 1.3387974225861494
+  v: 3.7812228199481748
+  a: 1.3374224723569006
   relative_time: 3.6999999999999948
 }
 trajectory_point {
   path_point {
-    x: 435705.14856426854
-    y: 4435953.730511534
-    theta: 1.9681840405349456
-    kappa: 0.0050164189686048672
-    s: 5.7459197781866251
+    x: 435705.14970509644
+    y: 4435953.7252154155
+    theta: 1.9694282195614907
+    kappa: 0.005311035620510828
+    s: 5.740606590648679
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.851598358799281
-  a: 1.3320505673146861
+  v: 3.8479276154985795
+  a: 1.3306786266075092
   relative_time: 3.7499999999999947
 }
 trajectory_point {
   path_point {
-    x: 435705.07322024519
-    y: 4435953.9095457
-    theta: 1.9691102197194041
-    kappa: 0.0047440524780027523
-    s: 5.9401617778773241
+    x: 435705.07420001767
+    y: 4435953.9039806379
+    theta: 1.9704015233625549
+    kappa: 0.0049844418870423389
+    s: 5.9346633396654154
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9180208645357819
-  a: 1.3247588521657827
+  v: 3.9142816103037479
+  a: 1.3233904425474692
   relative_time: 3.7999999999999945
 }
 trajectory_point {
   path_point {
-    x: 435704.99659162259
-    y: 4435954.09163236
-    theta: 1.9700521899927113
-    kappa: 0.0044670421982969007
-    s: 6.1377155611859626
+    x: 435704.99740769592
+    y: 4435954.0857935268
+    theta: 1.9713914203855099
+    kappa: 0.0046522802435054311
+    s: 6.1320284513847056
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9840651630179016
-  a: 1.3169222771394389
+  v: 3.9802575874481487
+  a: 1.3155579201767804
   relative_time: 3.8499999999999943
 }
 trajectory_point {
   path_point {
-    x: 435704.91853361134
-    y: 4435954.2766890563
-    theta: 1.9709508022897606
-    kappa: 0.0041731796035231706
-    s: 6.338561536674975
+    x: 435704.91918139753
+    y: 4435954.2705705706
+    theta: 1.9723279150087309
+    kappa: 0.0042997768862966142
+    s: 6.3326823445006237
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0497040112517677
-  a: 1.308540842235655
+  v: 4.0458283300162483
+  a: 1.3071810594954429
   relative_time: 3.8999999999999941
 }
 trajectory_point {
   path_point {
-    x: 435704.83880036423
-    y: 4435954.4645891543
-    theta: 1.9717075373364474
-    kappa: 0.0038421012588733859
-    s: 6.5426787507571023
+    x: 435704.83926166024
+    y: 4435954.4581789626
+    theta: 1.9730897217061976
+    kappa: 0.0039012956694111423
+    s: 6.5366040768614653
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.11491016624351
-  a: 1.2996145474544312
+  v: 4.1109666210925147
+  a: 1.2982598605034565
   relative_time: 3.949999999999994
 }
 trajectory_point {
   path_point {
-    x: 435704.75779800711
-    y: 4435954.6554800458
-    theta: 1.9724763176789986
-    kappa: 0.0035057531577949246
-    s: 6.7500448876953909
+    x: 435704.75806995248
+    y: 4435954.648773253
+    theta: 1.9738636534153811
+    kappa: 0.0034964723858005855
+    s: 6.74377134546975
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1796563849992534
-  a: 1.2901433927957668
+  v: 4.1756452437614158
+  a: 1.2887943232008217
   relative_time: 3.9999999999999938
 }
 trajectory_point {
   path_point {
-    x: 435704.67553572555
-    y: 4435954.84934009
-    theta: 1.9732570552291744
-    kappa: 0.0031641734407093559
-    s: 6.9606364330657771
+    x: 435704.67561548459
+    y: 4435954.842331823
+    theta: 1.9746496214141496
+    kappa: 0.0030853529573117416
+    s: 6.9541606499391238
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2439259105629628
-  a: 1.2806010825705481
+  v: 4.2398474663989267
+  a: 1.2792580896134691
   relative_time: 4.0499999999999936
 }
 trajectory_point {
   path_point {
-    x: 435704.59163554892
-    y: 4435955.0459823757
-    theta: 1.97382114591779
-    kappa: 0.0027922501181535551
-    s: 7.1744294354586007
+    x: 435704.59153733304
+    y: 4435955.0386744384
+    theta: 1.9751765591348882
+    kappa: 0.0026388320377546738
+    s: 7.1677480540166609
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3077128385396195
-  a: 1.2708394893258226
+  v: 4.303567403455669
+  a: 1.2695028999822655
   relative_time: 4.0999999999999934
 }
 trajectory_point {
   path_point {
-    x: 435704.50644627243
-    y: 4435955.2455288423
-    theta: 1.9743686582885946
-    kappa: 0.0024120525851054177
-    s: 7.3913994908907759
+    x: 435704.50616096694
+    y: 4435955.2379136346
+    theta: 1.975675549999848
+    kappa: 0.0021818273036088636
+    s: 7.3845091697283118
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3710062047785536
-  a: 1.2608586130616601
+  v: 4.3667941071297562
+  a: 1.25952875430728
   relative_time: 4.1499999999999932
 }
 trajectory_point {
   path_point {
-    x: 435704.4200193818
-    y: 4435955.4479742823
-    theta: 1.9749241247224203
-    kappa: 0.0020263316125936895
-    s: 7.6115216471716423
+    x: 435704.41954438266
+    y: 4435955.440047076
+    theta: 1.9761817893799263
+    kappa: 0.0017181838987891302
+    s: 7.6044190617098879
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.433795045128794
-  a: 1.2506584537780603
+  v: 4.429516629619
+  a: 1.2493356525885126
   relative_time: 4.1999999999999931
 }
 trajectory_point {
   path_point {
-    x: 435704.33217591187
-    y: 4435955.6532144183
-    theta: 1.97529497202539
-    kappa: 0.0016287426337356062
-    s: 7.8347704039029926
+    x: 435704.33154945966
+    y: 4435955.6449877564
+    theta: 1.9764743299056173
+    kappa: 0.0012444417040219113
+    s: 7.8274522472070931
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4960683954393685
-  a: 1.2402390114750235
+  v: 4.49172402312121
+  a: 1.2389235948259636
   relative_time: 4.2499999999999929
 }
 trajectory_point {
   path_point {
-    x: 435704.24302769336
-    y: 4435955.8612687495
-    theta: 1.9755846364405454
-    kappa: 0.0012227664548927289
-    s: 8.0611197124790674
+    x: 435704.24225679995
+    y: 4435955.85274194
+    theta: 1.9766579959176183
+    kappa: 0.00076232407153046155
+    s: 8.05358269607552
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5578152915593053
-  a: 1.2296002861525492
+  v: 4.5534053398342
+  a: 1.2282925810196326
   relative_time: 4.2999999999999927
 }
 trajectory_point {
   path_point {
-    x: 435704.15266879025
-    y: 4435956.0721485792
-    theta: 1.9758782346594406
-    kappa: 0.000811276883826827
-    s: 8.2905429760865612
+    x: 435704.15175161127
+    y: 4435956.0633172728
+    theta: 1.9768441559780934
+    kappa: 0.00027365963478011323
+    s: 8.2827838307806534
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6190247693376323
-  a: 1.218742277810638
+  v: 4.6145496319557768
+  a: 1.21744261116952
   relative_time: 4.3499999999999925
 }
 trajectory_point {
   path_point {
-    x: 435704.06104877195
-    y: 4435956.28580275
-    theta: 1.9759963735646451
-    kappa: 0.00040107681801886737
-    s: 8.52301304970462
+    x: 435704.060038052
+    y: 4435956.2766860183
+    theta: 1.9768344172028922
+    kappa: -0.00020699518039476485
+    s: 8.51502852639787
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6796858646233783
-  a: 1.2076649864492897
+  v: 4.6751459516837546
+  a: 1.2063736852756253
   relative_time: 4.3999999999999924
 }
 trajectory_point {
   path_point {
-    x: 435703.96820112818
-    y: 4435956.50221549
-    theta: 1.9760052853423413
-    kappa: -1.0279552993717198e-05
-    s: 8.75850224010484
+    x: 435703.9671287972
+    y: 4435956.4928235048
+    theta: 1.9766816818555133
+    kappa: -0.00068344955641444027
+    s: 8.7502891106124316
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7397876132655705
-  a: 1.1963684120685041
+  v: 4.7351833512159427
+  a: 1.195085803337949
   relative_time: 4.4499999999999922
 }
 trajectory_point {
   path_point {
-    x: 435703.8741742553
-    y: 4435956.72137682
-    theta: 1.9760143103057688
-    kappa: -0.00042686043399962936
-    s: 8.9969823058512652
+    x: 435703.8730396505
+    y: 4435956.7117058076
+    theta: 1.976527006861138
+    kappa: -0.001165954618411469
+    s: 8.9885373637194963
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7993190511132378
-  a: 1.1848525546682813
+  v: 4.7946508827501528
+  a: 1.183578965356491
   relative_time: 4.499999999999992
 }
 trajectory_point {
   path_point {
-    x: 435703.77903735085
-    y: 4435956.943285089
-    theta: 1.9758334588623658
-    kappa: -0.0008267555161085593
-    s: 9.2384244573004022
+    x: 435703.77791143861
+    y: 4435956.93336208
+    theta: 1.9761678731532126
+    kappa: -0.001618017352465246
+    s: 9.22974451862411
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8582692140154071
-  a: 1.1731174142486216
+  v: 4.8535375984841949
+  a: 1.171853171331251
   relative_time: 4.5499999999999918
 }
 trajectory_point {
   path_point {
-    x: 435703.68277396192
-    y: 4435957.16790132
-    theta: 1.9755547584848852
-    kappa: -0.001220502334622342
-    s: 9.4827993566011983
+    x: 435703.68170650763
+    y: 4435957.157744254
+    theta: 1.9756813166858005
+    kappa: -0.0020534330792362668
+    s: 9.4738812608412069
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.9166271378211084
-  a: 1.1611629908095245
+  v: 4.9118325506158813
+  a: 1.1599084212622293
   relative_time: 4.5999999999999917
 }
 trajectory_point {
   path_point {
-    x: 435703.58536708681
-    y: 4435957.3951857062
-    theta: 1.97527274751309
-    kappa: -0.0016189263626157913
-    s: 9.7300771176950533
+    x: 435703.584358906
+    y: 4435957.3847915195
+    theta: 1.9751889812056518
+    kappa: -0.0024940204407528769
+    s: 9.72091772849562
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.9743818583793686
-  a: 1.1489892843509903
+  v: 4.9695247913430212
+  a: 1.1477447151494256
   relative_time: 4.6499999999999915
 }
 trajectory_point {
   path_point {
-    x: 435703.48704954423
-    y: 4435957.6252047587
-    theta: 1.97477245226384
-    kappa: -0.00197596530691109
-    s: 9.9802273063158253
+    x: 435703.48619863985
+    y: 4435957.6146119442
+    theta: 1.9744753107171809
+    kappa: -0.0028675838988255565
+    s: 9.9708235123220632
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.0315224115392168
-  a: 1.1365962948730191
+  v: 5.026603372863427
+  a: 1.1353620529928403
   relative_time: 4.6999999999999913
 }
 trajectory_point {
   path_point {
-    x: 435703.38766509888
-    y: 4435957.8578579314
-    theta: 1.9742179071853989
-    kappa: -0.0023266662079664572
-    s: 10.233218939989817
+    x: 435703.38701951696
+    y: 4435957.84708367
+    theta: 1.9736884333872184
+    kappa: -0.0032236087090816265
+    s: 10.223567655665144
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.08803783314968
-  a: 1.1239840223756106
+  v: 5.0830573473749086
+  a: 1.1227604347924731
   relative_time: 4.7499999999999911
 }
 trajectory_point {
   path_point {
-    x: 435703.28720189404
-    y: 4435958.09310581
-    theta: 1.9736444362627887
-    kappa: -0.0026766221687941931
-    s: 10.489020488035786
+    x: 435703.28674287657
+    y: 4435958.0821387814
+    theta: 1.9728914198341447
+    kappa: -0.0035827153261812805
+    s: 10.479118654479366
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.1439171590597867
-  a: 1.1111524668587649
+  v: 5.1388757670752776
+  a: 1.1099398605483239
   relative_time: 4.7999999999999909
 }
 trajectory_point {
   path_point {
-    x: 435703.18606120552
-    y: 4435958.3310845047
-    theta: 1.9728542453789413
-    kappa: -0.002953875177012448
-    s: 10.747599871564939
+    x: 435703.18596415303
+    y: 4435958.3199956072
+    theta: 1.9718764238810365
+    kappa: -0.003815140624144165
+    s: 10.737444457329115
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.1991494251185655
-  a: 1.098101628322482
+  v: 5.194047684162344
+  a: 1.0969003302603932
   relative_time: 4.8499999999999908
 }
 trajectory_point {
   path_point {
-    x: 435703.08384675672
-    y: 4435958.571589699
-    theta: 1.9720556651419485
-    kappa: -0.0032340716421801717
-    s: 11.008924463480934
+    x: 435703.08411563339
+    y: 4435958.5603773538
+    theta: 1.9708506527166012
+    kappa: -0.0040500331857560336
+    s: 10.998512465388675
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.253723667175044
-  a: 1.084831506766762
+  v: 5.24856215083392
+  a: 1.0836418439286806
   relative_time: 4.8999999999999906
 }
 trajectory_point {
   path_point {
-    x: 435702.98078996636
-    y: 4435958.8146833954
-    theta: 1.9711871879154608
-    kappa: -0.0034766753040877786
-    s: 11.272961088479885
+    x: 435702.98144157429
+    y: 4435958.8033511145
+    theta: 1.9697786530544781
+    kappa: -0.0042325082907626677
+    s: 11.26228953244221
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.30762892107825
-  a: 1.071342102191605
+  v: 5.3024082192878152
+  a: 1.0701644015531862
   relative_time: 4.94999999999999
 }
 trajectory_point {
   path_point {
-    x: 435702.87704794412
-    y: 4435959.060395522
-    theta: 1.9702083242307979
-    kappa: -0.003654966459516277
-    s: 11.539676023050355
+    x: 435702.87820847309
+    y: 4435959.0489927754
+    theta: 1.9686215809604142
+    kappa: -0.0043024573226601026
+    s: 11.528741964883789
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.3608542226772133
-  a: 1.0576334145970105
+  v: 5.3555749417218417
+  a: 1.05646800313391
   relative_time: 4.99999999999999
 }
 trajectory_point {
   path_point {
-    x: 435702.77227749117
-    y: 4435959.3085434791
-    theta: 1.9692197568641088
-    kappa: -0.0038350250771398832
-    s: 11.809034995473352
+    x: 435702.77395210677
+    y: 4435959.29706928
+    theta: 1.9674530398078884
+    kappa: -0.00437309970193158
+    s: 11.79783552171736
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.413388607820961
-  a: 1.0437054439829792
+  v: 5.40805137033381
+  a: 1.0425526486708518
   relative_time: 5.04999999999999
 }
 trajectory_point {
   path_point {
-    x: 435702.66701229603
-    y: 4435959.5593139743
-    theta: 1.9681590728794534
-    kappa: -0.0039128092123998414
-    s: 12.081003185822345
+    x: 435702.66923058534
+    y: 4435959.5477764737
+    theta: 1.9662874749272907
+    kappa: -0.0042967564096630193
+    s: 12.069535414556764
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.46522111235852
-  a: 1.0295581903495106
+  v: 5.4598265573215308
+  a: 1.0284183381640122
   relative_time: 5.09999999999999
 }
 trajectory_point {
   path_point {
-    x: 435702.56090564164
-    y: 4435959.8125228006
-    theta: 1.9670697430732078
-    kappa: -0.0039603828645483653
-    s: 12.355545225963251
+    x: 435702.56373578508
+    y: 4435959.8009471074
+    theta: 1.9651165915677795
+    kappa: -0.0041606738860000755
+    s: 12.343806307625737
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.51634077213892
-  a: 1.0151916536966048
+  v: 5.5108895548828141
+  a: 1.0140650716133903
   relative_time: 5.14999999999999
 }
 trajectory_point {
   path_point {
-    x: 435702.45398015651
-    y: 4435960.0681399917
-    theta: 1.9659753563130091
-    kappa: -0.0039711863996365693
-    s: 12.632625199554433
+    x: 435702.45738759334
+    y: 4435960.0565083418
+    theta: 1.9639633782765149
+    kappa: -0.0039928620611366468
+    s: 12.6206123177579
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.56673662301119
-  a: 1.0006058340242618
+  v: 5.5612294152154726
+  a: 0.99949284901898694
   relative_time: 5.1999999999999895
 }
 trajectory_point {
   path_point {
-    x: 435702.34663921886
-    y: 4435960.3262944147
-    theta: 1.9648880958135937
-    kappa: -0.0038558232310439882
-    s: 12.912206642046716
+    x: 435702.35068015556
+    y: 4435960.3146258276
+    theta: 1.9629403854470919
+    kappa: -0.0036731313086500734
+    s: 12.899917014396769
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.616397700824356
-  a: 0.98580073133248169
+  v: 5.6108351905173164
+  a: 0.98470167038080159
   relative_time: 5.2499999999999893
 }
 trajectory_point {
   path_point {
-    x: 435702.23835209169
-    y: 4435960.5867244191
-    theta: 1.96379125127631
-    kappa: -0.0037394431585140889
-    s: 13.194252540683365
+    x: 435702.2430322303
+    y: 4435960.575018283
+    theta: 1.9619083760893203
+    kappa: -0.00335058254436564
+    s: 13.181683419595746
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.6653130414274475
-  a: 0.97077634562126447
+  v: 5.659695932986156
+  a: 0.96969153569883448
   relative_time: 5.2999999999999892
 }
 trajectory_point {
   path_point {
-    x: 435702.12963489862
-    y: 4435960.8496031752
-    theta: 1.962794005471185
-    kappa: -0.0034790759208878168
-    s: 13.478725334500107
+    x: 435702.13486122066
+    y: 4435960.83781713
+    theta: 1.9610414630516351
+    kappa: -0.0029690709642933044
+    s: 13.46587400801813
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.7134716806694907
-  a: 0.95553267689061
+  v: 5.7078006948198032
+  a: 0.95446244497308563
   relative_time: 5.349999999999989
 }
 trajectory_point {
   path_point {
-    x: 435702.0201791302
-    y: 4435961.1147616208
-    theta: 1.9618262763138794
-    kappa: -0.0031668327139274927
-    s: 13.765586914325112
+    x: 435702.02597307379
+    y: 4435961.1029012222
+    theta: 1.9602497478052952
+    kappa: -0.0025577189158558361
+    s: 13.7524507069371
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.7608626543995172
-  a: 0.94006972514051856
+  v: 5.7551385282160679
+  a: 0.939014398203555
   relative_time: 5.3999999999999888
 }
 trajectory_point {
   path_point {
-    x: 435701.90999667108
-    y: 4435961.3821623735
-    theta: 1.9609298159879918
-    kappa: -0.0028362102494801584
-    s: 14.054798622779007
+    x: 435701.91629423335
+    y: 4435961.3701982507
+    theta: 1.9595228425584628
+    kappa: -0.0021469714586509667
+    s: 14.041374896235741
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.8074749984665521
-  a: 0.92438749037098977
+  v: 5.8016984853727616
+  a: 0.92334739539024246
   relative_time: 5.4499999999999886
 }
 trajectory_point {
   path_point {
-    x: 435701.799314843
-    y: 4435961.6518566031
-    theta: 1.9602036688557822
-    kappa: -0.0024674911707515154
-    s: 14.346321254274866
+    x: 435701.80606023036
+    y: 4435961.6397624183
+    theta: 1.9590162046925947
+    kappa: -0.0017455444256875357
+    s: 14.332607408407014
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.8532977487196254
-  a: 0.90848597258202379
+  v: 5.8474696184876951
+  a: 0.90746143653314815
   relative_time: 5.4999999999999885
 }
 trajectory_point {
   path_point {
-    x: 435701.68777072465
-    y: 4435961.9236519439
-    theta: 1.9594718646074512
-    kappa: -0.0020958995073858738
-    s: 14.640115055018217
+    x: 435701.69496753969
+    y: 4435961.9114264054
+    theta: 1.9585056202909423
+    kappa: -0.0013409904047317004
+    s: 14.626108528553775
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.8983199410077631
-  a: 0.89236517177362085
+  v: 5.89244097975868
+  a: 0.89135652163227208
   relative_time: 5.5499999999999883
 }
 trajectory_point {
   path_point {
-    x: 435701.57574534946
-    y: 4435962.1976608969
-    theta: 1.9589498609624894
-    kappa: -0.0017944523809928802
-    s: 14.936139723007042
+    x: 435701.58324306528
+    y: 4435962.1852393877
+    theta: 1.9582005073967852
+    kappa: -0.0010257512652604968
+    s: 14.921837994388778
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.942530611179996
-  a: 0.87602508794578071
+  v: 5.9366016213835255
+  a: 0.875032650687614
   relative_time: 5.5999999999999881
 }
 trajectory_point {
   path_point {
-    x: 435701.46292185382
-    y: 4435962.473709547
-    theta: 1.9584420452456492
-    kappa: -0.0014968906645174145
-    s: 15.234354408031766
+    x: 435701.47073024686
+    y: 4435962.4610933457
+    theta: 1.9579308121069496
+    kappa: -0.000724806264943784
+    s: 15.219754996234652
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.98591879508535
-  a: 0.85946572109850328
+  v: 5.9799405955600449
+  a: 0.85848982369917437
   relative_time: 5.6499999999999879
 }
 trajectory_point {
   path_point {
-    x: 435701.34944373573
-    y: 4435962.7518117037
-    theta: 1.9580367119977391
-    kappa: -0.0012383010916209537
-    s: 15.534717711675277
+    x: 435701.35746410175
+    y: 4435962.7389578605
+    theta: 1.9577474605729719
+    kappa: -0.00047007807525657861
+    s: 15.519818177023936
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.0284735285728557
-  a: 0.84268707123178865
+  v: 6.0224469544860462
+  a: 0.84172804066695273
   relative_time: 5.6999999999999877
 }
 trajectory_point {
   path_point {
-    x: 435701.235285164
-    y: 4435963.0319115194
-    theta: 1.9577059655539246
-    kappa: -0.0010078919109887418
-    s: 15.837187687312905
+    x: 435701.24346161634
+    y: 4435963.0187946064
+    theta: 1.95765228196151
+    kappa: -0.00026259070597691884
+    s: 15.821985632299043
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.0701838474915384
-  a: 0.82568913834563706
+  v: 6.0641097503593429
+  a: 0.82474730159094944
   relative_time: 5.7499999999999876
 }
 trajectory_point {
   path_point {
-    x: 435701.12039288622
-    y: 4435963.3139413195
-    theta: 1.9574111173844295
-    kappa: -0.0007938323732685354
-    s: 16.141721840112432
+    x: 435701.1286852315
+    y: 4435963.3005424282
+    theta: 1.9575779171796661
+    kappa: -6.9678292661445472e-05
+    s: 16.126214910212287
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.1110387876904282
-  a: 0.80847192244004829
+  v: 6.1049180353777448
+  a: 0.80754760647116419
   relative_time: 5.7999999999999874
 }
 trajectory_point {
   path_point {
-    x: 435701.00485882122
-    y: 4435963.5978920618
-    theta: 1.957215871520896
-    kappa: -0.00062605571737884083
-    s: 16.4482771270341
+    x: 435701.01316477556
+    y: 4435963.5841670465
+    theta: 1.9575975638472081
+    kappa: 5.4105475214486615e-05
+    s: 16.432463011525865
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.1510273850185531
-  a: 0.79103542351502221
+  v: 6.1448608617390628
+  a: 0.7901289553075973
   relative_time: 5.8499999999999872
 }
 trajectory_point {
   path_point {
-    x: 435700.88857977575
-    y: 4435963.8836746551
-    theta: 1.9570197012493635
-    kappa: -0.00045739900379605878
-    s: 16.75680995683059
+    x: 435700.89689922146
+    y: 4435963.8696210207
+    theta: 1.9576173372342485
+    kappa: 0.00017868763890231136
+    s: 16.74068638961187
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.19013867532494
-  a: 0.773379641570559
+  v: 6.1839272816411075
+  a: 0.77249134810024844
   relative_time: 5.899999999999987
 }
 trajectory_point {
   path_point {
-    x: 435700.77166283608
-    y: 4435964.1712850016
-    theta: 1.9569195626242153
-    kappa: -0.00034638720493231697
-    s: 17.067276190047046
+    x: 435700.77987321187
+    y: 4435964.1568504842
+    theta: 1.9576922549629576
+    kappa: 0.0002358638364004473
+    s: 17.050840950452283
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.2283616944586182
-  a: 0.75550457660665871
+  v: 6.2221063472816915
+  a: 0.754634784849118
   relative_time: 5.9499999999999869
 }
 trajectory_point {
   path_point {
-    x: 435700.65403463424
-    y: 4435964.46064502
-    theta: 1.9568188148072991
-    kappa: -0.0002347000677826175
-    s: 17.379631139021054
+    x: 435700.6621321541
+    y: 4435964.4458257295
+    theta: 1.9577731796079234
+    kappa: 0.00028650821353832151
+    s: 17.362882052638977
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.2656854782686153
-  a: 0.73741022862332117
+  v: 6.2593871108586239
+  a: 0.73655926555420548
   relative_time: 5.9999999999999867
 }
 trajectory_point {
   path_point {
-    x: 435700.53575057187
-    y: 4435964.7517286111
-    theta: 1.9567634315658504
-    kappa: -0.00016153921407861948
-    s: 17.693829765734563
+    x: 435700.54365669593
+    y: 4435964.7364902869
+    theta: 1.9578633186043932
+    kappa: 0.00029582196810342608
+    s: 17.676764705239719
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.3021113218570219
-  a: 0.71961801878392717
+  v: 6.2957708842162
+  a: 0.71878619090098717
   relative_time: 6.0499999999999865
 }
 trajectory_point {
   path_point {
-    x: 435700.41680108604
-    y: 4435965.0444837036
-    theta: 1.9567218953656764
-    kappa: -0.000100036020778734
-    s: 18.009827430624906
+    x: 435700.42448287213
+    y: 4435965.0288106874
+    theta: 1.957958415551774
+    kappa: 0.00028402393935081897
+    s: 17.992444316470436
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.3376467305354272
-  a: 0.70179283223367628
+  v: 6.3312651821652519
+  a: 0.70098024922360713
   relative_time: 6.0999999999999863
 }
 trajectory_point {
   path_point {
-    x: 435700.297198753
-    y: 4435965.3388673048
-    theta: 1.9566952096423782
-    kappa: -6.2913970244351142e-05
-    s: 18.327579570725682
+    x: 435700.30462275079
+    y: 4435965.3227437753
+    theta: 1.9580420640319429
+    kappa: 0.00024956699297524527
+    s: 18.309876371476918
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.3722900554685289
-  a: 0.683934668971787
+  v: 6.3658683613548117
+  a: 0.68314144052128378
   relative_time: 6.1499999999999861
 }
 trajectory_point {
   path_point {
-    x: 435700.17695892637
-    y: 4435965.6348374616
-    theta: 1.9566804915730949
-    kappa: -4.5446285269844468e-05
-    s: 18.647041540628742
+    x: 435700.184087176
+    y: 4435965.6182458084
+    theta: 1.9581113480742425
+    kappa: 0.00018697716529164832
+    s: 18.629016273237404
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.4060396478207444
-  a: 0.66604352899825947
+  v: 6.3995787784336313
+  a: 0.665269764794017
   relative_time: 6.199999999999986
 }
 trajectory_point {
   path_point {
-    x: 435700.0560945266
-    y: 4435965.9323511478
-    theta: 1.9566651809890665
-    kappa: -4.1582035570451657e-05
-    s: 18.968168612484149
+    x: 435700.062916368
+    y: 4435965.9152849382
+    theta: 1.9581659520238945
+    kappa: 0.00011433354355451636
+    s: 18.949819342562577
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.4388938587564937
-  a: 0.64811941231309356
+  v: 6.4323947900504637
+  a: 0.647365222041807
   relative_time: 6.2499999999999858
 }
 trajectory_point {
   path_point {
-    x: 435699.93462391553
-    y: 4435966.2313674539
-    theta: 1.9566489196614729
-    kappa: -6.0889430709085896e-05
-    s: 19.290915976000189
+    x: 435699.94111391838
+    y: 4435966.2138142856
+    theta: 1.9581775537835706
+    kappa: 1.33343584247684e-05
+    s: 19.272240818095561
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.470851039440193
-  a: 0.63016231891628938
+  v: 6.4643147528540625
+  a: 0.62942781226465361
   relative_time: 6.2999999999999856
 }
 trajectory_point {
   path_point {
-    x: 435699.81256510079
-    y: 4435966.53184524
-    theta: 1.9566276338135973
-    kappa: -8.6346472879823166e-05
-    s: 19.61523873844337
+    x: 435699.81871857407
+    y: 4435966.5138012264
+    theta: 1.9581817299851185
+    kappa: -9.0446101312334886e-05
+    s: 19.596235856311907
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5019095410362615
-  a: 0.61217224880784693
+  v: 6.49533702349318
+  a: 0.61145753546255688
   relative_time: 6.3499999999999854
 }
 trajectory_point {
   path_point {
-    x: 435699.68994786451
-    y: 4435966.8337480593
-    theta: 1.9565878587552452
-    kappa: -0.00013444095856951773
-    s: 19.941091924638425
+    x: 435699.69576091011
+    y: 4435966.8152097128
+    theta: 1.9581130837599396
+    kappa: -0.00021699427231865377
+    s: 19.921759531519616
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5320677147091173
-  a: 0.59414920198776611
+  v: 6.5254599586165689
+  a: 0.59345439163551694
   relative_time: 6.3999999999999853
 }
 trajectory_point {
   path_point {
-    x: 435699.56677439931
-    y: 4435967.1370281689
-    theta: 1.9565447822668129
-    kappa: -0.00018430581542218513
-    s: 20.2684304769683
+    x: 435699.57224284584
+    y: 4435967.11799192
+    theta: 1.9580441246680087
+    kappa: -0.00034411920798874097
+    s: 20.248766835859115
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5613239116231776
-  a: 0.576093178456047
+  v: 6.5546819148729822
+  a: 0.57541838078353358
   relative_time: 6.4499999999999851
 }
 trajectory_point {
   path_point {
-    x: 435699.44309270789
-    y: 4435967.4416563436
-    theta: 1.9564626761796282
-    kappa: -0.00025369903700223127
-    s: 20.597209255374167
+    x: 435699.4482533104
+    y: 4435967.4221354173
+    theta: 1.9578853879084621
+    kappa: -0.000482267023643066
+    s: 20.577212679303273
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5896764829428607
-  a: 0.55800417821268966
+  v: 6.5830012489111729
+  a: 0.5573495029066069
   relative_time: 6.4999999999999849
 }
 trajectory_point {
   path_point {
-    x: 435699.31888623693
-    y: 4435967.74757705
-    theta: 1.9563802217175257
-    kappa: -0.00032338669301226427
-    s: 20.927383037355419
+    x: 435699.32374153379
+    y: 4435967.7275707172
+    theta: 1.9577212985995887
+    kappa: -0.00062154810986945043
+    s: 20.907051889657403
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.6171237798325855
-  a: 0.53988220125769393
+  v: 6.6104163173798938
+  a: 0.53924775800473679
   relative_time: 6.5499999999999847
 }
 trajectory_point {
   path_point {
-    x: 435699.19423759764
-    y: 4435968.0547749251
-    theta: 1.9562454923057753
-    kappa: -0.00040530205378114037
-    s: 21.258906517969663
+    x: 435699.19883539109
+    y: 4435968.0343010463
+    theta: 1.9574757384909498
+    kappa: -0.00075916311312505141
+    s: 21.23823921255925
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.6436641534567693
-  a: 0.52172724759105993
+  v: 6.6369254769278978
+  a: 0.52111314607792347
   relative_time: 6.5999999999999845
 }
 trajectory_point {
   path_point {
-    x: 435699.06910154526
-    y: 4435968.363182622
-    theta: 1.9561078632087046
-    kappa: -0.0004880845747919874
-    s: 21.591734309832741
+    x: 435699.07346182456
+    y: 4435968.3422476891
+    theta: 1.9572123424150811
+    kappa: -0.00089685273419290239
+    s: 21.570729311478992
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.66929595497983
-  a: 0.50353931721278766
+  v: 6.662527084203937
+  a: 0.50294566712616673
   relative_time: 6.6499999999999844
 }
 trajectory_point {
   path_point {
-    x: 435698.94358776708
-    y: 4435968.6727955556
-    theta: 1.9559194733007654
-    kappa: -0.00057440326088540293
-    s: 21.9258209431187
+    x: 435698.94775891153
+    y: 4435968.65141776
+    theta: 1.9568817174289381
+    kappa: -0.0010231847679366291
+    s: 21.904476767719252
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.6940175355661875
-  a: 0.485318410122877
+  v: 6.6872194958567652
+  a: 0.48474532114946672
   relative_time: 6.6999999999999842
 }
 trajectory_point {
   path_point {
-    x: 435698.8176342306
-    y: 4435968.9835394118
-    theta: 1.9557219540001745
-    kappa: -0.00066157721287601759
-    s: 22.261120865559818
+    x: 435698.82165515027
+    y: 4435968.9617330553
+    theta: 1.9565244610801054
+    kappa: -0.0011454147257264312
+    s: 22.239436080415089
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.7178272463802573
-  a: 0.46706452632132811
+  v: 6.7110010685351345
+  a: 0.46651210814782335
   relative_time: 6.749999999999984
 }
 trajectory_point {
   path_point {
-    x: 435698.69135530246
-    y: 4435969.2954112738
-    theta: 1.9554770326157311
-    kappa: -0.00074545953830700657
-    s: 22.597588442446593
+    x: 435698.69527732825
+    y: 4435969.2731957939
+    theta: 1.9561178584550809
+    kappa: -0.0012484111172921151
+    s: 22.575561666534
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.74072343858646
-  a: 0.44877766580814088
+  v: 6.7338701588877985
+  a: 0.44824602812123665
   relative_time: 6.7999999999999838
 }
 trajectory_point {
   path_point {
-    x: 435698.56468872062
-    y: 4435969.6083365958
-    theta: 1.9552175076267508
-    kappa: -0.00082856050268624538
-    s: 22.93517795662774
+    x: 435698.56857328262
+    y: 4435969.58573552
+    theta: 1.9556821756716576
+    kappa: -0.0013404200373348418
+    s: 22.912807860875919
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.7627044633492108
-  a: 0.43045782858331538
+  v: 6.7558251235635085
+  a: 0.42994708106970664
   relative_time: 6.8499999999999837
 }
 trajectory_point {
   path_point {
-    x: 435698.43774916895
-    y: 4435969.922312336
-    theta: 1.9549186419052988
-    kappa: -0.00090245790732468539
-    s: 23.273843608510195
+    x: 435698.44163400243
+    y: 4435969.8993396172
+    theta: 1.9552171430102541
+    kappa: -0.0014076220269490592
+    s: 23.251128916073213
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.78376867183293
-  a: 0.41210501464685156
+  v: 6.77686431921102
+  a: 0.41161526699323325
   relative_time: 6.8999999999999835
 }
 trajectory_point {
   path_point {
-    x: 435698.31047875469
-    y: 4435970.2372655887
-    theta: 1.9546025848208
-    kappa: -0.00097257717377353687
-    s: 23.61353951605912
+    x: 435698.31444054947
+    y: 4435970.2139509758
+    theta: 1.9547288161640395
+    kappa: -0.0014553875733155923
+    s: 23.590479002590694
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8039144152020352
-  a: 0.39371922399874942
+  v: 6.7969861024790834
+  a: 0.39325058589181655
   relative_time: 6.9499999999999833
 }
 trajectory_point {
   path_point {
-    x: 435698.18298206694
-    y: 4435970.5531889647
-    theta: 1.9542593972369482
-    kappa: -0.0010284442519279749
-    s: 23.95421971479789
+    x: 435698.18704089121
+    y: 4435970.5295392964
+    theta: 1.954231764038814
+    kappa: -0.0014761769565891361
+    s: 23.930812208725609
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8231400446209438
-  a: 0.37530045663900896
+  v: 6.8161888300164524
+  a: 0.37485303776545653
   relative_time: 6.9999999999999831
 }
 trajectory_point {
   path_point {
-    x: 435698.05521206313
-    y: 4435970.8700138172
-    theta: 1.9539009496683295
-    kappa: -0.0010765716918584277
-    s: 24.295838157808113
+    x: 435698.05945213838
+    y: 4435970.8460618854
+    theta: 1.9537260584708147
+    kappa: -0.0014700051538012523
+    s: 24.272082540607638
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8414439112540757
-  a: 0.35684871256763023
+  v: 6.83447085847188
+  a: 0.35642262261415314
   relative_time: 7.0499999999999829
 }
 trajectory_point {
   path_point {
-    x: 435697.92725393455
-    y: 4435971.1877246657
-    theta: 1.9535271722228003
-    kappa: -0.0011060101696051187
-    s: 24.638348715729606
+    x: 435697.93167776405
+    y: 4435971.1634702813
+    theta: 1.9532271778462527
+    kappa: -0.0014481972758344443
+    s: 24.614243922198909
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8588243662658464
-  a: 0.33836399178461324
+  v: 6.8518305444941179
+  a: 0.33795934043790643
   relative_time: 7.0999999999999828
 }
 trajectory_point {
   path_point {
-    x: 435697.79907578242
-    y: 4435971.5062588407
-    theta: 1.9531429775131872
-    kappa: -0.0011231112945645719
-    s: 24.981705176760414
+    x: 435697.80376636575
+    y: 4435971.481734287
+    theta: 1.9527369210065741
+    kappa: -0.0014074354817164916
+    s: 24.957250195293973
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8752797608206757
-  a: 0.31984629428995787
+  v: 6.8682662447319212
+  a: 0.3194631912367164
   relative_time: 7.1499999999999826
 }
 trajectory_point {
   path_point {
-    x: 435697.67074223561
-    y: 4435971.825592339
-    theta: 1.9527583750215183
-    kappa: -0.0011173570241758766
-    s: 25.325861246656796
+    x: 435697.67568439338
+    y: 4435971.8007904068
+    theta: 1.9522619644202603
+    kappa: -0.001352553902757195
+    s: 25.301055119519837
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8908084460829819
-  a: 0.30129562008366423
+  v: 6.8837763158340408
+  a: 0.30093417501058306
   relative_time: 7.1999999999999824
 }
 trajectory_point {
   path_point {
-    x: 435697.54223630694
-    y: 4435972.14566835
-    theta: 1.9523732998799144
-    kappa: -0.0010942326523229922
-    s: 25.670770548733241
+    x: 435697.54750195937
+    y: 4435972.1206168663
+    theta: 1.9518092011748769
+    kappa: -0.0012777345816621984
+    s: 25.645612372335925
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9054087732171814
-  a: 0.28271196916573227
+  v: 6.89835911444923
+  a: 0.2823722917595064
   relative_time: 7.2499999999999822
 }
 trajectory_point {
   path_point {
-    x: 435697.41359751183
-    y: 4435972.4664525688
-    theta: 1.9520059108941104
-    kappa: -0.0010443625597921626
-    s: 26.016386623862452
+    x: 435697.41916291433
+    y: 4435972.4411410149
+    theta: 1.9513808188963879
+    kappa: -0.0011878717370061701
+    s: 25.990875549034122
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.919079093387694
-  a: 0.264095341536162
+  v: 6.9120129972262419
+  a: 0.26377754148348637
   relative_time: 7.2999999999999821
 }
 trajectory_point {
   path_point {
-    x: 435697.28482223459
-    y: 4435972.7878934015
-    theta: 1.9516532922780367
-    kappa: -0.00097203845418494285
-    s: 26.362662930475352
+    x: 435697.29074780794
+    y: 4435972.7623450235
+    theta: 1.950991980561035
+    kappa: -0.0010740902488907162
+    s: 26.336798162738724
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.931817757758937
-  a: 0.24544573719495338
+  v: 6.92473632081383
+  a: 0.24514992418252302
   relative_time: 7.3499999999999819
 }
 trajectory_point {
   path_point {
-    x: 435697.15592914948
-    y: 4435973.109948012
-    theta: 1.9513369897104318
-    kappa: -0.00086890724441639872
-    s: 26.709552844561095
+    x: 435697.16218672221
+    y: 4435973.08415067
+    theta: 1.9506371046852058
+    kappa: -0.00094431803239118845
+    s: 26.683333644406488
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9436231174953287
-  a: 0.22676315614210651
+  v: 6.9365274418607452
+  a: 0.22648943985661629
   relative_time: 7.3999999999999817
 }
 trajectory_point {
   path_point {
-    x: 435697.02692423196
-    y: 4435973.4325684849
-    theta: 1.9510532376231862
-    kappa: -0.00073814541527872748
-    s: 27.057009659667045
+    x: 435697.0335579277
+    y: 4435973.40653898
+    theta: 1.9503421485864041
+    kappa: -0.00078676666083073437
+    s: 27.0304353428266
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.954493523761287
-  a: 0.20804759837762143
+  v: 6.9473847170157423
+  a: 0.20779608850576625
   relative_time: 7.4499999999999815
 }
 trajectory_point {
   path_point {
-    x: 435696.89780480682
-    y: 4435973.7557033012
-    theta: 1.9508280741766029
-    kappa: -0.000572502387028608
-    s: 27.404986586898787
+    x: 435696.90479018533
+    y: 4435973.7294311421
+    theta: 1.9500909914418758
+    kappa: -0.00061219732046655746
+    s: 27.378056524620675
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.96442732772123
-  a: 0.18929906390149798
+  v: 6.9573065029275725
+  a: 0.18906987012997289
   relative_time: 7.4999999999999813
 }
 trajectory_point {
   path_point {
-    x: 435696.76858294348
-    y: 4435974.0793068083
-    theta: 1.9506575335314524
-    kappa: -0.00037434923399131488
-    s: 27.753436754920138
+    x: 435696.77594700438
+    y: 4435974.0528020784
+    theta: 1.9499217999336493
+    kappa: -0.0004061681766072117
+    s: 27.726150374242778
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9734228805395766
-  a: 0.17051755271373614
+  v: 6.96629115624499
+  a: 0.17031078472923622
   relative_time: 7.5499999999999812
 }
 trajectory_point {
   path_point {
-    x: 435696.63923403318
-    y: 4435974.403318597
-    theta: 1.9505626820677087
-    kappa: -0.0001738549109588777
-    s: 28.102313209953124
+    x: 435696.64696703316
+    y: 4435974.3765768334
+    theta: 1.9498035521337143
+    kappa: -0.00019943881621860656
+    s: 28.074669993979413
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9814785333807432
-  a: 0.15170306481433604
+  v: 6.974337033616747
+  a: 0.15151883230355612
   relative_time: 7.599999999999981
 }
 trajectory_point {
   path_point {
-    x: 435696.50977387745
-    y: 4435974.72769433
-    theta: 1.9505395287030773
-    kappa: 2.8842834647617479e-05
-    s: 28.451568915777997
+    x: 435696.51788717054
+    y: 4435974.7007195326
+    theta: 1.9497842403065602
+    kappa: 8.3832910823485637e-06
+    s: 28.423568403949503
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9885926374091492
-  a: 0.13285560020329767
+  v: 6.9814424916915971
+  a: 0.13269401285293292
   relative_time: 7.6499999999999808
 }
 trajectory_point {
   path_point {
-    x: 435696.38017688127
-    y: 4435975.0523730554
-    theta: 1.9505802784616268
-    kappa: 0.00020638897929343666
-    s: 28.801156753733231
+    x: 435696.38867375965
+    y: 4435975.0251661129
+    theta: 1.9498093015151658
+    kappa: 0.00020338744460900938
+    s: 28.772798542104432
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9947635437892117
-  a: 0.11397515888062093
+  v: 6.987605887118292
+  a: 0.11383632637736618
   relative_time: 7.6999999999999806
 }
 trajectory_point {
   path_point {
-    x: 435696.25046129758
-    y: 4435975.3773112372
-    theta: 1.9506814109714155
-    kappa: 0.00036015308929980566
-    s: 29.151029522715518
+    x: 435696.25933365268
+    y: 4435975.3498685574
+    theta: 1.9499221791248593
+    kappa: 0.00037280874249831421
+    s: 29.122313264228005
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9999896036853508
-  a: 0.09506174084630592
+  v: 6.9928255765455845
+  a: 0.094945772876856127
   relative_time: 7.7499999999999805
 }
 trajectory_point {
   path_point {
-    x: 435696.12060861517
-    y: 4435975.7024505641
-    theta: 1.9508296052474035
-    kappa: 0.00048945439887345093
-    s: 29.501139939179772
+    x: 435696.1298697032
+    y: 4435975.67477716
+    theta: 1.9500701573797969
+    kappa: 0.00052933921751215248
+    s: 29.472065343936478
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.004269168261982
-  a: 0.076115346100352754
+  v: 6.9970999166222283
+  a: 0.076022352351402755
   relative_time: 7.79999999999998
 }
 trajectory_point {
   path_point {
-    x: 435695.99063984485
-    y: 4435976.0277484236
-    theta: 1.9510215383698484
-    kappa: 0.00059600210617558427
-    s: 29.851440637139127
+    x: 435696.00026374526
+    y: 4435975.9998337533
+    theta: 1.9502879203692924
+    kappa: 0.0006600706079858095
+    s: 29.822007472678528
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0076005886835251
-  a: 0.0571359746427611
+  v: 7.0004272639969756
+  a: 0.057066064801006067
   relative_time: 7.84999999999998
 }
 trajectory_point {
   path_point {
-    x: 435695.860540799
-    y: 4435976.3531480255
-    theta: 1.9512441900131778
-    kappa: 0.0006784174058431004
-    s: 30.201884168164945
+    x: 435695.87054797733
+    y: 4435976.3250001268
+    theta: 1.9505319377705259
+    kappa: 0.000777660310308236
+    s: 30.172092259735287
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0099822161143974
-  a: 0.03812362647353118
+  v: 7.0028059753185792
+  a: 0.038076910225666061
   relative_time: 7.89999999999998
 }
 trajectory_point {
   path_point {
-    x: 435695.73033640243
-    y: 4435976.6786081437
-    theta: 1.951494643882665
-    kappa: 0.00073896632294336928
-    s: 30.552423001386792
+    x: 435695.74068397831
+    y: 4435976.6502098367
+    theta: 1.9508278998140098
+    kappa: 0.00086912046286203811
+    s: 30.52227223222031
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0114124017190171
-  a: 0.019078301592662994
+  v: 7.0042344072357929
+  a: 0.019054888625382627
   relative_time: 7.94999999999998
 }
 trajectory_point {
   path_point {
-    x: 435695.600021919
-    y: 4435977.0040755551
-    theta: 1.9517593047129151
-    kappa: 0.00077593446647282937
-    s: 30.903009523492479
+    x: 435695.6107315718
+    y: 4435976.9754355038
+    theta: 1.9511408197598041
+    kappa: 0.00094729313700824317
+    s: 30.8724998350796
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0118894966618033
-  a: 1.5643042416968456e-13
+  v: 7.0047109163973689
+  a: 1.5598633495983449e-13
   relative_time: 7.99999999999998
 }
 decision {
diff --git a/scripts/plot_planning.sh b/scripts/plot_planning.sh
new file mode 100755
index 0000000000..34859ecc23
--- /dev/null
+++ b/scripts/plot_planning.sh
@@ -0,0 +1,26 @@
+#!/usr/bin/env bash
+
+###############################################################################
+# Copyright 2017 The Apollo Authors. All Rights Reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+###############################################################################
+
+
+DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+
+cd "${DIR}/.."
+
+source "${DIR}/apollo_base.sh"
+
+python modules/tools/mapshow/plot_planning.py $@
-- 
GitLab