From d97a4d7b3749ca7603da833742b55af8d273008d Mon Sep 17 00:00:00 2001
From: Liangliang Zhang <zhangliangliang@baidu.com>
Date: Wed, 22 Nov 2017 14:17:01 -0800
Subject: [PATCH] Planning: reduced the number of average point of kappa from
 10 to 4 to reduce speed and vehicle shaking. (#1412)

---
 modules/planning/conf/planning_config.pb.txt  |    4 +-
 .../planning/proto/st_boundary_config.proto   |    2 +-
 .../qp_spline_path_generator.cc               |   10 +-
 .../garage_test/result_follow_0.pb.txt        | 1202 +++++-----
 .../garage_test/result_out_of_map_0.pb.txt    | 1920 ++++++++--------
 .../garage_test/result_stop_dest_0.pb.txt     | 1920 ++++++++--------
 .../result_stop_over_line_0.pb.txt            |  140 +-
 .../result_avoid_change_left_0.pb.txt         | 1120 +++++-----
 .../result_change_lane_0.pb.txt               | 1738 +++++++--------
 .../result_cruise_0.pb.txt                    | 1320 +++++------
 .../result_follow_02_0.pb.txt                 |  944 ++++----
 .../result_follow_03_0.pb.txt                 | 1202 +++++-----
 .../result_mission_complete_0.pb.txt          | 1918 ++++++++--------
 .../sunnyvale_loop_test/result_nudge_0.pb.txt | 1114 +++++-----
 .../result_qp_path_failure_0.pb.txt           | 1924 ++++++++---------
 .../result_rightturn_01_0.pb.txt              | 1924 ++++++++---------
 .../result_rightturn_with_red_light_0.pb.txt  | 1924 ++++++++---------
 .../result_slowdown_01_0.pb.txt               | 1922 ++++++++--------
 .../sunnyvale_loop_test/result_stop_0.pb.txt  |  692 +++---
 19 files changed, 11470 insertions(+), 11470 deletions(-)

diff --git a/modules/planning/conf/planning_config.pb.txt b/modules/planning/conf/planning_config.pb.txt
index 9cfaccdc6d..cf03940514 100644
--- a/modules/planning/conf/planning_config.pb.txt
+++ b/modules/planning/conf/planning_config.pb.txt
@@ -71,7 +71,7 @@ em_planner_config {
         spline_order: 4
         max_spline_length: 20.0
         time_resolution: 0.1
-        regularization_weight: 1.0
+        regularization_weight: 1.0e-5
         first_spline_weight_factor: 0.0
         derivative_weight: 500.0
         second_derivative_weight: 1000.0
@@ -126,7 +126,7 @@ em_planner_config {
             minimal_kappa: 0.00001
             point_extension: 1.0
             lowest_speed: 2.5
-            num_points_to_avg_kappa: 10
+            num_points_to_avg_kappa: 4
         }
     }
 }
diff --git a/modules/planning/proto/st_boundary_config.proto b/modules/planning/proto/st_boundary_config.proto
index f2e011a739..1f79de6275 100644
--- a/modules/planning/proto/st_boundary_config.proto
+++ b/modules/planning/proto/st_boundary_config.proto
@@ -11,5 +11,5 @@ message StBoundaryConfig {
   optional double minimal_kappa = 6 [ default = 0.00001 ];
   optional double point_extension = 7 [ default = 1.0 ];
   optional double lowest_speed = 8 [ default = 2.5 ];
-  optional uint32 num_points_to_avg_kappa = 9 [ default = 20 ];
+  optional uint32 num_points_to_avg_kappa = 9 [ default = 4 ];
 }
diff --git a/modules/planning/tasks/qp_spline_path/qp_spline_path_generator.cc b/modules/planning/tasks/qp_spline_path/qp_spline_path_generator.cc
index 3ffe41fdb9..dba9d32fbd 100644
--- a/modules/planning/tasks/qp_spline_path/qp_spline_path_generator.cc
+++ b/modules/planning/tasks/qp_spline_path/qp_spline_path_generator.cc
@@ -98,7 +98,6 @@ bool QpSplinePathGenerator::Generate(
   if (is_change_lane_path_) {
     ref_l_ = init_frenet_point_.l();
   }
-
   double start_s = init_frenet_point_.s();
 
   constexpr double kDefaultPathLength = 50.0;
@@ -312,12 +311,13 @@ bool QpSplinePathGenerator::AddConstraint(const QpFrenetFrame& qp_frenet_frame,
 
   ADEBUG << "lat_shift = " << lat_shift;
   const double kEndPointBoundaryEpsilon = 1e-2;
-  constexpr double kReservedDistance = 10.0;
-  spline_constraint->AddPointConstraintInRange(
+  constexpr double kReservedDistance = 20.0;
+  const double target_s =
       std::fmin(qp_spline_path_config_.point_constraint_s_position(),
                 kReservedDistance + init_frenet_point_.s() +
-                    init_trajectory_point_.v() * FLAGS_look_forward_time_sec),
-      lat_shift, kEndPointBoundaryEpsilon);
+                    init_trajectory_point_.v() * FLAGS_look_forward_time_sec);
+  spline_constraint->AddPointConstraintInRange(target_s, lat_shift,
+                                               kEndPointBoundaryEpsilon);
   if (!is_change_lane_path_) {
     spline_constraint->AddPointDerivativeConstraintInRange(
         evaluated_s_.back(), 0.0, kEndPointBoundaryEpsilon);
diff --git a/modules/planning/testdata/garage_test/result_follow_0.pb.txt b/modules/planning/testdata/garage_test/result_follow_0.pb.txt
index f75d2c2116..6411744249 100644
--- a/modules/planning/testdata/garage_test/result_follow_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_follow_0.pb.txt
@@ -9,7 +9,7 @@ trajectory_point {
     y: 4140700.1599467541
     z: 0
     theta: 1.3256014779888403
-    kappa: 0.0014390229768810811
+    kappa: 0.0011389417552775663
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -20,12 +20,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586344.850048776
-    y: 4140700.2685516193
-    theta: 1.3257495980728795
-    kappa: 0.001382470633993914
+    x: 586344.8500564564
+    y: 4140700.2685497
+    theta: 1.3257186574454087
+    kappa: 0.0010929462295645501
     s: 0.11194464466504603
-    dkappa: -0.00011363887962376926
+    dkappa: -9.24257237726888e-05
     ddkappa: 0
   }
   v: 5.5944421199017391
@@ -34,12 +34,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586344.87716162927
-    y: 4140700.3770473311
-    theta: 1.3258975692961652
-    kappa: 0.0013259751292680631
+    x: 586344.87717698258
+    y: 4140700.3770434931
+    theta: 1.3258357191342196
+    kappa: 0.0010469969318409832
     s: 0.22377677891147563
-    dkappa: -0.00022716354603121336
+    dkappa: -0.00018475855469727659
     ddkappa: 0
   }
   v: 5.5887484572175818
@@ -48,12 +48,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586344.90424654074
-    y: 4140700.4854312292
-    theta: 1.3260453880248828
-    kappa: 0.0012695378476170302
+    x: 586344.90426955873
+    y: 4140700.485425476
+    theta: 1.3259526601824432
+    kappa: 0.0010010949884938778
     s: 0.335493661320598
-    dkappa: -0.00034057121631342644
+    dkappa: -0.00027699622935502266
     ddkappa: 0
   }
   v: 5.5829165542964976
@@ -62,12 +62,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586344.93130283442
-    y: 4140700.59370061
-    theta: 1.3261930505646229
-    kappa: 0.001213160197104221
+    x: 586344.93133350916
+    y: 4140700.5936929435
+    theta: 1.3260694776693016
+    kappa: 0.0009552415447386805
     s: 0.44709250464878
-    dkappa: -0.00045385906104301909
+    dkappa: -0.00036913644649238424
     ddkappa: 0
   }
   v: 5.576944190927839
@@ -76,12 +76,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586344.95829426858
-    y: 4140700.7018616009
-    theta: 1.3263239992176825
-    kappa: 0.001152606533246836
+    x: 586344.958340048
+    y: 4140700.7018501656
+    theta: 1.3261723933564746
+    kappa: 0.00090487430345014518
     s: 0.55857047866890464
-    dkappa: -0.00051368833308214779
+    dkappa: -0.00042003979642414813
     ddkappa: 0
   }
   v: 5.5708292889739068
@@ -90,12 +90,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586344.98522626818
-    y: 4140700.8099098913
-    theta: 1.3264410799587065
-    kappa: 0.0010886077181139306
+    x: 586344.98529331735
+    y: 4140700.8098931517
+    theta: 1.3262637760985618
+    kappa: 0.00085078018945794335
     s: 0.669924713011831
-    dkappa: -0.0005292378852916076
+    dkappa: -0.00043678697086410253
     ddkappa: 0
   }
   v: 5.564569912369949
@@ -104,12 +104,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.01212763693
-    y: 4140700.9178352933
-    theta: 1.3265580275416229
-    kappa: 0.0010246816912336631
+    x: 586345.01221593167
+    y: 4140700.9178132564
+    theta: 1.3263550549086802
+    kappa: 0.00079674759873031
     s: 0.78115230000785207
-    dkappa: -0.00054476975241217517
+    dkappa: -0.00045351509811448663
     ddkappa: 0
   }
   v: 5.5581642671241669
@@ -118,12 +118,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.03899766342
-    y: 4140701.0256349533
-    theta: 1.3266748388697998
-    kappa: 0.00096083014354090591
+    x: 586345.039107179
+    y: 4140701.025607625
+    theta: 1.3264462273708526
+    kappa: 0.00074277796050657621
     s: 0.89225029752815466
-    dkappa: -0.000560283523603743
+    dkappa: -0.00047022373569240123
     ddkappa: 0
   }
   v: 5.5516107013177054
@@ -132,12 +132,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.06583226938
-    y: 4140701.1333068227
-    theta: 1.3267892445234553
-    kappa: 0.00089668530070020945
+    x: 586345.0659637372
+    y: 4140701.1332740206
+    theta: 1.3265353459590115
+    kappa: 0.00068846326828625958
     s: 1.003215731826278
-    dkappa: -0.00057547301734962137
+    dkappa: -0.00048660552767269561
     ddkappa: 0
   }
   v: 5.5449077051046638
@@ -146,12 +146,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.09258779942
-    y: 4140701.2408586768
-    theta: 1.3268722752913047
-    kappa: 0.00082752642181703972
+    x: 586345.09275164187
+    y: 4140701.240817816
+    theta: 1.3265975652622315
+    kappa: 0.00062857540107096673
     s: 1.1140456003795725
-    dkappa: -0.00058642973413584258
+    dkappa: -0.00049874013216545806
     ddkappa: 0
   }
   v: 5.5380539107120867
@@ -160,12 +160,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.11930987122
-    y: 4140701.3482760359
-    theta: 1.3269552022294415
-    kappa: 0.000758454027017714
+    x: 586345.11950604792
+    y: 4140701.3482271265
+    theta: 1.3266597067599659
+    kappa: 0.00056876242441800118
     s: 1.2247368747306593
-    dkappa: -0.00059737274940421846
+    dkappa: -0.00051085956217637068
     ddkappa: 0
   }
   v: 5.5310480924399705
@@ -174,12 +174,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.14599774836
-    y: 4140701.4555559391
-    theta: 1.3270380230505823
-    kappa: 0.00068947002049849762
+    x: 586345.14622621785
+    y: 4140701.4554989906
+    theta: 1.3267217687386093
+    kappa: 0.00050902598725885136
     s: 1.3352865033288888
-    dkappa: -0.00060830176147635951
+    dkappa: -0.000522963483595498
     ddkappa: 0
   }
   v: 5.5238891666612577
@@ -188,12 +188,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.172650689
-    y: 4140701.562695405
-    theta: 1.3271207354522798
-    kappa: 0.00062057631909297672
+    x: 586345.17291140917
+    y: 4140701.5626304289
+    theta: 1.3267837494731902
+    kappa: 0.00044936774946824516
     s: 1.4456914143717996
-    dkappa: -0.00061921646667176841
+    dkappa: -0.00053505156009556285
     ddkappa: 0
   }
   v: 5.5165761918218417
@@ -202,12 +202,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.19924952532
-    y: 4140701.6696960158
-    theta: 1.3271833623773148
-    kappa: 0.00054977659539048754
+    x: 586345.19954745471
+    y: 4140701.6696217833
+    theta: 1.3268281658117633
+    kappa: 0.00038746855524209668
     s: 1.5559485186465785
-    dkappa: -0.00062801544628629589
+    dkappa: -0.00054501136948487106
     ddkappa: 0
   }
   v: 5.5091083684405655
@@ -216,12 +216,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.22579751757
-    y: 4140701.7765537621
-    theta: 1.3272302487969625
-    kappa: 0.00047750768562307525
+    x: 586345.22613652959
+    y: 4140701.7764693149
+    theta: 1.3268588261480514
+    kappa: 0.00032383591360745869
     s: 1.6660547123715168
-    dkappa: -0.00063515567893728821
+    dkappa: -0.00055330290795436218
     ddkappa: 0
   }
   v: 5.5014850391092205
@@ -230,12 +230,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.25230837229
-    y: 4140701.8832620271
-    theta: 1.3272770696279792
-    kappa: 0.000405339871852008
+    x: 586345.25268840918
+    y: 4140701.8831673791
+    theta: 1.3268894435940517
+    kappa: 0.00026029228681096942
     s: 1.7760068800374733
-    dkappa: -0.0006422859232138008
+    dkappa: -0.00056158284750330086
     ddkappa: 0
   }
   v: 5.4937056884925459
@@ -244,12 +244,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.27878133592
-    y: 4140701.9898177776
-    theta: 1.3273238235397016
-    kappa: 0.00033327520479415522
+    x: 586345.27920233924
+    y: 4140701.9897129433
+    theta: 1.3269200172796836
+    kappa: 0.00019683948050504635
     s: 1.8858018972493296
-    dkappa: -0.00064940597650318087
+    dkappa: -0.00056985095284935237
     ddkappa: 0
   }
   v: 5.4857699433282328
@@ -258,12 +258,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.30521531356
-    y: 4140702.0962180607
-    theta: 1.3273696831671324
-    kappa: 0.00026127122956556587
+    x: 586345.30567737424
+    y: 4140702.0961030177
+    theta: 1.3269498073310773
+    kappa: 0.00013342176063570473
     s: 1.9954366335674509
-    dkappa: -0.00065644561870054291
+    dkappa: -0.00057803592739893821
     ddkappa: 0
   }
   v: 5.4776775724269182
@@ -272,12 +272,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.33159505157
-    y: 4140702.2024634359
-    theta: 1.327379462869952
-    kappa: 0.00018743414574689361
+    x: 586345.33210475638
+    y: 4140702.2023365516
+    theta: 1.3269475991803599
+    kappa: 6.7610449995825824e-05
     s: 2.1049079553491454
-    dkappa: -0.000660422312019454
+    dkappa: -0.00058313609323998945
     ddkappa: 0
   }
   v: 5.4694284866721912
@@ -286,12 +286,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.357934656
-    y: 4140702.3085471704
-    theta: 1.3273892276940349
-    kappa: 0.000113709396906559
+    x: 586345.35849193216
+    y: 4140702.3084084634
+    theta: 1.3269453943891
+    kappa: 1.8992640056861038e-06
     s: 2.214212728590121
-    dkappa: -0.00066439295523813466
+    dkappa: -0.0005882284997412938
     ddkappa: 0
   }
   v: 5.4610227390205868
@@ -300,12 +300,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.3842333717
-    y: 4140702.4144662246
-    theta: 1.327398977359622
-    kappa: 4.0099095219951996e-05
+    x: 586345.38483814558
+    y: 4140702.4143157136
+    theta: 1.3269431930204645
+    kappa: -6.3709914743697573e-05
     s: 2.3233478217659482
-    dkappa: -0.00066835743459973886
+    dkappa: -0.00059331300100807561
     ddkappa: 0
   }
   v: 5.4524605245015927
@@ -314,12 +314,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.41049044568
-    y: 4140702.5202175654
-    theta: 1.32740871158743
-    kappa: -3.339465072670093e-05
+    x: 586345.411142642
+    y: 4140702.5200552684
+    theta: 1.3269409951375113
+    kappa: -0.00012921520686034509
     s: 2.4323101086735139
-    dkappa: -0.0006723156365407248
+    dkappa: -0.00059838945139347431
     ddkappa: 0
   }
   v: 5.443742180217642
@@ -328,12 +328,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.43670645019
-    y: 4140702.625797837
-    theta: 1.3274016311338235
-    kappa: -0.00010706981176052855
+    x: 586345.4374081688
+    y: 4140702.6256232308
+    theta: 1.3269237240769987
+    kappa: -0.00019516135022263508
     s: 2.5410964712724851
-    dkappa: -0.00067508877344095984
+    dkappa: -0.00060226852816266485
     ddkappa: 0
   }
   v: 5.43486818534412
@@ -342,12 +342,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.46288092365
-    y: 4140702.7312039537
-    theta: 1.3273740895120989
-    kappa: -0.00018098942315261497
+    x: 586345.46363473067
+    y: 4140702.7310163993
+    theta: 1.32688812870245
+    kappa: -0.00026166435040239329
     s: 2.6497038025267656
-    dkappa: -0.00067642090160581069
+    dkappa: -0.00060469364808777833
     ddkappa: 0
   }
   v: 5.4258391611293595
@@ -356,12 +356,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.4890115048
-    y: 4140702.8364333129
-    theta: 1.327346594075113
-    kappa: -0.000254785078134913
+    x: 586345.48981731292
+    y: 4140702.8362328326
+    theta: 1.3268525930180042
+    kappa: -0.00032805583114925419
     s: 2.7581290092459541
-    dkappa: -0.00067775079591388663
+    dkappa: -0.0006071147013091637
     ddkappa: 0
   }
   v: 5.4166558708946413
@@ -370,12 +370,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.51509745244
-    y: 4140702.9414829309
-    theta: 1.3273191456030748
-    kappa: -0.00032845468279790908
+    x: 586345.51595517283
+    y: 4140702.9412695454
+    theta: 1.3268171180320556
+    kappa: -0.00039433390864291435
     s: 2.8663690149268075
-    dkappa: -0.00067907841863019752
+    dkappa: -0.00060953161913082025
     ddkappa: 0
   }
   v: 5.4073192200341982
@@ -384,12 +384,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.54113802966
-    y: 4140703.0463498393
-    theta: 1.3272917448712405
-    kappa: -0.000401996156523525
+    x: 586345.542047572
+    y: 4140703.0461235722
+    theta: 1.3267817047465975
+    kappa: -0.00046049671102092992
     s: 2.9744207625946939
-    dkappa: -0.00068040373225928153
+    dkappa: -0.00061194433329280634
     ddkappa: 0
   }
   v: 5.3978302560152089
@@ -398,12 +398,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.5671541089
-    y: 4140703.1510257265
-    theta: 1.3272344835143159
-    kappa: -0.000475241249902934
+    x: 586345.56811792
+    y: 4140703.1507859556
+    theta: 1.3267194165891292
+    kappa: -0.00052682401679067016
     s: 3.0822812176450549
-    dkappa: -0.00068028086504939628
+    dkappa: -0.00061288882059528889
     ddkappa: 0
   }
   v: 5.388190168377804
@@ -412,12 +412,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.593127385
-    y: 4140703.2555120382
-    theta: 1.3271717007887238
-    kappa: -0.00054832314658746339
+    x: 586345.59414583026
+    y: 4140703.2552586719
+    theta: 1.3266521905297697
+    kappa: -0.000593084638915176
     s: 3.1899473706848656
-    dkappa: -0.00067988632481990707
+    dkappa: -0.00061355715209312329
     ddkappa: 0
   }
   v: 5.3784002887350608
@@ -426,12 +426,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.6190530688
-    y: 4140703.3598068939
-    theta: 1.3271090331035664
-    kappa: -0.00062127113079311251
+    x: 586345.6201260481
+    y: 4140703.3595399559
+    theta: 1.3265850876525869
+    kappa: -0.0006592238476040242
     s: 3.2974162403740905
-    dkappa: -0.00067949250753087981
+    dkappa: -0.00061422425896582287
     ddkappa: 0
   }
   v: 5.368462090773007
@@ -440,12 +440,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.64493044815
-    y: 4140703.463907429
-    theta: 1.3270464821798118
-    kappa: -0.00069408319980915649
+    x: 586345.64605786011
+    y: 4140703.4636269454
+    theta: 1.3265181098004244
+    kappa: -0.0007252398270743113
     s: 3.4046848762671456
-    dkappa: -0.000679099423994157
+    dkappa: -0.00061489012289866487
     ddkappa: 0
   }
   v: 5.35837719025062
@@ -454,12 +454,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.67076681484
-    y: 4140703.5678088246
-    theta: 1.3269773107988971
-    kappa: -0.000766530327175071
+    x: 586345.6719488363
+    y: 4140703.5675147464
+    theta: 1.3264451288855743
+    kappa: -0.0007910030567653591
     s: 3.5117503616543542
-    dkappa: -0.00067832825781713782
+    dkappa: -0.00061517114814615829
     ddkappa: 0
   }
   v: 5.3481473449998251
@@ -468,12 +468,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.69658701681
-    y: 4140703.6715019313
-    theta: 1.3268799908627684
-    kappa: -0.000837885180041574
+    x: 586345.69782459538
+    y: 4140703.6711940025
+    theta: 1.3263466536299333
+    kappa: -0.00085610559774745008
     s: 3.6186098164034095
-    dkappa: -0.00067596872739379742
+    dkappa: -0.00061384754232756572
     ddkappa: 0
   }
   v: 5.337774454925496
@@ -482,12 +482,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.72235674981
-    y: 4140703.7749923551
-    theta: 1.3267828611495949
-    kappa: -0.00090910056013424642
+    x: 586345.72364977677
+    y: 4140703.7746706037
+    theta: 1.3262483708554023
+    kappa: -0.00092108088695044821
     s: 3.7252603998008307
-    dkappa: -0.00067361380899388468
+    dkappa: -0.00061252652367673485
     ddkappa: 0
   }
   v: 5.3272605620054572
@@ -496,12 +496,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.74807533738
-    y: 4140703.8782773805
-    theta: 1.3266859242098474
-    kappa: -0.00098017459880031976
+    x: 586345.749423703
+    y: 4140703.8779418338
+    theta: 1.3261502831427734
+    kappa: -0.00098592721945816968
     s: 3.8316993133934236
-    dkappa: -0.00067126356440917474
+    dkappa: -0.00061120812685647521
     ddkappa: 0
   }
   v: 5.316607850290481
@@ -510,12 +510,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.77374211443
-    y: 4140703.9813543372
-    theta: 1.3265891825528988
-    kappa: -0.001051105457503484
+    x: 586345.775145707
+    y: 4140703.9810050232
+    theta: 1.3260523930312538
+    kappa: -0.0010506429178320001
     s: 3.9379238038297371
-    dkappa: -0.00066891805443556587
+    dkappa: -0.00060989238597094749
     ddkappa: 0
   }
   v: 5.30581864590429
@@ -524,12 +524,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.79938324774
-    y: 4140704.0842139125
-    theta: 1.3264749837792829
-    kappa: -0.0011207877199853943
+    x: 586345.80084189959
+    y: 4140704.0838508573
+    theta: 1.3259385094387433
+    kappa: -0.0011143737277603974
     s: 4.0439311657015269
-    dkappa: -0.00066553789476941612
+    dkappa: -0.00060753011904465652
     ddkappa: 0
   }
   v: 5.2948954170435547
@@ -538,12 +538,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.824998114
-    y: 4140704.1868535266
-    theta: 1.3263433182386224
-    kappa: -0.0011892188597664006
+    x: 586345.82651158143
+    y: 4140704.1864767764
+    theta: 1.3258086692749889
+    kappa: -0.0011771198469002324
     s: 4.1497187443852086
-    dkappa: -0.0006611224339894313
+    dkappa: -0.00060412359242933142
     ddkappa: 0
   }
   v: 5.2838407739778956
@@ -552,12 +552,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.85055913357
-    y: 4140704.2892773747
-    theta: 1.3262119294782635
-    kappa: -0.0012575061451981812
+    x: 586345.85212730092
+    y: 4140704.2888869573
+    theta: 1.3256791020544361
+    kappa: -0.0012397340626089069
     s: 4.2552839388833217
-    dkappa: -0.00065671625528832271
+    dkappa: -0.00060072422693496579
     ddkappa: 0
   }
   v: 5.2726574690498813
@@ -566,12 +566,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.87606568984
-    y: 4140704.3914829865
-    theta: 1.3260808206697305
-    kappa: -0.001325647930292785
+    x: 586345.87768844084
+    y: 4140704.3910789322
+    theta: 1.3255498109045163
+    kappa: -0.0013022148656414143
     s: 4.3606242046659869
-    dkappa: -0.00065231946487205529
+    dkappa: -0.00059733210449942876
     ddkappa: 0
   }
   v: 5.2613483966750305
@@ -580,12 +580,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.90151718119
-    y: 4140704.4934679526
-    theta: 1.325949994910673
-    kappa: -0.0013936426074276831
+    x: 586345.903194398
+    y: 4140704.4930502903
+    theta: 1.3254207988798123
+    kappa: -0.0013645607819309027
     s: 4.4657370565123626
-    dkappa: -0.00064793216647109834
+    dkappa: -0.00059394730515074446
     ddkappa: 0
   }
   v: 5.24991659334181
@@ -594,12 +594,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.92696626135
-    y: 4140704.5952166142
-    theta: 1.3257938654641106
-    kappa: -0.001459316480916701
+    x: 586345.92869670619
+    y: 4140704.5947856233
+    theta: 1.3252684964471879
+    kappa: -0.0014249703134489129
     s: 4.5706200713521072
-    dkappa: -0.00064253061678919729
+    dkappa: -0.00058953437472811731
     ddkappa: 0
   }
   v: 5.2383652376116361
@@ -608,12 +608,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.95237595378
-    y: 4140704.6967357825
-    theta: 1.3256299334347155
-    kappa: -0.0015241533193892302
+    x: 586345.95415907493
+    y: 4140704.6962915934
+    theta: 1.3251090604168025
+    kappa: -0.0014846756197655119
     s: 4.6752708911068392
-    dkappa: -0.00063681501563113776
+    dkappa: -0.00058480302587808565
     ddkappa: 0
   }
   v: 5.226697650118874
@@ -622,12 +622,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586345.977728712
-    y: 4140704.7980274828
-    theta: 1.3254663687132531
-    kappa: -0.0015888448815275667
+    x: 586345.97956439143
+    y: 4140704.7975701243
+    theta: 1.3249499816208632
+    kappa: -0.001544247147689489
     s: 4.779687225531589
-    dkappa: -0.00063111222110590482
+    dkappa: -0.00058008227830092473
     ddkappa: 0
   }
   v: 5.2149172935708394
@@ -636,12 +636,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.0030239973
-    y: 4140704.8990895618
-    theta: 1.3253031747853661
-    kappa: -0.0016533897921880885
+    x: 586346.00491211575
+    y: 4140704.8986190632
+    theta: 1.3247912634489547
+    kappa: -0.0016036836309134008
     s: 4.8838668550562678
-    dkappa: -0.00062542235443736839
+    dkappa: -0.00057537223234520709
     ddkappa: 0
   }
   v: 5.2030277727477943
@@ -650,12 +650,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.02826128853
-    y: 4140704.999919937
-    theta: 1.3251403550214165
-    kappa: -0.0017177867217745128
+    x: 586346.03020172589
+    y: 4140704.9994363291
+    theta: 1.3246329091785487
+    kappa: -0.0016629838450722845
     s: 4.9878076336271171
-    dkappa: -0.00061974553283423615
+    dkappa: -0.00057067298503577217
     ddkappa: 0
   }
   v: 5.1910328345029519
@@ -664,12 +664,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.05352283909
-    y: 4140705.10049585
-    theta: 1.3249471901415564
-    kappa: -0.0017787336374216517
+    x: 586346.05551287939
+    y: 4140705.0999997705
+    theta: 1.3244465113512949
+    kappa: -0.0017193090671391257
     s: 5.0915074915481764
-    dkappa: -0.00061295679201924707
+    dkappa: -0.00056484764593195077
     ddkappa: 0
   }
   v: 5.1789363677624749
@@ -678,12 +678,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.07872810494
-    y: 4140705.2008354459
-    theta: 1.3247534051521619
-    kappa: -0.0018394225544066966
+    x: 586346.08076745458
+    y: 4140705.2003269671
+    theta: 1.32425958914268
+    kappa: -0.0017754063705870487
     s: 5.1949644383227342
-    dkappa: -0.00060614467540034739
+    dkappa: -0.00055899749385459414
     ddkappa: 0
   }
   v: 5.1667424035254728
@@ -692,12 +692,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.1038737254
-    y: 4140705.3009375986
-    theta: 1.3245600787273186
-    kappa: -0.0018999678576761283
+    x: 586346.10596226749
+    y: 4140705.3004167494
+    theta: 1.3240731092592763
+    kappa: -0.0018313709258729631
     s: 5.2981765654947939
-    dkappa: -0.00059934867891387779
+    dkappa: -0.000553161185525297
     ddkappa: 0
   }
   v: 5.1544551148640059
@@ -706,12 +706,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.128959256
-    y: 4140705.4008005396
-    theta: 1.3243672142972049
-    kappa: -0.0019603684775026426
+    x: 586346.13109687308
+    y: 4140705.4002673505
+    theta: 1.3238870750088154
+    kappa: -0.0018872017442031954
     s: 5.4011420494905265
-    dkappa: -0.00059256892263295042
+    dkappa: -0.00054733882406119664
     ddkappa: 0
   }
   v: 5.1420788169230836
@@ -720,12 +720,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.2538814391
-    y: 4140705.8963681003
-    theta: 1.3232885284983806
-    kappa: -0.002244234926008008
+    x: 586346.256246969
+    y: 4140705.8957774062
+    theta: 1.3228511156496774
+    kappa: -0.0021503220810497855
     s: 5.9122122965161141
-    dkappa: -0.00055477139670113019
+    dkappa: -0.00051425786849591424
     ddkappa: 0
   }
   v: 5.0790231632083795
@@ -734,12 +734,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.37781208672
-    y: 4140706.3855996439
-    theta: 1.3220921297627894
-    kappa: -0.0025045141246231159
+    x: 586346.3803793143
+    y: 4140706.3849578155
+    theta: 1.321706762747227
+    kappa: -0.0023924739012735254
     s: 6.4168967078607881
-    dkappa: -0.0005154495979062478
+    dkappa: -0.00047956300291427913
     ddkappa: 0
   }
   v: 5.0144712412043368
@@ -748,12 +748,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.50075281761
-    y: 4140706.8683749503
-    theta: 1.3207875894701904
-    kappa: -0.0027388570177807577
+    x: 586346.503491909
+    y: 4140706.8676893292
+    theta: 1.3204620328956096
+    kappa: -0.0026112130047953029
     s: 6.9150798779920288
-    dkappa: -0.00047057827368551835
+    dkappa: -0.00043923813039363171
     ddkappa: 0
   }
   v: 4.9491222761047808
@@ -762,12 +762,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.62272423308
-    y: 4140707.3446455384
-    theta: 1.319389067658443
-    kappa: -0.0029460359203700736
+    x: 586346.6256026905
+    y: 4140707.3439242095
+    theta: 1.3191296342313921
+    kappa: -0.0028052195215647535
     s: 7.4067207636763577
-    dkappa: -0.00042215032916208121
+    dkappa: -0.00039528903220721509
     ddkappa: 0
   }
   v: 4.8837642886967068
@@ -776,12 +776,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.74376168626
-    y: 4140707.8144449145
-    theta: 1.3179129426376914
-    kappa: -0.0031255632978356303
+    x: 586346.74674519734
+    y: 4140707.8136965153
+    theta: 1.3177245919423419
+    kappa: -0.0029739153774846106
     s: 7.8918615635386562
-    dkappa: -0.00037138992506593744
+    dkappa: -0.0003489481547118112
     ddkappa: 0
   }
   v: 4.8192740953602842
@@ -790,12 +790,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.86391405959
-    y: 4140708.2778980867
-    theta: 1.3163751086636846
-    kappa: -0.0032772668680889163
+    x: 586346.86696724757
+    y: 4140708.2771316278
+    theta: 1.3162615519549741
+    kappa: -0.0031170401211617787
     s: 8.3706365976214769
-    dkappa: -0.00031875955238678188
+    dkappa: -0.00030068554595537062
     ddkappa: 0
   }
   v: 4.7566173080688516
@@ -804,12 +804,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586346.98323885049
-    y: 4140708.7352320841
-    theta: 1.3147962286565058
-    kappa: -0.0034021512571594578
+    x: 586346.98632591183
+    y: 4140708.7344567981
+    theta: 1.3147600393943917
+    kappa: -0.0032355139508391255
     s: 8.8432811869443739
-    dkappa: -0.00026649529635975374
+    dkappa: -0.00025274485272859857
     ddkappa: 0
   }
   v: 4.69684833438892
@@ -818,12 +818,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.1018222291
-    y: 4140709.1867799656
-    theta: 1.3131855515056203
-    kappa: -0.0034997498391006396
+    x: 586347.10490746365
+    y: 4140709.1860051751
+    theta: 1.3132282648096905
+    kappa: -0.0033287890014001512
     s: 9.3101405330632
-    dkappa: -0.00021178688065988816
+    dkappa: -0.00020231901858802084
     ddkappa: 0
   }
   v: 4.6411103774801736
@@ -832,12 +832,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.2197769701
-    y: 4140709.63299057
-    theta: 1.3115533076300745
-    kappa: -0.00357003206820979
+    x: 586347.22282517585
+    y: 4140709.6322255833
+    theta: 1.3116755350203642
+    kappa: -0.0033967612649741571
     s: 9.7716785976294389
-    dkappa: -0.00015516790123714047
+    dkappa: -0.00014995018575858542
     ddkappa: 0
   }
   v: 4.5906354360954653
@@ -846,12 +846,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.33724423149
-    y: 4140710.07443725
-    theta: 1.3099108169194382
-    kappa: -0.0036133806840156586
+    x: 586347.34022098372
+    y: 4140710.0736912885
+    theta: 1.3101123427315713
+    kappa: -0.0034397480599264057
     s: 10.228486981949525
-    dkappa: -9.7717737384088915e-05
+    dkappa: -9.6722440548493573e-05
     ddkappa: 0
   }
   v: 4.5467443045808205
@@ -860,12 +860,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.45439309964
-    y: 4140710.5118172914
-    theta: 1.3082687969148568
-    kappa: -0.0036302892218254904
+    x: 586347.45726491255
+    y: 4140710.511099441
+    theta: 1.308548666612168
+    kappa: -0.0034581887312999755
     s: 10.681284200246123
-    dkappa: -3.9837225179036704e-05
+    dkappa: -4.3042823121201907e-05
     ddkappa: 0
   }
   v: 4.5104619133718478
@@ -874,12 +874,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.57139535691
-    y: 4140710.9458578285
-    theta: 1.3066377222785992
-    kappa: -0.0036213222653283081
+    x: 586347.57412977947
+    y: 4140710.9451770047
+    theta: 1.3069942974267021
+    kappa: -0.0034526064152076122
     s: 11.130818237806835
-    dkappa: 1.7999312578088882e-05
+    dkappa: 1.0608863803587987e-05
     ddkappa: 0
   }
   v: 4.4813470471731778
@@ -888,12 +888,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.68840661121
-    y: 4140711.3772172453
-    theta: 1.3050267608809909
-    kappa: -0.003586773169987192
+    x: 586347.69097233575
+    y: 4140711.3765821578
+    theta: 1.3054577396697225
+    kappa: -0.0034232644458688415
     s: 11.577766419086775
-    dkappa: 7.6012347220678252e-05
+    dkappa: 6.4449443731993484e-05
     ddkappa: 0
   }
   v: 4.4586177926771215
@@ -902,12 +902,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.80557799654
-    y: 4140711.8064762666
-    theta: 1.30344206983785
-    kappa: -0.0035261813757174448
+    x: 586347.80794495391
+    y: 4140711.8058953881
+    theta: 1.3039444980381092
+    kappa: -0.0033696830063355545
     s: 12.02272987149348
-    dkappa: 0.00013575015785547375
+    dkappa: 0.00012002235255705934
     ddkappa: 0
   }
   v: 4.4415321222398072
@@ -916,12 +916,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586347.923083787
-    y: 4140712.2341343286
-    theta: 1.3018965286741038
-    kappa: -0.0034374281040636502
+    x: 586347.925220858
+    y: 4140712.2336165486
+    theta: 1.3024664257769696
+    kappa: -0.0032898885234148851
     s: 12.46623751395329
-    dkappa: 0.0001958316747739754
+    dkappa: 0.00017594405748088776
     ddkappa: 0
   }
   v: 4.4293878938811835
@@ -930,12 +930,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.04101135675
-    y: 4140712.6606438709
-    theta: 1.3004020222449579
-    kappa: -0.003320852064173286
+    x: 586348.04289030656
+    y: 4140712.6601973805
+    theta: 1.3010346163596365
+    kappa: -0.0031841730215297769
     s: 12.90875004547773
-    dkappa: 0.00025613877619074877
+    dkappa: 0.00023209572892322542
     ddkappa: 0
   }
   v: 4.4215228512850189
@@ -944,12 +944,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.1593840943
-    y: 4140713.0864087082
-    theta: 1.2989673363948524
-    kappa: -0.0031785988816083032
+    x: 586348.16098031425
+    y: 4140713.08604073
+    theta: 1.2996573853835416
+    kappa: -0.0030545713762252444
     s: 13.350663933729891
-    dkappa: 0.00031566263361818518
+    dkappa: 0.00028746751814252863
     ddkappa: 0
   }
   v: 4.4173146237989016
@@ -958,12 +958,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.27826407668
-    y: 4140713.5117597012
-    theta: 1.2976018225794295
-    kappa: -0.003010598246380683
+    x: 586348.27955513622
+    y: 4140713.5114769023
+    theta: 1.29834338040484
+    kappa: -0.0029010159058416429
     s: 13.792315403590822
-    dkappa: 0.00037514086070510532
+    dkappa: 0.00034279695838327327
     ddkappa: 0
   }
   v: 4.4161807264342405
@@ -972,12 +972,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.39770352142
-    y: 4140713.9369721632
-    theta: 1.2963148980589008
-    kappa: -0.0028165426206145519
+    x: 586348.39866941562
+    y: 4140713.9367805757
+    theta: 1.2971012943017584
+    kappa: -0.0027232050151753456
     s: 14.233984425725893
-    dkappa: 0.000435111842315297
+    dkappa: 0.00039862183907382484
     ddkappa: 0
   }
   v: 4.4175785598662651
@@ -986,12 +986,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.51773463166
-    y: 4140714.3622728521
-    theta: 1.2951168180550527
-    kappa: -0.0025962393185436396
+    x: 586348.5183580562
+    y: 4140714.3621777762
+    theta: 1.2959406356592154
+    kappa: -0.0025209543038048911
     s: 14.675898705151194
-    dkappa: 0.00049534698492875056
+    dkappa: 0.00045471305875396191
     ddkappa: 0
   }
   v: 4.421005410434022
@@ -1000,12 +1000,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.63836852682
-    y: 4140714.7878444269
-    theta: 1.2940186081062934
-    kappa: -0.0023496594570932266
+    x: 586348.63863519114
+    y: 4140714.7878503208
+    theta: 1.2948716604801227
+    kappa: -0.0022942443432181687
     s: 15.118237669799914
-    dkappa: 0.00055533833538092036
+    dkappa: 0.00051056277768217495
     ddkappa: 0
   }
   v: 4.4259984501403808
@@ -1014,12 +1014,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.75959758461
-    y: 4140715.2138289665
-    theta: 1.2930315776647818
-    kappa: -0.0020768828092381231
+    x: 586348.759496546
+    y: 4140715.2139393329
+    theta: 1.293904887227425
+    kappa: -0.0020431649911272103
     s: 15.561136459088711
-    dkappa: 0.00061483212512628861
+    dkappa: 0.00056591689014231855
     ddkappa: 0
   }
   v: 4.4321347366520287
@@ -1028,12 +1028,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586348.88138796436
-    y: 4140715.6403342625
-    theta: 1.2921672182521065
-    kappa: -0.0017783728018237088
+    x: 586348.8809119931
+    y: 4140715.640551514
+    theta: 1.2930509970200468
+    kappa: -0.0017681894334899927
     s: 16.00468991248411
-    dkappa: 0.000672894952784633
+    dkappa: 0.00061984220452999169
     ddkappa: 0
   }
   v: 4.4390312132994749
@@ -1042,12 +1042,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.00374280079
-    y: 4140716.0674199052
-    theta: 1.2914556935336039
-    kappa: -0.0014508762023608616
+    x: 586349.00288915832
+    y: 4140716.0677451747
+    theta: 1.2923378137006474
+    kappa: -0.0014663084981037262
     s: 16.448956558068872
-    dkappa: 0.00073070750267598919
+    dkappa: 0.00067351959127491621
     ddkappa: 0
   }
   v: 4.4463447090770476
@@ -1056,12 +1056,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.1265748787
-    y: 4140716.49513786
-    theta: 1.2908938082826293
-    kappa: -0.0011094398990670656
+    x: 586349.12534657249
+    y: 4140716.4955705474
+    theta: 1.2917625260643808
+    kappa: -0.0011515382433000818
     s: 16.893962601108392
-    dkappa: 0.00076395139599381957
+    dkappa: 0.00070427755725500419
     ddkappa: 0
   }
   v: 4.4537719386428938
@@ -1070,12 +1070,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.24981806776
-    y: 4140716.9235047661
-    theta: 1.290476305463264
-    kappa: -0.00078912477343953008
+    x: 586349.2482228697
+    y: 4140716.9240428386
+    theta: 1.2913201758504291
+    kappa: -0.00085661896274746591
     s: 17.339705912617056
-    dkappa: 0.00072429798050105041
+    dkappa: 0.0006669723732844763
     ddkappa: 0
   }
   v: 4.461049502318982
@@ -1084,12 +1084,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.37340793724
-    y: 4140717.3525115335
-    theta: 1.2901927246893421
-    kappa: -0.00049658399926235731
+    x: 586349.37145825336
+    y: 4140717.3531515682
+    theta: 1.2910009466752006
+    kappa: -0.00058769855721805923
     s: 17.786160017924651
-    dkappa: 0.00066031671249192308
+    dkappa: 0.00060708218570085693
     ddkappa: 0
   }
   v: 4.4679538860911006
@@ -1098,12 +1098,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.49727600836
-    y: 4140717.782129114
-    theta: 1.2900348113809834
-    kappa: -0.00021184762639582083
+    x: 586349.49498863728
+    y: 4140717.7828663662
+    theta: 1.2907973395576828
+    kappa: -0.00032473326390530241
     s: 18.233278085242727
-    dkappa: 0.00063601303811103662
+    dkappa: 0.00058705598236916362
     ddkappa: 0
   }
   v: 4.4743014616088566
@@ -1112,12 +1112,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.62134454027
-    y: 4140718.2123141317
-    theta: 1.2900021497255783
-    kappa: 7.5336357880991591e-05
+    x: 586349.61874039262
+    y: 4140718.2131426176
+    theta: 1.290709715023723
+    kappa: -5.7467697663962494e-05
     s: 18.680996914230988
-    dkappa: 0.0006416832165829291
+    dkappa: 0.00059699157285883606
     ddkappa: 0
   }
   v: 4.4799484861856786
@@ -1126,12 +1126,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.74553830619
-    y: 4140718.6430096161
-    theta: 1.2900961701120011
-    kappa: 0.00035645087853970482
+    x: 586349.74264211906
+    y: 4140718.6439222079
+    theta: 1.2907402910748296
+    kappa: 0.00020559023053629076
     s: 19.129240924563668
-    dkappa: 0.00062827291822012171
+    dkappa: 0.00058784676475739971
     ddkappa: 0
   }
   v: 4.4847911027988143
@@ -1140,12 +1140,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.86979410308
-    y: 4140719.0741469879
-    theta: 1.2903119291189129
-    kappa: 0.00062300002129876859
+    x: 586349.86663411732
+    y: 4140719.0751355216
+    theta: 1.2908849283890937
+    kappa: 0.00045595647294532178
     s: 19.577926715472902
-    dkappa: 0.00059474696451530774
+    dkappa: 0.00055859378946368379
     ddkappa: 0
   }
   v: 4.4887878012179856
@@ -1154,12 +1154,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586349.9940389283
-    y: 4140719.5056623938
-    theta: 1.2906432487445663
-    kappa: 0.00087484042937977337
+    x: 586349.99064655439
+    y: 4140719.5067177704
+    theta: 1.2911382650672076
+    kappa: 0.00069349513695706768
     s: 20.026972730341047
-    dkappa: 0.00056107703267896511
+    dkappa: 0.00052919780634681128
     ddkappa: 0
   }
   v: 4.4920116833707944
@@ -1168,12 +1168,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.11819290021
-    y: 4140719.9375032922
-    theta: 1.2910894481212178
-    kappa: 0.0011110094992047889
+    x: 586350.11460448569
+    y: 4140719.9386149826
+    theta: 1.2915001440245046
+    kappa: 0.00091735809849800929
     s: 20.47630642681963
-    dkappa: 0.00052727142867927985
+    dkappa: 0.000499666368266333
     ddkappa: 0
   }
   v: 4.4945566234207623
@@ -1182,12 +1182,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.24218129087
-    y: 4140720.369625065
-    theta: 1.291644232022914
-    kappa: 0.0013311680925942146
+    x: 586350.23843653942
+    y: 4140720.3707815832
+    theta: 1.291965242986242
+    kappa: 0.0011272327625859312
     s: 20.925864376556621
-    dkappa: 0.00049334205752046649
+    dkappa: 0.00047001093187705944
     ddkappa: 0
   }
   v: 4.4965110271335353
@@ -1196,12 +1196,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.36595912429
-    y: 4140720.8019833853
-    theta: 1.2922933447047844
-    kappa: 0.0015362954748409613
+    x: 586350.36209725169
+    y: 4140720.803173421
+    theta: 1.2925205383446043
+    kappa: 0.0013239694241381052
     s: 21.375591718356681
-    dkappa: 0.00045930681695635812
+    dkappa: 0.00044024937050194865
     ddkappa: 0
   }
   v: 4.497957831876886
@@ -1210,12 +1210,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.48946414643
-    y: 4140721.2345471475
-    theta: 1.2930298261728093
-    kappa: 0.0017263036523669734
+    x: 586350.48552595871
+    y: 4140721.2357589733
+    theta: 1.2931599043397959
+    kappa: 0.0015074824613600982
     s: 21.825441611341343
-    dkappa: 0.00042515161100282247
+    dkappa: 0.00041036747357489097
     ddkappa: 0
   }
   v: 4.4989745066207121
@@ -1224,12 +1224,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.61265551951
-    y: 4140721.6672868161
-    theta: 1.2938472165923267
-    kappa: 0.0019011320882457274
+    x: 586350.60868307028
+    y: 4140721.6685084039
+    theta: 1.2938777172254947
+    kappa: 0.0016777129275285432
     s: 22.275374688109238
-    dkappa: 0.00039091190139085884
+    dkappa: 0.00038040071675613364
     ddkappa: 0
   }
   v: 4.4996330519370344
@@ -1238,12 +1238,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.73549625091
-    y: 4140722.1001788853
-    theta: 1.2947389954216328
-    kappa: 0.0020607312439468468
+    x: 586350.73153247929
+    y: 4140722.1013980228
+    theta: 1.2946682927220741
+    kappa: 0.0018346122463724595
     s: 22.7253585078963
-    dkappa: 0.00035660220899532774
+    dkappa: 0.00035036365417948653
     ddkappa: 0
   }
   v: 4.5
@@ -1252,12 +1252,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.85794989928
-    y: 4140722.5332062519
-    theta: 1.2956984827912845
-    kappa: 0.0022050574164668634
+    x: 586350.85403827974
+    y: 4140722.5344106508
+    theta: 1.2955257869664303
+    kappa: 0.0019781372126444672
     s: 23.175367009735993
-    dkappa: 0.00032222162411576333
+    dkappa: 0.00032025546422524915
     ddkappa: 0
   }
   v: 4.5001364145858807
@@ -1266,12 +1266,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586350.97998339287
-    y: 4140722.9663568418
-    theta: 1.2967189438500748
-    kappa: 0.002334073365572524
+    x: 586350.9761675871
+    y: 4140722.9675342464
+    theta: 1.2964443003035144
+    kappa: 0.0021082507541949715
     s: 23.625379965619516
-    dkappa: 0.00028777373966618367
+    dkappa: 0.00029007985267491375
     ddkappa: 0
   }
   v: 4.5000978910730725
@@ -1280,12 +1280,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.10156792891
-    y: 4140723.3996227873
-    theta: 1.2977936323399746
-    kappa: 0.0024477481191146873
+    x: 586351.097891438
+    y: 4140723.4007610818
+    theta: 1.2974179205178209
+    kappa: 0.002224921851105234
     s: 24.075382433656
-    dkappa: 0.00025326438635147133
+    dkappa: 0.00025984276942940375
     ddkappa: 0
   }
   v: 4.4999345564420974
@@ -1294,12 +1294,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.22267881606
-    y: 4140723.8329999154
-    theta: 1.2989157965441755
-    kappa: 0.0025460559350774232
+    x: 586351.21918463509
+    y: 4140723.8340872237
+    theta: 1.2984407286342188
+    kappa: 0.0023281245938828396
     s: 24.525364211232745
-    dkappa: 0.00021869918939696132
+    dkappa: 0.00022954995444068895
     ddkappa: 0
   }
   v: 4.4996910692756007
@@ -1308,12 +1308,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.343265745
-    y: 4140724.2664954327
-    theta: 1.3000805893363931
-    kappa: 0.0026280756455015451
+    x: 586351.33999853907
+    y: 4140724.2675195388
+    theta: 1.2995089046575152
+    kappa: 0.0024170527446082863
     s: 24.97531928817542
-    dkappa: 0.00018408100459518407
+    dkappa: 0.00019920441572882875
     ddkappa: 0
   }
   v: 4.4994066197583553
@@ -1322,12 +1322,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.46330484946
-    y: 4140724.70011279
-    theta: 1.3012801183539366
-    kappa: 0.0026936553654692643
+    x: 586351.46030780161
+    y: 4140724.70106202
+    theta: 1.3006156156240403
+    kappa: 0.0024915619752561746
     s: 25.42524529990828
-    dkappa: 0.0001494162072461266
+    dkappa: 0.000168812595120016
     ddkappa: 0
   }
   v: 4.4991149296772566
@@ -1336,12 +1336,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.58281363128
-    y: 4140725.1338471822
-    theta: 1.302504784272104
-    kappa: 0.002743803072362239
+    x: 586351.58012554538
+    y: 4140725.1347111724
+    theta: 1.3017519051608806
+    kappa: 0.0025525374676133653
     s: 25.875142980614374
-    dkappa: 0.00011470174048613473
+    dkappa: 0.00013837145891973355
     ddkappa: 0
   }
   v: 4.4988442524213266
@@ -1350,12 +1350,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.70178281551
-    y: 4140725.5677038911
-    theta: 1.3037477788611813
-    kappa: 0.0027785057627943881
+    x: 586351.69944044983
+    y: 4140725.5684729442
+    theta: 1.3029117939250359
+    kappa: 0.0025999661868823585
     s: 26.325015616395753
-    dkappa: 7.9949644487868946e-05
+    dkappa: 0.00010789307663057147
     ddkappa: 0
   }
   v: 4.49861737298171
@@ -1364,12 +1364,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.82021067641
-    y: 4140726.0016881814
-    theta: 1.3050025088896426
-    kappa: 0.0027977616637113786
+    x: 586351.81824840256
+    y: 4140726.0023533474
+    theta: 1.3040895172283713
+    kappa: 0.002633846441027988
     s: 26.774868498433694
-    dkappa: 4.5184819385636144e-05
+    dkappa: 7.7402359172007391e-05
     ddkappa: 0
   }
   v: 4.4984516079516794
@@ -1378,12 +1378,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586351.93809679733
-    y: 4140726.4358064593
-    theta: 1.3062623056874614
-    kappa: 0.0028015680422791272
+    x: 586351.93654625467
+    y: 4140726.4363596207
+    theta: 1.3052792346976285
+    kappa: 0.002654175696448169
     s: 27.224708376148911
-    dkappa: 1.0410571327564086e-05
+    dkappa: 4.6902590834411824e-05
     ddkappa: 0
   }
   v: 4.49835880552663
@@ -1392,12 +1392,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.05544216628
-    y: 4140726.8700656891
-    theta: 1.3075204296540688
-    kappa: 0.0027899212934081865
+    x: 586352.054331922
+    y: 4140726.8704996435
+    theta: 1.3064750353621153
+    kappa: 0.0026609506784544541
     s: 27.674542910361765
-    dkappa: -2.4374636531175002e-05
+    dkappa: 1.639222244645015e-05
     ddkappa: 0
   }
   v: 4.4983453455040818
@@ -1406,12 +1406,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.17225063639
-    y: 4140727.3044724385
-    theta: 1.3087701414740762
-    kappa: 0.0027628199416426754
+    x: 586352.17160584067
+    y: 4140727.3047809778
+    theta: 1.3076710090973882
+    kappa: 0.0026541703604700049
     s: 28.124380126452461
-    dkappa: -5.9163633266616584e-05
+    dkappa: -1.4121602029151854e-05
     ddkappa: 0
   }
   v: 4.4984121392836824
@@ -1420,12 +1420,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.28852878651
-    y: 4140727.7390323654
-    theta: 1.3100047005066264
-    kappa: 0.0027202647980995543
+    x: 586352.288370827
+    y: 4140727.7392103495
+    theta: 1.3088612452637129
+    kappa: 0.0026338360994141935
     s: 28.574227867521291
-    dkappa: -9.3948853206200517e-05
+    dkappa: -4.4631348572607674e-05
     ddkappa: 0
   }
   v: 4.4985546298672023
@@ -1434,12 +1434,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.40428578563
-    y: 4140728.1737496965
-    theta: 1.3112173633773212
-    kappa: 0.002662259237152543
+    x: 586352.40463193937
+    y: 4140728.1737931329
+    theta: 1.310039831468133
+    kappa: 0.0025999518788550218
     s: 29.024093247548823
-    dkappa: -0.00012872231368864839
+    dkappa: -7.512906726847816e-05
     ddkappa: 0
   }
   v: 4.4987627918585362
@@ -1448,12 +1448,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.51950222754
-    y: 4140728.6086349166
-    theta: 1.3123990867159561
-    kappa: 0.0025878499921201609
+    x: 586352.52036589559
+    y: 4140728.6085409163
+    theta: 1.3111993167962992
+    kappa: 0.0025516876953451083
     s: 29.473982104556121
-    dkappa: -0.00016347353339157703
+    dkappa: -0.00010560432217617314
     ddkappa: 0
   }
   v: 4.4990211314637056
@@ -1462,12 +1462,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.63419528538
-    y: 4140729.0436868751
-    theta: 1.3135416402302944
-    kappa: 0.0024970371953942179
+    x: 586352.63558359409
+    y: 4140729.0434542252
+    theta: 1.312332478879602
+    kappa: 0.0024890370753367268
     s: 29.923898453764963
-    dkappa: -0.00019819158080973192
+    dkappa: -0.00013604619629041672
     ddkappa: 0
   }
   v: 4.4993086864908554
@@ -1476,12 +1476,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.748415064
-    y: 4140729.4788934239
-    theta: 1.3146401198028999
-    kappa: 0.0023908124163752845
+    x: 586352.75032917608
+    y: 4140729.4785224376
+    theta: 1.3134345491054633
+    kappa: 0.002412876463205098
     s: 30.373843940758029
-    dkappa: -0.00023286847395933666
+    dkappa: -0.00016644672071639623
     ddkappa: 0
   }
   v: 4.4995990263502561
@@ -1490,12 +1490,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.86218124069
-    y: 4140729.9142475557
-    theta: 1.3156877763306341
-    kappa: 0.0022691964440809302
+    x: 586352.86461662082
+    y: 4140729.913740017
+    theta: 1.3144996123577051
+    kappa: 0.0023232269438837284
     s: 30.823817294639156
-    dkappa: -0.0002674943797537192
+    dkappa: -0.0001967960609745189
     ddkappa: 0
   }
   v: 4.4998602520543045
@@ -1504,12 +1504,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586352.97551545594
-    y: 4140730.3497382374
-    theta: 1.3166778622993567
-    kappa: 0.0021322156217509513
+    x: 586352.97846151365
+    y: 4140730.349097474
+    theta: 1.3155217550341431
+    kappa: 0.002220114888063297
     s: 31.273813781193525
-    dkappa: -0.00030205904564420307
+    dkappa: -0.00022708396136468398
     ddkappa: 0
   }
   v: 4.5000549962175187
@@ -1518,12 +1518,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.08844119066
-    y: 4140730.7853498906
-    theta: 1.3176036356910179
-    kappa: 0.0019799027490959974
+    x: 586353.09188092523
+    y: 4140730.7845808435
+    theta: 1.3164950681815588
+    kappa: 0.0021035727413412182
     s: 31.723824656047853
-    dkappa: -0.00033655180460294475
+    dkappa: -0.00025729975663286688
     ddkappa: 0
   }
   v: 4.5001404230565454
@@ -1532,12 +1532,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.2009836426
-    y: 4140731.2210618593
-    theta: 1.3184583655108446
-    kappa: 0.0018122980869065071
+    x: 586353.2048932882
+    y: 4140731.2201711582
+    theta: 1.3174136519909025
+    kappa: 0.0019736398988279931
     s: 32.173836617830652
-    dkappa: -0.00037096158111815766
+    dkappa: -0.00028743238598536559
     ddkappa: 0
   }
   v: 4.5000682283901554
@@ -1546,12 +1546,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.31316959485
-    y: 4140731.6568478877
-    theta: 1.3192353391353684
-    kappa: 0.0016294504625260504
+    x: 586353.31751826988
+    y: 4140731.6558439219
+    theta: 1.3182716218329273
+    kappa: 0.0018303636615558143
     s: 32.6238312613324
-    dkappa: -0.00040527689563367659
+    dkappa: -0.000317470410627985
     ddkappa: 0
   }
   v: 4.499784639639242
@@ -1560,12 +1560,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.42502727825
-    y: 4140732.0926755923
-    theta: 1.3199278865584256
-    kappa: 0.0014316861594617591
+    x: 586353.42977664038
+    y: 4140732.0915685855
+    theta: 1.3190631266122537
+    kappa: 0.001673991131059436
     s: 33.073784530665769
-    dkappa: -0.00043895050994577085
+    dkappa: -0.00034702032417009525
     ddkappa: 0
   }
   v: 4.499230415826827
@@ -1574,12 +1574,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.53658617125
-    y: 4140732.5285059507
-    theta: 1.3205297262179463
-    kappa: 0.0012213857479502124
+    x: 586353.54169009591
+    y: 4140732.527308031
+    theta: 1.3197825973445976
+    kappa: 0.0015062373507940729
     s: 33.523666172425877
-    dkappa: -0.0004675251246856219
+    dkappa: -0.000372898979420025
     ddkappa: 0
   }
   v: 4.4983408475780537
@@ -1588,12 +1588,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.647864067
-    y: 4140732.9642960331
-    theta: 1.3210323779339357
-    kappa: 0.001016383446336086
+    x: 586353.65326449194
+    y: 4140732.9630222954
+    theta: 1.3204223042837537
+    kappa: 0.0013396553649752411
     s: 33.973439188850413
-    dkappa: -0.00045642121550563307
+    dkappa: -0.00037050433211093832
     ddkappa: 0
   }
   v: 4.4970457571201932
@@ -1602,12 +1602,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.75889742328
-    y: 4140733.399990682
-    theta: 1.3214410481187393
-    kappa: 0.00082282680862857027
+    x: 586353.76452744636
+    y: 4140733.3986583659
+    theta: 1.320984608849946
+    kappa: 0.0011786525382853604
     s: 34.423059290979914
-    dkappa: -0.000433039056294211
+    dkappa: -0.00035932419457122854
     ddkappa: 0
   }
   v: 4.49526949828264
@@ -1616,12 +1616,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.86972446414
-    y: 4140733.8355262829
-    theta: 1.3217662931639391
-    kappa: 0.00064013139183087943
+    x: 586353.87551471544
+    y: 4140733.8341531577
+    theta: 1.3214765059284324
+    kappa: 0.0010229868541100659
     s: 34.872474351817992
-    dkappa: -0.00040878950582915727
+    dkappa: -0.00034748465604307372
     ddkappa: 0
   }
   v: 4.492930956496914
@@ -1630,12 +1630,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586353.98036460986
-    y: 4140734.2708354089
-    theta: 1.322012930180591
-    kappa: 0.00046837242863254856
+    x: 586353.98624083854
+    y: 4140734.2694404153
+    theta: 1.3219003643474132
+    kappa: 0.00087273912642728811
     s: 35.321623859491481
-    dkappa: -0.00038439731088126515
+    dkappa: -0.0003354956689750803
     ddkappa: 0
   }
   v: 4.4899435487966608
@@ -1644,12 +1644,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.09083305812
-    y: 4140734.7058425182
-    theta: 1.3221858313535695
-    kappa: 0.00030762541802294933
+    x: 586354.09671719512
+    y: 4140734.7044455092
+    theta: 1.3222586081250218
+    kappa: 0.00072799344644569933
     s: 35.770438370410687
-    dkappa: -0.00035985708722080284
+    dkappa: -0.0003233500815701169
     ddkappa: 0
   }
   v: 4.4862152238176476
@@ -1658,12 +1658,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.20114060189
-    y: 4140735.1404634286
-    theta: 1.3222899243606525
-    kappa: 0.00015796473502476606
+    x: 586354.20695183147
+    y: 4140735.1390849119
+    theta: 1.32255372028781
+    kappa: 0.00058883642237313638
     s: 36.218838962429643
-    dkappa: -0.00033516453013389017
+    dkappa: -0.00031104150958352606
     ddkappa: 0
   }
   v: 4.4816484617977705
@@ -1672,12 +1672,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.31129344529
-    y: 4140735.5746047925
-    theta: 1.3223301911929133
-    kappa: 1.9462084104846207e-05
+    x: 586354.31694927951
+    y: 4140735.5732656703
+    theta: 1.3227882460101072
+    kappa: 0.0004553563095052099
     s: 36.666736688006218
-    dkappa: -0.00031031653346949909
+    dkappa: -0.00029856443658856605
     ddkappa: 0
   }
   v: 4.4761402745770491
@@ -1686,12 +1686,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 586354.42129301711
-    y: 4140736.0081635709
-    theta: 1.3223116651603719
-    kappa: -0.00010781520530582124
+    x: 586354.42671037186
+    y: 4140736.0068848836
+    theta: 1.3229647949391765
+    kappa: 0.00032764202708361758
     s: 37.11403202736242
-    dkappa: -0.00028531130344555634
+    dkappa: -0.00028591431319594066
     ddkappa: 0
   }
   v: 4.4695822055976278
diff --git a/modules/planning/testdata/garage_test/result_out_of_map_0.pb.txt b/modules/planning/testdata/garage_test/result_out_of_map_0.pb.txt
index 6f11a73dcc..5b31979e43 100644
--- a/modules/planning/testdata/garage_test/result_out_of_map_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_out_of_map_0.pb.txt
@@ -9,1693 +9,1693 @@ trajectory_point {
     y: 4140761.7226162944
     z: 0
     theta: 1.2867508012008013
-    kappa: 0.013159962977488119
+    kappa: 0.012977269686623459
     s: 0
     dkappa: 0
     ddkappa: 0
   }
   v: 4.4250001907348633
-  a: -0.398586546595553
+  a: -0.398586425749059
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 586361.40024217626
-    y: 4140761.8075333685
-    theta: 1.2878807360051818
-    kappa: 0.012894164141270084
-    s: 0.0884198587666354
-    dkappa: -0.0010532955625452442
+    x: 586361.40024406614
+    y: 4140761.80753282
+    theta: 1.2878654142356609
+    kappa: 0.012718029093969293
+    s: 0.088419858790962549
+    dkappa: -0.0010273064788495036
     ddkappa: 0
   }
-  v: 4.4169643432556205
-  a: -0.40498934988481627
+  v: 4.4169643456961705
+  a: -0.40498922668794018
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 586361.42483768938
-    y: 4140761.8922948656
-    theta: 1.2890086018870619
-    kappa: 0.012628852274942941
-    s: 0.17667772356223269
-    dkappa: -0.002104661382912276
+    x: 586361.42484146589
+    y: 4140761.89229377
+    theta: 1.2889779863702719
+    kappa: 0.012459263455707255
+    s: 0.17667772366016343
+    dkappa: -0.002052730830678725
     ddkappa: 0
   }
-  v: 4.4088009705226385
-  a: -0.4113390856989026
+  v: 4.4088009754500508
+  a: -0.41133896022109373
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 586361.44934999768
-    y: 4140761.9769091229
-    theta: 1.2901250513113185
-    kappa: 0.012364909416523044
-    s: 0.26477105448972038
-    dkappa: -0.0030026336511018038
+    x: 586361.449356152
+    y: 4140761.9769073394
+    theta: 1.290079399377936
+    kappa: 0.012201824499940393
+    s: 0.26477105471144358
+    dkappa: -0.0029285814096340428
     ddkappa: 0
   }
-  v: 4.4005111330616575
-  a: -0.41763583641401558
+  v: 4.4005111405208623
+  a: -0.41763570872393224
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 586361.47358869645
-    y: 4140762.0614284454
-    theta: 1.2911827602019081
-    kappa: 0.01210678392144025
-    s: 0.35269733284606658
-    dkappa: -0.0029781046918084548
+    x: 586361.47360023018
+    y: 4140762.0614251196
+    theta: 1.2911234908425313
+    kappa: 0.011950007462763534
+    s: 0.35269733324265595
+    dkappa: -0.0029048972718049028
     ddkappa: 0
   }
-  v: 4.3920958897508964
-  a: -0.42387968440635876
+  v: 4.3920958997854589
+  a: -0.42387955457186832
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 586361.49778065528
-    y: 4140762.1457847874
-    theta: 1.2922384301844587
-    kappa: 0.011849156175240946
-    s: 0.44045406108932844
-    dkappa: -0.0029536230322291359
+    x: 586361.497797558
+    y: 4140762.1457799221
+    theta: 1.2921655696320051
+    kappa: 0.011698676009586888
+    s: 0.44045406171271556
+    dkappa: -0.0028812588045885115
     ddkappa: 0
   }
-  v: 4.3835562978210474
-  a: -0.43007071205213576
+  v: 4.3835563104731845
+  a: -0.43007058014031452
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 586361.52185911033
-    y: 4140762.2299946151
-    theta: 1.2932749404718149
-    kappa: 0.01159369193279337
-    s: 0.52803876280570183
-    dkappa: -0.0029291309365870534
+    x: 586361.52188220073
+    y: 4140762.2299879822
+    theta: 1.2931889311549154
+    kappa: 0.011449438373050052
+    s: 0.52803876370864922
+    dkappa: -0.0028576110706966659
     ddkappa: 0
   }
-  v: 4.37489341285528
-  a: -0.43620900172755017
+  v: 4.3748934281658736
+  a: -0.43620886780468338
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 586361.5457081846
-    y: 4140762.3140884205
-    theta: 1.2942624722807363
-    kappa: 0.011343289668270008
-    s: 0.61544898267657067
-    dkappa: -0.0029045272336716225
+    x: 586361.54573972337
+    y: 4140762.3140793918
+    theta: 1.2941644808711403
+    kappa: 0.011205092959320716
+    s: 0.61544898391264524
+    dkappa: -0.0028338577927566229
     ddkappa: 0
   }
-  v: 4.3661082887892393
-  a: -0.44229463580880551
+  v: 4.3661083067978552
+  a: -0.44229449994038761
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 586361.56950898876
-    y: 4140762.398012023
-    theta: 1.2952480056896276
-    kappa: 0.011093394211676058
-    s: 0.70268228644555653
-    dkappa: -0.0028799733280363967
+    x: 586361.56954895915
+    y: 4140762.3980006026
+    theta: 1.2951380564271997
+    kappa: 0.010961242094452256
+    s: 0.7026822880691036
+    dkappa: -0.002810152590842923
     ddkappa: 0
   }
-  v: 4.3572019779110471
-  a: -0.44832769667210537
+  v: 4.3572019986559463
+  a: -0.44832755892283971
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 586361.59317394393
-    y: 4140762.4817876206
-    theta: 1.2962081564538068
-    kappa: 0.010846353579565728
-    s: 0.78973626088556825
-    dkappa: -0.0028554012993277453
+    x: 586361.59322335315
+    y: 4140762.4817735362
+    theta: 1.2960868361832949
+    kappa: 0.010720152469734914
+    s: 0.78973626295168531
+    dkappa: -0.0027864306911445226
     ddkappa: 0
   }
-  v: 4.3481755308613
-  a: -0.45430826669365337
+  v: 4.34817555437946
+  a: -0.45430812712745222
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 586361.61664545652
-    y: 4140762.5654289764
-    theta: 1.2971275996355176
-    kappa: 0.010603708251402612
-    s: 0.87660851376585136
-    dkappa: -0.0028307668821789629
+    x: 586361.61670597934
+    y: 4140762.5654117735
+    theta: 1.2969958627204359
+    kappa: 0.010483311336791905
+    s: 0.87660851633036285
+    dkappa: -0.0027626498786633832
     ddkappa: 0
   }
-  v: 4.3390299966330721
-  a: -0.46023642824965305
+  v: 4.3390300229601984
+  a: -0.46023628692963781
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 586361.6400672301
-    y: 4140762.6488930867
-    theta: 1.2980450945240414
-    kappa: 0.010361577118082641
-    s: 0.9632966738190375
-    dkappa: -0.0028061846683442009
+    x: 586361.64013884286
+    y: 4140762.6488727704
+    theta: 1.2979029630351608
+    kappa: 0.010246972098742296
+    s: 0.96329667693846932
+    dkappa: -0.0027389194605888095
     ddkappa: 0
   }
-  v: 4.32976642257191
-  a: -0.46611226371630804
+  v: 4.3297664517424552
+  a: -0.46611212070480906
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586361.66333800077
-    y: 4140762.7322057853
-    theta: 1.2989324688232258
-    kappa: 0.010122879287280414
-    s: 1.0497983907081943
-    dkappa: -0.0027815849776589023
+    x: 586361.66342178278
+    y: 4140762.7321820715
+    theta: 1.2987806349961883
+    kappa: 0.010013952567314379
+    s: 1.0497983944397491
+    dkappa: -0.0027151728153811975
     ddkappa: 0
   }
-  v: 4.32038585437584
-  a: -0.47193585546982186
+  v: 4.3203858864230167
+  a: -0.47193571082837854
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 586361.6864437164
-    y: 4140762.8153685792
-    theta: 1.2997859451592022
-    kappa: 0.0098880097617277968
-    s: 1.1361113349938745
-    dkappa: -0.0027569590873318893
+    x: 586361.68654088955
+    y: 4140762.8153411443
+    theta: 1.2996251896209985
+    kappa: 0.0097846342200645434
+    s: 1.1361113393954068
+    dkappa: -0.0026914016055247978
     ddkappa: 0
   }
-  v: 4.3108893360953635
-  a: -0.47770728588639821
+  v: 4.3108893710511609
+  a: -0.47770713967575879
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586361.70949828031
-    y: 4140762.8983472656
-    theta: 1.3006375320457537
-    kappa: 0.0096536601946365878
-    s: 1.2222331981011656
-    dkappa: -0.0027323877142523363
+    x: 586361.70960881491
+    y: 4140762.8983161179
+    theta: 1.3004678745482749
+    kappa: 0.0095558235418165029
+    s: 1.2222332032311576
+    dkappa: -0.0026676830207920483
     ddkappa: 0
   }
-  v: 4.3012779101334555
-  a: -0.48342663734224051
+  v: 4.301277948028658
+  a: -0.48342648962236245
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586361.7323931033
-    y: 4140762.9811695004
-    theta: 1.3014557021294253
-    kappa: 0.00942320115618361
-    s: 1.308161692286739
-    dkappa: -0.0027078052656494857
+    x: 586361.73251805513
+    y: 4140762.9811343681
+    theta: 1.3012779032311326
+    kappa: 0.0093307740156106239
+    s: 1.3081616982042767
+    dkappa: -0.00264395416465768
     ddkappa: 0
   }
-  v: 4.2915526172455705
-  a: -0.48909399221355243
+  v: 4.2915526581097687
+  a: -0.48909384304360209
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 586361.75514446362
-    y: 4140763.0638284292
-    theta: 1.3022453608366271
-    kappa: 0.0091961127080312042
-    s: 1.3938945506059008
-    dkappa: -0.002683222747590773
+    x: 586361.75528471428
+    y: 4140763.063789086
+    theta: 1.3020600642439268
+    kappa: 0.0091089839751615949
+    s: 1.3938945573706503
+    dkappa: -0.0026202255930029158
     ddkappa: 0
   }
-  v: 4.2817144965396361
-  a: -0.49470943287653751
+  v: 4.2817145404012464
+  a: -0.49470928231489031
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 586361.7778433112
-    y: 4140763.1462965719
-    theta: 1.3030331968450148
-    kappa: 0.0089695484081166316
-    s: 1.4794295268796398
-    dkappa: -0.0026586969690255342
+    x: 586361.77799882507
+    y: 4140763.1462530261
+    theta: 1.3028404198660337
+    kappa: 0.0088877058534089469
+    s: 1.4794295345518234
+    dkappa: -0.00259655178981045
     ddkappa: 0
   }
-  v: 4.2717645854760562
-  a: -0.50027304170739928
+  v: 4.2717646323623368
+  a: -0.50027288981163964
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 586361.80037931213
-    y: 4140763.2286018557
-    theta: 1.3037857108464863
-    kappa: 0.0087472033037047335
-    s: 1.5647643956616777
-    dkappa: -0.0026341701294366185
+    x: 586361.80055104755
+    y: 4140763.2285538688
+    theta: 1.303586247523671
+    kappa: 0.0086705062813188481
+    s: 1.5647644043020523
+    dkappa: -0.0025728771446783135
     ddkappa: 0
   }
-  v: 4.2617039198677125
-  a: -0.50578490108234153
+  v: 4.2617039698047758
+  a: -0.50578474790926276
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586361.82278741349
-    y: 4140763.3107324173
-    theta: 1.3045137230325519
-    kappa: 0.0085278887786177128
-    s: 1.6498969522055185
-    dkappa: -0.0026096617819780388
+    x: 586361.8229760027
+    y: 4140763.3106798315
+    theta: 1.3043081149253237
+    kappa: 0.0084562381736389622
+    s: 1.6498969618753514
+    dkappa: -0.0025492204704066841
     ddkappa: 0
   }
-  v: 4.2515335338799609
-  a: -0.51124509337756763
+  v: 4.2515335868927906
+  a: -0.51124493898317214
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586361.84514168848
-    y: 4140763.3926656935
-    theta: 1.3052399863780819
-    kappa: 0.0083091010674154683
-    s: 1.7348250124314988
-    dkappa: -0.0025852123058492491
+    x: 586361.845347091
+    y: 4140763.3926085196
+    theta: 1.3050282482495377
+    kappa: 0.0082424847577182853
+    s: 1.7348250231925455
+    dkappa: -0.0025256206216487985
     ddkappa: 0
   }
-  v: 4.2412544600306337
-  a: -0.51665370096928132
+  v: 4.2412545161431039
+  a: -0.51665354540878039
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586361.86733485677
-    y: 4140763.4744285932
-    theta: 1.3059303558691933
-    kappa: 0.00809472630084467
-    s: 1.819546412893835
-    dkappa: -0.0025607736758162243
+    x: 586361.86755788035
+    y: 4140763.4743666365
+    theta: 1.3057132828706322
+    kappa: 0.0080329971128685618
+    s: 1.8195464248083184
+    dkappa: -0.0025020312033955092
     ddkappa: 0
   }
-  v: 4.2308677291900381
-  a: -0.5220108062336859
+  v: 4.2308677884249253
+  a: -0.52201064956150023
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 586361.88941042987
-    y: 4140763.5560070723
-    theta: 1.3065989093283914
-    kappa: 0.0078831671901274138
-    s: 1.9040590107476765
-    dkappa: -0.0025363666021592929
+    x: 586361.88965152774
+    y: 4140763.5559402239
+    theta: 1.3063769733396704
+    kappa: 0.00782623368019212
+    s: 1.9040590238782635
+    dkappa: -0.002478472220593084
     ddkappa: 0
   }
-  v: 4.22037437058096
-  a: -0.52731649154698523
+  v: 4.22037443295996
+  a: -0.527316333816744
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586361.9114309072
-    y: 4140763.6373819495
-    theta: 1.30726579414368
-    kappa: 0.0076721360844923519
-    s: 1.9883606837161516
-    dkappa: -0.0025120204431984417
+    x: 586361.91169003421
+    y: 4140763.6373102209
+    theta: 1.3070390073040423
+    kappa: 0.0076199862834768275
+    s: 1.9883606981259325
+    dkappa: -0.0024549720358197722
     ddkappa: 0
   }
-  v: 4.2097754117786588
-  a: -0.53257083928538285
+  v: 4.2097754773224034
+  a: -0.53257068054992451
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586361.93329607975
-    y: 4140763.718578062
-    theta: 1.3078974824979759
-    kappa: 0.0074655712022495962
-    s: 2.0724493300574194
-    dkappa: -0.0024876975903417057
+    x: 586361.93357386079
+    y: 4140763.71850131
+    theta: 1.3076666113829654
+    kappa: 0.00741805633589949
+    s: 2.0724493458098867
+    dkappa: -0.0024314941372983183
     ddkappa: 0
   }
-  v: 4.1990718787108694
-  a: -0.53777393182508226
+  v: 4.1990719474389406
+  a: -0.53777377213645428
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586361.95504947274
-    y: 4140763.7995815212
-    theta: 1.308508776831792
-    kappa: 0.0072617386310415092
-    s: 2.1563228685317171
-    dkappa: -0.0024634155432682586
+    x: 586361.95534623275
+    y: 4140763.799499671
+    theta: 1.308274253573483
+    kappa: 0.0072187703643817637
+    s: 2.1563228856907446
+    dkappa: -0.0024080555083598959
     ddkappa: 0
   }
-  v: 4.1882647956578047
-  a: -0.542925851542287
+  v: 4.188264867588753
+  a: -0.54292569095174581
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586361.9767465411
-    y: 4140763.8803752428
-    theta: 1.3091184883161684
-    kappa: 0.0070584338310780044
-    s: 2.2399792383684134
-    dkappa: -0.0024391963682183007
+    x: 586361.97706223093
+    y: 4140763.8802883085
+    theta: 1.3088803223724395
+    kappa: 0.0070200003917140676
+    s: 2.2399792569982355
+    dkappa: -0.0023846775676148587
     ddkappa: 0
   }
-  v: 4.1773551852521535
-  a: -0.54802668081320083
+  v: 4.17735526040351
+  a: -0.54802651937121172
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586361.99829739216
-    y: 4140763.96098117
-    theta: 1.309694899808141
-    kappa: 0.00685950318772356
-    s: 2.3234163992330532
-    dkappa: -0.0024150125002588459
+    x: 586361.99863244034
+    y: 4140763.96088906
+    theta: 1.309453802377083
+    kappa: 0.0068254592816308461
+    s: 2.3234164193982454
+    dkappa: -0.0023613333827391088
     ddkappa: 0
   }
-  v: 4.1663440684790771
-  a: -0.55307650201402714
+  v: 4.1663441468673739
+  a: -0.55307633977026471
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586362.01973855519
-    y: 4140764.0413874378
-    theta: 1.3102511418389642
-    kappa: 0.0066633588231892939
-    s: 2.4066323311944111
-    dkappa: -0.00239087619845933
+    x: 586362.02009318932
+    y: 4140764.0412901035
+    theta: 1.3100075324859761
+    kappa: 0.0066336144575888812
+    s: 2.4066323529598685
+    dkappa: -0.0023380349209844264
     ddkappa: 0
   }
-  v: 4.1552324646762182
-  a: -0.55807539752096968
+  v: 4.1552325463169995
+  a: -0.55807523452431729
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586362.04112220171
-    y: 4140764.1215780135
-    theta: 1.310805891699977
-    kappa: 0.0064677406199829205
-    s: 2.4896250346915374
-    dkappa: -0.0023668046427902227
+    x: 586362.04149636941
+    y: 4140764.12147547
+    theta: 1.3105597771643331
+    kappa: 0.0064422842611080193
+    s: 2.4896250581224577
+    dkappa: -0.002314798957817393
     ddkappa: 0
   }
-  v: 4.14402139153369
-  a: -0.56302344971023188
+  v: 4.144021476441532
+  a: -0.563023286008782
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 586362.06237124035
-    y: 4140764.2015713416
-    theta: 1.3113303612801746
-    kappa: 0.0062762548432097436
-    s: 2.5723925305008111
-    dkappa: -0.0023427792256096557
+    x: 586362.06276517943
+    y: 4140764.2014635443
+    theta: 1.31108237364324
+    kappa: 0.006254949887533094
+    s: 2.572392555662673
+    dkappa: -0.0022916071508809295
     ddkappa: 0
   }
-  v: 4.1327118650940848
-  a: -0.56792074095801737
+  v: 4.1327119532826089
+  a: -0.56792057659907136
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 586362.08350971225
-    y: 4140764.2813590029
-    theta: 1.3118337607467683
-    kappa: 0.006087751723858178
-    s: 2.6549328597029871
-    dkappa: -0.0023188066632185353
+    x: 586362.08392356546
+    y: 4140764.28124593
+    theta: 1.3115843312440483
+    kappa: 0.0060705015812554958
+    s: 2.6549328866615327
+    dkappa: -0.0022684661050105821
     ddkappa: 0
   }
-  v: 4.1213048997524711
-  a: -0.57276735364052977
+  v: 4.1213049912343607
+  a: -0.57276718867059817
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 586362.10458951048
-    y: 4140764.3609252
-    theta: 1.3123357629225016
-    kappa: 0.0058997718281916783
-    s: 2.7372440836502463
-    dkappa: -0.0022949006409063158
+    x: 586362.10502322274
+    y: 4140764.3608068647
+    theta: 1.3120848955567257
+    kappa: 0.00588656524365486
+    s: 2.7372441124714624
+    dkappa: -0.0022453892911783423
     ddkappa: 0
   }
-  v: 4.1098015082563908
-  a: -0.57756337013397263
+  v: 4.1098016030434081
+  a: -0.57756320459877486
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586362.12554808462
-    y: 4140764.44028447
-    theta: 1.312811546612126
-    kappa: 0.0057155290987743416
-    s: 2.8193242839332457
-    dkappa: -0.0022710499630960077
+    x: 586362.1260017259
+    y: 4140764.4401608715
+    theta: 1.3125597738388404
+    kappa: 0.0057062434805361818
+    s: 2.8193243146833451
+    dkappa: -0.00222236552276846
     ddkappa: 0
   }
-  v: 4.0982027017058646
-  a: -0.58230887281454946
+  v: 4.0982027998088624
+  a: -0.582308706759014
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 586362.14639298222
-    y: 4140764.5194328576
-    theta: 1.3132643121281888
-    kappa: 0.0055346123886576178
-    s: 2.9011715623481678
-    dkappa: -0.0022472567025006744
+    x: 586362.14686660562
+    y: 4140764.5193039952
+    theta: 1.3130120979676649
+    kappa: 0.0055291394601358953
+    s: 2.90117159509357
+    dkappa: -0.0021993968496793921
     ddkappa: 0
   }
-  v: 4.0865094895533867
-  a: -0.587003944058464
+  v: 4.086509590982331
+  a: -0.58700377752672839
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586362.167178081
-    y: 4140764.5983541878
-    theta: 1.3137157787717451
-    kappa: 0.0053542146845010647
-    s: 2.9827840408637676
-    dkappa: -0.0022235316989726557
+    x: 586362.16767162911
+    y: 4140764.5982200778
+    theta: 1.31346312449061
+    kappa: 0.0053525435078614391
+    s: 2.9827840756710851
+    dkappa: -0.002176494068096699
     ddkappa: 0
   }
-  v: 4.0747228796039288
-  a: -0.59164866624191959
+  v: 4.0747229843679076
+  a: -0.59164849927733032
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586362.187856388
-    y: 4140764.6770588821
-    theta: 1.3141460446155282
-    kappa: 0.0051770018499456118
-    s: 3.0641598615884278
-    dkappa: -0.0021998693718677212
+    x: 586362.18836980232
+    y: 4140764.676919552
+    theta: 1.313893365922155
+    kappa: 0.0051790292640609172
+    s: 3.0641598985244443
+    dkappa: -0.0021536514653794424
     ddkappa: 0
   }
-  v: 4.0628438780149381
-  a: -0.59624312174112
+  v: 4.0628439861221821
+  a: -0.59624295438623254
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 586362.208416397
-    y: 4140764.7555480632
-    theta: 1.3145503798629865
-    kappa: 0.0050036095259242607
-    s: 3.145297186737201
-    dkappa: -0.0021762688031136848
+    x: 586362.20894961967
+    y: 4140764.7554035438
+    theta: 1.3142981913923821
+    kappa: 0.005009210345310015
+    s: 3.1452972258688572
+    dkappa: -0.0021308680782474807
     ddkappa: 0
   }
-  v: 4.0508734892963378
-  a: -0.6007873929322689
+  v: 4.0508736007542332
+  a: -0.60078722522884753
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 586362.22891551128
-    y: 4140764.8338047746
-    theta: 1.3149535175608758
-    kappa: 0.0048307307569069565
-    s: 3.2261941985988654
-    dkappa: -0.0021527381347445146
+    x: 586362.22946848371
+    y: 4140764.8336550817
+    theta: 1.3147018178615117
+    kappa: 0.0048398943976640588
+    s: 3.2261942399932422
+    dkappa: -0.0021081521711435662
     ddkappa: 0
   }
-  v: 4.0388127163105265
-  a: -0.6052815621915697
+  v: 4.0388128311256342
+  a: -0.605281394180588
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 586362.24932261335
-    y: 4140764.9118352891
-    theta: 1.3153413370930451
-    kappa: 0.004660324294306354
-    s: 3.3068490995029722
-    dkappa: -0.0021292755392731907
+    x: 586362.24989520281
+    y: 4140764.9116804656
+    theta: 1.3150904115315887
+    kappa: 0.0046729725367958786
+    s: 3.3068491432272715
+    dkappa: -0.0020855017438270664
     ddkappa: 0
   }
-  v: 4.02666256027238
-  a: -0.609725711895226
+  v: 4.026662678450446
+  a: -0.60972554361686648
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 586362.269605981
-    y: 4140764.989646059
-    theta: 1.315699437244944
-    kappa: 0.004494387766791918
-    s: 3.3872601117868939
-    dkappa: -0.0021058791365996285
+    x: 586362.27019798232
+    y: 4140764.9894861747
+    theta: 1.3154498654610873
+    kappa: 0.0045103731736033824
+    s: 3.3872601579084254
+    dkappa: -0.0020629147408827418
     ddkappa: 0
   }
-  v: 4.0144240207492476
-  a: -0.6141199244194413
+  v: 4.0144241422952263
+  a: -0.61411975591309553
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 586362.2898273851
-    y: 4140765.067219126
-    theta: 1.3160564434642892
-    kappa: 0.0043289581561061444
-    s: 3.4674254777628741
-    dkappa: -0.0020825542072202413
+    x: 586362.29043873912
+    y: 4140765.0670541967
+    theta: 1.31580822132298
+    kappa: 0.0043482705324388391
+    s: 3.4674255263490403
+    dkappa: -0.0020403967385893892
     ddkappa: 0
   }
-  v: 4.0020980956609566
-  a: -0.61846428214041937
+  v: 4.00209822057902
+  a: -0.61846411344468777
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 586362.3099712322
-    y: 4140765.1445567505
-    theta: 1.3164047836438149
-    kappa: 0.0041651249996361005
-    s: 3.547343459685079
-    dkappa: -0.0020593008922378032
+    x: 586362.310601799
+    y: 4140765.1443868172
+    theta: 1.3161580562383524
+    kappa: 0.0041877166667692025
+    s: 3.5473435108033562
+    dkappa: -0.0020179477479129924
     ddkappa: 0
   }
-  v: 3.9896857812798103
-  a: -0.62275886743436359
+  v: 3.989685909573367
+  a: -0.62275869858705579
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 586362.32998576178
-    y: 4140765.2216706215
-    theta: 1.3167188320025338
-    kappa: 0.0040065696699933245
-    s: 3.627012339716646
-    dkappa: -0.0020361184569692762
+    x: 586362.33063513949
+    y: 4140765.2214958058
+    theta: 1.3164742547818198
+    kappa: 0.00403226548345189
+    s: 3.6270123934345717
+    dkappa: -0.0019955666356948885
     ddkappa: 0
   }
-  v: 3.9771880722305877
-  a: -0.62700376267747782
+  v: 3.9771882039022968
+  a: -0.62700359371561221
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 586362.34993728506
-    y: 4140765.2985417349
-    theta: 1.3170318917528829
-    kappa: 0.0038485134769407747
-    s: 3.7064304198967304
-    dkappa: -0.002013009000652129
+    x: 586362.35060541448
+    y: 4140765.2983620539
+    theta: 1.3167894579488841
+    kappa: 0.0038773036646379475
+    s: 3.7064304762818883
+    dkappa: -0.0019732559798196171
     ddkappa: 0
   }
-  v: 3.9646059614905425
-  a: -0.63119905024596534
+  v: 3.9646060965423318
+  a: -0.6311988812057695
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 586362.36982474173
-    y: 4140765.3751686336
-    theta: 1.3173436073227727
-    kappa: 0.0036910117271119203
-    s: 3.78559602210756
-    dkappa: -0.0019899730285988446
+    x: 586362.37051155535
+    y: 4140765.3749841033
+    theta: 1.3171033156187184
+    kappa: 0.0037228848376916653
+    s: 3.7855960812275651
+    dkappa: -0.0019510162627424988
     ddkappa: 0
   }
-  v: 3.951940440389405
-  a: -0.63534481251602992
+  v: 3.9519405788224859
+  a: -0.63534464343294028
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 586362.389577789
-    y: 4140765.4515678268
-    theta: 1.3176157721432091
-    kappa: 0.00353975749360173
-    s: 3.86450748804148
-    dkappa: -0.0019670128852764088
+    x: 586362.3902826265
+    y: 4140765.4513786361
+    theta: 1.3173783608855556
+    kappa: 0.0035745051095420147
+    s: 3.8645075499639647
+    dkappa: -0.0019288491532675532
     ddkappa: 0
   }
-  v: 3.9391924986093825
-  a: -0.63944113186387508
+  v: 3.9391926404242645
+  a: -0.63944096277253726
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 586362.40926681086
-    y: 4140765.5277193869
-    theta: 1.3178870548227237
-    kappa: 0.0033889935186615138
-    s: 3.9431631791680073
-    dkappa: -0.0019441271623967626
+    x: 586362.409989614
+    y: 4140765.5275255521
+    theta: 1.3176525146761404
+    kappa: 0.0034266063227176326
+    s: 3.9431632439606061
+    dkappa: -0.0019067538937625542
     ddkappa: 0
   }
-  v: 3.9263631241851567
-  a: -0.64348809066570434
+  v: 3.9263632693816652
+  a: -0.64348792159997281
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 586362.4288914022
-    y: 4140765.6036217483
-    theta: 1.3181574497381732
-    kappa: 0.0032387229051138297
-    s: 4.0215614767008718
-    dkappa: -0.0019213163309632561
+    x: 586362.429632112
+    y: 4140765.6034232844
+    theta: 1.3179257713069585
+    kappa: 0.0032791915210735045
+    s: 4.0215615444312123
+    dkappa: -0.0018847309389625761
     ddkappa: 0
   }
-  v: 3.9134533035038865
-  a: -0.64748577129772134
+  v: 3.9134534520811775
+  a: -0.64748560229065966
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 586362.44840215961
-    y: 4140765.6792860106
-    theta: 1.3183973125608082
-    kappa: 0.0030935347573619178
-    s: 4.09970078156508
-    dkappa: -0.0018985846063767451
+    x: 586362.449160092
+    y: 4140765.679083107
+    theta: 1.3181690392170042
+    kappa: 0.0031366916814839774
+    s: 4.09970085230076
+    dkappa: -0.0018627839312914804
     ddkappa: 0
   }
-  v: 3.9004640213052055
-  a: -0.65143425613612971
+  v: 3.9004641732617813
+  a: -0.65143408722001039
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 586362.4678356091
-    y: 4140765.7547011161
-    theta: 1.31862896899567
-    kappa: 0.0029499767896718166
-    s: 4.1775795143639449
-    dkappa: -0.0018759296226735806
+    x: 586362.4686105533
+    y: 4140765.75449383
+    theta: 1.3184042294690599
+    kappa: 0.0029957732618961629
+    s: 4.1775795881725326
+    dkappa: -0.0018409109082974239
     ddkappa: 0
   }
-  v: 3.887396260681224
-  a: -0.65533362755713287
+  v: 3.88739641601495
+  a: -0.65533345876343763
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586362.56381878338
-    y: 4140766.1279897494
-    theta: 1.3196415728097102
-    kappa: 0.002261106059871809
-    s: 4.5630106918247835
-    dkappa: -0.001763832741140057
+    x: 586362.56467432133
+    y: 4140766.1277617468
+    theta: 1.3194367305816388
+    kappa: 0.002319220938302433
+    s: 4.5630107820092523
+    dkappa: -0.0017326815033389116
     ddkappa: 0
   }
-  v: 3.82091457407898
-  a: -0.67409666656819778
+  v: 3.820914746245224
+  a: -0.67409649883116052
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586362.6577811942
-    y: 4140766.4948386289
-    theta: 1.3203679800584569
-    kappa: 0.0016294288498407659
-    s: 4.94170191551911
-    dkappa: -0.0016537574405180832
+    x: 586362.658707246
+    y: 4140766.4945925763
+    theta: 1.320187339705541
+    kappa: 0.001698089662688314
+    s: 4.9417020237562728
+    dkappa: -0.0016264011413113894
     ddkappa: 0
   }
-  v: 3.7526174433344042
-  a: -0.69164422657467206
+  v: 3.7526176321932576
+  a: -0.69164406056268213
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586362.74982481531
-    y: 4140766.8550402424
-    theta: 1.320873627848453
-    kappa: 0.0010465636292645085
-    s: 5.3134777012660077
-    dkappa: -0.001545747843250723
+    x: 586362.75081157754
+    y: 4140766.8547786903
+    theta: 1.3207200932770302
+    kappa: 0.0011242935464075203
+    s: 5.3134778292155085
+    dkappa: -0.0015221138515309097
     ddkappa: 0
   }
-  v: 3.6826259014966833
-  a: -0.70798660460200313
+  v: 3.6826261068463415
+  a: -0.70798644088457563
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 586362.83996584616
-    y: 4140767.20842167
-    theta: 1.3211485116978936
-    kappa: 0.000517314705022565
-    s: 5.6781746167043456
-    dkappa: -0.0014398560940083931
+    x: 586362.84100118023
+    y: 4140767.2081477465
+    theta: 1.3210248509820293
+    kappa: 0.00060247549890623663
+    s: 5.678174766002968
+    dkappa: -0.0014198707463363839
     ddkappa: 0
   }
-  v: 3.61105995191246
-  a: -0.72313409767563852
+  v: 3.6110601734991108
+  a: -0.723133936723414
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586362.928260604
-    y: 4140767.55481217
-    theta: 1.3212532176237854
-    kappa: 3.2624058833733978e-05
-    s: 6.0356411783225346
-    dkappa: -0.0013361074291503906
+    x: 586362.92933387181
+    y: 4140767.5545285982
+    theta: 1.3211610371469305
+    kappa: 0.00012389458324778993
+    s: 6.03564135057949
+    dkappa: -0.0013196970257271896
     ddkappa: 0
   }
-  v: 3.5380385682258306
-  a: -0.73709700282102575
+  v: 3.5380388057535415
+  a: -0.73709684500577033
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586363.01472996094
-    y: 4140767.8940623114
-    theta: 1.3211819068213408
-    kappa: -0.0004032683308968283
-    s: 6.385737748488264
-    dkappa: -0.0012345387148812146
+    x: 586363.01582857373
+    y: 4140767.8937723041
+    theta: 1.3211225825548092
+    kappa: -0.00030735080169037578
+    s: 6.3857379452814795
+    dkappa: -0.0012216289389741625
     ddkappa: 0
   }
-  v: 3.4636796943783477
-  a: -0.74988561706361234
+  v: 3.4636799475189535
+  a: -0.74988546265821787
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 586363.0993925964
-    y: 4140768.2260353463
-    theta: 1.3209749041908281
-    kappa: -0.00079656429431974093
-    s: 6.7283364324782564
-    dkappa: -0.001135171624271842
+    x: 586363.10050535621
+    y: 4140768.225741758
+    theta: 1.3209490654786011
+    kappa: -0.00069724656380705454
+    s: 6.7283366553516393
+    dkappa: -0.0011256877092019524
     ddkappa: 0
   }
-  v: 3.38810024460902
-  a: -0.7615102374288456
+  v: 3.388100513012009
+  a: -0.76151008660732944
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586363.18225351023
-    y: 4140768.5506100091
-    theta: 1.3206551469552044
-    kappa: -0.0011502854382205035
-    s: 7.0633209755080033
-    dkappa: -0.0010380297559395069
+    x: 586363.18336989114
+    y: 4140768.5503155254
+    theta: 1.3206629425097918
+    kappa: -0.0010487064741255953
+    s: 7.0633212259697062
+    dkappa: -0.0010318962764654434
     ddkappa: 0
   }
-  v: 3.3114161034543086
-  a: -0.77198116094217317
+  v: 3.3114163867567132
+  a: -0.77198101377967832
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586363.26333943114
-    y: 4140768.8676713584
-    theta: 1.320221145593353
-    kappa: -0.0014606758655126765
-    s: 7.3905866597615173
-    dkappa: -0.00094313624785842976
+    x: 586363.264447432
+    y: 4140768.8673790479
+    theta: 1.320262392753937
+    kappa: -0.0013581009699783757
+    s: 7.3905869392831853
+    dkappa: -0.00094027733476890855
     ddkappa: 0
   }
-  v: 3.2337421257481331
-  a: -0.78130868462904268
+  v: 3.2337424235844128
+  a: -0.78130854110183745
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586363.34263036423
-    y: 4140769.1771281553
-    theta: 1.3197086773587714
-    kappa: -0.0017350972802407227
-    s: 7.7100402014210765
-    dkappa: -0.00085050920901885685
+    x: 586363.34372040094
+    y: 4140769.1768404795
+    theta: 1.3197827208556983
+    kappa: -0.0016325367898073996
+    s: 7.7100405114380814
+    dkappa: -0.00085084806723954387
     ddkappa: 0
   }
-  v: 3.1551921366218671
-  a: -0.78950310551490133
+  v: 3.155192448633799
+  a: -0.78950296550038035
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586363.42012007418
-    y: 4140769.4788973276
-    theta: 1.3191318546695352
-    kappa: -0.0019753545055661774
-    s: 8.0215996554002267
-    dkappa: -0.00076016741139643522
+    x: 586363.42118321615
+    y: 4140769.4786165906
+    theta: 1.3192377379298914
+    kappa: -0.0018737537456020906
+    s: 8.02159999731232
+    dkappa: -0.00076362648290719
     ddkappa: 0
   }
-  v: 3.0758792347897232
-  a: -0.79656586431942089
+  v: 3.0758795606131115
+  a: -0.79656572828992855
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 586363.49580091785
-    y: 4140769.7729078191
-    theta: 1.318503435149982
-    kappa: -0.0021831309418617012
-    s: 8.3251943893572413
-    dkappa: -0.00067212763641762727
+    x: 586363.49682891637
+    y: 4140769.7726361607
+    theta: 1.3186399365532797
+    kappa: -0.0020833753543603446
+    s: 8.32519476452385
+    dkappa: -0.0006786286183748726
     ddkappa: 0
   }
-  v: 2.9959165086002923
-  a: -0.802502277854648
+  v: 2.9959168477833535
+  a: -0.80250214685326116
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586363.56968715054
-    y: 4140770.0590945096
-    theta: 1.3178294665493722
-    kappa: -0.0023571133593622041
-    s: 8.6207650257307975
-    dkappa: -0.00058640313218599445
+    x: 586363.57067152823
+    y: 4140770.0588341462
+    theta: 1.3179950225324921
+    kappa: -0.0022601318122199582
+    s: 8.6207654354611769
+    dkappa: -0.00059586716291323057
     ddkappa: 0
   }
-  v: 2.9154157870223032
-  a: -0.80732845728459235
+  v: 2.9154161390157061
+  a: -0.80732833221918621
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586363.6417435552
-    y: 4140770.3374164812
-    theta: 1.3171291169383494
-    kappa: -0.0025025073057065752
-    s: 8.9082632893006242
-    dkappa: -0.00050300792469842631
+    x: 586363.64267781179
+    y: 4140770.3371691266
+    theta: 1.3173220398946721
+    kappa: -0.0024090487059747062
+    s: 8.9082637348447857
+    dkappa: -0.00051535528284969788
     ddkappa: 0
   }
-  v: 2.8344872879080829
-  a: -0.81106051377326349
+  v: 2.8344876520784696
+  a: -0.81106039541651187
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 586363.71195620007
-    y: 4140770.6078386372
-    theta: 1.3164123882043441
-    kappa: -0.0026212087098707263
-    s: 9.187651846075866
-    dkappa: -0.00042195273847886792
+    x: 586363.712834727
+    y: 4140770.6076057851
+    theta: 1.3166308396471094
+    kappa: -0.0025319548908258175
+    s: 9.1876523286168457
+    dkappa: -0.0004371030984979935
     ddkappa: 0
   }
-  v: 2.7532396179935588
-  a: -0.81371455848467122
+  v: 2.7532399936370631
+  a: -0.81371444747404609
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 586363.78031175327
-    y: 4140770.8703368623
-    theta: 1.315688063585666
-    kappa: -0.0027149872029001328
-    s: 9.4589041421834388
-    dkappa: -0.000343245826137522
+    x: 586363.78112980013
+    y: 4140770.8701197924
+    theta: 1.3159300841467083
+    kappa: -0.0026305581218662361
+    s: 9.4589046628309248
+    dkappa: -0.00036111836501148654
     ddkappa: 0
   }
-  v: 2.6717797728982569
-  a: -0.81530670258282512
+  v: 2.671780159254026
+  a: -0.81530659942059713
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 586363.84679681
-    y: 4140771.1248980374
-    theta: 1.3149640058172909
-    kappa: -0.0027856231584445471
-    s: 9.7220042427563929
-    dkappa: -0.00026689289886442323
+    x: 586363.84755048668
+    y: 4140771.1246978119
+    theta: 1.3152275476727817
+    kappa: -0.0027065767815921912
+    s: 9.7220048025417
+    dkappa: -0.00028740640099123417
     ddkappa: 0
   }
-  v: 2.5902131371253021
-  a: -0.815853057231735
+  v: 2.5902135333890159
+  a: -0.81585296228497306
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 586363.91139825259
-    y: 4140771.3715197863
-    theta: 1.3142471498570942
-    kappa: -0.0028349038806562187
-    s: 9.9769466708222652
-    dkappa: -0.00019289700868834017
+    x: 586363.91208451823
+    y: 4140771.3713372517
+    theta: 1.3145301090234653
+    kappa: -0.0027617362062286365
+    s: 9.97694727069467
+    dkappa: -0.00021596998626895408
     ddkappa: 0
   }
-  v: 2.5086434840614178
-  a: -0.81536973359541043
+  v: 2.5086438893988103
+  a: -0.815369647095982
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 586363.97410361271
-    y: 4140771.6102102227
-    theta: 1.3135434985748351
-    kappa: -0.00286461980725796
-    s: 10.223736246191452
-    dkappa: -0.00012125843437668115
+    x: 586363.97472025035
+    y: 4140771.6100460119
+    theta: 1.3138437469774018
+    kappa: -0.0027977650275674429
+    s: 10.223736887015868
+    dkappa: -0.00014680925477968939
     ddkappa: 0
   }
-  v: 2.4271729759769274
-  a: -0.8138728428378611
+  v: 2.4271733895373049
+  a: -0.813872764882432
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 586364.03490143479
-    y: 4140771.8409876935
-    theta: 1.3128581213555264
-    kappa: -0.0028765607352134884
-    s: 10.462387924345558
-    dkappa: -5.1974585885145711e-05
+    x: 586364.0354470131
+    y: 4140771.8408422349
+    theta: 1.3131735385725527
+    kappa: -0.0028163915370254377
+    s: 10.46238860690157
+    dkappa: -7.9921604941341726e-05
     ddkappa: 0
   }
-  v: 2.3459021640257527
-  a: -0.81137849612309665
+  v: 2.3459025849555157
+  a: -0.81137842667313109
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 586364.09378163994
-    y: 4140772.063880519
-    theta: 1.3121951555530251
-    kappa: -0.0028725120773857738
-    s: 10.69292663532576
-    dkappa: 1.4960066402197847e-05
+    x: 586364.09425546112
+    y: 4140772.0637540491
+    theta: 1.3125236601392112
+    kappa: -0.002819340079376685
+    s: 10.692927360308014
+    dkappa: -1.5301631276342643e-05
     ddkappa: 0
   }
-  v: 2.2649299882454148
-  a: -0.80790280461512676
+  v: 2.2649304157015768
+  a: -0.80790274349688762
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 586364.15073588688
-    y: 4140772.278926739
-    theta: 1.3115578107613439
-    kappa: -0.0028542511583729141
-    s: 10.915387122621169
-    dkappa: 7.9554040153842183e-05
+    x: 586364.15113792219
+    y: 4140772.2788193133
+    theta: 1.3118973910563376
+    kappa: -0.0028083274829952074
+    s: 10.915387890641103
+    dkappa: 4.7058924669923145e-05
     ddkappa: 0
   }
-  v: 2.1843537775570336
-  a: -0.80346187947796122
+  v: 2.1843542107207421
+  a: -0.8034618263825094
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586364.20575792983
-    y: 4140772.4861738491
-    theta: 1.3109483758414833
-    kappa: -0.0028235435603922452
-    s: 11.129813782057186
-    dkappa: 0.00014181885951535349
+    x: 586364.20608874154
+    y: 4140772.4860853646
+    theta: 1.3112971201747214
+    kappa: -0.0027850595366002546
+    s: 11.129814593646126
+    dkappa: 0.00010717119666423059
     ddkappa: 0
   }
-  v: 2.1042692497653284
-  a: -0.79807183187560959
+  v: 2.1042696878553855
+  a: -0.79807178635880482
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 586364.25885669515
-    y: 4140772.6856751489
-    theta: 1.3103693858306411
-    kappa: -0.002780761531529894
-    s: 11.336260500683865
-    dkappa: 0.00020176923011022959
+    x: 586364.25911721063
+    y: 4140772.6856054068
+    theta: 1.3107253308276816
+    kappa: -0.0027498972239472365
+    s: 11.336261356297461
+    dkappa: 0.00016504941003466571
     ddkappa: 0
   }
-  v: 2.0247705115586174
-  a: -0.79174877297208157
+  v: 2.0247709538449987
+  a: -0.79174873445458172
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 586364.31004086358
-    y: 4140772.8774936525
-    theta: 1.3098236575380326
-    kappa: -0.00272645101608477
-    s: 11.53479049566427
-    dkappa: 0.00025942241641894955
+    x: 586364.31023247167
+    y: 4140772.8774423297
+    theta: 1.3101847959914139
+    kappa: -0.0027033691995962273
+    s: 11.534791395688293
+    dkappa: 0.00022071024560655855
     ddkappa: 0
   }
-  v: 1.9459500585088174
-  a: -0.78450881393138683
+  v: 1.9459505043261927
+  a: -0.78450878169864835
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586364.359292713
-    y: 4140773.0617089374
-    theta: 1.309310053259344
-    kappa: -0.0026643098449766374
-    s: 11.725476153162838
-    dkappa: 0.0003147983279036009
+    x: 586364.35941743245
+    y: 4140773.0616755318
+    theta: 1.3096747930720793
+    kappa: -0.0026490477168615504
+    s: 11.725477097920326
+    dkappa: 0.00027417281595494406
     ddkappa: 0
   }
-  v: 1.8678987750714451
-  a: -0.77636806591753516
+  v: 1.8678992238326986
+  a: -0.77636803911981289
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 586364.4066164348
-    y: 4140773.238404104
-    theta: 1.3088277276485243
-    kappa: -0.0025960275683508446
-    s: 11.908398867233739
-    dkappa: 0.00036792016446276777
+    x: 586364.40667654062
+    y: 4140773.2383880373
+    theta: 1.3091946756843567
+    kappa: -0.0025885663362416713
+    s: 11.908399856993487
+    dkappa: 0.00032545946476099944
     ddkappa: 0
   }
-  v: 1.7907059345856151
-  a: -0.767342640094536
+  v: 1.7907063857953665
+  a: -0.76734261774688328
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 586364.45203712815
-    y: 4140773.4076658157
-    theta: 1.308378301325209
-    kappa: -0.0025211864004731173
-    s: 12.083648878709234
-    dkappa: 0.00041881378716785393
+    x: 586364.4520350392
+    y: 4140773.4076664736
+    theta: 1.308745961974161
+    kappa: -0.0025215243457363326
+    s: 12.08364991369565
+    dkappa: 0.00037459516908107583
     ddkappa: 0
   }
-  v: 1.7144591992740423
-  a: -0.75744864762639919
+  v: 1.7144596525421649
+  a: -0.75744862860866757
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586364.49557999335
-    y: 4140773.5695896782
-    theta: 1.3079645958668116
-    kappa: -0.0024393724860050118
-    s: 12.251325114088029
-    dkappa: 0.00046750735015164404
+    x: 586364.49551846238
+    y: 4140773.5696063554
+    theta: 1.3083313296661314
+    kappa: -0.0024475250268219803
+    s: 12.251326194492341
+    dkappa: 0.00042160709831434587
     ddkappa: 0
   }
-  v: 1.6392446202430395
-  a: -0.74670219967713436
+  v: 1.639245075298182
+  a: -0.74670218273397415
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 586364.53722791793
-    y: 4140773.7242915328
-    theta: 1.3075788780276341
-    kappa: -0.0023557570947788805
-    s: 12.41153502442365
-    dkappa: 0.00051403220132902668
+    x: 586364.53710976313
+    y: 4140773.72432349
+    theta: 1.3079438380067263
+    kappa: -0.0023715642690087337
+    s: 12.411536150416447
+    dkappa: 0.00046652535774568889
     ddkappa: 0
   }
-  v: 1.5651466374825196
-  a: -0.73511940741075121
+  v: 1.5651470941856258
+  a: -0.73511939115161073
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586364.57702745544
-    y: 4140773.8718783008
-    theta: 1.3072254674689834
-    kappa: -0.0022680774503728062
-    s: 12.564393969739909
-    dkappa: 0.000558421517230311
+    x: 586364.57685563725
+    y: 4140773.8719247663
+    theta: 1.3075874209840153
+    kappa: -0.0022914608093859033
+    s: 12.56439514148313
+    dkappa: 0.0005093820278969216
     ddkappa: 0
   }
-  v: 1.492229978522301
-  a: -0.72325554372077283
+  v: 1.4922304368033339
+  a: -0.72325552891800382
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 586364.61499470915
-    y: 4140774.0124680586
-    theta: 1.3069028159372247
-    kappa: -0.0021817295155297026
-    s: 12.710020182832155
-    dkappa: 0.000584762829076717
+    x: 586364.61477236275
+    y: 4140774.0125282081
+    theta: 1.3072607305677739
+    kappa: -0.0022122964940597817
+    s: 12.71002140047146
+    dkappa: 0.000535334515552481
     ddkappa: 0
   }
-  v: 1.4204884572660414
-  a: -0.71160382420052248
+  v: 1.4204889168349395
+  a: -0.71160381369698544
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 586364.65113375359
-    y: 4140774.1461804621
-    theta: 1.3066034046834507
-    kappa: -0.0020981012119460304
-    s: 12.848530245041239
-    dkappa: 0.00060132597109107376
+    x: 586364.65086366585
+    y: 4140774.1462535523
+    theta: 1.306956852439372
+    kappa: -0.0021354826088777548
+    s: 12.848531508679523
+    dkappa: 0.00055209813345983357
     ddkappa: 0
   }
-  v: 1.3499047080360822
-  a: -0.70008727406371107
+  v: 1.3499051683321872
+  a: -0.7000872704284733
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586364.68550259771
-    y: 4140774.2731191721
-    theta: 1.3063358657967303
-    kappa: -0.0020201356991114763
-    s: 12.980039386014191
-    dkappa: 0.00059807996599796093
+    x: 586364.68518822407
+    y: 4140774.2732042922
+    theta: 1.3066837393571611
+    kappa: -0.0020636249982731005
+    s: 12.98004069568589
+    dkappa: 0.00055031681978073106
     ddkappa: 0
   }
-  v: 1.2804690626333946
-  a: -0.68862891852404917
+  v: 1.2804695228528309
+  a: -0.68862892405238529
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586364.71808161307
-    y: 4140774.3934082687
-    theta: 1.306085255858112
-    kappa: -0.00194650317928611
-    s: 13.104662253452059
-    dkappa: 0.00059169471232549441
+    x: 586364.71772545506
+    y: 4140774.3935047458
+    theta: 1.3064276146544378
+    kappa: -0.0019957173969544655
+    s: 13.104663609100209
+    dkappa: 0.00054554175055456866
     ddkappa: 0
   }
-  v: 1.2121795503375776
-  a: -0.677151782795247
+  v: 1.2121800094606325
+  a: -0.6771517995086388
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586364.74893818039
-    y: 4140774.507148461
-    theta: 1.3058631881576521
-    kappa: -0.0018781804626063683
-    s: 13.222513682857789
-    dkappa: 0.00058565503782646573
+    x: 586364.74854374386
+    y: 4140774.5072553633
+    theta: 1.306199200120693
+    kappa: -0.0019324785220903667
+    s: 13.222515084313796
+    dkappa: 0.00054102481322207787
     ddkappa: 0
   }
-  v: 1.14504189790686
-  a: -0.66557889209101539
+  v: 1.1450423547253616
+  a: -0.66557892173715161
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586364.778059527
-    y: 4140774.6144631831
-    theta: 1.305655974953708
-    kappa: -0.0018139191361547305
-    s: 13.333709467284075
-    dkappa: 0.00057995624142748051
+    x: 586364.77762916591
+    y: 4140774.614579875
+    theta: 1.3059858231975285
+    kappa: -0.0018729627849596348
+    s: 13.333710914250254
+    dkappa: 0.000536762764146358
     ddkappa: 0
   }
-  v: 1.079069529578099
-  a: -0.65383327162506488
+  v: 1.0790699827227967
+  a: -0.65383331567784153
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 586364.805496832
-    y: 4140774.7154603098
-    theta: 1.3054702830935749
-    kappa: -0.0017542738860493075
-    s: 13.438367127081234
-    dkappa: 0.0005745918149241832
+    x: 586364.80503361835
+    y: 4140774.7155859666
+    theta: 1.3057936592268993
+    kappa: -0.0018175729535017955
+    s: 13.438368619116053
+    dkappa: 0.000532750610630885
     ddkappa: 0
   }
-  v: 1.0142835670667811
-  a: -0.641837946611106
+  v: 1.0142840150347241
+  a: -0.641838006270626
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 586364.83126788354
-    y: 4140774.8102593739
-    theta: 1.305301378453906
-    kappa: -0.0016987707450409641
-    s: 13.536606679645061
-    dkappa: 0.00056955595024019688
+    x: 586364.83077438769
+    y: 4140774.8103933036
+    theta: 1.305618291578065
+    kappa: -0.0017659421797966471
+    s: 13.536608216151155
+    dkappa: 0.00052898409336178827
     ddkappa: 0
   }
-  v: 0.95071282956702152
-  a: -0.62951594226284946
+  v: 0.95071327074893841
+  a: -0.62951601845542293
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586364.85538867791
-    y: 4140774.8989837817
-    theta: 1.3051436671102059
-    kappa: -0.0016468580501134593
-    s: 13.628551409164695
-    dkappa: 0.00056484274185913453
+    x: 586364.8548668857
+    y: 4140774.8991254447
+    theta: 1.3054545062099157
+    kappa: -0.0017176451475493942
+    s: 13.628552989379592
+    dkappa: 0.00052545889676668567
     ddkappa: 0
   }
-  v: 0.88839383375156467
-  a: -0.61679028379400569
+  v: 0.88839426645924235
+  a: -0.61679037717214991
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 586364.87792493287
-    y: 4140774.9817476012
-    theta: 1.305008276931493
-    kappa: -0.0015995054747385872
-    s: 13.714328636370476
-    dkappa: 0.00056044495673038611
+    x: 586364.877378179
+    y: 4140774.9818961024
+    theta: 1.3053126550577365
+    kappa: -0.0016733940593483261
+    s: 13.714330259360082
+    dkappa: 0.00052216941698768251
     ddkappa: 0
   }
-  v: 0.82737079377178335
-  a: -0.60358399641828531
+  v: 0.82737121626544718
+  a: -0.60358410736072465
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 586364.89887551346
-    y: 4140775.058688073
-    theta: 1.304882412945096
-    kappa: -0.0015554846760235039
-    s: 13.794070488281818
-    dkappa: 0.0005563566043564717
+    x: 586364.89830555464
+    y: 4140775.0588429314
+    theta: 1.3051807846952626
+    kappa: -0.0016322565229499523
+    s: 13.79407215293662
+    dkappa: 0.00051911138836868926
     ddkappa: 0
   }
-  v: 0.76769562125768009
-  a: -0.58982010534939888
+  v: 0.76769603177337231
+  a: -0.58982023396106475
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 586364.91827659821
-    y: 4140775.1299380609
-    theta: 1.3047658578054702
-    kappa: -0.0015147196361885783
-    s: 13.867914667955068
-    dkappa: 0.00055257062472305969
+    x: 586364.91768515133
+    y: 4140775.130098809
+    theta: 1.3050586674057869
+    kappa: -0.0015941614996946756
+    s: 13.86791637298909
+    dkappa: 0.00051627953013032247
     ddkappa: 0
   }
-  v: 0.7094279253178859
-  a: -0.575421635801057
+  v: 0.7094283220948453
+  a: -0.5754217819130879
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 586364.93618725811
-    y: 4140775.1956307697
-    theta: 1.3046663361637409
-    kappa: -0.0014778782203095607
-    s: 13.93600522423136
-    dkappa: 0.00054907923639264489
+    x: 586364.93557712075
+    y: 4140775.1957966518
+    theta: 1.3049535136805932
+    kappa: -0.0015595937382654711
+    s: 13.93600696818385
+    dkappa: 0.00051366789841329947
     ddkappa: 0
   }
-  v: 0.65263501253966072
-  a: -0.56031161298697019
+  v: 0.65263539384770175
+  a: -0.5603117761567119
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 586364.95262771845
-    y: 4140775.2559173652
-    theta: 1.304576307289022
-    kappa: -0.0014441906742627387
-    s: 13.998493321484494
-    dkappa: 0.00054587505289733162
+    x: 586364.95200061309
+    y: 4140775.25608791
+    theta: 1.3048582336382608
+    kappa: -0.0015279618887471193
+    s: 13.998495102724354
+    dkappa: 0.00051127108162784423
     ddkappa: 0
   }
-  v: 0.59739188698889345
-  a: -0.54441306212084906
+  v: 0.597392251155786
+  a: -0.54441324163185445
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 586364.96763603133
-    y: 4140775.3109523263
-    theta: 1.3044941209320275
-    kappa: -0.0014134376887454483
-    s: 14.055538009368785
-    dkappa: 0.00054294998947807406
+    x: 586364.9669934368
+    y: 4140775.311127129
+    theta: 1.3047712535466023
+    kappa: -0.0014990855249548943
+    s: 14.055539826101727
+    dkappa: 0.00050908305422843578
     ddkappa: 0
   }
-  v: 0.54378125021010193
-  a: -0.52764900841640427
+  v: 0.54378159564895012
+  a: -0.527649203278433
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 586364.98125631979
-    y: 4140775.36089745
-    theta: 1.304419535475525
-    kappa: -0.0013855288535920916
-    s: 14.107306992566929
-    dkappa: 0.00054029544679195759
+    x: 586364.98059966869
+    y: 4140775.3610761189
+    theta: 1.3046923176959471
+    kappa: -0.0014728797539010781
+    s: 14.107308842845388
+    dkappa: 0.00050709738368137722
     ddkappa: 0
   }
-  v: 0.49189350122643338
-  a: -0.50994247708734619
+  v: 0.49189382646305457
+  a: -0.50994268603636539
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 586364.99354301579
-    y: 4140775.4059214881
-    theta: 1.3043554436125153
-    kappa: -0.0013606717562349888
-    s: 14.15397740053788
-    dkappa: 0.00053790221966714541
+    x: 586364.99287419545
+    y: 4140775.406103509
+    theta: 1.30462411906892
+    kappa: -0.0014494816311090646
+    s: 14.153979282273646
+    dkappa: 0.0005053071408341489
     ddkappa: 0
   }
-  v: 0.44182673653966342
-  a: -0.49121649334738554
+  v: 0.44182704023996833
+  a: -0.49121671484556928
   relative_time: 5.7999999999999963
 }
 trajectory_point {
   path_point {
-    x: 586365.00454370328
-    y: 4140775.4462056295
-    theta: 1.3043008917765666
-    kappa: -0.0013386995850040969
-    s: 14.195736557264686
-    dkappa: 0.00053576072936559443
+    x: 586365.003864441
+    y: 4140775.44639053
+    theta: 1.3045657285594232
+    kappa: -0.0014287470963164396
+    s: 14.195738468244286
+    dkappa: 0.00050370516997039314
     ddkappa: 0
   }
-  v: 0.39368675013019655
-  a: -0.47139408241023295
+  v: 0.39368703112756814
+  a: -0.47139431464596232
   relative_time: 5.8999999999999959
 }
 trajectory_point {
   path_point {
-    x: 586365.01430284674
-    y: 4140775.4819432786
-    theta: 1.3042524966925009
-    kappa: -0.0013192072059414032
-    s: 14.232782751002377
-    dkappa: 0.00053386092891900849
+    x: 586365.01361432124
+    y: 4140775.4821307361
+    theta: 1.3045139280353018
+    kappa: -0.0014103526728291411
+    s: 14.232784688905193
+    dkappa: 0.00050228399844294146
     ddkappa: 0
   }
-  v: 0.3475870334570666
-  a: -0.45039826948959882
+  v: 0.34758729077973932
+  a: -0.4503985103774622
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 586365.02287577256
-    y: 4140775.5133370403
-    theta: 1.3042099839992467
-    kappa: -0.0013020841128550402
-    s: 14.265326004025811
-    dkappa: 0.000532192047979038
+    x: 586365.02217911009
+    y: 4140775.5135267437
+    theta: 1.304468423829773
+    kappa: -0.0013941940791034834
+    s: 14.265327966444932
+    dkappa: 0.00050103556935559212
     ddkappa: 0
   }
-  v: 0.30364877545793589
-  a: -0.4281520797991939
+  v: 0.30364900835637509
+  a: -0.42815232697998673
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 586365.03032110143
-    y: 4140775.5406015795
-    theta: 1.3041730630013497
-    kappa: -0.0012872132205188247
-    s: 14.293588842377552
-    dkappa: 0.00053074267497994917
+    x: 586365.02961737232
+    y: 4140775.5407932349
+    theta: 1.3044289047922166
+    kappa: -0.0013801608251343476
+    s: 14.293590826843364
+    dkappa: 0.00049995134614021349
     ddkappa: 0
   }
-  v: 0.26200086254909666
-  a: -0.40457853855272863
+  v: 0.26200107052337757
+  a: -0.40457878939345343
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
-    x: 586365.03670095012
-    y: 4140775.5639643678
-    theta: 1.3041414256634691
-    kappa: -0.0012744704596585389
-    s: 14.317807065615716
-    dkappa: 0.00052950071766446455
+    x: 586365.035991166
+    y: 4140775.564157696
+    theta: 1.3043950412119225
+    kappa: -0.0013681358302537546
+    s: 14.317809069622232
+    dkappa: 0.00049902228302747982
     ddkappa: 0
   }
-  v: 0.2227798786254688
-  a: -0.37960067096391364
+  v: 0.22278006145265619
+  a: -0.37960092255778005
   relative_time: 6.2999999999999945
 }
 trajectory_point {
   path_point {
-    x: 586365.04208113533
-    y: 4140775.5836664224
-    theta: 1.3041147456048634
-    kappa: -0.0012637243719381866
-    s: 14.33823051656185
-    dkappa: 0.00052845336360960468
+    x: 586365.04136624513
+    y: 4140775.583861162
+    theta: 1.3043664837418241
+    kappa: -0.0013579950409294279
+    s: 14.338232537595783
+    dkappa: 0.00049823879551760646
     ddkappa: 0
   }
-  v: 0.18613010506060207
-  a: -0.35314150224645952
+  v: 0.18613026282212924
+  a: -0.35314175141288418
   relative_time: 6.3999999999999941
 }
 trajectory_point {
   path_point {
-    x: 586365.046531376
-    y: 4140775.5999630508
-    theta: 1.3040926770938637
-    kappa: -0.0012548357049462766
-    s: 14.355123851048781
-    dkappa: 0.00052758704075252954
+    x: 586365.04581226211
+    y: 4140775.6001589582
+    theta: 1.3043428623222173
+    kappa: -0.0013496070485633715
+    s: 14.355125886621344
+    dkappa: 0.00049759073085108658
     ddkappa: 0
   }
-  v: 0.15220352070667487
-  a: -0.32512405761407681
+  v: 0.15220365381572309
+  a: -0.32512430089868366
   relative_time: 6.4999999999999938
 }
 trajectory_point {
   path_point {
-    x: 586365.05012549553
-    y: 4140775.6131245941
-    theta: 1.3040748540423406
-    kappa: -0.001247657007182079
-    s: 14.36876730766849
-    dkappa: 0.00052688737791637883
+    x: 586365.04940297082
+    y: 4140775.613321445
+    theta: 1.3043237851044689
+    kappa: -0.0013428327072904319
+    s: 14.368769355349945
+    dkappa: 0.00049706733847942614
     ddkappa: 0
   }
-  v: 0.12115980189449442
-  a: -0.2954713622804761
+  v: 0.12115991112337214
+  a: -0.29547159595509603
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
-    x: 586365.05294162559
-    y: 4140775.6234371662
-    theta: 1.3040608890001606
-    kappa: -0.0012420322230419071
-    s: 14.379457477519964
-    dkappa: 0.0005263391653361145
+    x: 586365.05221642845
+    y: 4140775.6236347565
+    theta: 1.3043088373747169
+    kappa: -0.0013375247517768748
+    s: 14.3794595349769
+    dkappa: 0.00049665724053588056
     ddkappa: 0
   }
-  v: 0.09316632243349679
-  a: -0.26410644145936796
+  v: 0.093166408941019713
+  a: -0.26410666152203904
   relative_time: 6.6999999999999931
 }
 trajectory_point {
   path_point {
-    x: 586365.05506380869
-    y: 4140775.6312030172
-    theta: 1.3040509802915206
-    kappa: -0.0012378563020284635
-    s: 14.387508073957076
-    dkappa: 0.00052592629732794408
+    x: 586365.05433670676
+    y: 4140775.6314011356
+    theta: 1.3042981558392941
+    kappa: -0.0013335723108762268
+    s: 14.387510138992429
+    dkappa: 0.00049634838156810727
     ddkappa: 0
   }
-  v: 0.0683981536117473
-  a: -0.23095232036446295
+  v: 0.0683982189706165
+  a: -0.23095252253943038
   relative_time: 6.7999999999999927
 }
 trajectory_point {
   path_point {
-    x: 586365.05657833314
-    y: 4140775.6367423311
-    theta: 1.3040442314564578
-    kappa: -0.0012349090477723175
-    s: 14.393250702336426
-    dkappa: 0.00052563178213062257
+    x: 586365.05584992934
+    y: 4140775.6369408104
+    theta: 1.3042908385343184
+    kappa: -0.0013307765336075695
+    s: 14.393252772932236
+    dkappa: 0.0004961280562826183
     ddkappa: 0
   }
-  v: 0.04703806419594056
-  a: -0.19593202420947167
+  v: 0.047038110420121709
+  a: -0.19593220394718761
   relative_time: 6.8999999999999924
 }
 trajectory_point {
   path_point {
-    x: 586365.057576546
-    y: 4140775.6403932548
-    theta: 1.3040397833450195
-    kappa: -0.0012329665324749973
-    s: 14.397035629765218
-    dkappa: 0.00052543766916093306
+    x: 586365.0568472842
+    y: 4140775.6405919734
+    theta: 1.3042860157480825
+    kappa: -0.0013289338557158858
+    s: 14.397037704128131
+    dkappa: 0.00049598284137687416
     ddkappa: 0
   }
-  v: 0.029276520431399115
-  a: -0.1589685782081047
+  v: 0.029276550003502991
+  a: -0.15896873068522863
   relative_time: 6.999999999999992
 }
 trajectory_point {
   path_point {
-    x: 586365.058155949
-    y: 4140775.6425123992
-    theta: 1.3040372014810901
-    kappa: -0.0012318390179683544
-    s: 14.399232554849124
-    dkappa: 0.000525324998136126
+    x: 586365.05742618931
+    y: 4140775.6427112557
+    theta: 1.3042832164076577
+    kappa: -0.0013278642908514282
+    s: 14.399234631458615
+    dkappa: 0.00049589855276534829
     ddkappa: 0
   }
-  v: 0.015311686042075277
-  a: -0.11998500757407254
+  v: 0.015311701940736109
+  a: -0.11998512769347097
   relative_time: 7.0999999999999917
 }
 trajectory_point {
   path_point {
-    x: 586365.05841937219
-    y: 4140775.643475859
-    theta: 1.304036027647699
-    kappa: -0.0012313263983213842
-    s: 14.400231377440139
-    dkappa: 0.00052527377273670084
+    x: 586365.057689386
+    y: 4140775.6436747788
+    theta: 1.3042819436994719
+    kappa: -0.0013273780177475139
+    s: 14.400233455099489
+    dkappa: 0.00049586023130761042
     ddkappa: 0
   }
-  v: 0.005349422230549879
-  a: -0.078904337521085832
+  v: 0.0053494279578049937
+  a: -0.078904419911832391
   relative_time: 7.1999999999999913
 }
 trajectory_point {
   path_point {
-    x: 586365.05847517587
-    y: 4140775.6436799583
-    theta: 1.3040357789824026
-    kappa: -0.0012312178047872075
-    s: 14.400442968384448
-    dkappa: 0.00052526292112928382
+    x: 586365.05774514168
+    y: 4140775.6438788916
+    theta: 1.3042816740884922
+    kappa: -0.0013272750054706718
+    s: 14.400445046274452
+    dkappa: 0.0004958521132756203
     ddkappa: 0
   }
   v: 0
-  a: -0.035649593262855168
+  a: -0.035649632280230388
   relative_time: 7.2999999999999909
 }
 trajectory_point {
   path_point {
-    x: 586365.05843745428
-    y: 4140775.6435419936
-    theta: 1.3040359470726637
-    kappa: -0.0012312912107500557
-    s: 14.400299939270287
-    dkappa: 0.0005252702564895053
+    x: 586365.05770745245
+    y: 4140775.6437409171
+    theta: 1.3042818563374035
+    kappa: -0.0013273446386707862
+    s: 14.400302017005705
+    dkappa: 0.000495857600821022
     ddkappa: 0
   }
   v: 0
-  a: 0.0098561999869089065
+  a: 0.00985621026141704
   relative_time: 7.3999999999999906
 }
 trajectory_point {
   path_point {
-    x: 586365.05842623848
-    y: 4140775.6435009721
-    theta: 1.3040359970512325
-    kappa: -0.00123131303667225
-    s: 14.40025741217581
-    dkappa: 0.00052527243752487692
+    x: 586365.05769624619
+    y: 4140775.6436998933
+    theta: 1.304281910525789
+    kappa: -0.0013273653428312459
+    s: 14.400259489864547
+    dkappa: 0.00049585923244243734
     ddkappa: 0
   }
-  v: 0.00165212846800878
-  a: 0.057690017014495928
+  v: 0.0016521303379893949
+  a: 0.057690082773192528
   relative_time: 7.49999999999999
 }
 trajectory_point {
   path_point {
-    x: 586365.05856769916
-    y: 4140775.644018359
-    theta: 1.3040353666915252
-    kappa: -0.0012310377550411892
-    s: 14.400793789416948
-    dkappa: 0.00052524492899766993
+    x: 586365.05783758545
+    y: 4140775.6442173142
+    theta: 1.3042812270693089
+    kappa: -0.0013271042095190865
+    s: 14.40079586772198
+    dkappa: 0.00049583865345275986
     ddkappa: 0
   }
-  v: 0.0099127085660663129
-  a: 0.107928832606195
+  v: 0.0099127200544059258
+  a: 0.10792896031517828
   relative_time: 7.59999999999999
 }
 trajectory_point {
   path_point {
-    x: 586365.05899435
-    y: 4140775.6455788175
-    theta: 1.3040334655030037
-    kappa: -0.0012302074953163297
-    s: 14.402411523295275
-    dkappa: 0.00052516196224779241
+    x: 586365.05826386972
+    y: 4140775.6457778751
+    theta: 1.3042791657388766
+    kappa: -0.0013263166216351378
+    s: 14.402413603499307
+    dkappa: 0.00049577658644644855
     ddkappa: 0
   }
-  v: 0.023320627434261255
-  a: 0.16064962154829587
+  v: 0.023320655070701503
+  a: 0.16064981794745636
   relative_time: 7.6999999999999895
 }
 trajectory_point {
   path_point {
-    x: 586365.05984525057
-    y: 4140775.6486909525
-    theta: 1.3040296738265518
-    kappa: -0.0012285516488761724
-    s: 14.405637885845863
-    dkappa: 0.00052499649571566677
+    x: 586365.059114039
+    y: 4140775.648890214
+    theta: 1.3042750546798367
+    kappa: -0.0013247458786641699
+    s: 14.405639969918735
+    dkappa: 0.00049565280176682184
     ddkappa: 0
   }
-  v: 0.042127931146948322
-  a: 0.21592935862708781
+  v: 0.0421279821489092
+  a: 0.21592963073010948
   relative_time: 7.7999999999999892
 }
 trajectory_point {
   path_point {
-    x: 586365.06126620877
-    y: 4140775.6538880514
-    theta: 1.304023341929853
-    kappa: -0.0012257864739652416
-    s: 14.41102573858516
-    dkappa: 0.00052472017546510707
+    x: 586365.06053377606
+    y: 4140775.6540876534
+    theta: 1.3042681894311394
+    kappa: -0.001322122821925038
+    s: 14.411027829253971
+    dkappa: 0.00049544608797335112
     ddkappa: 0
   }
-  v: 0.066594363257111169
-  a: 0.2738450186288604
+  v: 0.0665944455570718
+  a: 0.27384537372321988
   relative_time: 7.8999999999999888
 }
 trajectory_point {
   path_point {
-    x: 586365.06340998469
-    y: 4140775.6617288277
-    theta: 1.3040137891027668
-    kappa: -0.0012216147006410714
-    s: 14.419154302258836
-    dkappa: 0.00052430329570619689
+    x: 586365.06267570972
+    y: 4140775.6619289434
+    theta: 1.3042578319445157
+    kappa: -0.0013181654598208292
+    s: 14.41915640308083
+    dkappa: 0.000495134222308955
     ddkappa: 0
   }
-  v: 0.096987364796362829
-  a: 0.33447357633990288
+  v: 0.096987487069237988
+  a: 0.33447402198686976
   relative_time: 7.9999999999999885
 }
 decision {
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
index 6f11a73dcc..5b31979e43 100644
--- a/modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt
@@ -9,1693 +9,1693 @@ trajectory_point {
     y: 4140761.7226162944
     z: 0
     theta: 1.2867508012008013
-    kappa: 0.013159962977488119
+    kappa: 0.012977269686623459
     s: 0
     dkappa: 0
     ddkappa: 0
   }
   v: 4.4250001907348633
-  a: -0.398586546595553
+  a: -0.398586425749059
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 586361.40024217626
-    y: 4140761.8075333685
-    theta: 1.2878807360051818
-    kappa: 0.012894164141270084
-    s: 0.0884198587666354
-    dkappa: -0.0010532955625452442
+    x: 586361.40024406614
+    y: 4140761.80753282
+    theta: 1.2878654142356609
+    kappa: 0.012718029093969293
+    s: 0.088419858790962549
+    dkappa: -0.0010273064788495036
     ddkappa: 0
   }
-  v: 4.4169643432556205
-  a: -0.40498934988481627
+  v: 4.4169643456961705
+  a: -0.40498922668794018
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 586361.42483768938
-    y: 4140761.8922948656
-    theta: 1.2890086018870619
-    kappa: 0.012628852274942941
-    s: 0.17667772356223269
-    dkappa: -0.002104661382912276
+    x: 586361.42484146589
+    y: 4140761.89229377
+    theta: 1.2889779863702719
+    kappa: 0.012459263455707255
+    s: 0.17667772366016343
+    dkappa: -0.002052730830678725
     ddkappa: 0
   }
-  v: 4.4088009705226385
-  a: -0.4113390856989026
+  v: 4.4088009754500508
+  a: -0.41133896022109373
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 586361.44934999768
-    y: 4140761.9769091229
-    theta: 1.2901250513113185
-    kappa: 0.012364909416523044
-    s: 0.26477105448972038
-    dkappa: -0.0030026336511018038
+    x: 586361.449356152
+    y: 4140761.9769073394
+    theta: 1.290079399377936
+    kappa: 0.012201824499940393
+    s: 0.26477105471144358
+    dkappa: -0.0029285814096340428
     ddkappa: 0
   }
-  v: 4.4005111330616575
-  a: -0.41763583641401558
+  v: 4.4005111405208623
+  a: -0.41763570872393224
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 586361.47358869645
-    y: 4140762.0614284454
-    theta: 1.2911827602019081
-    kappa: 0.01210678392144025
-    s: 0.35269733284606658
-    dkappa: -0.0029781046918084548
+    x: 586361.47360023018
+    y: 4140762.0614251196
+    theta: 1.2911234908425313
+    kappa: 0.011950007462763534
+    s: 0.35269733324265595
+    dkappa: -0.0029048972718049028
     ddkappa: 0
   }
-  v: 4.3920958897508964
-  a: -0.42387968440635876
+  v: 4.3920958997854589
+  a: -0.42387955457186832
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 586361.49778065528
-    y: 4140762.1457847874
-    theta: 1.2922384301844587
-    kappa: 0.011849156175240946
-    s: 0.44045406108932844
-    dkappa: -0.0029536230322291359
+    x: 586361.497797558
+    y: 4140762.1457799221
+    theta: 1.2921655696320051
+    kappa: 0.011698676009586888
+    s: 0.44045406171271556
+    dkappa: -0.0028812588045885115
     ddkappa: 0
   }
-  v: 4.3835562978210474
-  a: -0.43007071205213576
+  v: 4.3835563104731845
+  a: -0.43007058014031452
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 586361.52185911033
-    y: 4140762.2299946151
-    theta: 1.2932749404718149
-    kappa: 0.01159369193279337
-    s: 0.52803876280570183
-    dkappa: -0.0029291309365870534
+    x: 586361.52188220073
+    y: 4140762.2299879822
+    theta: 1.2931889311549154
+    kappa: 0.011449438373050052
+    s: 0.52803876370864922
+    dkappa: -0.0028576110706966659
     ddkappa: 0
   }
-  v: 4.37489341285528
-  a: -0.43620900172755017
+  v: 4.3748934281658736
+  a: -0.43620886780468338
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 586361.5457081846
-    y: 4140762.3140884205
-    theta: 1.2942624722807363
-    kappa: 0.011343289668270008
-    s: 0.61544898267657067
-    dkappa: -0.0029045272336716225
+    x: 586361.54573972337
+    y: 4140762.3140793918
+    theta: 1.2941644808711403
+    kappa: 0.011205092959320716
+    s: 0.61544898391264524
+    dkappa: -0.0028338577927566229
     ddkappa: 0
   }
-  v: 4.3661082887892393
-  a: -0.44229463580880551
+  v: 4.3661083067978552
+  a: -0.44229449994038761
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 586361.56950898876
-    y: 4140762.398012023
-    theta: 1.2952480056896276
-    kappa: 0.011093394211676058
-    s: 0.70268228644555653
-    dkappa: -0.0028799733280363967
+    x: 586361.56954895915
+    y: 4140762.3980006026
+    theta: 1.2951380564271997
+    kappa: 0.010961242094452256
+    s: 0.7026822880691036
+    dkappa: -0.002810152590842923
     ddkappa: 0
   }
-  v: 4.3572019779110471
-  a: -0.44832769667210537
+  v: 4.3572019986559463
+  a: -0.44832755892283971
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 586361.59317394393
-    y: 4140762.4817876206
-    theta: 1.2962081564538068
-    kappa: 0.010846353579565728
-    s: 0.78973626088556825
-    dkappa: -0.0028554012993277453
+    x: 586361.59322335315
+    y: 4140762.4817735362
+    theta: 1.2960868361832949
+    kappa: 0.010720152469734914
+    s: 0.78973626295168531
+    dkappa: -0.0027864306911445226
     ddkappa: 0
   }
-  v: 4.3481755308613
-  a: -0.45430826669365337
+  v: 4.34817555437946
+  a: -0.45430812712745222
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 586361.61664545652
-    y: 4140762.5654289764
-    theta: 1.2971275996355176
-    kappa: 0.010603708251402612
-    s: 0.87660851376585136
-    dkappa: -0.0028307668821789629
+    x: 586361.61670597934
+    y: 4140762.5654117735
+    theta: 1.2969958627204359
+    kappa: 0.010483311336791905
+    s: 0.87660851633036285
+    dkappa: -0.0027626498786633832
     ddkappa: 0
   }
-  v: 4.3390299966330721
-  a: -0.46023642824965305
+  v: 4.3390300229601984
+  a: -0.46023628692963781
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 586361.6400672301
-    y: 4140762.6488930867
-    theta: 1.2980450945240414
-    kappa: 0.010361577118082641
-    s: 0.9632966738190375
-    dkappa: -0.0028061846683442009
+    x: 586361.64013884286
+    y: 4140762.6488727704
+    theta: 1.2979029630351608
+    kappa: 0.010246972098742296
+    s: 0.96329667693846932
+    dkappa: -0.0027389194605888095
     ddkappa: 0
   }
-  v: 4.32976642257191
-  a: -0.46611226371630804
+  v: 4.3297664517424552
+  a: -0.46611212070480906
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586361.66333800077
-    y: 4140762.7322057853
-    theta: 1.2989324688232258
-    kappa: 0.010122879287280414
-    s: 1.0497983907081943
-    dkappa: -0.0027815849776589023
+    x: 586361.66342178278
+    y: 4140762.7321820715
+    theta: 1.2987806349961883
+    kappa: 0.010013952567314379
+    s: 1.0497983944397491
+    dkappa: -0.0027151728153811975
     ddkappa: 0
   }
-  v: 4.32038585437584
-  a: -0.47193585546982186
+  v: 4.3203858864230167
+  a: -0.47193571082837854
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 586361.6864437164
-    y: 4140762.8153685792
-    theta: 1.2997859451592022
-    kappa: 0.0098880097617277968
-    s: 1.1361113349938745
-    dkappa: -0.0027569590873318893
+    x: 586361.68654088955
+    y: 4140762.8153411443
+    theta: 1.2996251896209985
+    kappa: 0.0097846342200645434
+    s: 1.1361113393954068
+    dkappa: -0.0026914016055247978
     ddkappa: 0
   }
-  v: 4.3108893360953635
-  a: -0.47770728588639821
+  v: 4.3108893710511609
+  a: -0.47770713967575879
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586361.70949828031
-    y: 4140762.8983472656
-    theta: 1.3006375320457537
-    kappa: 0.0096536601946365878
-    s: 1.2222331981011656
-    dkappa: -0.0027323877142523363
+    x: 586361.70960881491
+    y: 4140762.8983161179
+    theta: 1.3004678745482749
+    kappa: 0.0095558235418165029
+    s: 1.2222332032311576
+    dkappa: -0.0026676830207920483
     ddkappa: 0
   }
-  v: 4.3012779101334555
-  a: -0.48342663734224051
+  v: 4.301277948028658
+  a: -0.48342648962236245
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586361.7323931033
-    y: 4140762.9811695004
-    theta: 1.3014557021294253
-    kappa: 0.00942320115618361
-    s: 1.308161692286739
-    dkappa: -0.0027078052656494857
+    x: 586361.73251805513
+    y: 4140762.9811343681
+    theta: 1.3012779032311326
+    kappa: 0.0093307740156106239
+    s: 1.3081616982042767
+    dkappa: -0.00264395416465768
     ddkappa: 0
   }
-  v: 4.2915526172455705
-  a: -0.48909399221355243
+  v: 4.2915526581097687
+  a: -0.48909384304360209
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 586361.75514446362
-    y: 4140763.0638284292
-    theta: 1.3022453608366271
-    kappa: 0.0091961127080312042
-    s: 1.3938945506059008
-    dkappa: -0.002683222747590773
+    x: 586361.75528471428
+    y: 4140763.063789086
+    theta: 1.3020600642439268
+    kappa: 0.0091089839751615949
+    s: 1.3938945573706503
+    dkappa: -0.0026202255930029158
     ddkappa: 0
   }
-  v: 4.2817144965396361
-  a: -0.49470943287653751
+  v: 4.2817145404012464
+  a: -0.49470928231489031
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 586361.7778433112
-    y: 4140763.1462965719
-    theta: 1.3030331968450148
-    kappa: 0.0089695484081166316
-    s: 1.4794295268796398
-    dkappa: -0.0026586969690255342
+    x: 586361.77799882507
+    y: 4140763.1462530261
+    theta: 1.3028404198660337
+    kappa: 0.0088877058534089469
+    s: 1.4794295345518234
+    dkappa: -0.00259655178981045
     ddkappa: 0
   }
-  v: 4.2717645854760562
-  a: -0.50027304170739928
+  v: 4.2717646323623368
+  a: -0.50027288981163964
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 586361.80037931213
-    y: 4140763.2286018557
-    theta: 1.3037857108464863
-    kappa: 0.0087472033037047335
-    s: 1.5647643956616777
-    dkappa: -0.0026341701294366185
+    x: 586361.80055104755
+    y: 4140763.2285538688
+    theta: 1.303586247523671
+    kappa: 0.0086705062813188481
+    s: 1.5647644043020523
+    dkappa: -0.0025728771446783135
     ddkappa: 0
   }
-  v: 4.2617039198677125
-  a: -0.50578490108234153
+  v: 4.2617039698047758
+  a: -0.50578474790926276
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586361.82278741349
-    y: 4140763.3107324173
-    theta: 1.3045137230325519
-    kappa: 0.0085278887786177128
-    s: 1.6498969522055185
-    dkappa: -0.0026096617819780388
+    x: 586361.8229760027
+    y: 4140763.3106798315
+    theta: 1.3043081149253237
+    kappa: 0.0084562381736389622
+    s: 1.6498969618753514
+    dkappa: -0.0025492204704066841
     ddkappa: 0
   }
-  v: 4.2515335338799609
-  a: -0.51124509337756763
+  v: 4.2515335868927906
+  a: -0.51124493898317214
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586361.84514168848
-    y: 4140763.3926656935
-    theta: 1.3052399863780819
-    kappa: 0.0083091010674154683
-    s: 1.7348250124314988
-    dkappa: -0.0025852123058492491
+    x: 586361.845347091
+    y: 4140763.3926085196
+    theta: 1.3050282482495377
+    kappa: 0.0082424847577182853
+    s: 1.7348250231925455
+    dkappa: -0.0025256206216487985
     ddkappa: 0
   }
-  v: 4.2412544600306337
-  a: -0.51665370096928132
+  v: 4.2412545161431039
+  a: -0.51665354540878039
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586361.86733485677
-    y: 4140763.4744285932
-    theta: 1.3059303558691933
-    kappa: 0.00809472630084467
-    s: 1.819546412893835
-    dkappa: -0.0025607736758162243
+    x: 586361.86755788035
+    y: 4140763.4743666365
+    theta: 1.3057132828706322
+    kappa: 0.0080329971128685618
+    s: 1.8195464248083184
+    dkappa: -0.0025020312033955092
     ddkappa: 0
   }
-  v: 4.2308677291900381
-  a: -0.5220108062336859
+  v: 4.2308677884249253
+  a: -0.52201064956150023
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 586361.88941042987
-    y: 4140763.5560070723
-    theta: 1.3065989093283914
-    kappa: 0.0078831671901274138
-    s: 1.9040590107476765
-    dkappa: -0.0025363666021592929
+    x: 586361.88965152774
+    y: 4140763.5559402239
+    theta: 1.3063769733396704
+    kappa: 0.00782623368019212
+    s: 1.9040590238782635
+    dkappa: -0.002478472220593084
     ddkappa: 0
   }
-  v: 4.22037437058096
-  a: -0.52731649154698523
+  v: 4.22037443295996
+  a: -0.527316333816744
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586361.9114309072
-    y: 4140763.6373819495
-    theta: 1.30726579414368
-    kappa: 0.0076721360844923519
-    s: 1.9883606837161516
-    dkappa: -0.0025120204431984417
+    x: 586361.91169003421
+    y: 4140763.6373102209
+    theta: 1.3070390073040423
+    kappa: 0.0076199862834768275
+    s: 1.9883606981259325
+    dkappa: -0.0024549720358197722
     ddkappa: 0
   }
-  v: 4.2097754117786588
-  a: -0.53257083928538285
+  v: 4.2097754773224034
+  a: -0.53257068054992451
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586361.93329607975
-    y: 4140763.718578062
-    theta: 1.3078974824979759
-    kappa: 0.0074655712022495962
-    s: 2.0724493300574194
-    dkappa: -0.0024876975903417057
+    x: 586361.93357386079
+    y: 4140763.71850131
+    theta: 1.3076666113829654
+    kappa: 0.00741805633589949
+    s: 2.0724493458098867
+    dkappa: -0.0024314941372983183
     ddkappa: 0
   }
-  v: 4.1990718787108694
-  a: -0.53777393182508226
+  v: 4.1990719474389406
+  a: -0.53777377213645428
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586361.95504947274
-    y: 4140763.7995815212
-    theta: 1.308508776831792
-    kappa: 0.0072617386310415092
-    s: 2.1563228685317171
-    dkappa: -0.0024634155432682586
+    x: 586361.95534623275
+    y: 4140763.799499671
+    theta: 1.308274253573483
+    kappa: 0.0072187703643817637
+    s: 2.1563228856907446
+    dkappa: -0.0024080555083598959
     ddkappa: 0
   }
-  v: 4.1882647956578047
-  a: -0.542925851542287
+  v: 4.188264867588753
+  a: -0.54292569095174581
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586361.9767465411
-    y: 4140763.8803752428
-    theta: 1.3091184883161684
-    kappa: 0.0070584338310780044
-    s: 2.2399792383684134
-    dkappa: -0.0024391963682183007
+    x: 586361.97706223093
+    y: 4140763.8802883085
+    theta: 1.3088803223724395
+    kappa: 0.0070200003917140676
+    s: 2.2399792569982355
+    dkappa: -0.0023846775676148587
     ddkappa: 0
   }
-  v: 4.1773551852521535
-  a: -0.54802668081320083
+  v: 4.17735526040351
+  a: -0.54802651937121172
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586361.99829739216
-    y: 4140763.96098117
-    theta: 1.309694899808141
-    kappa: 0.00685950318772356
-    s: 2.3234163992330532
-    dkappa: -0.0024150125002588459
+    x: 586361.99863244034
+    y: 4140763.96088906
+    theta: 1.309453802377083
+    kappa: 0.0068254592816308461
+    s: 2.3234164193982454
+    dkappa: -0.0023613333827391088
     ddkappa: 0
   }
-  v: 4.1663440684790771
-  a: -0.55307650201402714
+  v: 4.1663441468673739
+  a: -0.55307633977026471
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586362.01973855519
-    y: 4140764.0413874378
-    theta: 1.3102511418389642
-    kappa: 0.0066633588231892939
-    s: 2.4066323311944111
-    dkappa: -0.00239087619845933
+    x: 586362.02009318932
+    y: 4140764.0412901035
+    theta: 1.3100075324859761
+    kappa: 0.0066336144575888812
+    s: 2.4066323529598685
+    dkappa: -0.0023380349209844264
     ddkappa: 0
   }
-  v: 4.1552324646762182
-  a: -0.55807539752096968
+  v: 4.1552325463169995
+  a: -0.55807523452431729
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586362.04112220171
-    y: 4140764.1215780135
-    theta: 1.310805891699977
-    kappa: 0.0064677406199829205
-    s: 2.4896250346915374
-    dkappa: -0.0023668046427902227
+    x: 586362.04149636941
+    y: 4140764.12147547
+    theta: 1.3105597771643331
+    kappa: 0.0064422842611080193
+    s: 2.4896250581224577
+    dkappa: -0.002314798957817393
     ddkappa: 0
   }
-  v: 4.14402139153369
-  a: -0.56302344971023188
+  v: 4.144021476441532
+  a: -0.563023286008782
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 586362.06237124035
-    y: 4140764.2015713416
-    theta: 1.3113303612801746
-    kappa: 0.0062762548432097436
-    s: 2.5723925305008111
-    dkappa: -0.0023427792256096557
+    x: 586362.06276517943
+    y: 4140764.2014635443
+    theta: 1.31108237364324
+    kappa: 0.006254949887533094
+    s: 2.572392555662673
+    dkappa: -0.0022916071508809295
     ddkappa: 0
   }
-  v: 4.1327118650940848
-  a: -0.56792074095801737
+  v: 4.1327119532826089
+  a: -0.56792057659907136
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 586362.08350971225
-    y: 4140764.2813590029
-    theta: 1.3118337607467683
-    kappa: 0.006087751723858178
-    s: 2.6549328597029871
-    dkappa: -0.0023188066632185353
+    x: 586362.08392356546
+    y: 4140764.28124593
+    theta: 1.3115843312440483
+    kappa: 0.0060705015812554958
+    s: 2.6549328866615327
+    dkappa: -0.0022684661050105821
     ddkappa: 0
   }
-  v: 4.1213048997524711
-  a: -0.57276735364052977
+  v: 4.1213049912343607
+  a: -0.57276718867059817
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 586362.10458951048
-    y: 4140764.3609252
-    theta: 1.3123357629225016
-    kappa: 0.0058997718281916783
-    s: 2.7372440836502463
-    dkappa: -0.0022949006409063158
+    x: 586362.10502322274
+    y: 4140764.3608068647
+    theta: 1.3120848955567257
+    kappa: 0.00588656524365486
+    s: 2.7372441124714624
+    dkappa: -0.0022453892911783423
     ddkappa: 0
   }
-  v: 4.1098015082563908
-  a: -0.57756337013397263
+  v: 4.1098016030434081
+  a: -0.57756320459877486
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586362.12554808462
-    y: 4140764.44028447
-    theta: 1.312811546612126
-    kappa: 0.0057155290987743416
-    s: 2.8193242839332457
-    dkappa: -0.0022710499630960077
+    x: 586362.1260017259
+    y: 4140764.4401608715
+    theta: 1.3125597738388404
+    kappa: 0.0057062434805361818
+    s: 2.8193243146833451
+    dkappa: -0.00222236552276846
     ddkappa: 0
   }
-  v: 4.0982027017058646
-  a: -0.58230887281454946
+  v: 4.0982027998088624
+  a: -0.582308706759014
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 586362.14639298222
-    y: 4140764.5194328576
-    theta: 1.3132643121281888
-    kappa: 0.0055346123886576178
-    s: 2.9011715623481678
-    dkappa: -0.0022472567025006744
+    x: 586362.14686660562
+    y: 4140764.5193039952
+    theta: 1.3130120979676649
+    kappa: 0.0055291394601358953
+    s: 2.90117159509357
+    dkappa: -0.0021993968496793921
     ddkappa: 0
   }
-  v: 4.0865094895533867
-  a: -0.587003944058464
+  v: 4.086509590982331
+  a: -0.58700377752672839
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586362.167178081
-    y: 4140764.5983541878
-    theta: 1.3137157787717451
-    kappa: 0.0053542146845010647
-    s: 2.9827840408637676
-    dkappa: -0.0022235316989726557
+    x: 586362.16767162911
+    y: 4140764.5982200778
+    theta: 1.31346312449061
+    kappa: 0.0053525435078614391
+    s: 2.9827840756710851
+    dkappa: -0.002176494068096699
     ddkappa: 0
   }
-  v: 4.0747228796039288
-  a: -0.59164866624191959
+  v: 4.0747229843679076
+  a: -0.59164849927733032
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586362.187856388
-    y: 4140764.6770588821
-    theta: 1.3141460446155282
-    kappa: 0.0051770018499456118
-    s: 3.0641598615884278
-    dkappa: -0.0021998693718677212
+    x: 586362.18836980232
+    y: 4140764.676919552
+    theta: 1.313893365922155
+    kappa: 0.0051790292640609172
+    s: 3.0641598985244443
+    dkappa: -0.0021536514653794424
     ddkappa: 0
   }
-  v: 4.0628438780149381
-  a: -0.59624312174112
+  v: 4.0628439861221821
+  a: -0.59624295438623254
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 586362.208416397
-    y: 4140764.7555480632
-    theta: 1.3145503798629865
-    kappa: 0.0050036095259242607
-    s: 3.145297186737201
-    dkappa: -0.0021762688031136848
+    x: 586362.20894961967
+    y: 4140764.7554035438
+    theta: 1.3142981913923821
+    kappa: 0.005009210345310015
+    s: 3.1452972258688572
+    dkappa: -0.0021308680782474807
     ddkappa: 0
   }
-  v: 4.0508734892963378
-  a: -0.6007873929322689
+  v: 4.0508736007542332
+  a: -0.60078722522884753
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 586362.22891551128
-    y: 4140764.8338047746
-    theta: 1.3149535175608758
-    kappa: 0.0048307307569069565
-    s: 3.2261941985988654
-    dkappa: -0.0021527381347445146
+    x: 586362.22946848371
+    y: 4140764.8336550817
+    theta: 1.3147018178615117
+    kappa: 0.0048398943976640588
+    s: 3.2261942399932422
+    dkappa: -0.0021081521711435662
     ddkappa: 0
   }
-  v: 4.0388127163105265
-  a: -0.6052815621915697
+  v: 4.0388128311256342
+  a: -0.605281394180588
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 586362.24932261335
-    y: 4140764.9118352891
-    theta: 1.3153413370930451
-    kappa: 0.004660324294306354
-    s: 3.3068490995029722
-    dkappa: -0.0021292755392731907
+    x: 586362.24989520281
+    y: 4140764.9116804656
+    theta: 1.3150904115315887
+    kappa: 0.0046729725367958786
+    s: 3.3068491432272715
+    dkappa: -0.0020855017438270664
     ddkappa: 0
   }
-  v: 4.02666256027238
-  a: -0.609725711895226
+  v: 4.026662678450446
+  a: -0.60972554361686648
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 586362.269605981
-    y: 4140764.989646059
-    theta: 1.315699437244944
-    kappa: 0.004494387766791918
-    s: 3.3872601117868939
-    dkappa: -0.0021058791365996285
+    x: 586362.27019798232
+    y: 4140764.9894861747
+    theta: 1.3154498654610873
+    kappa: 0.0045103731736033824
+    s: 3.3872601579084254
+    dkappa: -0.0020629147408827418
     ddkappa: 0
   }
-  v: 4.0144240207492476
-  a: -0.6141199244194413
+  v: 4.0144241422952263
+  a: -0.61411975591309553
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 586362.2898273851
-    y: 4140765.067219126
-    theta: 1.3160564434642892
-    kappa: 0.0043289581561061444
-    s: 3.4674254777628741
-    dkappa: -0.0020825542072202413
+    x: 586362.29043873912
+    y: 4140765.0670541967
+    theta: 1.31580822132298
+    kappa: 0.0043482705324388391
+    s: 3.4674255263490403
+    dkappa: -0.0020403967385893892
     ddkappa: 0
   }
-  v: 4.0020980956609566
-  a: -0.61846428214041937
+  v: 4.00209822057902
+  a: -0.61846411344468777
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 586362.3099712322
-    y: 4140765.1445567505
-    theta: 1.3164047836438149
-    kappa: 0.0041651249996361005
-    s: 3.547343459685079
-    dkappa: -0.0020593008922378032
+    x: 586362.310601799
+    y: 4140765.1443868172
+    theta: 1.3161580562383524
+    kappa: 0.0041877166667692025
+    s: 3.5473435108033562
+    dkappa: -0.0020179477479129924
     ddkappa: 0
   }
-  v: 3.9896857812798103
-  a: -0.62275886743436359
+  v: 3.989685909573367
+  a: -0.62275869858705579
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 586362.32998576178
-    y: 4140765.2216706215
-    theta: 1.3167188320025338
-    kappa: 0.0040065696699933245
-    s: 3.627012339716646
-    dkappa: -0.0020361184569692762
+    x: 586362.33063513949
+    y: 4140765.2214958058
+    theta: 1.3164742547818198
+    kappa: 0.00403226548345189
+    s: 3.6270123934345717
+    dkappa: -0.0019955666356948885
     ddkappa: 0
   }
-  v: 3.9771880722305877
-  a: -0.62700376267747782
+  v: 3.9771882039022968
+  a: -0.62700359371561221
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 586362.34993728506
-    y: 4140765.2985417349
-    theta: 1.3170318917528829
-    kappa: 0.0038485134769407747
-    s: 3.7064304198967304
-    dkappa: -0.002013009000652129
+    x: 586362.35060541448
+    y: 4140765.2983620539
+    theta: 1.3167894579488841
+    kappa: 0.0038773036646379475
+    s: 3.7064304762818883
+    dkappa: -0.0019732559798196171
     ddkappa: 0
   }
-  v: 3.9646059614905425
-  a: -0.63119905024596534
+  v: 3.9646060965423318
+  a: -0.6311988812057695
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 586362.36982474173
-    y: 4140765.3751686336
-    theta: 1.3173436073227727
-    kappa: 0.0036910117271119203
-    s: 3.78559602210756
-    dkappa: -0.0019899730285988446
+    x: 586362.37051155535
+    y: 4140765.3749841033
+    theta: 1.3171033156187184
+    kappa: 0.0037228848376916653
+    s: 3.7855960812275651
+    dkappa: -0.0019510162627424988
     ddkappa: 0
   }
-  v: 3.951940440389405
-  a: -0.63534481251602992
+  v: 3.9519405788224859
+  a: -0.63534464343294028
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 586362.389577789
-    y: 4140765.4515678268
-    theta: 1.3176157721432091
-    kappa: 0.00353975749360173
-    s: 3.86450748804148
-    dkappa: -0.0019670128852764088
+    x: 586362.3902826265
+    y: 4140765.4513786361
+    theta: 1.3173783608855556
+    kappa: 0.0035745051095420147
+    s: 3.8645075499639647
+    dkappa: -0.0019288491532675532
     ddkappa: 0
   }
-  v: 3.9391924986093825
-  a: -0.63944113186387508
+  v: 3.9391926404242645
+  a: -0.63944096277253726
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 586362.40926681086
-    y: 4140765.5277193869
-    theta: 1.3178870548227237
-    kappa: 0.0033889935186615138
-    s: 3.9431631791680073
-    dkappa: -0.0019441271623967626
+    x: 586362.409989614
+    y: 4140765.5275255521
+    theta: 1.3176525146761404
+    kappa: 0.0034266063227176326
+    s: 3.9431632439606061
+    dkappa: -0.0019067538937625542
     ddkappa: 0
   }
-  v: 3.9263631241851567
-  a: -0.64348809066570434
+  v: 3.9263632693816652
+  a: -0.64348792159997281
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 586362.4288914022
-    y: 4140765.6036217483
-    theta: 1.3181574497381732
-    kappa: 0.0032387229051138297
-    s: 4.0215614767008718
-    dkappa: -0.0019213163309632561
+    x: 586362.429632112
+    y: 4140765.6034232844
+    theta: 1.3179257713069585
+    kappa: 0.0032791915210735045
+    s: 4.0215615444312123
+    dkappa: -0.0018847309389625761
     ddkappa: 0
   }
-  v: 3.9134533035038865
-  a: -0.64748577129772134
+  v: 3.9134534520811775
+  a: -0.64748560229065966
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 586362.44840215961
-    y: 4140765.6792860106
-    theta: 1.3183973125608082
-    kappa: 0.0030935347573619178
-    s: 4.09970078156508
-    dkappa: -0.0018985846063767451
+    x: 586362.449160092
+    y: 4140765.679083107
+    theta: 1.3181690392170042
+    kappa: 0.0031366916814839774
+    s: 4.09970085230076
+    dkappa: -0.0018627839312914804
     ddkappa: 0
   }
-  v: 3.9004640213052055
-  a: -0.65143425613612971
+  v: 3.9004641732617813
+  a: -0.65143408722001039
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 586362.4678356091
-    y: 4140765.7547011161
-    theta: 1.31862896899567
-    kappa: 0.0029499767896718166
-    s: 4.1775795143639449
-    dkappa: -0.0018759296226735806
+    x: 586362.4686105533
+    y: 4140765.75449383
+    theta: 1.3184042294690599
+    kappa: 0.0029957732618961629
+    s: 4.1775795881725326
+    dkappa: -0.0018409109082974239
     ddkappa: 0
   }
-  v: 3.887396260681224
-  a: -0.65533362755713287
+  v: 3.88739641601495
+  a: -0.65533345876343763
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586362.56381878338
-    y: 4140766.1279897494
-    theta: 1.3196415728097102
-    kappa: 0.002261106059871809
-    s: 4.5630106918247835
-    dkappa: -0.001763832741140057
+    x: 586362.56467432133
+    y: 4140766.1277617468
+    theta: 1.3194367305816388
+    kappa: 0.002319220938302433
+    s: 4.5630107820092523
+    dkappa: -0.0017326815033389116
     ddkappa: 0
   }
-  v: 3.82091457407898
-  a: -0.67409666656819778
+  v: 3.820914746245224
+  a: -0.67409649883116052
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586362.6577811942
-    y: 4140766.4948386289
-    theta: 1.3203679800584569
-    kappa: 0.0016294288498407659
-    s: 4.94170191551911
-    dkappa: -0.0016537574405180832
+    x: 586362.658707246
+    y: 4140766.4945925763
+    theta: 1.320187339705541
+    kappa: 0.001698089662688314
+    s: 4.9417020237562728
+    dkappa: -0.0016264011413113894
     ddkappa: 0
   }
-  v: 3.7526174433344042
-  a: -0.69164422657467206
+  v: 3.7526176321932576
+  a: -0.69164406056268213
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586362.74982481531
-    y: 4140766.8550402424
-    theta: 1.320873627848453
-    kappa: 0.0010465636292645085
-    s: 5.3134777012660077
-    dkappa: -0.001545747843250723
+    x: 586362.75081157754
+    y: 4140766.8547786903
+    theta: 1.3207200932770302
+    kappa: 0.0011242935464075203
+    s: 5.3134778292155085
+    dkappa: -0.0015221138515309097
     ddkappa: 0
   }
-  v: 3.6826259014966833
-  a: -0.70798660460200313
+  v: 3.6826261068463415
+  a: -0.70798644088457563
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 586362.83996584616
-    y: 4140767.20842167
-    theta: 1.3211485116978936
-    kappa: 0.000517314705022565
-    s: 5.6781746167043456
-    dkappa: -0.0014398560940083931
+    x: 586362.84100118023
+    y: 4140767.2081477465
+    theta: 1.3210248509820293
+    kappa: 0.00060247549890623663
+    s: 5.678174766002968
+    dkappa: -0.0014198707463363839
     ddkappa: 0
   }
-  v: 3.61105995191246
-  a: -0.72313409767563852
+  v: 3.6110601734991108
+  a: -0.723133936723414
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586362.928260604
-    y: 4140767.55481217
-    theta: 1.3212532176237854
-    kappa: 3.2624058833733978e-05
-    s: 6.0356411783225346
-    dkappa: -0.0013361074291503906
+    x: 586362.92933387181
+    y: 4140767.5545285982
+    theta: 1.3211610371469305
+    kappa: 0.00012389458324778993
+    s: 6.03564135057949
+    dkappa: -0.0013196970257271896
     ddkappa: 0
   }
-  v: 3.5380385682258306
-  a: -0.73709700282102575
+  v: 3.5380388057535415
+  a: -0.73709684500577033
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586363.01472996094
-    y: 4140767.8940623114
-    theta: 1.3211819068213408
-    kappa: -0.0004032683308968283
-    s: 6.385737748488264
-    dkappa: -0.0012345387148812146
+    x: 586363.01582857373
+    y: 4140767.8937723041
+    theta: 1.3211225825548092
+    kappa: -0.00030735080169037578
+    s: 6.3857379452814795
+    dkappa: -0.0012216289389741625
     ddkappa: 0
   }
-  v: 3.4636796943783477
-  a: -0.74988561706361234
+  v: 3.4636799475189535
+  a: -0.74988546265821787
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 586363.0993925964
-    y: 4140768.2260353463
-    theta: 1.3209749041908281
-    kappa: -0.00079656429431974093
-    s: 6.7283364324782564
-    dkappa: -0.001135171624271842
+    x: 586363.10050535621
+    y: 4140768.225741758
+    theta: 1.3209490654786011
+    kappa: -0.00069724656380705454
+    s: 6.7283366553516393
+    dkappa: -0.0011256877092019524
     ddkappa: 0
   }
-  v: 3.38810024460902
-  a: -0.7615102374288456
+  v: 3.388100513012009
+  a: -0.76151008660732944
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586363.18225351023
-    y: 4140768.5506100091
-    theta: 1.3206551469552044
-    kappa: -0.0011502854382205035
-    s: 7.0633209755080033
-    dkappa: -0.0010380297559395069
+    x: 586363.18336989114
+    y: 4140768.5503155254
+    theta: 1.3206629425097918
+    kappa: -0.0010487064741255953
+    s: 7.0633212259697062
+    dkappa: -0.0010318962764654434
     ddkappa: 0
   }
-  v: 3.3114161034543086
-  a: -0.77198116094217317
+  v: 3.3114163867567132
+  a: -0.77198101377967832
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586363.26333943114
-    y: 4140768.8676713584
-    theta: 1.320221145593353
-    kappa: -0.0014606758655126765
-    s: 7.3905866597615173
-    dkappa: -0.00094313624785842976
+    x: 586363.264447432
+    y: 4140768.8673790479
+    theta: 1.320262392753937
+    kappa: -0.0013581009699783757
+    s: 7.3905869392831853
+    dkappa: -0.00094027733476890855
     ddkappa: 0
   }
-  v: 3.2337421257481331
-  a: -0.78130868462904268
+  v: 3.2337424235844128
+  a: -0.78130854110183745
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586363.34263036423
-    y: 4140769.1771281553
-    theta: 1.3197086773587714
-    kappa: -0.0017350972802407227
-    s: 7.7100402014210765
-    dkappa: -0.00085050920901885685
+    x: 586363.34372040094
+    y: 4140769.1768404795
+    theta: 1.3197827208556983
+    kappa: -0.0016325367898073996
+    s: 7.7100405114380814
+    dkappa: -0.00085084806723954387
     ddkappa: 0
   }
-  v: 3.1551921366218671
-  a: -0.78950310551490133
+  v: 3.155192448633799
+  a: -0.78950296550038035
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586363.42012007418
-    y: 4140769.4788973276
-    theta: 1.3191318546695352
-    kappa: -0.0019753545055661774
-    s: 8.0215996554002267
-    dkappa: -0.00076016741139643522
+    x: 586363.42118321615
+    y: 4140769.4786165906
+    theta: 1.3192377379298914
+    kappa: -0.0018737537456020906
+    s: 8.02159999731232
+    dkappa: -0.00076362648290719
     ddkappa: 0
   }
-  v: 3.0758792347897232
-  a: -0.79656586431942089
+  v: 3.0758795606131115
+  a: -0.79656572828992855
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 586363.49580091785
-    y: 4140769.7729078191
-    theta: 1.318503435149982
-    kappa: -0.0021831309418617012
-    s: 8.3251943893572413
-    dkappa: -0.00067212763641762727
+    x: 586363.49682891637
+    y: 4140769.7726361607
+    theta: 1.3186399365532797
+    kappa: -0.0020833753543603446
+    s: 8.32519476452385
+    dkappa: -0.0006786286183748726
     ddkappa: 0
   }
-  v: 2.9959165086002923
-  a: -0.802502277854648
+  v: 2.9959168477833535
+  a: -0.80250214685326116
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586363.56968715054
-    y: 4140770.0590945096
-    theta: 1.3178294665493722
-    kappa: -0.0023571133593622041
-    s: 8.6207650257307975
-    dkappa: -0.00058640313218599445
+    x: 586363.57067152823
+    y: 4140770.0588341462
+    theta: 1.3179950225324921
+    kappa: -0.0022601318122199582
+    s: 8.6207654354611769
+    dkappa: -0.00059586716291323057
     ddkappa: 0
   }
-  v: 2.9154157870223032
-  a: -0.80732845728459235
+  v: 2.9154161390157061
+  a: -0.80732833221918621
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586363.6417435552
-    y: 4140770.3374164812
-    theta: 1.3171291169383494
-    kappa: -0.0025025073057065752
-    s: 8.9082632893006242
-    dkappa: -0.00050300792469842631
+    x: 586363.64267781179
+    y: 4140770.3371691266
+    theta: 1.3173220398946721
+    kappa: -0.0024090487059747062
+    s: 8.9082637348447857
+    dkappa: -0.00051535528284969788
     ddkappa: 0
   }
-  v: 2.8344872879080829
-  a: -0.81106051377326349
+  v: 2.8344876520784696
+  a: -0.81106039541651187
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 586363.71195620007
-    y: 4140770.6078386372
-    theta: 1.3164123882043441
-    kappa: -0.0026212087098707263
-    s: 9.187651846075866
-    dkappa: -0.00042195273847886792
+    x: 586363.712834727
+    y: 4140770.6076057851
+    theta: 1.3166308396471094
+    kappa: -0.0025319548908258175
+    s: 9.1876523286168457
+    dkappa: -0.0004371030984979935
     ddkappa: 0
   }
-  v: 2.7532396179935588
-  a: -0.81371455848467122
+  v: 2.7532399936370631
+  a: -0.81371444747404609
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 586363.78031175327
-    y: 4140770.8703368623
-    theta: 1.315688063585666
-    kappa: -0.0027149872029001328
-    s: 9.4589041421834388
-    dkappa: -0.000343245826137522
+    x: 586363.78112980013
+    y: 4140770.8701197924
+    theta: 1.3159300841467083
+    kappa: -0.0026305581218662361
+    s: 9.4589046628309248
+    dkappa: -0.00036111836501148654
     ddkappa: 0
   }
-  v: 2.6717797728982569
-  a: -0.81530670258282512
+  v: 2.671780159254026
+  a: -0.81530659942059713
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 586363.84679681
-    y: 4140771.1248980374
-    theta: 1.3149640058172909
-    kappa: -0.0027856231584445471
-    s: 9.7220042427563929
-    dkappa: -0.00026689289886442323
+    x: 586363.84755048668
+    y: 4140771.1246978119
+    theta: 1.3152275476727817
+    kappa: -0.0027065767815921912
+    s: 9.7220048025417
+    dkappa: -0.00028740640099123417
     ddkappa: 0
   }
-  v: 2.5902131371253021
-  a: -0.815853057231735
+  v: 2.5902135333890159
+  a: -0.81585296228497306
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 586363.91139825259
-    y: 4140771.3715197863
-    theta: 1.3142471498570942
-    kappa: -0.0028349038806562187
-    s: 9.9769466708222652
-    dkappa: -0.00019289700868834017
+    x: 586363.91208451823
+    y: 4140771.3713372517
+    theta: 1.3145301090234653
+    kappa: -0.0027617362062286365
+    s: 9.97694727069467
+    dkappa: -0.00021596998626895408
     ddkappa: 0
   }
-  v: 2.5086434840614178
-  a: -0.81536973359541043
+  v: 2.5086438893988103
+  a: -0.815369647095982
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 586363.97410361271
-    y: 4140771.6102102227
-    theta: 1.3135434985748351
-    kappa: -0.00286461980725796
-    s: 10.223736246191452
-    dkappa: -0.00012125843437668115
+    x: 586363.97472025035
+    y: 4140771.6100460119
+    theta: 1.3138437469774018
+    kappa: -0.0027977650275674429
+    s: 10.223736887015868
+    dkappa: -0.00014680925477968939
     ddkappa: 0
   }
-  v: 2.4271729759769274
-  a: -0.8138728428378611
+  v: 2.4271733895373049
+  a: -0.813872764882432
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 586364.03490143479
-    y: 4140771.8409876935
-    theta: 1.3128581213555264
-    kappa: -0.0028765607352134884
-    s: 10.462387924345558
-    dkappa: -5.1974585885145711e-05
+    x: 586364.0354470131
+    y: 4140771.8408422349
+    theta: 1.3131735385725527
+    kappa: -0.0028163915370254377
+    s: 10.46238860690157
+    dkappa: -7.9921604941341726e-05
     ddkappa: 0
   }
-  v: 2.3459021640257527
-  a: -0.81137849612309665
+  v: 2.3459025849555157
+  a: -0.81137842667313109
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 586364.09378163994
-    y: 4140772.063880519
-    theta: 1.3121951555530251
-    kappa: -0.0028725120773857738
-    s: 10.69292663532576
-    dkappa: 1.4960066402197847e-05
+    x: 586364.09425546112
+    y: 4140772.0637540491
+    theta: 1.3125236601392112
+    kappa: -0.002819340079376685
+    s: 10.692927360308014
+    dkappa: -1.5301631276342643e-05
     ddkappa: 0
   }
-  v: 2.2649299882454148
-  a: -0.80790280461512676
+  v: 2.2649304157015768
+  a: -0.80790274349688762
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 586364.15073588688
-    y: 4140772.278926739
-    theta: 1.3115578107613439
-    kappa: -0.0028542511583729141
-    s: 10.915387122621169
-    dkappa: 7.9554040153842183e-05
+    x: 586364.15113792219
+    y: 4140772.2788193133
+    theta: 1.3118973910563376
+    kappa: -0.0028083274829952074
+    s: 10.915387890641103
+    dkappa: 4.7058924669923145e-05
     ddkappa: 0
   }
-  v: 2.1843537775570336
-  a: -0.80346187947796122
+  v: 2.1843542107207421
+  a: -0.8034618263825094
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586364.20575792983
-    y: 4140772.4861738491
-    theta: 1.3109483758414833
-    kappa: -0.0028235435603922452
-    s: 11.129813782057186
-    dkappa: 0.00014181885951535349
+    x: 586364.20608874154
+    y: 4140772.4860853646
+    theta: 1.3112971201747214
+    kappa: -0.0027850595366002546
+    s: 11.129814593646126
+    dkappa: 0.00010717119666423059
     ddkappa: 0
   }
-  v: 2.1042692497653284
-  a: -0.79807183187560959
+  v: 2.1042696878553855
+  a: -0.79807178635880482
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 586364.25885669515
-    y: 4140772.6856751489
-    theta: 1.3103693858306411
-    kappa: -0.002780761531529894
-    s: 11.336260500683865
-    dkappa: 0.00020176923011022959
+    x: 586364.25911721063
+    y: 4140772.6856054068
+    theta: 1.3107253308276816
+    kappa: -0.0027498972239472365
+    s: 11.336261356297461
+    dkappa: 0.00016504941003466571
     ddkappa: 0
   }
-  v: 2.0247705115586174
-  a: -0.79174877297208157
+  v: 2.0247709538449987
+  a: -0.79174873445458172
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 586364.31004086358
-    y: 4140772.8774936525
-    theta: 1.3098236575380326
-    kappa: -0.00272645101608477
-    s: 11.53479049566427
-    dkappa: 0.00025942241641894955
+    x: 586364.31023247167
+    y: 4140772.8774423297
+    theta: 1.3101847959914139
+    kappa: -0.0027033691995962273
+    s: 11.534791395688293
+    dkappa: 0.00022071024560655855
     ddkappa: 0
   }
-  v: 1.9459500585088174
-  a: -0.78450881393138683
+  v: 1.9459505043261927
+  a: -0.78450878169864835
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586364.359292713
-    y: 4140773.0617089374
-    theta: 1.309310053259344
-    kappa: -0.0026643098449766374
-    s: 11.725476153162838
-    dkappa: 0.0003147983279036009
+    x: 586364.35941743245
+    y: 4140773.0616755318
+    theta: 1.3096747930720793
+    kappa: -0.0026490477168615504
+    s: 11.725477097920326
+    dkappa: 0.00027417281595494406
     ddkappa: 0
   }
-  v: 1.8678987750714451
-  a: -0.77636806591753516
+  v: 1.8678992238326986
+  a: -0.77636803911981289
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 586364.4066164348
-    y: 4140773.238404104
-    theta: 1.3088277276485243
-    kappa: -0.0025960275683508446
-    s: 11.908398867233739
-    dkappa: 0.00036792016446276777
+    x: 586364.40667654062
+    y: 4140773.2383880373
+    theta: 1.3091946756843567
+    kappa: -0.0025885663362416713
+    s: 11.908399856993487
+    dkappa: 0.00032545946476099944
     ddkappa: 0
   }
-  v: 1.7907059345856151
-  a: -0.767342640094536
+  v: 1.7907063857953665
+  a: -0.76734261774688328
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 586364.45203712815
-    y: 4140773.4076658157
-    theta: 1.308378301325209
-    kappa: -0.0025211864004731173
-    s: 12.083648878709234
-    dkappa: 0.00041881378716785393
+    x: 586364.4520350392
+    y: 4140773.4076664736
+    theta: 1.308745961974161
+    kappa: -0.0025215243457363326
+    s: 12.08364991369565
+    dkappa: 0.00037459516908107583
     ddkappa: 0
   }
-  v: 1.7144591992740423
-  a: -0.75744864762639919
+  v: 1.7144596525421649
+  a: -0.75744862860866757
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586364.49557999335
-    y: 4140773.5695896782
-    theta: 1.3079645958668116
-    kappa: -0.0024393724860050118
-    s: 12.251325114088029
-    dkappa: 0.00046750735015164404
+    x: 586364.49551846238
+    y: 4140773.5696063554
+    theta: 1.3083313296661314
+    kappa: -0.0024475250268219803
+    s: 12.251326194492341
+    dkappa: 0.00042160709831434587
     ddkappa: 0
   }
-  v: 1.6392446202430395
-  a: -0.74670219967713436
+  v: 1.639245075298182
+  a: -0.74670218273397415
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 586364.53722791793
-    y: 4140773.7242915328
-    theta: 1.3075788780276341
-    kappa: -0.0023557570947788805
-    s: 12.41153502442365
-    dkappa: 0.00051403220132902668
+    x: 586364.53710976313
+    y: 4140773.72432349
+    theta: 1.3079438380067263
+    kappa: -0.0023715642690087337
+    s: 12.411536150416447
+    dkappa: 0.00046652535774568889
     ddkappa: 0
   }
-  v: 1.5651466374825196
-  a: -0.73511940741075121
+  v: 1.5651470941856258
+  a: -0.73511939115161073
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586364.57702745544
-    y: 4140773.8718783008
-    theta: 1.3072254674689834
-    kappa: -0.0022680774503728062
-    s: 12.564393969739909
-    dkappa: 0.000558421517230311
+    x: 586364.57685563725
+    y: 4140773.8719247663
+    theta: 1.3075874209840153
+    kappa: -0.0022914608093859033
+    s: 12.56439514148313
+    dkappa: 0.0005093820278969216
     ddkappa: 0
   }
-  v: 1.492229978522301
-  a: -0.72325554372077283
+  v: 1.4922304368033339
+  a: -0.72325552891800382
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 586364.61499470915
-    y: 4140774.0124680586
-    theta: 1.3069028159372247
-    kappa: -0.0021817295155297026
-    s: 12.710020182832155
-    dkappa: 0.000584762829076717
+    x: 586364.61477236275
+    y: 4140774.0125282081
+    theta: 1.3072607305677739
+    kappa: -0.0022122964940597817
+    s: 12.71002140047146
+    dkappa: 0.000535334515552481
     ddkappa: 0
   }
-  v: 1.4204884572660414
-  a: -0.71160382420052248
+  v: 1.4204889168349395
+  a: -0.71160381369698544
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 586364.65113375359
-    y: 4140774.1461804621
-    theta: 1.3066034046834507
-    kappa: -0.0020981012119460304
-    s: 12.848530245041239
-    dkappa: 0.00060132597109107376
+    x: 586364.65086366585
+    y: 4140774.1462535523
+    theta: 1.306956852439372
+    kappa: -0.0021354826088777548
+    s: 12.848531508679523
+    dkappa: 0.00055209813345983357
     ddkappa: 0
   }
-  v: 1.3499047080360822
-  a: -0.70008727406371107
+  v: 1.3499051683321872
+  a: -0.7000872704284733
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586364.68550259771
-    y: 4140774.2731191721
-    theta: 1.3063358657967303
-    kappa: -0.0020201356991114763
-    s: 12.980039386014191
-    dkappa: 0.00059807996599796093
+    x: 586364.68518822407
+    y: 4140774.2732042922
+    theta: 1.3066837393571611
+    kappa: -0.0020636249982731005
+    s: 12.98004069568589
+    dkappa: 0.00055031681978073106
     ddkappa: 0
   }
-  v: 1.2804690626333946
-  a: -0.68862891852404917
+  v: 1.2804695228528309
+  a: -0.68862892405238529
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586364.71808161307
-    y: 4140774.3934082687
-    theta: 1.306085255858112
-    kappa: -0.00194650317928611
-    s: 13.104662253452059
-    dkappa: 0.00059169471232549441
+    x: 586364.71772545506
+    y: 4140774.3935047458
+    theta: 1.3064276146544378
+    kappa: -0.0019957173969544655
+    s: 13.104663609100209
+    dkappa: 0.00054554175055456866
     ddkappa: 0
   }
-  v: 1.2121795503375776
-  a: -0.677151782795247
+  v: 1.2121800094606325
+  a: -0.6771517995086388
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586364.74893818039
-    y: 4140774.507148461
-    theta: 1.3058631881576521
-    kappa: -0.0018781804626063683
-    s: 13.222513682857789
-    dkappa: 0.00058565503782646573
+    x: 586364.74854374386
+    y: 4140774.5072553633
+    theta: 1.306199200120693
+    kappa: -0.0019324785220903667
+    s: 13.222515084313796
+    dkappa: 0.00054102481322207787
     ddkappa: 0
   }
-  v: 1.14504189790686
-  a: -0.66557889209101539
+  v: 1.1450423547253616
+  a: -0.66557892173715161
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586364.778059527
-    y: 4140774.6144631831
-    theta: 1.305655974953708
-    kappa: -0.0018139191361547305
-    s: 13.333709467284075
-    dkappa: 0.00057995624142748051
+    x: 586364.77762916591
+    y: 4140774.614579875
+    theta: 1.3059858231975285
+    kappa: -0.0018729627849596348
+    s: 13.333710914250254
+    dkappa: 0.000536762764146358
     ddkappa: 0
   }
-  v: 1.079069529578099
-  a: -0.65383327162506488
+  v: 1.0790699827227967
+  a: -0.65383331567784153
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 586364.805496832
-    y: 4140774.7154603098
-    theta: 1.3054702830935749
-    kappa: -0.0017542738860493075
-    s: 13.438367127081234
-    dkappa: 0.0005745918149241832
+    x: 586364.80503361835
+    y: 4140774.7155859666
+    theta: 1.3057936592268993
+    kappa: -0.0018175729535017955
+    s: 13.438368619116053
+    dkappa: 0.000532750610630885
     ddkappa: 0
   }
-  v: 1.0142835670667811
-  a: -0.641837946611106
+  v: 1.0142840150347241
+  a: -0.641838006270626
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 586364.83126788354
-    y: 4140774.8102593739
-    theta: 1.305301378453906
-    kappa: -0.0016987707450409641
-    s: 13.536606679645061
-    dkappa: 0.00056955595024019688
+    x: 586364.83077438769
+    y: 4140774.8103933036
+    theta: 1.305618291578065
+    kappa: -0.0017659421797966471
+    s: 13.536608216151155
+    dkappa: 0.00052898409336178827
     ddkappa: 0
   }
-  v: 0.95071282956702152
-  a: -0.62951594226284946
+  v: 0.95071327074893841
+  a: -0.62951601845542293
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586364.85538867791
-    y: 4140774.8989837817
-    theta: 1.3051436671102059
-    kappa: -0.0016468580501134593
-    s: 13.628551409164695
-    dkappa: 0.00056484274185913453
+    x: 586364.8548668857
+    y: 4140774.8991254447
+    theta: 1.3054545062099157
+    kappa: -0.0017176451475493942
+    s: 13.628552989379592
+    dkappa: 0.00052545889676668567
     ddkappa: 0
   }
-  v: 0.88839383375156467
-  a: -0.61679028379400569
+  v: 0.88839426645924235
+  a: -0.61679037717214991
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 586364.87792493287
-    y: 4140774.9817476012
-    theta: 1.305008276931493
-    kappa: -0.0015995054747385872
-    s: 13.714328636370476
-    dkappa: 0.00056044495673038611
+    x: 586364.877378179
+    y: 4140774.9818961024
+    theta: 1.3053126550577365
+    kappa: -0.0016733940593483261
+    s: 13.714330259360082
+    dkappa: 0.00052216941698768251
     ddkappa: 0
   }
-  v: 0.82737079377178335
-  a: -0.60358399641828531
+  v: 0.82737121626544718
+  a: -0.60358410736072465
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 586364.89887551346
-    y: 4140775.058688073
-    theta: 1.304882412945096
-    kappa: -0.0015554846760235039
-    s: 13.794070488281818
-    dkappa: 0.0005563566043564717
+    x: 586364.89830555464
+    y: 4140775.0588429314
+    theta: 1.3051807846952626
+    kappa: -0.0016322565229499523
+    s: 13.79407215293662
+    dkappa: 0.00051911138836868926
     ddkappa: 0
   }
-  v: 0.76769562125768009
-  a: -0.58982010534939888
+  v: 0.76769603177337231
+  a: -0.58982023396106475
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 586364.91827659821
-    y: 4140775.1299380609
-    theta: 1.3047658578054702
-    kappa: -0.0015147196361885783
-    s: 13.867914667955068
-    dkappa: 0.00055257062472305969
+    x: 586364.91768515133
+    y: 4140775.130098809
+    theta: 1.3050586674057869
+    kappa: -0.0015941614996946756
+    s: 13.86791637298909
+    dkappa: 0.00051627953013032247
     ddkappa: 0
   }
-  v: 0.7094279253178859
-  a: -0.575421635801057
+  v: 0.7094283220948453
+  a: -0.5754217819130879
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 586364.93618725811
-    y: 4140775.1956307697
-    theta: 1.3046663361637409
-    kappa: -0.0014778782203095607
-    s: 13.93600522423136
-    dkappa: 0.00054907923639264489
+    x: 586364.93557712075
+    y: 4140775.1957966518
+    theta: 1.3049535136805932
+    kappa: -0.0015595937382654711
+    s: 13.93600696818385
+    dkappa: 0.00051366789841329947
     ddkappa: 0
   }
-  v: 0.65263501253966072
-  a: -0.56031161298697019
+  v: 0.65263539384770175
+  a: -0.5603117761567119
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 586364.95262771845
-    y: 4140775.2559173652
-    theta: 1.304576307289022
-    kappa: -0.0014441906742627387
-    s: 13.998493321484494
-    dkappa: 0.00054587505289733162
+    x: 586364.95200061309
+    y: 4140775.25608791
+    theta: 1.3048582336382608
+    kappa: -0.0015279618887471193
+    s: 13.998495102724354
+    dkappa: 0.00051127108162784423
     ddkappa: 0
   }
-  v: 0.59739188698889345
-  a: -0.54441306212084906
+  v: 0.597392251155786
+  a: -0.54441324163185445
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 586364.96763603133
-    y: 4140775.3109523263
-    theta: 1.3044941209320275
-    kappa: -0.0014134376887454483
-    s: 14.055538009368785
-    dkappa: 0.00054294998947807406
+    x: 586364.9669934368
+    y: 4140775.311127129
+    theta: 1.3047712535466023
+    kappa: -0.0014990855249548943
+    s: 14.055539826101727
+    dkappa: 0.00050908305422843578
     ddkappa: 0
   }
-  v: 0.54378125021010193
-  a: -0.52764900841640427
+  v: 0.54378159564895012
+  a: -0.527649203278433
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 586364.98125631979
-    y: 4140775.36089745
-    theta: 1.304419535475525
-    kappa: -0.0013855288535920916
-    s: 14.107306992566929
-    dkappa: 0.00054029544679195759
+    x: 586364.98059966869
+    y: 4140775.3610761189
+    theta: 1.3046923176959471
+    kappa: -0.0014728797539010781
+    s: 14.107308842845388
+    dkappa: 0.00050709738368137722
     ddkappa: 0
   }
-  v: 0.49189350122643338
-  a: -0.50994247708734619
+  v: 0.49189382646305457
+  a: -0.50994268603636539
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 586364.99354301579
-    y: 4140775.4059214881
-    theta: 1.3043554436125153
-    kappa: -0.0013606717562349888
-    s: 14.15397740053788
-    dkappa: 0.00053790221966714541
+    x: 586364.99287419545
+    y: 4140775.406103509
+    theta: 1.30462411906892
+    kappa: -0.0014494816311090646
+    s: 14.153979282273646
+    dkappa: 0.0005053071408341489
     ddkappa: 0
   }
-  v: 0.44182673653966342
-  a: -0.49121649334738554
+  v: 0.44182704023996833
+  a: -0.49121671484556928
   relative_time: 5.7999999999999963
 }
 trajectory_point {
   path_point {
-    x: 586365.00454370328
-    y: 4140775.4462056295
-    theta: 1.3043008917765666
-    kappa: -0.0013386995850040969
-    s: 14.195736557264686
-    dkappa: 0.00053576072936559443
+    x: 586365.003864441
+    y: 4140775.44639053
+    theta: 1.3045657285594232
+    kappa: -0.0014287470963164396
+    s: 14.195738468244286
+    dkappa: 0.00050370516997039314
     ddkappa: 0
   }
-  v: 0.39368675013019655
-  a: -0.47139408241023295
+  v: 0.39368703112756814
+  a: -0.47139431464596232
   relative_time: 5.8999999999999959
 }
 trajectory_point {
   path_point {
-    x: 586365.01430284674
-    y: 4140775.4819432786
-    theta: 1.3042524966925009
-    kappa: -0.0013192072059414032
-    s: 14.232782751002377
-    dkappa: 0.00053386092891900849
+    x: 586365.01361432124
+    y: 4140775.4821307361
+    theta: 1.3045139280353018
+    kappa: -0.0014103526728291411
+    s: 14.232784688905193
+    dkappa: 0.00050228399844294146
     ddkappa: 0
   }
-  v: 0.3475870334570666
-  a: -0.45039826948959882
+  v: 0.34758729077973932
+  a: -0.4503985103774622
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 586365.02287577256
-    y: 4140775.5133370403
-    theta: 1.3042099839992467
-    kappa: -0.0013020841128550402
-    s: 14.265326004025811
-    dkappa: 0.000532192047979038
+    x: 586365.02217911009
+    y: 4140775.5135267437
+    theta: 1.304468423829773
+    kappa: -0.0013941940791034834
+    s: 14.265327966444932
+    dkappa: 0.00050103556935559212
     ddkappa: 0
   }
-  v: 0.30364877545793589
-  a: -0.4281520797991939
+  v: 0.30364900835637509
+  a: -0.42815232697998673
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 586365.03032110143
-    y: 4140775.5406015795
-    theta: 1.3041730630013497
-    kappa: -0.0012872132205188247
-    s: 14.293588842377552
-    dkappa: 0.00053074267497994917
+    x: 586365.02961737232
+    y: 4140775.5407932349
+    theta: 1.3044289047922166
+    kappa: -0.0013801608251343476
+    s: 14.293590826843364
+    dkappa: 0.00049995134614021349
     ddkappa: 0
   }
-  v: 0.26200086254909666
-  a: -0.40457853855272863
+  v: 0.26200107052337757
+  a: -0.40457878939345343
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
-    x: 586365.03670095012
-    y: 4140775.5639643678
-    theta: 1.3041414256634691
-    kappa: -0.0012744704596585389
-    s: 14.317807065615716
-    dkappa: 0.00052950071766446455
+    x: 586365.035991166
+    y: 4140775.564157696
+    theta: 1.3043950412119225
+    kappa: -0.0013681358302537546
+    s: 14.317809069622232
+    dkappa: 0.00049902228302747982
     ddkappa: 0
   }
-  v: 0.2227798786254688
-  a: -0.37960067096391364
+  v: 0.22278006145265619
+  a: -0.37960092255778005
   relative_time: 6.2999999999999945
 }
 trajectory_point {
   path_point {
-    x: 586365.04208113533
-    y: 4140775.5836664224
-    theta: 1.3041147456048634
-    kappa: -0.0012637243719381866
-    s: 14.33823051656185
-    dkappa: 0.00052845336360960468
+    x: 586365.04136624513
+    y: 4140775.583861162
+    theta: 1.3043664837418241
+    kappa: -0.0013579950409294279
+    s: 14.338232537595783
+    dkappa: 0.00049823879551760646
     ddkappa: 0
   }
-  v: 0.18613010506060207
-  a: -0.35314150224645952
+  v: 0.18613026282212924
+  a: -0.35314175141288418
   relative_time: 6.3999999999999941
 }
 trajectory_point {
   path_point {
-    x: 586365.046531376
-    y: 4140775.5999630508
-    theta: 1.3040926770938637
-    kappa: -0.0012548357049462766
-    s: 14.355123851048781
-    dkappa: 0.00052758704075252954
+    x: 586365.04581226211
+    y: 4140775.6001589582
+    theta: 1.3043428623222173
+    kappa: -0.0013496070485633715
+    s: 14.355125886621344
+    dkappa: 0.00049759073085108658
     ddkappa: 0
   }
-  v: 0.15220352070667487
-  a: -0.32512405761407681
+  v: 0.15220365381572309
+  a: -0.32512430089868366
   relative_time: 6.4999999999999938
 }
 trajectory_point {
   path_point {
-    x: 586365.05012549553
-    y: 4140775.6131245941
-    theta: 1.3040748540423406
-    kappa: -0.001247657007182079
-    s: 14.36876730766849
-    dkappa: 0.00052688737791637883
+    x: 586365.04940297082
+    y: 4140775.613321445
+    theta: 1.3043237851044689
+    kappa: -0.0013428327072904319
+    s: 14.368769355349945
+    dkappa: 0.00049706733847942614
     ddkappa: 0
   }
-  v: 0.12115980189449442
-  a: -0.2954713622804761
+  v: 0.12115991112337214
+  a: -0.29547159595509603
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
-    x: 586365.05294162559
-    y: 4140775.6234371662
-    theta: 1.3040608890001606
-    kappa: -0.0012420322230419071
-    s: 14.379457477519964
-    dkappa: 0.0005263391653361145
+    x: 586365.05221642845
+    y: 4140775.6236347565
+    theta: 1.3043088373747169
+    kappa: -0.0013375247517768748
+    s: 14.3794595349769
+    dkappa: 0.00049665724053588056
     ddkappa: 0
   }
-  v: 0.09316632243349679
-  a: -0.26410644145936796
+  v: 0.093166408941019713
+  a: -0.26410666152203904
   relative_time: 6.6999999999999931
 }
 trajectory_point {
   path_point {
-    x: 586365.05506380869
-    y: 4140775.6312030172
-    theta: 1.3040509802915206
-    kappa: -0.0012378563020284635
-    s: 14.387508073957076
-    dkappa: 0.00052592629732794408
+    x: 586365.05433670676
+    y: 4140775.6314011356
+    theta: 1.3042981558392941
+    kappa: -0.0013335723108762268
+    s: 14.387510138992429
+    dkappa: 0.00049634838156810727
     ddkappa: 0
   }
-  v: 0.0683981536117473
-  a: -0.23095232036446295
+  v: 0.0683982189706165
+  a: -0.23095252253943038
   relative_time: 6.7999999999999927
 }
 trajectory_point {
   path_point {
-    x: 586365.05657833314
-    y: 4140775.6367423311
-    theta: 1.3040442314564578
-    kappa: -0.0012349090477723175
-    s: 14.393250702336426
-    dkappa: 0.00052563178213062257
+    x: 586365.05584992934
+    y: 4140775.6369408104
+    theta: 1.3042908385343184
+    kappa: -0.0013307765336075695
+    s: 14.393252772932236
+    dkappa: 0.0004961280562826183
     ddkappa: 0
   }
-  v: 0.04703806419594056
-  a: -0.19593202420947167
+  v: 0.047038110420121709
+  a: -0.19593220394718761
   relative_time: 6.8999999999999924
 }
 trajectory_point {
   path_point {
-    x: 586365.057576546
-    y: 4140775.6403932548
-    theta: 1.3040397833450195
-    kappa: -0.0012329665324749973
-    s: 14.397035629765218
-    dkappa: 0.00052543766916093306
+    x: 586365.0568472842
+    y: 4140775.6405919734
+    theta: 1.3042860157480825
+    kappa: -0.0013289338557158858
+    s: 14.397037704128131
+    dkappa: 0.00049598284137687416
     ddkappa: 0
   }
-  v: 0.029276520431399115
-  a: -0.1589685782081047
+  v: 0.029276550003502991
+  a: -0.15896873068522863
   relative_time: 6.999999999999992
 }
 trajectory_point {
   path_point {
-    x: 586365.058155949
-    y: 4140775.6425123992
-    theta: 1.3040372014810901
-    kappa: -0.0012318390179683544
-    s: 14.399232554849124
-    dkappa: 0.000525324998136126
+    x: 586365.05742618931
+    y: 4140775.6427112557
+    theta: 1.3042832164076577
+    kappa: -0.0013278642908514282
+    s: 14.399234631458615
+    dkappa: 0.00049589855276534829
     ddkappa: 0
   }
-  v: 0.015311686042075277
-  a: -0.11998500757407254
+  v: 0.015311701940736109
+  a: -0.11998512769347097
   relative_time: 7.0999999999999917
 }
 trajectory_point {
   path_point {
-    x: 586365.05841937219
-    y: 4140775.643475859
-    theta: 1.304036027647699
-    kappa: -0.0012313263983213842
-    s: 14.400231377440139
-    dkappa: 0.00052527377273670084
+    x: 586365.057689386
+    y: 4140775.6436747788
+    theta: 1.3042819436994719
+    kappa: -0.0013273780177475139
+    s: 14.400233455099489
+    dkappa: 0.00049586023130761042
     ddkappa: 0
   }
-  v: 0.005349422230549879
-  a: -0.078904337521085832
+  v: 0.0053494279578049937
+  a: -0.078904419911832391
   relative_time: 7.1999999999999913
 }
 trajectory_point {
   path_point {
-    x: 586365.05847517587
-    y: 4140775.6436799583
-    theta: 1.3040357789824026
-    kappa: -0.0012312178047872075
-    s: 14.400442968384448
-    dkappa: 0.00052526292112928382
+    x: 586365.05774514168
+    y: 4140775.6438788916
+    theta: 1.3042816740884922
+    kappa: -0.0013272750054706718
+    s: 14.400445046274452
+    dkappa: 0.0004958521132756203
     ddkappa: 0
   }
   v: 0
-  a: -0.035649593262855168
+  a: -0.035649632280230388
   relative_time: 7.2999999999999909
 }
 trajectory_point {
   path_point {
-    x: 586365.05843745428
-    y: 4140775.6435419936
-    theta: 1.3040359470726637
-    kappa: -0.0012312912107500557
-    s: 14.400299939270287
-    dkappa: 0.0005252702564895053
+    x: 586365.05770745245
+    y: 4140775.6437409171
+    theta: 1.3042818563374035
+    kappa: -0.0013273446386707862
+    s: 14.400302017005705
+    dkappa: 0.000495857600821022
     ddkappa: 0
   }
   v: 0
-  a: 0.0098561999869089065
+  a: 0.00985621026141704
   relative_time: 7.3999999999999906
 }
 trajectory_point {
   path_point {
-    x: 586365.05842623848
-    y: 4140775.6435009721
-    theta: 1.3040359970512325
-    kappa: -0.00123131303667225
-    s: 14.40025741217581
-    dkappa: 0.00052527243752487692
+    x: 586365.05769624619
+    y: 4140775.6436998933
+    theta: 1.304281910525789
+    kappa: -0.0013273653428312459
+    s: 14.400259489864547
+    dkappa: 0.00049585923244243734
     ddkappa: 0
   }
-  v: 0.00165212846800878
-  a: 0.057690017014495928
+  v: 0.0016521303379893949
+  a: 0.057690082773192528
   relative_time: 7.49999999999999
 }
 trajectory_point {
   path_point {
-    x: 586365.05856769916
-    y: 4140775.644018359
-    theta: 1.3040353666915252
-    kappa: -0.0012310377550411892
-    s: 14.400793789416948
-    dkappa: 0.00052524492899766993
+    x: 586365.05783758545
+    y: 4140775.6442173142
+    theta: 1.3042812270693089
+    kappa: -0.0013271042095190865
+    s: 14.40079586772198
+    dkappa: 0.00049583865345275986
     ddkappa: 0
   }
-  v: 0.0099127085660663129
-  a: 0.107928832606195
+  v: 0.0099127200544059258
+  a: 0.10792896031517828
   relative_time: 7.59999999999999
 }
 trajectory_point {
   path_point {
-    x: 586365.05899435
-    y: 4140775.6455788175
-    theta: 1.3040334655030037
-    kappa: -0.0012302074953163297
-    s: 14.402411523295275
-    dkappa: 0.00052516196224779241
+    x: 586365.05826386972
+    y: 4140775.6457778751
+    theta: 1.3042791657388766
+    kappa: -0.0013263166216351378
+    s: 14.402413603499307
+    dkappa: 0.00049577658644644855
     ddkappa: 0
   }
-  v: 0.023320627434261255
-  a: 0.16064962154829587
+  v: 0.023320655070701503
+  a: 0.16064981794745636
   relative_time: 7.6999999999999895
 }
 trajectory_point {
   path_point {
-    x: 586365.05984525057
-    y: 4140775.6486909525
-    theta: 1.3040296738265518
-    kappa: -0.0012285516488761724
-    s: 14.405637885845863
-    dkappa: 0.00052499649571566677
+    x: 586365.059114039
+    y: 4140775.648890214
+    theta: 1.3042750546798367
+    kappa: -0.0013247458786641699
+    s: 14.405639969918735
+    dkappa: 0.00049565280176682184
     ddkappa: 0
   }
-  v: 0.042127931146948322
-  a: 0.21592935862708781
+  v: 0.0421279821489092
+  a: 0.21592963073010948
   relative_time: 7.7999999999999892
 }
 trajectory_point {
   path_point {
-    x: 586365.06126620877
-    y: 4140775.6538880514
-    theta: 1.304023341929853
-    kappa: -0.0012257864739652416
-    s: 14.41102573858516
-    dkappa: 0.00052472017546510707
+    x: 586365.06053377606
+    y: 4140775.6540876534
+    theta: 1.3042681894311394
+    kappa: -0.001322122821925038
+    s: 14.411027829253971
+    dkappa: 0.00049544608797335112
     ddkappa: 0
   }
-  v: 0.066594363257111169
-  a: 0.2738450186288604
+  v: 0.0665944455570718
+  a: 0.27384537372321988
   relative_time: 7.8999999999999888
 }
 trajectory_point {
   path_point {
-    x: 586365.06340998469
-    y: 4140775.6617288277
-    theta: 1.3040137891027668
-    kappa: -0.0012216147006410714
-    s: 14.419154302258836
-    dkappa: 0.00052430329570619689
+    x: 586365.06267570972
+    y: 4140775.6619289434
+    theta: 1.3042578319445157
+    kappa: -0.0013181654598208292
+    s: 14.41915640308083
+    dkappa: 0.000495134222308955
     ddkappa: 0
   }
-  v: 0.096987364796362829
-  a: 0.33447357633990288
+  v: 0.096987487069237988
+  a: 0.33447402198686976
   relative_time: 7.9999999999999885
 }
 decision {
diff --git a/modules/planning/testdata/garage_test/result_stop_over_line_0.pb.txt b/modules/planning/testdata/garage_test/result_stop_over_line_0.pb.txt
index 25a81b8f39..011a82d0b4 100644
--- a/modules/planning/testdata/garage_test/result_stop_over_line_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_stop_over_line_0.pb.txt
@@ -9,7 +9,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -24,7 +24,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -39,7 +39,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -54,7 +54,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -69,7 +69,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -84,7 +84,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -99,7 +99,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -114,7 +114,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -129,7 +129,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -144,7 +144,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -159,7 +159,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -174,7 +174,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -189,7 +189,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -204,7 +204,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -219,7 +219,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -234,7 +234,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -249,7 +249,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -264,7 +264,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -279,7 +279,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -294,7 +294,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -309,7 +309,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -324,7 +324,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -339,7 +339,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -354,7 +354,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -369,7 +369,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -384,7 +384,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -399,7 +399,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -414,7 +414,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -429,7 +429,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -444,7 +444,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -459,7 +459,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -474,7 +474,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -489,7 +489,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -504,7 +504,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -519,7 +519,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -534,7 +534,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -549,7 +549,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -564,7 +564,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -579,7 +579,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -594,7 +594,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -609,7 +609,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -624,7 +624,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -639,7 +639,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -654,7 +654,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -669,7 +669,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -684,7 +684,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -699,7 +699,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -714,7 +714,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -729,7 +729,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -744,7 +744,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -759,7 +759,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -774,7 +774,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -789,7 +789,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -804,7 +804,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -819,7 +819,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -834,7 +834,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -849,7 +849,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -864,7 +864,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -879,7 +879,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -894,7 +894,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -909,7 +909,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -924,7 +924,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -939,7 +939,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -954,7 +954,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -969,7 +969,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -984,7 +984,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -999,7 +999,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -1014,7 +1014,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -1029,7 +1029,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -1044,7 +1044,7 @@ trajectory_point {
     y: 4140735.5241937214
     z: 0
     theta: 1.3119887988590211
-    kappa: -0.0058950283300836471
+    kappa: -0.0050410904032342748
     s: 0
     dkappa: 0
     ddkappa: 0
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_avoid_change_left_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_avoid_change_left_0.pb.txt
index 3c255f7a85..6a06913c93 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_avoid_change_left_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_avoid_change_left_0.pb.txt
@@ -20,12 +20,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587568.78882458375
-    y: 4141500.4067281336
-    theta: 2.898853080264769
-    kappa: 0.00015095655106007476
+    x: 587568.78882432228
+    y: 4141500.406727077
+    theta: 2.8988566794349335
+    kappa: 0.00015883043563877356
     s: 0.22509446286299248
-    dkappa: 5.6574999380800477e-05
+    dkappa: 6.53261211071179e-05
     ddkappa: 0
   }
   v: 11.259477305033938
@@ -34,12 +34,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587568.57014173944
-    y: 4141500.4608717076
-    theta: 2.8988988485822307
-    kappa: 0.00020190358563174857
+    x: 587568.57014121627
+    y: 4141500.4608695945
+    theta: 2.8989060499827706
+    kappa: 0.00021765804948829882
     s: 0.45038031025730868
-    dkappa: 0.00011319810114316742
+    dkappa: 0.00013070778515790604
     ddkappa: 0
   }
   v: 11.269137320910886
@@ -48,12 +48,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587568.35126963852
-    y: 4141500.51506214
-    theta: 2.8989446565093595
-    kappa: 0.00025289471173178758
+    x: 587568.35126885341
+    y: 4141500.5150589687
+    theta: 2.8989554632577832
+    kappa: 0.00027653657502339145
     s: 0.67586112871155346
-    dkappa: 0.00016987020672132991
+    dkappa: 0.00019614603302204648
     ddkappa: 0
   }
   v: 11.278973291057017
@@ -62,12 +62,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587568.132204814
-    y: 4141500.5692997905
-    theta: 2.8989908525162491
-    kappa: 0.00030389655057521346
+    x: 587568.13220375485
+    y: 4141500.569295512
+    theta: 2.8990053212066673
+    kappa: 0.00033542674499202325
     s: 0.901540370357957
-    dkappa: 0.00022610600029117014
+    dkappa: 0.00026107967843223736
     ddkappa: 0
   }
   v: 11.288978532408626
@@ -76,12 +76,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587567.91292958125
-    y: 4141500.6235270086
-    theta: 2.8990807910664969
-    kappa: 0.00035065144472834096
+    x: 587567.91292673652
+    y: 4141500.6235155105
+    theta: 2.8991056385177929
+    kappa: 0.00038932184111806738
     s: 1.1274213544025757
-    dkappa: 0.00022129741273655359
+    dkappa: 0.00025542512321950132
     ddkappa: 0
   }
   v: 11.299146435412135
@@ -90,12 +90,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587567.69345541135
-    y: 4141500.6778034251
-    theta: 2.8991708112147783
-    kappa: 0.00039744875718167717
+    x: 587567.69345077965
+    y: 4141500.6777847
+    theta: 2.8992060468436209
+    kappa: 0.00044326583348072295
     s: 1.3535072685954965
-    dkappa: 0.00021648446259884993
+    dkappa: 0.00024976543792092282
     ddkappa: 0
   }
   v: 11.309470464024091
@@ -104,12 +104,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587567.47377933585
-    y: 4141500.7321297722
-    theta: 2.8992609141771397
-    kappa: 0.00044428912085900575
+    x: 587567.47377291566
+    y: 4141500.7321038148
+    theta: 2.8993065475401316
+    kappa: 0.00049725945166111249
     s: 1.5798011707010382
-    dkappa: 0.00021166708478392334
+    dkappa: 0.00024410054599045158
     ddkappa: 0
   }
   v: 11.319944155711168
@@ -118,12 +118,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587567.25389779778
-    y: 4141500.7865038658
-    theta: 2.8993523001853689
-    kappa: 0.000491043660849832
+    x: 587567.25388950051
+    y: 4141500.7864703164
+    theta: 2.8994085228986122
+    kappa: 0.00055115114725833557
     s: 1.8063059899679537
-    dkappa: 0.00020684517133545457
+    dkappa: 0.00023843033160851084
     ddkappa: 0
   }
   v: 11.330561121450163
@@ -132,12 +132,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587567.03378555621
-    y: 4141500.8408352276
-    theta: 2.8994828029704771
-    kappa: 0.00053362760292088461
+    x: 587567.033772554
+    y: 4141500.8407826135
+    theta: 2.8995555478775441
+    kappa: 0.00060013781135586058
     s: 2.0330245285996345
-    dkappa: 0.00020201722390355509
+    dkappa: 0.00023275338394044979
     ddkappa: 0
   }
   v: 11.341315045728004
@@ -146,12 +146,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587566.8134632241
-    y: 4141500.8952184469
-    theta: 2.8996134303207977
-    kappa: 0.00057625219007762679
+    x: 587566.81344551244
+    y: 4141500.89514675
+    theta: 2.8997027131934496
+    kappa: 0.00064917123174373969
     s: 2.2599594632243107
-    dkappa: 0.00019718466834174722
+    dkappa: 0.00022707101779741428
     ddkappa: 0
   }
   v: 11.35219968654174
@@ -160,12 +160,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587566.59292832343
-    y: 4141500.9496541359
-    theta: 2.8997441837011513
-    kappa: 0.00061891790175373827
+    x: 587566.59290589788
+    y: 4141500.9495633375
+    theta: 2.8998500204952511
+    kappa: 0.00069825195994105437
     s: 2.4871133463652555
-    dkappa: 0.00019234745029431634
+    dkappa: 0.00022138316926517343
     ddkappa: 0
   }
   v: 11.363208875398545
@@ -174,12 +174,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587566.37217635859
-    y: 4141501.0041342094
-    theta: 2.8998774916681125
-    kappa: 0.00066133579460974691
+    x: 587566.37214893859
+    y: 4141501.0040231678
+    theta: 2.9000002595626375
+    kappa: 0.000747040211068329
     s: 2.7144886079109858
-    dkappa: 0.00018750574541202581
+    dkappa: 0.00021569001065350045
     ddkappa: 0
   }
   v: 11.374336517315724
@@ -188,12 +188,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587566.15117718512
-    y: 4141501.0585460356
-    theta: 2.9000450537097033
-    kappa: 0.00069972681211914224
+    x: 587566.15114088985
+    y: 4141501.0583989331
+    theta: 2.900189846763999
+    kappa: 0.0007910919352281142
     s: 2.9420875565854669
-    dkappa: 0.00018266247425399365
+    dkappa: 0.00020999452920408135
     ddkappa: 0
   }
   v: 11.385576590820701
@@ -202,12 +202,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587565.92995868518
-    y: 4141501.1130118603
-    theta: 2.9002127820535661
-    kappa: 0.000738155930048602
+    x: 587565.9299135058
+    y: 4141501.1128286612
+    theta: 2.900379622129921
+    kappa: 0.00083518737766639136
     s: 3.1699123814183143
-    dkappa: 0.00017781439648552035
+    dkappa: 0.00020429339538466081
     ddkappa: 0
   }
   v: 11.396923147951034
@@ -216,12 +216,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587565.70851884829
-    y: 4141501.1675321809
-    theta: 2.9003806782148356
-    kappa: 0.00077662349769819748
+    x: 587565.70846477582
+    y: 4141501.1673128488
+    theta: 2.9005695873717063
+    kappa: 0.000879326939187082
     s: 3.3979651532149933
-    dkappa: 0.00017296146804017911
+    dkappa: 0.00019858655737499021
     ddkappa: 0
   }
   v: 11.408370314254398
@@ -230,12 +230,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587565.48685107892
-    y: 4141501.2220883528
-    theta: 2.9005526061138056
-    kappa: 0.000814612511714891
+    x: 587565.48678748577
+    y: 4141501.2218303154
+    theta: 2.9007641698959143
+    kappa: 0.00092290270486632727
     s: 3.626247826027027
-    dkappa: 0.0001681034845580854
+    dkappa: 0.00019287381751440454
     ddkappa: 0
   }
   v: 11.4199122887886
@@ -244,12 +244,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587565.26492323726
-    y: 4141501.2765573328
-    theta: 2.9007535233770962
-    kappa: 0.00084878070023063242
+    x: 587565.26484549686
+    y: 4141501.2762416219
+    theta: 2.9009919690244557
+    kappa: 0.00096198447495294507
     s: 3.854762238622194
-    dkappa: 0.00016323935517260514
+    dkappa: 0.0001871541685459489
     ddkappa: 0
   }
   v: 11.431543344121572
@@ -258,12 +258,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587565.04276866
-    y: 4141501.3310819622
-    theta: 2.900954645923953
-    kappa: 0.00088298379712290756
+    x: 587565.0426767579
+    y: 4141501.3307085186
+    theta: 2.9012200009070934
+    kappa: 0.0010011061734409522
     s: 4.0835101159547333
-    dkappa: 0.00015837025628574148
+    dkappa: 0.00018142867602334226
     ddkappa: 0
   }
   v: 11.443257826331365
@@ -272,12 +272,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587564.82038578135
-    y: 4141501.3856626255
-    theta: 2.9011559751559823
-    kappa: 0.00091722204351433459
+    x: 587564.82027970278
+    y: 4141501.3852313887
+    theta: 2.9014482671277024
+    kappa: 0.001040268076127782
     s: 4.3124930706355471
-    dkappa: 0.00015349615357165914
+    dkappa: 0.000175697299583405
     ddkappa: 0
   }
   v: 11.455050155006164
@@ -286,12 +286,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587564.59776454268
-    y: 4141501.4402645761
-    theta: 2.9013628777536455
-    kappa: 0.00095068068052679938
+    x: 587564.59764313058
+    y: 4141501.4397707689
+    theta: 2.9016828933190433
+    kappa: 0.0010785121190009222
     s: 4.5417126044024005
-    dkappa: 0.00014861753026762303
+    dkappa: 0.00016996054274186857
     ddkappa: 0
   }
   v: 11.466914823244279
@@ -300,12 +300,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587564.37487443583
-    y: 4141501.4947691355
-    theta: 2.9015935452818242
-    kappa: 0.00098059686650627713
+    x: 587564.37473265524
+    y: 4141501.494191966
+    theta: 2.9019446459926841
+    kappa: 0.0011125891783559916
     s: 4.7711701095901278
-    dkappa: 0.00014373610343682621
+    dkappa: 0.00016422020536020348
     ddkappa: 0
   }
   v: 11.478846397654138
@@ -314,12 +314,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587564.151751921
-    y: 4141501.549330527
-    theta: 2.9018244533465767
-    kappa: 0.0010105442461557591
+    x: 587564.15158975078
+    y: 4141501.5486699082
+    theta: 2.9022066716242354
+    kappa: 0.0011467017699324804
     s: 5.0008668706008326
-    dkappa: 0.0001388495867320122
+    dkappa: 0.00015847388251668439
     ddkappa: 0
   }
   v: 11.490839518354305
@@ -328,12 +328,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587563.92839585408
-    y: 4141501.60394903
-    theta: 2.9020556031074523
-    kappa: 0.0010405229730525256
+    x: 587563.92821327283
+    y: 4141501.6032048757
+    theta: 2.9024689715214773
+    kappa: 0.0011808500686678648
     s: 5.2308040653740919
-    dkappa: 0.00013395795509396207
+    dkappa: 0.00015272154474280214
     ddkappa: 0
   }
   v: 11.502888898973463
@@ -342,12 +342,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587563.70479118
-    y: 4141501.6585674779
-    theta: 2.9022937217653428
-    kappa: 0.001069348273435705
+    x: 587563.70458630612
+    y: 4141501.6577319526
+    theta: 2.9027391886426512
+    kappa: 0.0012136409775453931
     s: 5.4609827668571542
-    dkappa: 0.00012906130494122882
+    dkappa: 0.00014696334151356185
     ddkappa: 0
   }
   v: 11.51498932665042
@@ -356,12 +356,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587563.48091281531
-    y: 4141501.7130871709
-    theta: 2.9025503986112859
-    kappa: 0.0010949788224543797
+    x: 587563.48068055708
+    y: 4141501.7121390719
+    theta: 2.9030304924211974
+    kappa: 0.0012426737878902644
     s: 5.6914039444751507
-    dkappa: 0.00012415982109262414
+    dkappa: 0.00014119955240532859
     ddkappa: 0
   }
   v: 11.527135662034118
@@ -370,12 +370,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587563.25679801684
-    y: 4141501.7676644409
-    theta: 2.9028073465520277
-    kappa: 0.0011206364394178425
+    x: 587563.25653834536
+    y: 4141501.76660365
+    theta: 2.9033221038741006
+    kappa: 0.0012717372592446561
     s: 5.9220684656012885
-    dkappa: 0.0001192531608783436
+    dkappa: 0.00013542967626691747
     ddkappa: 0
   }
   v: 11.539322839283612
@@ -384,12 +384,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587563.03244603961
-    y: 4141501.8222994697
-    theta: 2.9030645664078238
-    kappa: 0.0011463212096189785
+    x: 587563.0321589259
+    y: 4141501.8211258673
+    theta: 2.9036140239211887
+    kappa: 0.0013008314882234368
     s: 6.15297709702706
-    dkappa: 0.00011434130798731649
+    dkappa: 0.00012965369391769373
     ddkappa: 0
   }
   v: 11.551545866068096
@@ -398,12 +398,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587562.80783524946
-    y: 4141501.8769060448
-    theta: 2.9033298396985137
-    kappa: 0.0011704054423426328
+    x: 587562.807517896
+    y: 4141501.8756078728
+    theta: 2.9039150346442373
+    kappa: 0.0013280426158210517
     s: 6.38413050643244
-    dkappa: 0.00010942491833545109
+    dkappa: 0.00012387236970826264
     ddkappa: 0
   }
   v: 11.563799823566878
@@ -412,12 +412,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587562.58294996934
-    y: 4141501.9314220585
-    theta: 2.9036087743542733
-    kappa: 0.0011917161544614115
+    x: 587562.58259757678
+    y: 4141501.9299791423
+    theta: 2.904231467681567
+    kappa: 0.0013519908668015376
     s: 6.6155292638560912
-    dkappa: 0.00010450446271706234
+    dkappa: 0.00011808625247488984
     ddkappa: 0
   }
   v: 11.576079866469403
@@ -426,12 +426,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587562.35782578681
-    y: 4141501.9859959856
-    theta: 2.9038880053558165
-    kappa: 0.0012130495055930339
+    x: 587562.35743831808
+    y: 4141501.9844081718
+    theta: 2.9045482369118809
+    kappa: 0.0013759645587320258
     s: 6.8471738431655691
-    dkappa: 9.9578779950124032e-05
+    dkappa: 0.00011229398847449998
     ddkappa: 0
   }
   v: 11.588381222975231
@@ -440,12 +440,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587562.13246233342
-    y: 4141502.0406279168
-    theta: 2.9041675331166692
-    kappa: 0.0012344055306569238
+    x: 587562.132039751
+    y: 4141502.0388950515
+    theta: 2.904865342790127
+    kappa: 0.0013999637308537733
     s: 7.0790646235275192
-    dkappa: 9.4647861972049e-05
+    dkappa: 0.00010649556822604533
     ddkappa: 0
   }
   v: 11.600699194794053
@@ -454,12 +454,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587561.90682983
-    y: 4141502.0951959249
-    theta: 2.9044556919602957
-    kappa: 0.0012536399917582584
+    x: 587561.90636824793
+    y: 4141502.0933015021
+    theta: 2.9051920986512694
+    kappa: 0.0014214672122202857
     s: 7.3112018908778822
-    dkappa: 8.9712888767561511e-05
+    dkappa: 0.00010069237377615019
     ddkappa: 0
   }
   v: 11.613029157145688
@@ -468,12 +468,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587561.68092592922
-    y: 4141502.1496911268
-    theta: 2.9047530921304707
-    kappa: 0.0012705959803606341
+    x: 587561.68042117008
+    y: 4141502.1476174071
+    theta: 2.9055291890078658
+    kappa: 0.0014402898204651854
     s: 7.5435858393920965
-    dkappa: 8.47739430090159e-05
+    dkappa: 9.4884502317119549e-05
     ddkappa: 0
   }
   v: 11.625366558760076
@@ -482,12 +482,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587561.45478212484
-    y: 4141502.204244202
-    theta: 2.9050508081549649
-    kappa: 0.0012875699757337212
+    x: 587561.454234143
+    y: 4141502.2019909946
+    theta: 2.9058666373795305
+    kappa: 0.0014591324177770043
     s: 7.7762165729553017
-    dkappa: 7.9829752232564444e-05
+    dkappa: 8.9070463066157846e-05
     ddkappa: 0
   }
   v: 11.637706921877285
@@ -496,12 +496,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587561.22839840245
-    y: 4141502.2588551524
-    theta: 2.9053488400002738
-    kappa: 0.0013045619789767468
+    x: 587561.22780715209
+    y: 4141502.2564222668
+    theta: 2.9062044437112546
+    kappa: 0.0014779950053759794
     s: 8.00909410663254
-    dkappa: 7.4880316118024e-05
+    dkappa: 8.3250255646751033e-05
     ddkappa: 0
   }
   v: 11.650045842247509
@@ -510,12 +510,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587561.00173539587
-    y: 4141502.3133600592
-    theta: 2.9056553299281251
-    kappa: 0.0013188363732686414
+    x: 587561.001095762
+    y: 4141502.310725526
+    theta: 2.9065515664690236
+    kappa: 0.0014936613590214925
     s: 8.2422183681389551
-    dkappa: 6.9926278327970935e-05
+    dkappa: 7.7424870022201834e-05
     ddkappa: 0
   }
   v: 11.662378989131069
@@ -524,12 +524,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587560.77480778412
-    y: 4141502.3678192534
-    theta: 2.9059672788242534
-    kappa: 0.0013314007341501109
+    x: 587560.77411650785
+    y: 4141502.3649690789
+    theta: 2.9069047082221373
+    kappa: 0.0015073152468899811
     s: 8.4755891993100043
-    dkappa: 6.4967405714428677e-05
+    dkappa: 7.1593946438449588e-05
     ddkappa: 0
   }
   v: 11.674702105298405
@@ -538,12 +538,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587560.54764064623
-    y: 4141502.4223359306
-    theta: 2.9062795570072013
-    kappa: 0.0013439783569394446
+    x: 587560.54689767293
+    y: 4141502.4192698868
+    theta: 2.9072582227505221
+    kappa: 0.0015209835466815757
     s: 8.7092063575716523
-    dkappa: 6.0003298922058616e-05
+    dkappa: 6.575686821045718e-05
     ddkappa: 0
   }
   v: 11.687011007030092
@@ -552,12 +552,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587560.32023429871
-    y: 4141502.4769100146
-    theta: 2.9065921639809118
-    kappa: 0.0013565692241114581
+    x: 587560.319439574
+    y: 4141502.4736278737
+    theta: 2.9076121094731935
+    kappa: 0.0015346662393513834
     s: 8.9430695174105743
-    dkappa: 5.5033964867659653e-05
+    dkappa: 5.9913643471388932e-05
     ddkappa: 0
   }
   v: 11.699301584116824
@@ -566,12 +566,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587560.092538509
-    y: 4141502.5313298763
-    theta: 2.9069122233740452
-    kappa: 0.0013657744873211612
+    x: 587560.09168559767
+    y: 4141502.5278036613
+    theta: 2.9079739689463735
+    kappa: 0.0015443673765230489
     s: 9.1771782718443635
-    dkappa: 5.0062906098649605e-05
+    dkappa: 5.4068315098496847e-05
     ddkappa: 0
   }
   v: 11.711569799859424
@@ -580,12 +580,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587559.86458829558
-    y: 4141502.5857397355
-    theta: 2.907234873659295
-    kappa: 0.0013739132173366539
+    x: 587559.86367510236
+    y: 4141502.5819603484
+    theta: 2.908338615977943
+    kappa: 0.0015528124732071377
     s: 9.411532133891729
-    dkappa: 4.50877489953363e-05
+    dkappa: 4.8218144626189569e-05
     ddkappa: 0
   }
   v: 11.723811691068841
@@ -594,12 +594,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587559.636400222
-    y: 4141502.64020637
-    theta: 2.9075578606325019
-    kappa: 0.0013820604399032886
+    x: 587559.635426684
+    y: 4141502.6361735454
+    theta: 2.9087036435248015
+    kappa: 0.0015612663821279451
     s: 9.6461305380427014
-    dkappa: 4.0107400446176138e-05
+    dkappa: 4.2361869654552686e-05
     ddkappa: 0
   }
   v: 11.736023368066146
@@ -608,12 +608,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587559.40797491325
-    y: 4141502.6947296308
-    theta: 2.90788118334154
-    kappa: 0.0013902161327057033
+    x: 587559.406940968
+    y: 4141502.6904431055
+    theta: 2.9090690504897543
+    kappa: 0.0015697290801300915
     s: 9.8809728417288323
-    dkappa: 3.51218740924164e-05
+    dkappa: 3.649950622400872e-05
     ddkappa: 0
   }
   v: 11.748201014682538
@@ -622,12 +622,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587559.17925038142
-    y: 4141502.749045996
-    theta: 2.9082096997252438
-    kappa: 0.0013942421973516977
+    x: 587559.178148241
+    y: 4141502.7444714806
+    theta: 2.90943962124493
+    kappa: 0.0015733362506438987
     s: 10.116058326793402
-    dkappa: 3.0131731840774664e-05
+    dkappa: 3.0632417450633523e-05
     ddkappa: 0
   }
   v: 11.760340888259346
@@ -636,12 +636,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587558.950284648
-    y: 4141502.8033958012
-    theta: 2.9085389712233813
-    kappa: 0.0013979178522317594
+    x: 587558.949113564
+    y: 4141502.7985300147
+    theta: 2.9098109853120966
+    kappa: 0.0015765292369380975
     s: 10.351386200961619
-    dkappa: 2.5136491252828582e-05
+    dkappa: 2.4759394878753029e-05
     ddkappa: 0
   }
   v: 11.772439319648017
@@ -650,12 +650,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587558.72108392
-    y: 4141502.8578013866
-    theta: 2.9088685806623493
-    kappa: 0.0014015972795486285
+    x: 587558.7198438216
+    y: 4141502.85264403
+    theta: 2.9101827305207633
+    kappa: 0.0015797255002916004
     s: 10.58695559931081
-    dkappa: 2.0136123896980306e-05
+    dkappa: 1.8880344644524693e-05
     ddkappa: 0
   }
   v: 11.784492713210128
@@ -664,12 +664,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587558.49164240388
-    y: 4141502.9122342127
-    theta: 2.9091986580219742
-    kappa: 0.0014048410662178381
+    x: 587558.49033243686
+    y: 4141502.9067814862
+    theta: 2.9105549092266685
+    kappa: 0.0015824097986027181
     s: 10.822765585740655
-    dkappa: 1.5131231109083725e-05
+    dkappa: 1.2995967519154159e-05
     ddkappa: 0
   }
   v: 11.796497546817381
@@ -678,12 +678,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587558.26190573967
-    y: 4141502.9664602582
-    theta: 2.9095302862354098
-    kappa: 0.0014040223864874482
+    x: 587558.260518373
+    y: 4141502.960678563
+    theta: 2.9109279666441688
+    kappa: 0.0015803150695211502
     s: 11.058815154443352
-    dkappa: 1.0126633675464227e-05
+    dkappa: 7.1118988003438042e-06
     ddkappa: 0
   }
   v: 11.808450371851606
@@ -692,12 +692,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587558.031936946
-    y: 4141503.0207410948
-    theta: 2.9098622495903621
-    kappa: 0.0014032028795500748
+    x: 587558.03047210141
+    y: 4141503.0146300988
+    theta: 2.9113014010889797
+    kappa: 0.0015782182238920975
     s: 11.295103231373844
-    dkappa: 5.1169795172284495e-06
+    dkappa: 1.2218847251828241e-06
     ddkappa: 0
   }
   v: 11.820347813204755
@@ -706,12 +706,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587557.80173713306
-    y: 4141503.07507646
-    theta: 2.9101945464109984
-    kappa: 0.0014023825493623436
+    x: 587557.80019473273
+    y: 4141503.068635833
+    theta: 2.911675210654082
+    kappa: 0.0015761192718392486
     s: 11.531628675720013
-    dkappa: 1.0229282126828748e-07
+    dkappa: -4.6740462690097194e-06
     ddkappa: 0
   }
   v: 11.832186569278907
@@ -720,12 +720,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587556.64698896161
-    y: 4141503.346170722
-    theta: 2.9118454818066621
-    kappa: 0.0013766085140986908
+    x: 587556.64501986629
+    y: 4141503.3379058274
+    theta: 2.9135267406777174
+    kappa: 0.0015401342279737834
     s: 12.717771991813251
-    dkappa: -2.5037544517519688e-05
+    dkappa: -3.4227812640208675e-05
     ddkappa: 0
   }
   v: 11.89038966024629
@@ -734,12 +734,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587555.48620001064
-    y: 4141503.6165851797
-    theta: 2.9134526230099911
-    kappa: 0.0013192168390000611
+    x: 587555.483750714
+    y: 4141503.6062504016
+    theta: 2.9153192096444784
+    kappa: 0.0014669534110513479
     s: 13.909642553275882
-    dkappa: -5.0267777155537391e-05
+    dkappa: -6.3884990941986521e-05
     ddkappa: 0
   }
   v: 11.946672647872939
@@ -748,12 +748,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587554.31963648472
-    y: 4141503.8865282922
-    theta: 2.914986312328776
-    kappa: 0.0012331330445807024
+    x: 587554.31666358456
+    y: 4141503.8739204765
+    theta: 2.9170184517517694
+    kappa: 0.0013600228168583989
     s: 15.107031466866163
-    dkappa: -7.5592371099500336e-05
+    dkappa: -9.3649490073093147e-05
     ddkappa: 0
   }
   v: 12.000705531310203
@@ -762,12 +762,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587553.14752873837
-    y: 4141504.1559212231
-    theta: 2.91640415294548
-    kappa: 0.0011160524107763647
+    x: 587553.14399255475
+    y: 4141504.1408506059
+    theta: 2.9185741317729135
+    kappa: 0.0012166430133807543
     s: 16.309699136448913
-    dkappa: -0.0001010006994025328
+    dkappa: -0.00012350985169162787
     ddkappa: 0
   }
   v: 12.052204253538005
@@ -776,12 +776,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587551.97015308542
-    y: 4141504.4247687939
-    theta: 2.9176574121947803
-    kappa: 0.00096597421889188734
+    x: 587551.9660208167
+    y: 4141504.4070741651
+    theta: 2.9199285042752927
+    kappa: 0.0010344567876955227
     s: 17.517379857378369
-    dkappa: -0.00012649142139754675
+    dkappa: -0.00015346491836551891
     ddkappa: 0
   }
   v: 12.100930701364844
@@ -790,12 +790,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587550.78787697863
-    y: 4141504.6933812061
-    theta: 2.918731915942149
-    kappa: 0.00079734879611931871
+    x: 587550.78312873153
+    y: 4141504.6729608076
+    theta: 2.9210657689762827
+    kappa: 0.00083055905123812813
     s: 18.729786410881054
-    dkappa: -0.0001398904522762584
+    dkappa: -0.00016909178798257162
     ddkappa: 0
   }
   v: 12.146692705427798
@@ -804,12 +804,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587549.6010222194
-    y: 4141504.9617961878
-    theta: 2.9196058769106
-    kappa: 0.00063947374414959936
+    x: 587549.59564738546
+    y: 4141504.9385899976
+    theta: 2.9219600543039892
+    kappa: 0.00063970207355111021
     s: 19.946614658438623
-    dkappa: -0.00012767738158519211
+    dkappa: -0.00015436120269960143
     ddkappa: 0
   }
   v: 12.189344040192518
@@ -818,12 +818,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587548.40994572826
-    y: 4141505.230152783
-    theta: 2.920294123478719
-    kappa: 0.00050027224403061316
+    x: 587548.40394426463
+    y: 4141505.2041500611
+    theta: 2.9226291792054804
+    kappa: 0.00047130314189626417
     s: 21.16754813617073
-    dkappa: -0.00011251021854763931
+    dkappa: -0.00013612237661087857
     ddkappa: 0
   }
   v: 12.228784423953233
@@ -832,12 +832,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587547.21502708935
-    y: 4141505.4986579558
-    theta: 2.9208354439756987
-    kappa: 0.00037787408510446839
+    x: 587547.208407342
+    y: 4141505.4698887616
+    theta: 2.9231199728101718
+    kappa: 0.00032312042087313223
     s: 22.392262649217869
-    dkappa: -9.7384746284566765e-05
+    dkappa: -0.00011791921034281065
     ddkappa: 0
   }
   v: 12.264959518832747
@@ -846,12 +846,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587546.01660644775
-    y: 4141505.7673308076
-    theta: 2.9212321881475605
-    kappa: 0.00027465348819864224
+    x: 587546.0093867695
+    y: 4141505.7358714961
+    theta: 2.9234352461131459
+    kappa: 0.00019802022242275646
     s: 23.620430866124252
-    dkappa: -8.23014738740899e-05
+    dkappa: -9.9758870605648408e-05
     ddkappa: 0
   }
   v: 12.297860930782441
@@ -860,12 +860,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587544.81504258909
-    y: 4141506.0362830213
-    theta: 2.9215144101694142
-    kappa: 0.00019004650210577745
+    x: 587544.80724736687
+    y: 4141506.00223941
+    theta: 2.923611129093076
+    kappa: 9.5271965563521922e-05
     s: 24.85172734330332
-    dkappa: -6.6898818160757129e-05
+    dkappa: -8.1271340577321961e-05
     ddkappa: 0
   }
   v: 12.32754302233136
@@ -874,12 +874,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587543.61067344132
-    y: 4141506.3055823897
-    theta: 2.9217102247330189
-    kappa: 0.00012286041809412735
+    x: 587543.60233064159
+    y: 4141506.2690780973
+    theta: 2.9236815427448155
+    kappa: 1.3436692785817471e-05
     s: 26.085837225438226
-    dkappa: -5.1496550000637037e-05
+    dkappa: -6.277910973157691e-05
     ddkappa: 0
   }
   v: 12.354157728543241
@@ -888,12 +888,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587542.40380390012
-    y: 4141506.575225953
-    theta: 2.9218286074381723
-    kappa: 7.6616365499789939e-05
+    x: 587542.39495270688
+    y: 4141506.5364360344
+    theta: 2.9236571162596907
+    kappa: -4.325729363543e-05
     s: 27.322462368204448
-    dkappa: -3.6113930660288741e-05
+    dkappa: -4.4301590794582284e-05
     ddkappa: 0
   }
   v: 12.377876779761353
@@ -902,12 +902,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587541.19472948066
-    y: 4141506.8452474815
-    theta: 2.9219039418824129
-    kappa: 4.8218090318860834e-05
+    x: 587541.18540662643
+    y: 4141506.8043368664
+    theta: 2.9235793022802405
+    kappa: -7.850067975573885e-05
     s: 28.561321754159682
-    dkappa: -2.0738473695510328e-05
+    dkappa: -2.5826174148390048e-05
     ddkappa: 0
   }
   v: 12.39887098328685
@@ -916,12 +916,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587539.98371550115
-    y: 4141507.1156237968
-    theta: 2.9219563595077651
-    kappa: 3.8352646121578734e-05
+    x: 587539.97396134725
+    y: 4141507.0727740922
+    theta: 2.9234723195826997
+    kappa: -9.1479581686023157e-05
     s: 29.802151400439634
-    dkappa: -5.3743996604864915e-06
+    dkappa: -7.3573683407617926e-06
     ddkappa: 0
   }
   v: 12.41731022337877
@@ -930,12 +930,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587538.771005573
-    y: 4141507.3863122789
-    theta: 2.9220090154033755
-    kappa: 4.8939145363548848e-05
+    x: 587538.76086602232
+    y: 4141507.34173014
+    theta: 2.9233639313442166
+    kappa: -7.9853498631775988e-05
     s: 31.044704266453817
-    dkappa: 9.9638101644996921e-06
+    dkappa: 1.1089892018589019e-05
     ddkappa: 0
   }
   v: 12.433363461254034
@@ -944,12 +944,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587537.55682267575
-    y: 4141507.6572552253
-    theta: 2.9220841692726531
-    kappa: 7.715363178132105e-05
+    x: 587537.546337164
+    y: 4141507.6111180391
+    theta: 2.9232811162427979
+    kappa: -4.7020567553095714e-05
     s: 32.288750161581319
-    dkappa: 2.5286576301994792e-05
+    dkappa: 2.9525496525100613e-05
     ddkappa: 0
   }
   v: 12.447198735087449
@@ -958,12 +958,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587536.34136304283
-    y: 4141507.9283519909
-    theta: 2.922209391938178
-    kappa: 0.00012533576509937935
+    x: 587536.33057484869
+    y: 4141507.8808541535
+    theta: 2.923256886081099
+    kappa: 9.8331123260399443e-06
     s: 33.534075652866605
-    dkappa: 4.0583686246057925e-05
+    dkappa: 4.7938538708441178e-05
     ddkappa: 0
   }
   v: 12.458983160011705
@@ -972,12 +972,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587535.12480353843
-    y: 4141508.1994915577
-    theta: 2.9224065465406297
-    kappa: 0.00019260349864474728
+    x: 587535.11375132657
+    y: 4141508.1508064
+    theta: 2.9233177167986852
+    kappa: 8.9678712617057314e-05
     s: 34.780483972715309
-    dkappa: 5.5853972675153e-05
+    dkappa: 6.632704889325349e-05
     ddkappa: 0
   }
   v: 12.468882928117377
@@ -986,12 +986,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587533.90730294061
-    y: 4141508.4705577814
-    theta: 2.922691307382066
-    kappa: 0.00027747910191815514
+    x: 587533.89601891325
+    y: 4141508.4208293278
+    theta: 2.9234824973187892
+    kappa: 0.00019072850134283469
     s: 36.027794926590019
-    dkappa: 7.10941741584388e-05
+    dkappa: 8.4692781341052088e-05
     ddkappa: 0
   }
   v: 12.477063308452925
@@ -1000,12 +1000,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587532.68897621555
-    y: 4141508.7413131078
-    theta: 2.9231039051817862
-    kappa: 0.00037190763094440327
+    x: 587532.67749337561
+    y: 4141508.6906885169
+    theta: 2.9237988528137366
+    kappa: 0.00030182529134273523
     s: 37.275844800706039
-    dkappa: 7.5533131389952409e-05
+    dkappa: 8.870012968826806e-05
     ddkappa: 0
   }
   v: 12.483688647024692
@@ -1014,12 +1014,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587531.46995453932
-    y: 4141509.0116677275
-    theta: 2.9236245027035577
-    kappa: 0.00046446022230023056
+    x: 587531.45829480013
+    y: 4141508.9602442915
+    theta: 2.9242407019502648
+    kappa: 0.00040823169804576578
     s: 38.524486269727241
-    dkappa: 7.3862338153985369e-05
+    dkappa: 8.4579594085198358e-05
     ddkappa: 0
   }
   v: 12.488922366796908
@@ -1028,12 +1028,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587530.25032565231
-    y: 4141509.2814105311
-    theta: 2.9242600978912869
-    kappa: 0.00055491759429464782
+    x: 587530.2385085565
+    y: 4141509.2292746613
+    theta: 2.9248128793380257
+    kappa: 0.000509479367149746
     s: 39.773588304461782
-    dkappa: 7.2160109354147008e-05
+    dkappa: 8.0427502514782736e-05
     ddkappa: 0
   }
   v: 12.492926967691684
@@ -1042,12 +1042,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587529.03016535565
-    y: 4141509.5503500765
-    theta: 2.9250121262954352
-    kappa: 0.00064314459909516888
+    x: 587529.01820728183
+    y: 4141509.4975738265
+    theta: 2.9255135419927782
+    kappa: 0.00060530718257568051
     s: 41.023036079557933
-    dkappa: 7.0422639137455686e-05
+    dkappa: 7.6240509940467648e-05
     ddkappa: 0
   }
   v: 12.495864026589018
@@ -1056,12 +1056,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587527.8095648638
-    y: 4141509.8184385151
-    theta: 2.9258649159339152
-    kappa: 0.00072935696796762306
+    x: 587527.7974779848
+    y: 4141509.7650751676
+    theta: 2.9263221068383825
+    kappa: 0.00069629868352239193
     s: 42.27273088119987
-    dkappa: 6.8656346626496414e-05
+    dkappa: 7.2025262942430432e-05
     ddkappa: 0
   }
   v: 12.497894197326792
@@ -1070,12 +1070,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587526.58853926149
-    y: 4141510.0853544944
-    theta: 2.9268331057897981
-    kappa: 0.00081314549218191581
+    x: 587526.57633631246
+    y: 4141510.0314596505
+    theta: 2.9272502250825587
+    kappa: 0.00078154838526731823
     s: 43.522590014803441
-    dkappa: 6.6855670895459151e-05
+    dkappa: 6.77766829322955e-05
     ddkappa: 0
   }
   v: 12.49917721070077
@@ -1084,12 +1084,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587525.36715813377
-    y: 4141510.3510980885
-    theta: 2.9279006226356268
-    kappa: 0.00089474671988762919
+    x: 587525.35484964936
+    y: 4141510.2967177625
+    theta: 2.9282776976662972
+    kappa: 0.00086167097298165893
     s: 44.772546712711957
-    dkappa: 6.5024580309870476e-05
+    dkappa: 6.3498760899926407e-05
     ddkappa: 0
   }
   v: 12.499871874464603
@@ -1098,12 +1098,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587524.14544476918
-    y: 4141510.6155304965
-    theta: 2.9290655052629511
-    kappa: 0.00097410647594288957
+    x: 587524.13304197555
+    y: 4141510.5607141121
+    theta: 2.9293989251226553
+    kappa: 0.00093659905920329123
     s: 46.022550041892011
-    dkappa: 6.3163030486371015e-05
+    dkappa: 5.9191622659263969e-05
     ddkappa: 0
   }
   v: 12.500136073329825
@@ -1112,12 +1112,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587522.923374752
-    y: 4141510.8783640764
-    theta: 2.9303356527298714
-    kappa: 0.0010509176584469265
+    x: 587522.9108915166
+    y: 4141510.8231734382
+    theta: 2.93061727227138
+    kappa: 0.0010056787671511498
     s: 47.272564811629223
-    dkappa: 6.1267575689403939e-05
+    dkappa: 5.4852027546881764e-05
     ddkappa: 0
   }
   v: 12.500126768965858
@@ -1126,12 +1126,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587521.70099504269
-    y: 4141511.13971476
-    theta: 2.9316930518969486
-    kappa: 0.0011254281394883811
+    x: 587521.68844549742
+    y: 4141511.0842138454
+    theta: 2.9319123184297236
+    kappa: 0.0010696405758654614
     s: 48.522571481224055
-    dkappa: 5.924963838944e-05
+    dkappa: 5.0391025612150631e-05
     ddkappa: 0
   }
   v: 12.5
@@ -1140,12 +1140,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587520.47824224853
-    y: 4141511.3992526964
-    theta: 2.9331463721155555
-    kappa: 0.0011970629413149845
+    x: 587520.46564601758
+    y: 4141511.3435318307
+    theta: 2.9332873053093915
+    kappa: 0.001127528045392274
     s: 49.772565403395106
-    dkappa: 5.7037431143791969e-05
+    dkappa: 4.5737229965760816e-05
     ddkappa: 0
   }
   v: 12.499884320248489
@@ -1154,12 +1154,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587519.25511413871
-    y: 4141511.6569681033
-    theta: 2.9346872223154077
-    kappa: 0.0012660605022430348
+    x: 587519.2424939
+    y: 4141511.6011333847
+    theta: 2.9347308289067557
+    kappa: 0.0011797338621786951
     s: 51.022549440646507
-    dkappa: 5.49302874717163e-05
+    dkappa: 4.1189603313381703e-05
     ddkappa: 0
   }
   v: 12.499801510634402
@@ -1168,12 +1168,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587518.03161575063
-    y: 4141511.9128865916
-    theta: 2.9363067992988023
-    kappa: 0.0013325629614352394
+    x: 587518.01899727457
+    y: 4141511.8570604203
+    theta: 2.9362319533165624
+    kappa: 0.0012266118864451962
     s: 52.272526647177415
-    dkappa: 5.2778074859007409e-05
+    dkappa: 3.6597871202580362e-05
     ddkappa: 0
   }
   v: 12.499746959637891
@@ -1182,12 +1182,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587516.80764719332
-    y: 4141512.1665259711
-    theta: 2.9380171345194306
-    kappa: 0.0013959951854875313
+    x: 587516.79506804969
+    y: 4141512.1108896937
+    theta: 2.9377940831633595
+    kappa: 0.0012670146202029752
     s: 53.522499629466594
-    dkappa: 5.0568317544130823e-05
+    dkappa: 3.19495551430224e-05
     ddkappa: 0
   }
   v: 12.499716324371331
@@ -1196,12 +1196,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587515.58327394444
-    y: 4141512.4181941552
-    theta: 2.939798076030613
-    kappa: 0.001453652427227149
+    x: 587515.57077154436
+    y: 4141512.3629311835
+    theta: 2.9393990834188912
+    kappa: 0.0013004462940152629
     s: 54.772470573135678
-    dkappa: 4.478920131530007e-05
+    dkappa: 2.5522697345789451e-05
     ddkappa: 0
   }
   v: 12.499705530579327
@@ -1210,12 +1210,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587514.35844110628
-    y: 4141512.6676144754
-    theta: 2.9416422860764855
-    kappa: 0.0014951057230503544
+    x: 587514.34606299654
+    y: 4141512.612961404
+    theta: 2.941038705629786
+    kappa: 0.0013213480473005619
     s: 56.022441269812354
-    dkappa: 3.0996689528709472e-05
+    dkappa: 1.5124441504189705e-05
     ddkappa: 0
   }
   v: 12.4997107726387
@@ -1224,12 +1224,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587513.13312761835
-    y: 4141512.9146687957
-    theta: 2.9435279463630066
-    kappa: 0.001517400200563871
+    x: 587513.12092817319
+    y: 4141512.8609005203
+    theta: 2.9426963758439197
+    kappa: 0.001328118361036172
     s: 57.27241314399361
-    dkappa: 1.6186015738808979e-05
+    dkappa: 4.198104566258154e-06
     ddkappa: 0
   }
   v: 12.499728513558502
@@ -1238,12 +1238,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587511.90738237544
-    y: 4141513.1595836948
-    theta: 2.9454295214456545
-    kappa: 0.0015225728380893282
+    x: 587511.89541363
+    y: 4141513.1069675121
+    theta: 2.9443546038028261
+    kappa: 0.0013222438095581719
     s: 58.522387279908955
-    dkappa: 1.3374922985254134e-06
+    dkappa: -6.7694379501934392e-06
     ddkappa: 0
   }
   v: 12.499755484980003
@@ -1252,12 +1252,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587510.68112732575
-    y: 4141513.4019486662
-    theta: 2.9473242918171252
-    kappa: 0.0015074175147784079
+    x: 587510.66945327411
+    y: 4141513.3508191244
+    theta: 2.9459945981608762
+    kappa: 0.00130133245245905
     s: 59.772364448383627
-    dkappa: -1.3567774803987249e-05
+    dkappa: -1.7797457697334364e-05
     ddkappa: 0
   }
   v: 12.499788687176705
@@ -1266,12 +1266,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587509.45443601056
-    y: 4141513.6421140255
-    theta: 2.9491902538654267
-    kappa: 0.001474437722545048
+    x: 587509.4431134027
+    y: 4141513.5927730221
+    theta: 2.9476016913005734
+    kappa: 0.001267220546796869
     s: 61.022345133701833
-    dkappa: -2.851679924154958e-05
+    dkappa: -2.8872097618241348e-05
     ddkappa: 0
   }
   v: 12.499825389054324
@@ -1280,12 +1280,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587508.22731377545
-    y: 4141513.8800875577
-    theta: 2.9510049867038459
-    kappa: 0.0014232492997501747
+    x: 587508.21639963984
+    y: 4141513.8328436511
+    theta: 2.9491590356247079
+    kappa: 0.0012196014667489735
     s: 62.272329560469963
-    dkappa: -4.3507299166021825e-05
+    dkappa: -3.9990529252194257e-05
     ddkappa: 0
   }
   v: 12.49986312815081
@@ -1294,12 +1294,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587506.99973446759
-    y: 4141514.1157120615
-    theta: 2.9527377495029232
-    kappa: 0.001351857210999962
+    x: 587506.98928977665
+    y: 4141514.0709011909
+    theta: 2.9506426631910276
+    kappa: 0.0011569799567515851
     s: 63.522317720479805
-    dkappa: -5.8563832579538151e-05
+    dkappa: -5.1176646832271812e-05
     ddkappa: 0
   }
   v: 12.499899710636331
@@ -1308,12 +1308,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587505.77178667136
-    y: 4141514.3494274397
-    theta: 2.9543780387484189
-    kappa: 0.001263051999533955
+    x: 587505.76186068519
+    y: 4141514.3073258814
+    theta: 2.9520465253728232
+    kappa: 0.0010814096628658722
     s: 64.772309399571782
-    dkappa: -7.366865685848892e-05
+    dkappa: -6.2411511503662948e-05
     ddkappa: 0
   }
   v: 12.49993321131328
@@ -1322,12 +1322,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587504.54345020163
-    y: 4141514.581108314
-    theta: 2.9558881026783519
-    kappa: 0.001154305214739693
+    x: 587504.53409419581
+    y: 4141514.5420088405
+    theta: 2.9533409372043553
+    kappa: 0.00099098500714728754
     s: 66.022304204498184
-    dkappa: -8.8830766187381229e-05
+    dkappa: -7.37028547891026e-05
     ddkappa: 0
   }
   v: 12.499961973616276
@@ -1336,12 +1336,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587503.31478056137
-    y: 4141514.8110300526
-    theta: 2.9572515739592542
-    kappa: 0.0010266590432829316
+    x: 587503.306037489
+    y: 4141514.7751824073
+    theta: 2.9545145206584436
+    kappa: 0.00088647695659809726
     s: 67.272301589786309
-    dkappa: -0.00010405835734838387
+    dkappa: -8.5057488773529607e-05
     ddkappa: 0
   }
   v: 12.499984609612161
@@ -1350,12 +1350,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587502.08583421132
-    y: 4141515.0394788254
-    theta: 2.9584560155360515
-    kappa: 0.000881199521686564
+    x: 587502.07773807121
+    y: 4141515.0070844968
+    theta: 2.9555585388329169
+    kappa: 0.00076866579871772073
     s: 68.522300884601847
-    dkappa: -0.0001193437100765345
+    dkappa: -9.6466532658478531e-05
     ddkappa: 0
   }
   v: 12.5
@@ -1364,12 +1364,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587500.85660785856
-    y: 4141515.2664226741
-    theta: 2.9594432388569922
-    kappa: 0.000714377842948549
+    x: 587500.849190485
+    y: 4141515.2376746056
+    theta: 2.9564276775986906
+    kappa: 0.00063490356747331273
     s: 69.772301319611941
-    dkappa: -0.0001347061503306641
+    dkappa: -0.00010794682131947601
     ddkappa: 0
   }
   v: 12.500007294111086
@@ -1378,12 +1378,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587499.627196094
-    y: 4141515.4923616434
-    theta: 2.9602275648612362
-    kappa: 0.00052977014634024585
+    x: 587499.6204752078
+    y: 4141515.4673711378
+    theta: 2.9571347067303773
+    kappa: 0.0004878399084222517
     s: 71.022302053848477
-    dkappa: -0.00015013522696074165
+    dkappa: -0.00011948762564382572
     ddkappa: 0
   }
   v: 12.50000590990893
@@ -1392,12 +1392,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587498.39763482939
-    y: 4141515.717482497
-    theta: 2.9607659296800137
-    kappa: 0.00032760057024058923
+    x: 587498.3916199184
+    y: 4141515.6963143339
+    theta: 2.9576462493245721
+    kappa: 0.00032780942008016556
     s: 72.272302201571321
-    dkappa: -0.0001633471752301404
+    dkappa: -0.00012916642340282113
     ddkappa: 0
   }
   v: 12.499995533989273
@@ -1406,12 +1406,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587497.16797891643
-    y: 4141515.9420776195
-    theta: 2.96104425779941
-    kappa: 0.00013284288253344179
+    x: 587497.16266834538
+    y: 4141515.9247320676
+    theta: 2.9579532631358267
+    kappa: 0.00017565588448809673
     s: 73.522300859131519
-    dkappa: -0.00015348578259770788
+    dkappa: -0.00011963756816781615
     ddkappa: 0
   }
   v: 12.499976121580076
@@ -1420,12 +1420,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587495.93828896433
-    y: 4141516.166473031
-    theta: 2.9611114970172991
-    kappa: -3.8160355911975429e-05
+    x: 587495.93367104756
+    y: 4141516.1528906021
+    theta: 2.9580969112133553
+    kappa: 4.4766694390681549e-05
     s: 74.772297131834534
-    dkappa: -0.00013290582159970036
+    dkappa: -0.00010121618749869184
     ddkappa: 0
   }
   v: 12.499947896541528
@@ -1434,12 +1434,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587494.7086149822
-    y: 4141516.390937875
-    theta: 2.9609641528369317
-    kappa: -0.00018094542184697769
+    x: 587494.70466411067
+    y: 4141516.3809794486
+    theta: 2.9580786832594881
+    kappa: -6.0836074160797008e-05
     s: 76.022290160803436
-    dkappa: -0.00011226439793985883
+    dkappa: -8.2735231546448315e-05
     ddkappa: 0
   }
   v: 12.499911351366038
@@ -1448,12 +1448,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587493.47899243387
-    y: 4141516.6156617939
-    theta: 2.960661787590495
-    kappa: -0.00029907932496434759
+    x: 587493.4756765879
+    y: 4141516.6091507361
+    theta: 2.9579482306564877
+    kappa: -0.00014438722942748054
     s: 77.272279149842177
-    dkappa: -9.1519391216346769e-05
+    dkappa: -6.4156095621745047e-05
     ddkappa: 0
   }
   v: 12.499867247178241
@@ -1462,12 +1462,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587492.24945511122
-    y: 4141516.8408251586
-    theta: 2.9602309846646855
-    kappa: -0.00039181487651480191
+    x: 587492.24673266
+    y: 4141516.8375307242
+    theta: 2.957729794400398
+    kappa: -0.00020516059774036775
     s: 78.52226339229874
-    dkappa: -7.067505440205656e-05
+    dkappa: -4.5484244547715811e-05
     ddkappa: 0
   }
   v: 12.499816613734998
@@ -1476,12 +1476,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587491.020041489
-    y: 4141517.0666333912
-    theta: 2.9596945046263072
-    kappa: -0.00045651313860198533
+    x: 587491.01785698452
+    y: 4141517.0662485082
+    theta: 2.9574465881291845
+    kappa: -0.00024084092584196362
     s: 79.772242297928472
-    dkappa: -4.9707086201367784e-05
+    dkappa: -2.6697034044005942e-05
     ddkappa: 0
   }
   v: 12.49976074942539
@@ -1490,12 +1490,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587489.79075195279
-    y: 4141517.2930842075
-    theta: 2.95909745640684
-    kappa: -0.00049693797380102108
+    x: 587489.78904990572
+    y: 4141517.2953029755
+    theta: 2.9571348105911435
+    kappa: -0.00025477020766518315
     s: 81.022215419757174
-    dkappa: -2.8646100636631085e-05
+    dkappa: -7.8238228045582986e-06
     ddkappa: 0
   }
   v: 12.499701221270724
@@ -1504,12 +1504,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587488.5616162623
-    y: 4141517.5203352161
-    theta: 2.9584657640497896
-    kappa: -0.0005094230040266982
+    x: 587488.56032505806
+    y: 4141517.524765132
+    theta: 2.9568214559689943
+    kappa: -0.00024362922309571563
     s: 82.272182480944409
-    dkappa: -7.47935287647055e-06
+    dkappa: 1.114781458968583e-05
     ddkappa: 0
   }
   v: 12.499639864924534
@@ -1518,12 +1518,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587487.332629717
-    y: 4141517.7483577877
-    theta: 2.9578346878762853
-    kappa: -0.0004956319890348002
+    x: 587487.331675066
+    y: 4141517.7545943814
+    theta: 2.9565362186680497
+    kappa: -0.00020896156412030626
     s: 83.52214340164673
-    dkappa: 1.3785871562948876e-05
+    dkappa: 3.0212035306403046e-05
     ddkappa: 0
   }
   v: 12.499578784672575
@@ -1532,12 +1532,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587486.10378105857
-    y: 4141517.9770895569
-    theta: 2.9572341977895849
-    kappa: -0.00045683040106206347
+    x: 587486.10308687307
+    y: 4141517.9847212043
+    theta: 2.9563046422155144
+    kappa: -0.00015183789952804614
     s: 84.772098325880833
-    dkappa: 3.513581146254164e-05
+    dkappa: 4.9357024262289215e-05
     ddkappa: 0
   }
   v: 12.499520353432825
@@ -1546,12 +1546,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587484.87508248212
-    y: 4141518.2065957091
-    theta: 2.9567078990047029
-    kappa: -0.00038833927864277674
+    x: 587484.87455226947
+    y: 4141518.21510357
+    theta: 2.956170395622443
+    kappa: -6.8111870548404237e-05
     s: 86.022047648386888
-    dkappa: 5.6585104461166186e-05
+    dkappa: 6.8598565159598763e-05
     ddkappa: 0
   }
   v: 12.499467212755491
@@ -1560,12 +1560,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587483.64649085444
-    y: 4141518.4366469141
-    theta: 2.9562751767906632
-    kappa: -0.00029503729985846765
+    x: 587483.64603977662
+    y: 4141518.4455770836
+    theta: 2.9561457857472608
+    kappa: 3.7878859233599661e-05
     s: 87.271992041491615
-    dkappa: 7.8110387822229784e-05
+    dkappa: 8.7915771395568855e-05
     ddkappa: 0
   }
   v: 12.499422272822997
@@ -1574,12 +1574,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587482.41798784945
-    y: 4141518.6671495195
-    theta: 2.955979166877206
-    kappa: -0.0001742206886041695
+    x: 587482.41751827521
+    y: 4141518.675981123
+    theta: 2.9562724324836869
+    kappa: 0.00016860133224112938
     s: 88.521932481971646
-    dkappa: 9.9712894081946144e-05
+    dkappa: 0.00010731179495456117
     ddkappa: 0
   }
   v: 12.499388712449997
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_change_lane_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_change_lane_0.pb.txt
index 6d8cccc137..f5c00cadab 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_change_lane_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_change_lane_0.pb.txt
@@ -8,653 +8,653 @@ trajectory_point {
     x: 587581.22468020068
     y: 4140851.2261450971
     theta: 1.2657530776109578
-    kappa: 0.023188605879143422
-    s: 6.9388939039072284e-18
-    dkappa: -5.5323772364018e-20
+    kappa: 0.020526337150301513
+    s: 1.7347234759768071e-18
+    dkappa: -1.1625031140965822e-20
     ddkappa: 0
   }
   v: 0
-  a: 1.03973363383033
+  a: 1.0397336338303291
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 587581.22474170185
-    y: 4140851.2263443312
-    theta: 1.2657577033390828
-    kappa: 0.0231877737703794
-    s: 0.00020851042061718208
-    dkappa: -1.6624527202030058e-06
+    x: 587581.22474182921
+    y: 4140851.2263442916
+    theta: 1.2657571816115396
+    kappa: 0.02052563773482197
+    s: 0.00020851042061717674
+    dkappa: -1.3973063525445534e-06
     ddkappa: 0
   }
-  v: 0.020878992553343419
-  a: 1.0481188522975124
+  v: 0.020878992553343405
+  a: 1.0481188522975118
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 587581.22492685949
-    y: 4140851.2269441518
-    theta: 1.2657716297174786
-    kappa: 0.023185268594975793
-    s: 0.00083625909929147479
-    dkappa: -6.6674903359581369e-06
+    x: 587581.2249273702
+    y: 4140851.2269439935
+    theta: 1.2657695372585305
+    kappa: 0.020523532050406631
+    s: 0.00083625909929146915
+    dkappa: -5.6040851500584258e-06
     ddkappa: 0
   }
-  v: 0.041922897519522995
-  a: 1.0562255849080686
+  v: 0.04192289751952296
+  a: 1.0562255849080679
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 587581.22523662983
-    y: 4140851.2279476579
-    theta: 1.2657949286915933
-    kappa: 0.023181077411654077
-    s: 0.0018864888710259317
-    dkappa: -1.5040967957317154e-05
+    x: 587581.22523778223
+    y: 4140851.2279473017
+    theta: 1.2657902083819634
+    kappa: 0.020520009219474462
+    s: 0.001886488871025925
+    dkappa: -1.2642067843356401e-05
     ddkappa: 0
   }
-  v: 0.0631261877690536
-  a: 1.0640580904267405
+  v: 0.063126187769053557
+  a: 1.06405809042674
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 587581.22567193734
-    y: 4140851.2293578433
-    theta: 1.265827669776354
-    kappa: 0.023175187716882241
-    s: 0.0033623328799868473
-    dkappa: -2.6807866129744531e-05
+    x: 587581.22567399137
+    y: 4140851.2293572091
+    theta: 1.2658192566549313
+    kappa: 0.020515058732385107
+    s: 0.0033623328799868403
+    dkappa: -2.2532250803910206e-05
     ddkappa: 0
   }
-  v: 0.084483421347744989
-  a: 1.07162062761827
+  v: 0.084483421347744947
+  a: 1.0716206276182696
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 587581.226233674
-    y: 4140851.2311775992
-    theta: 1.2658699200939645
-    kappa: 0.02316758743807654
-    s: 0.0052668162830096579
-    dkappa: -4.1992304416162952e-05
+    x: 587581.22623689135
+    y: 4140851.2311766045
+    theta: 1.2658567416271207
+    kappa: 0.020508670441724774
+    s: 0.00526681628300965
+    dkappa: -3.529490673967891e-05
     ddkappa: 0
   }
-  v: 0.10598924147670175
-  a: 1.0789174552473992
+  v: 0.10598924147670169
+  a: 1.078917455247399
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 587581.22692270111
-    y: 4140851.2334097126
-    theta: 1.2659217444116744
-    kappa: 0.02315826492680326
-    s: 0.00760285795310484
-    dkappa: -6.0617554978998791e-05
+    x: 587581.2269273455
+    y: 4140851.2334082779
+    theta: 1.2659027207583238
+    kappa: 0.020500834556592077
+    s: 0.0076028579531048314
+    dkappa: -5.0949596110939476e-05
     ddkappa: 0
   }
-  v: 0.12763837655232324
-  a: 1.08595283207887
+  v: 0.12763837655232319
+  a: 1.0859528320788698
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 587581.22773984831
-    y: 4140851.236056874
-    theta: 1.2659832051795126
-    kappa: 0.023147208951980486
-    s: 0.010373272182963805
-    dkappa: -8.2706056162227555e-05
+    x: 587581.22774618515
+    y: 4140851.2360549164
+    theta: 1.2659572494519287
+    kappa: 0.020491541636883891
+    s: 0.010373272182963795
+    dkappa: -6.95151785461171e-05
     ddkappa: 0
   }
-  v: 0.1494256401463038
+  v: 0.14942564014630372
   a: 1.0927310168774245
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 587581.2286859157
-    y: 4140851.2391216746
-    theta: 1.2660543625679928
-    kappa: 0.02313440869307988
-    s: 0.013580770388464801
-    dkappa: -0.00010827942607341934
+    x: 587581.22869421181
+    y: 4140851.2391191111
+    theta: 1.2660203810883872
+    kappa: 0.020480782587581223
+    s: 0.013580770388464787
+    dkappa: -9.1009824257615824e-05
     ddkappa: 0
   }
-  v: 0.17134593100563242
+  v: 0.17134593100563233
   a: 1.0992562684078047
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 587581.22976167314
-    y: 4140851.2426066077
-    theta: 1.2661352745057808
-    kappa: 0.023119853733328446
-    s: 0.017227962812178803
-    dkappa: -0.00013735847616578437
+    x: 587581.2297721972
+    y: 4140851.2426033551
+    theta: 1.2660921670586587
+    kappa: 0.020468548653035049
+    s: 0.017227962812178789
+    dkappa: -0.0001154510254576489
     ddkappa: 0
   }
-  v: 0.19339423305259307
-  a: 1.1055328454347524
+  v: 0.19339423305259298
+  a: 1.1055328454347526
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 587581.230967861
-    y: 4140851.2465140731
-    theta: 1.2662259967173317
-    kappa: 0.023103534052910293
-    s: 0.021317360226875418
-    dkappa: -0.00016996322482021832
+    x: 587581.23098088312
+    y: 4140851.2465100489
+    theta: 1.266172656797629
+    kappa: 0.020454831411252195
+    s: 0.0213173602268754
+    dkappa: -0.00014285560777406925
     ddkappa: 0
   }
-  v: 0.21556561538476457
-  a: 1.1115650067230098
+  v: 0.21556561538476446
+  a: 1.11156500672301
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 587581.23230519111
-    y: 4140851.2508463757
-    theta: 1.2663265827604877
-    kappa: 0.0230854400221684
-    s: 0.025851375639028767
-    dkappa: -0.00020611291092734787
+    x: 587581.232320983
+    y: 4140851.2508414946
+    theta: 1.2662618978175066
+    kappa: 0.020439622768181169
+    s: 0.025851375639028749
+    dkappa: -0.00017323974166619994
     ddkappa: 0
   }
-  v: 0.23785523227502045
-  a: 1.117357011037319
+  v: 0.2378552322750204
+  a: 1.1173570110373192
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587581.233774347
-    y: 4140851.2556057293
-    theta: 1.2664370840640424
-    kappa: 0.023065562394806404
-    s: 0.030832325992323405
-    dkappa: -0.00024582600746957627
+    x: 587581.2337931816
+    y: 4140851.2555999085
+    theta: 1.2663599357411925
+    kappa: 0.020422914951998036
+    s: 0.030832325992323391
+    dkappa: -0.00020661895384066473
     ddkappa: 0
   }
-  v: 0.26025832317152925
-  a: 1.1229131171424216
+  v: 0.26025832317152919
+  a: 1.1229131171424218
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 587581.235375984
-    y: 4140851.2607942577
-    theta: 1.2665575499652679
-    kappa: 0.023043892301090337
-    s: 0.036262433871160189
-    dkappa: -0.0002891202351031284
+    x: 587581.23539813573
+    y: 4140851.2607874116
+    theta: 1.2664668143356232
+    kappa: 0.020404700507392261
+    s: 0.036262433871160175
+    dkappa: -0.00024300813866721829
     ddkappa: 0
   }
   v: 0.28277021269775415
-  a: 1.1282375838030598
+  a: 1.1282375838030603
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587581.23711073049
-    y: 4140851.2664139965
-    theta: 1.2666880277474031
-    kappa: 0.023020421241050418
-    s: 0.042143829204162229
-    dkappa: -0.000336012575740097
+    x: 587581.23713647493
+    y: 4140851.2664060406
+    theta: 1.2665825755450901
+    kappa: 0.020384972289852584
+    s: 0.042143829204162209
+    dkappa: -0.00028242156959457711
     ddkappa: 0
   }
   v: 0.30538631065245347
-  a: 1.1333346697839757
+  a: 1.1333346697839761
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587581.23897918779
-    y: 4140851.2724668942
-    theta: 1.2668285626771048
-    kappa: 0.022995141077682826
-    s: 0.048478550967680688
-    dkappa: -0.00038651928613048708
+    x: 587581.23900880211
+    y: 4140851.2724577421
+    theta: 1.2667072595245294
+    kappa: 0.020363723459952854
+    s: 0.04847855096768066
+    dkappa: -0.00032487291056624933
     ddkappa: 0
   }
   v: 0.32810211200968004
-  a: 1.1382086338499111
+  a: 1.1382086338499116
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 587581.24098193122
-    y: 4140851.2789548128
-    theta: 1.2669791980418619
-    kappa: 0.022968044030151437
-    s: 0.055268548889300766
-    dkappa: -0.00044065591144426227
+    x: 587581.24101569317
+    y: 4140851.2789443792
+    theta: 1.2668409046727886
+    kappa: 0.0203409474776379
+    s: 0.055268548889300745
+    dkappa: -0.0003703752274363659
     ddkappa: 0
   }
   v: 0.3509131969187817
-  a: 1.1428637347656083
+  a: 1.1428637347656088
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 587581.24311950966
-    y: 4140851.2858795323
-    theta: 1.2671399751873669
-    kappa: 0.022939122666989623
+    x: 587581.24315769877
+    y: 4140851.2858677306
+    theta: 1.2669835476658622
+    kappa: 0.020316638096509392
     s: 0.062515685151347561
-    dkappa: -0.00049843729885338986
+    dkappa: -0.00041894099938551041
     ddkappa: 0
   }
   v: 0.37381523070440109
-  a: 1.1473042312958088
+  a: 1.1473042312958095
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 587581.24539244734
-    y: 4140851.2932427507
-    theta: 1.2673109335548525
-    kappa: 0.022908369899302006
-    s: 0.070221736094392029
-    dkappa: -0.00055987761111388667
+    x: 587581.24543534382
+    y: 4140851.2932294933
+    theta: 1.2671352234901014
+    kappa: 0.02029078935811169
+    s: 0.070221736094391987
+    dkappa: -0.00047058213033654976
     ddkappa: 0
   }
   v: 0.39680396386647576
-  a: 1.1515343822052553
+  a: 1.1515343822052557
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587581.24780124321
-    y: 4140851.3010460832
-    theta: 1.2674921107183843
-    kappa: 0.022875778973966233
-    s: 0.0783883939207567
-    dkappa: -0.00062499034014786351
+    x: 587581.24784912844
+    y: 4140851.301031285
+    theta: 1.2672959654753981
+    kappa: 0.0202633955862177
+    s: 0.078388393920756666
+    dkappa: -0.00052530996037046529
     ddkappa: 0
   }
   v: 0.41987523208023797
-  a: 1.155558446258689
+  a: 1.1555584462586896
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587581.25034637214
-    y: 4140851.3092910689
-    theta: 1.2676835424221178
-    kappa: 0.022841343466834724
-    s: 0.087017268398021819
-    dkappa: -0.00069378832062557208
+    x: 587581.25039952865
+    y: 4140851.3092746413
+    theta: 1.2674658053283381
+    kappa: 0.020234451381114727
+    s: 0.087017268398021791
+    dkappa: -0.00058313527714218185
     ddkappa: 0
   }
   v: 0.44302495619621485
-  a: 1.1593806822208523
+  a: 1.159380682220853
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587581.25302828522
-    y: 4140851.3179791691
-    theta: 1.2678852626175112
-    kappa: 0.022805057275936486
-    s: 0.096109888562531026
-    dkappa: -0.00076628374354744885
+    x: 587581.25308699615
+    y: 4140851.3179610251
+    theta: 1.2676447731653266
+    kappa: 0.020203951613890343
+    s: 0.096109888562531012
+    dkappa: -0.00064406832729639954
     ddkappa: 0
   }
   v: 0.46624914224022862
-  a: 1.1630053488564873
+  a: 1.163005348856488
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 587581.25584741007
-    y: 4140851.327111769
-    theta: 1.2680973035005023
-    kappa: 0.022766914614678821
-    s: 0.1056677044228975
-    dkappa: -0.00084248816982616275
+    x: 587581.25591195957
+    y: 4140851.3270918205
+    theta: 1.2678328975456887
+    kappa: 0.020171891420718249
+    s: 0.10566770442289745
+    dkappa: -0.00070811882788342348
     ddkappa: 0
   }
-  v: 0.48954388141339578
-  a: 1.1664367049303359
+  v: 0.48954388141339589
+  a: 1.1664367049303366
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587581.25880415156
-    y: 4140851.33669018
-    theta: 1.2683196955486413
-    kappa: 0.022726910005049138
+    x: 587581.25887482474
+    y: 4140851.3366683396
+    theta: 1.2680302055047388
+    kappa: 0.0201382661971441
     s: 0.11569208866350947
-    dkappa: -0.0009224125438686572
+    dkappa: -0.00077529597777499493
     ddkappa: 0
   }
   v: 0.51290535009212845
-  a: 1.1696790092071399
+  a: 1.1696790092071405
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587581.26189889223
-    y: 4140851.3467156421
-    theta: 1.2685524675581867
-    kappa: 0.022685038270816707
+    x: 587581.26197597478
+    y: 4140851.3466918203
+    theta: 1.2682367225868216
+    kappa: 0.020103071592371402
     s: 0.12618433834803661
-    dkappa: -0.0010060672071582
+    dkappa: -0.00084560846908012064
     ddkappa: 0
   }
-  v: 0.53632980982813283
-  a: 1.1727365204516416
+  v: 0.53632980982813294
+  a: 1.1727365204516422
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587581.26513199275
-    y: 4140851.3571893228
-    theta: 1.2687956466811581
-    kappa: 0.022641294530734428
-    s: 0.13714567662293542
-    dkappa: -0.0010934619118364239
+    x: 587581.26521577139
+    y: 4140851.3571634321
+    theta: 1.2684524728783286
+    kappa: 0.020066303503547347
+    s: 0.13714567662293539
+    dkappa: -0.00091906449856090386
     ddkappa: 0
   }
   v: 0.55981360734841035
-  a: 1.1756134974285826
+  a: 1.1756134974285832
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587581.268503793
-    y: 4140851.3681123229
-    theta: 1.2690492584623507
-    kappa: 0.022595674191740592
+    x: 587581.26859455474
+    y: 4140851.3680842742
+    theta: 1.2686774790406827
+    kappa: 0.02002795807004866
     s: 0.1485772544209556
-    dkappa: -0.0011846058342853763
+    dkappa: -0.00099567177904837621
     ddkappa: 0
   }
   v: 0.58335317455525737
-  a: 1.1783141989027053
+  a: 1.1783141989027059
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587581.27201461117
-    y: 4140851.3794856751
-    theta: 1.2693133268763102
-    kappa: 0.022548172942160674
-    s: 0.16048015216464562
-    dkappa: -0.0012795075887095629
+    x: 587581.27211264416
+    y: 4140851.3794553787
+    theta: 1.2689117623432971
+    kappa: 0.019988031667767484
+    s: 0.16048015216464559
+    dkappa: -0.0010754375508583256
     ddkappa: 0
   }
-  v: 0.60694502852626475
-  a: 1.1808428836387515
+  v: 0.60694502852626486
+  a: 1.1808428836387521
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587581.27566474606
-    y: 4140851.3913103449
-    theta: 1.2695878743642661
-    kappa: 0.022498786744909054
-    s: 0.17285538146985871
-    dkappa: -0.001378175240717993
+    x: 587581.2757703386
+    y: 4140851.3912777128
+    theta: 1.269155342696505
+    kappa: 0.019946520903397214
+    s: 0.17285538146985865
+    dkappa: -0.0011583685932071281
     ddkappa: 0
   }
-  v: 0.63058577151431849
-  a: 1.1832038104014633
+  v: 0.63058577151431872
+  a: 1.1832038104014639
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587581.279454476
-    y: 4140851.4035872356
-    theta: 1.2698729218710276
-    kappa: 0.022447511830690846
+    x: 587581.27956791723
+    y: 4140851.4035521774
+    theta: 1.2694082386844638
+    kappa: 0.019903422608718357
     s: 0.18570388684925873
-    dkappa: -0.0014806163209062238
+    dkappa: -0.0012444712356275791
     ddkappa: 0
   }
-  v: 0.65427209094759942
-  a: 1.1854012379555825
+  v: 0.65427209094759953
+  a: 1.1854012379555829
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587581.28338406014
-    y: 4140851.4163171863
-    theta: 1.2701684888818379
-    kappa: 0.022394344691203605
+    x: 587581.28350564
+    y: 4140851.4162796135
+    theta: 1.26967046759803
+    kappa: 0.019858733834884412
     s: 0.19902654741582618
-    dkappa: -0.0015868378384384092
+    dkappa: -0.0013337513693847214
     ddkappa: 0
   }
-  v: 0.67800075942958282
-  a: 1.1874394250658511
+  v: 0.67800075942958293
+  a: 1.1874394250658518
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587581.28745373932
-    y: 4140851.4295009766
-    theta: 1.2704745934591914
-    kappa: 0.022339282072339144
+    x: 587581.28758374788
+    y: 4140851.4294607993
+    theta: 1.2699420454676069
+    kappa: 0.019812451846707688
     s: 0.21282417858636404
-    dkappa: -0.0016968462946293432
+    dkappa: -0.0014262144588916784
     ddkappa: 0
   }
-  v: 0.70176863473903939
-  a: 1.1893226304970113
+  v: 0.7017686347390395
+  a: 1.1893226304970119
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587581.29166373576
-    y: 4140851.4431393272
-    theta: 1.2707912522796121
-    kappa: 0.022282320967385281
+    x: 587581.29180246335
+    y: 4140851.4430964552
+    theta: 1.2702229870959663
+    kappa: 0.01976457411694519
     s: 0.22709753378500352
-    dkappa: -0.0018106476965265037
+    dkappa: -0.001521865553125483
     ddkappa: 0
   }
-  v: 0.72557265983003416
-  a: 1.1910551130138052
+  v: 0.72557265983003449
+  a: 1.1910551130138056
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587581.29601425363
-    y: 4140851.457232899
-    theta: 1.2711184806703921
-    kappa: 0.022223458610227596
+    x: 587581.29616199154
+    y: 4140851.4571872423
+    theta: 1.2705133060910432
+    kappa: 0.019715098320584469
     s: 0.24184730614671024
-    dkappa: -0.0019282475704921016
+    dkappa: -0.0016207092970429082
     ddkappa: 0
   }
-  v: 0.74940986283192745
-  a: 1.1926411313809742
+  v: 0.74940986283192756
+  a: 1.1926411313809748
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587581.30050548026
-    y: 4140851.4717822997
-    theta: 1.2714562926462942
-    kappa: 0.022162692468551223
+    x: 587581.30066251988
+    y: 4140851.4717337685
+    theta: 1.2708130148987038
+    kappa: 0.01966402232912947
     s: 0.2570741302207899
-    dkappa: -0.0020496509757851242
+    dkappa: -0.0017227499429962987
     ddkappa: 0
   }
-  v: 0.773277357049374
-  a: 1.1940849443632611
+  v: 0.77327735704937428
+  a: 1.1940849443632615
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587581.30513758608
-    y: 4140851.4867880805
-    theta: 1.2718047009462159
-    kappa: 0.022100020237042606
+    x: 587581.305304219
+    y: 4140851.4867365845
+    theta: 1.2711221248354869
+    kappa: 0.019611344204886402
     s: 0.27277858367439412
-    dkappa: -0.002174862518143379
+    dkappa: -0.0018279913621493983
     ddkappa: 0
   }
-  v: 0.7971723409623237
-  a: 1.1953908107254072
+  v: 0.79717234096232392
+  a: 1.1953908107254076
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587581.30991072522
-    y: 4140851.5022507408
-    theta: 1.2721637170698157
-    kappa: 0.022035439830591266
+    x: 587581.31008724368
+    y: 4140851.50219619
+    theta: 1.2714406461213204
+    kappa: 0.019557062195249587
     s: 0.28896118899602669
-    dkappa: -0.0023038863633655436
+    dkappa: -0.0019364370558931851
     ddkappa: 0
   }
-  v: 0.821092098226021
-  a: 1.1965629892321548
+  v: 0.82109209822602114
+  a: 1.1965629892321552
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 587581.31482503586
-    y: 4140851.5181707283
-    theta: 1.2725333513141053
-    kappa: 0.021968949377491567
+    x: 587581.31501173228
+    y: 4140851.518113032
+    theta: 1.2717685879122107
+    kappa: 0.019501174726987314
     s: 0.30562241519904904
-    dkappa: -0.0024367262508932057
+    dkappa: -0.0020480901672616969
     ddkappa: 0
   }
-  v: 0.84503399767100551
-  a: 1.1976057386482457
+  v: 0.84503399767100573
+  a: 1.1976057386482462
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 587581.31988064118
-    y: 4140851.5345484428
-    theta: 1.2729136128100029
-    kappa: 0.0219005472126445
-    s: 0.3227626795251865
-    dkappa: -0.0025733855073929155
+    x: 587581.320077808
+    y: 4140851.53448751
+    theta: 1.2721059583329097
+    kappa: 0.0194436804005277
+    s: 0.32276267952518656
+    dkappa: -0.0021629534923478669
     ddkappa: 0
   }
-  v: 0.86899549330311143
-  a: 1.1985233177384222
+  v: 0.86899549330311177
+  a: 1.1985233177384227
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 587581.32507764944
-    y: 4140851.5513842339
-    theta: 1.2733045095588518
-    kappa: 0.021830231870759425
+    x: 587581.32528557966
+    y: 4140851.5513199754
+    theta: 1.2724527645095534
+    kappa: 0.019384577984244557
     s: 0.34038234914803406
-    dkappa: -0.002713867060338224
+    dkappa: -0.0022810294917193484
     ddkappa: 0
   }
-  v: 0.892974124303468
-  a: 1.1993199852674263
+  v: 0.89297412430346834
+  a: 1.1993199852674266
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 587581.3304161547
-    y: 4140851.5686784061
-    theta: 1.2737060484689049
-    kappa: 0.02175800207955586
-    s: 0.35848174287656193
-    dkappa: -0.0028581734515917309
+    x: 587581.33063514135
+    y: 4140851.568610731
+    theta: 1.2728090126022777
+    kappa: 0.019323866408743229
+    s: 0.358481742876562
+    dkappa: -0.00240232030183435
     ddkappa: 0
   }
-  v: 0.916967515028499
-  a: 1.1999999999999997
+  v: 0.91696751502849916
+  a: 1.2
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 587581.33589623717
-    y: 4140851.5864312211
-    theta: 1.2741182353917733
-    kappa: 0.021683856752965251
+    x: 587581.33612657338
+    y: 4140851.5863600383
+    theta: 1.2731747078378113
+    kappa: 0.019261544761146457
     s: 0.37706113285862214
-    dkappa: -0.0030063068509871334
+    dkappa: -0.0025268277464574637
     ddkappa: 0
   }
-  v: 0.9409733750099234
-  a: 1.2005676207008846
+  v: 0.94097337500992373
+  a: 1.2005676207008849
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 587581.34151796391
-    y: 4140851.6046428946
-    theta: 1.2745410751588406
-    kappa: 0.021607794984332716
-    s: 0.39612074628445376
-    dkappa: -0.0031582690699112678
+    x: 587581.34175994317
+    y: 4140851.6045681136
+    theta: 1.2735498545420409
+    kappa: 0.019197612279380251
+    s: 0.39612074628445382
+    dkappa: -0.0026545533480754968
     ddkappa: 0
   }
-  v: 0.96498949895475483
-  a: 1.2010271061348228
+  v: 0.964989498954755
+  a: 1.201027106134823
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 587581.34728138929
-    y: 4140851.6233136025
-    theta: 1.2749745716176446
-    kappa: 0.02152981603961884
+    x: 587581.347535305
+    y: 4140851.623235133
+    theta: 1.2739344561725581
+    kappa: 0.019132068346459721
     s: 0.41566076709018934
-    dkappa: -0.0033140615748861561
+    dkappa: -0.0027854983393133009
     ddkappa: 0
   }
-  v: 0.98901376674530161
+  v: 0.98901376674530206
   a: 1.2013827150665566
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 587581.353186555
-    y: 4140851.6424434814
-    theta: 1.2754187276682254
-    kappa: 0.021449919350601431
+    x: 587581.35345270077
+    y: 4140851.6423612321
+    theta: 1.2743285153511774
+    kappa: 0.019064912484774942
     s: 0.43568133766136019
-    dkappa: -0.003473685501151051
+    dkappa: -0.0029196636743496016
     ddkappa: 0
   }
-  v: 1.0130441434391673
-  a: 1.2016387062608276
+  v: 1.0130441434391675
+  a: 1.2016387062608278
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 587581.35923349159
-    y: 4140851.6620326289
-    theta: 1.27587354529944
-    kappa: 0.021368104508077284
-    s: 0.45618256053640294
-    dkappa: -0.0036371416662444819
+    x: 587581.359512161
+    y: 4140851.661946509
+    theta: 1.2747320338964361
+    kappa: 0.018996144350376831
+    s: 0.45618256053640305
+    dkappa: -0.0030570500403328338
     ddkappa: 0
   }
-  v: 1.0370786792692501
+  v: 1.0370786792692503
   a: 1.2017993384823782
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 587581.365422218
-    y: 4140851.6820811061
-    theta: 1.2763390256252454
-    kappa: 0.021284371255063959
-    s: 0.47716450011016504
-    dkappa: -0.0038044305835863023
+    x: 587581.36571370461
+    y: 4140851.6819910258
+    theta: 1.2751450128560693
+    kappa: 0.018925763727262977
+    s: 0.47716450011016515
+    dkappa: -0.0031976578687969645
     ddkappa: 0
   }
   v: 1.0611155096437428
@@ -663,278 +663,278 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587581.37175274233
-    y: 4140851.702588941
-    theta: 1.2768151689209504
-    kappa: 0.021198719480001543
+    x: 587581.37205733988
+    y: 4140851.7024948089
+    theta: 1.2755674525394625
+    kappa: 0.018853770521663513
     s: 0.49862718433741088
-    dkappa: -0.0039755524760597318
+    dkappa: -0.0033414873470773306
     ddkappa: 0
   }
   v: 1.0851528551461334
-  a: 1.2018515610662854
+  a: 1.2018515610662852
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 587581.37802162138
-    y: 4140851.723617746
-    theta: 1.2772625637811024
-    kappa: 0.021113608292936768
+    x: 587581.37836231955
+    y: 4140851.7235128391
+    theta: 1.275966247955211
+    kappa: 0.018782221601266882
     s: 0.52057060643632713
-    dkappa: -0.00398581483596525
+    dkappa: -0.0033502322450281014
     ddkappa: 0
   }
-  v: 1.1091890215352043
-  a: 1.2017516689581262
+  v: 1.1091890215352045
+  a: 1.201751668958126
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 587581.38440807967
-    y: 4140851.7451131945
-    theta: 1.2777159353862233
-    kappa: 0.021026871346569782
-    s: 0.54299472659202963
-    dkappa: -0.0039803150196024905
+    x: 587581.38478772319
+    y: 4140851.7449966776
+    theta: 1.2763705398387089
+    kappa: 0.018709306671303542
+    s: 0.54299472659202974
+    dkappa: -0.0033456289562269412
     ddkappa: 0
   }
-  v: 1.1332223997450332
+  v: 1.1332223997450335
   a: 1.2015734529362141
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 587581.39093142212
-    y: 4140851.7670693658
-    theta: 1.2781790261030153
-    kappa: 0.020938275325669289
-    s: 0.565899473660068
-    dkappa: -0.0039746973230402631
+    x: 587581.39135084569
+    y: 4140851.7669409909
+    theta: 1.2767834983668735
+    kappa: 0.018634828920707419
+    s: 0.5658994736600681
+    dkappa: -0.0033409270029311332
     ddkappa: 0
   }
-  v: 1.1572514658849926
-  a: 1.2013211717652916
+  v: 1.1572514658849928
+  a: 1.2013211717652914
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587581.42560012161
-    y: 4140851.8837567708
-    theta: 1.2806401719617133
-    kappa: 0.020467426387348282
-    s: 0.68762814420171392
-    dkappa: -0.0039448417331926656
+    x: 587581.42623095878
+    y: 4140851.8835653747
+    theta: 1.2789782072378575
+    kappa: 0.018239012388087873
+    s: 0.68762814420171414
+    dkappa: -0.0033159381863357156
     ddkappa: 0
   }
   v: 1.2772847416354181
-  a: 1.1990978454414893
+  a: 1.1990978454414889
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587581.46368360077
-    y: 4140852.0119375912
-    theta: 1.2833437511700929
-    kappa: 0.019950200013269984
+    x: 587581.46454667533
+    y: 4140852.0116769671
+    theta: 1.2813891012413865
+    kappa: 0.017804208922889281
     s: 0.82134678251942994
-    dkappa: -0.0039120454428624666
+    dkappa: -0.0032884880341781169
     ddkappa: 0
   }
   v: 1.3970306078394918
-  a: 1.1956617051037206
+  a: 1.1956617051037197
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587581.50517219992
-    y: 4140852.1515793148
-    theta: 1.2862890376760503
-    kappa: 0.019386727398095424
-    s: 0.9670214708311653
-    dkappa: -0.0038763167708408342
+    x: 587581.50628827652
+    y: 4140852.1512432722
+    theta: 1.2840155402859856
+    kappa: 0.017330528813171351
+    s: 0.96702147083116541
+    dkappa: -0.0032585835091995816
     ddkappa: 0
   }
   v: 1.516394400375455
-  a: 1.1915450963447345
+  a: 1.1915450963447334
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587581.5489094269
-    y: 4140852.302977067
-    theta: 1.2892397825558759
-    kappa: 0.018793528393750633
-    s: 1.1246114866706576
-    dkappa: -0.0038353067456842675
+    x: 587581.5504212497
+    y: 4140852.302526393
+    theta: 1.2866588897287066
+    kappa: 0.01683161666158884
+    s: 1.1246114866706578
+    dkappa: -0.0032246165601259153
     ddkappa: 0
   }
   v: 1.6353346896808232
-  a: 1.1872803647572803
+  a: 1.1872803647572789
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587581.5956053884
-    y: 4140852.4658796238
-    theta: 1.2923436847028571
-    kappa: 0.018160445928506602
-    s: 1.2940746263433596
-    dkappa: -0.0037905135991374312
+    x: 587581.59757810761
+    y: 4140852.4652961269
+    theta: 1.289443150898506
+    kappa: 0.01629909419111435
+    s: 1.29407462634336
+    dkappa: -0.0031876145279667754
     ddkappa: 0
   }
   v: 1.7538632807523873
-  a: 1.183399855934107
+  a: 1.1833998559341055
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587581.64556244609
-    y: 4140852.640158772
-    theta: 1.2956643350266568
-    kappa: 0.017483151013588213
+    x: 587581.64802824927
+    y: 4140852.6394331781
+    theta: 1.292421842681506
+    kappa: 0.015729382058599164
     s: 1.4753725283823673
-    dkappa: -0.0037425922428451319
+    dkappa: -0.0031480283925122542
     ddkappa: 0
   }
-  v: 1.8720452131462126
-  a: 1.180435915467964
+  v: 1.8720452131462122
+  a: 1.1804359154679624
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 587581.69737058517
-    y: 4140852.8261817312
-    theta: 1.2988941303324051
-    kappa: 0.016785109504272137
+    x: 587581.70050354814
+    y: 4140852.8252692036
+    theta: 1.2953361849276792
+    kappa: 0.015141701670618613
     s: 1.6684759970043457
-    dkappa: -0.0036891063265109686
+    dkappa: -0.003104158521061876
     ddkappa: 0
   }
-  v: 1.9899987609776395
-  a: 1.1789208889516005
+  v: 1.9899987609776391
+  a: 1.178920888951599
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587581.75211425219
-    y: 4140853.0236274628
-    theta: 1.3022712554359197
-    kappa: 0.016048241977468708
+    x: 587581.755977393
+    y: 4140853.022511031
+    theta: 1.2983863519943895
+    kappa: 0.014521258878511733
     s: 1.8733703255654588
-    dkappa: -0.0036319573277582935
+    dkappa: -0.0030573316072451404
     ddkappa: 0
   }
-  v: 2.1078954329212842
-  a: 1.1793871219777659
+  v: 2.1078954329212833
+  a: 1.1793871219777641
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587581.80934037059
-    y: 4140853.2326230705
-    theta: 1.3056860037131131
-    kappa: 0.015281842804180963
+    x: 587581.81403966784
+    y: 4140853.231276182
+    theta: 1.3014801853546025
+    kappa: 0.013875594558546731
     s: 2.090060620017296
-    dkappa: -0.0035705501203064788
+    dkappa: -0.0030071175980684118
     ddkappa: 0
   }
-  v: 2.2259599722110357
-  a: 1.1823669601392088
+  v: 2.2259599722110348
+  a: 1.1823669601392073
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587581.86867631972
-    y: 4140853.4533016533
-    theta: 1.3090497441438689
-    kappa: 0.014493136565102212
-    s: 2.318577122362798
-    dkappa: -0.0035043255316955639
+    x: 587581.87435211416
+    y: 4140853.4516898855
+    theta: 1.3045426707838239
+    kappa: 0.013210626155738888
+    s: 2.3185771223627989
+    dkappa: -0.0029531139261616377
     ddkappa: 0
   }
-  v: 2.3444703566400595
-  a: 1.1883927490286785
+  v: 2.34447035664006
+  a: 1.1883927490286776
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587581.93071040872
-    y: 4140853.6855597664
-    theta: 1.3124907254482574
-    kappa: 0.013672021067721633
-    s: 2.5589779692259804
-    dkappa: -0.0034342024006293491
+    x: 587581.937447953
+    y: 4140853.6836619917
+    theta: 1.3076822018428615
+    kappa: 0.012518059599309661
+    s: 2.5589779692259813
+    dkappa: -0.002895967665306259
     ddkappa: 0
   }
-  v: 2.4636548026981004
-  a: 1.1948869376386777
+  v: 2.4636548026981009
+  a: 1.1948869376386773
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587581.99454276881
-    y: 4140853.9297006833
-    theta: 1.3157791082618897
-    kappa: 0.012838621069271944
-    s: 2.8113256608965518
-    dkappa: -0.0033590888756342319
+    x: 587582.00250509719
+    y: 4140853.9274793938
+    theta: 1.3107044815863913
+    kappa: 0.011814293794307013
+    s: 2.8113256608965527
+    dkappa: -0.002834871041925504
     ddkappa: 0
   }
-  v: 2.5833667614079019
-  a: 1.1989516434518865
+  v: 2.5833667614079023
+  a: 1.1989516434518863
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587582.060966683
-    y: 4140854.1855530781
-    theta: 1.3191034628759553
-    kappa: 0.011976923707856513
-    s: 3.075660887639585
-    dkappa: -0.0032800017907938456
+    x: 587582.070246896
+    y: 4140854.1829861663
+    theta: 1.3137689234468151
+    kappa: 0.01108625242291229
+    s: 3.0756608876395863
+    dkappa: -0.0027705616951361344
     ddkappa: 0
   }
-  v: 2.7033658769061857
+  v: 2.7033658769061861
   a: 1.2006387147963216
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587582.12923334364
-    y: 4140854.4533277582
-    theta: 1.3222586010058786
-    kappa: 0.011106168427613687
-    s: 3.3520005633754644
-    dkappa: -0.0031962434734085932
+    x: 587582.13998104993
+    y: 4140854.4503824338
+    theta: 1.3167016797616742
+    kappa: 0.010349606327353482
+    s: 3.3520005633754657
+    dkappa: -0.0027025027006911204
     ddkappa: 0
   }
-  v: 2.8234169781624745
+  v: 2.8234169781624749
   a: 1.2000000000000002
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 587582.19974586973
-    y: 4140854.7329097176
-    theta: 1.3253367630636261
-    kappa: 0.010218947358780331
-    s: 3.6403383441631663
-    dkappa: -0.0031083537302473725
+    x: 587582.21207280469
+    y: 4140854.7295614816
+    theta: 1.3195807898163812
+    kappa: 0.0095982969039250347
+    s: 3.6403383441631672
+    dkappa: -0.0026310871069543376
     ddkappa: 0
   }
-  v: 2.9432900789790946
+  v: 2.943290078979095
   a: 1.1970873473909387
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587582.27238378359
-    y: 4140855.0242993343
-    theta: 1.3283038808308356
-    kappa: 0.00931871885246295
-    s: 3.9406451466835404
-    dkappa: -0.0030162633148844752
+    x: 587582.2864074572
+    y: 4140855.020522879
+    theta: 1.322376964479427
+    kappa: 0.0088351766925616367
+    s: 3.9406451466835417
+    dkappa: -0.0025562577310173271
     ddkappa: 0
   }
   v: 3.0627603779911721
@@ -943,12 +943,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587582.34678719193
-    y: 4140855.3275285633
-    theta: 1.3310211941969465
-    kappa: 0.0084216251249429521
-    s: 4.2528696667225887
-    dkappa: -0.0029199835662826034
+    x: 587582.36263305275
+    y: 4140855.3232992347
+    theta: 1.3249723836662681
+    kappa: 0.0080733492383099244
+    s: 4.25286966672259
+    dkappa: -0.002477983783533829
     ddkappa: 0
   }
   v: 3.1816082586666354
@@ -957,12 +957,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587582.42345020373
-    y: 4140855.6423989632
-    theta: 1.333645125530206
-    kappa: 0.0075119310019868017
+    x: 587582.44121091662
+    y: 4140855.6376972673
+    theta: 1.3274992916562967
+    kappa: 0.0073000597865210386
     s: 4.5769388976547472
-    dkappa: -0.0028198364043490924
+    dkappa: -0.0023965360772991149
     ddkappa: 0
   }
   v: 3.2996192893062153
@@ -971,362 +971,362 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587582.50192708278
-    y: 4140855.9689204553
-    theta: 1.3360082096302832
-    kappa: 0.0066091344054248708
+    x: 587582.52170359157
+    y: 4140855.9637276875
+    theta: 1.3298149114048519
+    kappa: 0.0065312040290124093
     s: 4.9127586489261628
-    dkappa: -0.0027157812669478874
+    dkappa: -0.0023118442666277442
     ddkappa: 0
   }
-  v: 3.4165842230434427
-  a: 1.1637343253876296
+  v: 3.4165842230434436
+  a: 1.1637343253876293
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 587582.58231998514
-    y: 4140856.3069471018
-    theta: 1.3381151603595365
-    kappa: 0.005715218127802053
-    s: 5.2602140645379762
-    dkappa: -0.0026080329413720937
+    x: 587582.60418915
+    y: 4140856.3012497881
+    theta: 1.3319226335792369
+    kappa: 0.0057684223420319935
+    s: 5.260214064537978
+    dkappa: -0.0022240696262790156
     ddkappa: 0
   }
-  v: 3.5322989978446526
-  a: 1.1502297086351205
+  v: 3.5322989978446531
+  a: 1.1502297086351203
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587582.6647971411
-    y: 4140856.6562990709
-    theta: 1.3400277941671597
-    kappa: 0.0048241643633857257
-    s: 5.6191701415296054
-    dkappa: -0.0024967040584019653
+    x: 587582.68882283755
+    y: 4140856.6500855782
+    theta: 1.3338745696187686
+    kappa: 0.0050068576605462461
+    s: 5.6191701415296063
+    dkappa: -0.0021333156008795152
     ddkappa: 0
   }
   v: 3.6465647365089797
-  a: 1.1347622440379725
+  a: 1.1347622440379721
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587582.74926219892
-    y: 4140857.0168393664
-    theta: 1.3416952580040384
-    kappa: 0.0039435348952310319
-    s: 5.9894722484620164
-    dkappa: -0.0023818920146317143
+    x: 587582.775496439
+    y: 4140857.0101013239
+    theta: 1.3356268718460997
+    kappa: 0.0042527022200912739
+    s: 5.9894722484620182
+    dkappa: -0.0020396473775932231
     ddkappa: 0
   }
-  v: 3.75918774666836
-  a: 1.1173837799242021
+  v: 3.7591877466683603
+  a: 1.1173837799242015
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587582.835570215
-    y: 4140857.3884219592
-    theta: 1.342988006131006
-    kappa: 0.0030935363230115195
-    s: 6.3709466439010116
-    dkappa: -0.0022637740912361819
+    x: 587582.86402455939
+    y: 4140857.3811612464
+    theta: 1.3370674917693446
+    kappa: 0.0035225098458638903
+    s: 6.3709466439010134
+    dkappa: -0.0019431801138204577
     ddkappa: 0
   }
-  v: 3.8699795207875329
-  a: 1.0981461646218262
+  v: 3.8699795207875334
+  a: 1.0981461646218256
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587582.92398048914
-    y: 4140857.7707882412
-    theta: 1.3440227783972842
-    kappa: 0.0022614501482291884
-    s: 6.7634009949005094
-    dkappa: -0.0021424084343657323
+    x: 587582.95465773949
+    y: 4140857.7630067025
+    theta: 1.3382955429375281
+    kappa: 0.0028059456259281867
+    s: 6.76340099490051
+    dkappa: -0.0018439958767480747
     ddkappa: 0
   }
-  v: 3.9787567361640388
-  a: 1.0771012464588618
+  v: 3.9787567361640392
+  a: 1.077101246458861
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 587583.0144903413
-    y: 4140858.16372265
-    theta: 1.3447776375500951
-    kappa: 0.0014527178055164171
-    s: 7.1666248954858176
-    dkappa: -0.0020178836063425428
+    x: 587583.04737226013
+    y: 4140858.1554267276
+    theta: 1.3392913854618969
+    kappa: 0.002107493321806723
+    s: 7.1666248954858194
+    dkappa: -0.0017421687413243011
     ddkappa: 0
   }
   v: 4.08534125492822
-  a: 1.0543008737633257
+  a: 1.0543008737633248
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587583.10712331766
-    y: 4140858.5669856016
-    theta: 1.3452364799281793
-    kappa: 0.00067266582884976532
-    s: 7.5803903851369228
-    dkappa: -0.0018902871530850616
+    x: 587583.14216684492
+    y: 4140858.5581870172
+    theta: 1.3400401777129052
+    kappa: 0.0014315473764821619
+    s: 7.5803903851369236
+    dkappa: -0.001637776007394556
     ddkappa: 0
   }
-  v: 4.1895601240432194
-  a: 1.0297968948632343
+  v: 4.1895601240432185
+  a: 1.0297968948632332
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587583.20192449947
-    y: 4140858.9803152345
-    theta: 1.3453889365360747
-    kappa: -7.3504650328343632e-05
-    s: 8.0044524672717614
-    dkappa: -0.0017596977456162675
+    x: 587583.23905888374
+    y: 4140858.9710315182
+    theta: 1.340531802815669
+    kappa: 0.0007824011272112364
+    s: 8.004452467271765
+    dkappa: -0.001530891992858157
     ddkappa: 0
   }
   v: 4.2912455753049823
-  a: 1.0036411580866049
+  a: 1.0036411580866038
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587583.29898251744
-    y: 4140859.4034228874
-    theta: 1.3451813393983982
-    kappa: -0.00077089988371655167
-    s: 8.4385496277295111
-    dkappa: -0.0016262013809740473
+    x: 587583.33807759383
+    y: 4140859.3936846144
+    theta: 1.3407180790471991
+    kappa: 0.00017223773750077708
+    s: 8.4385496277295129
+    dkappa: -0.0014215995743797455
     ddkappa: 0
   }
-  v: 4.3902350253422568
-  a: 0.975885511761454
+  v: 4.390235025342256
+  a: 0.97588551176145288
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587583.39838284883
-    y: 4140859.8360041939
-    theta: 1.34467838318152
-    kappa: -0.0014255876984321473
-    s: 8.8824043532538575
-    dkappa: -0.0014898170051860934
+    x: 587583.43930575764
+    y: 4140859.82584185
+    theta: 1.3406531822121488
+    kappa: -0.00040380075128604049
+    s: 8.88240435325386
+    dkappa: -0.0013099367630928175
     ddkappa: 0
   }
-  v: 4.48637107561659
-  a: 0.94658180421579852
+  v: 4.4863710756165895
+  a: 0.94658180421579741
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 587583.50018915487
-    y: 4140860.2777437503
-    theta: 1.3438855043447135
-    kappa: -0.0020330484168903304
-    s: 9.3357236499762823
-    dkappa: -0.001350597824646549
+    x: 587583.54277934961
+    y: 4140860.2671938608
+    theta: 1.3403403449338902
+    kappa: -0.00094202929769405326
+    s: 9.3357236499762841
+    dkappa: -0.0011959610938876018
     ddkappa: 0
   }
-  v: 4.5795015124223317
-  a: 0.915781883777655
+  v: 4.5795015124223326
+  a: 0.91578188377765468
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587583.60447051621
-    y: 4140860.7283108919
-    theta: 1.3428115608015074
-    kappa: -0.0025885803596569458
-    s: 9.79820112405032
-    dkappa: -0.001208596413620619
+    x: 587583.64853889972
+    y: 4140860.7174163787
+    theta: 1.3397853280939216
+    kappa: -0.0014385816005488784
+    s: 9.7982011240503226
+    dkappa: -0.0010797291430726776
     ddkappa: 0
   }
-  v: 4.6695417253938949
-  a: 0.88540677740979545
+  v: 4.6695417253938958
+  a: 0.88540677740979523
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587583.71130077261
-    y: 4140861.187377783
-    theta: 1.3414704153938657
-    kappa: -0.0030876565641810715
-    s: 10.269534575744878
-    dkappa: -0.0010638621300109764
+    x: 587583.75663069217
+    y: 4140861.1761879455
+    theta: 1.3389980405357038
+    kappa: -0.001889740547009436
+    s: 10.269534575744879
+    dkappa: -0.00096129323170484216
     ddkappa: 0
   }
-  v: 4.7566592617611274
-  a: 0.85732348674706427
+  v: 4.7566592617611283
+  a: 0.85732348674706416
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 587583.82075816765
-    y: 4140861.6546369814
-    theta: 1.339880639025488
-    kappa: -0.0035259347013638811
-    s: 10.749443147828606
-    dkappa: -0.00091644005857288117
+    x: 587583.86710715457
+    y: 4140861.6432073517
+    theta: 1.3379923022847142
+    kappa: -0.0022919471493532207
+    s: 10.749443147828607
+    dkappa: -0.00084069943049791529
     ddkappa: 0
   }
-  v: 4.8410818436113487
+  v: 4.84108184361135
   a: 0.83150282212558846
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587583.93292176339
-    y: 4140862.129803502
-    theta: 1.3380652423678856
-    kappa: -0.0038992427843815881
-    s: 11.237668609330569
-    dkappa: -0.000766374604931937
+    x: 587583.98002389283
+    y: 4140862.1181955831
+    theta: 1.3367856181808462
+    kappa: -0.00264178754727295
+    s: 11.237668609330571
+    dkappa: -0.00071798962844307583
     ddkappa: 0
   }
-  v: 4.9230342740654907
+  v: 4.9230342740654915
   a: 0.80791559388149536
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587584.0478677901
-    y: 4140862.6126153208
-    theta: 1.3360514358203481
-    kappa: -0.0042035650794614429
-    s: 11.73397506364361
-    dkappa: -0.00061371287310339165
+    x: 587584.095436607
+    y: 4140862.6008961955
+    theta: 1.3353989742835233
+    kappa: -0.0029359795380198778
+    s: 11.733975063643612
+    dkappa: -0.00059320376332311331
     ddkappa: 0
   }
-  v: 5.0027384372780981
-  a: 0.786532612350912
+  v: 5.002738437278099
+  a: 0.7865326123509121
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587584.16566625191
-    y: 4140863.10283388
-    theta: 1.3338704140467497
-    kappa: -0.0044350300074428046
-    s: 12.238148656627702
-    dkappa: -0.00045850700877710787
+    x: 587584.213398223
+    y: 4140863.0910756723
+    theta: 1.3338566555316302
+    kappa: -0.0031713604668475747
+    s: 12.238148656627706
+    dkappa: -0.00046638158870410291
     ddkappa: 0
   }
   v: 5.0804132984373274
-  a: 0.76732468786996577
+  a: 0.76732468786996588
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587584.28637776535
-    y: 4140863.6002445891
-    theta: 1.331557163695944
-    kappa: -0.004589899372027988
-    s: 12.749997284713315
-    dkappa: -0.00030081530423228667
+    x: 587584.33395621856
+    y: 4140863.5885237721
+    theta: 1.3321860836486557
+    kappa: -0.0033448761773285911
+    s: 12.749997284713317
+    dkappa: -0.00033756382983329554
     ddkappa: 0
   }
-  v: 5.1562749037649489
-  a: 0.75026263077478372
+  v: 5.15627490376495
+  a: 0.75026263077478383
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 587584.410050509
-    y: 4140864.1046573641
-    theta: 1.3291502928325309
-    kappa: -0.0046645581691244819
-    s: 13.269350303004776
-    dkappa: -0.00014070199717924962
+    x: 587584.45715003111
+    y: 4140864.0930538969
+    theta: 1.3304176739986768
+    kappa: -0.0034535706540923649
+    s: 13.26935030300478
+    dkappa: -0.00020679266135672527
     ddkappa: 0
   }
-  v: 5.2305363805163463
-  a: 0.73531725140149318
+  v: 5.2305363805163472
+  a: 0.73531725140149329
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 587584.53671732685
-    y: 4140864.6159071927
-    theta: 1.32669188185522
-    kappa: -0.0046555042028404974
-    s: 13.796058233383636
-    dkappa: 2.1764240466240792e-05
+    x: 587584.58300858655
+    y: 4140864.6045034649
+    theta: 1.3285847111177402
+    kappa: -0.003494575949050077
+    s: 13.796058233383638
+    dkappa: -7.4111464661349107e-05
     ddkappa: 0
   }
-  v: 5.3034079369805145
-  a: 0.72245936008622114
+  v: 5.3034079369805154
+  a: 0.72245936008622136
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587584.66639373114
-    y: 4140865.1338545252
-    theta: 1.3242273597877599
-    kappa: -0.0045593367448984205
-    s: 14.329992472612018
-    dkappa: 0.00018651307124244068
+    x: 587584.71154871024
+    y: 4140865.12273411
+    theta: 1.3267232465771135
+    kappa: -0.0034651019518132731
+    s: 14.32999247261202
+    dkappa: 6.043616826733533e-05
     ddkappa: 0
   }
-  v: 5.3750968624800617
-  a: 0.71165976716509516
+  v: 5.3750968624800626
+  a: 0.71165976716509527
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 587584.79907505063
-    y: 4140865.6583859166
-    theta: 1.3218053973461861
-    kappa: -0.0043727437089102492
-    s: 14.871045000435993
-    dkappa: 0.00035347602333595565
+    x: 587584.84277266043
+    y: 4140865.647632123
+    theta: 1.3248720101261873
+    kappa: -0.0033624257325569288
+    s: 14.871045000435997
+    dkappa: 0.0001968078204468264
     ddkappa: 0
   }
-  v: 5.4458075273712083
+  v: 5.4458075273712092
   a: 0.702889282974242
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 587584.93473366834
-    y: 4140866.1894146856
-    theta: 1.3194778222654122
-    kappa: -0.0040924868248662536
-    s: 15.419128087688932
-    dkappa: 0.000522590820043914
+    x: 587584.976665742
+    y: 4140866.1791088972
+    theta: 1.3230723391006853
+    kappa: -0.00318388011287202
+    s: 15.419128087688936
+    dkappa: 0.000334964217312269
     ddkappa: 0
   }
-  v: 5.51574138304379
-  a: 0.696118717849789
+  v: 5.5157413830437907
+  a: 0.69611871784978907
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 587585.073316896
-    y: 4140866.7268814258
-    theta: 1.3172995606961042
-    kappa: -0.003715384514866991
-    s: 15.974174004394873
-    dkappa: 0.00069380603693130452
+    x: 587585.11319455085
+    y: 4140866.7171012354
+    theta: 1.3213681292621511
+    kappa: -0.0029268412303580563
+    s: 15.974174004394877
+    dkappa: 0.00047487149870099996
     ddkappa: 0
   }
-  v: 5.58509696192125
-  a: 0.69131888212786341
+  v: 5.5850969619212512
+  a: 0.69131888212786352
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 587585.21479610482
-    y: 4140867.2707411349
-    theta: 1.315343675050094
-    kappa: -0.0032347231964201155
+    x: 587585.25234673475
+    y: 4140867.2615610217
+    theta: 1.3198167726497645
+    kappa: -0.0025857177683065004
     s: 16.536134727871875
-    dkappa: 0.0008641655621987091
+    dkappa: 0.00061416211641391065
     ddkappa: 0
   }
   v: 5.6540698774606506
@@ -1335,26 +1335,26 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587585.35904398467
-    y: 4140867.8209949667
-    theta: 1.3136713395683532
-    kappa: -0.0026886058764454385
-    s: 17.104981650835381
-    dkappa: 0.00095916144774498013
+    x: 587585.39403219684
+    y: 4140867.8124802448
+    theta: 1.3184670978857487
+    kappa: -0.00219069731117333
+    s: 17.104981650835384
+    dkappa: 0.00069382209667032775
     ddkappa: 0
   }
-  v: 5.7228528241526613
+  v: 5.7228528241526622
   a: 0.68751464023610331
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 587585.5058715106
-    y: 4140868.3776809559
-    theta: 1.312284509521455
-    kappa: -0.0021612006346475414
-    s: 17.680705289501592
-    dkappa: 0.00091388001294421617
+    x: 587585.53811326378
+    y: 4140868.36988341
+    theta: 1.3173225844856058
+    kappa: -0.0018083149746091457
+    s: 17.680705289501596
+    dkappa: 0.00066262455345356954
     ddkappa: 0
   }
   v: 5.7916355775215678
@@ -1363,320 +1363,320 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587585.65514334571
-    y: 4140868.9408433749
-    theta: 1.3111757033627189
-    kappa: -0.0016587002659545697
-    s: 18.263314991690812
-    dkappa: 0.00085903193020130347
+    x: 587585.68449471868
+    y: 4140868.9338040436
+    theta: 1.3163781570122846
+    kappa: -0.0014433882215452696
+    s: 18.263314991690816
+    dkappa: 0.00062391349153808717
     ddkappa: 0
   }
-  v: 5.8606049941252669
-  a: 0.6912430399879792
+  v: 5.8606049941252678
+  a: 0.69124303998797909
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 587585.80672955536
-    y: 4140869.5105448
-    theta: 1.310336168851383
-    kappa: -0.0011820536487778554
+    x: 587585.83308628039
+    y: 4140869.5042938772
+    theta: 1.3156278114532529
+    kappa: -0.0010965918183815106
     s: 18.852838644930817
-    dkappa: 0.00080353645952017613
+    dkappa: 0.00058473544465472164
     ddkappa: 0
   }
-  v: 5.9299450115552679
-  a: 0.69585900632059849
+  v: 5.9299450115552688
+  a: 0.69585900632059838
   relative_time: 5.7999999999999963
 }
 trajectory_point {
   path_point {
-    x: 587585.960506324
-    y: 4140870.0868654903
-    theta: 1.3097562104785818
-    kappa: -0.00073219580825901011
+    x: 587585.98380310833
+    y: 4140870.0814223485
+    theta: 1.3150648710569888
+    kappa: -0.00076859129738031775
     s: 19.449322384560208
-    dkappa: 0.00074739887184369384
+    dkappa: 0.00054509291877125076
     ddkappa: 0
   }
-  v: 5.9998366484366956
-  a: 0.70227056407250854
+  v: 5.9998366484366965
+  a: 0.70227056407250832
   relative_time: 5.8999999999999959
 }
 trajectory_point {
   path_point {
-    x: 587586.116364247
-    y: 4140870.6699007461
-    theta: 1.3094417473670341
-    kappa: -0.00031236977640686724
+    x: 587586.13657738059
+    y: 4140870.6652732044
+    theta: 1.3146942365552303
+    kappa: -0.0004617004595542955
     s: 20.052830301831786
-    dkappa: 0.00069061932894532124
+    dkappa: 0.00050498549565088319
     ddkappa: 0
   }
-  v: 6.070458004428283
-  a: 0.71044852357983612
+  v: 6.0704580044282839
+  a: 0.7104485235798359
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 587586.27417110093
-    y: 4140871.259770453
-    theta: 1.3093816001593443
-    kappa: 7.5927473481072566e-05
-    s: 20.663444152015906
-    dkappa: 0.0006331913683942891
+    x: 587586.29132109159
+    y: 4140871.2559538763
+    theta: 1.314508007487925
+    kappa: -0.00017694667691888483
+    s: 20.663444152015909
+    dkappa: 0.00046441001974688203
     ddkappa: 0
   }
-  v: 6.1419842602223795
-  a: 0.7203636951787088
+  v: 6.1419842602223813
+  a: 0.72036369517870846
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 587586.43380763172
-    y: 4140871.85660911
-    theta: 1.309543412066102
-    kappa: 0.000434310583579342
-    s: 21.281263062503832
-    dkappa: 0.00057510206293216791
+    x: 587586.44794952078
+    y: 4140871.853589
+    theta: 1.3144831554247118
+    kappa: 8.679065661551466e-05
+    s: 21.281263062503836
+    dkappa: 0.0004233601677762881
     ddkappa: 0
   }
-  v: 6.2145876775449462
-  a: 0.73198688920525345
+  v: 6.214587677544948
+  a: 0.73198688920525312
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
-    x: 587586.59518156131
-    y: 4140872.4605616666
-    theta: 1.3099134466603941
-    kappa: 0.00076182963416177344
-    s: 21.906403240911118
-    dkappa: 0.00051633673927118143
+    x: 587586.6064044605
+    y: 4140872.4583139578
+    theta: 1.3146101317869978
+    kappa: 0.00032883939695708557
+    s: 21.906403240911125
+    dkappa: 0.00038182784250678011
     ddkappa: 0
   }
   v: 6.2884375991555572
-  a: 0.74528891599559766
+  a: 0.74528891599559721
   relative_time: 6.2999999999999945
 }
 trajectory_point {
   path_point {
-    x: 587586.75819243118
-    y: 4140873.07179253
-    theta: 1.3104847826275254
-    kappa: 0.0010558608634995618
-    s: 22.538997683180945
-    dkappa: 0.00045687823085060179
+    x: 587586.76662623417
+    y: 4140873.0702818558
+    theta: 1.314884604894681
+    kappa: 0.00054732501927318416
+    s: 22.538997683180948
+    dkappa: 0.00033980319542852613
     ddkappa: 0
   }
-  v: 6.3637004488473972
-  a: 0.76024058588586818
+  v: 6.3637004488473981
+  a: 0.76024058588586774
   relative_time: 6.3999999999999941
 }
 trajectory_point {
   path_point {
-    x: 587586.922715073
-    y: 4140873.69048955
-    theta: 1.3112529891704328
-    kappa: 0.001312143803394747
+    x: 587586.92854178662
+    y: 4140873.6896662908
+    theta: 1.3153039802358444
+    kappa: 0.00073928009458159682
     s: 23.179195881687495
-    dkappa: 0.00039670758774075
+    dkappa: 0.00029727554632271965
     ddkappa: 0
   }
   v: 6.4405397314472657
-  a: 0.77681270921219259
+  a: 0.77681270921219214
   relative_time: 6.4999999999999938
 }
 trajectory_point {
   path_point {
-    x: 587587.08872785629
-    y: 4140874.316829537
-    theta: 1.3121775947265737
-    kappa: 0.0015342311776898516
-    s: 23.827163533339306
-    dkappa: 0.00033580405329803806
+    x: 587587.09213609248
+    y: 4140874.3166423365
+    theta: 1.3158391040932578
+    kappa: 0.00090719931658006243
+    s: 23.827163533339309
+    dkappa: 0.00025423217640435636
     ddkappa: 0
   }
-  v: 6.5191160328155737
-  a: 0.79497609631069766
+  v: 6.5191160328155746
+  a: 0.79497609631069721
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
-    x: 587587.25618279842
-    y: 4140874.9510125932
-    theta: 1.3132407133180062
-    kappa: 0.0017210736631197443
+    x: 587587.25738640688
+    y: 4140874.9514034521
+    theta: 1.3164775032016709
+    kappa: 0.0010503461815678057
     s: 24.483082247682649
-    dkappa: 0.00027414716444769741
+    dkappa: 0.00021065993886093483
     ddkappa: 0
   }
   v: 6.5995870198463482
-  a: 0.81470155751751117
+  a: 0.8147015575175105
   relative_time: 6.6999999999999931
 }
 trajectory_point {
   path_point {
-    x: 587587.42491685855
-    y: 4140875.5932849105
-    theta: 1.31443793362806
-    kappa: 0.0018648514457713897
-    s: 25.147149255004869
-    dkappa: 0.00021171450480893982
+    x: 587587.42420014692
+    y: 4140875.5941772358
+    theta: 1.3172175026513724
+    kappa: 0.0011631785322587858
+    s: 25.147149255004877
+    dkappa: 0.000166544601324621
     ddkappa: 0
   }
-  v: 6.6821074404672229
-  a: 0.8359599031687599
+  v: 6.6821074404672247
+  a: 0.83595990316875923
   relative_time: 6.7999999999999927
 }
 trajectory_point {
   path_point {
-    x: 587587.59499495558
-    y: 4140876.2438481767
-    theta: 1.3157304299604247
-    kappa: 0.0019698172556406857
+    x: 587587.59262709878
+    y: 4140876.2451699753
+    theta: 1.318030753264597
+    kappa: 0.001248753448627307
     s: 25.819577114437763
-    dkappa: 0.00014848635879498247
+    dkappa: 0.00012187180341771914
     ddkappa: 0
   }
   v: 6.766829123639452
-  a: 0.85872194360057108
+  a: 0.85872194360057041
   relative_time: 6.8999999999999924
 }
 trajectory_point {
   path_point {
-    x: 587587.766429403
-    y: 4140876.9029334141
-    theta: 1.3170941059190568
-    kappa: 0.0020357800942769982
-    s: 26.500593422060923
-    dkappa: 8.44420991576527e-05
+    x: 587587.76268787566
+    y: 4140876.9046110315
+    theta: 1.3189003017994572
+    kappa: 0.0013069198655670949
+    s: 26.500593422060927
+    dkappa: 7.6626466249973927e-05
     ddkappa: 0
   }
-  v: 6.8539009793578929
-  a: 0.88295848914907193
+  v: 6.8539009793578938
+  a: 0.88295848914907127
   relative_time: 6.999999999999992
 }
 trajectory_point {
   path_point {
-    x: 587587.9390455262
-    y: 4140877.5708350451
-    theta: 1.3185075917001803
-    kappa: 0.0020527578570177212
-    s: 27.190440519005122
-    dkappa: 1.9559655625616653e-05
+    x: 587587.93428475026
+    y: 4140877.5727752978
+    theta: 1.3198128382904337
+    kappa: 0.001330609225290956
+    s: 27.190440519005129
+    dkappa: 3.079264370115439e-05
     ddkappa: 0
   }
-  v: 6.9434689986510252
-  a: 0.90864035015038958
+  v: 6.9434689986510261
+  a: 0.90864035015038891
   relative_time: 7.0999999999999917
 }
 trajectory_point {
   path_point {
-    x: 587588.11304345191
-    y: 4140878.2477650875
-    theta: 1.3199368195770114
-    kappa: 0.0020276942856015893
-    s: 27.889375199555648
-    dkappa: -4.6180811888340115e-05
+    x: 587588.10756400751
+    y: 4140878.2498897146
+    theta: 1.3207429790225462
+    kappa: 0.0013247789163918289
+    s: 27.889375199555655
+    dkappa: -1.5645680072523547e-05
     ddkappa: 0
   }
-  v: 7.0356762535809354
-  a: 0.93573833694065145
+  v: 7.0356762535809363
+  a: 0.93573833694065089
   relative_time: 7.1999999999999913
 }
 trajectory_point {
   path_point {
-    x: 587588.288386111
-    y: 4140878.9340114654
-    theta: 1.32135000488147
-    kappa: 0.0019550193743078717
-    s: 28.59766841925568
-    dkappa: -0.0001128014092389241
+    x: 587588.28251940082
+    y: 4140878.9362349864
+    theta: 1.3216692425586187
+    kappa: 0.0012854525981428357
+    s: 28.59766841925569
+    dkappa: -6.2705581735194416e-05
     ddkappa: 0
   }
   v: 7.1306628972433241
-  a: 0.96422325985598467
+  a: 0.96422325985598412
   relative_time: 7.2999999999999909
 }
 trajectory_point {
   path_point {
-    x: 587588.46512321429
-    y: 4140879.6298539918
-    theta: 1.3227108563363732
-    kappa: 0.0018319472698096848
-    s: 29.315605003009662
-    dkappa: -0.00018032509378761527
+    x: 587588.45920454059
+    y: 4140879.6320907548
+    theta: 1.3225665030039053
+    kappa: 0.0012107130069609702
+    s: 29.315605003009669
+    dkappa: -0.00011040491864442367
     ddkappa: 0
   }
   v: 7.2285661637675052
-  a: 0.994065929232516
+  a: 0.99406592923251547
   relative_time: 7.3999999999999906
 }
 trajectory_point {
   path_point {
-    x: 587588.64342926163
-    y: 4140880.3355548731
-    theta: 1.3239895226318019
-    kappa: 0.0016605684458742831
-    s: 30.04348335318663
-    dkappa: -0.00024877735045963993
+    x: 587588.637755522
+    y: 4140880.3377297497
+    theta: 1.3234131845165906
+    kappa: 0.0011019934936059225
+    s: 30.043483353186637
+    dkappa: -0.00015876280470727091
     ddkappa: 0
   }
   v: 7.3295203683164045
-  a: 1.0252371554063731
+  a: 1.0252371554063726
   relative_time: 7.49999999999999
 }
 trajectory_point {
   path_point {
-    x: 587588.82331081294
-    y: 4140881.0514327353
-    theta: 1.3251275079583715
-    kappa: 0.0014319250641672198
-    s: 30.781615157723607
-    dkappa: -0.00031818483311406813
+    x: 587588.81820103619
+    y: 4140881.0534656961
+    theta: 1.324169658118004
+    kappa: 0.0009530153241271717
+    s: 30.781615157723614
+    dkappa: -0.00020779920165940629
     ddkappa: 0
   }
-  v: 7.43365690708656
-  a: 1.0577077487136834
+  v: 7.4336569070865606
+  a: 1.0577077487136828
   relative_time: 7.59999999999999
 }
 trajectory_point {
   path_point {
-    x: 587589.00506438466
-    y: 4140881.77774682
-    theta: 1.3261072562517977
-    kappa: 0.0011520581930108808
+    x: 587589.00076261349
+    y: 4140881.7795771309
+    theta: 1.3248222412464699
+    kappa: 0.00076800274983329284
     s: 31.53032509822895
-    dkappa: -0.00038857974211276057
+    dkappa: -0.00025753622774793043
     ddkappa: 0
   }
   v: 7.5411042573081266
-  a: 1.0914485194905732
+  a: 1.091448519490573
   relative_time: 7.6999999999999895
 }
 trajectory_point {
   path_point {
-    x: 587589.188775314
-    y: 4140882.51482287
-    theta: 1.3268450186827159
-    kappa: 0.00080961607290987148
+    x: 587589.18552463164
+    y: 4140882.5163904196
+    theta: 1.3253139620543677
+    kappa: 0.00053898634113586606
     s: 32.2899505580857
-    dkappa: -0.00045999557921888948
+    dkappa: -0.00030799693074412516
     ddkappa: 0
   }
   v: 7.6519879772448647
-  a: 1.1264302780731708
+  a: 1.1264302780731705
   relative_time: 7.7999999999999892
 }
 trajectory_point {
   path_point {
-    x: 587589.37478716765
-    y: 4140883.2629352645
-    theta: 1.3273280936556917
-    kappa: 0.00041095835408638186
+    x: 587589.37274461228
+    y: 4140883.2642013938
+    theta: 1.3256344865446625
+    kappa: 0.00027041690605171759
     s: 33.060841330554965
-    dkappa: -0.00053184687091261838
+    dkappa: -0.00035876708586839027
     ddkappa: 0
   }
   v: 7.7664307061941544
@@ -1685,16 +1685,16 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587589.56334873685
-    y: 4140884.0223949067
-    theta: 1.3274694367862634
-    kappa: -2.3934231012156939e-05
+    x: 587589.56262029335
+    y: 4140884.023333557
+    theta: 1.3257242238800115
+    kappa: -2.3842837772964635e-05
     s: 33.843359326879273
-    dkappa: -0.00054847886262226857
+    dkappa: -0.00037110357381584034
     ddkappa: 0
   }
   v: 7.8845521644869834
-  a: 1.1999999999999957
+  a: 1.199999999999996
   relative_time: 7.9999999999999885
 }
 decision {
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
index f294d008d4..cebc2c78e9 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
@@ -9,835 +9,835 @@ trajectory_point {
     y: 4140719.1569185639
     z: 0
     theta: 2.3560555247297881
-    kappa: -0.010492345623168791
+    kappa: -0.010754204813311985
     s: 0
     dkappa: 0
     ddkappa: 0
   }
   v: 5.4222221374511719
-  a: 1.1477460050966795
+  a: 1.14774600509668
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 587503.27934785094
-    y: 4140719.2339772871
-    theta: 2.3549230337477476
-    kappa: -0.010465223763975592
+    x: 587503.27935260546
+    y: 4140719.2339820154
+    theta: 2.3548967989224217
+    kappa: -0.010718215706372219
     s: 0.10867417714522677
-    dkappa: 5.4547244751669972e-05
+    dkappa: 7.2380965570781433e-05
     ddkappa: 0
   }
   v: 5.4452047589191848
-  a: 1.1505005835535411
+  a: 1.1505005835535416
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 587503.202394182
-    y: 4140719.311362328
-    theta: 2.3537857433021108
-    kappa: -0.010437987053190958
+    x: 587503.20240371116
+    y: 4140719.3113718037
+    theta: 2.3537331622887194
+    kappa: -0.010682074198146943
     s: 0.21780855143639763
-    dkappa: 0.00010932547801425626
+    dkappa: 0.00014506843922518827
     ddkappa: 0
   }
   v: 5.4682415432978484
-  a: 1.1531625376905967
+  a: 1.1531625376905972
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 587503.12511526432
-    y: 4140719.38907444
-    theta: 2.352643645209235
-    kappa: -0.010410635225064829
+    x: 587503.1251295883
+    y: 4140719.3890886838
+    theta: 2.3525646065951462
+    kappa: -0.010645779936001517
     s: 0.32740418770347313
-    dkappa: 0.00016433523426192747
+    dkappa: 0.00021806313017881098
     ddkappa: 0
   }
   v: 5.4913306525924943
-  a: 1.1557333166805261
+  a: 1.1557333166805266
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 587503.047510373
-    y: 4140719.4671143536
-    theta: 2.3514967317403928
-    kappa: -0.010383168022949007
+    x: 587503.04752951209
+    y: 4140719.4671333842
+    theta: 2.35139112407721
+    kappa: -0.010609332579378914
     s: 0.43746211430635507
-    dkappa: 0.0002195770296632963
+    dkappa: 0.00029136572335685074
     ddkappa: 0
   }
   v: 5.5144702777919061
-  a: 1.1582143696960094
+  a: 1.1582143696960099
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 587502.96976134344
-    y: 4140719.5456633731
-    theta: 2.3503500569068341
-    kappa: -0.010358132869478596
+    x: 587502.96978931315
+    y: 4140719.5456911218
+    theta: 2.3502197772386277
+    kappa: -0.010575486548647577
     s: 0.54798332371455538
-    dkappa: 0.00024444778635645772
+    dkappa: 0.00032562620476711925
     ddkappa: 0
   }
   v: 5.5376586388683222
-  a: 1.1606071459097262
+  a: 1.1606071459097267
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 587502.89190149517
-    y: 4140719.6247558123
-    theta: 2.3492045464651525
-    kappa: -0.010336003991708108
+    x: 587502.8919430559
+    y: 4140719.6247969391
+    theta: 2.3490518652335077
+    kappa: -0.010544754566169372
     s: 0.6589687730868653
-    dkappa: 0.00023324852118759159
+    dkappa: 0.00031351661535847372
     ddkappa: 0
   }
   v: 5.5608939847774348
-  a: 1.1629130944943569
+  a: 1.1629130944943571
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 587502.81371532066
-    y: 4140719.7041797447
-    theta: 2.3480542325933498
-    kappa: -0.010313782367349932
+    x: 587502.81377052912
+    y: 4140719.7042343039
+    theta: 2.347879055767327
+    kappa: -0.010513893779757345
     s: 0.77041938485102424
-    dkappa: 0.00022200231764074564
+    dkappa: 0.00030135627221944731
     ddkappa: 0
   }
   v: 5.5841745934583873
-  a: 1.1651336646225807
+  a: 1.1651336646225812
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 587502.73520219664
-    y: 4140719.7839358021
-    theta: 2.346899109156932
-    kappa: -0.01029146781929473
+    x: 587502.73527111
+    y: 4140719.78400385
+    theta: 2.3467013427077821
+    kappa: -0.010482903943446887
     s: 0.88233604728338944
-    dkappa: 0.00021070908608216584
+    dkappa: 0.00028914507843047065
     ddkappa: 0
   }
   v: 5.60749877183378
-  a: 1.1672703054670781
+  a: 1.1672703054670786
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 587502.65636211436
-    y: 4140719.8640251728
-    theta: 2.3457391830950205
-    kappa: -0.010269066799028347
+    x: 587502.65644480253
+    y: 4140719.8641067781
+    theta: 2.3455187392566561
+    kappa: -0.010451792160058761
     s: 0.99471961508860451
-    dkappa: 0.00019937208535397155
+    dkappa: 0.00027688625195985571
     ddkappa: 0
   }
   v: 5.6308648558096639
-  a: 1.1693244662005289
+  a: 1.1693244662005293
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 587502.57759725791
-    y: 4140719.9448429439
-    theta: 2.3445830659858222
-    kappa: -0.010251093306870416
+    x: 587502.57770118688
+    y: 4140719.944945245
+    theta: 2.3443440774869191
+    kappa: -0.010425546333323647
     s: 1.1075709099792685
-    dkappa: 0.0001902715862241915
+    dkappa: 0.000266830108264045
     ddkappa: 0
   }
   v: 5.654271210275545
-  a: 1.1712975959956129
+  a: 1.1712975959956133
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 587502.4985053991
-    y: 4140720.02599624
-    theta: 2.3434221453746438
-    kappa: -0.010233045195492634
+    x: 587502.49863065709
+    y: 4140720.0261193234
+    theta: 2.34316453504207
+    kappa: -0.010399191543728221
     s: 1.220890721255605
-    dkappa: 0.00018113330522434758
+    dkappa: 0.00025673221521921058
     ddkappa: 0
   }
   v: 5.6777162291043837
-  a: 1.1731911440250102
+  a: 1.1731911440250105
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587502.41908600961
-    y: 4140720.1074856045
-    theta: 2.3422564166037221
-    kappa: -0.010214922344255391
+    x: 587502.41923268477
+    y: 4140720.1076295548
+    theta: 2.3419801072915623
+    kappa: -0.010372727615108258
     s: 1.3346798063851322
-    dkappa: 0.00017195718127111328
+    dkappa: 0.00024659250532765815
     ddkappa: 0
   }
-  v: 5.7011983351525917
-  a: 1.1750065594614005
+  v: 5.7011983351525926
+  a: 1.1750065594614008
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 587502.33933858224
-    y: 4140720.1893115561
-    theta: 2.3410858754135058
-    kappa: -0.010196724637496653
+    x: 587502.33950676315
+    y: 4140720.1894764607
+    theta: 2.3407907900120137
+    kappa: -0.010346154378568048
     s: 1.4489388915823296
-    dkappa: 0.00016274315580145444
+    dkappa: 0.00023641091387664192
     ddkappa: 0
   }
   v: 5.7247159802600374
-  a: 1.1767452914774639
+  a: 1.1767452914774641
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587502.25952527882
-    y: 4140720.2717292644
-    theta: 2.3399158253294465
-    kappa: -0.010180801471574286
-    s: 1.5636686723883098
-    dkappa: 0.00015454601362161426
+    x: 587502.25971888634
+    y: 4140720.27191877
+    theta: 2.3396044230053654
+    kappa: -0.01032212864773388
+    s: 1.56366867238831
+    dkappa: 0.00022723111628395186
     ddkappa: 0
   }
   v: 5.7482676452500394
-  a: 1.17840878924588
+  a: 1.1784087892458803
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587502.17954278609
-    y: 4140720.3546394808
-    theta: 2.3387441731418344
-    kappa: -0.010166232707215008
-    s: 1.6788698142504861
-    dkappa: 0.00014695264050619216
+    x: 587502.17976422468
+    y: 4140720.3548558257
+    theta: 2.3384179189798551
+    kappa: -0.010299609677474219
+    s: 1.6788698142504863
+    dkappa: 0.0002186442790164102
     ddkappa: 0
   }
   v: 5.771851839929373
-  a: 1.1799985019393293
+  a: 1.1799985019393295
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 587502.09923259274
-    y: 4140720.437889393
-    theta: 2.337567718775297
-    kappa: -0.010151604252363011
+    x: 587502.09948197647
+    y: 4140720.4381326875
+    theta: 2.3372265518583442
+    kappa: -0.010276998443492173
     s: 1.7945429531022412
-    dkappa: 0.0001393281561605054
+    dkappa: 0.00021002226014150971
     ddkappa: 0
   }
   v: 5.7954671030882645
-  a: 1.1815158787304911
+  a: 1.1815158787304914
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 587502.01859427709
-    y: 4140720.5214794381
-    theta: 2.3363864593115333
-    kappa: -0.010136916030254944
+    x: 587502.0188717203
+    y: 4140720.5217497917
+    theta: 2.3360303187696432
+    kappa: -0.010254294827134467
     s: 1.9106886959425977
-    dkappa: 0.00013167252057479377
+    dkappa: 0.00020136501441489133
     ddkappa: 0
   }
   v: 5.8191120025003951
-  a: 1.1829623687920459
+  a: 1.1829623687920461
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 587501.9377724746
-    y: 4140720.6055492014
-    theta: 2.3352038492488236
-    kappa: -0.010123223210562284
+    x: 587501.93807968148
+    y: 4140720.6058481485
+    theta: 2.3348339260828621
+    kappa: -0.010232716923467881
     s: 2.0273076214158858
-    dkappa: 0.00012434416531592566
+    dkappa: 0.0001930283230018514
     ddkappa: 0
   }
-  v: 5.8427851349229
-  a: 1.1843394212966729
+  v: 5.8427851349229005
+  a: 1.1843394212966734
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587501.8569284617
-    y: 4140720.690254197
-    theta: 2.33402373025312
-    kappa: -0.010111701902327611
-    s: 2.1444002803914128
-    dkappa: 0.00011774260611808713
+    x: 587501.85726882564
+    y: 4140720.6905847774
+    theta: 2.3336426078082497
+    kappa: -0.010213621754976825
+    s: 2.1444002803914133
+    dkappa: 0.00018540854661859313
     ddkappa: 0
   }
   v: 5.8664851260963671
-  a: 1.1856484854170528
+  a: 1.185648485417053
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587501.77575700847
-    y: 4140720.7753022714
-    theta: 2.332838828796632
-    kappa: -0.010100133929655072
+    x: 587501.776130664
+    y: 4140720.7756646136
+    theta: 2.3324464616366427
+    kappa: -0.010194449245850015
     s: 2.2619671965431332
-    dkappa: 0.00011111430880659543
+    dkappa: 0.00017775790806462015
     ddkappa: 0
   }
   v: 5.8902106307448383
-  a: 1.186891010325865
+  a: 1.1868910103258652
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587501.69425777171
-    y: 4140720.8606937835
-    theta: 2.3316491431848658
-    kappa: -0.010088519243636719
+    x: 587501.69466485339
+    y: 4140720.8610880161
+    theta: 2.3312454859212886
+    kappa: -0.010175199315028466
     s: 2.380008866929316
-    dkappa: 0.00010445924535783559
+    dkappa: 0.00017007637499398607
     ddkappa: 0
   }
   v: 5.913960332575809
-  a: 1.1880684451957897
+  a: 1.18806844519579
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 587501.61246927909
-    y: 4140720.9464659751
-    theta: 2.3304550123706091
-    kappa: -0.010077023667591698
+    x: 587501.61291029945
+    y: 4140720.9468925544
+    theta: 2.3300403613701319
+    kappa: -0.010156085490909668
     s: 2.4985257625722146
-    dkappa: 9.8061762385659034e-05
+    dkappa: 0.00016264853382095198
     ddkappa: 0
   }
   v: 5.9377329442802287
-  a: 1.1891822391995064
+  a: 1.1891822391995068
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587501.53073568654
-    y: 4140721.032946324
-    theta: 2.32925944458819
-    kappa: -0.010067117741273807
+    x: 587501.5312139946
+    y: 4140721.0334081287
+    theta: 2.3288371165748964
+    kappa: -0.010138998682953071
     s: 2.6175183290377366
-    dkappa: 9.4443058390375533e-05
+    dkappa: 0.00015799398408192409
     ddkappa: 0
   }
-  v: 5.961527207532499
-  a: 1.1902338415096956
+  v: 5.9615272075325
+  a: 1.1902338415096958
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587501.44867507624
-    y: 4140721.119772682
-    theta: 2.3280590898799343
-    kappa: -0.010057172181157127
+    x: 587501.44919082127
+    y: 4140721.1202698536
+    theta: 2.3276290540718487
+    kappa: -0.010121843510354254
     s: 2.7369869870151122
-    dkappa: 9.0809875891877586e-05
+    dkappa: 0.00015332081140124467
     ddkappa: 0
   }
-  v: 5.9853418929904771
-  a: 1.1912247012990369
+  v: 5.985341892990478
+  a: 1.1912247012990371
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587501.36628717626
-    y: 4140721.2069453383
-    theta: 2.3268539477017112
-    kappa: -0.010047186954242683
+    x: 587501.36684050714
+    y: 4140721.2074780175
+    theta: 2.3264161733579551
+    kappa: -0.010104619916193049
     s: 2.8569321328965618
-    dkappa: 8.7162202835411244e-05
+    dkappa: 0.00014862900027351402
     ddkappa: 0
   }
   v: 6.0091758002954734
-  a: 1.19215626774021
+  a: 1.1921562677402104
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587501.28357173025
-    y: 4140721.2944645626
-    theta: 2.325644017830796
-    kappa: -0.010037162029505936
+    x: 587501.28416279668
+    y: 4140721.2950328905
+    theta: 2.3251984742547318
+    kappa: -0.01008732784695498
     s: 2.9773541393569687
-    dkappa: 8.3500027887494692e-05
+    dkappa: 0.00014391853612106704
     ddkappa: 0
   }
   v: 6.0330277580722491
-  a: 1.1930299900058954
+  a: 1.1930299900058956
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587501.20092889608
-    y: 4140721.3827071846
-    theta: 2.3244321100753029
-    kappa: -0.010028064502249828
+    x: 587501.20156012347
+    y: 4140721.3833131068
+    theta: 2.3239820317321689
+    kappa: -0.010071421467705031
     s: 3.0982533559335446
-    dkappa: 8.1307258142826667e-05
+    dkappa: 0.0001406769139282754
     ddkappa: 0
   }
-  v: 6.0568966239290232
-  a: 1.1938473172687725
+  v: 6.0568966239290241
+  a: 1.1938473172687727
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587501.11798918038
-    y: 4140721.4713261435
-    theta: 2.3232156192453424
-    kappa: -0.010019002409937217
+    x: 587501.11866080971
+    y: 4140721.4719698611
+    theta: 2.3227612278484187
+    kappa: -0.010055559265691866
     s: 3.2196301096055002
-    dkappa: 7.921533311354946e-05
+    dkappa: 0.00013753194303867163
     ddkappa: 0
   }
-  v: 6.0807812844574656
-  a: 1.1946096987015213
+  v: 6.0807812844574665
+  a: 1.1946096987015218
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587501.03472294332
-    y: 4140721.5602939818
-    theta: 2.3219943388265114
-    kappa: -0.010009904641524726
+    x: 587501.03543513373
+    y: 4140721.5609756438
+    theta: 2.3215356173920187
+    kappa: -0.010039634616565631
     s: 3.3414847053737144
-    dkappa: 7.71151724888146e-05
+    dkappa: 0.00013437459086981053
     ddkappa: 0
   }
-  v: 6.1046806552327011
-  a: 1.1953185834768221
+  v: 6.104680655232702
+  a: 1.1953185834768223
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587500.95112999144
-    y: 4140721.6496109073
-    theta: 2.3207682696357632
-    kappa: -0.01000077117583837
+    x: 587500.951882902
+    y: 4140721.6503306623
+    theta: 2.3203052012089156
+    kappa: -0.010023647483263572
     s: 3.4638174268404036
-    dkappa: 7.5006771380745635e-05
+    dkappa: 0.00013120485007332689
     ddkappa: 0
   }
   v: 6.1285936808133075
-  a: 1.1959754207673543
+  a: 1.1959754207673547
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587500.86757647165
-    y: 4140721.7396182907
-    theta: 2.3195396902737797
-    kappa: -0.0099918755016346339
+    x: 587500.86837135046
+    y: 4140721.7403769861
+    theta: 2.3190750176340758
+    kappa: -0.010008313002805238
     s: 3.5866285367887891
-    dkappa: 7.4112139479504062e-05
+    dkappa: 0.00012925084256462497
     ddkappa: 0
   }
   v: 6.1525193347413154
-  a: 1.1965816597457983
+  a: 1.1965816597457986
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587500.78376916051
-    y: 4140721.8300433694
-    theta: 2.3183067662199717
-    kappa: -0.0099829987969730742
+    x: 587500.7846062422
+    y: 4140721.8308411604
+    theta: 2.3178410223704558
+    kappa: -0.0099930587362700649
     s: 3.7099182777627684
-    dkappa: 7.3453674047816941e-05
+    dkappa: 0.00012752959519130391
     ddkappa: 0
   }
   v: 6.1764566195422095
-  a: 1.1971387495848336
+  a: 1.1971387495848338
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587500.69963634433
-    y: 4140721.9208196565
-    theta: 2.3170690528191469
-    kappa: -0.0099740876154385781
+    x: 587500.70051579271
+    y: 4140721.9216566952
+    theta: 2.3166022335986511
+    kappa: -0.0099777452225897952
     s: 3.8336868726465827
-    dkappa: 7.2792651154787429e-05
+    dkappa: 0.00012580166254126803
     ddkappa: 0
   }
-  v: 6.20040456672493
-  a: 1.1976481394571403
+  v: 6.2004045667249308
+  a: 1.1976481394571405
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587500.61517788435
-    y: 4140722.0119473017
-    theta: 2.3158265518392533
-    kappa: -0.0099651419423575
+    x: 587500.61609986331
+    y: 4140722.01282374
+    theta: 2.3153586530947829
+    kappa: -0.0099623724365483484
     s: 3.9579345252444864
-    dkappa: 7.2129069711938452e-05
+    dkappa: 0.00012406704176920732
     ddkappa: 0
   }
-  v: 6.2243622367818681
-  a: 1.1981112785353984
+  v: 6.224362236781869
+  a: 1.1981112785353987
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587500.53077604121
-    y: 4140722.103779004
-    theta: 2.314585395953515
-    kappa: -0.0099562931847988218
+    x: 587500.53174090385
+    y: 4140722.1046948386
+    theta: 2.3141188979232767
+    kappa: -0.0099474997510544151
     s: 4.0826614208604166
-    dkappa: 7.1735171674346021e-05
+    dkappa: 0.00012260509911303147
     ddkappa: 0
   }
-  v: 6.24832871918887
-  a: 1.1985296159922876
+  v: 6.248328719188871
+  a: 1.198529615992288
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587500.44614316244
-    y: 4140722.1960497731
-    theta: 2.3133409634889488
-    kappa: -0.0099474425080933546
+    x: 587500.44715089991
+    y: 4140722.1970049157
+    theta: 2.3128764733023668
+    kappa: -0.0099327061814319731
     s: 4.2078677268776632
-    dkappa: 7.1406240911277567e-05
+    dkappa: 0.00012120559790619746
     ddkappa: 0
   }
-  v: 6.2723031324052352
-  a: 1.1989046010004882
+  v: 6.2723031324052361
+  a: 1.1989046010004885
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 587500.36118612613
-    y: 4140722.288673955
-    theta: 2.3120917639147067
-    kappa: -0.00993855793186172
-    s: 4.3335535933385332
-    dkappa: 7.1076050290078358e-05
+    x: 587500.36223690235
+    y: 4140722.2896685549
+    theta: 2.3116292892673314
+    kappa: -0.0099178559500403534
+    s: 4.3335535933385341
+    dkappa: 0.00011980073638293088
     ddkappa: 0
   }
-  v: 6.2962846238737162
-  a: 1.1992376827326796
+  v: 6.2962846238737171
+  a: 1.19923768273268
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 587500.27590484207
-    y: 4140722.3816516465
-    theta: 2.3108377998187986
-    kappa: -0.0099296394466824485
+    x: 587500.27699882141
+    y: 4140722.3826858555
+    theta: 2.3103773483894297
+    kappa: -0.0099029490411319358
     s: 4.4597191535240261
-    dkappa: 7.0744599460604513e-05
+    dkappa: 0.0001183905130534752
     ddkappa: 0
   }
-  v: 6.32027237002052
-  a: 1.1995303103615422
+  v: 6.320272370020521
+  a: 1.1995303103615425
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 587500.19068878808
-    y: 4140722.4753384553
-    theta: 2.3095836797924
-    kappa: -0.0099200739122997528
-    s: 4.5863645245335
-    dkappa: 7.1917914348731247e-05
+    x: 587500.19182544155
+    y: 4140722.4764114637
+    theta: 2.3091276465901172
+    kappa: -0.00988781767187473
+    s: 4.5863645245335007
+    dkappa: 0.00011848989403154106
     ddkappa: 0
   }
   v: 6.3442655762553075
-  a: 1.1997839330597555
+  a: 1.1997839330597559
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 587500.105228703
-    y: 4140722.5694522769
-    theta: 2.3083257368964736
-    kappa: -0.00991034796655565
+    x: 587500.10640778136
+    y: 4140722.5705637923
+    theta: 2.3078746172976206
+    kappa: -0.0098725951057501982
     s: 4.713489807864339
-    dkappa: 7.3400656346805433e-05
+    dkappa: 0.00011889539002085657
     ddkappa: 0
   }
   v: 6.3682634769711912
-  a: 1.1999999999999997
+  a: 1.2000000000000002
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 587500.01944593829
-    y: 4140722.663921454
-    theta: 2.3070630437409623
-    kappa: -0.0099005852976505118
+    x: 587500.02066760184
+    y: 4140722.6650716206
+    theta: 2.3066168563065781
+    kappa: -0.0098573150623620757
     s: 4.841095089991625
-    dkappa: 7.4888996872221047e-05
+    dkappa: 0.00011930241707922254
     ddkappa: 0
   }
   v: 6.3922653355447387
-  a: 1.2001799603549548
+  a: 1.2001799603549552
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 587499.93334044551
-    y: 4140722.7587460377
-    theta: 2.3057956036563234
-    kappa: -0.0098907859000733674
+    x: 587499.93460485432
+    y: 4140722.7599350023
+    theta: 2.3053543669030616
+    kappa: -0.0098419775330848731
     s: 4.9691804429478053
-    dkappa: 7.6382936765137779e-05
+    dkappa: 0.00011971097543640335
     ddkappa: 0
   }
   v: 6.4162704443359715
-  a: 1.2003252632973005
+  a: 1.2003252632973009
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 587499.84733296791
-    y: 4140722.8543063221
-    theta: 2.3045265056289486
-    kappa: -0.0098794528470516358
+    x: 587499.848638562
+    y: 4140722.8555323416
+    theta: 2.3040926621036428
+    kappa: -0.0098255792668446446
     s: 5.0977459249023642
-    dkappa: 7.95067147103369e-05
+    dkappa: 0.00012175523679353289
     ddkappa: 0
   }
   v: 6.4402781246883647
-  a: 1.2004373579997167
+  a: 1.2004373579997172
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 587499.76104228478
-    y: 4140722.950257869
-    theta: 2.3032529429494657
-    kappa: -0.0098679422103392159
+    x: 587499.762388766
+    y: 4140722.9515206427
+    theta: 2.3028267354582903
+    kappa: -0.0098090298162574526
     s: 5.2267915807414873
-    dkappa: 8.2788919637670087e-05
+    dkappa: 0.00012395363389854113
     ddkappa: 0
   }
   v: 6.4642877269288457
-  a: 1.2005176936348836
+  a: 1.2005176936348838
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 587499.67443049583
-    y: 4140723.0465664715
-    theta: 2.3019746409997022
-    kappa: -0.009856388740123026
+    x: 587499.67581801629
+    y: 4140723.0478661368
+    theta: 2.3015560979619676
+    kappa: -0.0097924187816787051
     s: 5.3563174426477342
-    dkappa: 8.6083338340890578e-05
+    dkappa: 0.00012616021170224015
     ddkappa: 0
   }
   v: 6.4882986303677965
-  a: 1.2005677193754807
+  a: 1.2005677193754811
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 587499.58752456564
-    y: 4140723.14325629
-    theta: 2.3006918600858497
-    kappa: -0.0098446658965169846
+    x: 587499.58895342366
+    y: 4140723.144593094
+    theta: 2.300281153793426
+    kappa: -0.0097756498065536927
     s: 5.4863235306797069
-    dkappa: 8.9451685956198277e-05
+    dkappa: 0.0001284379731274126
     ddkappa: 0
   }
   v: 6.5123102432990523
-  a: 1.2005888843941883
+  a: 1.2005888843941885
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 587499.50073735591
-    y: 4140723.2406968391
-    theta: 2.2994085182461834
-    kappa: -0.0098308371620034821
+    x: 587499.50220438361
+    y: 4140723.2420676262
+    theta: 2.2990079399051369
+    kappa: -0.0097572685531483079
     s: 5.61680985335172
-    dkappa: 9.3838432472082239e-05
+    dkappa: 0.00013173785942209327
     ddkappa: 0
   }
   v: 6.5363220029999027
-  a: 1.2005826378636861
+  a: 1.2005826378636864
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 587499.413630741
-    y: 4140723.338496001
-    theta: 2.2981204496620422
-    kappa: -0.0098169575332405223
+    x: 587499.41513607884
+    y: 4140723.339900896
+    theta: 2.2977300366349738
+    kappa: -0.0097388196507403914
     s: 5.7477764082134648
-    dkappa: 9.8241323644677976e-05
+    dkappa: 0.00013504989037369051
     ddkappa: 0
   }
-  v: 6.5603333757310889
-  a: 1.200550428956654
+  v: 6.56033337573109
+  a: 1.2005504289566544
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 587499.32620472938
-    y: 4140723.4366537668
-    theta: 2.2968276587173242
-    kappa: -0.009803027011588368
+    x: 587499.32774851774
+    y: 4140723.4380928949
+    theta: 2.2964474482658641
+    kappa: -0.009720303101138009
     s: 5.8792231824296861
-    dkappa: 0.00010266035904248424
+    dkappa: 0.00013837406565761186
     ddkappa: 0
   }
   v: 6.58434385673681
-  a: 1.2004937068457722
+  a: 1.2004937068457724
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 587499.23858467734
-    y: 4140723.5352811916
-    theta: 2.2955326644633396
-    kappa: -0.0097884679754729042
+    x: 587499.24016643525
+    y: 4140723.5367540452
+    theta: 2.2951632482482629
+    kappa: -0.0097012770335787677
     s: 6.0111501533598437
-    dkappa: 0.00010713534145735314
+    dkappa: 0.0001417523364069337
     ddkappa: 0
   }
-  v: 6.6083529702447121
-  a: 1.20041392070372
+  v: 6.608352970244713
+  a: 1.2004139207037205
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587498.79832658451
-    y: 4140724.0361218108
-    theta: 2.2890400260039749
-    kappa: -0.0097017689988660866
+    x: 587498.8000757324
+    y: 4140724.0377418022
+    theta: 2.2887361824352079
+    kappa: -0.009594888067861778
     s: 6.67798655296944
-    dkappa: 0.00013224623854591708
+    dkappa: 0.00016136070210558796
     ddkappa: 0
   }
   v: 6.7283634021264653
-  a: 1.1997197505697079
+  a: 1.1997197505697084
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587498.35325511359
-    y: 4140724.5486874324
-    theta: 2.2824912918752505
-    kappa: -0.0095924451515383632
-    s: 7.3568198507306679
-    dkappa: 0.0001666422309848239
+    x: 587498.35514091514
+    y: 4140724.550426112
+    theta: 2.2822656719080787
+    kappa: -0.0094690197620547652
+    s: 7.3568198507306688
+    dkappa: 0.00019019492872551853
     ddkappa: 0
   }
   v: 6.8482849461960793
-  a: 1.198666355558434
+  a: 1.1986663555584345
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587497.90370249026
-    y: 4140725.0732171983
-    theta: 2.2759022564898728
-    kappa: -0.0094529879116260814
+    x: 587497.905684492
+    y: 4140725.0750383865
+    theta: 2.2757664547381067
+    kappa: -0.0093165819194521785
     s: 8.0476396636489049
-    dkappa: 0.00020837568648049786
+    dkappa: 0.00022629729574324793
     ddkappa: 0
   }
   v: 6.9680907372950767
-  a: 1.1974348822548881
+  a: 1.1974348822548886
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587497.45012968825
-    y: 4140725.6100494135
-    theta: 2.2693284225243144
-    kappa: -0.009278845258677719
+    x: 587497.45215622487
+    y: 4140725.611908298
+    theta: 2.2692921070748069
+    kappa: -0.0091334056526925847
     s: 8.7504338279466012
-    dkappa: 0.00025346541515385186
+    dkappa: 0.00026567769016548631
     ddkappa: 0
   }
-  v: 7.08777202492348
-  a: 1.1962064772440604
+  v: 7.0877720249234812
+  a: 1.1962064772440606
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587496.99207085674
-    y: 4140726.1587356222
-    theta: 2.2627592734823008
-    kappa: -0.00906704928458471
+    x: 587496.99409134989
+    y: 4140726.1605895278
+    theta: 2.2628288774092655
+    kappa: -0.0089161760938968257
     s: 9.4651902105291352
-    dkappa: 0.00030783878998521223
+    dkappa: 0.00031424904101644513
     ddkappa: 0
   }
   v: 7.2073381732398127
-  a: 1.1951622871109402
+  a: 1.1951622871109406
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587496.53041005193
-    y: 4140726.7199590271
-    theta: 2.2562714371248385
-    kappa: -0.0088051634409955139
+    x: 587496.53235761332
+    y: 4140726.7217530091
+    theta: 2.256451291740706
+    kappa: -0.0086534787551831669
     s: 10.191898520450664
-    dkappa: 0.00036638464201105015
+    dkappa: 0.00036689049048212337
     ddkappa: 0
   }
-  v: 7.3268166610610921
-  a: 1.1944834584405177
+  v: 7.326816661061093
+  a: 1.194483458440518
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 587496.06453596835
-    y: 4140727.293167667
-    theta: 2.2498730224973769
-    kappa: -0.00849682360367121
+    x: 587496.06635047181
+    y: 4140727.2948535536
+    theta: 2.2501641042400284
+    kappa: -0.00834832271227182
     s: 10.930552120379971
-    dkappa: 0.00043090532794797052
+    dkappa: 0.00042540170832504662
     ddkappa: 0
   }
-  v: 7.4462530818628387
-  a: 1.1943511378177827
+  v: 7.44625308186284
+  a: 1.1943511378177829
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587495.59505200689
-    y: 4140727.8788116304
-    theta: 2.2436364714926467
-    kappa: -0.0081272841843057116
+    x: 587495.59666204907
+    y: 4140727.8803336364
+    theta: 2.2440360527744927
+    kappa: -0.0079869033625992181
     s: 11.681149838066318
-    dkappa: 0.0005014798111197
+    dkappa: 0.00048985202406128729
     ddkappa: 0
   }
-  v: 7.5657111437790716
+  v: 7.5657111437790725
   a: 1.194946471827725
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587495.12152667809
-    y: 4140728.4765163818
-    theta: 2.2375894045510467
-    kappa: -0.0077007119897367308
+    x: 587495.12286871334
+    y: 4140728.4778260267
+    theta: 2.2380918948628765
+    kappa: -0.0075729494084616706
     s: 12.443697777805289
-    dkappa: 0.00057560431755036812
+    dkappa: 0.00055774273488336408
     ddkappa: 0
   }
   v: 7.6852726696023073
@@ -846,460 +846,460 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587494.644414651
-    y: 4140729.0866251625
-    theta: 2.2318265193513143
-    kappa: -0.007200930370571651
-    s: 13.218211131904651
-    dkappa: 0.00065635870389444762
+    x: 587494.64541951672
+    y: 4140729.0876710489
+    theta: 2.2324207500658457
+    kappa: -0.0070912367766675314
+    s: 13.218211131904647
+    dkappa: 0.00063215114777240094
     ddkappa: 0
   }
-  v: 7.8050375967835617
-  a: 1.1990446900856007
+  v: 7.805037596783559
+  a: 1.1990446900856011
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587494.163157972
-    y: 4140729.7086994573
-    theta: 2.2263673942514535
-    kappa: -0.0066340231630515369
-    s: 14.004713873268914
-    dkappa: 0.00073924881651675619
+    x: 587494.16377012886
+    y: 4140729.70944138
+    theta: 2.2270399320638981
+    kappa: -0.0065473153374266168
+    s: 14.004713873268908
+    dkappa: 0.00070859034992552458
     ddkappa: 0
   }
-  v: 7.9250392538041581
-  a: 1.2003697398022612
+  v: 7.9250392538041554
+  a: 1.2003697398022615
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587493.67809390672
-    y: 4140730.3429857492
-    theta: 2.2213380412701849
-    kappa: -0.0059832859984550544
-    s: 14.803217309767767
-    dkappa: 0.00082879102777600833
+    x: 587493.67825858574
+    y: 4140730.3433852438
+    theta: 2.2220678262133293
+    kappa: -0.0059254943341584985
+    s: 14.803217309767762
+    dkappa: 0.00079157870147214353
     ddkappa: 0
   }
-  v: 8.0449914542067535
+  v: 8.04499145420675
   a: 1.1980920734011029
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587493.18875120231
-    y: 4140730.9890678041
-    theta: 2.2167805056317085
-    kappa: -0.00525284990958235
-    s: 15.613698847315577
-    dkappa: 0.000919015094557156
+    x: 587493.18843262747
+    y: 4140730.9891010192
+    theta: 2.2175436590839666
+    kappa: -0.0052295458712045585
+    s: 15.613698847315574
+    dkappa: 0.00087515373128590362
     ddkappa: 0
   }
-  v: 8.16454488108222
+  v: 8.1645448810822181
   a: 1.1924307849351905
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587492.69495985343
-    y: 4140731.6467274148
-    theta: 2.2127714687419209
-    kappa: -0.0044352958600208667
-    s: 16.436102055606064
-    dkappa: 0.0010153831040088258
+    x: 587492.69413614529
+    y: 4140731.6463810592
+    theta: 2.2135383090731295
+    kappa: -0.0044523963004241083
+    s: 16.436102055606057
+    dkappa: 0.00096478021900840488
     ddkappa: 0
   }
-  v: 8.2833721269267357
+  v: 8.2833721269267322
   a: 1.1836049684575891
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 587492.19666130212
-    y: 4140732.3157929983
-    theta: 2.2094594499281772
-    kappa: -0.003573894203067484
-    s: 17.270338859052828
-    dkappa: 0.001022426008353854
+    x: 587492.195334726
+    y: 4140732.3150718166
+    theta: 2.2101926690624802
+    kappa: -0.0036341483820485896
+    s: 17.270338859052821
+    dkappa: 0.00097148150109224534
     ddkappa: 0
   }
-  v: 8.4011676936417832
+  v: 8.4011676936417814
   a: 1.1718337180213636
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587491.69330162066
-    y: 4140732.9956934103
-    theta: 2.206775701310479
-    kappa: -0.0027686874719571263
-    s: 18.11629172772988
-    dkappa: 0.00093224135308261034
+    x: 587491.69149759959
+    y: 4140732.9946184685
+    theta: 2.2074415313492883
+    kappa: -0.0028673769258912149
+    s: 18.116291727729877
+    dkappa: 0.00088833920461902824
     ddkappa: 0
   }
-  v: 8.5176479925341564
+  v: 8.5176479925341546
   a: 1.1573361276795786
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 587491.18467037589
-    y: 4140733.6860857881
-    theta: 2.2047039865794318
-    kappa: -0.0020312022646593636
-    s: 18.97381586831219
-    dkappa: 0.00083690249365184612
+    x: 587491.182436523
+    y: 4140733.6846939526
+    theta: 2.2052715229538071
+    kappa: -0.0021627667905613161
+    s: 18.973815868312183
+    dkappa: 0.00080033652400794577
     ddkappa: 0
   }
-  v: 8.63255134431595
+  v: 8.6325513443159476
   a: 1.1403312914852992
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 587490.67058073718
-    y: 4140734.3866165327
-    theta: 2.2032481969868574
-    kappa: -0.0013709684434609108
-    s: 19.842741415016185
-    dkappa: 0.0007427660576673189
+    x: 587490.66799535765
+    y: 4140734.3849665746
+    theta: 2.2036887054995189
+    kappa: -0.0015290142184950521
+    s: 19.842741415016178
+    dkappa: 0.0007136345425329065
     ddkappa: 0
   }
-  v: 8.7456379791045684
-  a: 1.1210383034915898
+  v: 8.7456379791045666
+  a: 1.1210383034915903
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 587490.150682455
-    y: 4140735.0967868646
-    theta: 2.2023114648924125
-    kappa: -0.00078558388906051538
-    s: 20.722875620540307
-    dkappa: 0.00064502485751051942
+    x: 587490.14783703524
+    y: 4140735.0949464422
+    theta: 2.2026032860994409
+    kappa: -0.0009640169406340349
+    s: 20.7228756205403
+    dkappa: 0.00062342264971017845
     ddkappa: 0
   }
-  v: 8.856690036422723
-  a: 1.099676257751516
+  v: 8.85669003642272
+  a: 1.0996762577515165
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 587489.62478007539
-    y: 4140735.8161882265
-    theta: 2.201839255575933
-    kappa: -0.00027774602984552432
-    s: 21.614005047005531
-    dkappa: 0.00054758005897817725
+    x: 587489.62178267457
+    y: 4140735.8142366633
+    theta: 2.2019654499602259
+    kappa: -0.000470183068206723
+    s: 21.614005047005527
+    dkappa: 0.00053359876196013511
     ddkappa: 0
   }
-  v: 8.96551156519843
-  a: 1.0764642483181419
+  v: 8.9655115651984261
+  a: 1.0764642483181428
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587489.092718901
-    y: 4140736.5444198968
-    theta: 2.2017862734809182
-    kappa: 0.00017255496683028613
-    s: 22.515897756895903
-    dkappa: 0.0004943765024799456
+    x: 587489.08969124162
+    y: 4140736.5424462231
+    theta: 2.2017352154918552
+    kappa: -2.7202528813507081e-05
+    s: 22.5158977568959
+    dkappa: 0.00048812785364283338
     ddkappa: 0
   }
-  v: 9.07192852376501
-  a: 1.0516213692445329
+  v: 9.071928523765008
+  a: 1.051621369244534
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587488.55434249854
-    y: 4140737.2810600656
-    theta: 2.2021443573578177
-    kappa: 0.00060304429602311407
-    s: 23.42830550399907
-    dkappa: 0.00045586034885931941
+    x: 587488.55142002122
+    y: 4140737.2791632572
+    theta: 2.2019103487122638
+    kappa: 0.00040309967937517225
+    s: 23.428305503999063
+    dkappa: 0.00045740185706188291
     ddkappa: 0
   }
-  v: 9.1757887798610973
-  a: 1.0253667145837537
+  v: 9.1757887798610938
+  a: 1.0253667145837548
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587488.00949440652
-    y: 4140738.0256692013
-    theta: 2.2028821923393007
-    kappa: 0.00097184642270755684
-    s: 24.350965924346792
-    dkappa: 0.00038467383604562616
+    x: 587488.00681626482
+    y: 4140738.02395113
+    theta: 2.2024665380261612
+    kappa: 0.0007790448396760584
+    s: 24.350965924346781
+    dkappa: 0.00039410603731251904
     ddkappa: 0
   }
-  v: 9.2769621106306239
-  a: 0.997919378388869
+  v: 9.2769621106306221
+  a: 0.99791937838887035
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587487.45808071864
-    y: 4140738.77783804
-    theta: 2.20393842817128
-    kappa: 0.0012795593095098908
-    s: 25.2836047271555
-    dkappa: 0.00031398665227998755
+    x: 587487.45578229672
+    y: 4140738.7763982173
+    theta: 2.2033492812032036
+    kappa: 0.001101319749259996
+    s: 25.283604727155492
+    dkappa: 0.00033139396547951684
     ddkappa: 0
   }
-  v: 9.3753402026228354
-  a: 0.96949845471294371
+  v: 9.3753402026228336
+  a: 0.96949845471294527
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 587486.900027053
-    y: 4140739.5371585828
-    theta: 2.2052656562764597
-    kappa: 0.0015243021426221779
-    s: 26.225937885766818
-    dkappa: 0.00024342252482745068
+    x: 587486.89823761955
+    y: 4140739.5360926026
+    theta: 2.2045184788369392
+    kappa: 0.0013682914331240785
+    s: 26.225937885766811
+    dkappa: 0.00026888802243077686
     ddkappa: 0
   }
-  v: 9.47083665179228
-  a: 0.94032303760904279
+  v: 9.4708366517922791
+  a: 0.94032303760904457
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587486.33531004679
-    y: 4140740.3032497792
-    theta: 2.2068067456305331
-    kappa: 0.0017050275786735436
-    s: 27.177673828588077
-    dkappa: 0.00017307934710318742
+    x: 587486.33414648427
+    y: 4140740.30264479
+    theta: 2.20592478812552
+    kappa: 0.0015791499346518908
+    s: 27.17767382858807
+    dkappa: 0.00020668223667257016
     ddkappa: 0
   }
-  v: 9.5633869634988145
-  a: 0.910612221130231
+  v: 9.5633869634988127
+  a: 0.91061222113023288
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587485.76393030991
-    y: 4140741.07574045
-    theta: 2.2085058422862884
-    kappa: 0.001820051833458487
-    s: 28.138515630032874
-    dkappa: 0.00010282738494767901
+    x: 587485.763490298
+    y: 4140741.0756701427
+    theta: 2.2075205894881362
+    kappa: 0.001732439183061746
+    s: 28.138515630032863
+    dkappa: 0.00014464562536518736
     ddkappa: 0
   }
-  v: 9.6529485525075991
-  a: 0.88058509932957341
+  v: 9.6529485525075973
+  a: 0.88058509932957529
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587485.18595683109
-    y: 4140741.8543049684
-    theta: 2.2102990262836424
-    kappa: 0.0018685146430224566
-    s: 29.108163201461572
-    dkappa: 3.2772721568950407e-05
+    x: 587485.18631152273
+    y: 4140741.8548240024
+    theta: 2.2092506253485094
+    kappa: 0.0018275157150383376
+    s: 29.108163201461565
+    dkappa: 8.2880660452017911e-05
     ddkappa: 0
   }
-  v: 9.7395007429891027
-  a: 0.85046076626013445
+  v: 9.739500742989101
+  a: 0.85046076626013667
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587484.60148282535
-    y: 4140742.6386337765
-    theta: 2.2121231138889477
-    kappa: 0.0018489674514489786
-    s: 30.086315482121861
-    dkappa: -3.7128473449836066e-05
+    x: 587484.60266908933
+    y: 4140742.639771807
+    theta: 2.2110607684032839
+    kappa: 0.001863138426278848
+    s: 30.086315482121854
+    dkappa: 2.1342304022850569e-05
     ddkappa: 0
   }
-  v: 9.823044768519102
-  a: 0.82045831597497942
+  v: 9.8230447685191
+  a: 0.82045831597498142
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 587484.01068184862
-    y: 4140743.4284782554
-    theta: 2.2139088428030154
-    kappa: 0.0017607309094975687
-    s: 31.072672630089258
-    dkappa: -0.00010677369842190315
+    x: 587484.0126940927
+    y: 4140743.430233432
+    theta: 2.2128911881161226
+    kappa: 0.0018388251796898022
+    s: 31.072672630089251
+    dkappa: -3.9869667786112236e-05
     ddkappa: 0
   }
   v: 9.9036037720786769
-  a: 0.790796842527173
+  a: 0.79079684252717442
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587483.41374999273
-    y: 4140744.2236115141
-    theta: 2.2155869473819814
-    kappa: 0.0016025965677012454
-    s: 32.066938596175838
-    dkappa: -0.00017623273763082363
+    x: 587483.41653161694
+    y: 4140744.2259436822
+    theta: 2.2146823966500051
+    kappa: 0.0017535562094977732
+    s: 32.066938596175831
+    dkappa: -0.00010082590623284845
     ddkappa: 0
   }
   v: 9.9812379551250885
-  a: 0.76214142929317141
+  a: 0.76214142929317263
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587482.8109760636
-    y: 4140745.0238891239
-    theta: 2.2170836495980653
-    kappa: 0.0013744627365832
-    s: 33.068827256070982
-    dkappa: -0.00024467196510124281
+    x: 587482.8144105532
+    y: 4140745.0267125778
+    theta: 2.21637071701346
+    kappa: 0.0016073179212488025
+    s: 33.068827256070975
+    dkappa: -0.00016087860598482611
     ddkappa: 0
   }
   v: 10.056083238562186
-  a: 0.73502143480544524
+  a: 0.73502143480544646
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 587482.20265416813
-    y: 4140745.82919033
-    theta: 2.2183343145206651
-    kappa: 0.0011066137739719473
-    s: 34.078067422752127
-    dkappa: -0.00026139029018076658
+    x: 587482.20655644836
+    y: 4140745.8323668889
+    theta: 2.2178986351956254
+    kappa: 0.001419663068326344
+    s: 34.078067422752113
+    dkappa: -0.00018712941370392487
     ddkappa: 0
   }
   v: 10.128293939603955
-  a: 0.70945236585108151
+  a: 0.70945236585108262
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587481.589157842
-    y: 4140746.6394748529
-    theta: 2.219333916610744
-    kappa: 0.00086743847168114011
-    s: 35.094403418452032
-    dkappa: -0.00022649728031230422
+    x: 587481.59327515494
+    y: 4140746.6428141221
+    theta: 2.2192467048463791
+    kappa: 0.0012352692583579314
+    s: 35.094403418452025
+    dkappa: -0.00017944929892512015
     ddkappa: 0
   }
   v: 10.198025926143085
-  a: 0.68544972921716718
+  a: 0.68544972921716818
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587480.97078934521
-    y: 4140747.4546691962
-    theta: 2.2201130981153483
-    kappa: 0.00066302393035215046
+    x: 587480.97481874877
+    y: 4140747.4579417193
+    theta: 2.2204186313367029
+    kappa: 0.0010580530485399678
     s: 36.11759522972671
-    dkappa: -0.00019060137905316531
+    dkappa: -0.00017095255993324163
     ddkappa: 0
   }
   v: 10.265436616750973
-  a: 0.66302903169078919
+  a: 0.66302903169079008
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587480.3478610632
-    y: 4140748.2747281082
-    theta: 2.220705941870798
-    kappa: 0.00049556407982972517
-    s: 37.147418662523215
-    dkappa: -0.00015294601651131358
+    x: 587480.35147146764
+    y: 4140748.2776821377
+    theta: 2.2214202497136628
+    kappa: 0.0008896576306705044
+    s: 37.147418662523208
+    dkappa: -0.00016087784492748743
     ddkappa: 0
   }
-  v: 10.330684980677729
-  a: 0.64220578005903428
+  v: 10.330684980677731
+  a: 0.642205780059035
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587479.72062057874
-    y: 4140749.0995777007
-    theta: 2.2211490408782137
-    kappa: 0.00036700129571813611
-    s: 38.183665497247532
-    dkappa: -0.00011399779350599405
+    x: 587479.72347618034
+    y: 4140749.1019571703
+    theta: 2.2222588320476975
+    kappa: 0.000731497930988002
+    s: 38.183665497247524
+    dkappa: -0.00014968562401774756
     ddkappa: 0
   }
-  v: 10.393931537852168
-  a: 0.6229954811089895
+  v: 10.39393153785217
+  a: 0.62299548110899006
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 587479.08928681281
-    y: 4140749.929142857
-    theta: 2.2214820798956305
-    kappa: 0.00027955316654406227
-    s: 39.2261436438325
-    dkappa: -7.3185898888485535e-05
+    x: 587479.09107186738
+    y: 4140749.9307064768
+    theta: 2.2229437264072787
+    kappa: 0.00058528674131109841
+    s: 39.226143643832494
+    dkappa: -0.00013679828563012067
     ddkappa: 0
   }
-  v: 10.45533835888182
-  a: 0.60541364162774147
+  v: 10.455338358881821
+  a: 0.60541364162774192
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 587478.45404743543
-    y: 4140750.7633453179
-    theta: 2.2217486135994116
-    kappa: 0.00023567980685618154
-    s: 40.274677296805628
-    dkappa: -3.053530667027944e-05
+    x: 587478.4544922628
+    y: 4140750.7638866436
+    theta: 2.2234871845739068
+    kappa: 0.00045300031240964926
+    s: 40.274677296805621
+    dkappa: -0.00012223403132173241
     ddkappa: 0
   }
-  v: 10.515069065052913
-  a: 0.58947576840237725
+  v: 10.515069065052915
+  a: 0.5894757684023777
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587477.81501573964
-    y: 4140751.6020709421
-    theta: 2.221993700747984
-    kappa: 0.0002377921779445428
+    x: 587477.81392400782
+    y: 4140751.6014393144
+    theta: 2.2239020415066593
+    kappa: 0.00033658480950437647
     s: 41.32910709035697
-    dkappa: 1.4021994855520637e-05
+    dkappa: -0.00010591846521341495
     ddkappa: 0
   }
   v: 10.573288828330394
-  a: 0.57519736821998391
+  a: 0.57519736821998424
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 587477.17228666274
-    y: 4140752.4452124871
-    theta: 2.2222679461876393
-    kappa: 0.00028872488352413934
-    s: 42.389290253407054
-    dkappa: 6.0890035714661868e-05
+    x: 587477.16956376866
+    y: 4140752.4433349203
+    theta: 2.2242057957574684
+    kappa: 0.00023842620414987677
+    s: 42.389290253407047
+    dkappa: -8.7442566176924565e-05
     ddkappa: 0
   }
   v: 10.630164371357912
-  a: 0.56259394786764816
+  a: 0.56259394786764849
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 587476.52588954091
-    y: 4140753.2926337491
-    theta: 2.2226249646089427
-    kappa: 0.00039139466058921792
+    x: 587476.52157214808
+    y: 4140753.289537556
+    theta: 2.224418102133991
+    kappa: 0.00016100523283188084
     s: 43.455100764674654
-    dkappa: 0.00011009326576909468
+    dkappa: -6.6778561736506557e-05
     ddkappa: 0
   }
   v: 10.685863967457829
@@ -1308,72 +1308,72 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587475.87576972949
-    y: 4140754.1441556117
-    theta: 2.2231250468750168
-    kappa: 0.00055025762626842727
-    s: 44.526429507744751
-    dkappa: 0.00016197000832061787
+    x: 587475.870065219
+    y: 4140754.139998605
+    theta: 2.2245590255431544
+    kappa: 0.00010766332126867787
+    s: 44.526429507744744
+    dkappa: -4.3586582625693485e-05
     ddkappa: 0
   }
   v: 10.740557440631211
-  a: 0.542474073801497
+  a: 0.54247407380149715
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 587475.22184511635
-    y: 4140754.9995991793
-    theta: 2.22383195575212
-    kappa: 0.00076821574355562163
-    s: 45.603184426136359
-    dkappa: 0.00021665851568090184
+    x: 587475.215161226
+    y: 4140754.9946926516
+    theta: 2.22465778261561
+    kappa: 8.1228324720743772e-05
+    s: 45.603184426136352
+    dkappa: -1.7729157511413474e-05
     ddkappa: 0
   }
-  v: 10.794416165557836
+  v: 10.794416165557838
   a: 0.53498863366185567
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 587474.56395595532
-    y: 4140755.8587474008
-    theta: 2.2248110614138947
-    kappa: 0.0010484327585397516
-    s: 46.6852906783704
-    dkappa: 0.00027432744717642858
+    x: 587474.55692837713
+    y: 4140755.8535776595
+    theta: 2.2247449325348234
+    kappa: 8.4712231689248812e-05
+    s: 46.685290678370386
+    dkappa: 1.0961688567848998e-05
     ddkappa: 0
   }
-  v: 10.847613067596194
+  v: 10.847613067596196
   a: 0.52924020050061926
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 587473.90186403284
-    y: 4140756.7213440952
-    theta: 2.2261332518471844
-    kappa: 0.0013948905917785372
-    s: 47.772690793037583
-    dkappa: 0.00033532839752434149
+    x: 587473.89539191732
+    y: 4140756.7166005559
+    theta: 2.2248539204386582
+    kappa: 0.00012167909908598287
+    s: 47.772690793037569
+    dkappa: 4.2837925493331891e-05
     ddkappa: 0
   }
   v: 10.900322622783477
-  a: 0.525244281104875
+  a: 0.52524428110487487
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 587473.23525985656
-    y: 4140757.5870988667
-    theta: 2.2278751254628544
-    kappa: 0.0018118201192002714
-    s: 48.865344823866273
-    dkappa: 0.00039970933328465345
+    x: 587473.23054017348
+    y: 4140757.5837020418
+    theta: 2.2250230816393066
+    kappa: 0.00019594400446314302
+    s: 48.865344823866259
+    dkappa: 7.7951348685446035e-05
     ddkappa: 0
   }
-  v: 10.952720857835589
-  a: 0.52301638226170988
+  v: 10.95272085783559
+  a: 0.52301638226170966
   relative_time: 5.6999999999999966
 }
 decision {
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_follow_02_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_follow_02_0.pb.txt
index 8bdaf90911..0654160249 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_follow_02_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_follow_02_0.pb.txt
@@ -8,9 +8,9 @@ trajectory_point {
     x: 587713.447347739
     y: 4141426.4113659179
     theta: -1.7821759466983953
-    kappa: -0.0050528654901513519
+    kappa: -0.0051517527438800512
     s: 1.7574917215990027e-16
-    dkappa: 2.76829987571959e-19
+    dkappa: 2.8715823946510174e-19
     ddkappa: 0
   }
   v: 6.0611109733581525
@@ -19,12 +19,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587713.42171447235
-    y: 4141426.2926393147
-    theta: -1.7827641080302681
-    kappa: -0.0049568215216868883
+    x: 587713.42171167373
+    y: 4141426.2926399186
+    theta: -1.7827752145296731
+    kappa: -0.0050521253487241669
     s: 0.12146221946716322
-    dkappa: 0.00019132030206643123
+    dkappa: 0.00019845827252023052
     ddkappa: 0
   }
   v: 6.0851109733581525
@@ -33,12 +33,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587713.395979907
-    y: 4141426.1734435214
-    theta: -1.7833545941212472
-    kappa: -0.0048603980022388742
+    x: 587713.39597429847
+    y: 4141426.1734447326
+    theta: -1.7833768510364059
+    kappa: -0.0049521042414338873
     s: 0.24340443893432628
-    dkappa: 0.00038339667252511409
+    dkappa: 0.00039770082159351242
     ddkappa: 0
   }
   v: 6.1091109733581526
@@ -47,12 +47,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587713.37014404288
-    y: 4141426.0537785394
-    theta: -1.7839474045622838
-    kappa: -0.0047635949318073105
+    x: 587713.37013561348
+    y: 4141426.0537803592
+    theta: -1.7839808557859314
+    kappa: -0.0048516894220092116
     s: 0.36582665840148926
-    dkappa: 0.00057622911137604873
+    dkappa: 0.00059772764721984591
     ddkappa: 0
   }
   v: 6.1331109733581526
@@ -61,12 +61,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587713.34420688008
-    y: 4141425.9336443678
-    theta: -1.7845425389345662
-    kappa: -0.0046664123103921963
+    x: 587713.34419561888
+    y: 4141425.9336467995
+    theta: -1.7845872283352613
+    kappa: -0.0047508808904501414
     s: 0.48872887786865238
-    dkappa: 0.0007698176186192356
+    dkappa: 0.00079853874939923115
     ddkappa: 0
   }
   v: 6.1571109733581526
@@ -75,12 +75,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587713.31791536266
-    y: 4141425.8130959542
-    theta: -1.7851011519623456
-    kappa: -0.0045885536144512351
+    x: 587713.31789672025
+    y: 4141425.8130999967
+    theta: -1.7851555228788958
+    kappa: -0.0046695012628879583
     s: 0.61211109733581548
-    dkappa: 0.0007514402144334349
+    dkappa: 0.00078070514164769122
     ddkappa: 0
   }
   v: 6.1811109733581526
@@ -89,12 +89,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587713.291490928
-    y: 4141425.6920852158
-    theta: -1.7856572360704017
-    kappa: -0.0045127772560987905
+    x: 587713.29146432481
+    y: 4141425.6920909975
+    theta: -1.7857211321780151
+    kappa: -0.0045902048220102963
     s: 0.73597331680297862
-    dkappa: 0.00070723992379029689
+    dkappa: 0.00073623746193967872
     ddkappa: 0
   }
   v: 6.2051109733581526
@@ -103,12 +103,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587713.26496409148
-    y: 4141425.5706055285
-    theta: -1.7862154752029593
-    kappa: -0.0044367072436186736
+    x: 587713.26492949645
+    y: 4141425.5706130555
+    theta: -1.7862889334167871
+    kappa: -0.0045106010857217738
     s: 0.86031553627014157
-    dkappa: 0.00066286834492384138
+    dkappa: 0.00069159745780254106
     ddkappa: 0
   }
   v: 6.2291109733581527
@@ -117,12 +117,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587713.238334853
-    y: 4141425.448656891
-    theta: -1.7867758690106104
-    kappa: -0.0043603435770108844
+    x: 587713.23829223542
+    y: 4141425.4486661702
+    theta: -1.7868589262275389
+    kappa: -0.00443069005402239
     s: 0.98513775573730467
-    dkappa: 0.00061832547783406818
+    dkappa: 0.00064678512923627834
     ddkappa: 0
   }
   v: 6.2531109733581527
@@ -131,12 +131,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587713.21137474023
-    y: 4141425.3262894563
-    theta: -1.787309668724423
-    kappa: -0.0042998320691665691
+    x: 587713.21132041758
+    y: 4141425.3263013158
+    theta: -1.7874008710184317
+    kappa: -0.0043667487500948805
     s: 1.1104399752044676
-    dkappa: 0.000579555931905518
+    dkappa: 0.00060771350546853179
     ddkappa: 0
   }
   v: 6.2771109733581527
@@ -145,12 +145,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587713.18427201593
-    y: 4141425.2034618985
-    theta: -1.7878405641274158
-    kappa: -0.0042418684628269714
+    x: 587713.18420530972
+    y: 4141425.2034764914
+    theta: -1.7879396853914205
+    kappa: -0.0043053651530694176
     s: 1.2362221946716307
-    dkappa: 0.00054166130944908412
+    dkappa: 0.00056951033986582934
     ddkappa: 0
   }
   v: 6.3011109733581527
@@ -159,12 +159,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587713.15706586442
-    y: 4141425.0801656153
-    theta: -1.7883734855329725
-    kappa: -0.0041836836604299266
+    x: 587713.15698672738
+    y: 4141425.0801829514
+    theta: -1.7884805559886328
+    kappa: -0.0042437473088928306
     s: 1.3624844141387935
-    dkappa: 0.00050362207657906287
+    dkappa: 0.00053116138641197451
     ddkappa: 0
   }
   v: 6.3251109733581528
@@ -173,12 +173,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587713.12975628569
-    y: 4141424.9564006077
-    theta: -1.7889084326371421
-    kappa: -0.0041252776619754364
+    x: 587713.12966467056
+    y: 4141424.9564206977
+    theta: -1.7890234824923126
+    kappa: -0.0041818952175651193
     s: 1.4892266336059565
-    dkappa: 0.00046543823329545434
+    dkappa: 0.00049266664510696741
     ddkappa: 0
   }
   v: 6.3491109733581528
@@ -187,12 +187,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587713.10211812123
-    y: 4141424.8322168011
-    theta: -1.7894232388147435
-    kappa: -0.0040807798234537892
+    x: 587713.10201091436
+    y: 4141424.8322403636
+    theta: -1.7895448119594539
+    kappa: -0.0041340878083172333
     s: 1.6164488530731196
-    dkappa: 0.00043260767391151904
+    dkappa: 0.0004594988185699047
     ddkappa: 0
   }
   v: 6.3731109733581528
@@ -201,12 +201,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587713.07434475655
-    y: 4141424.7075713165
-    theta: -1.7899369431846646
-    kappa: -0.0040380546461617223
+    x: 587713.07422146981
+    y: 4141424.7075984627
+    theta: -1.7900648592556925
+    kappa: -0.0040880616304135456
     s: 1.7441510725402829
-    dkappa: 0.0004004083368304574
+    dkappa: 0.000426957679082411
     ddkappa: 0
   }
   v: 6.3971109733581528
@@ -215,12 +215,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587713.04646699876
-    y: 4141424.58245732
-    theta: -1.7904525784606178
-    kappa: -0.0039951688758495347
+    x: 587713.04632757162
+    y: 4141424.5824880637
+    theta: -1.7905868613002591
+    kappa: -0.0040418624518722334
     s: 1.8723332920074458
-    dkappa: 0.00036808797067203593
+    dkappa: 0.00039429422576968247
     ddkappa: 0
   }
   v: 6.4211109733581528
@@ -229,12 +229,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587713.01848484809
-    y: 4141424.4568748134
-    theta: -1.790970144367122
-    kappa: -0.0039521225125172281
+    x: 587713.01832922013
+    y: 4141424.456909168
+    theta: -1.7911108178073423
+    kappa: -0.0039954902726932985
     s: 2.000995511474609
-    dkappa: 0.00033564657543625468
+    dkappa: 0.00036150845863171927
     ddkappa: 0
   }
   v: 6.4451109733581529
@@ -243,12 +243,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.99015854311
-    y: 4141424.3308774675
-    theta: -1.7914722707469726
-    kappa: -0.0039219692257726514
+    x: 587712.989984028
+    y: 4141424.3309160694
+    theta: -1.791617787599286
+    kappa: -0.0039621758863161291
     s: 2.1301377309417719
-    dkappa: 0.00030848656495507368
+    dkappa: 0.00033398190576942004
     ddkappa: 0
   }
   v: 6.4691109733581529
@@ -257,12 +257,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.961718716
-    y: 4141424.2044136538
-    theta: -1.791975666858828
-    kappa: -0.0038921523789479072
+    x: 587712.96152514359
+    y: 4141424.2044565435
+    theta: -1.7921259903873759
+    kappa: -0.00392919281133633
     s: 2.2597599504089354
-    dkappa: 0.0002814112282916116
+    dkappa: 0.00030653816429983895
     ddkappa: 0
   }
   v: 6.4931109733581529
@@ -271,12 +271,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.93317357439
-    y: 4141424.0774815371
-    theta: -1.7924809270724056
-    kappa: -0.0038622251182872116
+    x: 587712.932960874
+    y: 4141424.07752873
+    theta: -1.7926360750763988
+    kappa: -0.003896087597759806
     s: 2.3898621698760985
-    dkappa: 0.00025423562979301142
+    dkappa: 0.00027899279676676708
     ddkappa: 0
   }
   v: 6.5171109733581529
@@ -285,12 +285,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.904495856
-    y: 4141423.950087273
-    theta: -1.7929867216802158
-    kappa: -0.0038334434650181158
+    x: 587712.90426374588
+    y: 4141423.9501388376
+    theta: -1.7931465383607508
+    kappa: -0.0038641375379045843
     s: 2.5204443893432615
-    dkappa: 0.00022751825639539077
+    dkappa: 0.00025190232517207159
     ddkappa: 0
   }
   v: 6.5411109733581529
@@ -299,12 +299,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.87549690669
-    y: 4141423.8222734779
-    theta: -1.7934838511178302
-    kappa: -0.0038144991464625667
+    x: 587712.87524354691
+    y: 4141423.8223298653
+    theta: -1.7936469742170564
+    kappa: -0.003842185983404777
     s: 2.6515066088104247
-    dkappa: 0.00020512386456554116
+    dkappa: 0.0002291198375561028
     ddkappa: 0
   }
   v: 6.565110973358153
@@ -313,12 +313,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.84639175225
-    y: 4141423.6939915791
-    theta: -1.793982801430168
-    kappa: -0.0037954854465632286
+    x: 587712.846117065
+    y: 4141423.6940528071
+    theta: -1.7941492430614141
+    kappa: -0.0038201540339124025
     s: 2.7830488282775878
-    dkappa: 0.00018264745590336544
+    dkappa: 0.00020625391175253024
     ddkappa: 0
   }
   v: 6.589110973358153
@@ -327,12 +327,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.81718039257
-    y: 4141423.5652415776
-    theta: -1.7944835723695938
-    kappa: -0.0037764023653201004
+    x: 587712.8168843
+    y: 4141423.5653076642
+    theta: -1.7946533446412141
+    kappa: -0.0037980416894274611
     s: 2.915071047744751
-    dkappa: 0.00016008903040886356
+    dkappa: 0.0001833045477613539
     ddkappa: 0
   }
   v: 6.613110973358153
@@ -341,12 +341,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.78778752137
-    y: 4141423.4360406324
-    theta: -1.7949840481092325
-    kappa: -0.0037601062530293527
+    x: 587712.78746956214
+    y: 4141423.4361116961
+    theta: -1.7951567025956898
+    kappa: -0.0037787680978105241
     s: 3.0475732672119142
-    dkappa: 0.00013883354434573902
+    dkappa: 0.00016165308409534786
     ddkappa: 0
   }
   v: 6.637110973358153
@@ -355,12 +355,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.758122605
-    y: 4141423.3064093734
-    theta: -1.7954816856608529
-    kappa: -0.0037500310293777884
+    x: 587712.75778182247
+    y: 4141423.3064856618
+    theta: -1.7956562191172221
+    kappa: -0.0037658440053540613
     s: 3.1805554866790771
-    dkappa: 0.00012054599958216886
+    dkappa: 0.0001429608031882894
     ddkappa: 0
   }
   v: 6.6611109733581531
@@ -369,12 +369,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.72835061315
-    y: 4141423.1763102091
-    theta: -1.7959811195782758
-    kappa: -0.0037399194391550226
+    x: 587712.72798692447
+    y: 4141423.1763917413
+    theta: -1.7961575387883169
+    kappa: -0.0037528732633199027
     s: 3.3140177061462404
-    dkappa: 0.00010219244583242228
+    dkappa: 0.00012420105239787846
     ddkappa: 0
   }
   v: 6.6851109733581531
@@ -383,12 +383,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.69847154559
-    y: 4141423.0457431404
-    theta: -1.7964823496125226
-    kappa: -0.0037297714823610554
+    x: 587712.69808486861
+    y: 4141423.0458299355
+    theta: -1.7966606613571647
+    kappa: -0.00373985587170805
     s: 3.4479599256134033
-    dkappa: 8.3772883096499366e-05
+    dkappa: 0.00010537383172411525
     ddkappa: 0
   }
   v: 6.7091109733581531
@@ -397,12 +397,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.668349048
-    y: 4141422.9147395021
-    theta: -1.79698389823647
-    kappa: -0.0037237117369672843
+    x: 587712.66793898528
+    y: 4141422.914831677
+    theta: -1.7971633227109309
+    kappa: -0.0037310341057984649
     s: 3.5823821450805666
-    dkappa: 6.7515488912598483e-05
+    dkappa: 8.8703031253232459e-05
     ddkappa: 0
   }
   v: 6.7331109733581531
@@ -411,12 +411,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.63801437244
-    y: 4141422.7832921119
-    theta: -1.7974861045225825
-    kappa: -0.0037207948534166584
+    x: 587712.63758058171
+    y: 4141422.7833897653
+    theta: -1.797666041924963
+    kappa: -0.0037254361735673093
     s: 3.7172843645477296
-    dkappa: 5.2909566995337004e-05
+    dkappa: 7.36792171307365e-05
     ddkappa: 0
   }
   v: 6.7571109733581531
@@ -425,12 +425,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.60757176229
-    y: 4141422.6513770148
-    theta: -1.797990097798531
-    kappa: -0.0037178675912072556
+    x: 587712.607114159
+    y: 4141422.6514801653
+    theta: -1.7981705499541307
+    kappa: -0.003719818323150363
     s: 3.852666584014893
-    dkappa: 3.8251675269513575e-05
+    dkappa: 5.860194628375306e-05
     ddkappa: 0
   }
   v: 6.7811109733581532
@@ -439,12 +439,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.57702121732
-    y: 4141422.5189942108
-    theta: -1.7984958778076949
-    kappa: -0.0037149299503390731
+    x: 587712.576539717
+    y: 4141422.5191028779
+    theta: -1.7986768465410261
+    kappa: -0.0037141805545476254
     s: 3.9885288034820561
-    dkappa: 2.3541813735128268e-05
+    dkappa: 4.3471218712282237e-05
     ddkappa: 0
   }
   v: 6.8051109733581532
@@ -453,12 +453,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.54615144944
-    y: 4141422.3861926626
-    theta: -1.7990040539956649
-    kappa: -0.0037168811196416135
+    x: 587712.54564604466
+    y: 4141422.3863068894
+    theta: -1.7991844088008255
+    kappa: -0.0037136075318231817
     s: 4.1248710229492191
-    dkappa: 1.1823662695504161e-05
+    dkappa: 3.1326682706357983e-05
     ddkappa: 0
   }
   v: 6.8291109733581532
@@ -467,12 +467,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.51514130912
-    y: 4141422.2529309238
-    theta: -1.7995141108938892
-    kappa: -0.0037195740488689716
+    x: 587712.51461191848
+    y: 4141422.2530507343
+    theta: -1.7996936797346723
+    kappa: -0.0037137954121124516
     s: 4.2616932424163823
-    dkappa: 5.2081703754427629e-07
+    dkappa: 1.9595470434592456e-05
     ddkappa: 0
   }
   v: 6.8531109733581532
@@ -481,12 +481,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.484022379
-    y: 4141422.1192016765
-    theta: -1.8000259571638866
-    kappa: -0.0037222764254363459
+    x: 587712.4834689186
+    y: 4141422.11932709
+    theta: -1.8002047372830265
+    kappa: -0.00371398395152375
     s: 4.3989954618835458
-    dkappa: -1.0821681287482074e-05
+    dkappa: 7.8231026990990227e-06
     ddkappa: 0
   }
   v: 6.8771109733581532
@@ -495,12 +495,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.45273944177
-    y: 4141421.9850178231
-    theta: -1.800540192425877
-    kappa: -0.0037259208086543897
+    x: 587712.45216198545
+    y: 4141421.9851488331
+    theta: -1.8007179136007876
+    kappa: -0.0037151536837871668
     s: 4.536777681350709
-    dkappa: -2.1547264273636156e-05
+    dkappa: -3.3344447960448361e-06
     ddkappa: 0
   }
   v: 6.9011109733581533
@@ -509,12 +509,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.42115284794
-    y: 4141421.8504119972
-    theta: -1.8010583341904507
-    kappa: -0.0037328657285535145
+    x: 587712.42055185069
+    y: 4141421.8505485328
+    theta: -1.8012340498629014
+    kappa: -0.0037197848114903509
     s: 4.6750399008178718
-    dkappa: -2.99954098057852e-05
+    dkappa: -1.2217919963162266e-05
     ddkappa: 0
   }
   v: 6.9251109733581533
@@ -523,12 +523,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.389456596
-    y: 4141421.7153388644
-    theta: -1.8015782749331373
-    kappa: -0.003739834758882548
+    x: 587712.38883197634
+    y: 4141421.7154809465
+    theta: -1.8017519781388038
+    kappa: -0.0037244320169138711
     s: 4.813782120285035
-    dkappa: -3.8472884462485138e-05
+    dkappa: -2.1132235573147266e-05
     ddkappa: 0
   }
   v: 6.9491109733581533
@@ -537,12 +537,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.357650686
-    y: 4141421.579798426
-    theta: -1.8021000143730608
-    kappa: -0.003746827899641491
+    x: 587712.357002362
+    y: 4141421.5799460723
+    theta: -1.8022716981508673
+    kappa: -0.003729095300057726
     s: 4.9530043397521988
-    dkappa: -4.6979688243735981e-05
+    dkappa: -3.0077391625999838e-05
     ddkappa: 0
   }
   v: 6.9731109733581533
@@ -551,12 +551,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.32556293462
-    y: 4141421.4438312715
-    theta: -1.8026239970489868
-    kappa: -0.003755939155576845
+    x: 587712.32489161333
+    y: 4141421.4439843465
+    theta: -1.8027929647642922
+    kappa: -0.0037360051306007192
     s: 5.0927065592193612
-    dkappa: -5.4418683103159378e-05
+    dkappa: -3.7956866711358892e-05
     ddkappa: 0
   }
   v: 6.9971109733581534
@@ -565,12 +565,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.29325868923
-    y: 4141421.3074219963
-    theta: -1.8031500546848549
-    kappa: -0.0037663738082435337
+    x: 587712.29256477836
+    y: 4141421.3075804226
+    theta: -1.803315871442321
+    kappa: -0.0037443149952355145
     s: 5.232888778686525
-    dkappa: -6.12062557961859e-05
+    dkappa: -4.5186815316537724e-05
     ddkappa: 0
   }
   v: 7.0211109733581534
@@ -579,12 +579,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.2608438303
-    y: 4141421.1705456395
-    theta: -1.8036779136506693
-    kappa: -0.0037768441903579305
+    x: 587712.26012725267
+    y: 4141421.1707094368
+    theta: -1.80384056866019
+    kappa: -0.0037526533138000583
     s: 5.3735509981536875
-    dkappa: -6.8017069916727474e-05
+    dkappa: -5.2441520095100513e-05
     ddkappa: 0
   }
   v: 7.0451109733581534
@@ -593,12 +593,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.228302971
-    y: 4141421.0332058617
-    theta: -1.8042079724872366
-    kappa: -0.0037874441326024777
+    x: 587712.22756375349
+    y: 4141421.0333750239
+    theta: -1.8043673935458204
+    kappa: -0.0037611274662052046
     s: 5.5146932176208514
-    dkappa: -7.4624042035037664e-05
+    dkappa: -5.9493817301231827e-05
     ddkappa: 0
   }
   v: 7.0691109733581534
@@ -607,12 +607,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.1954124996
-    y: 4141420.8954558345
-    theta: -1.8047460275585834
-    kappa: -0.0037995372319800571
+    x: 587712.19465227192
+    y: 4141420.8956300151
+    theta: -1.8049012479790336
+    kappa: -0.0037712973438974963
     s: 5.656315437088014
-    dkappa: -7.7727075891057757e-05
+    dkappa: -6.3043734144461761e-05
     ddkappa: 0
   }
   v: 7.0931109733581525
@@ -621,12 +621,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.16241055261
-    y: 4141420.757238932
-    theta: -1.8052859064795026
-    kappa: -0.0038116713184832667
+    x: 587712.16162924352
+    y: 4141420.7574181473
+    theta: -1.8054369120194742
+    kappa: -0.0037815016903427184
     s: 5.7984176565551779
-    dkappa: -8.0840626855597261e-05
+    dkappa: -6.6605682716125024e-05
     ddkappa: 0
   }
   v: 7.1171109733581535
@@ -635,12 +635,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.12929713
-    y: 4141420.6185551528
-    theta: -1.8058276089359171
-    kappa: -0.0038238463921121055
+    x: 587712.12849466829
+    y: 4141420.618739421
+    theta: -1.8059743853603638
+    kappa: -0.0037917405055408709
     s: 5.9409998760223406
-    dkappa: -8.39646949286561e-05
+    dkappa: -7.0179663016221549e-05
     ddkappa: 0
   }
   v: 7.1411109733581526
@@ -649,12 +649,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.09593045025
-    y: 4141420.4794384907
-    theta: -1.8063750780204495
-    kappa: -0.0038363161472540486
+    x: 587712.095108008
+    y: 4141420.479627552
+    theta: -1.8065171124301416
+    kappa: -0.0038023904146023768
     s: 6.0840620954895037
-    dkappa: -8.5897318810434264e-05
+    dkappa: -7.2563283455303652e-05
     ddkappa: 0
   }
   v: 7.1651109733581535
@@ -663,12 +663,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.0623332198
-    y: 4141420.3398835021
-    theta: -1.8069276827469043
-    kappa: -0.0038490399544881615
+    x: 587712.06149179058
+    y: 4141420.3400771357
+    theta: -1.8070645415921791
+    kappa: -0.0038133910460575837
     s: 6.2276043149566664
-    dkappa: -8.6830993857589143e-05
+    dkappa: -7.3949282502857688e-05
     ddkappa: 0
   }
   v: 7.1891109733581526
@@ -677,12 +677,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587712.02862364124
-    y: 4141420.1998618469
-    theta: -1.8074821354303412
-    kappa: -0.0038618063096743324
+    x: 587712.02776316181
+    y: 4141420.2000600677
+    theta: -1.8076138014024228
+    kappa: -0.0038244284632272521
     s: 6.3716265344238305
-    dkappa: -8.776779108039588e-05
+    dkappa: -7.5339916281191971e-05
     ddkappa: 0
   }
   v: 7.2131109733581535
@@ -691,12 +691,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587711.99478299625
-    y: 4141420.0593780493
-    theta: -1.8080389516660487
-    kappa: -0.0038745789571814813
+    x: 587711.99390357861
+    y: 4141420.0595808337
+    theta: -1.8081653508657431
+    kappa: -0.0038354823780040537
     s: 6.5161287538909933
-    dkappa: -8.8569044308249126e-05
+    dkappa: -7.65963223503264e-05
     ddkappa: 0
   }
   v: 7.2371109733581527
@@ -705,12 +705,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587711.96058184141
-    y: 4141419.9184875777
-    theta: -1.8086044558412648
-    kappa: -0.0038869134947445588
+    x: 587711.95968599874
+    y: 4141419.91869435
+    theta: -1.8087248144374859
+    kappa: -0.003846304381205323
     s: 6.6611109733581557
-    dkappa: -8.7535057288074913e-05
+    dkappa: -7.6018258067659462e-05
     ddkappa: 0
   }
   v: 7.2611109733581527
@@ -719,12 +719,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587711.78715526755
-    y: 4141419.2072145008
-    theta: -1.8114784869451002
-    kappa: -0.0039454733030346656
+    x: 587711.786184788
+    y: 4141419.2074394673
+    theta: -1.811566474776553
+    kappa: -0.0038978338862852239
     s: 7.3932220706939713
-    dkappa: -7.7624357342455377e-05
+    dkappa: -6.84068912198902e-05
     ddkappa: 0
   }
   v: 7.3811109733581528
@@ -733,12 +733,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587711.60869107628
-    y: 4141418.484821877
-    theta: -1.8144291035957152
-    kappa: -0.0039916669951115128
+    x: 587711.60767048877
+    y: 4141418.485059212
+    theta: -1.8144795195896992
+    kappa: -0.0039386254393094112
     s: 8.1373331680297873
-    dkappa: -5.8637531251894649e-05
+    dkappa: -5.1749420642995352e-05
     ddkappa: 0
   }
   v: 7.5011109733581529
@@ -747,12 +747,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587711.42517960921
-    y: 4141417.7513186485
-    theta: -1.8174552730486586
-    kappa: -0.0040157520354790087
+    x: 587711.424137006
+    y: 4141417.7515614815
+    theta: -1.8174640601083794
+    kappa: -0.0039589778635478389
     s: 8.8934442653656038
-    dkappa: -2.5404277067884345e-05
+    dkappa: -2.0877142917298581e-05
     ddkappa: 0
   }
   v: 7.621110973358153
@@ -761,12 +761,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587711.23648637021
-    y: 4141417.006745819
-    theta: -1.8205429850560606
-    kappa: -0.0040076687089654868
+    x: 587711.23545395583
+    y: 4141417.0069860537
+    theta: -1.8205072977596237
+    kappa: -0.0039489510901337815
     s: 9.66155536270142
-    dkappa: 1.7984684873784037e-05
+    dkappa: 2.011713719796795e-05
     ddkappa: 0
   }
   v: 7.7411109733581531
@@ -775,12 +775,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587711.042532516
-    y: 4141416.2511304459
-    theta: -1.8236526167973242
-    kappa: -0.0039629607787947455
+    x: 587711.04154372821
+    y: 4141416.2513594721
+    theta: -1.8235709337411858
+    kappa: -0.0039040961082356471
     s: 10.441666460037235
-    dkappa: 6.7603133657798557e-05
+    dkappa: 6.7304501358464759e-05
     ddkappa: 0
   }
   v: 7.8611109733581532
@@ -789,12 +789,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587710.84311232949
-    y: 4141415.4845335349
-    theta: -1.8267559751211879
-    kappa: -0.0038690516443856816
+    x: 587710.84220480581
+    y: 4141415.4847414112
+    theta: -1.8266283279517697
+    kappa: -0.0038120713426315735
     s: 11.233777557373053
-    dkappa: 0.00012847087851083412
+    dkappa: 0.00012570315753480628
     ddkappa: 0
   }
   v: 7.9811109733581533
@@ -803,12 +803,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587710.63843041076
-    y: 4141414.7069094153
-    theta: -1.8298192346008659
-    kappa: -0.0037244782757130763
+    x: 587710.63763837342
+    y: 4141414.70708689
+    theta: -1.8296470438867938
+    kappa: -0.003671285644514112
     s: 12.037888654708867
-    dkappa: 0.00019489402234337165
+    dkappa: 0.00018961726671378842
     ddkappa: 0
   }
   v: 8.1011109733581534
@@ -817,12 +817,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587710.42814060161
-    y: 4141413.9183569392
-    theta: -1.8327701949643689
-    kappa: -0.003516013831086762
+    x: 587710.42750180175
+    y: 4141413.9184935559
+    theta: -1.8325570507896618
+    kappa: -0.0034688652289619794
     s: 12.853999752044684
-    dkappa: 0.00026813496696501815
+    dkappa: 0.00026030877711721574
     ddkappa: 0
   }
   v: 8.2211109733581544
@@ -831,12 +831,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587710.21253808984
-    y: 4141413.1188053391
-    theta: -1.8355744029569618
-    kappa: -0.0032407026731979091
+    x: 587710.2120845014
+    y: 4141413.1188920233
+    theta: -1.835325492983632
+    kappa: -0.0032017605548100786
     s: 13.6821108493805
-    dkappa: 0.00034811052236810537
+    dkappa: 0.00033769479638495281
     ddkappa: 0
   }
   v: 8.3411109733581537
@@ -845,12 +845,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587709.99167939613
-    y: 4141412.3082452626
-    theta: -1.8381639408269326
-    kappa: -0.002894948955577637
+    x: 587709.9914390191
+    y: 4141412.3082738756
+    theta: -1.8378862312903725
+    kappa: -0.0028664408411223606
     s: 14.522221946716316
-    dkappa: 0.00043184167586140187
+    dkappa: 0.000418797636871744
     ddkappa: 0
   }
   v: 8.4611109733581547
@@ -859,12 +859,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587709.765548714
-    y: 4141411.4866884239
-    theta: -1.8404457756664505
-    kappa: -0.0024636819415721788
+    x: 587709.76554541173
+    y: 4141411.4866518164
+    theta: -1.8401494495386521
+    kappa: -0.0024481561304149175
     s: 15.374331704378827
-    dkappa: 0.00052241167264089152
+    dkappa: 0.00050670125751064326
     ddkappa: 0
   }
   v: 8.581057522610898
@@ -873,12 +873,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587709.5345182959
-    y: 4141410.6540551279
-    theta: -1.8423510706650843
-    kappa: -0.0019486357877964036
+    x: 587709.53476549033
+    y: 4141410.653949054
+    theta: -1.8420479581300213
+    kappa: -0.0019485497118743389
     s: 16.238422924510985
-    dkappa: 0.00061497815576318107
+    dkappa: 0.00059656501483226461
     ddkappa: 0
   }
   v: 8.7006888147780082
@@ -887,12 +887,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587709.29890874121
-    y: 4141409.8103078352
-    theta: -1.8438128872087125
-    kappa: -0.0013665013638883765
+    x: 587709.2994097406
+    y: 4141409.810130931
+    theta: -1.8435170414495634
+    kappa: -0.001383662551652412
     s: 17.114448931382203
-    dkappa: 0.00067178415454966737
+    dkappa: 0.00065195593464503787
     ddkappa: 0
   }
   v: 8.8197045731180879
@@ -901,12 +901,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587709.05906927353
-    y: 4141408.9554297277
-    theta: -1.8447871714841888
-    kappa: -0.00081512996239385162
+    x: 587709.05981529853
+    y: 4141408.9551841198
+    theta: -1.8445134448077993
+    kappa: -0.00084816959249236644
     s: 18.002333838697453
-    dkappa: 0.00060358699451738785
+    dkappa: 0.00058637448834028018
     ddkappa: 0
   }
   v: 8.9378208630834575
@@ -915,12 +915,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587708.81538856146
-    y: 4141408.0894202325
-    theta: -1.8452850774867098
-    kappa: -0.00032281634705478245
+    x: 587708.81635607488
+    y: 4141408.089112333
+    theta: -1.8450476233802506
+    kappa: -0.00036932888380015105
     s: 18.901974183816613
-    dkappa: 0.00053325628202908126
+    dkappa: 0.00051883210960244528
     ddkappa: 0
   }
   v: 9.0547700923201475
@@ -929,12 +929,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587708.56832036062
-    y: 4141407.2122864258
-    theta: -1.8453709251272643
-    kappa: 0.00011156224654109093
+    x: 587708.56947548781
+    y: 4141407.2119257031
+    theta: -1.8451812262453677
+    kappa: 5.3926186124956137e-05
     s: 19.81324056197386
-    dkappa: 0.00046151563224326953
+    dkappa: 0.00044991589463943974
     ddkappa: 0
   }
   v: 9.1703010106679059
@@ -943,12 +943,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587708.318228877
-    y: 4141406.3240854661
-    theta: -1.8450902631290034
-    kappa: 0.00048683601300305408
+    x: 587708.31952750171
+    y: 4141406.3236843487
+    theta: -1.8449579554851756
+    kappa: 0.00042053492744377512
     s: 20.735979260497022
-    dkappa: 0.00039028538339246077
+    dkappa: 0.00038154511973342614
     ddkappa: 0
   }
   v: 9.2841787101601962
@@ -957,12 +957,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587708.06548080989
-    y: 4141405.4248974603
-    theta: -1.8444827984621635
-    kappa: 0.00080992978875943063
+    x: 587708.066869854
+    y: 4141405.4244709285
+    theta: -1.84441544973836
+    kappa: 0.00073753110644965165
     s: 21.670013893026965
-    dkappa: 0.00033696635936265927
+    dkappa: 0.00033112356359085883
     ddkappa: 0
   }
   v: 9.3961846250241923
@@ -971,12 +971,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587707.810412826
-    y: 4141404.5148332408
-    theta: -1.8435718341747167
-    kappa: 0.0011179301515826072
+    x: 587707.8118318927
+    y: 4141404.5143982871
+    theta: -1.8435747247429093
+    kappa: 0.0010421041130118734
     s: 22.615147033736953
-    dkappa: 0.00031750891563685885
+    dkappa: 0.00031459234315441432
     ddkappa: 0
   }
   v: 9.5061165316807852
@@ -985,12 +985,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587707.55338144442
-    y: 4141403.5940188873
-    theta: -1.8423727923039361
-    kappa: 0.0013868722932344959
+    x: 587707.5547648049
+    y: 4141403.5935938931
+    theta: -1.8424487076357636
+    kappa: 0.0013103920266240114
     s: 23.571161851552034
-    dkappa: 0.00026970991889594358
+    dkappa: 0.0002697552383408373
     ddkappa: 0
   }
   v: 9.61378854874458
@@ -999,12 +999,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587707.29472562869
-    y: 4141402.6626047967
-    theta: -1.8409204700074433
-    kappa: 0.0016130097356296287
+    x: 587707.29600455856
+    y: 4141402.6622088007
+    theta: -1.8410694937586907
+    kappa: 0.001538748427481139
     s: 24.537823744368396
-    dkappa: 0.00022201077429432332
+    dkappa: 0.00022505068582320128
     ddkappa: 0
   }
   v: 9.7190311370238973
@@ -1013,12 +1013,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587707.034752119
-    y: 4141401.7207680927
-    theta: -1.8392529412924739
-    kappa: 0.001796008793225876
+    x: 587707.03585755965
+    y: 4141401.7204199932
+    theta: -1.8394722585403607
+    kappa: 0.0017269365095959447
     s: 25.514881973272743
-    dkappa: 0.000175820395156343
+    dkappa: 0.00018188684465657629
     ddkappa: 0
   }
   v: 9.8216910995207662
@@ -1027,12 +1027,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587706.77378049656
-    y: 4141400.7686985787
-    theta: -1.8374072000454369
-    kappa: 0.0019349048595755515
+    x: 587706.7746458844
+    y: 4141400.7684163051
+    theta: -1.8376908916447603
+    kappa: 0.0018740921945248241
     s: 26.50207129676167
-    dkappa: 0.00012930652790516103
+    dkappa: 0.00013843085471774148
     ddkappa: 0
   }
   v: 9.92163158143094
@@ -1041,12 +1041,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587706.51203160756
-    y: 4141399.806627566
-    theta: -1.8354274242867477
-    kappa: 0.0020297951128115917
+    x: 587706.512596572
+    y: 4141399.8064270713
+    theta: -1.8357663078431794
+    kappa: 0.0019803937839834472
     s: 27.499113604961032
-    dkappa: 8.3897687043284867e-05
+    dkappa: 9.6110395795966652e-05
     ddkappa: 0
   }
   v: 10.018732070143878
@@ -1055,12 +1055,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587706.24976036348
-    y: 4141398.8347895867
-    theta: -1.8333548976963858
-    kappa: 0.0020799225799635868
+    x: 587706.24997415813
+    y: 4141398.8346839254
+    theta: -1.8337364280761124
+    kappa: 0.002045158480811687
     s: 28.505719553845317
-    dkappa: 3.8499160100401441e-05
+    dkappa: 5.382997428441984e-05
     ddkappa: 0
   }
   v: 10.112888395242758
@@ -1069,12 +1069,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587705.9871307275
-    y: 4141397.8534545205
-    theta: -1.8312357292350327
-    kappa: 0.0020849087347659832
+    x: 587705.98695565155
+    y: 4141397.8534530085
+    theta: -1.8316436498561719
+    kappa: 0.0020681146395123804
     s: 29.52159019945702
-    dkappa: -6.3825519347680641e-06
+    dkappa: 1.2095245308168735e-05
     ddkappa: 0
   }
   v: 10.20401272850447
@@ -1083,12 +1083,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587705.72429786762
-    y: 4141396.8629032327
-    theta: -1.829116220308312
-    kappa: 0.0020443006057662364
+    x: 587705.72371373465
+    y: 4141396.8630103497
+    theta: -1.8295305603826113
+    kappa: 0.002048880901383208
     s: 30.546418632126006
-    dkappa: -5.06192958619108e-05
+    dkappa: -2.8966426930375689e-05
     ddkappa: 0
   }
   v: 10.29203358389962
@@ -1097,12 +1097,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587705.46134678531
-    y: 4141395.86344203
-    theta: -1.8270448051537191
-    kappa: 0.0019578726125885865
+    x: 587705.460355252
+    y: 4141395.8636564235
+    theta: -1.8274416973908021
+    kappa: 0.0019873058607656033
     s: 31.579891610688883
-    dkappa: -9.4828369455731614e-05
+    dkappa: -6.9973227773471053e-05
     ddkappa: 0
   }
   v: 10.376895817592528
@@ -1111,12 +1111,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587705.1983132757
-    y: 4141394.8553947369
-    theta: -1.8250709809976076
-    kappa: 0.0018254491270177533
+    x: 587705.196942315
+    y: 4141394.8557082219
+    theta: -1.8254225292023203
+    kappa: 0.0018832855124846768
     s: 32.621691196708376
-    dkappa: -0.00013805111609252398
+    dkappa: -0.00010996737575586437
     ddkappa: 0
   }
   v: 10.458560627941228
@@ -1125,12 +1125,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587704.935199831
-    y: 4141393.8390966272
-    theta: -1.823244590418555
-    kappa: 0.0016508846383642168
+    x: 587704.93350842874
+    y: 4141393.8394931448
+    theta: -1.8235189394323608
+    kappa: 0.0017397995570162909
     s: 33.671496388692717
-    dkappa: -0.00017350671685637168
+    dkappa: -0.00014394913552701397
     ddkappa: 0
   }
   v: 10.537005555497471
@@ -1139,12 +1139,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587704.67192400072
-    y: 4141392.814904484
-    theta: -1.8215951224445364
-    kappa: 0.0014712911986657677
+    x: 587704.670005098
+    y: 4141392.8153595319
+    theta: -1.8217611634468467
+    kappa: 0.0015855109131557921
     s: 34.728985893399319
-    dkappa: -0.00016718447271008859
+    dkappa: -0.0001455388850080868
     ddkappa: 0
   }
   v: 10.612269710928395
@@ -1153,12 +1153,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587704.40843106259
-    y: 4141391.7831532042
-    theta: -1.8201199630104019
-    kappa: 0.001301526715025033
+    x: 587704.40640692529
+    y: 4141391.783635159
+    theta: -1.8201549362309992
+    kappa: 0.0014318980322121973
     s: 35.793851859258687
-    dkappa: -0.00015650442711056124
+    dkappa: -0.00014367874431659458
     ddkappa: 0
   }
   v: 10.68457924070011
@@ -1167,12 +1167,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587704.14460668864
-    y: 4141390.7441657828
-    theta: -1.8188097893507811
-    kappa: 0.001143078119228402
+    x: 587704.14261980634
+    y: 4141390.7446383024
+    theta: -1.8187005317369349
+    kappa: 0.0012802630843509603
     s: 36.865811923814888
-    dkappa: -0.00014450104644758585
+    dkappa: -0.0001405547651832529
     ddkappa: 0
   }
   v: 10.754193761670104
@@ -1181,12 +1181,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587703.88039913
-    y: 4141389.6982225357
-    theta: -1.8176554318736822
-    kappa: 0.00099712646253096457
+    x: 587703.87860232138
+    y: 4141389.6986470814
+    theta: -1.8173987396813873
+    kappa: 0.0011316114107068269
     s: 37.944608970695157
-    dkappa: -0.0001315794863759831
+    dkappa: -0.00013657001178367216
     ddkappa: 0
   }
   v: 10.821358578645194
@@ -1195,12 +1195,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587703.61569854093
-    y: 4141388.6455933335
-    theta: -1.816644541634862
-    kappa: 0.00086553520256832622
+    x: 587703.614245909
+    y: 4141388.645931405
+    theta: -1.8162477029459021
+    kappa: 0.00098753963234013333
     s: 39.030009698404911
-    dkappa: -0.00011719140001551905
+    dkappa: -0.00013117408660507518
     ddkappa: 0
   }
   v: 10.88630468438153
@@ -1209,12 +1209,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587703.35047666577
-    y: 4141387.5865039593
-    theta: -1.8157663969730176
-    kappa: 0.0007501085963831631
+    x: 587703.349512602
+    y: 4141387.5867198119
+    theta: -1.8152481438468346
+    kappa: 0.00084951062193685406
     s: 40.121803189122616
-    dkappa: -0.00010158616063527526
+    dkappa: -0.00012461430166568421
     ddkappa: 0
   }
   v: 10.949248759584595
@@ -1223,12 +1223,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587703.08460061066
-    y: 4141386.52118443
-    theta: -1.8149985538873845
-    kappa: 0.00065237871443428659
+    x: 587703.08424155659
+    y: 4141386.5212494796
+    theta: -1.8143883901844109
+    kappa: 0.0007191584194550394
     s: 41.21979947749481
-    dkappa: -8.4335984675706639e-05
+    dkappa: -0.00011646092512990335
     ddkappa: 0
   }
   v: 11.010393172909204
@@ -1237,12 +1237,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587702.81803928851
-    y: 4141385.4498188356
-    theta: -1.8143236978663337
-    kappa: 0.00057419453976668945
+    x: 587702.81836559833
+    y: 4141385.4497136
+    theta: -1.8136624173555786
+    kappa: 0.00059818548650895594
     s: 42.323828119430921
-    dkappa: -6.55307430920171e-05
+    dkappa: -0.00010680203533403566
     ddkappa: 0
   }
   v: 11.069925980959505
@@ -1251,12 +1251,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587702.55073194671
-    y: 4141384.3725798028
-    theta: -1.8137190458138928
-    kappa: 0.00051771661341454256
+    x: 587702.55177532509
+    y: 4141384.3722968865
+    theta: -1.8130590081544182
+    kappa: 0.00048861087686723214
     s: 43.433736760898292
-    dkappa: -4.4865852296498295e-05
+    dkappa: -9.5331544704197464e-05
     ddkappa: 0
   }
   v: 11.128020928288979
@@ -1265,12 +1265,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587702.28267449385
-    y: 4141383.2896085847
-    theta: -1.8131614770487756
-    kappa: 0.000485699351616596
+    x: 587702.28440313262
+    y: 4141383.2891562791
+    theta: -1.8125682094032973
+    kappa: 0.00039287203538142251
     s: 44.549389706717058
-    dkappa: -2.2211359128743367e-05
+    dkappa: -8.1918255207484754e-05
     ddkappa: 0
   }
   v: 11.184837447400438
@@ -1279,12 +1279,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587702.01387298317
-    y: 4141382.2010281617
-    theta: -1.8126223340842502
-    kappa: 0.00048117728360745973
+    x: 587702.01617331419
+    y: 4141382.2004348412
+    theta: -1.8121751921642348
+    kappa: 0.00031372678037496626
     s: 45.670666489355121
-    dkappa: 2.6421084917483528e-06
+    dkappa: -6.6351986630868587e-05
     ddkappa: 0
   }
   v: 11.240520658746027
@@ -1293,12 +1293,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587701.74433037557
-    y: 4141381.1069480269
-    theta: -1.8120685968792449
-    kappa: 0.00050613433344807329
+    x: 587701.74699922488
+    y: 4141381.1062639686
+    theta: -1.8118574292975722
+    kappa: 0.00025332748237559544
     s: 46.797460437723053
-    dkappa: 2.9811912811506886e-05
+    dkappa: -4.8514838652374045e-05
     ddkappa: 0
   }
   v: 11.295201370727224
@@ -1307,12 +1307,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587701.47412130819
-    y: 4141380.00744727
-    theta: -1.8114673243439294
-    kappa: 0.00056366120428749883
+    x: 587701.476850449
+    y: 4141380.0067483797
+    theta: -1.8115946378735657
+    kappa: 0.00021463596740525261
     s: 47.929677245969053
-    dkappa: 5.9619191437993319e-05
+    dkappa: -2.80853747965944e-05
     ddkappa: 0
   }
   v: 11.348996079694837
@@ -1321,12 +1321,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587701.20334986132
-    y: 4141378.9025866035
-    theta: -1.8107762302976116
-    kappa: 0.00065778348061035013
+    x: 587701.2056979253
+    y: 4141378.9019811507
+    theta: -1.8113604577575289
+    kappa: 0.00020129707478506163
     s: 49.06723354227384
-    dkappa: 9.2078184467277535e-05
+    dkappa: -5.0491132435185209e-06
     ddkappa: 0
   }
   v: 11.402006969949008
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_follow_03_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_follow_03_0.pb.txt
index 7ef3ba0f83..13c221a322 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_follow_03_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_follow_03_0.pb.txt
@@ -9,7 +9,7 @@ trajectory_point {
     y: 4140955.2434454006
     z: 0
     theta: -1.827545023130303
-    kappa: 0.014029947685582236
+    kappa: 0.013854573065965967
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -20,12 +20,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.57998726692
-    y: 4140955.1546382122
-    theta: -1.8263200907681909
-    kappa: 0.013781843989729054
+    x: 587260.57998357585
+    y: 4140955.1546391677
+    theta: -1.8263349225875172
+    kappa: 0.013611453394760983
     s: 0.091735645072861971
-    dkappa: -0.00049604134302733355
+    dkappa: -0.00048607613306483937
     ddkappa: 0
   }
   v: 4.5736049385212034
@@ -34,12 +34,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.55712524778
-    y: 4140955.0663397382
-    theta: -1.8251021688960978
-    kappa: 0.013535161507841368
+    x: 587260.55711788672
+    y: 4140955.0663416442
+    theta: -1.8251317477995446
+    kappa: 0.013369726387501678
     s: 0.18294580027156818
-    dkappa: -0.00098924120930136733
+    dkappa: -0.00096936787315144769
     ddkappa: 0
   }
   v: 4.5474503830691724
@@ -48,12 +48,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.53439374641
-    y: 4140954.9785453551
-    theta: -1.8238911974272463
-    kappa: 0.013289887322256675
+    x: 587260.53438273643
+    y: 4140954.9785482059
+    theta: -1.8239354393194105
+    kappa: 0.013129379386021666
     s: 0.2736352418655682
-    dkappa: -0.0014796254255016932
+    dkappa: -0.0014499005280949931
     ddkappa: 0
   }
   v: 4.5215330491593653
@@ -62,12 +62,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.51179158187
-    y: 4140954.8912505005
-    theta: -1.8226871169984897
-    kappa: 0.013046008686962035
+    x: 587260.51177694369
+    y: 4140954.8912542919
+    theta: -1.8227459384179541
+    kappa: 0.012890399900355967
     s: 0.36380868265736527
-    dkappa: -0.0019672194751236445
+    dkappa: -0.0019276990694407766
     ddkappa: 0
   }
   v: 4.4958497792142467
@@ -76,12 +76,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.48931758839
-    y: 4140954.8044506735
-    theta: -1.8214898689654206
-    kappa: 0.012803513025888075
+    x: 587260.48929934262
+    y: 4140954.8044553981
+    theta: -1.8215631870789104
+    kappa: 0.012652775607069247
     s: 0.45347077261330754
-    dkappa: -0.0024520485018891683
+    dkappa: -0.002402788135786582
     ddkappa: 0
   }
   v: 4.470397447195853
@@ -90,12 +90,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.46723152662
-    y: 4140954.7180747604
-    theta: -1.8203562831812989
-    kappa: 0.012563582767066357
+    x: 587260.46720663423
+    y: 4140954.718081174
+    theta: -1.8204428892532964
+    kappa: 0.012417577922198671
     s: 0.54262609949437957
-    dkappa: -0.0027021613765395524
+    dkappa: -0.0026480535689598328
     ddkappa: 0
   }
   v: 4.4451729586057924
@@ -104,12 +104,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.44555521745
-    y: 4140954.6321125133
-    theta: -1.8192912951778131
-    kappa: 0.012326310794232249
+    x: 587260.4455203769
+    y: 4140954.6321214358
+    theta: -1.8193898838099789
+    kappa: 0.01218489021352444
     s: 0.6312791894869938
-    dkappa: -0.0026971726750589461
+    dkappa: -0.0026435293627300252
     ddkappa: 0
   }
   v: 4.4201732504852451
@@ -118,12 +118,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.424000617
-    y: 4140954.5466329288
-    theta: -1.8182322836781035
-    kappa: 0.012090371062163079
+    x: 587260.42395588406
+    y: 4140954.5466443463
+    theta: -1.8183427876978344
+    kappa: 0.011953509005771043
     s: 0.71943450783378227
-    dkappa: -0.0026922119842674955
+    dkappa: -0.0026390305591295136
     ddkappa: 0
   }
   v: 4.3953952914149648
@@ -132,12 +132,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.4025666482
-    y: 4140954.4616317358
-    theta: -1.8171791981752898
-    kappa: 0.011855751781463275
+    x: 587260.40251207829
+    y: 4140954.4616456348
+    theta: -1.8173015509249462
+    kappa: 0.011723422737322462
     s: 0.80709645946438757
-    dkappa: -0.0026872790562902653
+    dkappa: -0.0026345569333628615
     ddkappa: 0
   }
   v: 4.370836081515276
@@ -146,12 +146,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.38125224854
-    y: 4140954.377104722
-    theta: -1.8161319887998979
-    kappa: 0.011622441322471273
+    x: 587260.38118789659
+    y: 4140954.3771210876
+    theta: -1.8162661241313911
+    kappa: 0.011494620003210505
     s: 0.89426938962625446
-    dkappa: -0.0026823736466107654
+    dkappa: -0.0026301082636803743
     ddkappa: 0
   }
   v: 4.3464926524460745
@@ -160,12 +160,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.36005637026
-    y: 4140954.29304773
-    theta: -1.8150906063145067
-    kappa: 0.011390428213571243
+    x: 587260.35998229042
+    y: 4140954.2930665496
+    theta: -1.8152364585839
+    kappa: 0.011267089553459167
     s: 0.98095758451542137
-    dkappa: -0.0026774955140354568
+    dkappa: -0.0026256843313459113
     ddkappa: 0
   }
   v: 4.3223620674068295
@@ -174,12 +174,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.3393460185
-    y: 4140954.2093649646
-    theta: -1.8141450606964429
-    kappa: 0.0111614754236595
+    x: 587260.33925826836
+    y: 4140954.2093871636
+    theta: -1.8143008778682623
+    kappa: 0.011042414025075744
     s: 1.0671652719073121
-    dkappa: -0.0026728611446242044
+    dkappa: -0.0026215120864498359
     ddkappa: 0
   }
   v: 4.298441421136582
@@ -188,12 +188,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.31885567249
-    y: 4140954.1261182828
-    theta: -1.8132305707259975
-    kappa: 0.010934296653890466
+    x: 587260.31875319174
+    y: 4140954.1261441084
+    theta: -1.8133958165762853
+    kappa: 0.010819437140367531
     s: 1.1528966217875265
-    dkappa: -0.0026683145490657978
+    dkappa: -0.0026174280640151
     ddkappa: 0
   }
   v: 4.2747278399139459
@@ -202,12 +202,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.29847819125
-    y: 4140954.0433301395
-    theta: -1.812321116215595
-    kappa: 0.01070836922826995
+    x: 587260.29836106114
+    y: 4140954.0433595725
+    theta: -1.8124957388621799
+    kappa: 0.010597688455020637
     s: 1.238155746982633
-    dkappa: -0.0026637929970259977
+    dkappa: -0.0026133665371576636
     ddkappa: 0
   }
   v: 4.2512184815571059
@@ -216,12 +216,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.27821260516
-    y: 4140953.9609965952
-    theta: -1.8114166554174775
-    kappa: 0.010483682398047763
+    x: 587260.27808090625
+    y: 4140953.9610296162
+    theta: -1.8116006033777512
+    kappa: 0.010377157419093109
     s: 1.322946703790959
-    dkappa: -0.0026592962733869036
+    dkappa: -0.0026093273126458392
     ddkappa: 0
   }
   v: 4.22791053542382
@@ -230,12 +230,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.25805795821
-    y: 4140953.8791137678
-    theta: -1.8105171471307855
-    kappa: 0.010260225562595872
+    x: 587260.25791177014
+    y: 4140953.8791503557
+    theta: -1.8107103693170663
+    kappa: 0.010157833628025508
     s: 1.407273492613383
-    dkappa: -0.002654824165995029
+    dkappa: -0.0026053101999107525
     ddkappa: 0
   }
   v: 4.2048012224114153
@@ -244,12 +244,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.23801330756
-    y: 4140953.7976778247
-    theta: -1.8096225506961803
-    kappa: 0.0100379882677369
+    x: 587260.23785270937
+    y: 4140953.797717961
+    theta: -1.8098249964111042
+    kappa: 0.0099397068210003037
     s: 1.4911400585841257
-    dkappa: -0.0026503764656278452
+    dkappa: -0.0026013150110162889
     ddkappa: 0
   }
   v: 4.1818877949567952
@@ -258,12 +258,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.21843918425
-    y: 4140953.7165969778
-    theta: -1.8088298165336671
-    kappa: 0.009819593047970223
+    x: 587260.21826069057
+    y: 4140953.7166414345
+    theta: -1.8090396633210728
+    kappa: 0.0097251952354439255
     s: 1.5745502922015413
-    dkappa: -0.0026446255934327906
+    dkappa: -0.0025960223538153745
     ddkappa: 0
   }
   v: 4.1591675370364332
@@ -272,12 +272,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.19901552272
-    y: 4140953.6359452102
-    theta: -1.8080532610953535
-    kappa: 0.0096027050911849723
+    x: 587260.19881880842
+    y: 4140953.6359940572
+    theta: -1.8082702556245831
+    kappa: 0.00951214489573264
     s: 1.6575080299589098
-    dkappa: -0.0026387433375739022
+    dkappa: -0.0025905967741827753
     ddkappa: 0
   }
   v: 4.136637764166375
@@ -286,12 +286,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.17969692242
-    y: 4140953.5557296826
-    theta: -1.8072809049518335
-    kappa: 0.0093869902665537665
+    x: 587260.179482086
+    y: 4140953.5557828955
+    theta: -1.8075050085896758
+    kappa: 0.00930024693076659
     s: 1.7400170549752272
-    dkappa: -0.0026328928984354471
+    dkappa: -0.0025852005411400726
     ddkappa: 0
   }
   v: 4.1142958234022382
@@ -300,12 +300,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.16048250988
-    y: 4140953.4759467673
-    theta: -1.8065127141144575
-    kappa: 0.0091724388212368717
+    x: 587260.1602496492
+    y: 4140953.4760043239
+    theta: -1.8067438885236473
+    kappa: 0.0090894917602728637
     s: 1.822081097625998
-    dkappa: -0.0026270740115089683
+    dkappa: -0.0025798334107142522
     ddkappa: 0
   }
   v: 4.0921390933392132
@@ -314,12 +314,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.141371424
-    y: 4140953.3965928904
-    theta: -1.8057486550527067
-    kappa: 0.0089590411386400361
+    x: 587260.1411206359
+    y: 4140953.3966547665
+    theta: -1.8059868621882467
+    kappa: 0.0088798699378132909
     s: 1.9037038361740259
-    dkappa: -0.0026212864159811475
+    dkappa: -0.0025744951423405603
     ddkappa: 0
   }
   v: 4.07016498411206
@@ -328,12 +328,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.12236281554
-    y: 4140953.3176645241
-    theta: -1.8049886946890814
-    kappa: 0.008746787736765323
+    x: 587260.12209419627
+    y: 4140953.3177306973
+    theta: -1.8052338967945982
+    kappa: 0.00867137214916448
     s: 1.9848888974002066
-    dkappa: -0.0026155298546890761
+    dkappa: -0.0025691854988212494
     ddkappa: 0
   }
   v: 4.0483709373951156
@@ -342,12 +342,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.10373317078
-    y: 4140953.2390920436
-    theta: -1.8043174498875429
-    kappa: 0.0085380082763920739
+    x: 587260.1034444063
+    y: 4140953.2391629936
+    theta: -1.8045681388181765
+    kappa: 0.0084661457922764281
     s: 2.0656398572343164
-    dkappa: -0.0026097606788737862
+    dkappa: -0.0025638656673359749
     ddkappa: 0
   }
   v: 4.0267544264022845
@@ -356,12 +356,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.0852677729
-    y: 4140953.1609230395
-    theta: -1.8036695967054988
-    kappa: 0.0083308895326765185
+    x: 587260.08495842491
+    y: 4140953.1609988548
+    theta: -1.8039254044442
+    kappa: 0.0082625186480778691
     s: 2.1459602413858061
-    dkappa: -0.0026040121079373172
+    dkappa: -0.0025585651695379089
     ddkappa: 0
   }
   v: 4.0053129558870451
@@ -370,12 +370,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.06690056389
-    y: 4140953.083169695
-    theta: -1.8030251877484065
-    kappa: 0.00812487213235969
+    x: 587260.06657074182
+    y: 4140953.08325035
+    theta: -1.8032860870926453
+    kappa: 0.0080599742808749666
     s: 2.2258535259745935
-    dkappa: -0.0025982941047347934
+    dkappa: -0.0025532928568676345
     ddkappa: 0
   }
   v: 3.984044062142448
@@ -384,12 +384,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.04863075586
-    y: 4140953.0058286749
-    theta: -1.8023841959153553
-    kappa: 0.0079199472379674618
+    x: 587260.04828056809
+    y: 4140953.0059141433
+    theta: -1.802650159868266
+    kappa: 0.0078585040021753837
     s: 2.3053231381618509
-    dkappa: -0.0025926064239825387
+    dkappa: -0.0025480485031601466
     ddkappa: 0
   }
   v: 3.9629453130011161
@@ -398,12 +398,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.03045757185
-    y: 4140952.9288966917
-    theta: -1.8017465944782596
-    kappa: 0.007716106136646938
+    x: 587260.0300871263
+    y: 4140952.9289869475
+    theta: -1.8020175962459604
+    kappa: 0.0076580992460071468
     s: 2.3843724567807989
-    dkappa: -0.0025869488238557327
+    dkappa: -0.0025428318854396918
     ddkappa: 0
   }
   v: 3.9420143078352434
@@ -412,12 +412,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587260.01238024619
-    y: 4140952.8523705015
-    theta: -1.8011123570772158
-    kappa: 0.0075133402385398444
+    x: 587260.01198964962
+    y: 4140952.8524655206
+    theta: -1.8013883700661579
+    kappa: 0.0074587515673194714
     s: 2.4630048129674997
-    dkappa: -0.0025813210659432657
+    dkappa: -0.002537642783878147
     ddkappa: 0
   }
   v: 3.9212486775565969
@@ -426,12 +426,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.99454522645
-    y: 4140952.7762124236
-    theta: -1.8005345479091637
-    kappa: 0.0073130645201597913
+    x: 587259.99413350224
+    y: 4140952.7763123917
+    theta: -1.800814678701635
+    kappa: 0.0072617606618562754
     s: 2.5412234907916451
-    dkappa: -0.0025758099772666444
+    dkappa: -0.0025325699545407036
     ddkappa: 0
   }
   v: 3.9006460846165156
@@ -440,12 +440,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.97693672043
-    y: 4140952.7004228309
-    theta: -1.8000077121904727
-    kappa: 0.0071151251220132328
+    x: 587259.97650302725
+    y: 4140952.7005279064
+    theta: -1.8002911653368465
+    kappa: 0.0070669848378116309
     s: 2.6190317278873483
-    dkappa: -0.0025704064262435608
+    dkappa: -0.0025276041040824698
     ddkappa: 0
   }
   v: 3.8802042230059097
@@ -454,12 +454,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.95942037739
-    y: 4140952.6250299225
-    theta: -1.7994836334442934
-    kappa: 0.006918221740167714
+    x: 587259.95896483038
+    y: 4140952.6251400788
+    theta: -1.7997703915645356
+    kappa: 0.0068732284718973444
     s: 2.6964327160839385
-    dkappa: -0.0025650311574472377
+    dkappa: -0.0025226642449229439
     ddkappa: 0
   }
   v: 3.8599208182552633
@@ -468,12 +468,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.94199548545
-    y: 4140952.5500306319
-    theta: -1.7989622906580747
-    kappa: 0.0067223463697244435
+    x: 587259.94151819847
+    y: 4140952.5501458421
+    theta: -1.7992523365009976
+    kappa: 0.0066804836871532276
     s: 2.7734296020367504
-    dkappa: -0.0025596839523518073
+    dkappa: -0.0025177501762373745
     ddkappa: 0
   }
   v: 3.8397936274346307
@@ -482,12 +482,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.92466134229
-    y: 4140952.4754219376
-    theta: -1.7984436631118466
-    kappa: 0.00652749111909982
+    x: 587259.92416242883
+    y: 4140952.4755421765
+    theta: -1.7987369795533759
+    kappa: 0.0064887427181232245
     s: 2.8500254878579128
-    dkappa: -0.002554364595524793
+    dkappa: -0.0025128617000438327
     ddkappa: 0
   }
   v: 3.8198204391536397
@@ -496,12 +496,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.907417256
-    y: 4140952.4012008617
-    theta: -1.7979277303741772
-    kappa: 0.0063336482084207276
+    x: 587259.90689682832
+    y: 4140952.4013261022
+    theta: -1.7982243004156433
+    kappa: 0.0062979979092763387
     s: 2.9262234317471463
-    dkappa: -0.0025490728745833054
+    dkappa: -0.00250799862116295
     ddkappa: 0
   }
   v: 3.799999073561489
@@ -510,12 +510,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.89026820147
-    y: 4140952.3273631614
-    theta: -1.7974168128624297
-    kappa: 0.0061408866520829344
+    x: 587259.88972633064
+    y: 4140952.3274933849
+    theta: -1.7977165786752454
+    kappa: 0.0061083129670703442
     s: 3.0020264486225483
-    dkappa: -0.00254378546603945
+    dkappa: -0.0025031377272780207
     ddkappa: 0
   }
   v: 3.7803273823469503
@@ -524,12 +524,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.87342960131
-    y: 4140952.253856088
-    theta: -1.7970002948073831
-    kappa: 0.0059521280685674383
+    x: 587259.87286521867
+    y: 4140952.2539914721
+    theta: -1.7973018008972073
+    kappa: 0.0059224073563934228
     s: 3.0774375107513885
-    dkappa: -0.0025376192512513293
+    dkappa: -0.0024973978479598717
     ddkappa: 0
   }
   v: 3.7608032487383669
@@ -538,12 +538,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.85667786677
-    y: 4140952.1807282167
-    theta: -1.7965859251234544
-    kappa: 0.005764343237593986
+    x: 587259.8560910885
+    y: 4140952.1808687351
+    theta: -1.7968891625167827
+    kappa: 0.005737460780572589
     s: 3.1524595483808997
-    dkappa: -0.0025314848462342546
+    dkappa: -0.0024916875790677658
     ddkappa: 0
   }
   v: 3.7414245875036549
@@ -552,12 +552,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.8400123528
-    y: 4140952.1079767328
-    theta: -1.7961736880008767
-    kappa: 0.0055775249281711792
+    x: 587259.83940329426
+    y: 4140952.1081223581
+    theta: -1.796478647789046
+    kappa: 0.005553466117908544
     s: 3.2270954503690685
-    dkappa: -0.00252538201477197
+    dkappa: -0.0024860067007175662
     ddkappa: 0
   }
   v: 3.7221893449503018
@@ -566,12 +566,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.82343242329
-    y: 4140952.0355988583
-    theta: -1.795763567850591
-    kappa: 0.0053916660113285742
+    x: 587259.82280119881
+    y: 4140952.0357495649
+    theta: -1.796070241188894
+    kappa: 0.005370416347180957
     s: 3.3013480648154259
-    dkappa: -0.002519310523980957
+    dkappa: -0.00248035499612745
     ddkappa: 0
   }
   v: 3.7030954989253675
@@ -580,12 +580,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.80693745147
-    y: 4140951.9635918569
-    theta: -1.79535554930087
-    kappa: 0.0052067594585377582
+    x: 587259.80628417479
+    y: 4140951.9637476192
+    theta: -1.7956639274076822
+    kappa: 0.0051883045460934113
     s: 3.3752201996918414
-    dkappa: -0.0025132701442588572
+    dkappa: -0.002474732251569892
     ddkappa: 0
   }
   v: 3.6841410588154835
@@ -594,12 +594,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.79052681907
-    y: 4140951.89195303
-    theta: -1.7949496171939405
-    kappa: 0.0050227983401334537
+    x: 587259.78985160275
+    y: 4140951.8921138216
+    theta: -1.7952596913498626
+    kappa: 0.0050071238897183646
     s: 3.4487146234733119
-    dkappa: -0.0025072606492328956
+    dkappa: -0.0024691382563236533
     ddkappa: 0
   }
   v: 3.6653240655468542
@@ -608,12 +608,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.77425186685
-    y: 4140951.8206678843
-    theta: -1.7945728433831698
-    kappa: 0.0048406314473273381
+    x: 587259.77355466667
+    y: 4140951.8208336979
+    theta: -1.7948842057000962
+    kappa: 0.0048276614932636
     s: 3.5218340657687537
-    dkappa: -0.0025013506526090461
+    dkappa: -0.0024636419456023195
     ddkappa: 0
   }
   v: 3.6466425915852558
@@ -622,12 +622,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.758181657
-    y: 4140951.7497179261
-    theta: -1.7942615350378301
-    kappa: 0.0046613994017677659
+    x: 587259.75746223622
+    y: 4140951.7498887768
+    theta: -1.7945732515779012
+    kappa: 0.0046509755113435837
     s: 3.594581217951796
-    dkappa: -0.0024956322419548739
+    dkappa: -0.0024583358565572794
     ddkappa: 0
   }
   v: 3.6280947409360365
@@ -636,12 +636,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.74219310156
-    y: 4140951.6791284732
-    theta: -1.7939518083566597
-    kappa: 0.004483078054097105
+    x: 587259.74145157321
+    y: 4140951.6793043348
+    theta: -1.794263877320464
+    kappa: 0.0044751872904809295
     s: 3.6669587337915686
-    dkappa: -0.0024899428871790431
+    dkappa: -0.0024530567283387846
     ddkappa: 0
   }
   v: 3.609678649144116
@@ -650,12 +650,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.72628562246
-    y: 4140951.6088969726
-    theta: -1.7936436522024728
-    kappa: 0.0043056609571406838
+    x: 587259.72552209871
+    y: 4140951.6090778192
+    theta: -1.7939560718031327
+    kappa: 0.0043002904750856878
     s: 3.7389692300834971
-    dkappa: -0.0024842823825840053
+    dkappa: -0.0024478043700809507
     ddkappa: 0
   }
   v: 3.5913924832939879
@@ -664,12 +664,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.71045865
-    y: 4140951.5390209081
-    theta: -1.7933370555917922
-    kappa: 0.0041291417532643131
+    x: 587259.70967324206
+    y: 4140951.5392067148
+    theta: -1.7936498240547933
+    kappa: 0.0041262787978364329
     s: 3.8106152872800916
-    dkappa: -0.0024786505253290079
+    dkappa: -0.0024425785935687019
     ddkappa: 0
   }
   v: 3.5732344420097153
@@ -678,12 +678,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.69471162173
-    y: 4140951.4694977966
-    theta: -1.7930320076921946
-    kappa: 0.00395351417282016
+    x: 587259.69390444015
+    y: 4140951.4696885394
+    theta: -1.7933451232552187
+    kappa: 0.0039531460781482121
     s: 3.88189945012174
-    dkappa: -0.0024730471153805085
+    dkappa: -0.0024373792131917598
     ddkappa: 0
   }
   v: 3.5552027554549364
@@ -692,12 +692,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.67904398334
-    y: 4140951.4003251917
-    theta: -1.7927284978196534
-    kappa: 0.0037787720325926223
+    x: 587259.6782151378
+    y: 4140951.4005208449
+    theta: -1.7930419587324118
+    kappa: 0.0037808862206405005
     s: 3.9528242282674988
-    dkappa: -0.0024674719554625916
+    dkappa: -0.0024322060458986374
     ddkappa: 0
   }
   v: 3.5372956853328583
@@ -706,12 +706,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.66349636577
-    y: 4140951.3314913912
-    theta: -1.7924554137857023
-    kappa: 0.0036058284252124145
+    x: 587259.6626460033
+    y: 4140951.3316919082
+    theta: -1.792768824266989
+    kappa: 0.00361034591360511
     s: 4.0233920969258818
-    dkappa: -0.0024619283629668731
+    dkappa: -0.0024270625588169972
     ddkappa: 0
   }
   v: 3.5195115248862634
@@ -720,12 +720,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.58732707123
-    y: 4140950.9923702562
-    theta: -1.7913966082129185
-    kappa: 0.0027631283004490893
+    x: 587259.58637111576
+    y: 4140950.9925945071
+    theta: -1.7917058747323678
+    kappa: 0.0027788244919743845
     s: 4.3709620981471575
-    dkappa: -0.0024346591277919502
+    dkappa: -0.0024017653196505167
     ddkappa: 0
   }
   v: 3.4323768390528997
@@ -734,12 +734,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.513296956
-    y: 4140950.6615524967
-    theta: -1.7906126804322127
-    kappa: 0.0019503268285172722
+    x: 587259.51224001683
+    y: 4140950.6617993629
+    theta: -1.7909146769949429
+    kappa: 0.0019763300812609308
     s: 4.70996193828592
-    dkappa: -0.0024063036380341379
+    dkappa: -0.0023753333812638212
     ddkappa: 0
   }
   v: 3.3480770167776783
@@ -748,12 +748,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.44125877472
-    y: 4140950.3387898854
-    theta: -1.7900455281163814
-    kappa: 0.0011645304770842543
+    x: 587259.44010520726
+    y: 4140950.3390583349
+    theta: -1.7903379318590478
+    kappa: 0.0012001158995042741
     s: 5.0406660462976918
-    dkappa: -0.0023778281265767979
+    dkappa: -0.0023487344213703426
     ddkappa: 0
   }
   v: 3.2664341954268172
@@ -762,12 +762,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.37113819586
-    y: 4140950.023835199
-    theta: -1.7898286826810839
-    kappa: 0.00040928140005196291
+    x: 587259.36989507719
+    y: 4140950.0241235988
+    theta: -1.7901077744137885
+    kappa: 0.00045341421560961769
     s: 5.3633320504862416
-    dkappa: -0.0023516900213764523
+    dkappa: -0.0023244265666355146
     ddkappa: 0
   }
   v: 3.1872902245990953
@@ -776,12 +776,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.30271513131
-    y: 4140949.7164887469
-    theta: -1.7898253151482078
-    kappa: -0.00032032690970575464
+    x: 587259.301388777
+    y: 4140949.7167956885
+    theta: -1.7900891867681892
+    kappa: -0.00026835908107524116
     s: 5.6782027497268466
-    dkappa: -0.0023258593764828449
+    dkappa: -0.0023003813927809742
     ddkappa: 0
   }
   v: 3.1105066661258549
@@ -790,12 +790,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.23593864182
-    y: 4140949.4165263004
-    theta: -1.7899770568349531
-    kappa: -0.0010268998742332025
+    x: 587259.23453414231
+    y: 4140949.4168506493
+    theta: -1.7902243958542217
+    kappa: -0.00096766250901368785
     s: 5.9855080846895454
-    dkappa: -0.0023004081440002187
+    dkappa: -0.0022766723421044966
     ddkappa: 0
   }
   v: 3.0359647940710026
@@ -804,12 +804,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.170632086
-    y: 4140949.1237627966
-    theta: -1.7904450504475491
-    kappa: -0.0017039484501551378
+    x: 587259.16915961227
+    y: 4140949.1241023163
+    theta: -1.7906730845472556
+    kappa: -0.001638424993991589
     s: 6.2854671090623953
-    dkappa: -0.00227398004406989
+    dkappa: -0.0022519438538918879
     ddkappa: 0
   }
   v: 2.9635655947310058
@@ -818,12 +818,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.1067838301
-    y: 4140948.8379856106
-    theta: -1.7910067688775662
-    kappa: -0.0023611014079660407
+    x: 587259.10524807777
+    y: 4140948.8383392729
+    theta: -1.7912150447986375
+    kappa: -0.0022897034445814924
     s: 6.57828996077473
-    dkappa: -0.0022487080449725395
+    dkappa: -0.0022283309928160478
     ddkappa: 0
   }
   v: 2.8932297666348967
@@ -832,12 +832,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587259.04421105853
-    y: 4140948.5590274283
-    theta: -1.7917921360887277
-    kappa: -0.0029942703964006548
+    x: 587259.042620677
+    y: 4140948.55939335
+    theta: -1.791979073994181
+    kappa: -0.0029177288007921245
     s: 6.8641798332204109
-    dkappa: -0.0022256276961395115
+    dkappa: -0.0022068698060978033
     ddkappa: 0
   }
   v: 2.8248977205442687
@@ -846,12 +846,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.98287910863
-    y: 4140948.2866932303
-    theta: -1.7927173024075787
-    kappa: -0.0036064814348801258
+    x: 587258.98124090675
+    y: 4140948.2870699214
+    theta: -1.7928822341738238
+    kappa: -0.0035253245466429434
     s: 7.143334946481092
-    dkappa: -0.0022026423072929492
+    dkappa: -0.00218546510801651
     ddkappa: 0
   }
   v: 2.7585295794532811
@@ -860,12 +860,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.92277073266
-    y: 4140948.020786948
-    theta: -1.7937655318648502
-    kappa: -0.0041988258256243664
+    x: 587258.92109094909
+    y: 4140948.0211730422
+    theta: -1.7939078544211728
+    kappa: -0.0041135357726866575
     s: 7.415950367122246
-    dkappa: -0.0021797850940146464
+    dkappa: -0.002164151392230538
     ddkappa: 0
   }
   v: 2.694098954518231
@@ -874,12 +874,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.86368963763
-    y: 4140947.7611567592
-    theta: -1.7949823210382208
-    kappa: -0.0047693702501630884
+    x: 587258.86197692272
+    y: 4140947.7615503436
+    theta: -1.7951013299784375
+    kappa: -0.0046805605319354994
     s: 7.6822180465464331
-    dkappa: -0.0021568010107599982
+    dkappa: -0.0021426745065254433
     ddkappa: 0
   }
   v: 2.6315675935819862
@@ -888,12 +888,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.80581044755
-    y: 4140947.5075704409
-    theta: -1.7962564297928876
-    kappa: -0.0053231687718212576
+    x: 587258.8040690471
+    y: 4140947.5079705752
+    theta: -1.7963520765574994
+    kappa: -0.0052311515855097222
     s: 7.9423257572679429
-    dkappa: -0.0021340567298064428
+    dkappa: -0.0021214025421649029
     ddkappa: 0
   }
   v: 2.5708923646726807
@@ -902,12 +902,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.748716779
-    y: 4140947.2599358661
-    theta: -1.7977140655453014
-    kappa: -0.0058564579053353352
+    x: 587258.74695619568
+    y: 4140947.2603404173
+    theta: -1.7977858432356755
+    kappa: -0.0057618647360724983
     s: 8.19645694232586
-    dkappa: -0.0021135383885451707
+    dkappa: -0.0021023228100491976
     ddkappa: 0
   }
   v: 2.5120298102580567
@@ -916,12 +916,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.69277334015
-    y: 4140947.0179855023
-    theta: -1.7991979607687973
-    kappa: -0.0063754041969361387
+    x: 587258.69099660183
+    y: 4140947.01839379
+    theta: -1.7992460757135622
+    kappa: -0.00627845137627231
     s: 8.4447906827280228
-    dkappa: -0.002093964474725811
+    dkappa: -0.0020841543486914229
     ddkappa: 0
   }
   v: 2.454936147245466
@@ -930,12 +930,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.637499234
-    y: 4140946.7816525036
-    theta: -1.800843468259222
-    kappa: -0.0068748675424292758
+    x: 587258.635715548
+    y: 4140946.7820624085
+    theta: -1.8008677322100282
+    kappa: -0.00677614095265877
     s: 8.6875016648949863
-    dkappa: -0.0020742428254473012
+    dkappa: -0.0020658059299686862
     ddkappa: 0
   }
   v: 2.399567266981868
@@ -944,12 +944,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.58330002578
-    y: 4140946.5506675821
-    theta: -1.8025065333509558
-    kappa: -0.0073609506466237786
+    x: 587258.5815121748
+    y: 4140946.5510784648
+    theta: -1.8025072278364163
+    kappa: -0.0072606407729368851
     s: 8.9247601481039815
-    dkappa: -0.0020547991785036159
+    dkappa: -0.0020477047103525004
     ddkappa: 0
   }
   v: 2.3458787352538297
@@ -958,12 +958,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.52975061769
-    y: 4140946.3249615431
-    theta: -1.8042891766208575
-    kappa: -0.0078295238030384427
+    x: 587258.527966293
+    y: 4140946.3253715797
+    theta: -1.8042664547394391
+    kappa: -0.0077281112753544729
     s: 9.1567319319328782
-    dkappa: -0.002035387873815409
+    dkappa: -0.0020296054196113581
     ddkappa: 0
   }
   v: 2.2938257922875289
@@ -972,12 +972,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.47712970036
-    y: 4140946.1043027015
-    theta: -1.8061039412689213
-    kappa: -0.0082846949918596938
+    x: 587258.47535252268
+    y: 4140946.1047110343
+    theta: -1.8060580851010797
+    kappa: -0.0081824100798358109
     s: 9.383578323704139
-    dkappa: -0.0020162224440317322
+    dkappa: -0.00201172295400121
     ddkappa: 0
   }
   v: 2.2433633527487489
@@ -986,12 +986,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.42520652106
-    y: 4140945.8885864555
-    theta: -1.8079875258184706
-    kappa: -0.00872583345019024
+    x: 587258.42344145547
+    y: 4140945.88899186
+    theta: -1.8079189635932476
+    kappa: -0.0086229958452625349
     s: 9.6054561059287913
-    dkappa: -0.0019983888756598686
+    dkappa: -0.00199514440082956
     ddkappa: 0
   }
   v: 2.1944460057428845
@@ -1000,12 +1000,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.37393279374
-    y: 4140945.6776678567
-    theta: -1.8099442547342899
-    kappa: -0.0091531287088337174
+    x: 587258.37218539964
+    y: 4140945.6780689666
+    theta: -1.8098532896323827
+    kappa: -0.0090500698009267962
     s: 9.8225175037503814
-    dkappa: -0.0019819003272973988
+    dkappa: -0.0019798828847152538
     ddkappa: 0
   }
   v: 2.1470280148149357
@@ -1014,12 +1014,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.32359193242
-    y: 4140945.4713276336
-    theta: -1.8118947135804484
-    kappa: -0.0095698226240034882
+    x: 587258.321863394
+    y: 4140945.4717241367
+    theta: -1.8117818911877943
+    kappa: -0.0094666484067137679
     s: 10.034910152388932
-    dkappa: -0.0019657235341596557
+    dkappa: -0.0019649064770051392
     ddkappa: 0
   }
   v: 2.1010633179495137
@@ -1028,12 +1028,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.27352437563
-    y: 4140945.2695805319
-    theta: -1.81397208068879
-    kappa: -0.0099710030975478942
+    x: 587258.27182316291
+    y: 4140945.2699702559
+    theta: -1.8138378428026787
+    kappa: -0.0098682039002351528
     s: 10.24277706458491
-    dkappa: -0.0019496897099983623
+    dkappa: -0.0019500466502811076
     ddkappa: 0
   }
   v: 2.0565055275708368
@@ -1042,12 +1042,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.22451357846
-    y: 4140945.0720916404
-    theta: -1.8160056020716056
-    kappa: -0.010363715986419896
+    x: 587258.22283911472
+    y: 4140945.0724747283
+    theta: -1.8158504006791187
+    kappa: -0.010261283893660482
     s: 10.446256598043185
-    dkappa: -0.0019339943065047271
+    dkappa: -0.0019355004650458837
     ddkappa: 0
   }
   v: 2.0133079305427315
@@ -1056,12 +1056,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.17580136284
-    y: 4140944.8789133504
-    theta: -1.8181337695594275
-    kappa: -0.010742348900807767
+    x: 587258.17416006466
+    y: 4140944.8792880671
+    theta: -1.8179583376633053
+    kappa: -0.010640689198290122
     s: 10.64548242287699
-    dkappa: -0.0019182439878024401
+    dkappa: -0.0019208740873206602
     ddkappa: 0
   }
   v: 1.9714234881686332
@@ -1070,12 +1070,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.12784690363
-    y: 4140944.6897975244
-    theta: -1.8202652641520423
-    kappa: -0.011111114803535792
+    x: 587258.12624090782
+    y: 4140944.690163292
+    theta: -1.8200700430630707
+    kappa: -0.011010357289055247
     s: 10.84058348905188
-    dkappa: -0.0019026874531223748
+    dkappa: -0.0019064180118227416
     ddkappa: 0
   }
   v: 1.9308048361915864
@@ -1084,12 +1084,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.08068857645
-    y: 4140944.5046075676
-    theta: -1.8223847753918139
-    kappa: -0.011471095120285274
+    x: 587258.0791183922
+    y: 4140944.5049642129
+    theta: -1.8221703381903112
+    kappa: -0.011371313513380601
     s: 11.031683993829699
-    dkappa: -0.0018876790944519779
+    dkappa: -0.0018924877486131613
     ddkappa: 0
   }
   v: 1.8914042847942425
@@ -1098,12 +1098,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587258.03362786025
-    y: 4140944.3233994665
-    theta: -1.8246070633350162
-    kappa: -0.011818139636508141
+    x: 587258.03210057679
+    y: 4140944.323744975
+    theta: -1.8243741991153171
+    kappa: -0.011719750983566257
     s: 11.218903349212528
-    dkappa: -0.0018740287504066396
+    dkappa: -0.0018798918769981833
     ddkappa: 0
   }
   v: 1.8531738185988629
@@ -1112,12 +1112,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.98751393054
-    y: 4140944.1458369833
-    theta: -1.8267846458430905
-    kappa: -0.012158202169871629
+    x: 587257.98602868512
+    y: 4140944.14617158
+    theta: -1.826533725234075
+    kappa: -0.012061178446877638
     s: 11.402356149386666
-    dkappa: -0.0018606530295398678
+    dkappa: -0.001867549414256142
     ddkappa: 0
   }
   v: 1.816065096667316
@@ -1126,12 +1126,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.94180569821
-    y: 4140943.9719489887
-    theta: -1.8289977689055392
-    kappa: -0.012488569428150173
+    x: 587257.940365085
+    y: 4140943.9722718461
+    theta: -1.82872959452423
+    kappa: -0.012393117529302529
     s: 11.58215213816657
-    dkappa: -0.0018478443056870225
+    dkappa: -0.0018557527452878306
     ddkappa: 0
   }
   v: 1.78002945250108
@@ -1140,12 +1140,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.896428087
-    y: 4140943.8016463798
-    theta: -1.8312551807895232
-    kappa: -0.012809160426497358
+    x: 587257.89503600681
+    y: 4140943.8019563123
+    theta: -1.8309704372641478
+    kappa: -0.012715515224740504
     s: 11.758396588954552
-    dkappa: -0.0018356233399868063
+    dkappa: -0.0018445226042892822
     ddkappa: 0
   }
   v: 1.7450342610845011
@@ -1154,12 +1154,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.8519378308
-    y: 4140943.634674021
-    theta: -1.8334684480217549
-    kappa: -0.013123482298091533
+    x: 587257.8505933343
+    y: 4140943.6349712824
+    theta: -1.8331674593763667
+    kappa: -0.013031608463634162
     s: 11.931194596120596
-    dkappa: -0.0018236413541188802
+    dkappa: -0.0018335120676393802
     ddkappa: 0
   }
   v: 1.7111066965869794
@@ -1168,12 +1168,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.80766611733
-    y: 4140943.4711002368
-    theta: -1.8357290366643235
-    kappa: -0.013427951974717888
+    x: 587257.80637226126
+    y: 4140943.4713837882
+    theta: -1.8354127856926281
+    kappa: -0.013338100583772489
     s: 12.100654484716619
-    dkappa: -0.0018114522313560197
+    dkappa: -0.0018222739028463351
     ddkappa: 0
   }
   v: 1.6782752802348841
@@ -1182,12 +1182,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.76382913324
-    y: 4140943.3107523657
-    theta: -1.8380040056879299
-    kappa: -0.013724212840044865
+    x: 587257.76258829737
+    y: 4140943.3110214313
+    theta: -1.8376730935904184
+    kappa: -0.013636531666118881
     s: 12.266886614125019
-    dkappa: -0.0017992157081762534
+    dkappa: -0.0018109696852105757
     ddkappa: 0
   }
   v: 1.6465521774113936
@@ -1196,12 +1196,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.7208141319
-    y: 4140943.1534111612
-    theta: -1.8402363145383305
-    kappa: -0.014014918549252063
+    x: 587257.71962532192
+    y: 4140943.1536660129
+    theta: -1.8398910162604205
+    kappa: -0.01392936689883593
     s: 12.430001742474342
-    dkappa: -0.0017872086307471158
+    dkappa: -0.0017998774317628955
     ddkappa: 0
   }
   v: 1.6159331976564952
@@ -1210,12 +1210,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.677977246
-    y: 4140942.9991413956
-    theta: -1.8425071049024597
-    kappa: -0.014297001155543115
+    x: 587257.67684264458
+    y: 4140942.999381193
+    theta: -1.8421484262606342
+    kappa: -0.01421379497625948
     s: 12.590109391054989
-    dkappa: -0.0017756850961049616
+    dkappa: -0.0017892508417190717
     ddkappa: 0
   }
   v: 1.5863977946669854
@@ -1224,12 +1224,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.63547203282
-    y: 4140942.84778985
-    theta: -1.8447943353882683
-    kappa: -0.014571614739056778
+    x: 587257.63439389679
+    y: 4140942.848013801
+    theta: -1.844422926267359
+    kappa: -0.014490900737099539
     s: 12.747316208734865
-    dkappa: -0.0017645598056247581
+    dkappa: -0.0017790051001606198
     ddkappa: 0
   }
   v: 1.557909066296469
@@ -1238,12 +1238,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.59372352285
-    y: 4140942.6991327554
-    theta: -1.8470408474450999
-    kappa: -0.014841339486880762
+    x: 587257.59270084719
+    y: 4140942.6993411426
+    theta: -1.8466569344642718
+    kappa: -0.01476307329501469
     s: 12.901724336375086
-    dkappa: -0.0017536325742371225
+    dkappa: -0.0017689417594373967
     ddkappa: 0
   }
   v: 1.5304137545553602
@@ -1252,12 +1252,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.55228388787
-    y: 4140942.553197966
-    theta: -1.8492990252091284
-    kappa: -0.015104707914767146
+    x: 587257.5513170294
+    y: 4140942.5533905039
+    theta: -1.8489033900654754
+    kappa: -0.015028999429323394
     s: 13.05342977124565
-    dkappa: -0.001743559533592003
+    dkappa: -0.0017597178625623796
     ddkappa: 0
   }
   v: 1.503842245610882
@@ -1266,12 +1266,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.51084138453
-    y: 4140942.4099826263
-    theta: -1.8516049919270148
-    kappa: -0.015360758262640882
+    x: 587257.50993346341
+    y: 4140942.4101581229
+    theta: -1.851198568766967
+    kappa: -0.015287829444822945
     s: 13.20252073144111
-    dkappa: -0.0017347868283306793
+    dkappa: -0.0017517774244729493
     ddkappa: 0
   }
   v: 1.4781085697870664
@@ -1280,12 +1280,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.47010371648
-    y: 4140942.2692030296
-    theta: -1.8538717472113511
-    kappa: -0.01561245382248504
+    x: 587257.46925373049
+    y: 4140942.2693617735
+    theta: -1.8534547193946702
+    kappa: -0.015542257396968344
     s: 13.34907602029627
-    dkappa: -0.0017261633252562544
+    dkappa: -0.0017439720337483389
     ddkappa: 0
   }
   v: 1.453110401564754
@@ -1294,12 +1294,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.43005205051
-    y: 4140942.1307940898
-    theta: -1.8561003159024596
-    kappa: -0.015859910953982481
+    x: 587257.42925902409
+    y: 4140942.1309363651
+    theta: -1.8556728622096044
+    kappa: -0.015792400908636373
     s: 13.493163390801858
-    dkappa: -0.0017176850376949728
+    dkappa: -0.0017362980819308144
     ddkappa: 0
   }
   v: 1.4287290595815951
@@ -1308,12 +1308,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.38964893459
-    y: 4140941.9950030046
-    theta: -1.8584050412284308
-    kappa: -0.016098632557464479
+    x: 587257.38891553041
+    y: 4140941.9951275494
+    theta: -1.8579687865065162
+    kappa: -0.01603414237472825
     s: 13.6348379100202
-    dkappa: -0.0017082270303295558
+    dkappa: -0.0017276280174841519
     ddkappa: 0
   }
   v: 1.4048295066320475
@@ -1322,12 +1322,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.34988804837
-    y: 4140941.8614955726
-    theta: -1.8606749997071679
-    kappa: -0.016333203287547872
+    x: 587257.34921409772
+    y: 4140941.861602426
+    theta: -1.8602300597369898
+    kappa: -0.016271698018603037
     s: 13.774140323500921
-    dkappa: -0.0016988897918004788
+    dkappa: -0.0017190662133802618
     ddkappa: 0
   }
   v: 1.3812603496673796
@@ -1336,12 +1336,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.31079715339
-    y: 4140941.73023781
-    theta: -1.8629067040402854
-    kappa: -0.016563821380232303
+    x: 587257.31018165452
+    y: 4140941.7303272691
+    theta: -1.8624532252051158
+    kappa: -0.016505250727746879
     s: 13.9110954196966
-    dkappa: -0.0016897098905410597
+    dkappa: -0.0017106486800773004
     ddkappa: 0
   }
   v: 1.3578538397956674
@@ -1350,12 +1350,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.27199944272
-    y: 4140941.6013360983
-    theta: -1.8651391402042166
-    kappa: -0.016788943937343936
+    x: 587257.271441894
+    y: 4140941.601408123
+    theta: -1.8646778940877375
+    kappa: -0.016733387835051284
     s: 14.045710394378473
-    dkappa: -0.0016807857977950253
+    dkappa: -0.0017024735101036246
     ddkappa: 0
   }
   v: 1.3344258722817968
@@ -1364,12 +1364,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.23321248218
-    y: 4140941.47488838
-    theta: -1.8674018133785055
-    kappa: -0.017007364504363576
+    x: 587257.23271483718
+    y: 4140941.4749420453
+    theta: -1.8669338292985189
+    kappa: -0.016955009777824836
     s: 14.177973215052102
-    dkappa: -0.0016721938036151685
+    dkappa: -0.0016946160590911076
     ddkappa: 0
   }
   v: 1.3107759865474617
@@ -1378,12 +1378,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.19512495364
-    y: 4140941.3507208508
-    theta: -1.869623696585845
-    kappa: -0.017221846367764561
+    x: 587257.19468613225
+    y: 4140941.3507564869
+    theta: -1.8691490959219983
+    kappa: -0.017172635287662212
     s: 14.30785098537306
-    dkappa: -0.0016637567459370775
+    dkappa: -0.0016869002988138481
     ddkappa: 0
   }
   v: 1.2866873661711657
@@ -1392,12 +1392,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.15775310236
-    y: 4140941.2288864697
-    theta: -1.8718038201578693
-    kappa: -0.017432298046227031
+    x: 587257.15737199923
+    y: 4140941.2289044168
+    theta: -1.8713227273086661
+    kappa: -0.017386171542407923
     s: 14.435288309562605
-    dkappa: -0.00165547822335487
+    dkappa: -0.0016793295202164639
     ddkappa: 0
   }
   v: 1.261926838888221
@@ -1406,12 +1406,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.12059381325
-    y: 4140941.1096253037
-    theta: -1.8739924890571074
-    kappa: -0.017636750655192393
+    x: 587257.1202697109
+    y: 4140941.1096254992
+    theta: -1.8735059129010645
+    kappa: -0.017593817926272494
     s: 14.560205656823378
-    dkappa: -0.0016477384386031542
+    dkappa: -0.0016722834468189697
     ddkappa: 0
   }
   v: 1.2362448765907486
@@ -1420,12 +1420,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.08369815443
-    y: 4140940.9930317346
-    theta: -1.8761859621555108
-    kappa: -0.017835100765343944
+    x: 587257.08343189373
+    y: 4140940.9930136418
+    theta: -1.8756947208014851
+    kappa: -0.017795467075859047
     s: 14.682497725755059
-    dkappa: -0.0016405298502115554
+    dkappa: -0.0016657527974578558
     ddkappa: 0
   }
   v: 1.2093755953276781
@@ -1434,12 +1434,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.0476345832
-    y: 4140940.8790676352
-    theta: -1.878329978119355
-    kappa: -0.018028977594315886
+    x: 587257.0474248596
+    y: 4140940.8790316666
+    theta: -1.877834176692287
+    kappa: -0.017992568542840134
     s: 14.802031808770076
-    dkappa: -0.001633483833157875
+    dkappa: -0.0016593694302477588
     ddkappa: 0
   }
   v: 1.1810367553047494
@@ -1448,12 +1448,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587257.01245189947
-    y: 4140940.7678872193
-    theta: -1.8804216159759437
-    kappa: -0.018218118794733711
+    x: 587257.01229733194
+    y: 4140940.7678338122
+    theta: -1.8799213658982967
+    kappa: -0.018184855616373358
     s: 14.918646156509258
-    dkappa: -0.0016266099218670603
+    dkappa: -0.0016531419829389307
     ddkappa: 0
   }
   v: 1.1509297608845097
@@ -1462,12 +1462,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.97790151683
-    y: 4140940.6597725716
-    theta: -1.8824858649160712
-    kappa: -0.018401261610396149
+    x: 587256.97780021175
+    y: 4140940.6597021557
+    theta: -1.8819818538838076
+    kappa: -0.018371152679474041
     s: 15.032148342257537
-    dkappa: -0.0016200365794850138
+    dkappa: -0.0016471979456680851
     ddkappa: 0
   }
   v: 1.1187396605863165
@@ -1476,12 +1476,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.9436778638
-    y: 4140940.5550580481
-    theta: -1.8845533791345799
-    kappa: -0.018576883987574226
+    x: 587256.94363013888
+    y: 4140940.5549701368
+    theta: -1.8840467282773938
+    kappa: -0.018550051953150723
     s: 15.14231362635962
-    dkappa: -0.0016139196624808559
+    dkappa: -0.0016416909624626824
     ddkappa: 0
   }
   v: 1.0841351470863345
@@ -1490,12 +1490,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.91057120729
-    y: 4140940.4537612125
-    theta: -1.8865534298474538
-    kappa: -0.018746774377241426
+    x: 587256.91057531349
+    y: 4140940.4536563749
+    theta: -1.8860442252470577
+    kappa: -0.018723112287513988
     s: 15.248883320635668
-    dkappa: -0.0016080023902624949
+    dkappa: -0.0016363637169391353
     ddkappa: 0
   }
   v: 1.0467685572175394
@@ -1504,12 +1504,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.87867296475
-    y: 4140940.3561617751
-    theta: -1.8884804774784154
-    kappa: -0.018910463658845845
+    x: 587256.87872701057
+    y: 4140940.3560406305
+    theta: -1.8879688123340539
+    kappa: -0.018889855808802244
     s: 15.351563152796984
-    dkappa: -0.0016023011022709927
+    dkappa: -0.0016312309192900868
     ddkappa: 0
   }
   v: 1.0062758719697138
@@ -1518,12 +1518,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.848086117
-    y: 4140940.2625748287
-    theta: -1.8903282873061436
-    kappa: -0.019067423377254381
+    x: 587256.84818804916
+    y: 4140940.2624380467
+    theta: -1.8898142628136871
+    kappa: -0.019049744201562756
     s: 15.45002163086169
-    dkappa: -0.0015968342045673616
+    dkappa: -0.0016263091402600488
     ddkappa: 0
   }
   v: 0.9622767164894509
@@ -1532,12 +1532,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.81851557991
-    y: 4140940.1734885778
-    theta: -1.8921252644365465
-    kappa: -0.019215674597672205
+    x: 587256.818662582
+    y: 4140940.1733368463
+    theta: -1.8916097817848208
+    kappa: -0.019200911882804123
     s: 15.5438884075704
-    dkappa: -0.0015913950496598032
+    dkappa: -0.0016213890365460303
     ddkappa: 0
   }
   v: 0.91437436008015183
@@ -1546,12 +1546,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.79011547659
-    y: 4140940.0892847371
-    theta: -1.8938614491972681
-    kappa: -0.0193546520665485
+    x: 587256.79030624637
+    y: 4140940.0891182562
+    theta: -1.8933452206574548
+    kappa: -0.019342778505632978
     s: 15.632752644801917
-    dkappa: -0.0015860211260489321
+    dkappa: -0.0016165067938964236
     ddkappa: 0
   }
   v: 0.862155716202027
@@ -1560,12 +1560,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.76345889666
-    y: 4140940.0102502918
-    theta: -1.8954910596109542
-    kappa: -0.019485097507612778
+    x: 587256.7636907472
+    y: 4140940.0100699682
+    theta: -1.8949741309478267
+    kappa: -0.0194759357313102
     s: 15.716161377988904
-    dkappa: -0.0015809771153806286
+    dkappa: -0.0016119242791840741
     ddkappa: 0
   }
   v: 0.80519134247209567
@@ -1574,12 +1574,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.73870458885
-    y: 4140939.9368559187
-    theta: -1.8970043805856682
-    kappa: -0.019606234075112049
+    x: 587256.73897458869
+    y: 4140939.9366627387
+    theta: -1.8964868017486609
+    kappa: -0.019599590564508367
     s: 15.793617880533562
-    dkappa: -0.0015762930564069855
+    dkappa: -0.0016076687827999928
     ddkappa: 0
   }
   v: 0.743035440664186
@@ -1588,12 +1588,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.71602581057
-    y: 4140939.8696153103
-    theta: -1.8983908134697725
-    kappa: -0.019717213924590242
+    x: 587256.716330761
+    y: 4140939.8694103519
+    theta: -1.8978726389697465
+    kappa: -0.019712877535229795
     s: 15.864580028223321
-    dkappa: -0.0015720017332287468
+    dkappa: -0.0016037700893013246
     ddkappa: 0
   }
   v: 0.6752258567089352
@@ -1602,12 +1602,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.69561085058
-    y: 4140939.809086726
-    theta: -1.8996388439060008
-    kappa: -0.0198171156549484
+    x: 587256.69594726281
+    y: 4140939.8088711658
+    theta: -1.8991201332059631
+    kappa: -0.0198148560876906
     s: 15.928458663646509
-    dkappa: -0.0015681387742046732
+    dkappa: -0.0016002605672711155
     ddkappa: 0
   }
   v: 0.601284080693789
@@ -1616,12 +1616,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.67766355153
-    y: 4140939.755874543
-    theta: -1.9007360095570378
-    kappa: -0.019904941750504883
+    x: 587256.67802762252
+    y: 4140939.7556496621
+    theta: -1.9002168274769655
+    kappa: -0.019904507969204815
     s: 15.98461596060805
-    dkappa: -0.0015647427508609056
+    dkappa: -0.0015971752591780752
     ddkappa: 0
   }
   v: 0.52071524686300263
@@ -1630,12 +1630,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.66208593233
-    y: 4140939.7107397597
-    theta: -1.9016948187899212
-    kappa: -0.01997859089162414
+    x: 587256.66247259255
+    y: 4140939.7105070874
+    theta: -1.9011758902341891
+    kappa: -0.019979801921142203
     s: 16.032363788545119
-    dkappa: -0.0015618803626723245
+    dkappa: -0.0015945772150662105
     ddkappa: 0
   }
   v: 0.43300813361764
@@ -1644,12 +1644,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.64939270215
-    y: 4140939.6742882887
-    theta: -1.902478117509973
-    kappa: -0.02003780273672558
+    x: 587256.64979830652
+    y: 4140939.674049024
+    theta: -1.9019595194656187
+    kappa: -0.020040375512960742
     s: 16.070962076942845
-    dkappa: -0.0015595744119589236
+    dkappa: -0.0015924849268691003
     ddkappa: 0
   }
   v: 0.33763516351557332
@@ -1658,12 +1658,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.63996933564
-    y: 4140939.6472269674
-    theta: -1.9030596359779295
-    kappa: -0.020081761201704629
+    x: 587256.64038900414
+    y: 4140939.6469828105
+    theta: -1.9025412833051527
+    kappa: -0.020085344929300475
     s: 16.099617179749991
-    dkappa: -0.0015578624900705784
+    dkappa: -0.0015909316265576309
     ddkappa: 0
   }
   v: 0.2340524032714851
@@ -1672,12 +1672,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.63409498369
-    y: 4140939.630357442
-    theta: -1.9034221448701405
-    kappa: -0.020109164094120512
+    x: 587256.63452341943
+    y: 4140939.630110234
+    theta: -1.9029039451581626
+    kappa: -0.020113378030059342
     s: 16.117480239794617
-    dkappa: -0.0015567953096912572
+    dkappa: -0.001589963327929709
     ddkappa: 0
   }
   v: 0.12169956375686547
@@ -1686,12 +1686,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587256.63206749118
-    y: 4140939.62453504
-    theta: -1.9035472625096805
-    kappa: -0.020118622015637314
+    x: 587256.63249895279
+    y: 4140939.6242867792
+    theta: -1.9030291155911778
+    kappa: -0.02012305346368971
     s: 16.123645553199783
-    dkappa: -0.0015564269796581095
+    dkappa: -0.001589629126258998
     ddkappa: 0
   }
   v: 1.0436096431439395e-13
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_mission_complete_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_mission_complete_0.pb.txt
index 3fee9e252a..a65b187483 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_mission_complete_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_mission_complete_0.pb.txt
@@ -9,1693 +9,1693 @@ trajectory_point {
     y: 4140980.4472917006
     z: 0
     theta: 1.3202319779073468
-    kappa: -0.0067364876821149541
+    kappa: -0.0052442117163229692
     s: 0
     dkappa: 0
     ddkappa: 0
   }
   v: 0
-  a: 0.11763242736718585
+  a: 0.11763246897426773
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 587610.56679027271
-    y: 4140980.4473146112
-    theta: 1.320231826962063
-    kappa: -0.006736454180930886
-    s: 2.365862082059716e-05
-    dkappa: 6.7003541645553123e-08
+    x: 587610.56679026445
+    y: 4140980.447314613
+    theta: 1.3202318600979501
+    kappa: -0.0052441868308813607
+    s: 2.3658629198602687e-05
+    dkappa: 4.9771729081618308e-08
     ddkappa: 0
   }
-  v: 0.0023723706277491768
-  a: 0.11958501785714389
+  v: 0.0023723714683473169
+  a: 0.11958506030351028
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 587610.56680811092
-    y: 4140980.4473838387
-    theta: 1.3202313708540372
-    kappa: -0.006736352951143991
-    s: 9.5147352057054112e-05
-    dkappa: 2.6946666140693549e-07
+    x: 587610.566808078
+    y: 4140980.4473838466
+    theta: 1.3202315041159016
+    kappa: -0.0052441116350928456
+    s: 9.5147385790346787e-05
+    dkappa: 2.0016586204669263e-07
     ddkappa: 0
   }
-  v: 0.0047826213688862028
-  a: 0.12142070653716897
+  v: 0.00478262306648787
+  a: 0.12142074978487152
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 587610.56683806714
-    y: 4140980.4475000943
-    theta: 1.3202306048981467
-    kappa: -0.0067361829529264377
-    s: 0.00021520052274762053
-    dkappa: 6.0946905136210057e-07
+    x: 587610.56683799252
+    y: 4140980.4475001134
+    theta: 1.320230906304569
+    kappa: -0.0052439853565495933
+    s: 0.0002152005991340328
+    dkappa: 4.5272724080453587e-07
     ddkappa: 0
   }
-  v: 0.0072284302570843123
-  a: 0.12314110039445061
+  v: 0.0072284328273408451
+  a: 0.12314114440603297
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 587610.5668803131
-    y: 4140980.4476640443
-    theta: 1.3202295247035072
-    kappa: -0.006735943211754517
-    s: 0.00038450634400144875
-    dkappa: 1.0889597930768208e-06
+    x: 587610.56688017992
+    y: 4140980.4476640788
+    theta: 1.3202300632369655
+    kappa: -0.0052438072713531583
+    s: 0.00038450648064438145
+    dkappa: 8.0890368685811089e-07
     ddkappa: 0
   }
-  v: 0.0097075074657605349
-  a: 0.12474780641617837
+  v: 0.0097075109235790664
+  a: 0.1247478511546761
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 587610.56693500932
-    y: 4140980.4478763123
-    theta: 1.3202281261693702
-    kappa: -0.00673563281749841
-    s: 0.00060370755179346952
-    dkappa: 1.7097591780630117e-06
+    x: 587610.56693480024
+    y: 4140980.4478763659
+    theta: 1.3202289717125504
+    kappa: -0.0052435767034383421
+    s: 0.00060370776658710606
+    dkappa: 1.2700473535811178e-06
     ddkappa: 0
   }
-  v: 0.012217595308075686
-  a: 0.12624243158954179
+  v: 0.012217599667628979
+  a: 0.12624247701848243
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 587610.56700230471
-    y: 4140980.4481374756
-    theta: 1.3202264054810238
-    kappa: -0.0067352509235119864
-    s: 0.000873402049759268
-    dkappa: 2.4735605282370581e-06
+    x: 587610.56700200227
+    y: 4140980.4481375539
+    theta: 1.3202276287540278
+    kappa: -0.0052432930238970723
+    s: 0.00087340236087398511
+    dkappa: 1.8374160784950828e-06
     ddkappa: 0
   }
-  v: 0.014756468236934375
-  a: 0.12762658290173037
+  v: 0.014756473511670662
+  a: 0.12762662898513344
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 587610.56708233769
-    y: 4140980.4484480717
-    theta: 1.3202243591056895
-    kappa: -0.0067347967457225925
-    s: 0.0011941435519899589
-    dkappa: 3.3819320163781374e-06
+    x: 587610.56708192406
+    y: 4140980.4484481784
+    theta: 1.320226031604145
+    kappa: -0.0052429556503022744
+    s: 0.001194143977857935
+    dkappa: 2.512174735546447e-06
     ddkappa: 0
   }
-  v: 0.017321932844985
-  a: 0.12890186733993364
+  v: 0.017321939047637826
+  a: 0.12890191404231066
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 587610.56717523537
-    y: 4140980.4488085937
-    theta: 1.3202219837884226
-    kappa: -0.006734269561720828
-    s: 0.0015664422258270634
-    dkappa: 4.4363184865865472e-06
+    x: 587610.56717469275
+    y: 4140980.4488087338
+    theta: 1.3202241777224923
+    kappa: -0.0052425640460317443
+    s: 0.0015664427851280827
+    dkappa: 3.2953965873836537e-06
     ddkappa: 0
   }
-  v: 0.019911827864619751
-  a: 0.13006989189134119
+  v: 0.019911835007217805
+  a: 0.13006993917769558
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 587610.56728111452
-    y: 4140980.4492194951
-    theta: 1.32021927654801
-    kappa: -0.0067336687098503416
-    s: 0.0019907653346573841
-    dkappa: 5.6380432747420272e-06
+    x: 587610.567280425
+    y: 4140980.4492196729
+    theta: 1.3202220647823024
+    kappa: -0.0052421177195920206
+    s: 0.0019907660463048382
+    dkappa: 4.18806463763424e-06
     ddkappa: 0
   }
-  v: 0.022524024167974603
-  a: 0.1311322635431425
+  v: 0.022524032261851571
+  a: 0.13113231137896966
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 587610.56740008108
-    y: 4140980.4496811861
-    theta: 1.3202162346728705
-    kappa: -0.0067329935882976177
-    s: 0.0024675378807078809
-    dkappa: 6.9883100289620872e-06
+    x: 587610.56739922648
+    y: 4140980.4496814073
+    theta: 1.3202196906672488
+    kappa: -0.00524161622394226
+    s: 0.0024675387638349667
+    dkappa: 5.1910729831819182e-06
     ddkappa: 0
   }
-  v: 0.025156424766929333
-  a: 0.13209058928252709
+  v: 0.025156433822733731
+  a: 0.13209063763381446
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 587610.56753223075
-    y: 4140980.45019404
-    theta: 1.3202128557169517
-    kappa: -0.0067322436541817554
-    s: 0.0029971432478405474
-    dkappa: 8.48820453006033e-06
+    x: 587610.56753119268
+    y: 4140980.4501943076
+    theta: 1.320217053468246
+    kappa: -0.0052410591558181069
+    s: 0.0029971443217866631
+    dkappa: 6.3052281664436751e-06
     ddkappa: 0
   }
-  v: 0.027806964813107505
-  a: 0.13294647609668456
+  v: 0.0278069748408125
+  a: 0.13294652492991146
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587610.56767764885
-    y: 4140980.4507583859
-    theta: 1.3202091374956302
-    kappa: -0.0067314184226442619
-    s: 0.003579923844347287
-    dkappa: 1.013869651200478e-05
+    x: 587610.56767640891
+    y: 4140980.4507587068
+    theta: 1.3202141514802472
+    kappa: -0.0052404461550555692
+    s: 0.0035799251286446217
+    dkappa: 7.53125052764685e-06
     ddkappa: 0
   }
-  v: 0.030473611597876461
-  a: 0.13370153097280438
+  v: 0.030473622606789742
+  a: 0.13370158025494217
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 587610.567836411
-    y: 4140980.4513745178
-    theta: 1.3202050780816106
-    kappa: -0.0067305174659388414
-    s: 0.0042161817457447865
-    dkappa: 1.1940641482376198e-05
+    x: 587610.56783495075
+    y: 4140980.4513748959
+    theta: 1.320210983199045
+    kappa: -0.0052397769039148889
+    s: 0.0042161832601051111
+    dkappa: 8.8697755571062284e-06
     ddkappa: 0
   }
-  v: 0.033154364552347354
-  a: 0.13435736089807612
+  v: 0.033154376551120954
+  a: 0.13435741059658804
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587610.56800858246
-    y: 4140980.4520426895
-    theta: 1.3202006758008236
-    kappa: -0.0067295404125211735
-    s: 0.0049061793375693953
-    dkappa: 1.3894782542826426e-05
+    x: 587610.56800688326
+    y: 4140980.4520431291
+    theta: 1.3202075473180697
+    kappa: -0.0052390511264044162
+    s: 0.0049061811018710462
+    dkappa: 1.0321355247501129e-05
     ddkappa: 0
   }
-  v: 0.035847255247375122
-  a: 0.1349155728596893
+  v: 0.03584726824401524
+  a: 0.13491562294253062
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587610.56819421914
-    y: 4140980.4527631183
-    theta: 1.3201959292283252
-    kappa: -0.006728486946138707
-    s: 0.0056501399581719977
-    dkappa: 1.6001752209536685e-05
+    x: 587610.5681922622
+    y: 4140980.4527636236
+    theta: 1.3202038427251883
+    kappa: -0.005238268587604485
+    s: 0.0056501419924470594
+    dkappa: 1.1886459446152492e-05
     ddkappa: 0
   }
-  v: 0.038550347393558469
-  a: 0.13537777384483346
+  v: 0.03855036139543537
+  a: 0.13537782428045142
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 587610.56839336688
-    y: 4140980.453535981
-    theta: 1.3201908371841959
-    kappa: -0.0067273568049204422
-    s: 0.0064482485415128911
-    dkappa: 1.8262074233675927e-05
+    x: 587610.56839113357
+    y: 4140980.453536558
+    theta: 1.3201998684995051
+    kappa: -0.0052374290929912805
+    s: 0.0064482508659345775
+    dkappa: 1.3565477207299975e-05
     ddkappa: 0
   }
-  v: 0.041261736841239925
-  a: 0.1357455708406981
+  v: 0.0412617518550977
+  a: 0.13574562159803191
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 587610.56860606279
-    y: 4140980.4543614215
-    theta: 1.3201853987294396
-    kappa: -0.0067261497804667249
-    s: 0.0073006522599566607
-    dkappa: 2.0676165421859136e-05
+    x: 587610.56860353414
+    y: 4140980.4543620753
+    theta: 1.3201956239081585
+    kappa: -0.0052365324877607158
+    s: 0.0073006548948268838
+    dkappa: 1.5358718144379014e-05
     ddkappa: 0
   }
-  v: 0.0439795515805058
-  a: 0.1360205708344728
+  v: 0.043979567612472258
+  a: 0.13602062188295361
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 587610.568832334
-    y: 4140980.455239547
-    theta: 1.3201796131618817
-    kappa: -0.0067248657169390229
-    s: 0.0082074611670670556
-    dkappa: 2.3244337456605678e-05
+    x: 587610.56882949139
+    y: 4140980.4552402822
+    theta: 1.3201911084031226
+    kappa: -0.0052355786561523038
+    s: 0.0082074641328042063
+    dkappa: 1.7266413782297956e-05
     ddkappa: 0
   }
-  v: 0.04670195174118618
-  a: 0.1362043808133471
+  v: 0.046701968796782678
+  a: 0.13620443212289798
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587610.569072199
-    y: 4140980.4561704271
-    theta: 1.3201734800120688
-    kappa: -0.006723504510149721
-    s: 0.0091687488404018653
-    dkappa: 2.5966798716797598e-05
+    x: 587610.56906902348
+    y: 4140980.4561712486
+    theta: 1.3201863216180045
+    kappa: -0.0052345675207730322
+    s: 0.0091687521575287787
+    dkappa: 1.928871890971511e-05
     ddkappa: 0
   }
-  v: 0.049427129592854946
-  a: 0.13629860776451047
+  v: 0.049427147677006206
+  a: 0.13629865930554655
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587610.56932566734
-    y: 4140980.4571541
-    theta: 1.3201669990391676
-    kappa: -0.0067220661066519014
-    s: 0.010184553024307798
-    dkappa: 2.8843656098137982e-05
+    x: 587610.56932214007
+    y: 4140980.4571550121
+    theta: 1.3201812633648446
+    kappa: -0.0052334990419212312
+    s: 0.010184556713439916
+    dkappa: 2.1425712931315853e-05
     ddkappa: 0
   }
-  v: 0.052153309544829794
-  a: 0.13630485867515249
+  v: 0.052153328661873777
+  a: 0.13630491041858084
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587610.56959273957
-    y: 4140980.4581905678
-    theta: 1.3201601702268635
-    kappa: -0.00672055050282914
-    s: 0.011254876272715344
-    dkappa: 3.1874916833609229e-05
+    x: 587610.56958884152
+    y: 4140980.4581915755
+    theta: 1.3201759336309158
+    kappa: -0.0052323732169104525
+    s: 0.011254880354549088
+    dkappa: 2.3677401220089715e-05
     ddkappa: 0
   }
-  v: 0.054878748146172177
-  a: 0.13622474053246267
+  v: 0.054878768299869894
+  a: 0.13622479244968233
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 587610.56987340748
-    y: 4140980.459279798
-    theta: 1.3201529937792593
-    kappa: -0.006718957743985279
-    s: 0.012379686591933673
-    dkappa: 3.5060490313931437e-05
+    x: 587610.56986911979
+    y: 4140980.4592809067
+    theta: 1.3201703325755214
+    kappa: -0.0052311900793933375
+    s: 0.01237969108723499
+    dkappa: 2.604371646960746e-05
     ddkappa: 0
   }
-  v: 0.057601734085687362
-  a: 0.13605986032363057
+  v: 0.057601755279232732
+  a: 0.13605991238653253
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587610.570167655
-    y: 4140980.4604217289
-    theta: 1.3201454701167752
-    kappa: -0.00671728792343423
-    s: 0.01355891808344549
-    dkappa: 3.840018990802069e-05
+    x: 587610.57016295893
+    y: 4140980.4604229429
+    theta: 1.3201644605267966
+    kappa: -0.0052299496986854968
+    s: 0.013558923013038621
+    dkappa: 2.852452004629819e-05
     ddkappa: 0
   }
-  v: 0.060320588191924387
-  a: 0.13581182503584571
+  v: 0.060320610427954065
+  a: 0.13581187721681293
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587610.570475457
-    y: 4140980.4616162633
-    theta: 1.3201375998720464
-    kappa: -0.0067155411815897461
-    s: 0.014792471586701916
-    dkappa: 4.1893734783447358e-05
+    x: 587610.5704703338
+    y: 4140980.4616175876
+    theta: 1.320158317978505
+    kappa: -0.0052286521790893721
+    s: 0.014792476971458348
+    dkappa: 3.1119603341726424e-05
     ddkappa: 0
   }
-  v: 0.0630336634331761
-  a: 0.13548224165629763
+  v: 0.063033686713779333
+  a: 0.13548229392820502
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587610.570796781
-    y: 4140980.4628632735
-    theta: 1.3201293838858228
-    kappa: -0.0067137177050552175
-    s: 0.016080215321917373
-    dkappa: 4.55407517268945e-05
+    x: 587610.57079121179
+    y: 4140980.4628647133
+    theta: 1.3201519055868403
+    kappa: -0.0052272976592181225
+    s: 0.016080221182744978
+    dkappa: 3.382868912486917e-05
     ddkappa: 0
   }
-  v: 0.065739344917479126
-  a: 0.13507271717217584
+  v: 0.065739369244207549
+  a: 0.13507276950839031
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587610.57113158586
-    y: 4140980.4641626012
-    theta: 1.3201208232028678
-    kappa: -0.0067118177257134565
-    s: 0.017421985532864462
-    dkappa: 4.9340776964616131e-05
+    x: 587610.571125552
+    y: 4140980.4641641611
+    theta: 1.3201452241672229
+    kappa: -0.0052258863113194831
+    s: 0.017421991890696851
+    dkappa: 3.665143289439306e-05
     ddkappa: 0
   }
-  v: 0.0684360498926139
-  a: 0.1345848585706699
+  v: 0.068436075266491439
+  a: 0.13458491094505032
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587610.57147982286
-    y: 4140980.4655140573
-    theta: 1.3201119190678574
-    kappa: -0.0067098415198164826
-    s: 0.018817587129668807
-    dkappa: 5.3293257982895544e-05
+    x: 587610.5714733057
+    y: 4140980.465515743
+    theta: 1.3201382746911023
+    kappa: -0.0052244183405996524
+    s: 0.018817594005454883
+    dkappa: 3.9587424230931396e-05
     ddkappa: 0
   }
-  v: 0.071122227746104633
-  a: 0.13402027283896933
+  v: 0.071122254167637289
+  a: 0.13402032522586652
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587610.571841436
-    y: 4140980.4669174245
-    theta: 1.3201026729212788
-    kappa: -0.006707789407075314
-    s: 0.020266794331603975
-    dkappa: 5.7397555348503667e-05
+    x: 587610.571834417
+    y: 4140980.4669192396
+    theta: 1.320131058282753
+    kappa: -0.0052228939845471566
+    s: 0.020266801746297657
+    dkappa: 4.263618814936124e-05
     ddkappa: 0
   }
-  v: 0.073796360005219316
-  a: 0.13338056696426365
+  v: 0.07379638747440502
+  a: 0.13338061933852041
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587610.57221636118
-    y: 4140980.4683724535
-    theta: 1.3200930863953293
-    kappa: -0.0067056617497497422
-    s: 0.021769351309886321
-    dkappa: 6.1652944529157373e-05
+    x: 587610.57220882166
+    y: 4140980.4683744023
+    theta: 1.3201235762160759
+    kappa: -0.0052213135122567214
+    s: 0.021769359284436487
+    dkappa: 4.5797186451080516e-05
     ddkappa: 0
   }
-  v: 0.076456960336969748
-  a: 0.13266734793374244
+  v: 0.076456988853308233
+  a: 0.13266740027069354
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587610.57260452723
-    y: 4140980.4698788677
-    theta: 1.3200831613098158
-    kappa: -0.006703458951738136
-    s: 0.023324972830469876
-    dkappa: 6.60586177139778e-05
+    x: 587610.57259644894
+    y: 4140980.4698809567
+    theta: 1.3201158299113969
+    kappa: -0.00521967722375315
+    s: 0.023324981385810507
+    dkappa: 4.9069819076285123e-05
     ddkappa: 0
   }
-  v: 0.079102574548111509
-  a: 0.13188222273459516
+  v: 0.079102604110614108
+  a: 0.13188227501006736
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587610.5730058559
-    y: 4140980.4714363646
-    theta: 1.3200728996680535
-    kappa: -0.0067011814576672154
-    s: 0.024933344896841222
-    dkappa: 7.0613685633948647e-05
+    x: 587610.57299722044
+    y: 4140980.4714385974
+    theta: 1.3201078209322659
+    kappa: -0.0052179854493151949
+    s: 0.024933354053881709
+    dkappa: 5.2453425456245952e-05
     ddkappa: 0
   }
-  v: 0.081731780585143979
-  a: 0.1310267983540114
+  v: 0.081731811192343462
+  a: 0.13102685054432336
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587610.57342026173
-    y: 4140980.473044612
-    theta: 1.3200623036527641
-    kappa: -0.006698829751981843
-    s: 0.026594125392814371
-    dkappa: 7.5317179382374634e-05
+    x: 587610.57341105118
+    y: 4140980.4730469938
+    theta: 1.3200995509822557
+    kappa: -0.0052162385487994284
+    s: 0.026594135172430068
+    dkappa: 5.5947285865586063e-05
     ddkappa: 0
   }
-  v: 0.084343188534310332
-  a: 0.13010268177918069
+  v: 0.0843432201842708
+  a: 0.13010273386114313
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587610.57384765241
-    y: 4140980.4747032523
-    theta: 1.3200513756219763
-    kappa: -0.0066964043580348106
-    s: 0.028306944725325609
-    dkappa: 8.0168052235339544e-05
+    x: 587610.57383784885
+    y: 4140980.4747057874
+    theta: 1.3200910219017625
+    kappa: -0.0052144369109641178
+    s: 0.028306955148348554
+    dkappa: 5.955062277455769e-05
     ddkappa: 0
   }
-  v: 0.0869354406215975
-  a: 0.12911147999729256
+  v: 0.08693547331192418
+  a: 0.12911153194820804
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587610.57428792934
-    y: 4140980.4764119014
-    theta: 1.3200401181049233
-    kappa: -0.0066939058371766258
-    s: 0.030071406467228445
-    dkappa: 8.5165181472164934e-05
+    x: 587610.57427751448
+    y: 4140980.4764145939
+    theta: 1.3200822356648036
+    kappa: -0.0052125809527930941
+    s: 0.030071417554438259
+    dkappa: 6.32626022013194e-05
     ddkappa: 0
   }
-  v: 0.089507211212736235
-  a: 0.12805479999553651
+  v: 0.0895072449405853
+  a: 0.1280548517931997
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587610.57474098692
-    y: 4140980.47817015
-    theta: 1.3200285337979425
-    kappa: -0.0066913347878452987
-    s: 0.031887088000088409
-    dkappa: 9.0307370195868116e-05
+    x: 587610.57472994318
+    y: 4140980.4781730054
+    theta: 1.3200731943758175
+    kappa: -0.0052106711188196365
+    s: 0.031887099772203421
+    dkappa: 6.7082335064213116e-05
     ddkappa: 0
   }
-  v: 0.0920572068132011
-  a: 0.1269342487611021
+  v: 0.092057241575289564
+  a: 0.12693430038379955
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587610.575206713
-    y: 4140980.479977564
-    theta: 1.3200166255603745
-    kappa: -0.006688691844656129
-    s: 0.033753541156977963
-    dkappa: 9.5593349153620693e-05
+    x: 587610.57519502309
+    y: 4140980.4799805866
+    theta: 1.3200639002664625
+    kappa: -0.005208707880450328
+    s: 0.033753553634646556
+    dkappa: 7.1008878534041314e-05
     ddkappa: 0
   }
-  v: 0.094584166068210393
-  a: 0.12575143328117888
+  v: 0.094584201860825889
+  a: 0.12575148470768913
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 587610.57568499
-    y: 4140980.4818336857
-    theta: 1.3200043964104613
-    kappa: -0.0066859776774914909
-    s: 0.035670292865271375
-    dkappa: 0.00010102177855720674
+    x: 587610.57567263627
+    y: 4140980.4818368806
+    theta: 1.3200543556924165
+    kappa: -0.0052066917352889446
+    s: 0.035670306069063455
+    dkappa: 7.50412373863439e-05
     ddkappa: 0
   }
-  v: 0.097086859762726269
-  a: 0.12450796054295635
+  v: 0.097086896581736928
+  a: 0.1245080117525499
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 587610.57617569366
-    y: 4140980.4837380336
-    theta: 1.3199918495212462
-    kappa: -0.0066831929905906244
-    s: 0.0376368457894396
-    dkappa: 0.00010659124990348126
+    x: 587610.57616265875
+    y: 4140980.4837414045
+    theta: 1.3200445631301763
+    kappa: -0.0052046232064603152
+    s: 0.037636859739838345
+    dkappa: 7.9178365353675624e-05
     ddkappa: 0
   }
-  v: 0.0995640908214546
-  a: 0.12320543753362406
+  v: 0.09956412866231891
+  a: 0.12320548850606339
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 587610.576678694
-    y: 4140980.4856901029
-    theta: 1.3199789882164725
-    kappa: -0.0066803385216394149
-    s: 0.039652678973845147
-    dkappa: 0.00011230028779482847
+    x: 587610.57666496083
+    y: 4140980.485693654
+    theta: 1.3200345251738557
+    kappa: -0.0052025028419342059
+    s: 0.039652693691238861
+    dkappa: 8.3419166477882771e-05
     ddkappa: 0
   }
-  v: 0.10201469430884511
-  a: 0.12184547124037154
+  v: 0.1020147331666217
+  a: 0.12184552195591108
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 587610.57719385508
-    y: 4140980.4876893666
-    theta: 1.3199658159664822
-    kappa: -0.0066774150408601875
-    s: 0.041717248485536949
-    dkappa: 0.00011814735175962003
+    x: 587610.577179407
+    y: 4140980.4876931026
+    theta: 1.3200242445319863
+    kappa: -0.0052003312138491856
+    s: 0.0417172639902112
+    dkappa: 8.776249646238061e-05
     ddkappa: 0
   }
-  v: 0.10443753742909126
-  a: 0.12042966865038832
+  v: 0.10443757729844881
+  a: 0.12042971908977448
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 587610.577721036
-    y: 4140980.4897352769
-    theta: 1.3199523363841148
-    kappa: -0.00667442335010149
-    s: 0.043829988057045251
-    dkappa: 0.00012413083807267348
+    x: 587610.57770585606
+    y: 4140980.4897392024
+    theta: 1.3200137240243153
+    kappa: -0.0051981089178364994
+    s: 0.043830004369175182
+    dkappa: 9.22071640244303e-05
     ddkappa: 0
   }
-  v: 0.10683151952613036
-  a: 0.11895963675086393
+  v: 0.10683156040135733
+  a: 0.11895968689533511
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 587610.57826008962
-    y: 4140980.491827264
-    theta: 1.3199385532206067
-    kappa: -0.0066713642819278794
-    s: 0.0459903097291765
-    dkappa: 0.0001302490815757106
+    x: 587610.57824416168
+    y: 4140980.4918313837
+    theta: 1.3200029665786053
+    kappa: -0.0051958365723439471
+    s: 0.045990326868819278
+    dkappa: 9.6751932247415964e-05
     ddkappa: 0
   }
-  v: 0.10919557208364347
-  a: 0.11743698252898792
+  v: 0.10919561395865807
+  a: 0.11743703236027443
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 587610.57881086436
-    y: 4140980.4939647391
-    theta: 1.3199244703614905
-    kappa: -0.0066682386987097107
-    s: 0.048197604493808159
-    dkappa: 0.00013650035749781549
+    x: 587610.5787941718
+    y: 4140980.4939690554
+    theta: 1.3199919752274338
+    kappa: -0.0051935148179597474
+    s: 0.048197622480895728
+    dkappa: 0.00010139551993312188
     ddkappa: 0
   }
-  v: 0.11152865872505548
-  a: 0.11586331297194981
+  v: 0.11152870159341537
+  a: 0.11586336247227397
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 587610.579373203
-    y: 4140980.4961470915
-    theta: 1.3199100918224926
-    kappa: -0.006665047491712919
-    s: 0.050451242936683668
-    dkappa: 0.00014288288327589325
+    x: 587610.57935572986
+    y: 4140980.4961516093
+    theta: 1.3199807531049919
+    kappa: -0.0051911443167364183
+    s: 0.05045126179101557
+    dkappa: 0.00010613660295400944
     ddkappa: 0
   }
-  v: 0.113829775213535
-  a: 0.11424023506693914
+  v: 0.11382981906844732
+  a: 0.11424028421901522
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 587610.57994694333
-    y: 4140980.4983736929
-    theta: 1.3198954217454337
-    kappa: -0.006661791580188813
-    s: 0.052750575880207262
-    dkappa: 0.00014939482037512806
+    x: 587610.579928674
+    y: 4140980.498378417
+    theta: 1.3199693034438835
+    kappa: -0.0051887257515146473
+    s: 0.052750595621443762
+    dkappa: 0.00011097381560549438
     ddkappa: 0
   }
-  v: 0.11609794945199449
-  a: 0.11256935580114544
+  v: 0.11609799428632549
+  a: 0.11256940458817971
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 587610.580531919
-    y: 4140980.5006438959
-    theta: 1.3198804643941267
-    kappa: -0.006658471910463858
-    s: 0.055094935026238863
-    dkappa: 0.00015603427610944151
+    x: 587610.58051283762
+    y: 4140980.50064883
+    theta: 1.3199576295719251
+    kappa: -0.0051862598252471644
+    s: 0.055094955673894226
+    dkappa: 0.00011590575195822374
     ddkappa: 0
   }
-  v: 0.11833224148309018
-  a: 0.11085228216175824
+  v: 0.1183322872893752
+  a: 0.11085233056744888
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 587610.58112795837
-    y: 4140980.5029570363
-    theta: 1.3198652241502757
-    kappa: -0.0066550894550294632
-    s: 0.057483633598888945
-    dkappa: 0.00016279930546195105
+    x: 587610.58110804984
+    y: 4140980.5029621846
+    theta: 1.3199457349089443
+    kappa: -0.005183747260322614
+    s: 0.057483655172324941
+    dkappa: 0.00012093096721035308
     ddkappa: 0
   }
-  v: 0.12053174348922213
-  a: 0.10909062113596707
+  v: 0.12053179025967536
+  a: 0.10909066914450427
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 587610.58173488569
-    y: 4140980.5053124311
-    theta: 1.3198497055093761
-    kappa: -0.0066516452116317662
-    s: 0.059915966987313454
-    dkappa: 0.00016968791290542827
+    x: 587610.58171413478
+    y: 4140980.5053177974
+    theta: 1.3199336229635794
+    kappa: -0.00518118879788943
+    s: 0.059915989505732965
+    dkappa: 0.00012604797903982345
     ddkappa: 0
   }
-  v: 0.12269557979253412
-  a: 0.10728597971096149
+  v: 0.12269562751905848
+  a: 0.10728602730702737
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 587610.582352521
-    y: 4140980.5077093812
-    theta: 1.3198339130766119
-    kappa: -0.0066481402023614239
-    s: 0.062391213388508608
-    dkappa: 0.00017669805422275715
+    x: 587610.58233091282
+    y: 4140980.50771497
+    theta: 1.3199212973300782
+    kappa: -0.0051785851971797069
+    s: 0.062391236870949579
+    dkappa: 0.00013125526895663855
     ddkappa: 0
   }
-  v: 0.12482290685491375
-  a: 0.10543996487393098
+  v: 0.12482295552911078
+  a: 0.1054400120426997
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 587610.58298067993
-    y: 4140980.510147173
-    theta: 1.3198178515627557
-    kappa: -0.0066445754727433989
-    s: 0.06490863445010582
-    dkappa: 0.00018382763832739237
+    x: 587610.58295819978
+    y: 4140980.5101529858
+    theta: 1.3199087616850982
+    kappa: -0.0051759372348330742
+    s: 0.064908658915435277
+    dkappa: 0.00013655128365514169
     ddkappa: 0
   }
-  v: 0.12691291327799242
-  a: 0.10355418361206511
+  v: 0.12691296289117202
+  a: 0.10355423033920277
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587610.58627261128
-    y: 4140980.5229226686
-    theta: 1.3197336795285854
-    kappa: -0.0066258941385260676
-    s: 0.078101439867840752
-    dkappa: 0.00022119096114878452
+    x: 587610.58624556218
+    y: 4140980.5229296647
+    theta: 1.3198430673363337
+    kappa: -0.0051620603142686784
+    s: 0.0781014695242462
+    dkappa: 0.00016430559646569195
     ddkappa: 0
   }
-  v: 0.136776986643036
-  a: 0.093585025481839
+  v: 0.13677704081125955
+  a: 0.093585069803031246
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587610.5897979039
-    y: 4140980.5366038051
-    theta: 1.3196435406194247
-    kappa: -0.0066058885062125258
-    s: 0.09222946794660869
-    dkappa: 0.00026120292655131287
+    x: 587610.58976596186
+    y: 4140980.5366120664
+    theta: 1.3197727159828967
+    kappa: -0.0051471996748357922
+    s: 0.0922295032370649
+    dkappa: 0.00019402738045019741
     ddkappa: 0
   }
-  v: 0.14560481462294511
-  a: 0.082862754809158723
+  v: 0.14560487309087802
+  a: 0.082862796439852671
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587610.59352984477
-    y: 4140980.5510869119
-    theta: 1.3195481178622721
-    kappa: -0.00658471016873575
-    s: 0.10718566337418178
-    dkappa: 0.00030356034335888871
+    x: 587610.593492723
+    y: 4140980.5510965134
+    theta: 1.3196982407121598
+    kappa: -0.0051314679232018264
+    s: 0.10718570471480979
+    dkappa: 0.00022549141844629495
     ddkappa: 0
   }
-  v: 0.15333112963340897
-  a: 0.071588244992715974
+  v: 0.15333119212033613
+  a: 0.071588283709854636
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587610.597440343
-    y: 4140980.5662629744
-    theta: 1.3194481295186993
-    kappa: -0.0065625185388696735
-    s: 0.12285744844689445
-    dkappa: 0.00034794438043959106
+    x: 587610.59739779367
+    y: 4140980.5662739794
+    theta: 1.3196202021109986
+    kappa: -0.0051149834747902271
+    s: 0.12285749622473105
+    dkappa: 0.00025846087558210716
     ddkappa: 0
   }
-  v: 0.15991075142998595
-  a: 0.059962369431202332
+  v: 0.15991081763596127
+  a: 0.059962405073224707
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587610.60150043084
-    y: 4140980.5820195707
-    theta: 1.3193443162688598
-    kappa: -0.00653947800481287
-    s: 0.13912873180363047
-    dkappa: 0.0003940262556379332
+    x: 587610.60145224631
+    y: 4140980.5820320342
+    theta: 1.3195391782631967
+    kappa: -0.0050978684408834341
+    s: 0.13912878637501247
+    dkappa: 0.00029269152514214351
     ddkappa: 0
   }
-  v: 0.16531858710810352
-  a: 0.048186001523309568
+  v: 0.1653186567200996
+  a: 0.048186033990150492
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587610.60568076512
-    y: 4140980.5982428268
-    theta: 1.3192374283954396
-    kappa: -0.0065157550857722416
-    s: 0.1558819171598099
-    dkappa: 0.00044147292470712813
+    x: 587610.60562677868
+    y: 4140980.598256791
+    theta: 1.3194557547468215
+    kappa: -0.0050802465157258432
+    s: 0.15588197884937358
+    dkappa: 0.00032793597443320194
     ddkappa: 0
   }
-  v: 0.16954963110305826
-  a: 0.036460014667729163
+  v: 0.16954970380111611
+  a: 0.03646004392081946
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 587610.60995212849
-    y: 4140980.6148193525
-    theta: 1.3191282129675477
-    kappa: -0.0064915155875466994
-    s: 0.17299991204137574
-    dkappa: 0.00048995277024135365
+    x: 587610.6098922136
+    y: 4140980.6148348507
+    theta: 1.3193705146315744
+    kappa: -0.0050622408636267586
+    s: 0.17299998114167112
+    dkappa: 0.00036394789065026978
     ddkappa: 0
   }
-  v: 0.17261896519001615
-  a: 0.0249852822631529
+  v: 0.17261904065339453
+  a: 0.024985308325419231
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587610.61428593029
-    y: 4140980.6316381926
-    theta: 1.319017401024563
-    kappa: -0.0064669217581108549
-    s: 0.19036813651878123
-    dkappa: 0.00053914129060801891
+    x: 587610.61422000057
+    y: 4140980.6316552479
+    theta: 1.3192840284761167
+    kappa: -0.0050439720060633571
+    s: 0.19036821329050127
+    dkappa: 0.00040048622674242521
     ddkappa: 0
   }
-  v: 0.17456175848401206
-  a: 0.013962677708272336
+  v: 0.17456183639733727
+  a: 0.013962700664137365
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587610.61865470826
-    y: 4140980.64859277
-    theta: 1.3189056947599467
-    kappa: -0.006442129443198703
-    s: 0.20787653194097644
-    dkappa: 0.00058872678888002925
+    x: 587610.618582715
+    y: 4140980.6486113947
+    theta: 1.3191968443253823
+    kappa: -0.0050255557087836421
+    s: 0.20787661661380144
+    dkappa: 0.00043731944727873887
     ddkappa: 0
   }
-  v: 0.17543326743995022
-  a: 0.0035930744017791533
+  v: 0.17543334749936554
+  a: 0.0035930943971613782
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587610.62303262926
-    y: 4140980.66558283
-    theta: 1.3187937547050366
-    kappa: -0.0064172852418873071
-    s: 0.22542156966939533
-    dkappa: 0.00063841606176805231
+    x: 587610.62295456009
+    y: 4140980.6656030281
+    theta: 1.3191094777078791
+    kappa: -0.0050071008689094049
+    s: 0.22542166244345194
+    dkappa: 0.00047422975431417428
     ddkappa: 0
   }
-  v: 0.17530883585260387
-  a: -0.00592265425763506
+  v: 0.17530891777191934
+  a: -0.0059226370153211
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587610.62739589391
-    y: 4140980.6825160105
-    theta: 1.3186821894037901
-    kappa: -0.0063925242150274396
-    s: 0.24290786939002576
-    dkappa: 0.000687938982839506
+    x: 587610.62731176894
+    y: 4140980.6825377764
+    theta: 1.3190224035771227
+    kappa: -0.0049887078127068239
+    s: 0.24290797043749285
+    dkappa: 0.00051101649190624673
     ddkappa: 0
   }
-  v: 0.174268166408751
-  a: -0.014861066866588883
+  v: 0.17426824990690976
+  a: -0.014861052666342304
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587610.63172212034
-    y: 4140980.6993054524
-    theta: 1.3185715711360089
-    kappa: -0.0063679733749238245
-    s: 0.26024573469241014
-    dkappa: 0.00073704152303584447
+    x: 587610.63163199113
+    y: 4140980.6993287718
+    theta: 1.3189360685833149
+    kappa: -0.004970470887804097
+    s: 0.26024584415467011
+    dkappa: 0.000547490961591651
     ddkappa: 0
   }
-  v: 0.17234367249608121
-  a: -0.023587978971689103
+  v: 0.17234375723064288
+  a: -0.023587968557602878
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587610.63598954678
-    y: 4140980.7158666993
-    theta: 1.3184624563387342
-    kappa: -0.0063437562177330147
-    s: 0.277347952278809
-    dkappa: 0.00078547668571802252
+    x: 587610.635893495
+    y: 4140980.7158915531
+    theta: 1.3188509070118257
+    kappa: -0.0049524818299628241
+    s: 0.27734807025948549
+    dkappa: 0.00058346968872905109
     ddkappa: 0
   }
-  v: 0.16955985356366529
-  a: -0.032036402599226133
+  v: 0.16955993912395403
+  a: -0.032036396591917975
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587610.64017710637
-    y: 4140980.7321179952
-    theta: 1.3183553836807644
-    kappa: -0.0063199922961468076
-    s: 0.29413009373625826
-    dkappa: 0.00083300536131464937
+    x: 587610.640075243
+    y: 4140980.7321443544
+    theta: 1.3187673392804369
+    kappa: -0.004934829445656843
+    s: 0.29413022029501013
+    dkappa: 0.000618775057352206
     ddkappa: 0
   }
-  v: 0.16594790785794505
-  a: -0.040139349775490385
+  v: 0.16594799377739697
+  a: -0.040139348672102726
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 587610.64426459419
-    y: 4140980.7479809271
-    theta: 1.3182508697886075
-    kappa: -0.0062967962708261917
-    s: 0.31051118541630679
-    dkappa: 0.00087939822448730709
+    x: 587610.644157058
+    y: 4140980.7480087555
+    theta: 1.3186857686035756
+    kappa: -0.0049175989074547437
+    s: 0.31051132056385644
+    dkappa: 0.00065323671942889121
     ddkappa: 0
   }
-  v: 0.16154573242273329
-  a: -0.04782983252677226
+  v: 0.16154581819124397
+  a: -0.047829836700972288
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587610.6482328343
-    y: 4140980.763381077
-    theta: 1.3181494049724547
-    kappa: -0.0062742769618353081
-    s: 0.32641437831475317
-    dkappa: 0.000924437631295866
+    x: 587610.648119791
+    y: 4140980.7634103326
+    theta: 1.3186065776565581
+    kappa: -0.0049008710494023782
+    s: 0.32641452200915017
+    dkappa: 0.00068669300411981955
     ddkappa: 0
   }
-  v: 0.15639792309921366
-  a: -0.05504086287936219
+  v: 0.15639800817548583
+  a: -0.055040872581341813
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 587610.65206384752
-    y: 4140980.7782486696
-    theta: 1.3180514489521808
-    kappa: -0.0062525364000754037
-    s: 0.34176761795138288
-    dkappa: 0.00096791951636380168
+    x: 587610.6519454876
+    y: 4140980.7782793026
+    theta: 1.3185301252398485
+    kappa: -0.0048847216624053821
+    s: 0.34176777009550185
+    dkappa: 0.00071899232703756088
     ddkappa: 0
   }
-  v: 0.15055577452594088
-  a: -0.061705452859550577
+  v: 0.15055585834983176
+  a: -0.061705468216026446
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 587610.65574101789
-    y: 4140980.7925192211
-    theta: 1.3179574265833758
-    kappa: -0.0062316688787187862
-    s: 0.35650431424970558
-    dkappa: 0.0010096552900435124
+    x: 587610.65561755479
+    y: 4140980.7925511762
+    theta: 1.3184567429433289
+    kappa: -0.0048692207896116847
+    s: 0.35650447468997903
+    dkappa: 0.00074999459950546353
     ddkappa: 0
   }
-  v: 0.14407728013884058
-  a: -0.067756614493627826
+  v: 0.14407736214370953
+  a: -0.067756635507841334
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 587610.65924926032
-    y: 4140980.8061341876
-    theta: 1.3178677235834042
-    kappa: -0.0062117600046427794
-    s: 0.37056401141669182
-    dkappa: 0.0010494737355816344
+    x: 587610.6591209285
+    y: 4140980.8061674051
+    theta: 1.3183867318105855
+    kappa: -0.0048544320217940246
+    s: 0.37056417994307783
+    dkappa: 0.00077957263781657437
     ddkappa: 0
   }
-  v: 0.13702713217120932
-  a: -0.073127359807884373
+  v: 0.13702721179626531
+  a: -0.0731273863596016
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 587610.66257518728
-    y: 4140980.8190416149
-    theta: 1.317782682257495
-    kappa: -0.0061928857498636794
-    s: 0.38389305782251049
-    dkappa: 0.00108722290628436
+    x: 587610.66244223982
+    y: 4140980.8190760291
+    theta: 1.318320359003208
+    kappa: -0.0048404117927324658
+    s: 0.383893234169695
+    dkappa: 0.0008076135724925603
     ddkappa: 0
   }
-  v: 0.12947672165371471
-  a: -0.077750700828610589
+  v: 0.12947679835636383
+  a: -0.077750732674122428
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587610.66570727632
-    y: 4140980.8311967868
-    theta: 1.3177025972248568
-    kappa: -0.0061751115029707072
-    s: 0.39644527588026579
-    dkappa: 0.0011227720226827536
+    x: 587610.66556998226
+    y: 4140980.831232328
+    theta: 1.318257854465102
+    kappa: -0.0048272086745969126
+    s: 0.39644545973009959
+    dkappa: 0.00083402025754262729
     ddkappa: 0
   }
-  v: 0.1215041384143952
-  a: -0.08155964958209691
+  v: 0.12150421168258829
+  a: -0.08155968635421898
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587610.668636037
-    y: 4140980.842562872
-    theta: 1.3176277111448187
-    kappa: -0.0061584911205599694
-    s: 0.40818263192573423
-    dkappa: 0.0011560133696980688
+    x: 587610.66849467868
+    y: 4140980.8425994669
+    theta: 1.3181994075868109
+    kappa: -0.0048148626733296217
+    s: 0.408182822910905
+    dkappa: 0.00085871267972244291
     ddkappa: 0
   }
-  v: 0.11319417107866028
-  a: -0.084487218094633762
+  v: 0.11319424044324033
+  a: -0.084487259302706355
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587610.67135417857
-    y: 4140980.8531115744
-    theta: 1.3175582104429868
-    kappa: -0.0061430659786684055
-    s: 0.41907590609710182
-    dkappa: 0.0011868641938070643
+    x: 587610.67120904813
+    y: 4140980.8531491477
+    theta: 1.3181451638698494
+    kappa: -0.0048034045240277221
+    s: 0.41907610380604066
+    dkappa: 0.00088162936779305533
     ddkappa: 0
   }
-  v: 0.10463830706929041
-  a: -0.086466418392511527
+  v: 0.10463837211634014
+  a: -0.08646646342239972
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587610.673856776
-    y: 4140980.8628237834
-    theta: 1.3174942210374148
-    kappa: -0.0061288640242077516
-    s: 0.4291053622147013
-    dkappa: 0.0012152686002073212
+    x: 587610.67370817275
+    y: 4140980.8628622568
+    theta: 1.3180952215910406
+    kappa: -0.0047928549863257234
+    s: 0.429105566197724
+    dkappa: 0.000902728801779815
     ddkappa: 0
   }
-  v: 0.095934732606436923
-  a: -0.087430262502020628
+  v: 0.09593479298962633
+  a: -0.087430310616114226
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 587610.67614143842
-    y: 4140980.8716902183
-    theta: 1.3174358040647809
-    kappa: -0.00611589882639849
-    s: 0.438261417660749
-    dkappa: 0.0012411994499825604
+    x: 587610.67598966463
+    y: 4140980.8717295141
+    theta: 1.3180496284668564
+    kappa: -0.0047832241397780346
+    s: 0.43826162743743219
+    dkappa: 0.0009219908222312948
     ddkappa: 0
   }
-  v: 0.08718833270762219
-  a: -0.087311762449451491
+  v: 0.087188388160556074
+  a: -0.087311812786665
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587610.67820847547
-    y: 4140980.879712081
-    theta: 1.3173829516065638
-    kappa: -0.006104168628203805
-    s: 0.446545313259082
-    dkappa: 0.0012646602572679577
+    x: 587610.6780538332
+    y: 4140980.8797521209
+    theta: 1.3180083783177592
+    kappa: -0.00477451067924148
+    s: 0.446545528326874
+    dkappa: 0.00093941803947821065
     ddkappa: 0
   }
-  v: 0.07851069118773954
-  a: -0.0860439302610945
+  v: 0.07851074153630494
+  a: -0.086043981836867234
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587610.68006106431
-    y: 4140980.8869017032
-    theta: 1.3173355824152158
-    kappa: -0.0060936553977635416
-    s: 0.45396978315489528
-    dkappa: 0.0012856870864154618
+    x: 587610.67990385112
+    y: 4140980.8869424104
+    theta: 1.31797140773254
+    kappa: -0.0047667012102578111
+    s: 0.45397000299896167
+    dkappa: 0.000955037242892342
     ddkappa: 0
   }
-  v: 0.0700200906590532
-  a: -0.083559777963240087
+  v: 0.070020135833767053
+  a: -0.083559829669536054
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587610.68170541781
-    y: 4140980.8932831944
-    theta: 1.3172935376403203
-    kappa: -0.0060843238798281565
-    s: 0.46055972469447881
-    dkappa: 0.0013043504491591106
+    x: 587610.68154592277
+    y: 4140980.8933244934
+    theta: 1.3179385927326517
+    kappa: -0.0047597695444362214
+    s: 0.46055994879878304
+    dkappa: 0.00096890081014545351
     ddkappa: 0
   }
-  v: 0.06184151253119842
-  a: -0.079792317582178654
+  v: 0.061841552579554995
+  a: -0.0797923681874866
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587610.68315095059
-    y: 4140980.8988930918
-    theta: 1.317256576554739
-    kappa: -0.006076120647192677
-    s: 0.4663528683049547
-    dkappa: 0.0013207572017803479
+    x: 587610.68298944947
+    y: 4140980.898934911
+    theta: 1.3179097454365341
+    kappa: -0.0047536759948358653
+    s: 0.46635309616457271
+    dkappa: 0.000981088116468214
     ddkappa: 0
   }
-  v: 0.054106637011181347
-  a: -0.074674561144200638
+  v: 0.054106672109999823
+  a: -0.074674609293534
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 587610.68441044644
-    y: 4140980.903781008
-    theta: 1.3172243722807384
-    kappa: -0.0060689731521306528
-    s: 0.471400447374014
-    dkappa: 0.0013350524422733395
+    x: 587610.68424719758
+    y: 4140980.9038232807
+    theta: 1.3178846107239321
+    kappa: -0.0047483666713483662
+    s: 0.47140067850868461
+    dkappa: 0.00099170694390911815
     ddkappa: 0
   }
-  v: 0.046953843103379175
-  a: -0.068139520675596435
+  v: 0.046953873571151154
+  a: -0.068139564890493456
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587610.68550001807
-    y: 4140980.9080094737
-    theta: 1.3171965128322551
-    kappa: -0.0060627899577093977
-    s: 0.47576703489451144
-    dkappa: 0.0013474190477063953
+    x: 587610.68533525732
+    y: 4140980.9080521385
+    theta: 1.3178628670493475
+    kappa: -0.0047437736525235393
+    s: 0.47576726886260706
+    dkappa: 0.0010008931376772115
     ddkappa: 0
   }
-  v: 0.040494837742490346
-  a: -0.061123407280826277
+  v: 0.040494864019093177
+  a: -0.061123446943053654
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587610.68643697014
-    y: 4140980.9116456453
-    theta: 1.3171725557419696
-    kappa: -0.0060574728618544736
-    s: 0.47952198141949631
-    dkappa: 0.0013580534256683013
+    x: 587610.68627090915
+    y: 4140980.9116886477
+    theta: 1.3178441690772513
+    kappa: -0.0047398239916053705
+    s: 0.47952221782413063
+    dkappa: 0.0010087925937640118
     ddkappa: 0
   }
-  v: 0.034712884739040828
-  a: -0.054595601131250115
+  v: 0.034712907263805451
+  a: -0.054595636557664411
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 587610.68723759474
-    y: 4140980.91475275
-    theta: 1.3171520844342317
-    kappa: -0.0060529294089018935
-    s: 0.48273057937244218
-    dkappa: 0.0013671404907256556
+    x: 587610.68707042269
+    y: 4140980.9147960404
+    theta: 1.3178281916799124
+    kappa: -0.00473644901040323
+    s: 0.48273081785909627
+    dkappa: 0.0010155426708851755
     ddkappa: 0
   }
-  v: 0.029560015087179221
-  a: -0.048538867853508173
+  v: 0.029560034268310078
+  a: -0.048538899349782783
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587610.68791700853
-    y: 4140980.9173894534
-    theta: 1.3171347123886981
-    kappa: -0.0060490738136944177
-    s: 0.48545341044810414
-    dkappa: 0.0013748518161978951
+    x: 587610.68774889363
+    y: 4140980.9174329885
+    theta: 1.3178146331870342
+    kappa: -0.0047335849857321043
+    s: 0.48545365070156993
+    dkappa: 0.0010212708175767063
     ddkappa: 0
   }
-  v: 0.024989983218390088
-  a: -0.042935973074240651
+  v: 0.024989999434083442
+  a: -0.04293600093486577
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587610.68848919554
-    y: 4140980.9196100268
-    theta: 1.3171200820407165
-    kappa: -0.006045826717538629
-    s: 0.48774651795625285
-    dkappa: 0.0013813461222517039
+    x: 587610.68832028657
+    y: 4140980.9196537677
+    theta: 1.3178032145275418
+    kappa: -0.0047311729681318972
+    s: 0.48774675969768794
+    dkappa: 0.0010260949347625046
     ddkappa: 0
   }
-  v: 0.020958267001493971
-  a: -0.037769682420087776
+  v: 0.020958280601056235
+  a: -0.037769706928370457
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587610.68896705075
-    y: 4140980.9214645112
-    theta: 1.3171078636817188
-    kappa: -0.0060431149441619991
-    s: 0.48966157916540753
-    dkappa: 0.0013867697639954215
+    x: 587610.6887974788
+    y: 4140980.9215084244
+    theta: 1.3177936783713717
+    kappa: -0.0047291586005867485
+    s: 0.48966182214950238
+    dkappa: 0.0010301237383219144
     ddkappa: 0
   }
-  v: 0.017422067742647398
-  a: -0.03302276151768975
+  v: 0.017422079047613424
+  a: -0.033022782945753865
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587610.68936242221
-    y: 4140980.92299889
-    theta: 1.3170977543596141
-    kappa: -0.0060408712556699624
-    s: 0.49124607764657
-    dkappa: 0.0013912572195734516
+    x: 587610.68919230159
+    y: 4140980.9230429456
+    theta: 1.3177857882712636
+    kappa: -0.0047274919372443276
+    s: 0.49124632165882648
+    dkappa: 0.0010334571216572724
     ddkappa: 0
   }
-  v: 0.01434031018534287
-  a: -0.028677975993686784
+  v: 0.014340319490594293
+  a: -0.028677994602473032
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 587610.68968615518
-    y: 4140980.9242552491
-    theta: 1.31708947677919
-    kappa: -0.0060390341085029831
-    s: 0.4925434756169581
-    dkappa: 0.0013949315782606695
+    x: 587610.6895155852
+    y: 4140980.9242994208
+    theta: 1.3177793278045533
+    kappa: -0.0047261272621351462
+    s: 0.49254372047108019
+    dkappa: 0.001036186518261456
     ddkappa: 0
   }
-  v: 0.011673642510408888
-  a: -0.0247180914747191
+  v: 0.011673650085292414
+  a: -0.024718107513985
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 587610.689948134
-    y: 4140980.9252719493
-    theta: 1.3170827782025165
-    kappa: -0.0060375474093936239
-    s: 0.49359338628373928
-    dkappa: 0.0013979050285568312
+    x: 587610.68977720034
+    y: 4140980.9253162155
+    theta: 1.3177740997149689
+    kappa: -0.0047250229078918664
+    s: 0.49359363181913551
+    dkappa: 0.0010383952642854315
     ddkappa: 0
   }
-  v: 0.0093844363360098965
-  a: -0.021125873587426902
+  v: 0.0093844424254556427
+  a: -0.021125887295746819
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587610.69015732547
-    y: 4140980.9260837906
-    theta: 1.3170774293493521
-    kappa: -0.0060363602713236243
-    s: 0.4944317461877642
-    dkappa: 0.0014002793462809815
+    x: 587610.6899861017
+    y: 4140980.9261281318
+    theta: 1.3177699250544279
+    kappa: -0.0047241410744686069
+    s: 0.49443199226716195
+    dkappa: 0.0010401589611058026
     ddkappa: 0
   }
-  v: 0.007436786717646332
-  a: -0.017884087958450416
+  v: 0.0074367915432861642
+  a: -0.017884099563215509
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 587610.69032182253
-    y: 4140980.9267221792
-    theta: 1.3170732232975568
-    kappa: -0.0060354267694809581
-    s: 0.49509098754730052
-    dkappa: 0.0014021463826658627
+    x: 587610.69015037047
+    y: 4140980.92676658
+    theta: 1.3177666423248358
+    kappa: -0.004723447647860245
+    s: 0.49509123405447192
+    dkappa: 0.0010415458378923586
     ddkappa: 0
   }
-  v: 0.0057965121481546285
-  a: -0.014975500214429841
+  v: 0.0057965159094404252
+  a: -0.014975509931848128
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 587610.69044888625
-    y: 4140980.9272152949
-    theta: 1.3170699743835037
-    kappa: -0.0060347056972169157
-    s: 0.49560021060176623
-    dkappa: 0.0014035885524523223
+    x: 587610.69027725782
+    y: 4140980.9272597414
+    theta: 1.3177641066198869
+    kappa: -0.004722912018821729
+    s: 0.49560045743936626
+    dkappa: 0.0010426171141756219
     ddkappa: 0
   }
-  v: 0.00443115455770717
-  a: -0.012382875982005402
+  v: 0.0044311574330291922
+  a: -0.012382884017101706
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 587610.69054498954
-    y: 4140980.927588257
-    theta: 1.3170675171024948
-    kappa: -0.0060341603220031648
-    s: 0.49598535595546334
-    dkappa: 0.0014046793219837216
+    x: 587610.69037322782
+    y: 4140980.9276327384
+    theta: 1.3177621887668645
+    kappa: -0.0047225069015873851
+    s: 0.49598560304297945
+    dkappa: 0.0010434273624143975
     ddkappa: 0
   }
-  v: 0.0033099793138123496
-  a: -0.010088980887817298
+  v: 0.0033099814616174986
+  a: -0.010088987434433297
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 587610.69061585981
-    y: 4140980.9278632938
-    theta: 1.3170657050091796
-    kappa: -0.006033758141388827
-    s: 0.49626937692131157
-    dkappa: 0.0014054836973003443
+    x: 587610.69044399972
+    y: 4140980.9279078008
+    theta: 1.3177607744684434
+    kappa: -0.0047222081525902185
+    s: 0.49626962419312542
+    dkappa: 0.0010440248705633206
     ddkappa: 0
   }
-  v: 0.0024039752213144954
-  a: -0.0080765805585057537
+  v: 0.0024039767812247034
+  a: -0.0080765857992999273
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 587610.690666522
-    y: 4140980.9280599067
-    theta: 1.3170644096179731
-    kappa: -0.0060334706389575457
-    s: 0.4964724118645818
-    dkappa: 0.0014060587122338042
+    x: 587610.6904945916
+    y: 4140980.9281044318
+    theta: 1.3177597634444933
+    kappa: -0.0047219945891812274
+    s: 0.49647265926814255
+    dkappa: 0.0010444520046404046
     ddkappa: 0
   }
-  v: 0.0016858545223939641
-  a: -0.0063284406207109846
+  v: 0.0016858556163244381
+  a: -0.0063284447271586318
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 587610.69070134184
-    y: 4140980.9281950374
-    theta: 1.3170635193034759
-    kappa: -0.0060332730402845531
-    s: 0.49661195654662976
-    dkappa: 0.0014064539165014542
+    x: 587610.69052936311
+    y: 4140980.9282395747
+    theta: 1.3177590685738807
+    kappa: -0.0047218478083487026
+    s: 0.49661220404073936
+    dkappa: 0.00104474557129459
     ddkappa: 0
   }
-  v: 0.0011300528965670824
-  a: -0.00482732670107318
+  v: 0.0011300536298446738
+  a: -0.0048273298334664694
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 587610.69072406855
-    y: 4140980.9282832365
-    theta: 1.3170629382008958
-    kappa: -0.0060331440688937464
-    s: 0.49670303646862968
-    dkappa: 0.0014067118638007947
+    x: 587610.69055205828
+    y: 4140980.9283277816
+    theta: 1.3177586150362743
+    kappa: -0.00472175200543754
+    s: 0.49670328402183994
+    dkappa: 0.0010449371803732919
     ddkappa: 0
   }
-  v: 0.00071272946068609588
-  a: -0.0035560044262325946
+  v: 0.00071272992316761424
+  a: -0.0035560067336804662
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 587610.690737878
-    y: 4140980.9283368285
-    theta: 1.3170625851064692
-    kappa: -0.0060330657022147523
-    s: 0.49675837921530774
-    dkappa: 0.0014068685999038813
+    x: 587610.69056584861
+    y: 4140980.9283813788
+    theta: 1.3177583394539474
+    kappa: -0.004721693792868545
+    s: 0.49675862680442923
+    dkappa: 0.0010450536074899492
     ddkappa: 0
   }
-  v: 0.00041176676893932236
-  a: -0.0024972394228293861
+  v: 0.00041176703612981431
+  a: -0.0024972410432576777
   relative_time: 5.7999999999999963
 }
 trajectory_point {
   path_point {
-    x: 587610.69074541552
-    y: 4140980.9283660804
-    theta: 1.317062392377883
-    kappa: -0.0060330229275400013
-    s: 0.49678858679867577
-    dkappa: 0.0014069541507517333
+    x: 587610.69057337556
+    y: 4140980.9284106339
+    theta: 1.3177581890335832
+    kappa: -0.0047216620188577389
+    s: 0.49678883440739863
+    dkappa: 0.0010451171565915721
     ddkappa: 0
   }
-  v: 0.00020677081285097197
-  a: -0.0016337973175038166
+  v: 0.00020677094702207675
+  a: -0.0016337983776551167
   relative_time: 5.8999999999999959
 }
 trajectory_point {
   path_point {
-    x: 587610.69074883929
-    y: 4140980.9283793676
-    theta: 1.3170623048346977
-    kappa: -0.006033003497981795
-    s: 0.49680230800176484
-    dkappa: 0.0014069930105487425
+    x: 587610.69057679467
+    y: 4140980.9284239225
+    theta: 1.3177581207080791
+    kappa: -0.0047216475861356667
+    s: 0.49680255561939118
+    dkappa: 0.0010451460225262901
     ddkappa: 0
   }
-  v: 7.9071021281299276e-05
-  a: -0.0009484437368960652
+  v: 7.9071072589541653e-05
+  a: -0.00094844435232985247
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 587610.69074986293
-    y: 4140980.9283833406
-    theta: 1.3170622786587693
-    kappa: -0.0060329976884293769
-    s: 0.49680641072235882
-    dkappa: 0.0014070046298570811
+    x: 587610.690577817
+    y: 4140980.9284278955
+    theta: 1.3177581002783505
+    kappa: -0.0047216432706667028
+    s: 0.49680665834264742
+    dkappa: 0.0010451546536109017
     ddkappa: 0
   }
-  v: 1.1720260426478484e-05
-  a: -0.00042394430764638021
+  v: 1.1720268031638042e-05
+  a: -0.00042394458273889841
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 587610.69074979913
-    y: 4140980.9283830924
-    theta: 1.317062280294673
-    kappa: -0.0060329980515060038
-    s: 0.49680615431672803
-    dkappa: 0.00140700390369111
+    x: 587610.69057775312
+    y: 4140980.9284276473
+    theta: 1.3177581015551367
+    kappa: -0.0047216435403683605
+    s: 0.49680640193685022
+    dkappa: 0.0010451541141984206
     ddkappa: 0
   }
   v: 0
-  a: -4.3064656394940605e-05
+  a: -4.3064684339309646e-05
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
-    x: 587610.69074960123
-    y: 4140980.928382325
-    theta: 1.3170622853501253
-    kappa: -0.0060329991735260137
-    s: 0.49680536194336272
-    dkappa: 0.0014070016596117877
+    x: 587610.69057755568
+    y: 4140980.92842688
+    theta: 1.3177581055008047
+    kappa: -0.0047216443738305929
+    s: 0.49680560956297076
+    dkappa: 0.0010451524472456259
     ddkappa: 0
   }
-  v: 7.1554095277373452e-18
-  a: 0.00021142959021801914
+  v: 7.1554141706651008e-18
+  a: 0.00021142972741185876
   relative_time: 6.2999999999999945
 }
 trajectory_point {
   path_point {
-    x: 587610.69074990845
-    y: 4140980.9283835171
-    theta: 1.3170622774964078
-    kappa: -0.0060329974304518972
-    s: 0.49680659290670687
-    dkappa: 0.0014070051458210775
+    x: 587610.69057786232
+    y: 4140980.928428072
+    theta: 1.3177580993711533
+    kappa: -0.0047216430790351068
+    s: 0.49680684052711366
+    dkappa: 0.0010451550368806093
     ddkappa: 0
   }
-  v: 2.8142384153067076e-05
-  a: 0.0003567728055522924
+  v: 2.8142402414230491e-05
+  a: 0.00035677303705757956
   relative_time: 6.3999999999999941
 }
 trajectory_point {
   path_point {
-    x: 587610.6907510868
-    y: 4140980.9283880894
-    theta: 1.3170622473687896
-    kappa: -0.0060329907438513687
-    s: 0.4968113150008916
-    dkappa: 0.0014070185192563576
+    x: 587610.69057903893
+    y: 4140980.9284326448
+    theta: 1.3177580758572192
+    kappa: -0.004721638112074664
+    s: 0.49681156262436249
+    dkappa: 0.0010451649709703231
     ddkappa: 0
   }
-  v: 6.7185154839365316e-05
-  a: 0.00041019936296767239
+  v: 6.7185198434902219e-05
+  a: 0.00041019962914082535
   relative_time: 6.4999999999999938
 }
 trajectory_point {
   path_point {
-    x: 587610.690753273
-    y: 4140980.9283965747
-    theta: 1.3170621914669509
-    kappa: -0.0060329783368544363
-    s: 0.49682007685346891
-    dkappa: 0.0014070433336848278
+    x: 587610.6905812223
+    y: 4140980.9284411306
+    theta: 1.3177580322270803
+    kappa: -0.0047216288958723922
+    s: 0.4968203244826252
+    dkappa: 0.0010451834036881295
     ddkappa: 0
   }
-  v: 0.00010769284726127836
-  a: 0.00038894363582395251
+  v: 0.00010769291714174089
+  a: 0.00038894388820455504
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
-    x: 587610.69075641793
-    y: 4140980.9284087792
-    theta: 1.3170621110554048
-    kappa: -0.0060329604901104665
-    s: 0.49683268026914507
-    dkappa: 0.0014070790277979184
+    x: 587610.69058436283
+    y: 4140980.9284533365
+    theta: 1.3177579694676615
+    kappa: -0.0047216156389010867
+    s: 0.49683292790647959
+    dkappa: 0.0010452099180813478
     ddkappa: 0
   }
-  v: 0.00014305895163085943
-  a: 0.00031023999748088437
+  v: 0.00014305904445991637
+  a: 0.00031024019879169961
   relative_time: 6.6999999999999931
 }
 trajectory_point {
   path_point {
-    x: 587610.69076032843
-    y: 4140980.9284239556
-    theta: 1.3170620110639217
-    kappa: -0.0060329382977452641
-    s: 0.49684835257351456
-    dkappa: 0.0014071234133056986
+    x: 587610.690588268
+    y: 4140980.9284685147
+    theta: 1.3177578914265387
+    kappa: -0.0047215991539025252
+    s: 0.49684860022101857
+    dkappa: 0.0010452428886388036
     ddkappa: 0
   }
-  v: 0.00016840039549615072
-  a: 0.00019132282129830292
+  v: 0.00016840050476896374
+  a: 0.0001913229454452734
   relative_time: 6.7999999999999927
 }
 trajectory_point {
   path_point {
-    x: 587610.69076471182
-    y: 4140980.9284409666
-    theta: 1.3170618989879519
-    kappa: -0.0060329134233181323
-    s: 0.49686591895679327
-    dkappa: 0.0014071731630312849
+    x: 587610.69059264509
+    y: 4140980.9284855267
+    theta: 1.317757803953743
+    kappa: -0.0047215806766067641
+    s: 0.49686616661569588
+    dkappa: 0.0010452798438583761
     ddkappa: 0
   }
-  v: 0.00018055754374118367
-  a: 4.9426480635973747e-05
+  v: 0.00018055766090261006
+  a: 4.9426512708124238e-05
   relative_time: 6.8999999999999924
 }
 trajectory_point {
   path_point {
-    x: 587610.69076921709
-    y: 4140980.9284584513
-    theta: 1.3170617837890475
-    kappa: -0.0060328878557789518
-    s: 0.49688397481755237
-    dkappa: 0.0014072242990052495
+    x: 587610.6905971443
+    y: 4140980.9285030132
+    theta: 1.3177577140435666
+    kappa: -0.0047215616844514538
+    s: 0.49688422248817127
+    dkappa: 0.0010453178288145472
     ddkappa: 0
   }
-  v: 0.00017809419858601334
-  a: -9.8214651146323755e-05
+  v: 0.00017809431414899613
+  a: -9.8214714876622571e-05
   relative_time: 6.999999999999992
 }
 trajectory_point {
   path_point {
-    x: 587610.69077347987
-    y: 4140980.9284749944
-    theta: 1.317061674795287
-    kappa: -0.006032863665425243
-    s: 0.49690105810645191
-    dkappa: 0.0014072726805600285
+    x: 587610.690601401
+    y: 4140980.9285195577
+    theta: 1.3177576289763666
+    kappa: -0.004721543715301142
+    s: 0.49690130578815594
+    dkappa: 0.001045353767725949
     ddkappa: 0
   }
-  v: 0.00016129759958658676
-  a: -0.00023436620068881015
+  v: 0.00016129770425045461
+  a: -0.00023436635276617474
   relative_time: 7.0999999999999917
 }
 trajectory_point {
   path_point {
-    x: 587610.69077716372
-    y: 4140980.9284892906
-    theta: 1.3170615806016972
-    kappa: -0.0060328427598592424
-    s: 0.49691582166997422
-    dkappa: 0.0014073144924243294
+    x: 587610.69060507987
+    y: 4140980.9285338554
+    theta: 1.31775755546037
+    kappa: -0.00472152818616658
+    s: 0.49691606936125815
+    dkappa: 0.0010453848265229129
     ddkappa: 0
   }
-  v: 0.00013217842363491642
-  a: -0.00034179379463167823
+  v: 0.0001321785094037182
+  a: -0.0003417940164174069
   relative_time: 7.1999999999999913
 }
 trajectory_point {
   path_point {
-    x: 587610.69078000425
-    y: 4140980.9285003147
-    theta: 1.3170615079706767
-    kappa: -0.0060328266399449675
-    s: 0.49692720559415771
-    dkappa: 0.001407346732817542
+    x: 587610.69060791645
+    y: 4140980.9285448804
+    theta: 1.3177574987734777
+    kappa: -0.0047215162119240324
+    s: 0.4969274532928285
+    dkappa: 0.0010454087754150168
     ddkappa: 0
   }
-  v: 9.4470784958906653e-05
-  a: -0.0004032630596151209
+  v: 9.4470846259697485e-05
+  a: -0.00040326332128749911
   relative_time: 7.2999999999999909
 }
 trajectory_point {
   path_point {
-    x: 587610.69078185176
-    y: 4140980.9285074845
-    theta: 1.3170614607324187
-    kappa: -0.0060328161557652912
-    s: 0.4969346095483303
-    dkappa: 0.001407367701544143
+    x: 587610.69060976128
+    y: 4140980.9285520511
+    theta: 1.3177574619050707
+    kappa: -0.004721508424034581
+    s: 0.49693485725180542
+    dkappa: 0.0010454243514586344
     ddkappa: 0
   }
-  v: 5.3632235122506477e-05
-  a: -0.00040153962227940023
+  v: 5.3632269923689246e-05
+  a: -0.00040153988283342303
   relative_time: 7.3999999999999906
 }
 trajectory_point {
   path_point {
-    x: 587610.690782714
-    y: 4140980.9285108307
-    theta: 1.3170614386853343
-    kappa: -0.0060328112625790105
-    s: 0.49693806512884314
-    dkappa: 0.0014073774880881075
+    x: 587610.69061062241
+    y: 4140980.9285553978
+    theta: 1.3177574446978129
+    kappa: -0.00472150478926343
+    s: 0.49693831283456052
+    dkappa: 0.0010454316211244827
     ddkappa: 0
   }
-  v: 1.684376302564696e-05
-  a: -0.000319389109264723
+  v: 1.684377395520977e-05
+  a: -0.00031938931651224767
   relative_time: 7.49999999999999
 }
 trajectory_point {
   path_point {
-    x: 587610.69078279962
-    y: 4140980.9285111628
-    theta: 1.3170614364964746
-    kappa: -0.006032810776777915
-    s: 0.49693840820280422
-    dkappa: 0.001407378459707316
+    x: 587610.69061070785
+    y: 4140980.92855573
+    theta: 1.3177574429894567
+    kappa: -0.0047215044283992185
+    s: 0.4969386559087442
+    dkappa: 0.0010454323428651712
     ddkappa: 0
   }
   v: 0
-  a: -0.00013957714721126813
+  a: -0.0001395772377810004
   relative_time: 7.59999999999999
 }
 trajectory_point {
   path_point {
-    x: 587610.69078256085
-    y: 4140980.9285102361
-    theta: 1.3170614426019551
-    kappa: -0.0060328121318438581
-    s: 0.49693745125181193
-    dkappa: 0.0014073757495279631
+    x: 587610.69061046944
+    y: 4140980.9285548031
+    theta: 1.3177574477546481
+    kappa: -0.004721505434973323
+    s: 0.49693769895713091
+    dkappa: 0.0010454303296827487
     ddkappa: 0
   }
   v: 0
-  a: 0.00015513063724071605
+  a: 0.00015513073790329154
   relative_time: 7.6999999999999895
 }
 trajectory_point {
   path_point {
-    x: 587610.69078273664
-    y: 4140980.9285109183
-    theta: 1.3170614381073775
-    kappa: -0.0060328111343058285
-    s: 0.49693815571568839
-    dkappa: 0.0014073777446389649
+    x: 587610.690610645
+    y: 4140980.9285554853
+    theta: 1.3177574442467306
+    kappa: -0.0047215046939791646
+    s: 0.4969384034214645
+    dkappa: 0.0010454318116962528
     ddkappa: 0
   }
-  v: 2.8440262210946117e-05
-  a: 0.00058196861745105056
+  v: 2.84402806654063e-05
+  a: 0.000581968995083573
   relative_time: 7.7999999999999892
 }
 trajectory_point {
   path_point {
-    x: 587610.69078439567
-    y: 4140980.928517357
-    theta: 1.3170613956882533
-    kappa: -0.0060328017196970181
-    s: 0.49694480433621363
-    dkappa: 0.0014073965741863686
+    x: 587610.6906123017
+    y: 4140980.928561924
+    theta: 1.3177574111395498
+    kappa: -0.0047214977005915161
+    s: 0.496945052046304
+    dkappa: 0.0010454457987092577
     ddkappa: 0
   }
-  v: 0.00011413073679086998
-  a: 0.0011581711667795287
+  v: 0.00011413081084881032
+  a: 0.0011581719183028306
   relative_time: 7.8999999999999888
 }
 trajectory_point {
   path_point {
-    x: 587610.69078897918
-    y: 4140980.9285351448
-    theta: 1.3170612784904268
-    kappa: -0.0060327757085118939
-    s: 0.49696317350085878
-    dkappa: 0.0014074485974677608
+    x: 587610.6906168788
+    y: 4140980.9285797141
+    theta: 1.3177573196692594
+    kappa: -0.0047214783788858093
+    s: 0.49696342122286874
+    dkappa: 0.0010454844427774223
     ddkappa: 0
   }
-  v: 0.00026562779364940981
-  a: 0.0019009726585858884
+  v: 0.00026562796601198541
+  a: 0.0019009738921039539
   relative_time: 7.9999999999999885
 }
 decision {
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_nudge_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_nudge_0.pb.txt
index f1b4a30d76..a4bb74a159 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_nudge_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_nudge_0.pb.txt
@@ -8,8 +8,8 @@ trajectory_point {
     x: 587325.33820609981
     y: 4141206.1755347056
     z: 0
-    theta: -1.8233679519349359
-    kappa: -0.00027054333501513736
+    theta: -1.8231679377244114
+    kappa: -0.00027054449859613774
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -20,12 +20,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587325.28312034253
-    y: 4141205.9621873545
-    theta: -1.8234194921203246
-    kappa: -0.00025240376812389696
+    x: 587325.28316319024
+    y: 4141205.9621762964
+    theta: -1.8232188709692863
+    kappa: -0.00025105072646402772
     s: 0.22034412371128145
-    dkappa: 2.0593856593746046e-05
+    dkappa: 2.213130032542547e-05
     ddkappa: 0
   }
   v: 11.023327392508563
@@ -34,12 +34,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587325.2279731161
-    y: 4141205.748601933
-    theta: -1.8234710898187239
-    kappa: -0.0002342439596264577
+    x: 587325.22805885924
+    y: 4141205.7485798039
+    theta: -1.8232698610498888
+    kappa: -0.00023153520159321152
     s: 0.44093412534275389
-    dkappa: 4.121069349004201e-05
+    dkappa: 4.4287296558340932e-05
     ddkappa: 0
   }
   v: 11.035697974310629
@@ -48,12 +48,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587325.17276366428
-    y: 4141205.5347755132
-    theta: -1.8235227457373331
-    kappa: -0.00021606366052753941
+    x: 587325.17289235129
+    y: 4141205.5347423004
+    theta: -1.8233209086650959
+    kappa: -0.00021199765639972297
     s: 0.66177302947858563
-    dkappa: 6.1850793373316085e-05
+    dkappa: 6.6468292487109159e-05
     ddkappa: 0
   }
   v: 11.048216577736797
@@ -62,12 +62,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587325.11749091221
-    y: 4141205.32070538
-    theta: -1.8235743173507672
-    kappa: -0.00019787339263835715
+    x: 587325.11766260141
+    y: 4141205.3206610684
+    theta: -1.8233718607980323
+    kappa: -0.00019244995937070069
     s: 0.88286373287802122
-    dkappa: 8.231158986784438e-05
+    dkappa: 8.8455905781361692e-05
     ddkappa: 0
   }
   v: 11.06087684742452
@@ -76,12 +76,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587325.0621168057
-    y: 4141205.1063985014
-    theta: -1.8236105504287012
-    kappa: -0.00018081918306714961
+    x: 587325.06233254774
+    y: 4141205.1063428121
+    theta: -1.8234063510101179
+    kappa: -0.00017418311303642376
     s: 1.104209005910727
-    dkappa: 8.0985817169260615e-05
+    dkappa: 8.6962424203535876e-05
     ddkappa: 0
   }
   v: 11.073672499778571
@@ -90,12 +90,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587325.00667835167
-    y: 4141204.891842586
-    theta: -1.8236468256115037
-    kappa: -0.00016374515559206096
+    x: 587325.00693819765
+    y: 4141204.8917755056
+    theta: -1.8234408813017557
+    kappa: -0.00015589503964947389
     s: 1.3258114939921386
-    dkappa: 7.9658503851875426e-05
+    dkappa: 8.5467207120218912e-05
     ddkappa: 0
   }
   v: 11.086597322971029
@@ -104,12 +104,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.95117491891
-    y: 4141204.6770351939
-    theta: -1.8236831433119003
-    kappa: -0.00014665111590601756
+    x: 587324.95147892064
+    y: 4141204.676956709
+    theta: -1.8234754520658274
+    kappa: -0.00013758553108660494
     s: 1.5476737190188055
-    dkappa: 7.8329634810502492e-05
+    dkappa: 8.3970237515434035e-05
     ddkappa: 0
   }
   v: 11.099645176941291
@@ -118,12 +118,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.89560502709
-    y: 4141204.4619742273
-    theta: -1.8237189701586896
-    kappa: -0.0001295798613957789
+    x: 587324.89595330728
+    y: 4141204.4618843063
+    theta: -1.8235094929043199
+    kappa: -0.00011930281627447835
     s: 1.7697980808037386
-    dkappa: 7.6999197819107177e-05
+    dkappa: 8.2471501468023333e-05
     ddkappa: 0
   }
   v: 11.112809993396064
@@ -132,12 +132,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.839945841
-    y: 4141204.2466632351
-    theta: -1.8237408045812373
-    kappa: -0.00011361848440757295
+    x: 587324.840340313
+    y: 4141204.2465613782
+    theta: -1.8235285684625084
+    kappa: -0.00010227150076347247
     s: 1.9921868585117564
-    dkappa: 7.5667233559519021e-05
+    dkappa: 8.0971041039082812e-05
     ddkappa: 0
   }
   v: 11.126085775809369
@@ -146,12 +146,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.7842199367
-    y: 4141204.0310941506
-    theta: -1.8237626651765746
-    kappa: -9.7637974628218138e-05
+    x: 587324.78466065577
+    y: 4141204.0309803439
+    theta: -1.8235476668864485
+    kappa: -8.5219769933481064e-05
     s: 2.2148422120948306
-    dkappa: 7.4333672683673055e-05
+    dkappa: 7.946878201870277e-05
     ddkappa: 0
   }
   v: 11.139466599422537
@@ -160,12 +160,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.728426803
-    y: 4141203.815264998
-    theta: -1.8237845521451839
-    kappa: -8.1638185485945214e-05
+    x: 587324.728913825
+    y: 4141203.8151392266
+    theta: -1.8235667883512952
+    kappa: -6.8147467387593685e-05
     s: 2.4377661837274327
-    dkappa: 7.2998502960271286e-05
+    dkappa: 7.7964710628301322e-05
     ddkappa: 0
   }
   v: 11.152946611244214
@@ -174,12 +174,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.67256486672
-    y: 4141203.5991741926
-    theta: -1.8238053399693108
-    kappa: -6.5716439307796757e-05
+    x: 587324.67309847509
+    y: 4141203.5990363848
+    theta: -1.8235847342960687
+    kappa: -5.1164251773396626e-05
     s: 2.6609606992418806
-    dkappa: 7.16617569283291e-05
+    dkappa: 7.6458858377366221e-05
     ddkappa: 0
   }
   v: 11.166520030050357
@@ -188,12 +188,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.61662266462
-    y: 4141203.38282283
-    theta: -1.8238136686466322
-    kappa: -5.0856138753581848e-05
+    x: 587324.61720543716
+    y: 4141203.3826723155
+    theta: -1.8235894094260896
+    kappa: -3.5377956067668187e-05
     s: 2.8844275695636834
-    dkappa: 7.0323868513598887e-05
+    dkappa: 7.4951662095162436e-05
     ddkappa: 0
   }
   v: 11.180181146384239
@@ -202,12 +202,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.56061185664
-    y: 4141203.1662061419
-    theta: -1.8238220075379648
-    kappa: -3.5977614026064821e-05
+    x: 587324.56124385376
+    y: 4141203.1660429044
+    theta: -1.8235940902895331
+    kappa: -1.9572300579296714e-05
     s: 3.1081684921468904
-    dkappa: 6.8984339357469116e-05
+    dkappa: 7.34426174380791e-05
     ddkappa: 0
   }
   v: 11.193924322556441
@@ -216,12 +216,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.50453204627
-    y: 4141202.9493225915
-    theta: -1.8238303567023959
-    kappa: -2.108075969670069e-05
+    x: 587324.50521332864
+    y: 4141202.9491466172
+    theta: -1.8235987769195674
+    kappa: -3.7471733101311083e-06
     s: 3.3321850524094345
-    dkappa: 6.7643159968097528e-05
+    dkappa: 7.1931713713094849e-05
     ddkappa: 0
   }
   v: 11.207743992644858
@@ -230,12 +230,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.44838258077
-    y: 4141202.7321708268
-    theta: -1.8238368500822701
-    kappa: -6.3403488290524983e-06
+    x: 587324.44911372056
+    y: 4141202.7319819666
+    theta: -1.8236014917432632
+    kappa: 1.1900506946667514e-05
     s: 3.5564787251684793
-    dkappa: 6.6300368511831712e-05
+    dkappa: 7.0418988776158948e-05
     ddkappa: 0
   }
   v: 11.221634662494699
@@ -244,12 +244,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.39216176583
-    y: 4141202.5147498734
-    theta: -1.8238325850108807
-    kappa: 7.4094465850700857e-06
+    x: 587324.39294577506
+    y: 4141202.514547349
+    theta: -1.8235928013914198
+    kappa: 2.6431014371059889e-05
     s: 3.7810508760757675
-    dkappa: 6.4956181017870391e-05
+    dkappa: 6.8904661442357879e-05
     ddkappa: 0
   }
   v: 11.235590909718486
@@ -258,12 +258,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.33587092
-    y: 4141202.2970580915
-    theta: -1.8238283146267475
-    kappa: 2.1176369295322822e-05
+    x: 587324.33670786454
+    y: 4141202.2968418859
+    theta: -1.8235841002145248
+    kappa: 4.0979621577745995e-05
     s: 4.0059027630529629
-    dkappa: 6.3610319150104208e-05
+    dkappa: 6.738844780174985e-05
     ddkappa: 0
   }
   v: 11.24960738369605
@@ -272,12 +272,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.279509755
-    y: 4141202.0790943671
-    theta: -1.8238240389079987
-    kappa: 3.4960489811939032e-05
+    x: 587324.28039970086
+    y: 4141202.0788644631
+    theta: -1.8235753881680146
+    kappa: 5.5546403080523155e-05
     s: 4.231035537727001
-    dkappa: 6.2262776015414316e-05
+    dkappa: 6.5870340088723057e-05
     ddkappa: 0
   }
   v: 11.263678805574539
@@ -286,12 +286,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.2230801658
-    y: 4141201.8608571282
-    theta: -1.8238170597062833
-    kappa: 4.8486140318225615e-05
+    x: 587324.22402412281
+    y: 4141201.8606132655
+    theta: -1.823563822065289
+    kappa: 6.9820765823467e-05
     s: 4.4564502468654323
-    dkappa: 6.0913592858756212e-05
+    dkappa: 6.4350379881253182e-05
     ddkappa: 0
   }
   v: 11.277799968268411
@@ -300,12 +300,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.16658691026
-    y: 4141201.6423441702
-    theta: -1.8238011310545121
-    kappa: 6.1115081511043453e-05
+    x: 587324.16758807108
+    y: 4141201.6420855261
+    theta: -1.8235428211796494
+    kappa: 8.30837214849845e-05
     s: 4.6821478338117712
-    dkappa: 5.9562874016511858e-05
+    dkappa: 6.2828673391314486e-05
     ddkappa: 0
   }
   v: 11.291965736459437
@@ -314,12 +314,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.11002263823
-    y: 4141201.4235565243
-    theta: -1.8237851823791993
-    kappa: 7.375989825073727e-05
+    x: 587324.11108107481
+    y: 4141201.4232830806
+    theta: -1.8235217938942738
+    kappa: 9.6363349698151674e-05
     s: 4.9081291399208373
-    dkappa: 5.8210457217172841e-05
+    dkappa: 6.1305053999704634e-05
     ddkappa: 0
   }
   v: 11.306171046596699
@@ -328,12 +328,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587324.05338716449
-    y: 4141201.2044934742
-    theta: -1.8237692136280705
-    kappa: 8.6420631988961058e-05
+    x: 587324.05450294889
+    y: 4141201.2042052122
+    theta: -1.82350074014025
+    kappa: 0.00010965969399563222
     s: 5.1343949059941076
-    dkappa: 5.6856338027308915e-05
+    dkappa: 5.977951671176496e-05
     ddkappa: 0
   }
   v: 11.320410906896596
@@ -342,12 +342,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.99668624671
-    y: 4141200.9851528681
-    theta: -1.8237496549457091
-    kappa: 9.8697026074540388e-05
+    x: 587323.997860982
+    y: 4141200.9848493757
+    theta: -1.8234759241451359
+    kappa: 0.00012252177563578477
     s: 5.3609457737150619
-    dkappa: 5.5500699413360446e-05
+    dkappa: 5.8252245278249545e-05
     ddkappa: 0
   }
   v: 11.334680397342836
@@ -356,12 +356,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.93992558436
-    y: 4141200.7655326705
-    theta: -1.8237229864394888
-    kappa: 0.0001101944200869632
+    x: 587323.94116238132
+    y: 4141200.7652131477
+    theta: -1.8234436632099562
+    kappa: 0.00013450501666503769
     s: 5.5877822870845222
-    dkappa: 5.4143722312189485e-05
+    dkappa: 5.672342143694321e-05
     ddkappa: 0
   }
   v: 11.34897466968644
@@ -370,12 +370,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.88309333369
-    y: 4141200.5456354818
-    theta: -1.8236962842980797
-    kappa: 0.00012170631497721471
+    x: 587323.8843922707
+    y: 4141200.5452999081
+    theta: -1.8234113615862215
+    kappa: 0.00014650337133777967
     s: 5.8149048938560091
-    dkappa: 5.2785033748478489e-05
+    dkappa: 5.5192669394540147e-05
     ddkappa: 0
   }
   v: 11.363288947445742
@@ -384,12 +384,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.82618940633
-    y: 4141200.3254609588
-    theta: -1.823669548480026
-    kappa: 0.00013323272863442607
+    x: 587323.82755056187
+    y: 4141200.3251093142
+    theta: -1.8233790192238033
+    kappa: 0.00015851685829908546
     s: 6.0423139469710794
-    dkappa: 5.1424631610867124e-05
+    dkappa: 5.3659986772298674e-05
     ddkappa: 0
   }
   v: 11.377618525906389
@@ -398,12 +398,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.76922626235
-    y: 4141200.1050056145
-    theta: -1.8236383705293777
-    kappa: 0.0001442247233038177
+    x: 587323.77065198263
+    y: 4141200.1046372973
+    theta: -1.8233420614195
+    kappa: 0.0001699269734511913
     s: 6.2700097059946787
-    dkappa: 5.00626033624139e-05
+    dkappa: 5.2125461901708315e-05
     ddkappa: 0
   }
   v: 11.39195877212134
@@ -412,12 +412,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.71220621548
-    y: 4141199.8842686787
-    theta: -1.823601921309036
-    kappa: 0.000154579063209408
+    x: 587323.71369927144
+    y: 4141199.8838829785
+    theta: -1.82329962841682
+    kappa: 0.00018061748551501252
     s: 6.4979923385504827
-    dkappa: 4.8698964742067985e-05
+    dkappa: 5.0589110582231468e-05
     ddkappa: 0
   }
   v: 11.406305124910869
@@ -426,12 +426,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.6551144002
-    y: 4141199.6632539118
-    theta: -1.8235654262117877
-    kappa: 0.00016494643561897683
+    x: 587323.65667487658
+    y: 4141199.6628508065
+    theta: -1.8232571420057109
+    kappa: 0.00019132145320635558
     s: 6.7262619217562474
-    dkappa: 4.733360977612139e-05
+    dkappa: 4.90508255320807e-05
     ddkappa: 0
   }
   v: 11.420653094862557
@@ -440,12 +440,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.59795081965
-    y: 4141199.4419613252
-    theta: -1.8235288852396407
-    kappa: 0.00017532683998966011
+    x: 587323.59957880108
+    y: 4141199.4415407935
+    theta: -1.8232146021885509
+    kappa: 0.00020203887596473139
     s: 6.9548184436591507
-    dkappa: 4.5966538536067832e-05
+    dkappa: 4.7510606831804855e-05
     ddkappa: 0
   }
   v: 11.434998264331302
@@ -454,12 +454,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.54073643254
-    y: 4141199.22038561
-    theta: -1.8234871353217532
-    kappa: 0.00018499865586699252
+    x: 587323.5424351386
+    y: 4141199.2199468287
+    theta: -1.823166707898304
+    kappa: 0.00021195666170235162
     s: 7.1836618046711456
-    dkappa: 4.459799556488147e-05
+    dkappa: 4.5968699680941675e-05
     ddkappa: 0
   }
   v: 11.449336287439314
@@ -468,12 +468,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.48346442776
-    y: 4141198.9985287176
-    theta: -1.8234418678894169
-    kappa: 0.000194198272644748
+    x: 587323.48523604812
+    y: 4141198.9980711257
+    theta: -1.8231151963190708
+    kappa: 0.00022134124802489579
     s: 7.4127918190043
-    dkappa: 4.3227902057882578e-05
+    dkappa: 4.4425025200262716e-05
     ddkappa: 0
   }
   v: 11.463662890076115
@@ -482,12 +482,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.42612084036
-    y: 4141198.7763945321
-    theta: -1.8233965438786071
-    kappa: 0.0002034093877490383
+    x: 587323.4279654664
+    y: 4141198.7759181065
+    theta: -1.8230636203569632
+    kappa: 0.00023073756386189008
     s: 7.6422082161061473
-    dkappa: 4.1856096111886787e-05
+    dkappa: 4.2879421326906754e-05
     ddkappa: 0
   }
   v: 11.477973869898541
@@ -496,12 +496,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.36870575882
-    y: 4141198.5539833973
-    theta: -1.8233511633594242
-    kappa: 0.00021263198697144255
+    x: 587323.37062348146
+    y: 4141198.5534881153
+    theta: -1.823011980091813
+    kappa: 0.00024014559471923603
     s: 7.8719106420950258
-    dkappa: 4.0482579842945858e-05
+    dkappa: 4.133189044501403e-05
     ddkappa: 0
   }
   v: 11.492265096330739
@@ -510,12 +510,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.31125074741
-    y: 4141198.3312876173
-    theta: -1.823299958351231
-    kappa: 0.00022094775788913693
+    x: 587323.31324575469
+    y: 4141198.3307724106
+    theta: -1.8229544344698507
+    kappa: 0.00024853059195830383
     s: 8.1018986611954364
-    dkappa: 3.9107684500635836e-05
+    dkappa: 3.9782763921884734e-05
     ddkappa: 0
   }
   v: 11.506532510564167
@@ -524,12 +524,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.253734557
-    y: 4141198.1083132094
-    theta: -1.8232468491983502
-    kappa: 0.00022898079513325562
+    x: 587323.25580824935
+    y: 4141198.1077777212
+    theta: -1.8228949539674559
+    kappa: 0.00025659586399808746
     s: 8.3321717571733789
-    dkappa: 3.7731189810374349e-05
+    dkappa: 3.8231821993286121e-05
     ddkappa: 0
   }
   v: 11.520772125557599
@@ -538,12 +538,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.19614731031
-    y: 4141197.8850633372
-    theta: -1.8231936744338595
-    kappa: 0.0002370237564696443
+    x: 587323.19829978491
+    y: 4141197.8845075411
+    theta: -1.8228353999822053
+    kappa: 0.00026467109995332086
     s: 8.5627293347717011
-    dkappa: 3.6352994585196841e-05
+    dkappa: 3.6678964015958115e-05
     ddkappa: 0
   }
   v: 11.534980026037122
@@ -552,12 +552,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.13848917559
-    y: 4141197.6615386512
-    theta: -1.8231404342132418
-    kappa: 0.00024507661842622261
+    x: 587323.1407205296
+    y: 4141197.6609625234
+    theta: -1.8227757726883203
+    kappa: 0.00027275627625773541
     s: 8.7935707211454446
-    dkappa: 3.4973102847142917e-05
+    dkappa: 3.512419452166528e-05
     ddkappa: 0
   }
   v: 11.549152368496129
@@ -566,12 +566,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.08080647676
-    y: 4141197.4377279822
-    theta: -1.823080991424997
-    kappa: 0.0002520002491749443
+    x: 587323.0831219136
+    y: 4141197.4371302
+    theta: -1.8227099715773614
+    kappa: 0.00027956781725373272
     s: 9.0246951672972
-    dkappa: 3.3591628756173961e-05
+    dkappa: 3.3567627001295365e-05
     ddkappa: 0
   }
   v: 11.563285381195334
@@ -580,12 +580,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587323.02305649221
-    y: 4141197.2136431951
-    theta: -1.8230210566338534
-    kappa: 0.000258854489607251
+    x: 587323.02545646043
+    y: 4141197.2130236453
+    theta: -1.8226436735606475
+    kappa: 0.00028630003605445957
     s: 9.256101849512433
-    dkappa: 3.2208475182986371e-05
+    dkappa: 3.2009166093612738e-05
     ddkappa: 0
   }
   v: 11.577375364162759
@@ -594,12 +594,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587322.9652362963
-    y: 4141196.98928597
-    theta: -1.8229610489753452
-    kappa: 0.00026571706327173166
+    x: 587322.9677208988
+    y: 4141196.9886446265
+    theta: -1.822577294940307
+    kappa: 0.00029304043973619055
     s: 9.4877898707948525
-    dkappa: 3.082364000262853e-05
+    dkappa: 3.0448810444024577e-05
     ddkappa: 0
   }
   v: 11.591418689193739
@@ -608,12 +608,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587322.90735313029
-    y: 4141196.7646554443
-    theta: -1.8229002410091826
-    kappa: 0.0002724270222323717
+    x: 587322.90992318233
+    y: 4141196.7639920986
+    theta: -1.8225101311689402
+    kappa: 0.00029960759392372614
     s: 9.7197582623017418
-    dkappa: 2.9437202411013304e-05
+    dkappa: 2.888663975772927e-05
     ddkappa: 0
   }
   v: 11.605411799850923
@@ -622,12 +622,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587322.849446418
-    y: 4141196.5397425331
-    theta: -1.8228345611224912
-    kappa: 0.0002780838937994092
+    x: 587322.85210669658
+    y: 4141196.5390559775
+    theta: -1.8224382410709956
+    kappa: 0.00030498702233182443
     s: 9.9520059847793139
-    dkappa: 2.8049579365892737e-05
+    dkappa: 2.7323070267227339e-05
     ddkappa: 0
   }
   v: 11.61935121146427
@@ -636,12 +636,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587322.79147033591
-    y: 4141196.314560187
-    theta: -1.8227688025536981
-    kappa: 0.00028374754205427391
+    x: 587322.794220949
+    y: 4141196.3138503926
+    theta: -1.8223662648512498
+    kappa: 0.00031037289506282773
     s: 10.184531929998053
-    dkappa: 2.6660294008293052e-05
+    dkappa: 2.5757627688003534e-05
     ddkappa: 0
   }
   v: 11.633233511131055
@@ -650,12 +650,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587322.73342517728
-    y: 4141196.0891095437
-    theta: -1.8227029656358804
-    kappa: 0.00028941793835866864
+    x: 587322.73626623279
+    y: 4141196.0883764825
+    theta: -1.8222942028743958
+    kappa: 0.00031576518488301371
     s: 10.417334922188063
-    dkappa: 2.5269353363150337e-05
+    dkappa: 2.4190319935734793e-05
     ddkappa: 0
   }
   v: 11.647055357715864
@@ -664,12 +664,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587322.67533132515
-    y: 4141195.86338664
-    theta: -1.8226355123000406
-    kappa: 0.0002946720627202699
+    x: 587322.67826472758
+    y: 4141195.8626298327
+    theta: -1.8222206250329334
+    kappa: 0.00032068713402722351
     s: 10.650413719474408
-    dkappa: 2.3876850765561095e-05
+    dkappa: 2.2621240500389181e-05
     ddkappa: 0
   }
   v: 11.660813481850592
@@ -678,12 +678,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587322.61720722856
-    y: 4141195.6373880776
-    theta: -1.82256505393354
-    kappa: 0.00029912798209201605
+    x: 587322.62023649248
+    y: 4141195.6366066374
+    theta: -1.82214424103261
+    kappa: 0.00032470857169549655
     s: 10.883767015312467
-    dkappa: 2.2482871591347716e-05
+    dkappa: 2.1050474988035097e-05
     ddkappa: 0
   }
   v: 11.674504685934455
@@ -692,12 +692,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587322.55901510059
-    y: 4141195.411124995
-    theta: -1.822494513098541
-    kappa: 0.00030358911690237471
+    x: 587322.562140338
+    y: 4141195.4103188934
+    theta: -1.8220677676280159
+    kappa: 0.00032873471626251329
     s: 11.117393439923275
-    dkappa: 2.1087260831774795e-05
+    dkappa: 1.9477870970443178e-05
     ddkappa: 0
   }
   v: 11.688125844133973
@@ -706,12 +706,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587322.50075529783
-    y: 4141195.1845987788
-    theta: -1.8224238902279943
-    kappa: 0.0003080554398151305
+    x: 587322.50397662027
+    y: 4141195.1837679865
+    theta: -1.8219912052886467
+    kappa: 0.00033276554305752072
     s: 11.351291561728873
-    dkappa: 1.9690027038638672e-05
+    dkappa: 1.7903438083960216e-05
     ddkappa: 0
   }
   v: 11.701673902382982
@@ -720,12 +720,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587322.20879250462
-    y: 4141194.0479864059
-    theta: -1.8220520942288572
-    kappa: 0.00032379859507984855
+    x: 587322.21252204455
+    y: 4141194.0470251911
+    theta: -1.8215921733293383
+    kappa: 0.00034547799715889654
     s: 12.524803378373962
-    dkappa: 1.2681496806599041e-05
+    dkappa: 1.000585360989842e-05
     ddkappa: 0
   }
   v: 11.768215825766566
@@ -734,12 +734,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587321.9156357703
-    y: 4141192.9049236728
-    theta: -1.8216644518287468
-    kappa: 0.00033132902750620434
+    x: 587321.91990537755
+    y: 4141192.9038240854
+    theta: -1.8211813541129289
+    kappa: 0.00034890767069528314
     s: 13.704859865627691
-    dkappa: 5.636249670221978e-06
+    dkappa: 2.06658103113633e-06
     ddkappa: 0
   }
   v: 11.832513406667617
@@ -748,12 +748,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587321.62133415719
-    y: 4141191.7556457836
-    theta: -1.8212706442518394
-    kappa: 0.00033106974610271523
+    x: 587321.62616914709
+    y: 4141191.754401566
+    theta: -1.820769345897262
+    kappa: 0.000343531890026738
     s: 14.891221164451531
-    dkappa: -1.4446922357929961e-06
+    dkappa: -5.9131704991851421e-06
     ddkappa: 0
   }
   v: 11.894265625042323
@@ -762,12 +762,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587321.32602042239
-    y: 4141190.6003952939
-    theta: -1.8208810215741986
-    kappa: 0.00032172490766133857
+    x: 587321.33144193562
+    y: 4141190.5990013033
+    theta: -1.8203687417974497
+    kappa: 0.00032789090226209317
     s: 16.083619556531087
-    dkappa: -8.5592137969941488e-06
+    dkappa: -1.3931076807012413e-05
     ddkappa: 0
   }
   v: 11.953216315417231
@@ -776,12 +776,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587321.02972246357
-    y: 4141189.4394657183
-    theta: -1.8205055040439417
-    kappa: 0.00030393966696869916
+    x: 587321.035741464
+    y: 4141189.4379193983
+    theta: -1.8199902476491003
+    kappa: 0.00030271308643926956
     s: 17.281763949733136
-    dkappa: -1.5705989529382644e-05
+    dkappa: -2.1985584064415173e-05
     ddkappa: 0
   }
   v: 12.00915416688926
@@ -790,12 +790,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587320.73247541033
-    y: 4141188.2731681652
-    theta: -1.8201516902295853
-    kappa: 0.00028093070124013858
+    x: 587320.739093406
+    y: 4141188.2714693467
+    theta: -1.8196421199896744
+    kappa: 0.0002717918357183313
     s: 18.485344363562678
-    dkappa: -1.9634755250395998e-05
+    dkappa: -2.6223656920134097e-05
     ddkappa: 0
   }
   v: 12.061912723125694
@@ -804,12 +804,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587320.43440975458
-    y: 4141187.1018042923
-    theta: -1.8198269992229366
-    kappa: 0.00025918796640596472
+    x: 587320.441615094
+    y: 4141187.0999561744
+    theta: -1.8193333673001006
+    kappa: 0.00024292277960731804
     s: 19.694036414619937
-    dkappa: -1.7844921180972248e-05
+    dkappa: -2.3681327251872532e-05
     ddkappa: 0
   }
   v: 12.111370382364182
@@ -818,12 +818,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587320.13550968561
-    y: 4141185.9257232221
-    theta: -1.8195244920291564
-    kappa: 0.00023944486490112658
+    x: 587320.14328283665
+    y: 4141185.9237309396
+    theta: -1.8190544478193937
+    kappa: 0.00021688856993877363
     s: 20.90750580205744
-    dkappa: -1.5999752618555446e-05
+    dkappa: -2.1072289349714017e-05
     ddkappa: 0
   }
   v: 12.157450397412749
@@ -832,12 +832,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587319.83585583663
-    y: 4141184.7452550968
-    theta: -1.81924328185218
-    kappa: 0.00022185095362713537
+    x: 587319.84416720807
+    y: 4141184.74312632
+    theta: -1.8188039235429805
+    kappa: 0.0001938993747136068
     s: 22.125412793037029
-    dkappa: -1.4145783543915063e-05
+    dkappa: -1.8451663505470185e-05
     ddkappa: 0
   }
   v: 12.200120875649777
@@ -846,12 +846,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587319.53553448094
-    y: 4141183.5607296056
-    theta: -1.8189824463014044
-    kappa: 0.00020660780269289586
+    x: 587319.5443432
+    y: 4141183.5584748425
+    theta: -1.8185802511810949
+    kappa: 0.00017423871549417731
     s: 23.3474167081869
-    dkappa: -1.2284329270555459e-05
+    dkappa: -1.5820982615384849e-05
     ddkappa: 0
   }
   v: 12.239394779024021
@@ -860,12 +860,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587319.23456587
-    y: 4141182.3724894719
-    theta: -1.8187368610429573
-    kappa: 0.0001934259181694011
+    x: 587319.24383017968
+    y: 4141182.370119404
+    theta: -1.8183764506715416
+    kappa: 0.00015749586549346093
     s: 24.573180442051331
-    dkappa: -1.0414687590852676e-05
+    dkappa: -1.3179757767074107e-05
     ddkappa: 0
   }
   v: 12.275331011342642
@@ -874,12 +874,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587318.93305885792
-    y: 4141181.1808467559
-    theta: -1.8185063200968796
-    kappa: 0.00018267256973876679
+    x: 587318.94272397179
+    y: 4141181.1783753475
+    theta: -1.8181918246710542
+    kappa: 0.00014418813042894489
     s: 25.802374833595721
-    dkappa: -8.5376152631571672e-06
+    dkappa: -1.0528956903417998e-05
     ddkappa: 0
   }
   v: 12.308025183343142
@@ -888,12 +888,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587318.6310485272
-    y: 4141179.9861216089
-    theta: -1.8182867392452664
-    kappa: 0.00017414571885446348
+    x: 587318.64106051985
+    y: 4141179.9835625663
+    theta: -1.8180207877367265
+    kappa: 0.00013402836561193196
     s: 27.034680974740837
-    dkappa: -6.6536654776952772e-06
+    dkappa: -7.869334899544349e-06
     ddkappa: 0
   }
   v: 12.337584481442953
@@ -902,12 +902,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587318.32860147092
-    y: 4141178.7886147606
-    theta: -1.8180755142925755
-    kappa: 0.00016785134617786009
+    x: 587318.33890402806
+    y: 4141178.7859823671
+    theta: -1.8178596115930974
+    kappa: 0.00012702377934485388
     s: 28.269791044258895
-    dkappa: -4.76343365148502e-06
+    dkappa: -5.2016824967650754e-06
     ddkappa: 0
   }
   v: 12.364123456253347
@@ -916,12 +916,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587318.02580689569
-    y: 4141177.5886090305
-    theta: -1.8178705707908145
-    kappa: 0.00016406272516882367
+    x: 587318.03633571044
+    y: 4141177.5859195683
+    theta: -1.8177050868360991
+    kappa: 0.00012355812217267124
     s: 29.507409044192926
-    dkappa: -2.8670283386875603e-06
+    dkappa: -2.5262983223912834e-06
     ddkappa: 0
   }
   v: 12.387764022579447
@@ -930,12 +930,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587317.72269311652
-    y: 4141176.3863896513
-    theta: -1.8176683572558532
-    kappa: 0.00016242620660458416
+    x: 587317.73339337355
+    y: 4141176.3836569749
+    theta: -1.817552512334053
+    kappa: 0.00012313112615358886
     s: 30.74725153627617
-    dkappa: -9.65260342331633e-07
+    dkappa: 1.5583051937148341e-07
     ddkappa: 0
   }
   v: 12.408635459420211
@@ -944,12 +944,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587317.41936157411
-    y: 4141175.1822096966
-    theta: -1.8174662699816855
-    kappa: 0.00016332972182711439
+    x: 587317.43017065176
+    y: 4141175.1794496123
+    theta: -1.8173978207134316
+    kappa: 0.00012628847053781388
     s: 31.989048378351448
-    dkappa: 9.4179123362156261e-07
+    dkappa: 2.8444516465562768e-06
     ddkappa: 0
   }
   v: 12.426874409968448
@@ -958,12 +958,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587317.11585232662
-    y: 4141173.9763232395
-    theta: -1.8172614896686752
-    kappa: 0.00016652056661783184
+    x: 587317.12671708385
+    y: 4141173.9735491406
+    theta: -1.8172372471300426
+    kappa: 0.00013267206800143716
     s: 33.232543460790566
-    dkappa: 2.853558984410988e-06
+    dkappa: 5.5388392338745587e-06
     ddkappa: 0
   }
   v: 12.442624881610813
@@ -972,12 +972,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587316.8122329613
-    y: 4141172.7689621877
-    theta: -1.8170510689504624
-    kappa: 0.00017206373633435991
+    x: 587316.82310347317
+    y: 4141172.7661866406
+    theta: -1.8170665666426089
+    kappa: 0.00014237241061345536
     s: 34.477495442913664
-    dkappa: 4.7696510371236477e-06
+    dkappa: 8.2384511968725816e-06
     ddkappa: 0
   }
   v: 12.4560382459278
@@ -986,12 +986,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587316.50858698227
-    y: 4141171.5603384674
-    theta: -1.8168317567687915
-    kappa: 0.00017954838752379351
+    x: 587316.5194163986
+    y: 4141171.5575732449
+    theta: -1.8168809947399576
+    kappa: 0.00015482847237444986
     s: 35.723678489408655
-    dkappa: 6.0354494045514844e-06
+    dkappa: 1.004323776381588e-05
     ddkappa: 0
   }
   v: 12.467273238693751
@@ -1000,12 +1000,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587316.204938747
-    y: 4141170.3506621271
-    theta: -1.8166038703488385
-    kappa: 0.0001863232597702648
+    x: 587316.21569282643
+    y: 4141170.3479158171
+    theta: -1.8166812222007767
+    kappa: 0.00016638405749881498
     s: 36.970883006750555
-    dkappa: 5.1477895660179416e-06
+    dkappa: 8.8873737961228181e-06
     ddkappa: 0
   }
   v: 12.476495959876852
@@ -1014,12 +1014,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587315.90139986458
-    y: 4141169.1401038114
-    theta: -1.8163678212831553
-    kappa: 0.00019115568428825538
+    x: 587315.91204511933
+    y: 4141169.1373847923
+    theta: -1.8164671430800647
+    kappa: 0.00017535908235753305
     s: 38.218916379620893
-    dkappa: 3.73855647556157e-06
+    dkappa: 7.0139210820986632e-06
     ddkappa: 0
   }
   v: 12.483879873639131
@@ -1028,12 +1028,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587315.59797027789
-    y: 4141167.9288439229
-    theta: -1.8161270081638374
-    kappa: 0.00019436035262276075
+    x: 587315.60848454922
+    y: 4141167.9261577232
+    theta: -1.8162438271959693
+    kappa: 0.00018217089511802796
     s: 39.467603707327115
-    dkappa: 2.3307849840803546e-06
+    dkappa: 5.1416986624811866e-06
     ddkappa: 0
   }
   v: 12.489605808336462
@@ -1042,12 +1042,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587315.29472066
-    y: 4141166.7170261135
-    theta: -1.8158831703957492
-    kappa: 0.00019576645651169612
+    x: 587315.30508493586
+    y: 4141166.7143774587
+    theta: -1.8160132648829974
+    kappa: 0.00018659089742938302
     s: 40.716788540221913
-    dkappa: 9.2452469075655394e-07
+    dkappa: 3.2708136106631439e-06
     ddkappa: 0
   }
   v: 12.493861956518563
@@ -1056,12 +1056,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587314.99168287613
-    y: 4141165.5047839815
-    theta: -1.8156385846019605
-    kappa: 0.00019540142137414784
+    x: 587315.00188297336
+    y: 4141165.50217638
+    theta: -1.8157785175436316
+    kappa: 0.0001886546146210033
     s: 41.966333616122668
-    dkappa: -4.7972133853475831e-07
+    dkappa: 1.4018178935646104e-06
     ddkappa: 0
   }
   v: 12.496843874929
@@ -1070,12 +1070,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587314.68886477523
-    y: 4141164.2922365875
-    theta: -1.8153953480576042
-    kappa: 0.00019338235943744696
+    x: 587314.69889093353
+    y: 4141164.2896724381
+    theta: -1.8155425811251813
+    kappa: 0.0001885166072586448
     s: 43.2161215967308
-    dkappa: -1.8823703499268633e-06
+    dkappa: -4.6566399853163356e-07
     ddkappa: 0
   }
   v: 12.498754484505177
@@ -1084,12 +1084,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587314.38633152121
-    y: 4141163.0794673832
-    theta: -1.8151561386803188
-    kappa: 0.00018945436602255071
+    x: 587314.39617552515
+    y: 4141163.076948687
+    theta: -1.8153085354162861
+    kappa: 0.000185836482226034
     s: 44.466055804051166
-    dkappa: -3.2824597939250723e-06
+    dkappa: -2.3306341060821385e-06
     ddkappa: 0
   }
   v: 12.499804070378348
@@ -1098,12 +1098,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587314.08404188941
-    y: 4141161.8665643157
-    theta: -1.8149222904581608
-    kappa: 0.0001839302456648504
+    x: 587314.0936986875
+    y: 4141161.8640922923
+    theta: -1.8150787313196659
+    kappa: 0.00018102985463851288
     s: 45.716060956811432
-    dkappa: -4.6809678502974337e-06
+    dkappa: -4.1940443947746928e-06
     ddkappa: 0
   }
   v: 12.500210281873606
@@ -1112,12 +1112,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587313.78204770572
-    y: 4141160.6535693118
-    theta: -1.8146971337393385
-    kappa: 0.00017653693670639607
+    x: 587313.79151248606
+    y: 4141160.6511451108
+    theta: -1.8148570824825614
+    kappa: 0.00017373152048553547
     s: 46.966083906881487
-    dkappa: -6.076665692285729e-06
+    dkappa: -6.0546458974162095e-06
     ddkappa: 0
   }
   v: 12.500198132509896
@@ -1126,12 +1126,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587313.48031295312
-    y: 4141159.44052261
-    theta: -1.81448185527111
-    kappa: 0.00016746665309705843
+    x: 587313.48958153767
+    y: 4141159.4381472273
+    theta: -1.8146455295579422
+    kappa: 0.00016419689679065527
     s: 48.216094375692776
-    dkappa: -7.4697231180070711e-06
+    dkappa: -7.9125980361848822e-06
     ddkappa: 0
   }
   v: 12.499999999999998
@@ -1140,12 +1140,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587313.17883498047
-    y: 4141158.2274328796
-    theta: -1.8142789095012855
-    kappa: 0.00015667459241669069
+    x: 587313.18790239538
+    y: 4141158.2251075096
+    theta: -1.8144472487317522
+    kappa: 0.00015236472909325855
     s: 49.466084643924
-    dkappa: -8.8601955598780631e-06
+    dkappa: -9.7679531144130025e-06
     ddkappa: 0
   }
   v: 12.499813698192773
@@ -1154,12 +1154,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587312.87760799215
-    y: 4141157.0142975268
-    theta: -1.8140911825587194
-    kappa: 0.00014407044973554389
+    x: 587312.88646731712
+    y: 4141157.0120238448
+    theta: -1.8142659281572344
+    kappa: 0.00013811449871178072
     s: 50.716058679915626
-    dkappa: -1.0249775432229451e-05
+    dkappa: -1.1622404182669696e-05
     ddkappa: 0
   }
   v: 12.499674344553263
@@ -1168,12 +1168,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587312.57658415672
-    y: 4141155.8011239413
-    theta: -1.8139187347187093
-    kappa: 0.0001298834303713209
+    x: 587312.585227172
+    y: 4141155.7989039519
+    theta: -1.8141020616376731
+    kappa: 0.00012175100622047187
     s: 51.966020880607218
-    dkappa: -1.1637026651841143e-05
+    dkappa: -1.3474516906852245e-05
     ddkappa: 0
   }
   v: 12.499576034729539
@@ -1182,12 +1182,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587312.27578166919
-    y: 4141154.5879037092
-    theta: -1.8137673249364201
-    kappa: 0.00011423452745933827
+    x: 587312.2841944627
+    y: 4141154.5857408233
+    theta: -1.8139626966293878
+    kappa: 0.00010339363376954077
     s: 53.215975068460843
-    dkappa: -1.2483420291538167e-05
+    dkappa: -1.4651946280947747e-05
     ddkappa: 0
   }
   v: 12.499513183523831
@@ -1196,12 +1196,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587311.975138672
-    y: 4141153.3746488206
-    theta: -1.8136332712110705
-    kappa: 0.00010006350193626291
+    x: 587311.9833060978
+    y: 4141153.3725467627
+    theta: -1.8138437283918878
+    kappa: 8.6742489409775378e-05
     s: 54.465924523376515
-    dkappa: -1.0937229623801134e-05
+    dkappa: -1.2843035016505373e-05
     ddkappa: 0
   }
   v: 12.49948052489253
@@ -1210,12 +1210,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587311.67465420114
-    y: 4141152.1613566834
-    theta: -1.8135156155656413
-    kappa: 8.8890476372889646e-05
+    x: 587311.6825553677
+    y: 4141152.159320598
+    theta: -1.8137439873955934
+    kappa: 7.3675802650526137e-05
     s: 55.715872014607562
-    dkappa: -8.6255008475953761e-06
+    dkappa: -1.0079354676308427e-05
     ddkappa: 0
   }
   v: 12.499473111946175
@@ -1224,12 +1224,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587311.374303655
-    y: 4141150.9480310488
-    theta: -1.8134101029951875
-    kappa: 8.0597478282069752e-05
+    x: 587311.38191563939
+    y: 4141150.9460665849
+    theta: -1.8136584033974537
+    kappa: 6.40515650160155e-05
     s: 56.965819832676104
-    dkappa: -6.311766395416794e-06
+    dkappa: -7.3136760209126075e-06
     ddkappa: 0
   }
   v: 12.499486316949476
@@ -1238,12 +1238,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587311.07407096261
-    y: 4141149.7346740104
-    theta: -1.8133128585415981
-    kappa: 7.5076204666863587e-05
+    x: 587311.08136981
+    y: 4141149.7327870713
+    theta: -1.8135823542745662
+    kappa: 5.7740017685135006e-05
     s: 58.215769821288447
-    dkappa: -3.994762175564367e-06
+    dkappa: -4.5447346833324126e-06
     ddkappa: 0
   }
   v: 12.499515831321293
@@ -1252,12 +1252,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587310.77395970025
-    y: 4141148.5212832242
-    theta: -1.8132209614805359
-    kappa: 7.2674379322981448e-05
+    x: 587310.78091720515
+    y: 4141148.5194807611
+    theta: -1.813512313123967
+    kappa: 5.51557884291451e-05
     s: 59.465723409250458
-    dkappa: -1.6760340123185446e-06
+    dkappa: -1.774078146897402e-06
     ddkappa: 0
   }
   v: 12.499557665634649
@@ -1266,12 +1266,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587310.47394870711
-    y: 4141147.3078628569
-    theta: -1.8131303081787171
-    kappa: 7.2921248374521229e-05
+    x: 587310.48054064065
+    y: 4141147.3061508373
+    theta: -1.8134434304405367
+    kappa: 5.5735889022834795e-05
     s: 60.715681642383039
-    dkappa: 6.4572019848496926e-07
+    dkappa: 9.9959729334423457e-07
     ddkappa: 0
   }
   v: 12.499608149616719
@@ -1280,12 +1280,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587310.17405629426
-    y: 4141146.0944076772
-    theta: -1.8130371420402409
-    kappa: 7.6318886103035548e-05
+    x: 587310.18025449011
+    y: 4141146.0927930335
+    theta: -1.8133711618071806
+    kappa: 6.0079781369371893e-05
     s: 61.965645215437512
-    dkappa: 2.9695982065478507e-06
+    dkappa: 3.7753883669771315e-06
     ddkappa: 0
   }
   v: 12.499663932148847
@@ -1294,12 +1294,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587309.874273396
-    y: 4141144.88091955
-    theta: -1.8129383315487448
-    kappa: 8.2475914088821521e-05
+    x: 587309.88005453791
+    y: 4141144.8794080121
+    theta: -1.8132918109597158
+    kappa: 6.7718840027132331e-05
     s: 63.215614504011036
-    dkappa: 5.2963903281864831e-06
+    dkappa: 6.5540872361848441e-06
     ddkappa: 0
   }
   v: 12.499721981266523
@@ -1308,12 +1308,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587309.57461442112
-    y: 4141143.6673948378
-    theta: -1.8128299010024187
-    kappa: 9.1556310141288481e-05
+    x: 587309.5799561009
+    y: 4141143.6659919014
+    theta: -1.8132006145323538
+    kappa: 7.8848826118187463e-05
     s: 64.465589596462024
-    dkappa: 7.6252464875863484e-06
+    dkappa: 9.3348456093477677e-06
     ddkappa: 0
   }
   v: 12.499779584159404
@@ -1322,12 +1322,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587309.27509817691
-    y: 4141142.4538290831
-    theta: -1.8127079160786947
-    kappa: 0.00010363666599629795
+    x: 587309.27998060163
+    y: 4141142.452539586
+    theta: -1.8130928624627274
+    kappa: 9.3560005455540471e-05
     s: 65.7155703258256
-    dkappa: 9.9572332326401269e-06
+    dkappa: 1.2118731520351568e-05
     ddkappa: 0
   }
   v: 12.499834347171303
@@ -1336,12 +1336,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587308.97572531609
-    y: 4141141.2402225463
-    theta: -1.8125704322485587
-    kappa: 0.00011841217575102638
+    x: 587308.98013431032
+    y: 4141141.2390499329
+    theta: -1.8129662737262315
+    kappa: 0.00011148845057028764
     s: 66.9655563017289
-    dkappa: 1.2291316072387282e-05
+    dkappa: 1.4904714488007673e-05
     ddkappa: 0
   }
   v: 12.499884195800188
@@ -1350,12 +1350,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587308.67655134376
-    y: 4141140.0265621608
-    theta: -1.812410215214719
-    kappa: 0.00013642882033548275
+    x: 587308.680475334
+    y: 4141140.0255092056
+    theta: -1.8128121468428611
+    kappa: 0.00013328472773188415
     s: 68.215546942306645
-    dkappa: 1.4628107330977979e-05
+    dkappa: 1.7693410022715997e-05
     ddkappa: 0
   }
   v: 12.499927374698192
@@ -1364,12 +1364,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587308.37756619789
-    y: 4141138.8128512041
-    theta: -1.8122278986614244
-    kappa: 0.00015711445112836814
+    x: 587308.38100094744
+    y: 4141138.8119188733
+    theta: -1.8126313094685027
+    kappa: 0.00015826563716263826
     s: 69.465541506116438
-    dkappa: 1.6967771594952291e-05
+    dkappa: 2.0484986642353642e-05
     ddkappa: 0
   }
   v: 12.499962447671601
@@ -1378,12 +1378,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587308.07883143483
-    y: 4141137.5990754515
-    theta: -1.8120164018163802
-    kappa: 0.00018060708453510065
+    x: 587308.0817788979
+    y: 4141137.5982631561
+    theta: -1.8124152653470671
+    kappa: 0.0001865790640471395
     s: 70.7155391240542
-    dkappa: 1.9001910141539709e-05
+    dkappa: 2.2893029801898233e-05
     ddkappa: 0
   }
   v: 12.49998829768086
@@ -1392,12 +1392,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587307.78036563681
-    y: 4141136.3852313831
-    theta: -1.8117755168765277
-    kappa: 0.00020451988090620146
+    x: 587307.78283468564
+    y: 4141136.3845368223
+    theta: -1.8121638827071569
+    kappa: 0.000215244517673703
     s: 71.965538831269669
-    dkappa: 1.9017536059898481e-05
+    dkappa: 2.2770378235299381e-05
     ddkappa: 0
   }
   v: 12.500004126840574
@@ -1406,12 +1406,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587307.48220036831
-    y: 4141135.1713123675
-    theta: -1.8115061785430042
-    kappa: 0.00022749153224430082
+    x: 587307.48420671606
+    y: 4141135.1707315505
+    theta: -1.8118785289340695
+    kappa: 0.00024255733463896261
     s: 73.215539599081666
-    dkappa: 1.8242299439802529e-05
+    dkappa: 2.1656226196795605e-05
     ddkappa: 0
   }
   v: 12.500009456419505
@@ -1420,12 +1420,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587307.18440201948
-    y: 4141133.9573032875
-    theta: -1.8112070040066495
-    kappa: 0.00024943932464242829
+    x: 587307.18596979254
+    y: 4141133.9568301383
+    theta: -1.811557867951425
+    kappa: 0.000268396225094978
     s: 74.465540366893677
-    dkappa: 1.7472669805167729e-05
+    dkappa: 2.0547686547321664e-05
     ddkappa: 0
   }
   v: 12.500004126840574
@@ -1434,12 +1434,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587306.88694827852
-    y: 4141132.7432108186
-    theta: -1.8108832728525586
-    kappa: 0.00027050835465804017
+    x: 587306.88810361968
+    y: 4141132.7428387916
+    theta: -1.811208429186921
+    kappa: 0.00029296965086721138
     s: 75.715540074109128
-    dkappa: 1.6701393448058403e-05
+    dkappa: 1.9437500120744043e-05
     ddkappa: 0
   }
   v: 12.499988297680861
@@ -1448,12 +1448,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587306.58994709351
-    y: 4141131.5290097157
-    theta: -1.8105313535412346
-    kappa: 0.000290513890298566
+    x: 587306.59072737291
+    y: 4141131.5287294947
+    theta: -1.8108263100048054
+    kappa: 0.0003160140083983895
     s: 76.9655376920469
-    dkappa: 1.5926731871150509e-05
+    dkappa: 1.8323925437910709e-05
     ddkappa: 0
   }
   v: 12.499962447671603
@@ -1462,12 +1462,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587306.29335853143
-    y: 4141130.314710903
-    theta: -1.8101565801348838
-    kappa: 0.00030960979642840486
+    x: 587306.29379991244
+    y: 4141130.3145135082
+    theta: -1.8104180190963126
+    kappa: 0.00033775273303552066
     s: 78.215532255856672
-    dkappa: 1.5148159829021055e-05
+    dkappa: 1.7206434856185753e-05
     ddkappa: 0
   }
   v: 12.499927374698194
@@ -1476,12 +1476,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587305.99724984693
-    y: 4141129.1002990236
-    theta: -1.8097581509463976
-    kappa: 0.00032771216813853551
+    x: 587305.99739577994
+    y: 4141129.100173709
+    theta: -1.8099829880963914
+    kappa: 0.00035806640987148596
     s: 79.465522896434436
-    dkappa: 1.436729208003386e-05
+    dkappa: 1.6086639659847552e-05
     ddkappa: 0
   }
   v: 12.499884195800192
@@ -1490,12 +1490,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587305.70164684241
-    y: 4141127.8857687563
-    theta: -1.8093373610603969
-    kappa: 0.00034481977934427239
+    x: 587305.70154398913
+    y: 4141127.8857040233
+    theta: -1.80952305783175
+    kappa: 0.00037695522243957826
     s: 80.715508872337736
-    dkappa: 1.3583186217502337e-05
+    dkappa: 1.4963596423606858e-05
     ddkappa: 0
   }
   v: 12.499834347171305
@@ -1504,12 +1504,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587305.40653590742
-    y: 4141126.6711242311
-    theta: -1.808897058516874
-    kappa: 0.0003610055619525404
+    x: 587305.40622996434
+    y: 4141126.6711088619
+    theta: -1.8090417584686156
+    kappa: 0.00039452475579642267
     s: 81.965489601701307
-    dkappa: 1.2796401474763708e-05
+    dkappa: 1.3837863735480138e-05
     ddkappa: 0
   }
   v: 12.499779584159407
@@ -1518,12 +1518,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587305.11202637828
-    y: 4141125.4563395455
-    theta: -1.8084353183025563
-    kappa: 0.00037610992929101731
+    x: 587305.11157297064
+    y: 4141125.4563599396
+    theta: -1.8085374420899121
+    kappa: 0.00041054866070238625
     s: 83.2154646941523
-    dkappa: 1.2006518435719993e-05
+    dkappa: 1.2709020859399845e-05
     ddkappa: 0
   }
   v: 12.499721981266527
@@ -1532,12 +1532,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587304.81803986558
-    y: 4141124.2414341536
-    theta: -1.8079568671776394
-    kappa: 0.00039031041150172863
+    x: 587304.817486701
+    y: 4141124.2414786951
+    theta: -1.8080155881081694
+    kappa: 0.0004252819163728519
     s: 84.465433982725813
-    dkappa: 1.1213841113734185e-05
+    dkappa: 1.1577374185777909e-05
     ddkappa: 0
   }
   v: 12.499663932148851
@@ -1546,12 +1546,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587304.52468631533
-    y: 4141123.0263816547
-    theta: -1.8074602229466288
-    kappa: 0.0004034518570231645
+    x: 587304.52408993384
+    y: 4141123.0264366348
+    theta: -1.8074751370099884
+    kappa: 0.00043850458340393653
     s: 85.715397555780285
-    dkappa: 1.0418438323325243e-05
+    dkappa: 1.0442992527240018e-05
     ddkappa: 0
   }
   v: 12.499608149616725
@@ -1560,12 +1560,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587304.23192695761
-    y: 4141121.8111913437
-    theta: -1.8069482512740591
-    kappa: 0.00041562729945125428
+    x: 587304.23133882566
+    y: 4141121.8112443397
+    theta: -1.8069195058657175
+    kappa: 0.00045035108290005497
     s: 86.965355788912859
-    dkappa: 9.6197245614521818e-06
+    dkappa: 9.3052933893904628e-06
     ddkappa: 0
   }
   v: 12.499557665634653
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_qp_path_failure_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_qp_path_failure_0.pb.txt
index fae2bbf002..048e11ac95 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_qp_path_failure_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_qp_path_failure_0.pb.txt
@@ -9,1693 +9,1693 @@ trajectory_point {
     y: 4141003.4874083046
     z: 0
     theta: 2.6134265853950165
-    kappa: -0.041713217257917774
+    kappa: -0.041807219110442544
     s: 0
     dkappa: 0
     ddkappa: 0
   }
   v: 4.8822846412658691
-  a: -0.59284290753125612
+  a: -0.60744348396603676
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 586916.51255610515
-    y: 4141003.5374346697
-    theta: 2.609174734675098
-    kappa: -0.042468232567604292
-    s: 0.0975272299525378
-    dkappa: -0.0015146441382504897
+    x: 586916.51255973789
+    y: 4141003.5374350226
+    theta: 2.60916640679587
+    kappa: -0.042559382760054894
+    s: 0.097524292517214062
+    dkappa: -0.0015089234825800802
     ddkappa: 0
   }
-  v: 4.8704434312728413
-  a: -0.59131956092750015
+  v: 4.8701488252235317
+  a: -0.60617889201368869
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 586916.42903983931
-    y: 4141003.5873397044
-    theta: 2.6049329576796092
-    kappa: -0.043221416714148246
-    s: 0.19481792394799793
-    dkappa: -0.0030256147608987466
+    x: 586916.42905220436
+    y: 4141003.587337357
+    theta: 2.604916580668883
+    kappa: -0.043309676267384987
+    s: 0.194806105484827
+    dkappa: -0.0030140952528739727
     ddkappa: 0
   }
-  v: 4.8586302322874362
-  a: -0.59004019552366183
+  v: 4.8580358870261957
+  a: -0.605154083930278
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 586916.34572618129
-    y: 4141003.6371236714
-    theta: 2.6007013858922945
-    kappa: -0.04397277366177061
-    s: 0.291872594054791
-    dkappa: -0.0045329198206100615
+    x: 586916.34575246484
+    y: 4141003.6371155176
+    theta: 2.6006772438090358
+    kappa: -0.044058102796489446
+    s: 0.29184584914917933
+    dkappa: -0.0045155216583292815
     ddkappa: 0
   }
-  v: 4.8468402613603718
-  a: -0.5889951438482397
+  v: 4.8459411279286213
+  a: -0.60435936650179556
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 586916.26261477149
-    y: 4141003.6867867848
-    theta: 2.5964801526818886
-    kappa: -0.044722306649108513
-    s: 0.38869165861583621
-    dkappa: -0.0060365658144483888
+    x: 586916.26266024681
+    y: 4141003.686769668
+    theta: 2.5964485347612771
+    kappa: -0.044804664801588116
+    s: 0.3886438417203496
+    dkappa: -0.0060132076223824915
     ddkappa: 0
   }
-  v: 4.8350689288917943
-  a: -0.58817473842973234
+  v: 4.83386004304985
+  a: -0.603785046514232
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 586916.17970532831
-    y: 4141003.7363292137
-    theta: 2.5922693931209269
-    kappa: -0.045470018219151892
-    s: 0.48527544611554996
-    dkappa: -0.0075365578439325169
+    x: 586916.17977535236
+    y: 4141003.7362999259
+    theta: 2.5922305936374928
+    kappa: -0.045549364056967925
+    s: 0.48520031324943996
+    dkappa: -0.0075071567044494285
     ddkappa: 0
   }
-  v: 4.8233118386312794
-  a: -0.58756931179663818
+  v: 4.821788321373206
+  a: -0.60342143075357813
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 586916.0979873878
-    y: 4141003.7873653881
-    theta: 2.5877419020956367
-    kappa: -0.0462375771376066
-    s: 0.58162419904683471
-    dkappa: -0.0077850709198892818
+    x: 586916.09808789543
+    y: 4141003.7873213273
+    theta: 2.5876978165137587
+    kappa: -0.046313966282239979
+    s: 0.58151540950586178
+    dkappa: -0.0077562597298462118
     ddkappa: 0
   }
-  v: 4.8115647876778356
-  a: -0.58716919647745591
+  v: 4.8097218457462887
+  a: -0.60325882600582459
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 586916.0166254174
-    y: 4141003.8385328334
-    theta: 2.5831733309596956
-    kappa: -0.047006696684633795
-    s: 0.677738077778067
-    dkappa: -0.0078353387856056767
+    x: 586916.01676302892
+    y: 4141003.8384724613
+    theta: 2.583124172944153
+    kappa: -0.047080123994036563
+    s: 0.6775891958546213
+    dkappa: -0.0078067993722188093
     ddkappa: 0
   }
-  v: 4.7998237664798991
-  a: -0.58696472500068386
+  v: 4.79765669288098
+  a: -0.6032875390569622
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 586915.93546220241
-    y: 4141003.889575284
-    theta: 2.5786157818960338
-    kappa: -0.047773937385570453
-    s: 0.77361716442008654
-    dkappa: -0.0078854838543091152
+    x: 586915.93564243661
+    y: 4141003.889495112
+    theta: 2.5785618785122537
+    kappa: -0.047844357247501121
+    s: 0.77342166113360511
+    dkappa: -0.0078572120675746721
     ddkappa: 0
   }
-  v: 4.788084958835336
-  a: -0.5869462298948207
+  v: 4.7855891333534437
+  a: -0.60349787669298161
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 586915.85449773609
-    y: 4141003.9404927441
-    theta: 2.5740694441838778
-    kappa: -0.048539299302195645
-    s: 0.86926146669318483
-    dkappa: -0.0079355061300373352
+    x: 586915.85472618951
+    y: 4141003.940389235
+    theta: 2.5740111274590083
+    kappa: -0.048606665374260206
+    s: 0.86901272153086606
+    dkappa: -0.0079074977718245013
     ddkappa: 0
   }
-  v: 4.7763447418914433
-  a: -0.587104043688365
+  v: 4.7735156316041216
+  a: -0.60388014569987369
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 586915.77373207186
-    y: 4141003.9912851811
-    theta: 2.5695345076679215
-    kappa: -0.049302781931946975
-    s: 0.96467092179409364
-    dkappa: -0.0079854055799440261
+    x: 586915.77401441673
+    y: 4141003.99115475
+    theta: 2.569472114424133
+    kappa: -0.049367047157501252
+    s: 0.96436222446190856
+    dkappa: -0.00795765640470118
     ddkappa: 0
   }
-  v: 4.7645996861449484
-  a: -0.58742849890981508
+  v: 4.7614328459377369
+  a: -0.604424652863629
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 586915.69403258257
-    y: 4141004.0432936177
-    theta: 2.5647505753623552
-    kappa: -0.050079458368213148
-    s: 1.0598454002629731
-    dkappa: -0.0080324368112669863
+    x: 586915.69437063579
+    y: 4141004.0431263261
+    theta: 2.5646868307066875
+    kappa: -0.050140584529302736
+    s: 1.0594699524469742
+    dkappa: -0.00800496667368157
     ddkappa: 0
   }
-  v: 4.7528465554420078
-  a: -0.58790992808766973
+  v: 4.7493376285232909
+  a: -0.60512170497023832
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586915.61496933457
-    y: 4141004.0958528733
-    theta: 2.5598461861846262
-    kappa: -0.050861851458667368
-    s: 1.1547847098504016
-    dkappa: -0.0080779611944735655
+    x: 586915.61537368118
+    y: 4141004.0956522948
+    theta: 2.5597806193991421
+    kappa: -0.050919912789346641
+    s: 1.1543356269883271
+    dkappa: -0.0080507566648850351
     ddkappa: 0
   }
-  v: 4.7410823069782086
-  a: -0.58853866375042729
+  v: 4.737227025394068
+  a: -0.6059616088056925
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 586915.53610213893
-    y: 4141004.1482817978
-    theta: 2.5549537329650156
-    kappa: -0.05164230445682487
-    s: 1.2494885993843627
-    dkappa: -0.0081233726913238491
+    x: 586915.53657856979
+    y: 4141004.1480440553
+    theta: 2.5548867169141434
+    kappa: -0.05169724980585013
+    s: 1.2489589124475411
+    dkappa: -0.0080964296591452684
     ddkappa: 0
   }
-  v: 4.7293040912985678
-  a: -0.58930503842658633
+  v: 4.725098276447631
+  a: -0.6069346711559821
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586915.4574312506
-    y: 4141004.2005802225
-    theta: 2.5500734665313152
-    kappa: -0.0524208148390695
-    s: 1.3439567626372353
-    dkappa: -0.0081686711549785185
+    x: 586915.4579856256
+    y: 4141004.2003013929
+    theta: 2.5500053786169521
+    kappa: -0.05247259238395701
+    s: 1.3433394199227824
+    dkappa: -0.0081419854687462
     ddkappa: 0
   }
-  v: 4.7175092522975328
-  a: -0.5901993846446455
+  v: 4.7129488154458237
+  a: -0.608031198807098
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586915.37895696727
-    y: 4141004.2527479483
-    theta: 2.5452056368641074
-    kappa: -0.053197379659940647
-    s: 1.4381888421927809
-    dkappa: -0.00821385641405278
+    x: 586915.37959521345
+    y: 4141004.2524240641
+    theta: 2.54513685882544
+    kappa: -0.053245936923100576
+    s: 1.4374767111260982
+    dkappa: -0.0081874238821339519
     ddkappa: 0
   }
-  v: 4.7056953272189794
-  a: -0.59121203493310315
+  v: 4.7007762700147682
+  a: -0.60924149854503074
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 586915.30123773345
-    y: 4141004.3056007139
-    theta: 2.5401965873708696
-    kappa: -0.053978976787382366
-    s: 1.5321844333131334
-    dkappa: -0.0082550044231427585
+    x: 586915.30195436347
+    y: 4141004.3052107934
+    theta: 2.540131304424099
+    kappa: -0.054024169218761783
+    s: 1.5313703022607001
+    dkappa: -0.0082289104808604147
     ddkappa: 0
   }
-  v: 4.6938600466562157
-  a: -0.592333321820458
+  v: 4.68857846164487
+  a: -0.61055587715577131
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 586915.22455581231
-    y: 4141004.359550396
-    theta: 2.5349679021216946
-    kappa: -0.054769128613219011
-    s: 1.6259430878057868
-    dkappa: -0.0082901330977340169
+    x: 586915.22536834213
+    y: 4141004.3591068517
+    theta: 2.534904335681698
+    kappa: -0.054811127062267512
+    s: 1.6250196678982511
+    dkappa: -0.00826430686274274
     ddkappa: 0
   }
-  v: 4.6820013345519786
-  a: -0.59355357783520835
+  v: 4.6763534056908123
+  a: -0.61196464142531026
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 586915.14806807227
-    y: 4141004.4133634628
-    theta: 2.5297520995023874
-    kappa: -0.055557279542921
-    s: 1.7194643178905842
-    dkappa: -0.0083251728162197433
+    x: 586915.14898250788
+    y: 4141004.4128620303
+    theta: 2.5296906688772838
+    kappa: -0.055596027888406993
+    s: 1.7184242448561504
+    dkappa: -0.008299610722550553
     ddkappa: 0
   }
-  v: 4.6701173081984351
-  a: -0.5948631355058529
+  v: 4.664099311371559
+  a: -0.61345809813963836
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586915.07177494164
-    y: 4141004.4670396112
-    theta: 2.5245494941168585
-    kappa: -0.056343425164680883
-    s: 1.8127476000667062
-    dkappa: -0.0083601233824592176
+    x: 586915.072797349
+    y: 4141004.4664759887
+    theta: 2.5244906224728987
+    kappa: -0.056378866679946918
+    s: 1.8115834360748198
+    dkappa: -0.00833482183461496
     ddkappa: 0
   }
-  v: 4.6582062782371816
-  a: -0.59625232736089018
+  v: 4.6518145817703545
+  a: -0.61502655408474627
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586914.99567687442
-    y: 4141004.5205785227
-    theta: 2.5193603980486636
-    kappa: -0.057127560798244173
-    s: 1.9057923789796605
-    dkappa: -0.0083949845883770653
+    x: 586914.99681337806
+    y: 4141004.5199483638
+    theta: 2.5193045121746214
+    kappa: -0.057159638167559423
+    s: 1.9044966144949877
+    dkappa: -0.00836993996192817
     ddkappa: 0
   }
-  v: 4.6462667486592464
-  a: -0.59771148592881873
+  v: 4.6394978138347236
+  a: -0.616660316046625
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586914.91987405438
-    y: 4141004.5741174016
-    theta: 2.5141601010231382
-    kappa: -0.057910425957557
-    s: 1.9985980712882683
-    dkappa: -0.0084290039618571712
+    x: 586914.92110714992
+    y: 4141004.5733836172
+    theta: 2.5141136457937292
+    kappa: -0.057938911689852247
+    s: 1.9971631269349768
+    dkappa: -0.0084043923396980788
     ddkappa: 0
   }
-  v: 4.6342974168050866
-  a: -0.599230943738137
+  v: 4.6271477983764715
+  a: -0.6183496908112649
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 586914.8457057873
-    y: 4141004.6295027076
-    theta: 2.5086120905154941
-    kappa: -0.058702011341454974
-    s: 2.0911640695316551
-    dkappa: -0.0084520804072609236
+    x: 586914.847064325
+    y: 4141004.6286915122
+    theta: 2.5085712034863756
+    kappa: -0.058727155067759579
+    s: 2.089582297967989
+    dkappa: -0.0084277472962677215
     ddkappa: 0
   }
-  v: 4.622297173364589
-  a: -0.60080103331734347
+  v: 4.6147635200716817
+  a: -0.62008498516465693
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586914.77173007745
-    y: 4141004.6847442221
-    theta: 2.50307793113975
-    kappa: -0.059491541594839104
-    s: 2.1834897459962384
-    dkappa: -0.0084750969411149374
+    x: 586914.77322021674
+    y: 4141004.6838509715
+    theta: 2.5030430783680679
+    kappa: -0.059513282952712646
+    s: 2.1817534337993889
+    dkappa: -0.0084510395726456172
     ddkappa: 0
   }
-  v: 4.61026510237707
-  a: -0.60241208719493688
+  v: 4.6023441574607205
+  a: -0.62185650589279173
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586914.6979474409
-    y: 4141004.7398415585
-    theta: 2.4975580034299121
-    kappa: -0.060279011209643796
-    s: 2.2755744565827167
-    dkappa: -0.0084980534028470528
+    x: 586914.69957539253
+    y: 4141004.7388615711
+    theta: 2.4975296538816059
+    kappa: -0.060297289303745075
+    s: 2.2736758261439936
+    dkappa: -0.0084742689898432561
     ddkappa: 0
   }
-  v: 4.5982004812312782
-  a: -0.6040544378994156
+  v: 4.5898890829482317
+  a: -0.62365455978166018
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586914.62435840373
-    y: 4141004.7947943239
-    theta: 2.4920526834556855
-    kappa: -0.061064414570748481
-    s: 2.3674175446730557
-    dkappa: -0.0085209496287642211
+    x: 586914.6261304284
+    y: 4141004.793722881
+    theta: 2.4920313087323422
+    kappa: -0.061079167989369909
+    s: 2.3653487561033533
+    dkappa: -0.0084974353661900846
     ddkappa: 0
   }
-  v: 4.5861027806653913
-  a: -0.60571841795927817
+  v: 4.5773978628031422
+  a: -0.62546945361725281
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586914.550963499
-    y: 4141004.8496021209
-    theta: 2.4865623424779941
-    kappa: -0.06184774598904659
-    s: 2.4590183449974825
-    dkappa: -0.0085437854530165426
+    x: 586914.55288590549
+    y: 4141004.8484344669
+    theta: 2.4865484165436418
+    kappa: -0.061858912820649
+    s: 2.4567714980430422
+    dkappa: -0.00852053851831332
     ddkappa: 0
   }
-  v: 4.573971664767015
-  a: -0.60739435990302315
+  v: 4.564870257158657
+  a: -0.62729149418556052
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586914.47891510732
-    y: 4141004.9057575753
-    theta: 2.4808270497323028
-    kappa: -0.06263139610826321
-    s: 2.5503761875014672
-    dkappa: -0.00855639475617239
+    x: 586914.48094855587
+    y: 4141004.9044292509
+    theta: 2.4808321792743322
+    kappa: -0.062638917788351636
+    s: 2.5479433234699393
+    dkappa: -0.00853382711964271
     ddkappa: 0
   }
-  v: 4.5618069909731878
-  a: -0.60907259625914911
+  v: 4.5523062200122615
+  a: -0.629110988272574
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586914.40765636775
-    y: 4141004.9625377832
-    theta: 2.4749715293966479
-    kappa: -0.063414199540921784
-    s: 2.6414904012127165
-    dkappa: -0.0085636968634525985
+    x: 586914.40985036374
+    y: 4141004.9610995818
+    theta: 2.4749869424294335
+    kappa: -0.063418159887846548
+    s: 2.638863504909517
+    dkappa: -0.0085414318690859459
     ddkappa: 0
   }
-  v: 4.5496088100703762
-  a: -0.6107434595561545
+  v: 4.5397058992257229
+  a: -0.63091824266428387
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586914.33658868819
-    y: 4141005.0191657506
-    theta: 2.4691312803427419
-    kappa: -0.064194904109108941
-    s: 2.7323603181081619
-    dkappa: -0.00857097939221169
+    x: 586914.33894951851
+    y: 4141005.0176126137
+    theta: 2.4691574915083856
+    kappa: -0.064195239050031022
+    s: 2.7295313197831268
+    dkappa: -0.0085490155100748054
     ddkappa: 0
   }
-  v: 4.5373773661944785
-  a: -0.612397282322538
+  v: 4.527069636525086
+  a: -0.63270356414668094
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 586914.26571258565
-    y: 4141005.0756410663
-    theta: 2.4633067452871575
-    kappa: -0.064973504132097418
-    s: 2.8229852769809445
-    dkappa: -0.0085782422894589913
+    x: 586914.268246578
+    y: 4141005.0739679015
+    theta: 2.4633442705767
+    kappa: -0.064970149157151241
+    s: 2.8199460542852819
+    dkappa: -0.0085565779829051367
     ddkappa: 0
   }
-  v: 4.5251130968308226
-  a: -0.6140243970867979
+  v: 4.5143979675006776
+  a: -0.63445725950575593
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 586914.1950285692
-    y: 4141005.1319633243
-    theta: 2.4574983598464382
-    kappa: -0.065749994020944169
-    s: 2.9133646273074083
-    dkappa: -0.00858548550306001
+    x: 586914.19774209114
+    y: 4141005.1301650079
+    theta: 2.4575477162877406
+    kappa: -0.0657428841998756
+    s: 2.9101070072609456
+    dkappa: -0.0085641192289309057
     ddkappa: 0
   }
-  v: 4.5128166328141646
-  a: -0.61561513637743293
+  v: 4.5016916216071046
+  a: -0.63616963552749972
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 586914.12486876477
-    y: 4141005.1885344116
-    theta: 2.4516393733158162
-    kappa: -0.066523157271466349
-    s: 3.0034977331140857
-    dkappa: -0.0085890178416955943
+    x: 586914.12769506953
+    y: 4141005.1865169494
+    theta: 2.4517160497468948
+    kappa: -0.066512516483883372
+    s: 3.0000134940828165
+    dkappa: -0.0085687642964890134
     ddkappa: 0
   }
-  v: 4.5004887983286928
-  a: -0.6171598327229415
+  v: 4.4889515221632532
+  a: -0.63783099899790274
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586914.05647159321
-    y: 4141005.2468556231
-    theta: 2.4454780695191443
-    kappa: -0.067288467886842215
-    s: 3.0933839768446885
-    dkappa: -0.008575060601173138
+    x: 586914.059486178
+    y: 4141005.2446969533
+    theta: 2.4455696110403085
+    kappa: -0.0672741181953368
+    s: 3.0896648505286142
+    dkappa: -0.0085551625450265983
     ddkappa: 0
   }
-  v: 4.4881306109080246
-  a: -0.61864881865182231
+  v: 4.4761787863522908
+  a: -0.639431656702956
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 586913.98826271947
-    y: 4141005.3050162764
-    theta: 2.4393329852824333
-    kappa: -0.068051671597673322
-    s: 3.1830227632270929
-    dkappa: -0.00856114178501446
+    x: 586913.99147188256
+    y: 4141005.3027109723
+    theta: 2.4394399497015176
+    kappa: -0.068033547099616246
+    s: 3.1790604366583626
+    dkappa: -0.0085415995986019254
     ddkappa: 0
   }
-  v: 4.4757432814352072
-  a: -0.62007242669257367
+  v: 4.463374725221664
+  a: -0.64096191542865011
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586913.92024257663
-    y: 4141005.3630160009
-    theta: 2.4332046278608375
-    kappa: -0.06881276355834258
-    s: 3.2724135231403326
-    dkappa: -0.0085472614815907955
+    x: 586913.92365264881
+    y: 4141005.3605586104
+    theta: 2.4333275727306547
+    kappa: -0.068790797999555675
+    s: 3.2681996406916802
+    dkappa: -0.0085280755500332719
     ddkappa: 0
   }
-  v: 4.4633282141427184
-  a: -0.62142098937369428
+  v: 4.4505408436831
+  a: -0.64241208196097577
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586913.85241157492
-    y: 4141005.4208544488
-    theta: 2.427093494894617
-    kappa: -0.069571739178813777
-    s: 3.3615557174815835
-    dkappa: -0.0085334197746122567
+    x: 586913.85602891771
+    y: 4141005.4182394911
+    theta: 2.4272329771622667
+    kappa: -0.069545865970147086
+    s: 3.3570818828850597
+    dkappa: -0.0085145904872783382
     ddkappa: 0
   }
-  v: 4.4508870066124668
-  a: -0.62268483922368256
+  v: 4.4376788405126071
+  a: -0.64377246308592373
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 586913.78477009863
-    y: 4141005.4785312908
-    theta: 2.4210000739615012
-    kappa: -0.070328594157555921
-    s: 3.4504488410331553
-    dkappa: -0.00851961674252739
+    x: 586913.78860110312
+    y: 4141005.4757532608
+    theta: 2.4211566496208596
+    kappa: -0.0702987463914787
+    s: 3.4457066194091595
+    dkappa: -0.0085011444928459919
     ddkappa: 0
   }
-  v: 4.438421449775789
-  a: -0.6238543087710372
+  v: 4.4247906083504729
+  a: -0.64503336558948476
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 586913.71853780071
-    y: 4141005.5374275865
-    theta: 2.4147039178182097
-    kappa: -0.071071869797827217
-    s: 3.5390924263294776
-    dkappa: -0.0084911820432877951
+    x: 586913.72247628414
+    y: 4141005.534352968
+    theta: 2.4148990923212108
+    kappa: -0.071039128348411723
+    s: 3.5340733462260845
+    dkappa: -0.00847442671167249
     ddkappa: 0
   }
-  v: 4.4259335279134531
-  a: -0.62491973054425654
+  v: 4.4118782337012634
+  a: -0.64618509625764953
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 586913.653278152
-    y: 4141005.5970481406
-    theta: 2.408282491210227
-    kappa: -0.071805666503089366
-    s: 3.62748604752409
-    dkappa: -0.0084533719018612449
+    x: 586913.657437291
+    y: 4141005.5937919645
+    theta: 2.4084980027424496
+    kappa: -0.071769033689966871
+    s: 3.6221816029666747
+    dkappa: -0.00843705346577851
     ddkappa: 0
   }
-  v: 4.4134254186556561
-  a: -0.62587143707183923
+  v: 4.3989439969338289
+  a: -0.6472179618764089
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 586913.58820332878
-    y: 4141005.6564998408
-    theta: 2.4018786154301508
-    kappa: -0.072537384982129249
-    s: 3.7156293242566316
-    dkappa: -0.0084156688446268384
+    x: 586913.59258939791
+    y: 4141005.6530563151
+    theta: 2.4021150582772077
+    kappa: -0.072496794396976344
+    s: 3.7100309768077895
+    dkappa: -0.0083997900313212519
     ddkappa: 0
   }
-  v: 4.4008994929820267
-  a: -0.62669976088228374
+  v: 4.385990372281297
+  a: -0.64812226923175353
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 586913.52331357531
-    y: 4141005.7157824626
-    theta: 2.3954928428812932
-    kappa: -0.073267022487100986
-    s: 3.8035219255198269
-    dkappa: -0.0083780730131720743
+    x: 586913.52793287137
+    y: 4141005.7121457756
+    theta: 2.3957508087816386
+    kappa: -0.073222407475536072
+    s: 3.7976211063495944
+    dkappa: -0.0083626365615971886
     ddkappa: 0
   }
-  v: 4.3883583152216223
-  a: -0.62739503450408862
+  v: 4.3730200278410756
+  a: -0.64888832510967409
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 586913.45860909682
-    y: 4141005.7748958194
-    theta: 2.3891257132173291
-    kappa: -0.073994576711963342
-    s: 3.8911635735264749
-    dkappa: -0.0083405845263197081
+    x: 586913.46346793766
+    y: 4141005.7710601408
+    theta: 2.3894057909809225
+    kappa: -0.073945870389862445
+    s: 3.8849516854928443
+    dkappa: -0.0083255931864457088
     ddkappa: 0
   }
-  v: 4.3758046430529314
-  a: -0.62794759046575233
+  v: 4.3600358255748528
+  a: -0.64950643629616134
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 586913.39409005607
-    y: 4141005.8338397616
-    theta: 2.3827777528563523
-    kappa: -0.074720045824581527
-    s: 3.9785540475764414
-    dkappa: -0.0083032034784736447
+    x: 586913.39919477876
+    y: 4141005.8297992428
+    theta: 2.3830805279889389
+    kappa: -0.074667181094412477
+    s: 3.9720224673161737
+    dkappa: -0.0082886600106044771
     ddkappa: 0
   }
-  v: 4.36324142750387
-  a: -0.62834776129577363
+  v: 4.3470408213085987
+  a: -0.649966909577206
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 586913.33194560371
-    y: 4141005.8949200595
-    theta: 2.3761039675390574
-    kappa: -0.0754105575112419
-    s: 4.0656931879236433
-    dkappa: -0.0082353701992875315
+    x: 586913.3371233939
+    y: 4141005.8904793924
+    theta: 2.376459088915031
+    kappa: -0.0753562964977251
+    s: 4.0588332679533767
+    dkappa: -0.0082237781360529338
     ddkappa: 0
   }
-  v: 4.3506718129517878
-  a: -0.62858587952265077
+  v: 4.3340382647325608
+  a: -0.650260051738799
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 586913.27008713456
-    y: 4141005.955936485
-    theta: 2.3694315400527635
-    kappa: -0.0760974749835018
-    s: 4.1525808996430387
-    dkappa: -0.0081662434404730349
+    x: 586913.2755150015
+    y: 4141005.9512694422
+    theta: 2.3698128907069496
+    kappa: -0.076039206609149534
+    s: 4.1453839704706938
+    dkappa: -0.0081552233167377728
     ddkappa: 0
   }
-  v: 4.3380991371234616
-  a: -0.62865227767488252
+  v: 4.32103159940127
+  a: -0.65037616956693078
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 586913.20840768528
-    y: 4141006.0167763289
-    theta: 2.3627778862995363
-    kappa: -0.076782404502152277
-    s: 4.2392171564976158
-    dkappa: -0.0080973167359521675
+    x: 586913.21409178455
+    y: 4141006.0118767754
+    theta: 2.363186088552458
+    kappa: -0.0767200641071413
+    s: 4.2316745287441027
+    dkappa: -0.008086874551690439
     ddkappa: 0
   }
-  v: 4.3255269310951
-  a: -0.62853728828096722
+  v: 4.3080244627335338
+  a: -0.65030556984759214
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 586913.14690722269
-    y: 4141006.0774396216
-    theta: 2.3561435939074742
-    kappa: -0.077465346433374521
-    s: 4.32560200480538
-    dkappa: -0.0080285900488749114
+    x: 586913.15285372292
+    y: 4141006.0723014129
+    theta: 2.3565792648442967
+    kappa: -0.0773988692170707
+    s: 4.3177049713365987
+    dkappa: -0.0080187318182868388
     ddkappa: 0
   }
-  v: 4.312958919292341
-  a: -0.62823124386940343
+  v: 4.2950206860124425
+  a: -0.6500385593667739
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 586913.08558565937
-    y: 4141006.1379264509
-    theta: 2.3495292343836138
-    kappa: -0.078146301747527389
-    s: 4.411735567306347
-    dkappa: -0.007960063281590872
+    x: 586913.09180074011
+    y: 4141006.1325434307
+    theta: 2.349992985447888
+    kappa: -0.078075622784203158
+    s: 4.40347540537548
+    dkappa: -0.0079507950316739066
     ddkappa: 0
   }
-  v: 4.3003990194902517
-  a: -0.62772447696868983
+  v: 4.2820242943853657
+  a: -0.64956544491046686
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 586913.02492968366
-    y: 4141006.198711887
-    theta: 2.3428691734986598
-    kappa: -0.078815222318803155
-    s: 4.4976180470295253
-    dkappa: -0.0078855990886594375
+    x: 586913.03118381475
+    y: 4141006.1928478535
+    theta: 2.343393745778763
+    kappa: -0.078745158307904309
+    s: 4.4889860204296372
+    dkappa: -0.0078798974098417841
     ddkappa: 0
   }
-  v: 4.2878513428133305
-  a: -0.62700732010732485
+  v: 4.2690395068639537
+  a: -0.64887653326466155
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 586912.96632258035
-    y: 4141006.2611457943
-    theta: 2.3359737321733558
-    kappa: -0.079443550923732537
-    s: 4.5832497311599072
-    dkappa: -0.0077877564894053234
+    x: 586912.97284725471
+    y: 4141006.2550136885
+    theta: 2.3365300143307781
+    kappa: -0.079369517462412886
+    s: 4.5742370923868343
+    dkappa: -0.00778277886181355
     ddkappa: 0
   }
-  v: 4.2753201937355065
-  a: -0.626070105813807
+  v: 4.2560707363241361
+  a: -0.64796213121534885
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586912.676778792
-    y: 4141006.5714206663
-    theta: 2.3016886096720763
-    kappa: -0.0825339268376445
-    s: 5.0076626475541319
-    dkappa: -0.0072897497837078606
+    x: 586912.68441128731
+    y: 4141006.5635505263
+    theta: 2.3024512696674586
+    kappa: -0.082447733684669691
+    s: 4.99661549264952
+    dkappa: -0.00729323427750464
     ddkappa: 0
   }
-  v: 4.2130765533804126
-  a: -0.6177448113613736
+  v: 4.1916351595061307
+  a: -0.63966850242584927
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586912.40329683048
-    y: 4141006.8878592374
-    theta: 2.2665516508556158
-    kappa: -0.085275706772130491
-    s: 5.4259039087985164
-    dkappa: -0.0066320757287864438
+    x: 586912.41245093709
+    y: 4141006.8783254805
+    theta: 2.2675109079545113
+    kappa: -0.085169674762206285
+    s: 5.4126031172564382
+    dkappa: -0.00664042658790427
     ddkappa: 0
   }
-  v: 4.1520029500141993
-  a: -0.6024679603798988
+  v: 4.1283734105786021
+  a: -0.62428363144727339
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586912.14570318512
-    y: 4141007.2096376996
-    theta: 2.2308185223417447
-    kappa: -0.087619232220534221
-    s: 5.8381272904311565
-    dkappa: -0.00583417759013396
+    x: 586912.15600410081
+    y: 4141007.1978443214
+    theta: 2.2320422699178226
+    kappa: -0.087507980203248445
+    s: 5.8223548246271672
+    dkappa: -0.005854065707661801
     ddkappa: 0
   }
-  v: 4.0928549609866565
-  a: -0.57903111893169878
+  v: 4.0670551963480168
+  a: -0.60059586652846264
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 586911.902782077
-    y: 4141007.5353891933
-    theta: 2.1947687312552113
-    kappa: -0.089564011892708462
-    s: 6.2445681678948173
-    dkappa: -0.0049283680062404533
+    x: 586911.91415688372
+    y: 4141007.5210387353
+    theta: 2.1962824572598496
+    kappa: -0.089455228154534647
+    s: 6.2261085021206872
+    dkappa: -0.0049613499593355176
     ddkappa: 0
   }
-  v: 4.03650900704134
-  a: -0.54622585307908988
+  v: 4.0085713887959553
+  a: -0.56739355591825835
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586911.67429545627
-    y: 4141007.8647873113
-    theta: 2.1585464701526429
-    kappa: -0.09109128342602768
-    s: 6.6455556008763059
-    dkappa: -0.0039236262756654327
+    x: 586911.68661619117
+    y: 4141007.8475693129
+    theta: 2.1603713138532603
+    kappa: -0.0909947425325781
+    s: 6.6241971825528942
+    dkappa: -0.0039719596566590906
     ddkappa: 0
   }
-  v: 3.9839623523155749
-  a: -0.50284372888438877
+  v: 3.9539340250791168
+  a: -0.52346504786550219
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586911.45983853028
-    y: 4141008.1975788
-    theta: 2.1222738319183638
-    kappa: -0.092193172522830963
-    s: 7.04152441764585
-    dkappa: -0.0028380743740494584
+    x: 586911.47296292894
+    y: 4141008.1772039332
+    theta: 2.124423005822345
+    kappa: -0.092118944228405733
+    s: 7.0170611607141069
+    dkappa: -0.0029002167998496446
     ddkappa: 0
   }
-  v: 3.9363331043404548
-  a: -0.44767631240991157
+  v: 3.9042763075293125
+  a: -0.46759869061903536
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 586911.2600609581
-    y: 4141008.5342736589
-    theta: 2.0860400995035611
-    kappa: -0.092824405929889039
-    s: 7.4330272993964766
-    dkappa: -0.0017051182058761957
+    x: 586911.27489935129
+    y: 4141008.5110740322
+    theta: 2.0885090853255472
+    kappa: -0.092742135809732618
+    s: 7.4052601098865836
+    dkappa: -0.0017775736343256872
     ddkappa: 0
   }
-  v: 3.894860214040841
-  a: -0.37951516971797467
+  v: 3.8608526036534716
+  a: -0.39858283242769932
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586911.076634217
-    y: 4141008.8758236822
-    theta: 2.0500051447282637
-    kappa: -0.092910210262503429
-    s: 7.820746864583386
-    dkappa: -0.00052943783832990019
+    x: 586911.09202111862
+    y: 4141008.8489241078
+    theta: 2.05282675870473
+    kappa: -0.0928669277905386
+    s: 7.78948519836203
+    dkappa: -0.00061734688680372936
     ddkappa: 0
   }
-  v: 3.8609034757353644
-  a: -0.2971518668708944
+  v: 3.82503844613364
+  a: -0.31520582154033544
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586910.90586669743
-    y: 4141009.2205064939
-    theta: 2.0143125710983583
-    kappa: -0.092585614910187
-    s: 8.2055077532633316
-    dkappa: 0.00062962883484296627
+    x: 586910.921571984
+    y: 4141009.1896547731
+    theta: 2.0174806019102873
+    kappa: -0.092591512391943909
+    s: 8.1705712059591118
+    dkappa: 0.00053168647526961779
     ddkappa: 0
   }
-  v: 3.8359435271364228
-  a: -0.19937796993098728
+  v: 3.7983305328269772
+  a: -0.21625600620578528
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586910.74714701565
-    y: 4141009.5687309979
-    theta: 1.9789840389719522
-    kappa: -0.091869753349679811
-    s: 8.5882887114339947
-    dkappa: 0.0017600139105033803
+    x: 586910.76289267826
+    y: 4141009.5336894961
+    theta: 1.9824833006144336
+    kappa: -0.091934159061188958
+    s: 8.5495086405409726
+    dkappa: 0.0016546433513079583
     ddkappa: 0
   }
-  v: 3.8215818493501912
-  a: -0.084985044960562528
+  v: 3.7823467267657711
+  a: -0.10052173467287667
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586910.5999617005
-    y: 4141009.9211544367
-    theta: 1.9440195147658248
-    kappa: -0.090776600581990052
-    s: 8.9702227771493668
-    dkappa: 0.002853930242414481
+    x: 586910.61682127661
+    y: 4141009.8822553689
+    theta: 1.9478828720573758
+    kappa: -0.090864140919093422
+    s: 8.9274438499691442
+    dkappa: 0.0027393779629807198
     ddkappa: 0
   }
-  v: 3.81906412269577
-  a: 0.032900255492753022
+  v: 3.7783451576385589
+  a: 0.018745890464724968
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 586910.46825703175
-    y: 4141010.2800031947
-    theta: 1.9096697550734534
-    kappa: -0.089201769909247788
-    s: 9.3524773400413022
-    dkappa: 0.00384490541778332
+    x: 586910.4851954045
+    y: 4141010.236687893
+    theta: 1.9138685715283579
+    kappa: -0.089346489146456168
+    s: 9.3055579428607427
+    dkappa: 0.0037279156654206284
     ddkappa: 0
   }
-  v: 3.8278216853142149
-  a: 0.14057910191026735
+  v: 3.7857528872098882
+  a: 0.12772298613749883
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586910.347762714
-    y: 4141010.6441621585
-    theta: 1.875852245836211
-    kappa: -0.0873062283815553
-    s: 9.7361294803810452
-    dkappa: 0.004751147086020824
+    x: 586910.36402313772
+    y: 4141010.5961045022
+    theta: 1.8803333560877218
+    kappa: -0.087525118669114887
+    s: 9.684940983906694
+    dkappa: 0.004638545052332424
     ddkappa: 0
   }
-  v: 3.8468514002259986
-  a: 0.23840166277303479
+  v: 3.8035584865851417
+  a: 0.22676203338276252
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586910.23772938736
-    y: 4141011.0140874148
-    theta: 1.8425431630915308
-    kappa: -0.085126124782664472
-    s: 10.12215771558429
-    dkappa: 0.0055584671469266567
+    x: 586910.253052599
+    y: 4141010.961183853
+    theta: 1.8472700769040968
+    kappa: -0.08541810168233932
+    s: 10.066583657313647
+    dkappa: 0.0054587262289490564
     ddkappa: 0
   }
-  v: 3.8751851472996988
-  a: 0.32671810656210964
+  v: 3.8307857749734344
+  a: 0.31621551323783353
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 586910.13811119436
-    y: 4141011.3903698456
-    theta: 1.8097658325652677
-    kappa: -0.082681426589859428
-    s: 10.511445501895997
-    dkappa: 0.0062569490628379149
+    x: 586910.15233297844
+    y: 4141011.332565587
+    theta: 1.814706916501563
+    kappa: -0.083041352461107013
+    s: 10.451380791614351
+    dkappa: 0.006168122534225759
     ddkappa: 0
   }
-  v: 3.9118898232519994
-  a: 0.4058786017585464
+  v: 3.8664938196876126
+  a: 0.39643590674002949
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 586910.051297772
-    y: 4141011.7740092375
-    theta: 1.7777740128784205
-    kappa: -0.079961525675485368
-    s: 10.904784736075197
-    dkappa: 0.0068393543255306187
+    x: 586910.06630529254
+    y: 4141011.7116774796
+    theta: 1.7830920551406606
+    kappa: -0.080356677312817509
+    s: 10.840134884478026
+    dkappa: 0.0067444582803516794
     ddkappa: 0
   }
-  v: 3.9560673416476888
-  a: 0.47623331684339931
+  v: 3.9097769361442554
+  a: 0.46777569492666787
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 586909.97655959963
-    y: 4141012.1649914049
-    theta: 1.7466060030475954
-    kappa: -0.077031022180692243
-    s: 11.302879257079802
-    dkappa: 0.00727073160457453
+    x: 586909.989952544
+    y: 4141012.0975603564
+    theta: 1.7520839085466617
+    kappa: -0.077494182633107839
+    s: 11.233559627520735
+    dkappa: 0.007199172559702479
     ddkappa: 0
   }
-  v: 4.0068546328996613
-  a: 0.53813242029772268
+  v: 3.9597646878636716
+  a: 0.53058735883506647
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 586909.912483192
-    y: 4141012.5632660184
-    theta: 1.7161879132387434
-    kappa: -0.07394589980242687
-    s: 11.70634834775143
-    dkappa: 0.0076042513918809888
+    x: 586909.92384774215
+    y: 4141012.4906894639
+    theta: 1.7217643174481772
+    kappa: -0.07447117606598018
+    s: 11.632283431115763
+    dkappa: 0.0075501214461221391
     ddkappa: 0
   }
-  v: 4.0634236442689176
-  a: 0.59192608060257079
+  v: 4.015621886469904
+  a: 0.58522337950254233
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 586909.85909088736
-    y: 4141012.9690896776
-    theta: 1.6865559401570289
-    kappa: -0.070732704890500486
-    s: 12.115730236500191
-    dkappa: 0.0078363889888104828
+    x: 586909.86834334617
+    y: 4141012.8913920522
+    theta: 1.692203116129372
+    kappa: -0.071310640458915817
+    s: 12.036852949203977
+    dkappa: 0.0078053140480815481
     ddkappa: 0
   }
-  v: 4.1249813398645614
-  a: 0.637964466238998
+  v: 4.0765485916907274
+  a: 0.63203623796641373
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 586909.816273864
-    y: 4141013.3825992341
-    theta: 1.6577401365097653
-    kappa: -0.067421348097919581
-    s: 12.531485598989519
-    dkappa: 0.00796317339250129
+    x: 586909.82342835
+    y: 4141013.2998134289
+    theta: 1.6634443805501251
+    kappa: -0.068043753340000471
+    s: 12.447736604104218
+    dkappa: 0.00794872914458209
     ddkappa: 0
   }
-  v: 4.1907697006438047
-  a: 0.676597745688059
+  v: 4.1417801113576447
+  a: 0.67137841526399789
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 586909.78421008715
-    y: 4141013.8038963135
-    theta: 1.6298041187444483
-    kappa: -0.064039523547156654
-    s: 12.954001059820975
-    dkappa: 0.0080039903756408022
+    x: 586909.79145108769
+    y: 4141013.7161754067
+    theta: 1.6358268800759588
+    kappa: -0.064704357455480629
+    s: 12.865328111323659
+    dkappa: 0.0079892476976079728
     ddkappa: 0
   }
-  v: 4.2600657244119633
-  a: 0.70817608743080773
+  v: 4.2105870014058944
+  a: 0.70360239243261224
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 586909.76582623378
-    y: 4141014.23309421
-    theta: 1.6031767494046654
-    kappa: -0.060627998774574719
-    s: 13.383592694219049
-    dkappa: 0.0079501388251913187
+    x: 586909.77027415263
+    y: 4141014.1402303744
+    theta: 1.6091599512972448
+    kappa: -0.061330128191435296
+    s: 13.289950004368182
+    dkappa: 0.00795411913987349
     ddkappa: 0
   }
-  v: 4.3321814258224585
-  a: 0.73304965994829874
+  v: 4.2822750658744457
+  a: 0.72906065050957447
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586909.75798864663
-    y: 4141014.6699147355
-    theta: 1.5775217270153226
-    kappa: -0.05720940692711643
-    s: 13.820509529715986
-    dkappa: 0.00783909477776032
+    x: 586909.75965013448
+    y: 4141014.5719622709
+    theta: 1.5834493595623138
+    kappa: -0.057941685511855565
+    s: 13.721857159552755
+    dkappa: 0.0078572398752307857
     ddkappa: 0
   }
-  v: 4.4064638363768163
-  a: 0.75156863172158628
+  v: 4.3561853569059981
+  a: 0.74810567053220223
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 586909.76077939966
-    y: 4141015.1142974012
-    theta: 1.5528901388143839
-    kappa: -0.053802689456564606
-    s: 14.264937047836586
-    dkappa: 0.0076805655885915907
+    x: 586909.75953806634
+    y: 4141015.0113031808
+    theta: 1.5587311163449979
+    kappa: -0.054558979434276858
+    s: 14.161240320811803
+    dkappa: 0.0077100407718914254
     ddkappa: 0
   }
-  v: 4.48229500442467
-  a: 0.76408317123172464
+  v: 4.4316941747469851
+  a: 0.76108993353781274
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 586909.77423368371
-    y: 4141015.5661235014
-    theta: 1.52933155687453
-    kappa: -0.050425168900043491
-    s: 14.71700068578302
-    dkappa: 0.0074843270534523049
+    x: 586909.76999226725
+    y: 4141015.4581359774
+    theta: 1.5350567943797688
+    kappa: -0.051200399340750706
+    s: 14.60822962450958
+    dkappa: 0.0075223965996011107
     ddkappa: 0
   }
-  v: 4.5590919951637581
-  a: 0.77094344695976824
+  v: 4.5082130677475707
+  a: 0.7683659205637241
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586909.7984065651
-    y: 4141016.0252212714
-    theta: 1.5069040395388142
-    kappa: -0.047092904055530453
-    s: 15.176769338119632
-    dkappa: 0.0072578616950897869
+    x: 586909.79104949161
+    y: 4141015.9122913228
+    theta: 1.5124802903196186
+    kappa: -0.047882556259451085
+    s: 15.062898124250541
+    dkappa: 0.007303006164128825
     ddkappa: 0
   }
-  v: 4.636306890639923
-  a: 0.77249962738677158
+  v: 4.5851888323616512
+  a: 0.77028611264725355
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 586909.833241286
-    y: 4141016.4913818068
-    theta: 1.4856544202646633
-    kappa: -0.043818658618338266
-    s: 15.644258858457764
-    dkappa: 0.007011013612494228
+    x: 586909.82273756946
+    y: 4141016.373555637
+    theta: 1.4910594138476394
+    kappa: -0.044620055375419049
+    s: 15.525265315689722
+    dkappa: 0.00705924613051923
     ddkappa: 0
   }
-  v: 4.7134267897471132
-  a: 0.76910188099378884
+  v: 4.6621035131468531
+  a: 0.76720299082571874
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 586909.87846219691
-    y: 4141016.9643785795
-    theta: 1.4656010372628203
-    kappa: -0.040609992225013485
-    s: 16.119435561140552
-    dkappa: 0.0067571342090051046
+    x: 586909.86482864583
+    y: 4141016.841694396
+    theta: 1.4708186088606741
+    kappa: -0.041422033636204919
+    s: 15.995300661343103
+    dkappa: 0.00680520107844799
     ddkappa: 0
   }
-  v: 4.7899738082273835
-  a: 0.7611003762618741
+  v: 4.7384744027645365
+  a: 0.75946903613643713
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586909.93371992279
-    y: 4141017.4439711007
-    theta: 1.4467545245712381
-    kappa: -0.037472458179878446
-    s: 16.602219722927742
-    dkappa: 0.0065016578241713
+    x: 586909.91694982641
+    y: 4141017.3164650672
+    theta: 1.4517630291804744
+    kappa: -0.038292989438206221
+    s: 16.47292711539799
+    dkappa: 0.0065517197056264246
     ddkappa: 0
   }
-  v: 4.8655050786708927
-  a: 0.74884528167208231
+  v: 4.8138540419797931
+  a: 0.74743672961672636
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 586909.99870734964
-    y: 4141017.9298986965
-    theta: 1.4291348357630134
-    kappa: -0.03441703434236662
-    s: 17.092489084680505
-    dkappa: 0.0062337296708942719
+    x: 586909.9788136346
+    y: 4141017.7976010796
+    theta: 1.4339152517025866
+    kappa: -0.03524092257717764
+    s: 16.958024648523377
+    dkappa: 0.0062917305304690417
     ddkappa: 0
   }
-  v: 4.9396127505159031
-  a: 0.73268676570546454
+  v: 4.887830219661442
+  a: 0.73145855230390033
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586910.07305679249
-    y: 4141018.4218950868
-    theta: 1.412751583118887
-    kappa: -0.031456843433630394
-    s: 17.5900843226455
-    dkappa: 0.0059493633115325056
+    x: 586910.05012564082
+    y: 4141018.2848210456
+    theta: 1.4172973477496353
+    kappa: -0.032283576828898729
+    s: 17.450435754601965
+    dkappa: 0.0060078558647943016
     ddkappa: 0
   }
-  v: 5.01200249744035
-  a: 0.7153163896505581
+  v: 4.9601049713722327
+  a: 0.71424302919871951
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 586910.15635137516
-    y: 4141018.9197133337
-    theta: 1.3976066217439245
-    kappa: -0.028591205863731151
-    s: 18.094833773205476
-    dkappa: 0.005676677984834606
+    x: 586910.13037671451
+    y: 4141018.7778870566
+    theta: 1.4018928193180791
+    kappa: -0.029419255928915079
+    s: 17.949990331438659
+    dkappa: 0.0057340298381624343
     ddkappa: 0
   }
-  v: 5.0827179763850561
-  a: 0.69920445176688351
+  v: 5.0307204441456124
+  a: 0.6982760654263338
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 586910.248136074
-    y: 4141019.423147874
-    theta: 1.3836979307701147
-    kappa: -0.02581627549629879
-    s: 18.60657633220988
-    dkappa: 0.0054208785566285328
+    x: 586910.21911986719
+    y: 4141019.2765905121
+    theta: 1.3876974883050437
+    kappa: -0.026644395042015136
+    s: 18.456528724855076
+    dkappa: 0.0054760607611125355
     ddkappa: 0
   }
-  v: 5.1518859448640191
-  a: 0.684369225991984
+  v: 5.0998024214760562
+  a: 0.68357621221066245
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586910.34792494169
-    y: 4141019.9320353572
-    theta: 1.3710212065748986
-    kappa: -0.023126395616760751
-    s: 19.125163662629237
-    dkappa: 0.0051843336650470731
+    x: 586910.316001859
+    y: 4141019.780740161
+    theta: 1.3747309899004176
+    kappa: -0.023955834820919251
+    s: 18.969903951778416
+    dkappa: 0.0052368536642247226
     ddkappa: 0
   }
-  v: 5.2196349877849952
-  a: 0.67082898626340326
+  v: 5.1674785419804312
+  a: 0.6701620207756247
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586910.4552073169
-    y: 4141020.4462529253
-    theta: 1.3595664613332352
-    kappa: -0.020551186177925635
-    s: 19.650460377294554
-    dkappa: 0.0048952510614104286
+    x: 586910.42046727031
+    y: 4141020.2902163896
+    theta: 1.3629764108396503
+    kappa: -0.021352504885773234
+    s: 19.489981885753682
+    dkappa: 0.0050046106240001775
     ddkappa: 0
   }
-  v: 5.2860955174494952
-  a: 0.65860200651868472
+  v: 5.2338782993979995
+  a: 0.65805204234514
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586910.56946422067
-    y: 4141020.9657136532
-    theta: 1.3492762021590914
-    kappa: -0.018209588664956252
-    s: 20.182344221636654
-    dkappa: 0.0043857570976061676
+    x: 586910.53200179513
+    y: 4141020.8049276369
+    theta: 1.3524043693289789
+    kappa: -0.018939500002666677
+    s: 20.016641442455928
+    dkappa: 0.004577856867137747
     ddkappa: 0
   }
-  v: 5.351399773552779
-  a: 0.64770656069537191
+  v: 5.2991330425904088
+  a: 0.64726482814312769
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586910.69020349975
-    y: 4141021.4903567894
-    theta: 1.3400393720099981
-    kappa: -0.016124538291293226
-    s: 20.720706256425593
-    dkappa: 0.0038592179048613394
+    x: 586910.65010907769
+    y: 4141021.3248110227
+    theta: 1.3428999431083168
+    kappa: -0.016818374576661712
+    s: 20.549774765202478
+    dkappa: 0.0039743173542904067
     ddkappa: 0
   }
-  v: 5.415681823183867
-  a: 0.63816092273100855
+  v: 5.3633759755417039
+  a: 0.63781892939350715
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 586910.81697121891
-    y: 4141022.0201422218
-    theta: 1.3317701359429202
-    kappa: -0.014215959384636406
-    s: 21.265451040510008
-    dkappa: 0.0034869859956578562
+    x: 586910.77433536411
+    y: 4141021.8498239033
+    theta: 1.3343708077561904
+    kappa: -0.01487546006589177
+    s: 21.089287410465197
+    dkappa: 0.0035948246342259885
     ddkappa: 0
   }
-  v: 5.47907756082553
-  a: 0.629983366563138
+  v: 5.42674215735832
+  a: 0.62973289732019777
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 586910.94937921793
-    y: 4141022.555040638
-    theta: 1.3244090286779437
-    kappa: -0.012470396326573081
-    s: 21.816496813556494
-    dkappa: 0.0031477350234876827
+    x: 586910.904273561
+    y: 4141022.3799396288
+    theta: 1.3267528256732755
+    kappa: -0.013096227942462183
+    s: 21.6350985333827
+    dkappa: 0.0032507867708011003
     ddkappa: 0
   }
-  v: 5.5417247083542955
-  a: 0.62319216612930373
+  v: 5.4893685022690839
+  a: 0.6230252831471188
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586911.087044257
-    y: 4141023.0950461128
-    theta: 1.3178972516205087
-    kappa: -0.010874953121940804
-    s: 22.373775678789002
-    dkappa: 0.0028397384878330223
+    x: 586911.0395628406
+    y: 4141022.9151451737
+    theta: 1.3199921361138895
+    kappa: -0.011468481719314595
+    s: 22.187141073272606
+    dkappa: 0.0029368431139321785
     ddkappa: 0
   }
-  v: 5.6037628150404428
-  a: 0.61780559536704949
+  v: 5.5513937796252142
+  a: 0.61771463809818961
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 586911.22953193448
-    y: 4141023.6401894037
-    theta: 1.3121621537548358
-    kappa: -0.0094147654748305588
-    s: 22.937233785728189
-    dkappa: 0.0025654945101810664
+    x: 586911.179786932
+    y: 4141023.4554650192
+    theta: 1.3140207671439785
+    kappa: -0.0099779357115444417
+    s: 22.745361939143788
+    dkappa: 0.0026557535944336376
     ddkappa: 0
   }
-  v: 5.6653332575480082
-  a: 0.6138419282139187
+  v: 5.6129586139003234
+  a: 0.61381951339732965
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 586911.37667695037
-    y: 4141024.1904522725
-    theta: 1.3072019939218147
-    kappa: -0.00808816589299874
-    s: 23.506831512930802
-    dkappa: 0.0023198242167203672
+    x: 586911.32455520052
+    y: 4141024.0009400565
+    theta: 1.3087745720568229
+    kappa: -0.008611575831907024
+    s: 23.309722195208586
+    dkappa: 0.0024036314232622697
     ddkappa: 0
   }
-  v: 5.7265792399347824
-  a: 0.61131943860745486
+  v: 5.6742054846904137
+  a: 0.61135846026845819
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 586911.5279723861
-    y: 4141024.7459281408
-    theta: 1.3029211385342303
-    kappa: -0.0068772406629804429
-    s: 24.082543650729065
-    dkappa: 0.0020973093584760187
+    x: 586911.47353690537
+    y: 4141024.5516170934
+    theta: 1.3042017079432493
+    kappa: -0.00735909600287483
+    s: 23.880197246395063
+    dkappa: 0.0021749260866340625
     ddkappa: 0
   }
-  v: 5.787645793652306
-  a: 0.61025640048520147
+  v: 5.7352787267138785
+  a: 0.6103500299354947
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 586911.68304731906
-    y: 4141025.3066961733
-    theta: 1.2992539437239263
-    kappa: -0.0057699462689230294
-    s: 24.664359583970025
-    dkappa: 0.0018932934363461178
+    x: 586911.62643281592
+    y: 4141025.1075546667
+    theta: 1.300256511587299
+    kappa: -0.0062117010993632026
+    s: 24.456777023859249
+    dkappa: 0.0019670295861336797
     ddkappa: 0
   }
-  v: 5.84867977754588
-  a: 0.610671087784702
+  v: 5.796324529811506
+  a: 0.61081277362235831
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 586911.84165309626
-    y: 4141025.8728216728
-    theta: 1.2961671214983042
-    kappa: -0.0047602828890318787
-    s: 25.252283474754964
-    dkappa: 0.0017034966326104569
+    x: 586911.7831937134
+    y: 4141025.6687608203
+    theta: 1.296972995796847
+    kappa: -0.0051748003693572776
+    s: 25.039466170497359
+    dkappa: 0.001775784049036245
     ddkappa: 0
   }
-  v: 5.9098298778545555
-  a: 0.61258177444350037
+  v: 5.8574909389464747
+  a: 0.6127652425529686
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 586912.003492673
-    y: 4141026.4444020535
-    theta: 1.2936097396311146
-    kappa: -0.0038396130267301515
-    s: 25.846334445178758
-    dkappa: 0.0015327462625353871
+    x: 586911.94329391443
+    y: 4141026.2353949859
+    theta: 1.2942347713269045
+    kappa: -0.0042280073679965046
+    s: 25.628284226458053
+    dkappa: 0.0016008092783728867
     ddkappa: 0
   }
-  v: 5.9712466082111417
-  a: 0.61600673439913967
+  v: 5.9189278542043553
+  a: 0.61622598795124484
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 586912.1682949306
-    y: 4141027.0215457659
-    theta: 1.2915328266931208
-    kappa: -0.0029999993941449382
-    s: 26.446546760069236
-    dkappa: 0.0013780025444463263
+    x: 586912.10643525422
+    y: 4141026.8075730149
+    theta: 1.2919862712896009
+    kappa: -0.0033620342249129336
+    s: 26.223265814654674
+    dkappa: 0.0014446891119831743
     ddkappa: 0
   }
-  v: 6.0330823096421966
-  a: 0.62096424158916352
+  v: 5.9807870307931088
+  a: 0.62121356104110648
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 586912.3359608769
-    y: 4141027.6043297718
-    theta: 1.2899611550445138
-    kappa: -0.0022471240203823851
-    s: 27.052970009726582
-    dkappa: 0.0012359828278960864
+    x: 586912.27239866252
+    y: 4141027.3854063773
+    theta: 1.2901915314097685
+    kappa: -0.00257120624886901
+    s: 26.824460826277466
+    dkappa: 0.0013009792899525061
     ddkappa: 0
   }
-  v: 6.095491150568038
-  a: 0.62747256995111556
+  v: 6.0432220790430931
+  a: 0.62774651304647278
   relative_time: 5.7999999999999963
 }
 trajectory_point {
   path_point {
-    x: 586912.50616479712
-    y: 4141028.1929136403
-    theta: 1.2888139000204526
-    kappa: -0.0015676760352403341
-    s: 27.665669292662695
-    dkappa: 0.0011009966098391513
+    x: 586912.44101259124
+    y: 4141027.9690103848
+    theta: 1.2888318087108763
+    kappa: -0.0018529703227167461
+    s: 27.431934606305841
+    dkappa: 0.0011632870956100349
     ddkappa: 0
   }
-  v: 6.1586291268027349
-  a: 0.63554999342253937
+  v: 6.1063884644070505
+  a: 0.635843395191263
   relative_time: 5.8999999999999959
 }
 trajectory_point {
   path_point {
-    x: 586912.67867546284
-    y: 4141028.7874474879
-    theta: 1.2880370103742553
-    kappa: -0.0009536806832875441
-    s: 28.284725398340576
-    dkappa: 0.000979915348930223
+    x: 586912.61212155351
+    y: 4141028.5585129694
+    theta: 1.287913159035742
+    kappa: -0.0012100237567302776
+    s: 28.045768139020595
+    dkappa: 0.0010406533512285112
     ddkappa: 0
   }
-  v: 6.2226540615541124
-  a: 0.64521478594097847
+  v: 6.1704435074601225
+  a: 0.64552275869939679
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 586912.85332902858
-    y: 4141029.38807913
-    theta: 1.2875958940410628
-    kappa: -0.00040066771961697
-    s: 28.910234989913686
-    dkappa: 0.000868050430720075
+    x: 586912.78545210278
+    y: 4141029.1540936171
+    theta: 1.2873543404882739
+    kappa: -0.00062948318531666936
+    s: 28.666058233516168
+    dkappa: 0.00092883399265297545
     ddkappa: 0
   }
-  v: 6.2877256054237494
-  a: 0.65648522144397614
+  v: 6.2355463838998393
+  a: 0.65680315479479312
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 586913.02999255515
-    y: 4141029.9949644655
-    theta: 1.2875082213745266
-    kappa: 8.534991689463308e-05
-    s: 29.542310786965338
-    dkappa: 0.00075996894113268933
+    x: 586912.96084617928
+    y: 4141029.7559156138
+    theta: 1.2871234264690283
+    kappa: -0.00010712525522149211
+    s: 29.292917709212862
+    dkappa: 0.00082164804457772185
     ddkappa: 0
   }
-  v: 6.3540052364069775
-  a: 0.66937957386907632
+  v: 6.30185812454612
+  a: 0.66970313470137166
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
-    x: 586913.20848636073
-    y: 4141030.6082900716
-    theta: 1.287713243009718
-    kappa: 0.0005130937362283611
-    s: 30.181081748248062
-    dkappa: 0.00066251822109475444
+    x: 586913.13816526532
+    y: 4141030.3641536585
+    theta: 1.2871951236200658
+    kappa: 0.00035980003309926038
+    s: 29.926475581369104
+    dkappa: 0.00072168782958632448
     ddkappa: 0
   }
-  v: 6.4216562598928855
-  a: 0.68391611715382206
+  v: 6.3695416153412827
+  a: 0.68424124964305166
   relative_time: 6.2999999999999945
 }
 trajectory_point {
   path_point {
-    x: 586913.388700282
-    y: 4141031.2282393826
-    theta: 1.2881607775219301
-    kappa: 0.00088982495679758853
-    s: 30.826693254422992
-    dkappa: 0.000572049194554774
+    x: 586913.31724929227
+    y: 4141030.9790058332
+    theta: 1.2875664013450698
+    kappa: 0.00076837749212729431
+    s: 30.566877246593652
+    dkappa: 0.00063164842054290178
     ddkappa: 0
   }
-  v: 6.4908438086643141
-  a: 0.70011312523575731
+  v: 6.4387615973500312
+  a: 0.70043605084375249
   relative_time: 6.3999999999999941
 }
 trajectory_point {
   path_point {
-    x: 586913.57051469013
-    y: 4141031.8550157202
-    theta: 1.2888445883207418
-    kappa: 0.0012137338926147183
-    s: 31.47930729079922
-    dkappa: 0.00048198458210220137
+    x: 586913.49798500387
+    y: 4141031.6006737063
+    theta: 1.2881866701725158
+    kappa: 0.0011261516065641777
+    s: 31.21428466835788
+    dkappa: 0.00054457575900163611
     ddkappa: 0
   }
-  v: 6.56173484289786
-  a: 0.71798887205242556
+  v: 6.5096846667594637
+  a: 0.71830608952739361
   relative_time: 6.4999999999999938
 }
 trajectory_point {
   path_point {
-    x: 586913.75378304638
-    y: 4141032.4888473684
-    theta: 1.2897466212534279
-    kappa: 0.0014828289023328194
-    s: 32.139102630073204
-    dkappa: 0.00040266705735863986
+    x: 586913.68028561352
+    y: 4141032.2293683845
+    theta: 1.2890252315413822
+    kappa: 0.0014366867452933953
+    s: 31.868876562507971
+    dkappa: 0.00046277298526433531
     ddkappa: 0
   }
-  v: 6.634498150163874
-  a: 0.73756163154137
+  v: 6.5824792748790708
+  a: 0.737869916917894
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
-    x: 586913.93851026217
-    y: 4141033.12993611
-    theta: 1.2908112183769036
-    kappa: 0.0017069582108871157
-    s: 32.8062750150681
-    dkappa: 0.0003258614934314167
+    x: 586913.8640378674
+    y: 4141032.8653259296
+    theta: 1.2900628315979652
+    kappa: 0.0016995657537683447
+    s: 32.530848582777182
+    dkappa: 0.00038777129159428065
     ddkappa: 0
   }
-  v: 6.7093043454264611
-  a: 0.75884967764013422
+  v: 6.6573157281407349
+  a: 0.7591460842391734
   relative_time: 6.6999999999999931
 }
 trajectory_point {
   path_point {
-    x: 586914.12461348844
-    y: 4141033.7785266605
-    theta: 1.2920225868580264
-    kappa: 0.0018863982786809699
-    s: 33.4810373414732
-    dkappa: 0.00025373501187532467
+    x: 586914.04913440545
+    y: 4141033.508798047
+    theta: 1.2912813358572177
+    kappa: 0.0019148357815135086
+    s: 33.200413506298077
+    dkappa: 0.00031499173734169349
     ddkappa: 0
   }
-  v: 6.78632587104348
-  a: 0.78187128428626218
+  v: 6.7343661880987291
+  a: 0.78215314271515113
   relative_time: 6.7999999999999927
 }
 trajectory_point {
   path_point {
-    x: 586914.3119736932
-    y: 4141034.4348915969
-    theta: 1.2933653289522042
-    kappa: 0.0020332084270960845
-    s: 34.163619840583223
-    dkappa: 0.00021221864122934666
+    x: 586914.23561075749
+    y: 4141034.160012953
+    theta: 1.2926379944719331
+    kappa: 0.0020947359369011646
+    s: 33.877801419114768
+    dkappa: 0.00025745180609247815
     ddkappa: 0
   }
-  v: 6.865736996766544
-  a: 0.80664472541729715
+  v: 6.8138046714297182
+  a: 0.80690964356974648
   relative_time: 6.8999999999999924
 }
 trajectory_point {
   path_point {
-    x: 586914.50067335821
-    y: 4141035.099263703
-    theta: 1.2948130453486124
-    kappa: 0.0021587717037465813
-    s: 34.85427026203778
-    dkappa: 0.00017588962424974682
+    x: 586914.42334460572
+    y: 4141034.8192619011
+    theta: 1.2941257935979986
+    kappa: 0.0022447438158973718
+    s: 34.563259901695176
+    dkappa: 0.00021343382187697082
     ddkappa: 0
   }
-  v: 6.94771381974102
-  a: 0.83318827497078263
+  v: 6.8958070499327615
+  a: 0.8334341380268786
   relative_time: 6.999999999999992
 }
 trajectory_point {
   path_point {
-    x: 586914.69062214694
-    y: 4141035.7719431538
-    theta: 1.2963599226797673
-    kappa: 0.0022632582787099663
-    s: 35.553254056560675
-    dkappa: 0.00014531915287569727
+    x: 586914.61229149112
+    y: 4141035.4868317274
+    theta: 1.2957307681931525
+    kappa: 0.0023669874623687102
+    s: 35.257054214443187
+    dkappa: 0.0001716206552990888
     ddkappa: 0
   }
-  v: 7.0324342645060325
-  a: 0.86152020688426212
+  v: 6.9805510505293089
+  a: 0.86174517731046718
   relative_time: 7.0999999999999917
 }
 trajectory_point {
   path_point {
-    x: 586914.88181816542
-    y: 4141036.453223106
-    theta: 1.2979934463776404
-    kappa: 0.0023487384340496807
-    s: 36.260854558699329
-    dkappa: 0.00011717626102453676
+    x: 586914.8025865393
+    y: 4141036.1629767204
+    theta: 1.2974246818125135
+    kappa: 0.0024670859740455668
+    s: 35.959467483211021
+    dkappa: 0.00013530518335762111
     ddkappa: 0
   }
-  v: 7.1200780829944561
-  a: 0.89165879509527923
+  v: 7.0682162552631995
+  a: 0.89186131264443125
   relative_time: 7.1999999999999913
 }
 trajectory_point {
   path_point {
-    x: 586915.07432795374
-    y: 4141037.1433960735
-    theta: 1.2997004724658132
-    kappa: 0.0024177342607716677
-    s: 36.977373169564125
-    dkappa: 9.14217195186513e-05
+    x: 586914.99393914244
+    y: 4141036.84808928
+    theta: 1.299210875285375
+    kappa: 0.0025401205575585595
+    s: 36.670800884811364
+    dkappa: 9.9714634339520081e-05
     ddkappa: 0
   }
-  v: 7.2108268545329235
-  a: 0.92362231354137758
+  v: 7.15898410130067
+  a: 0.9238010952526905
   relative_time: 7.2999999999999909
 }
 trajectory_point {
   path_point {
-    x: 586915.2680102787
-    y: 4141037.8428310482
-    theta: 1.301476451213732
-    kappa: 0.0024701074162425622
-    s: 37.703129539567811
-    dkappa: 6.9900284612703663e-05
+    x: 586915.18664622155
+    y: 4141037.5424156906
+    theta: 1.301060469644203
+    kappa: 0.00259356139880638
+    s: 37.391373832529652
+    dkappa: 6.863694118443464e-05
     ddkappa: 0
   }
-  v: 7.304863985841819
-  a: 0.95742903616010033
+  v: 7.2530378809303446
+  a: 0.95758307635916418
   relative_time: 7.3999999999999906
 }
 trajectory_point {
   path_point {
-    x: 586915.4630697309
-    y: 4141038.5518198134
-    theta: 1.3033070247543657
-    kappa: 0.0025094414297832791
-    s: 38.438461751164866
-    dkappa: 4.9234778218351668e-05
+    x: 586915.38051024755
+    y: 4141038.2463588417
+    theta: 1.3029672878319449
+    kappa: 0.0026249327484798761
+    s: 38.121524161636287
+    dkappa: 3.9160280326128742e-05
     ddkappa: 0
   }
-  v: 7.4023747110352822
-  a: 0.99309723688899143
+  v: 7.3505627415632411
+  a: 0.99322580718777154
   relative_time: 7.49999999999999
 }
 trajectory_point {
   path_point {
-    x: 586915.659324311
-    y: 4141039.2707798118
-    theta: 1.3051884679723713
-    kappa: 0.0025355498608184476
-    s: 39.183726501590847
-    dkappa: 3.3284007301557134e-05
+    x: 586915.57568636723
+    y: 4141038.9602430179
+    theta: 1.304915906897252
+    kappa: 0.0026375487615965584
+    s: 38.86160831489893
+    dkappa: 1.264770973922493e-05
     ddkappa: 0
   }
-  v: 7.5035460916212067
-  a: 1.0306451896655939
+  v: 7.4517456857327673
+  a: 1.0307478389624318
   relative_time: 7.59999999999999
 }
 trajectory_point {
   path_point {
-    x: 586915.85701459181
-    y: 4141040.000031855
-    theta: 1.307110391387414
-    kappa: 0.00255149943788849
-    s: 39.9392992856018
-    dkappa: 1.7662090960675374e-05
+    x: 586915.77213556122
+    y: 4141039.68446491
+    theta: 1.306893748619613
+    kappa: 0.0026318043187134807
+    s: 39.612001528094666
+    dkappa: -1.1308048168356645e-05
     ddkappa: 0
   }
-  v: 7.6085670165012411
-  a: 1.0700911684274521
+  v: 7.5567755710947253
+  a: 1.0701677229070647
   relative_time: 7.6999999999999895
 }
 trajectory_point {
   path_point {
-    x: 586916.05595414946
-    y: 4141040.7400322948
-    theta: 1.3090687544511246
-    kappa: 0.0025576489151482472
-    s: 40.705574578213636
-    dkappa: 6.7673365853962272e-06
+    x: 586915.96994186775
+    y: 4141040.4194072881
+    theta: 1.3088895705702484
+    kappa: 0.0026092508788461092
+    s: 40.373098015522309
+    dkappa: -3.3399705139129551e-05
     ddkappa: 0
   }
-  v: 7.7176282019707907
-  a: 1.1114534471121089
+  v: 7.66584311042731
+  a: 1.1115040102455895
   relative_time: 7.7999999999999892
 }
 trajectory_point {
   path_point {
-    x: 586916.25641232578
-    y: 4141041.4911339316
-    theta: 1.3110570438188953
-    kappa: 0.0025567535546409648
-    s: 41.48296601744147
-    dkappa: -3.4036321469969119e-06
+    x: 586916.169121091
+    y: 4141041.1654906757
+    theta: 1.310890160482713
+    kappa: 0.0025710773378415379
+    s: 41.145311155514591
+    dkappa: -5.2383877448138023e-05
     ddkappa: 0
   }
-  v: 7.8309221917190079
-  a: 1.1547502996571082
+  v: 7.7791408716311032
+  a: 1.1547752522019257
   relative_time: 7.8999999999999888
 }
 trajectory_point {
   path_point {
-    x: 586916.45821329008
-    y: 4141042.253828737
-    theta: 1.3130715878367507
-    kappa: 0.0025497379345121442
-    s: 42.271906587039048
-    dkappa: -9.6291548156227718e-06
+    x: 586916.36982251331
+    y: 4141041.9231201331
+    theta: 1.3128869713246574
+    kappa: 0.0025191420476794149
+    s: 41.929073675950413
+    dkappa: -6.9769848794761123e-05
     ddkappa: 0
   }
-  v: 7.9486433568288071
-  a: 1.1999999999999933
+  v: 7.8968632777290839
+  a: 1.1999999999999917
   relative_time: 7.9999999999999885
 }
 decision {
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_rightturn_01_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_rightturn_01_0.pb.txt
index 6842c07399..8abec76715 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_rightturn_01_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_rightturn_01_0.pb.txt
@@ -9,1693 +9,1693 @@ trajectory_point {
     y: 4141446.9076648518
     z: 0
     theta: -0.21266574936068361
-    kappa: -0.0098341145237096049
+    kappa: -0.00991597654145587
     s: 0
     dkappa: 0
     ddkappa: 0
   }
   v: 5.4499998092651367
-  a: 0.0046257383282925762
+  a: 0.0053958329514911425
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 587695.2160292028
-    y: 4141446.8843828822
-    theta: -0.21378501028152566
-    kappa: -0.010019487609553183
-    s: 0.10900137772332054
-    dkappa: -0.000371316499045017
+    x: 587695.2160289014
+    y: 4141446.8843807871
+    theta: -0.2137932436838563
+    kappa: -0.010098603574803173
+    s: 0.10900153084908426
+    dkappa: -0.00036581620037551353
     ddkappa: 0
   }
-  v: 5.4501603208811105
-  a: 0.011333203537609214
+  v: 5.4501755876105555
+  a: 0.01208954353824283
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 587695.32251952926
-    y: 4141446.8610999486
-    theta: -0.21490432118122893
-    kappa: -0.010204868373756358
-    s: 0.21800727040506107
-    dkappa: -0.00074264837843282289
+    x: 587695.32251922193
+    y: 4141446.861095693
+    theta: -0.21492079153617391
+    kappa: -0.010281238679559226
+    s: 0.21800787914438069
+    dkappa: -0.00073164856841266568
     ddkappa: 0
   }
-  v: 5.4504494728076294
-  a: 0.017490978895199009
+  v: 5.4504797167660337
+  a: 0.018232118030045711
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 587695.42901667312
-    y: 4141446.8378155245
-    theta: -0.21602370454997985
-    kappa: -0.010390261005616366
-    s: 0.32702014139726709
-    dkappa: -0.00111400402964687
+    x: 587695.42901664926
+    y: 4141446.8378090453
+    theta: -0.21604841546860251
+    kappa: -0.010463885972767512
+    s: 0.32702150215650044
+    dkappa: -0.0010975053508708481
     ddkappa: 0
   }
-  v: 5.450856343818403
-  a: 0.023106321475427724
+  v: 5.4509012462540305
+  a: 0.023830780862810663
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 587695.53552282869
-    y: 4141446.8145291302
-    theta: -0.2171431806489986
-    kappa: -0.010575669325435329
-    s: 0.43604223707887324
-    dkappa: -0.0014853911050451282
+    x: 587695.53552337142
+    y: 4141446.8145203646
+    theta: -0.21717613578056758
+    kappa: -0.010646549206951959
+    s: 0.43604463959139128
+    dkappa: -0.0014633940643483455
     ddkappa: 0
   }
-  v: 5.4513701578286264
-  a: 0.028186488352661111
+  v: 5.4514293700857248
+  a: 0.028892756472448562
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 587695.641987365
-    y: 4141446.7910027015
-    theta: -0.21830217449180916
-    kappa: -0.010764351061706602
-    s: 0.54507558975853276
-    dkappa: -0.0017071683886577408
+    x: 587695.64198839385
+    y: 4141446.7909900211
+    theta: -0.21834281879587569
+    kappa: -0.010832544396925626
+    s: 0.54507931648011154
+    dkappa: -0.0016820901178966003
     ddkappa: 0
   }
-  v: 5.4519802838949847
-  a: 0.032738736601264924
+  v: 5.4520534267610135
+  a: 0.0334252692948703
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 587695.74839213723
-    y: 4141446.7671458675
-    theta: -0.21951563398759824
-    kappa: -0.0109575415859921
-    s: 0.65412202057744762
-    dkappa: -0.0017226757814290689
+    x: 587695.748393438
+    y: 4141446.7671270939
+    theta: -0.21956319419978104
+    kappa: -0.011023128365275789
+    s: 0.65412734606860468
+    dkappa: -0.0016978744207952925
     ddkappa: 0
   }
-  v: 5.4526762362156491
-  a: 0.03677032329560493
+  v: 5.4527628992685111
+  a: 0.037435543765986762
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 587695.85481124453
-    y: 4141446.74328582
-    theta: -0.22072926006753263
-    kappa: -0.011150758137402286
-    s: 0.7631831424121982
-    dkappa: -0.0017381852633962659
+    x: 587695.85481307679
+    y: 4141446.743260893
+    theta: -0.22078374014751684
+    kappa: -0.011213738474162615
+    s: 0.76319033270747372
+    dkappa: -0.001713660888672247
     ddkappa: 0
   }
-  v: 5.4534476741302766
-  a: 0.040288505510046868
+  v: 5.45354741508555
+  a: 0.040930804321708814
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 587695.96124606044
-    y: 4141446.7194222496
-    theta: -0.22194306481980342
-    kappa: -0.011344003209540276
-    s: 0.87226036277757246
-    dkappa: -0.0017536970347207166
+    x: 587695.96124867466
+    y: 4141446.7193911136
+    theta: -0.22200446865220658
+    kappa: -0.011404377167483047
+    s: 0.87226967474175543
+    dkappa: -0.0017294497239327866
     ddkappa: 0
   }
-  v: 5.4542844021200141
-  a: 0.0433005403189565
+  v: 5.4543967461781833
+  a: 0.043918275397947334
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 587696.06769776077
-    y: 4141446.6955548939
-    theta: -0.22315705807648098
-    kappa: -0.01153727893732571
-    s: 0.98135488672939652
-    dkappa: -0.0017692112667723038
+    x: 587696.0677013977
+    y: 4141446.6955174939
+    theta: -0.22322538945058978
+    kappa: -0.011595046534147207
+    s: 0.98136656740069472
+    dkappa: -0.0017452410995819666
     ddkappa: 0
   }
-  v: 5.4551763698074938
-  a: 0.045813684796699582
+  v: 5.4553008090011783
+  a: 0.046405181430613215
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 587696.17405014858
-    y: 4141446.6711678454
-    theta: -0.22445377972959274
-    kappa: -0.011737550467176906
-    s: 1.0904677197673642
-    dkappa: -0.0017855869026785376
+    x: 587696.1740543996
+    y: 4141446.6711214771
+    theta: -0.22452798563106965
+    kappa: -0.011792830167478514
+    s: 1.0904820056875195
+    dkappa: -0.0017618955476619142
     ddkappa: 0
   }
-  v: 5.4561136719568362
-  a: 0.047835196017641884
+  v: 5.4562496644980234
+  a: 0.048398746855617347
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 587696.28039947059
-    y: 4141446.6466810238
-    theta: -0.22576601782078271
-    kappa: -0.011939146631974484
-    s: 1.199599670737866
-    dkappa: -0.0018021244489116854
+    x: 587696.28040445677
+    y: 4141446.6466252347
+    theta: -0.22584589229480209
+    kappa: -0.011991959129774965
+    s: 1.1996167872692141
+    dkappa: -0.0017787121349951795
     ddkappa: 0
   }
-  v: 5.45708654847365
-  a: 0.049372331056149146
+  v: 5.4572335181009244
+  a: 0.049906196108870593
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587696.38676802244
-    y: 4141446.6221897737
-    theta: -0.22707849519120896
-    kappa: -0.012140779249000953
-    s: 1.3087513547368201
-    dkappa: -0.0018186649854319806
+    x: 587696.38677395135
+    y: 4141446.622124515
+    theta: -0.22716404185984035
+    kappa: -0.012191124486783098
+    s: 1.3087715153662942
+    dkappa: -0.0017955317958886162
     ddkappa: 0
   }
-  v: 5.4580853844050283
-  a: 0.050432346986587134
+  v: 5.4582427197308059
+  a: 0.050934753626283844
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 587696.4931562175
-    y: 4141446.5976940016
-    theta: -0.22839121242028929
-    kappa: -0.012342449101957365
-    s: 1.4179231960125012
-    dkappa: -0.0018352085765288033
+    x: 587696.49316328485
+    y: 4141446.5976192257
+    theta: -0.22848243471665375
+    kappa: -0.012390326989630909
+    s: 1.4179466016425817
+    dkappa: -0.0018123545937755361
     ddkappa: 0
   }
-  v: 5.4591007099395554
-  a: 0.051022500883321595
+  v: 5.4592677637973095
+  a: 0.051491643843767976
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587696.59952255851
-    y: 4141446.5730149592
-    theta: -0.22973186732598613
-    kappa: -0.012546550019578577
-    s: 1.52711543086837
-    dkappa: -0.0018520656045111598
+    x: 587696.59953073284
+    y: 4141446.5729297847
+    theta: -0.22982847067576789
+    kappa: -0.012592000805661242
+    s: 1.5271422690949781
+    dkappa: -0.0018294916694313792
     ddkappa: 0
   }
-  v: 5.460123200407299
-  a: 0.051150049820718295
+  v: 5.4602992891987965
+  a: 0.051584091197233867
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587696.7057965874
-    y: 4141446.5478506954
-    theta: -0.23114727453375011
-    kappa: -0.012757126233071858
-    s: 1.6363281105659049
-    dkappa: -0.0018697604724399706
+    x: 587696.70580561331
+    y: 4141446.5477534691
+    theta: -0.23124836292998263
+    kappa: -0.012800249629780467
+    s: 1.6363585549432389
+    dkappa: -0.0018474675932308451
     ddkappa: 0
   }
-  v: 5.4611436762798178
-  a: 0.050822250873142988
+  v: 5.4613280793223451
+  a: 0.051219320122592407
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 587696.81209038373
-    y: 4141446.5226817513
-    theta: -0.23256295076334768
-    kappa: -0.012967741614522832
-    s: 1.7455611042274282
-    dkappa: -0.0018874586316799265
+    x: 587696.81210041512
+    y: 4141446.5225724336
+    theta: -0.23266852714845337
+    kappa: -0.013008537490400074
+    s: 1.7455953135197491
+    dkappa: -0.0018654468866395235
     ddkappa: 0
   }
-  v: 5.462153103170154
-  a: 0.05004636111496142
+  v: 5.4623450620437515
+  a: 0.050404555055754474
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 587696.91840364563
-    y: 4141446.4975081971
-    theta: -0.23397888632288386
-    kappa: -0.013178395565991475
-    s: 1.8548141017389397
-    dkappa: -0.0019051600319857055
+    x: 587696.91841482138
+    y: 4141446.4973867545
+    theta: -0.23408895337039365
+    kappa: -0.013216863766555922
+    s: 1.8548522191592962
+    dkappa: -0.001883429496056132
     ddkappa: 0
   }
-  v: 5.4631425918328409
-  a: 0.048829637620539365
+  v: 5.4633413097275332
+  a: 0.049147020432630958
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 587697.02473589987
-    y: 4141446.472330146
-    theta: -0.23539506922893225
-    kappa: -0.013389087149543918
-    s: 1.964086616652944
-    dkappa: -0.0019228645945420617
+    x: 587697.02474834281
+    y: 4141446.4721965464
+    theta: -0.23550962932614911
+    kappa: -0.013425227499582436
+    s: 1.9641287690888458
+    dkappa: -0.001901415338729186
     ddkappa: 0
   }
-  v: 5.4641033981638971
-  a: 0.047179337464242563
+  v: 5.4643080392269212
+  a: 0.047453940689132723
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587697.13096669177
-    y: 4141446.4466491872
-    theta: -0.23688660638297082
-    kappa: -0.013606472701529841
-    s: 2.0733779890912816
-    dkappa: -0.0019414709208636946
+    x: 587697.1309800545
+    y: 4141446.4465017654
+    theta: -0.23700491655800007
+    kappa: -0.013640388458572352
+    s: 2.0734242863173153
+    dkappa: -0.0019203047259766033
     ddkappa: 0
   }
-  v: 5.4650269232008277
-  a: 0.045102717720436766
+  v: 5.4652366118838689
+  a: 0.045332540261170645
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587697.23716429831
-    y: 4141446.4207529863
-    theta: -0.23841018969527816
-    kappa: -0.013826711787220305
-    s: 2.1826873886479561
-    dkappa: -0.0019604606735080829
+    x: 587697.23717858514
+    y: 4141446.4205914754
+    theta: -0.23853186981173979
+    kappa: -0.013858440482271141
+    s: 2.1827379225253472
+    dkappa: -0.0019395775198309245
     ddkappa: 0
   }
-  v: 5.4659047131226268
-  a: 0.042607035463487743
+  v: 5.4661185335290448
+  a: 0.042790043584655629
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587697.34337844909
-    y: 4141446.3948527519
-    theta: -0.23993401446729004
-    kappa: -0.014046985183506572
-    s: 2.2920138172919686
-    dkappa: -0.0019794533845271184
+    x: 587697.3433937307
+    y: 4141446.394677131
+    theta: -0.24005906608596461
+    kappa: -0.014076526620415138
+    s: 2.2920686609550858
+    dkappa: -0.0019588533289320604
     ddkappa: 0
   }
-  v: 5.4667284592497749
-  a: 0.039699547767761252
+  v: 5.466945454481837
+  a: 0.039833675095498566
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 587697.44960801466
-    y: 4141446.3689487581
-    theta: -0.24145805741578932
-    kappa: -0.014267290547646867
-    s: 2.4013561122701423
-    dkappa: -0.0019984488519217474
+    x: 587697.44962434249
+    y: 4141446.3687590137
+    theta: -0.24158648174203243
+    kappa: -0.014294644514612919
+    s: 2.40141531929995
+    dkappa: -0.0019781319448307112
     ddkappa: 0
   }
-  v: 5.4674899980442406
-  a: 0.036387511707623055
+  v: 5.4677091695503526
+  a: 0.036470659229610285
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587697.55582562636
-    y: 4141446.3429360068
-    theta: -0.24299776768608058
-    kappa: -0.014489033663882789
-    s: 2.5107129490099567
-    dkappa: -0.0020176455331815855
+    x: 587697.55584284919
+    y: 4141446.3427314484
+    theta: -0.24312949141295551
+    kappa: -0.014514226243014036
+    s: 2.5107765525944088
+    dkappa: -0.0019976129661325439
     ddkappa: 0
   }
-  v: 5.46818131110948
-  a: 0.032678184357438876
+  v: 5.4684016180314163
+  a: 0.03270822042290171
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587697.66189314367
-    y: 4141446.31626288
-    theta: -0.24463424381005971
-    kappa: -0.014719593543799167
-    s: 2.6200828440223729
-    dkappa: -0.0020380845343795778
+    x: 587697.66191101749
+    y: 4141446.3160428377
+    theta: -0.24476828459958927
+    kappa: -0.014742740228816659
+    s: 2.6201508561037525
+    dkappa: -0.0020183367591437035
     ddkappa: 0
   }
-  v: 5.4687945251904342
-  a: 0.028578822791574504
+  v: 5.4690148837105692
+  a: 0.028553583111283715
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587697.767971735
-    y: 4141446.2895869678
-    theta: -0.24627090081138683
-    kappa: -0.014950177495332432
-    s: 2.7294641578046677
-    dkappa: -0.0020585256695126097
+    x: 587697.76799024944
+    y: 4141446.2893514424
+    theta: -0.24640725878220468
+    kappa: -0.014971278050426706
+    s: 2.7295365682138715
+    dkappa: -0.0020390627138095977
     ddkappa: 0
   }
-  v: 5.4693219121735339
-  a: 0.024096684084395692
+  v: 5.4695411948620745
+  a: 0.024013971730667182
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587697.87405966176
-    y: 4141446.2629087083
-    theta: -0.24790770310123178
-    kappa: -0.015180781739519722
-    s: 2.8388550977432612
-    dkappa: -0.0020789686035777885
+    x: 587697.8740787846
+    y: 4141446.2626577076
+    theta: -0.24804637795331483
+    kappa: -0.015199835914532282
+    s: 2.8389318733210289
+    dkappa: -0.0020597904861171007
     ddkappa: 0
   }
-  v: 5.4697558890866969
-  a: 0.019239025310268207
+  v: 5.4699729242489088
+  a: 0.019096610716962968
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587697.98015504
-    y: 4141446.236228575
-    theta: -0.24954461283848819
-    kappa: -0.015411402180748469
-    s: 2.9482537210165454
-    dkappa: -0.002099412973501409
+    x: 587697.98017471528
+    y: 4141446.2359621106
+    theta: -0.24968560383669819
+    kappa: -0.015428409712130978
+    s: 2.9483348047216333
+    dkappa: -0.0020805197034233077
     ddkappa: 0
   }
-  v: 5.4700890180993271
-  a: 0.014013103543557764
+  v: 5.4703025891227712
+  a: 0.013808724506081982
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587698.0861318917
-    y: 4141446.2090623192
-    theta: -0.25125158759175559
-    kappa: -0.015648597134746207
-    s: 3.0576579374977162
-    dkappa: -0.0021212272673146204
+    x: 587698.08615165867
+    y: 4141446.2087792018
+    theta: -0.25139442851064558
+    kappa: -0.015663649939412987
+    s: 3.0577432475020143
+    dkappa: -0.0021026217372683547
     ddkappa: 0
   }
-  v: 5.4703140065223179
-  a: 0.0084261758586301229
+  v: 5.4705228512240769
+  a: 0.0081575375339351
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587698.19202088751
-    y: 4141446.1815389097
-    theta: -0.25301007518335705
-    kappa: -0.015890623264033739
-    s: 3.1670655126576031
-    dkappa: -0.0021440484177094521
+    x: 587698.19204068242
+    y: 4141446.1812395318
+    theta: -0.25315424300283729
+    kappa: -0.015903771627759453
+    s: 3.1671549414281981
+    dkappa: -0.0021257291986389857
     ddkappa: 0
   }
-  v: 5.470423706808047
-  a: 0.0024854993298510696
+  v: 5.47062651678196
+  a: 0.0021502742364331775
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587698.29791083443
-    y: 4141446.1540152533
-    theta: -0.25476858622463217
-    kappa: -0.016132651567091937
-    s: 3.2764740704674953
-    dkappa: -0.0021668697730736707
+    x: 587698.29793052748
+    y: 4141446.153699649
+    theta: -0.25491407880484324
+    kappa: -0.0161438951782356
+    s: 3.2765674838356809
+    dkappa: -0.0021488368392066303
     ddkappa: 0
   }
-  v: 5.4704111165503821
-  a: -0.0038016689684136403
+  v: 5.4706065365142713
+  a: -0.0042058409505128427
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587698.40379929857
-    y: 4141446.1264919816
-    theta: -0.25652706942269682
-    kappa: -0.016374676481188462
-    s: 3.3858810963019779
-    dkappa: -0.0021896908088855989
+    x: 587698.40381873329
+    y: 4141446.1261601923
+    theta: -0.25667388412659542
+    kappa: -0.016384015011560102
+    s: 3.3859783325192003
+    dkappa: -0.0021719441220642384
     ddkappa: 0
   }
-  v: 5.4702693784846765
-  a: -0.01042807196179828
+  v: 5.4704560056275824
+  a: -0.010903583590992146
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587698.50968007208
-    y: 4141446.0989559824
-    theta: -0.25828741104596148
-    kappa: -0.016616878756133911
-    s: 3.4952839398417543
-    dkappa: -0.0022125254994285535
+    x: 587698.50969881564
+    y: 4141446.0986071229
+    theta: -0.25843565971228194
+    kappa: -0.01662432681597709
+    s: 3.4953848086225174
+    dkappa: -0.0021950660086205512
     ddkappa: 0
   }
-  v: 5.4699917804877716
-  a: -0.017386452575937066
+  v: 5.4701681638171813
+  a: -0.017935729249093851
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587698.615326519
-    y: 4141446.0705607994
-    theta: -0.26016882822800624
-    kappa: -0.016870724210958126
-    s: 3.604679817976483
-    dkappa: -0.00223626631500897
+    x: 587698.61534437374
+    y: 4141446.07019549
+    theta: -0.26031747607629463
+    kappa: -0.0168764098343544
+    s: 3.6047840995281817
+    dkappa: -0.0022190943545474131
     ddkappa: 0
   }
-  v: 5.4695717555779959
-  a: -0.024669553736464234
+  v: 5.4697363952670743
+  a: -0.025295053488907003
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587698.72096342617
-    y: 4141446.0421681814
-    theta: -0.26205009236196453
-    kappa: -0.0171245467436403
-    s: 3.7140658177076005
-    dkappa: -0.0022600049868033282
+    x: 587698.72098015074
+    y: 4141446.0417864872
+    theta: -0.26219913505160369
+    kappa: -0.017128469513733331
+    s: 3.714173261747312
+    dkappa: -0.00224312047582015
     ddkappa: 0
   }
-  v: 5.4690028819151664
-  a: -0.03227011836901407
+  v: 5.4691542286499857
+  a: -0.0329743318745208
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587698.82658785768
-    y: 4141446.0137789156
-    theta: -0.26393113784616667
-    kappa: -0.017378339300111102
-    s: 3.8234388990511574
-    dkappa: -0.0022837408550820336
+    x: 587698.82660318061
+    y: 4141446.0133809131
+    theta: -0.26408057047700778
+    kappa: -0.017380498776686241
+    s: 3.8235492238093687
+    dkappa: -0.0022671436978241822
     ddkappa: 0
   }
-  v: 5.4682788828005844
-  a: -0.040180889399220737
+  v: 5.46841533712736
+  a: -0.040966339970024288
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587698.932196758
-    y: 4141445.985393825
-    theta: -0.26581189695383178
-    kappa: -0.017632094538376338
-    s: 3.9327958979406441
-    dkappa: -0.0023074732331874103
+    x: 587698.93221037625
+    y: 4141445.9849795969
+    theta: -0.26596171404865609
+    kappa: -0.01763249025754287
+    s: 3.932908789151929
+    dkappa: -0.0022911633184698791
     ddkappa: 0
   }
-  v: 5.4673936266770422
-  a: -0.048394609752718559
+  v: 5.4675135383493583
+  a: -0.049263853339506683
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 587699.03767108463
-    y: 4141445.956590144
-    theta: -0.26775098798704983
-    kappa: -0.017891600559423345
-    s: 4.0421335291298215
-    dkappa: -0.0023320462419271068
+    x: 587699.03768229275
+    y: 4141445.9561586753
+    theta: -0.26790095669627462
+    kappa: -0.017890313079580764
+    s: 4.0422486390104577
+    dkappa: -0.0023160262959391964
     ddkappa: 0
   }
-  v: 5.4663411271288167
-  a: -0.056904022355141783
+  v: 5.46644279445486
+  a: -0.057859647547057011
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 587699.14298410038
-    y: 4141445.9272830235
-    theta: -0.26976026427222244
-    kappa: -0.018158022427299254
-    s: 4.15144838909555
-    dkappa: -0.0023576301234906841
+    x: 587699.14299277344
+    y: 4141445.9268356003
+    theta: -0.26990975882526635
+    kappa: -0.01815511385799647
+    s: 4.1515653353080895
+    dkappa: -0.0023418984098367247
     ddkappa: 0
   }
-  v: 5.4651155428816729
-  a: -0.065701870132124612
+  v: 5.4651972120714616
+  a: -0.06674649815676445
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 587699.2482717887
-    y: 4141445.8979829522
-    theta: -0.2717690707581063
-    kappa: -0.018424380220967373
-    s: 4.2607369589406234
-    dkappa: -0.00238320785215489
+    x: 587699.248277525
+    y: 4141445.8975196891
+    theta: -0.27191808356663383
+    kappa: -0.018419849940758119
+    s: 4.2608553235453908
+    dkappa: -0.0023677642027055148
     ddkappa: 0
   }
-  v: 5.4637111778028631
-  a: -0.074780896009301281
+  v: 5.4637710423154813
+  a: -0.07591718073271804
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 587699.35353065073
-    y: 4141445.8686909019
-    theta: -0.27377732448169795
-    kappa: -0.018690665090066783
-    s: 4.369995607296592
-    dkappa: -0.0024087785780398433
+    x: 587699.35353301349
+    y: 4141445.868211926
+    theta: -0.2739258473121311
+    kappa: -0.01868451244269248
+    s: 4.3701149356901494
+    dkappa: -0.0023936228064279745
     ddkappa: 0
   }
-  v: 5.4621224809011286
-  a: -0.084133842912306078
+  v: 5.4621586807919513
+  a: -0.085364470839007028
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 587699.4587570827
-    y: 4141445.8394078771
-    theta: -0.27578494049320795
-    kappa: -0.018956867917196734
-    s: 4.4792205932265974
-    dkappa: -0.0024343414256224388
+    x: 587699.4587555991
+    y: 4141445.8389133252
+    theta: -0.27593296444788051
+    kappa: -0.01894909221061274
+    s: 4.4793403930671349
+    dkappa: -0.0024194733267004953
     ddkappa: 0
   }
-  v: 5.4603440463266946
-  a: -0.093753453766773256
+  v: 5.4603546675946246
+  a: -0.095081144039720428
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 587699.56369731878
-    y: 4141445.8092525136
-    theta: -0.27791246787360924
-    kappa: -0.019235195552312841
-    s: 4.5884080691281994
-    dkappa: -0.0024616827313068261
+    x: 587699.5636910894
+    y: 4141445.8087416482
+    theta: -0.27805947329355241
+    kappa: -0.019225942210603794
+    s: 4.5885278092478838
+    dkappa: -0.0024471057398699958
     ddkappa: 0
   }
-  v: 5.4583706133712768
-  a: -0.10363247149833697
+  v: 5.4583536873059719
+  a: -0.10505997589894736
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 587699.66856625734
-    y: 4141445.7789976266
-    theta: -0.28005438215218448
-    kappa: -0.019514953878569347
-    s: 4.6975540836362093
-    dkappa: -0.0024892384274548688
+    x: 587699.6685551065
+    y: 4141445.7784719821
+    theta: -0.28020009258316403
+    kappa: -0.019504219653706134
+    s: 4.6976731929404654
+    dkappa: -0.0024749497438089408
     ddkappa: 0
   }
-  v: 5.4561970664680768
-  a: -0.11376363903263155
+  v: 5.4561505689971828
+  a: -0.11529374198077702
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 587699.77339146589
-    y: 4141445.7487553554
-    theta: -0.28219541105723916
-    kappa: -0.019794595546294182
-    s: 4.8066545845255151
-    dkappa: -0.0025167826329456186
+    x: 587699.77337480732
+    y: 4141445.7482151082
+    theta: -0.28233981498802568
+    kappa: -0.019782379494429056
+    s: 4.8067724508792606
+    dkappa: -0.0025027819806384883
     ddkappa: 0
   }
-  v: 5.4538184351917822
-  a: -0.12413969929529114
+  v: 5.4537402862281628
+  a: -0.12577521784929846
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 587699.87816895684
-    y: 4141445.7185268509
-    theta: -0.2843354535186382
-    kappa: -0.020074109917920482
-    s: 4.9157054216139144
-    dkappa: -0.0025443142999977135
+    x: 587699.87814616365
+    y: 4141445.7179721892
+    theta: -0.28447853869574585
+    kappa: -0.020060411043943877
+    s: 4.915821390714739
+    dkappa: -0.0025306013808512272
     ddkappa: 0
   }
-  v: 5.4512298942585717
-  a: -0.13475339521195009
+  v: 5.451117957047539
+  a: -0.13649717906860084
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 587699.98279678321
-    y: 4141445.6879803827
-    theta: -0.28651941557758281
-    kappa: -0.020358154496652108
-    s: 5.0247023496649428
-    dkappa: -0.0025725136952358635
+    x: 587699.98276680417
+    y: 4141445.6874103742
+    theta: -0.2866611356045643
+    kappa: -0.020343036966754047
+    s: 5.0248157239032256
+    dkappa: -0.002559091366014672
     ddkappa: 0
   }
-  v: 5.4484267635261068
-  a: -0.14559746970824264
+  v: 5.4482788439926528
+  a: -0.14745240120277328
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 587700.08715625945
-    y: 4141445.6567276055
-    theta: -0.28879992276722982
-    kappa: -0.020652180803206327
-    s: 5.1336410312907041
-    dkappa: -0.0026021769812561617
+    x: 587700.08711886476
+    y: 4141445.656144469
+    theta: -0.28893959026296784
+    kappa: -0.020635721777551855
+    s: 5.1337510685966823
+    dkappa: -0.0025890426456707037
     ddkappa: 0
   }
-  v: 5.44540450799354
-  a: -0.15666466570980303
+  v: 5.445218354089568
+  a: -0.15863365981590477
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 587700.191455697
-    y: 4141445.6254928079
-    theta: -0.29107914106695976
-    kappa: -0.020946037954703817
-    s: 5.2425170398547012
-    dkappa: -0.0026318232018142278
+    x: 587700.19141013455
+    y: 4141445.6248967787
+    theta: -0.29121674060378927
+    kappa: -0.020928236083543342
+    s: 5.2426229525324777
+    dkappa: -0.00261897647707883
     ddkappa: 0
   }
-  v: 5.4421587378015088
-  a: -0.16794772614226539
+  v: 5.4419320388530634
+  a: -0.17003373047208462
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 587700.29569077259
-    y: 4141445.594277285
-    theta: -0.29335695230250658
-    kappa: -0.021239713770572079
-    s: 5.3513258623746625
-    dkappa: -0.0026614511280546919
+    x: 587700.29563624575
+    y: 4141445.5936686117
+    theta: -0.29349246762019632
+    kappa: -0.021220567633603478
+    s: 5.3514268159231637
+    dkappa: -0.0026488916065461427
     ddkappa: 0
   }
-  v: 5.4386852082321386
-  a: -0.1794393939312641
+  v: 5.4384155942866368
+  a: -0.18164538873540195
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 587700.3998570831
-    y: 4141445.5630823555
-    theta: -0.29563323664058849
-    kappa: -0.021533195845025502
-    s: 5.4600629024253751
-    dkappa: -0.00269105950840122
+    x: 587700.3997927492
+    y: 4141445.5624613
+    theta: -0.29576665062378354
+    kappa: -0.021512703949211905
+    s: 5.4601580143462494
+    dkappa: -0.0026787867571097129
     ddkappa: 0
   }
-  v: 5.434979819709044
-  a: -0.19113241200243325
+  v: 5.4346648608825054
+  a: -0.19346141016994564
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587700.91783306818
-    y: 4141445.4020187552
-    theta: -0.30771832928210113
-    kappa: -0.023074661102311225
-    s: 6.0025050155083148
-    dkappa: -0.0028499559735264439
+    x: 587700.91770572134
+    y: 4141445.4013439412
+    theta: -0.30783816136579828
+    kappa: -0.023048142757744309
+    s: 6.002555916819075
+    dkappa: -0.0028391177982346754
     ddkappa: 0
   }
-  v: 5.4128416675182471
-  a: -0.25236375898803609
+  v: 5.41226139273412
+  a: -0.25535410965418126
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587701.43126452109
-    y: 4141445.2350081671
-    theta: -0.32063863450630636
-    kappa: -0.024705420468691931
-    s: 6.5424202033381258
-    dkappa: -0.0030218244621558238
+    x: 587701.43104826147
+    y: 4141445.2342998111
+    theta: -0.32074077847422788
+    kappa: -0.024673586675753277
+    s: 6.5423969236837793
+    dkappa: -0.0030124031369671857
     ddkappa: 0
   }
-  v: 5.3843755613814563
-  a: -0.31754030187303905
+  v: 5.3834598584885347
+  a: -0.32127222303244762
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587701.939427064
-    y: 4141445.0622346462
-    theta: -0.33437637051273356
-    kappa: -0.026425073839611238
-    s: 7.0791574564312034
-    dkappa: -0.0032062864111616117
+    x: 587701.93908955657
+    y: 4141445.0615162118
+    theta: -0.33445718642692529
+    kappa: -0.026388569557595061
+    s: 7.0790226063519892
+    dkappa: -0.0031982588754284704
     ddkappa: 0
   }
-  v: 5.3492323384235165
-  a: -0.38575490636172249
+  v: 5.3479028694807909
+  a: -0.39031269581588446
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587702.44146782649
-    y: 4141444.8836176377
-    theta: -0.348951196575446
-    kappa: -0.028236948062895041
-    s: 7.6120353846879061
-    dkappa: -0.0034033083970067251
+    x: 587702.44097017928
+    y: 4141444.8829180282
+    theta: -0.34900706549530514
+    kappa: -0.028196356121092309
+    s: 7.611743312641277
+    dkappa: -0.0033966422607589475
     ddkappa: 0
   }
-  v: 5.3071535491988469
-  a: -0.45610043815836715
+  v: 5.3053233424948143
+  a: -0.46157247351563174
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587702.9365078921
-    y: 4141444.6991173392
-    theta: -0.36437913895230234
-    kappa: -0.030143598509109547
-    s: 8.1403512887355163
-    dkappa: -0.003612507606179146
+    x: 587702.93580408779
+    y: 4141444.698471528
+    theta: -0.3644063467937057
+    kappa: -0.030099422251257037
+    s: 8.1398471973200515
+    dkappa: -0.0036071597819925818
     ddkappa: 0
   }
-  v: 5.2579714576914354
-  a: -0.52766976296725321
+  v: 5.2555444997634169
+  a: -0.53414850164282923
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587703.4236534792
-    y: 4141444.5087395716
-    theta: -0.38067170238185088
-    kappa: -0.032146548718057263
-    s: 8.6633902312711886
-    dkappa: -0.0038330686590637361
+    x: 587703.42268984916
+    y: 4141444.508189403
+    theta: -0.38066629799767637
+    kappa: -0.032099192485006631
+    s: 8.66260925265245
+    dkappa: -0.0038289842472376415
     ddkappa: 0
   }
-  v: 5.2016090413148452
-  a: -0.5995557464926613
+  v: 5.1984798689682954
+  a: -0.6071377257086169
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 587703.9020077904
-    y: 4141444.3125408161
-    theta: -0.39783486829975551
-    kappa: -0.034245998947626685
-    s: 9.1804341084049188
-    dkappa: -0.0040636578290528056
+    x: 587703.90072255244
+    y: 4141444.3121359823
+    theta: -0.39779249296449809
+    kappa: -0.034195746710842863
+    s: 9.17930033894322
+    dkappa: -0.0040607695682479024
     ddkappa: 0
   }
-  v: 5.1380799909122095
-  a: -0.670851254438872
+  v: 5.1341332832400353
+  a: -0.67963709122413429
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587704.37068363454
-    y: 4141444.1106332792
-    theta: -0.41586799366870991
-    kappa: -0.036440513737384661
-    s: 9.6907707210024885
-    dkappa: -0.0043023416050068245
+    x: 587704.36900652142
+    y: 4141444.1104325019
+    theta: -0.41578367950330658
+    kappa: -0.036387506534064293
+    s: 9.6891962150826139
+    dkappa: -0.0043005703718976915
     ddkappa: 0
   }
-  v: 5.0674887107562343
-  a: -0.74064915251016539
+  v: 5.062598881158106
+  a: -0.75074354370052165
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587704.82881691039
-    y: 4141443.9031897541
-    theta: -0.43476263971463719
-    kappa: -0.038726705823117477
-    s: 10.193702846028431
-    dkappa: -0.0045465194214620759
+    x: 587704.82666892116
+    y: 4141443.9032621905
+    theta: -0.43463057522839732
+    kappa: -0.038670919131589029
+    s: 10.191586569091276
+    dkappa: -0.0045457764657677565
     ddkappa: 0
   }
-  v: 4.9900303185491977
-  a: -0.80804230641082231
+  v: 4.9840611067508638
+  a: -0.8195540286489188
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587705.27558102645
-    y: 4141443.6904480038
-    theta: -0.45450138067961549
-    kappa: -0.041098942748580607
-    s: 10.68855730788899
-    dkappa: -0.0047928836134337894
+    x: 587705.272874338
+    y: 4141443.6908749114
+    theta: -0.45431464276420674
+    kappa: -0.041040165625731685
+    s: 10.685784048665132
+    dkappa: -0.00479307547792067
     ddkappa: 0
   }
-  v: 4.90599064542295
-  a: -0.87212358184511951
+  v: 4.89879470949555
+  a: -0.88516549158046309
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587705.71020873636
-    y: 4141443.4727109792
-    theta: -0.47505698623814635
-    kappa: -0.043549150425262972
-    s: 11.174701305828037
-    dkappa: -0.0050374259699623638
+    x: 587705.706846812
+    y: 4141443.4735876471
+    theta: -0.47480723880064141
+    kappa: -0.043486969077037668
+    s: 11.171140538924405
+    dkappa: -0.00503846327772896
     ddkappa: 0
   }
-  v: 4.8160345635520425
-  a: -0.92343174141290407
+  v: 4.8074527546495744
+  a: -0.93812845977507764
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587706.13201519416
-    y: 4141443.2502011941
-    theta: -0.49641079104232638
-    kappa: -0.046068468374160045
-    s: 11.651628238643484
-    dkappa: -0.0052782570952687848
+    x: 587706.12789462518
+    y: 4141443.2516419077
+    theta: -0.496087561905966
+    kappa: -0.046002224795334751
+    s: 11.647132907669379
+    dkappa: -0.005280002055649745
     ddkappa: 0
   }
-  v: 4.7219943887615132
-  a: -0.95403715234634
+  v: 4.71185554087859
+  a: -0.97049572782800542
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587706.54052214348
-    y: 4141443.0231308537
-    theta: -0.51854042103892162
-    kappa: -0.048646086046854373
-    s: 12.119030893665453
-    dkappa: -0.0055102181604509788
+    x: 587706.53553024
+    y: 4141443.025265119
+    theta: -0.51813164336836026
+    kappa: -0.048574973145822709
+    s: 12.113436356930787
+    dkappa: -0.0055127656678847633
     ddkappa: 0
   }
-  v: 4.62587088228216
-  a: -0.96533008729819758
+  v: 4.6139951208725583
+  a: -0.98361764222337467
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587706.93558519811
-    y: 4141442.7919367766
-    theta: -0.54138599836473211
-    kappa: -0.051266403961812958
-    s: 12.576795182983879
-    dkappa: -0.0057203115830592732
+    x: 587706.92960376793
+    y: 4141442.7949161846
+    theta: -0.54087704816417537
+    kappa: -0.05118943369454286
+    s: 12.569918542275939
+    dkappa: -0.00572367214505443
     ddkappa: 0
   }
-  v: 4.5295257780795009
-  a: -0.9587008189212467
+  v: 4.5157285126730295
+  a: -0.97884454944531374
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 587707.31720002159
-    y: 4141442.5569227375
-    theta: -0.56490148042902766
-    kappa: -0.0539144464143909
-    s: 13.02498624072199
-    dkappa: -0.0059062866337522262
+    x: 587707.31010758784
+    y: 4141442.5609230129
+    theta: -0.5642749446807569
+    kappa: -0.053830521919362308
+    s: 13.01662606934388
+    dkappa: -0.0059106068995391685
     ddkappa: 0
   }
-  v: 4.4346817828537786
-  a: -0.93553961986825773
+  v: 4.4187776996731394
+  a: -0.9575267959779511
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587707.68534514168
-    y: 4141442.3180567021
-    theta: -0.58908047772946515
-    kappa: -0.056576799199147626
-    s: 13.463834520309771
-    dkappa: -0.00606261528787339
+    x: 587707.67686802521
+    y: 4141442.3230523267
+    theta: -0.58834570940582953
+    kappa: -0.05648615100173001
+    s: 13.453770990380544
+    dkappa: -0.00606732617435238
     ddkappa: 0
   }
-  v: 4.3429225760399577
-  a: -0.8972367627920006
+  v: 4.3247296306176111
+  a: -0.921014728305415
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 587708.03937193868
-    y: 4141442.0742035983
-    theta: -0.61405291649751548
-    kappa: -0.0592409236947107
-    s: 13.893721891757442
-    dkappa: -0.0061790670473028881
+    x: 587708.02950083872
+    y: 4141442.0806039758
+    theta: -0.61316263530246073
+    kappa: -0.05914090283910322
+    s: 13.881717300773925
+    dkappa: -0.0061850632320226608
     ddkappa: 0
   }
-  v: 4.2556928098077282
-  a: -0.84518252034524544
+  v: 4.2350362196027573
+  a: -0.870658692911834
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 587708.38025094732
-    y: 4141441.8264284916
-    theta: -0.63965395979221751
-    kappa: -0.061878734303382324
-    s: 14.315167738928924
-    dkappa: -0.0062498685396959264
+    x: 587708.36887151608
+    y: 4141441.8344939779
+    theta: -0.6385810380607676
+    kappa: -0.061768365033401121
+    s: 14.300967435589214
+    dkappa: -0.0062579128235991264
     ddkappa: 0
   }
-  v: 4.1742981090615
-  a: -0.78076716518076239
+  v: 4.151014346076475
+  a: -0.80780903628133593
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 587708.70866035251
-    y: 4141441.5750070317
-    theta: -0.665817871923231
-    kappa: -0.064471050126541765
-    s: 14.72881505681532
-    dkappa: -0.0062679716862770341
+    x: 587708.69565668469
+    y: 4141441.5850099605
+    theta: -0.66453462171061262
+    kappa: -0.064349898692966673
+    s: 14.712148766103986
+    dkappa: -0.00627919657819459
     ddkappa: 0
   }
-  v: 4.0999050714404071
-  a: -0.7053809699513216
+  v: 4.0738458548382495
+  a: -0.73381610489804983
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 587709.02523451787
-    y: 4141441.3199316002
-    theta: -0.6925140288936148
-    kappa: -0.067001014733186648
-    s: 15.135416548808379
-    dkappa: -0.0062298758251807182
+    x: 587709.01051862584
+    y: 4141441.3321893043
+    theta: -0.69098816997197154
+    kappa: -0.066869390295571526
+    s: 15.116000096343335
+    dkappa: -0.0062455862106993131
     ddkappa: 0
   }
-  v: 4.0335412673183084
-  a: -0.62041420730969321
+  v: 4.0045775560391537
+  a: -0.65003024524610364
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587709.330571604
-    y: 4141441.06096035
-    theta: -0.719740182692806
-    kappa: -0.069453273717481792
-    s: 15.535820723973973
-    dkappa: -0.0061314948267917743
+    x: 587709.31409986119
+    y: 4141441.075842422
+    theta: -0.71793491345114668
+    kappa: -0.0693129286881272
+    s: 15.513358159615041
+    dkappa: -0.0061541480723456641
     ddkappa: 0
   }
-  v: 3.9760952398037825
-  a: -0.52725714990864714
+  v: 3.9441212251818465
+  a: -0.557801803809626
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587709.62410984992
-    y: 4141440.7964442628
-    theta: -0.74765119557523207
-    kappa: -0.071797427777168729
-    s: 15.930957994325571
-    dkappa: -0.00595168048562401
+    x: 587709.60521112941
+    y: 4141440.8136393242
+    theta: -0.74560692850221322
+    kappa: -0.0716424839219891
+    s: 15.905144115044738
+    dkappa: -0.005976655318354427
     ddkappa: 0
   }
-  v: 3.9283165047401338
-  a: -0.42730007040095358
+  v: 3.8932536031205753
+  a: -0.45848112707274485
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587709.90612289647
-    y: 4141440.5258358149
-    theta: -0.776239439381122
-    kappa: -0.073996697646448881
-    s: 16.3218267720977
-    dkappa: -0.0056932317188660188
+    x: 587709.88526706747
+    y: 4141440.5462997509
+    theta: -0.77385404791941392
+    kappa: -0.073835591081784085
+    s: 16.292350044111053
+    dkappa: -0.0057274242773030608
     ddkappa: 0
   }
-  v: 3.8908155507053879
-  a: -0.32193324143938284
+  v: 3.8526163960611735
+  a: -0.35341856151958895
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587710.17848047777
-    y: 4141440.2500731023
-    theta: -0.80534909958040146
-    kappa: -0.076046744656930565
-    s: 16.709479567019443
-    dkappa: -0.0053559250735115641
+    x: 587710.155718396
+    y: 4141440.2742508817
+    theta: -0.80258363300889568
+    kappa: -0.075885458120179439
+    s: 16.676025447180781
+    dkappa: -0.0054034305094647706
     ddkappa: 0
   }
-  v: 3.8640638390122941
-  a: -0.21254693567670502
+  v: 3.8227162755610626
+  a: -0.24396445363428632
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 587710.44183886913
-    y: 4141439.968600255
-    theta: -0.83499979577012817
-    kappa: -0.077934713149321738
-    s: 17.095009083587872
-    dkappa: -0.004942587814720594
+    x: 587710.41724006855
+    y: 4141439.9969221782
+    theta: -0.8318216654179682
+    kappa: -0.07778066510539268
+    s: 17.057263740044043
+    dkappa: -0.0050043954532351127
     ddkappa: 0
   }
-  v: 3.8483938037083245
-  a: -0.10053142576568985
+  v: 3.80392487852925
+  a: -0.13146914990096548
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587710.69682099181
-    y: 4141439.6807816364
-    theta: -0.86521999685720885
-    kappa: -0.079647884080822892
-    s: 17.479534318341557
-    dkappa: -0.0044585891544597763
+    x: 587710.66945165815
+    y: 4141439.7127878303
+    theta: -0.86167254933976467
+    kappa: -0.079483969362384679
+    s: 17.437188750449426
+    dkappa: -0.0045270126289032146
     ddkappa: 0
   }
-  v: 3.8439988515756749
-  a: 0.012723015640892177
+  v: 3.7964788072263325
+  a: -0.017282996803755224
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587710.940656747
-    y: 4141439.383289542
-    theta: -0.89621132393323
-    kappa: -0.081077291666733761
-    s: 17.864186657134024
-    dkappa: -0.0038786409770222682
+    x: 587710.91128166264
+    y: 4141439.42002206
+    theta: -0.89220635360346445
+    kappa: -0.080926109820862013
+    s: 17.816941214639172
+    dkappa: -0.0039622576348745074
     ddkappa: 0
   }
-  v: 3.8509333621312627
-  a: 0.12582611589027126
+  v: 3.8004796292644913
+  a: 0.097243659173216757
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587711.17615755834
-    y: 4141439.0776539631
-    theta: -0.92776875259843183
-    kappa: -0.082255362287508
-    s: 18.250095972407212
-    dkappa: -0.0032038595356461749
+    x: 587711.14527323
+    y: 4141439.1197925727
+    theta: -0.9232678225002201
+    kappa: -0.082137372489518773
+    s: 18.197665273884319
+    dkappa: -0.003311772191226298
     ddkappa: 0
   }
-  v: 3.86911268762673
-  a: 0.23738760232967704
+  v: 3.8158938776074969
+  a: 0.21076047154582211
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587711.40414011711
-    y: 4141438.763445708
-    theta: -0.95988778774967143
-    kappa: -0.0831793229782415
-    s: 18.638376720464983
-    dkappa: -0.0024724237360333507
+    x: 587711.37191206752
+    y: 4141438.8113356661
+    theta: -0.9548858383701061
+    kappa: -0.083106113442251439
+    s: 18.580494971019846
+    dkappa: -0.0025950968095558281
     ddkappa: 0
   }
-  v: 3.8983131530484405
-  a: 0.34601720230633903
+  v: 3.8425530505707064
+  a: 0.32191709382993217
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 587711.62476949207
-    y: 4141438.4398042457
-    theta: -0.99258261325810115
-    kappa: -0.083828899886648475
-    s: 19.030114038746554
-    dkappa: -0.0016945224949421636
+    x: 587711.59149268374
+    y: 4141438.4938207939
+    theta: -0.98708168229117221
+    kappa: -0.083814908205522634
+    s: 18.966540746979859
+    dkappa: -0.0018455094575894812
     ddkappa: 0
   }
-  v: 3.9381720561174829
-  a: 0.45032464316749404
+  v: 3.880153611821064
+  a: 0.42936317954141656
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587711.8356784475
-    y: 4141438.1043652152
-    theta: -1.0258783995295491
-    kappa: -0.084107098934728453
-    s: 19.426348730028803
-    dkappa: -0.00080335329923754374
+    x: 587711.79949243111
+    y: 4141438.1635286468
+    theta: -1.0198925987677891
+    kappa: -0.084097885519287846
+    s: 19.356874408710031
+    dkappa: -0.00096837550832091688
     ddkappa: 0
   }
-  v: 3.9881445894750933
-  a: 0.547699569468744
+  v: 3.9281972693555773
+  a: 0.53002794589785118
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587712.03709570307
-    y: 4141437.7568435189
-    theta: -1.0596594019273073
-    kappa: -0.084004993470252351
-    s: 19.828053427969341
-    dkappa: 6.647982262350657e-05
+    x: 587712.00034826377
+    y: 4141437.8227583375
+    theta: -1.0531724319221785
+    kappa: -0.084075973661144582
+    s: 19.75250108907203
+    dkappa: -0.00010438714083189473
     ddkappa: 0
   }
-  v: 4.0474324085553572
-  a: 0.63668765209375422
+  v: 3.9858687679146336
+  a: 0.621979719674283
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 587712.23008099524
-    y: 4141437.39739518
-    theta: -1.0938487357811182
-    kappa: -0.083549403633678326
-    s: 20.236118299866789
-    dkappa: 0.00098033838636306186
+    x: 587712.19337540632
+    y: 4141437.4704219252
+    theta: -1.0868954447537897
+    kappa: -0.083719030825335813
+    s: 20.154340604401831
+    dkappa: 0.00078606765587687592
     ddkappa: 0
   }
-  v: 4.1152140173326126
-  a: 0.71763265788182129
+  v: 4.0523147241328363
+  a: 0.70557681872279732
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587712.41436347144
-    y: 4141437.0253872937
-    theta: -1.1283875251250071
-    kappa: -0.082731853099832386
-    s: 20.651353082251386
-    dkappa: 0.0018924309855139366
+    x: 587712.37799448892
+    y: 4141437.1056516715
+    theta: -1.1210114567389082
+    kappa: -0.083005504587911588
+    s: 20.563229224288129
+    dkappa: 0.0017022834502088947
     ddkappa: 0
   }
-  v: 4.1907022964651315
-  a: 0.79087835367224157
+  v: 4.1267175864299981
+  a: 0.78117756089547929
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587712.58948939131
-    y: 4141436.6402649162
-    theta: -1.1631855586555726
-    kappa: -0.081562350486449678
-    s: 21.074490518553414
-    dkappa: 0.0027321802722299871
+    x: 587712.55401390733
+    y: 4141436.7279683654
+    theta: -1.1554443017381848
+    kappa: -0.081934768373199726
+    s: 20.979923254750869
+    dkappa: 0.0025665244839557016
     ddkappa: 0
   }
-  v: 4.2731445032951152
-  a: 0.85676850630431167
+  v: 4.20829563501114
+  a: 0.84914026404441489
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587712.7544773065
-    y: 4141436.2413654285
-    theta: -1.19812380748875
-    kappa: -0.080076459252113874
-    s: 21.506189796771547
-    dkappa: 0.0034363116302225574
+    x: 587712.71783480817
+    y: 4141436.33561622
+    theta: -1.1899406518889357
+    kappa: -0.080491141097564889
+    s: 21.405102621419733
+    dkappa: 0.0032846215438874348
     ddkappa: 0
   }
-  v: 4.3618222718486921
-  a: 0.91564688261732818
+  v: 4.296302981866492
+  a: 0.90982324602168885
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587712.90794359369
-    y: 4141435.8280895054
-    theta: -1.2330762080117772
-    kappa: -0.078355721031140177
-    s: 21.947039987141288
-    dkappa: 0.0038797198357334579
+    x: 587712.87145884312
+    y: 4141435.9294720064
+    theta: -1.2245117319187946
+    kappa: -0.078830907867660338
+    s: 21.839374452712718
+    dkappa: 0.0037669146250623388
     ddkappa: 0
   }
-  v: 4.456051612835922
-  a: 0.96785724945058726
+  v: 4.3900295707714916
+  a: 0.96358482467938689
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 587713.04916506726
-    y: 4141435.4002893791
-    theta: -1.2678840782857883
-    kappa: -0.076261071622458529
-    s: 22.397563479803331
-    dkappa: 0.00457579209372711
+    x: 587713.01455929957
+    y: 4141435.50934125
+    theta: -1.2590623330388171
+    kappa: -0.076853658806923464
+    s: 22.283276663014611
+    dkappa: 0.0043921031310770993
     ddkappa: 0
   }
-  v: 4.5551829136507935
-  a: 1.0137433736433858
+  v: 4.4888011772867857
+  a: 1.0107833178695942
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 587713.1783062066
-    y: 4141434.9581535575
-    theta: -1.3024247669322506
-    kappa: -0.073839529643995427
-    s: 22.858219422471979
-    dkappa: 0.0052123450578930863
+    x: 587713.14610791439
+    y: 4141435.0748988544
+    theta: -1.293426777122866
+    kappa: -0.074545908968146
+    s: 22.737281535855519
+    dkappa: 0.0050467672010195731
     ddkappa: 0
   }
-  v: 4.6586009383712259
-  a: 1.05364902203502
+  v: 4.5919794087582293
+  a: 1.0517770434443963
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587713.29526149225
-    y: 4141434.5017674817
-    theta: -1.3365786325169124
-    kappa: -0.071132591720142524
-    s: 23.329407158103507
-    dkappa: 0.0057230427080221958
+    x: 587713.26561390958
+    y: 4141434.626093369
+    theta: -1.327459963053232
+    kappa: -0.071939471229927748
+    s: 23.201799307089402
+    dkappa: 0.0055933658050671548
     ddkappa: 0
   }
-  v: 4.765724827759068
-  a: 1.0879179614647869
+  v: 4.6989617043168872
+  a: 1.0869243192558786
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 587713.39905145194
-    y: 4141434.0310760536
-    theta: -1.3701520080454472
-    kappa: -0.068175993206596508
-    s: 23.81146966256458
-    dkappa: 0.0061236588787510543
+    x: 587713.37245860416
+    y: 4141434.1629504673
+    theta: -1.3609962623037031
+    kappa: -0.069073364262378784
+    s: 23.677181748072581
+    dkappa: 0.0060209212534102947
     ddkappa: 0
   }
-  v: 4.8760080992600976
-  a: 1.1168939587719824
+  v: 4.80918133487903
+  a: 1.116583463156126
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 587713.48917940352
-    y: 4141433.5462148846
-    theta: -1.4029908169975385
-    kappa: -0.065016439576932622
-    s: 24.304696982300637
-    dkappa: 0.0064037437357555814
+    x: 587713.46582227584
+    y: 4141433.68551548
+    theta: -1.3938538451131604
+    kappa: -0.065988783039663809
+    s: 24.163725748842275
+    dkappa: 0.0063372376809766292
     ddkappa: 0
   }
-  v: 4.98893864700402
-  a: 1.140920780795903
+  v: 4.9221074031461391
+  a: 1.1411127929972247
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 587713.56551568978
-    y: 4141433.0474446621
-    theta: -1.4349797753187674
-    kappa: -0.061705263866623353
-    s: 24.809329672004282
-    dkappa: 0.0065623193730006118
+    x: 587713.54552741011
+    y: 4141433.1940437644
+    theta: -1.4259087060338294
+    kappa: -0.0627372210627664
+    s: 24.6616769012951
+    dkappa: 0.0065297073903368662
     ddkappa: 0
   }
-  v: 5.1040387418044775
-  a: 1.1603421943758458
+  v: 5.0372448436049053
+  a: 1.1608706266312594
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 587713.6275707318
-    y: 4141432.535007427
-    theta: -1.4659736057810793
-    kappa: -0.058291885239402923
-    s: 25.325562232283684
-    dkappa: 0.0066124926930523163
+    x: 587713.61124736536
+    y: 4141432.6888010493
+    theta: -1.4570300051848049
+    kappa: -0.059368749848728058
+    s: 25.171233082365607
+    dkappa: 0.006610744577539825
     ddkappa: 0
   }
-  v: 5.2208650311590334
-  a: 1.175501966351107
+  v: 5.1541344225272248
+  a: 1.1762152819103153
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 587713.67494267062
-    y: 4141432.0091969762
-    theta: -1.4958418619094509
-    kappa: -0.054825115156267527
-    s: 25.853546547330957
-    dkappa: 0.0065631474849310312
+    x: 587713.66245004314
+    y: 4141432.1700610826
+    theta: -1.4870714934976557
+    kappa: -0.0559319873352931
+    s: 25.6925480372048
+    dkappa: 0.0065912162391768281
     ddkappa: 0
   }
-  v: 5.3390085392491855
-  a: 1.1867438635609833
+  v: 5.2723527379702047
+  a: 1.1875050766864785
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 587713.70782046043
-    y: 4141431.4703827039
-    theta: -1.5245302985022606
-    kappa: -0.051348925068822396
-    s: 26.393395322590571
-    dkappa: 0.006430577039534327
+    x: 587713.69913960923
+    y: 4141431.6381856622
+    theta: -1.5159537348221264
+    kappa: -0.052472775877105471
+    s: 26.225734962358647
+    dkappa: 0.00648346141922302
     ddkappa: 0
   }
-  v: 5.45809466694036
-  a: 1.1944116528447706
+  v: 5.3915122197761587
+  a: 1.1950983288118338
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 587713.72627664555
-    y: 4141430.91892197
-    theta: -1.5519824267154267
-    kappa: -0.04790162675287167
-    s: 26.945185522427725
-    dkappa: 0.00623059911225558
+    x: 587713.72149451694
+    y: 4141431.093552073
+    theta: -1.5436287214719195
+    kappa: -0.049030650121437244
+    s: 26.770870088946619
+    dkappa: 0.00630490652017684
     ddkappa: 0
   }
-  v: 5.5777831917819123
-  a: 1.1988491010417659
+  v: 5.5112611295726133
+  a: 1.1993533561384668
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 587713.72943483968
-    y: 4141430.3551714267
-    theta: -1.5780270979750906
-    kappa: -0.044526548083776242
-    s: 27.508961807796755
-    dkappa: 0.0059747960080886618
+    x: 587713.72952725575
+    y: 4141430.5365199391
+    theta: -1.5700389498991312
+    kappa: -0.045639885679511161
+    s: 27.327996265840181
+    dkappa: 0.0060695877606638821
     ddkappa: 0
   }
-  v: 5.6977682680071284
-  a: 1.2003999749912653
+  v: 5.6312835607722969
+  a: 1.2006284765184629
   relative_time: 5.7999999999999963
 }
 trajectory_point {
   path_point {
-    x: 587713.71752245422
-    y: 4141429.7795372573
-    theta: -1.6026377491809813
-    kappa: -0.041253822723926033
-    s: 28.084739973909524
-    dkappa: 0.005675646555652435
+    x: 587713.72320795571
+    y: 4141429.9674520036
+    theta: -1.5951280231026796
+    kappa: -0.04233073216348731
+    s: 27.897126542841352
+    dkappa: 0.0057878237418438668
     ddkappa: 0
   }
-  v: 5.8177784265332244
-  a: 1.1994080415325659
+  v: 5.7512994385731533
+  a: 1.1992820078039077
   relative_time: 5.8999999999999959
 }
 trajectory_point {
   path_point {
-    x: 587713.6914751837
-    y: 4141429.1923696054
-    theta: -1.625892780038489
-    kappa: -0.038097129903090443
-    s: 28.672510387903813
-    dkappa: 0.0053537312119157649
+    x: 587713.70253218524
+    y: 4141429.3867150694
+    theta: -1.618847762478635
+    kappa: -0.039129161448252611
+    s: 28.478247753861194
+    dkappa: 0.0054751770280285546
     ddkappa: 0
   }
-  v: 5.9375765749613443
-  a: 1.1962170675049639
+  v: 5.8710645199583293
+  a: 1.1956722678468856
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 587713.65175029484
-    y: 4141428.5939806136
-    theta: -1.6478170789255848
-    kappa: -0.035068992679294836
-    s: 29.272241426511712
-    dkappa: 0.0050209895441164967
+    x: 587713.66665530915
+    y: 4141428.7947390168
+    theta: -1.6410370096905469
+    kappa: -0.0360703601111142
+    s: 29.071324100098341
+    dkappa: 0.0051485778747803633
     ddkappa: 0
   }
-  v: 6.0569599975765627
-  a: 1.1911708197477555
+  v: 5.9903703936961836
+  a: 1.190157574499483
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 587713.59869874222
-    y: 4141427.9846635824
-    theta: -1.6684266095724094
-    kappa: -0.032179003626227075
-    s: 29.883882913728009
-    dkappa: 0.0046831674923239362
+    x: 587713.61743275868
+    y: 4141428.1917879926
+    theta: -1.6619110607975691
+    kappa: -0.033145460433274911
+    s: 29.676300733217534
+    dkappa: 0.004814968639246831
     ddkappa: 0
   }
-  v: 6.1757603553478839
-  a: 1.1846130651002376
+  v: 6.1090444803402821
+  a: 1.1830962456137846
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
-    x: 587713.53213105188
-    y: 4141427.3647552859
-    theta: -1.6876631392779904
-    kappa: -0.029443040526659733
-    s: 30.507369558478604
-    dkappa: 0.0043478986150012021
+    x: 587713.5552480109
+    y: 4141427.5781435231
+    theta: -1.6814918001893278
+    kappa: -0.030360901077842478
+    s: 30.293107338528131
+    dkappa: 0.0044815978467479187
     ddkappa: 0
   }
-  v: 6.2938436859282421
-  a: 1.176887570401707
+  v: 6.2269500322293991
+  a: 1.1748465990418768
   relative_time: 6.2999999999999945
 }
 trajectory_point {
   path_point {
-    x: 587713.45182271721
-    y: 4141426.7346092574
-    theta: -1.7054625789153692
-    kappa: -0.026876219389895609
-    s: 31.142624392288869
-    dkappa: 0.0040221058587647787
+    x: 587713.48051783314
+    y: 4141426.9540612823
+    theta: -1.6998089128753002
+    kappa: -0.02771974014083085
+    s: 30.921661718162618
+    dkappa: 0.0041540863956878709
     ddkappa: 0
   }
-  v: 6.4111104036545008
-  a: 1.1683381024914596
+  v: 6.3439861334875189
+  a: 1.1657669526358434
   relative_time: 6.3999999999999941
 }
 trajectory_point {
   path_point {
-    x: 587713.35955843353
-    y: 4141426.0942978533
-    theta: -1.7220549589574776
-    kappa: -0.024454919356065235
-    s: 31.78956220695207
-    dkappa: 0.0037080922559099365
+    x: 587713.39249725768
+    y: 4141426.3199412543
+    theta: -1.7167219281279054
+    kappa: -0.025244401300968354
+    s: 31.561873374255143
+    dkappa: 0.0038388105034905112
     ddkappa: 0
   }
-  v: 6.5274952995474536
-  a: 1.1593084282087924
+  v: 6.4600877000238324
+  a: 1.156215624247771
   relative_time: 6.4999999999999938
 }
 trajectory_point {
   path_point {
-    x: 587713.255804637
-    y: 4141425.4440011173
-    theta: -1.7374846054929136
-    kappa: -0.022176375796673056
-    s: 32.448092992197751
-    dkappa: 0.0034081250497264913
+    x: 587713.29232351528
+    y: 4141425.6759225018
+    theta: -1.7323666038439873
+    kappa: -0.022921751122527232
+    s: 32.213647092120041
+    dkappa: 0.0035380647774258612
     ddkappa: 0
   }
-  v: 6.6429675413118208
-  a: 1.1501423143930014
+  v: 6.57522547953274
+  a: 1.1465509317297444
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
-    x: 587713.13951603509
-    y: 4141424.7841451112
-    theta: -1.7515668728607234
-    kappa: -0.020066534945765588
-    s: 33.118125373360122
-    dkappa: 0.0031274710505856769
+    x: 587713.18110509508
+    y: 4141425.0220842878
+    theta: -1.7468806485067818
+    kappa: -0.020736540641695077
+    s: 32.876886523430329
+    dkappa: 0.0032523479841988544
     ddkappa: 0
   }
-  v: 6.7575306733362588
-  a: 1.1411835278833833
+  v: 6.6894060514938518
+  a: 1.137131192933849
   relative_time: 6.6999999999999931
 }
 trajectory_point {
   path_point {
-    x: 587713.01257425651
-    y: 4141424.1146363951
-    theta: -1.7645583905947821
-    kappa: -0.01808926136834859
-    s: 33.799570049046437
-    dkappa: 0.0028700804623931248
+    x: 587713.05853069876
+    y: 4141424.3587108473
+    theta: -1.7601868875549191
+    kappa: -0.018698989558446337
+    s: 33.551497769396278
+    dkappa: 0.0029857647522931389
     ddkappa: 0
   }
-  v: 6.8712226166933457
-  a: 1.1327758355192348
+  v: 6.8026718271719835
+  a: 1.1283147257121702
   relative_time: 6.7999999999999927
 }
 trajectory_point {
   path_point {
-    x: 587712.87537053868
-    y: 4141423.435592154
-    theta: -1.7764877960868604
-    kappa: -0.016232445975670116
-    s: 34.492343228805446
-    dkappa: 0.0026383078067755287
+    x: 587712.92497970932
+    y: 4141423.6859497624
+    theta: -1.7723090972893369
+    kappa: -0.016800277305210533
+    s: 34.237392963943861
+    dkappa: 0.0027435386824010179
     ddkappa: 0
   }
-  v: 6.9841156691395962
-  a: 1.1252630041398524
+  v: 6.915101049617161
+  a: 1.1204598479167938
   relative_time: 6.8999999999999924
 }
 trajectory_point {
   path_point {
-    x: 587712.7273842725
-    y: 4141422.7472987669
-    theta: -1.7872307404611878
-    kappa: -0.014507861936339121
-    s: 35.196370070795687
-    dkappa: 0.0024309717025613373
+    x: 587712.78187970677
+    y: 4141423.0036997511
+    theta: -1.7834376549423816
+    kappa: -0.015014406455586408
+    s: 34.934493856893354
+    dkappa: 0.0025209360840180374
     ddkappa: 0
   }
-  v: 7.0963165051154506
-  a: 1.1189888005845325
+  v: 7.0268077936646209
+  a: 1.1139248773998049
   relative_time: 6.999999999999992
 }
 trajectory_point {
   path_point {
-    x: 587712.57053764025
-    y: 4141422.0494948626
-    theta: -1.7970591062887713
-    kappa: -0.012881971641842814
-    s: 35.911588119454009
-    dkappa: 0.0022382114517837965
+    x: 587712.62859286112
+    y: 4141422.3122497862
+    theta: -1.7934261748071345
+    kappa: -0.013357954147042491
+    s: 35.642735397137777
+    dkappa: 0.0023180204179871744
     ddkappa: 0
   }
-  v: 7.20796617574528
-  a: 1.1142969916925713
+  v: 7.1379419659348047
+  a: 1.1090681320132882
   relative_time: 7.0999999999999917
 }
 trajectory_point {
   path_point {
-    x: 587712.40429520188
-    y: 4141421.3424158976
-    theta: -1.8058371914234077
-    kappa: -0.011368919386260813
-    s: 36.637950743163834
-    dkappa: 0.002061281025773506
+    x: 587712.46664726886
+    y: 4141421.611386118
+    theta: -1.8024903498680291
+    kappa: -0.011804235119090019
+    s: 36.362069315821493
+    dkappa: 0.0021293351102371011
     ddkappa: 0
   }
-  v: 7.3192401088373851
-  a: 1.1115313443032659
+  v: 7.2486893048333636
+  a: 1.1062479296093306
   relative_time: 7.1999999999999913
 }
 trajectory_point {
   path_point {
-    x: 587712.22973717016
-    y: 4141420.6258952641
-    theta: -1.8137049188413419
-    kappa: -0.00995155031901331
-    s: 37.375430571923644
-    dkappa: 0.0018920128172985749
+    x: 587712.29601142614
+    y: 4141420.9012031285
+    theta: -1.8105796850444851
+    kappa: -0.010358652875324368
+    s: 37.092467709518665
+    dkappa: 0.0019524126337647249
     ddkappa: 0
   }
-  v: 7.4303481088839982
-  a: 1.1110356252559119
+  v: 7.3592713805511574
+  a: 1.105822588040015
   relative_time: 7.2999999999999909
 }
 trajectory_point {
   path_point {
-    x: 587712.04700671451
-    y: 4141419.8999501434
-    theta: -1.820633805691652
-    kappa: -0.0086322261594193248
-    s: 38.124022935015333
-    dkappa: 0.0017400403700725453
+    x: 587712.1172057651
+    y: 4141420.1816291227
+    theta: -1.8177401204919563
+    kappa: -0.0090156296606823273
+    s: 37.8339266234118
+    dkappa: 0.0017861200989906915
     ddkappa: 0
   }
-  v: 7.5415343570612761
-  a: 1.1131536013898069
+  v: 7.4699455950642557
+  a: 1.1081504251574301
   relative_time: 7.3999999999999906
 }
 trajectory_point {
   path_point {
-    x: 587711.85696814943
-    y: 4141419.1643774188
-    theta: -1.8267389141942925
-    kappa: -0.0073976452368642153
-    s: 38.883749298672605
-    dkappa: 0.0015993525508491272
+    x: 587711.93092583539
+    y: 4141419.4525080575
+    theta: -1.8240536668700644
+    kappa: -0.0077658997676191216
+    s: 38.586469634470291
+    dkappa: 0.0016353348382891462
     ddkappa: 0
   }
-  v: 7.65307741122931
-  a: 1.1182290395442469
+  v: 7.5810051821339366
+  a: 1.1135897588136581
   relative_time: 7.49999999999999
 }
 trajectory_point {
   path_point {
-    x: 587711.65971358086
-    y: 4141418.4191307104
-    theta: -1.8319742954293687
-    kappa: -0.0062538695654560349
-    s: 39.654660703749371
-    dkappa: 0.001462792065723582
+    x: 587711.73751302436
+    y: 4141418.7137256842
+    theta: -1.8295396133809549
+    kappa: -0.0066083583780596434
+    s: 39.35015143462892
+    dkappa: 0.0014908167056968461
     ddkappa: 0
   }
-  v: 7.7652902059321187
-  a: 1.1266057065585284
+  v: 7.6927792073066845
+  a: 1.1224989068607867
   relative_time: 7.59999999999999
 }
 trajectory_point {
   path_point {
-    x: 587711.45609366137
-    y: 4141417.6639194326
-    theta: -1.8364684575087968
-    kappa: -0.0051876132861280178
-    s: 40.436841203388134
-    dkappa: 0.0013368017644781655
+    x: 587711.537383268
+    y: 4141417.9651058656
+    theta: -1.8342280649198741
+    kappa: -0.0055417915212550711
+    s: 40.12506141396635
+    dkappa: 0.0013536545437224097
     ddkappa: 0
   }
-  v: 7.8785200523976524
-  a: 1.138627369271948
+  v: 7.8056325679141914
+  a: 1.1352361871508994
   relative_time: 7.6999999999999895
 }
 trajectory_point {
   path_point {
-    x: 587711.24618374114
-    y: 4141416.8986153649
-    theta: -1.8401663655086455
-    kappa: -0.0042079809647894272
-    s: 41.2304113006884
-    dkappa: 0.0012165806399686457
+    x: 587711.3311727111
+    y: 4141417.2063633474
+    theta: -1.8382160144976609
+    kappa: -0.0045571940507628476
+    s: 40.911327243883711
+    dkappa: 0.0012265701521810167
     ddkappa: 0
   }
-  v: 7.9931486385377912
-  a: 1.154637794523802
+  v: 7.919965993073367
+  a: 1.1521599175360828
   relative_time: 7.7999999999999892
 }
 trajectory_point {
   path_point {
-    x: 587711.03074202058
-    y: 4141416.1228560894
-    theta: -1.84319710344462
-    kappa: -0.0033038284883224914
-    s: 42.035531386375048
-    dkappa: 0.0010980620889833959
+    x: 587711.11898039235
+    y: 4141416.4373091403
+    theta: -1.841464191363323
+    kappa: -0.0036649182957875135
+    s: 41.709118460283051
+    dkappa: 0.0010987401624607341
     ddkappa: 0
   }
-  v: 8.1095920289483381
-  a: 1.1749807491533875
+  v: 8.0362160436863128
+  a: 1.1736284158684218
   relative_time: 7.8999999999999888
 }
 trajectory_point {
   path_point {
-    x: 587710.80999191967
-    y: 4141415.3363754139
-    theta: -1.8455529080089139
-    kappa: -0.0024811058223746536
-    s: 42.852405176466746
-    dkappa: 0.00098704556145650388
+    x: 587710.90151948191
+    y: 4141415.6575324973
+    theta: -1.844115774280874
+    kappa: -0.00285167775304138
+    s: 42.518650046745918
+    dkappa: 0.00097845238257211536
     ddkappa: 0
   }
-  v: 8.2283006649090336
-  a: 1.1999999999999997
+  v: 8.154855112440357
+  a: 1.2000000000000022
   relative_time: 7.9999999999999885
 }
 decision {
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_rightturn_with_red_light_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_rightturn_with_red_light_0.pb.txt
index 6842c07399..8abec76715 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_rightturn_with_red_light_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_rightturn_with_red_light_0.pb.txt
@@ -9,1693 +9,1693 @@ trajectory_point {
     y: 4141446.9076648518
     z: 0
     theta: -0.21266574936068361
-    kappa: -0.0098341145237096049
+    kappa: -0.00991597654145587
     s: 0
     dkappa: 0
     ddkappa: 0
   }
   v: 5.4499998092651367
-  a: 0.0046257383282925762
+  a: 0.0053958329514911425
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 587695.2160292028
-    y: 4141446.8843828822
-    theta: -0.21378501028152566
-    kappa: -0.010019487609553183
-    s: 0.10900137772332054
-    dkappa: -0.000371316499045017
+    x: 587695.2160289014
+    y: 4141446.8843807871
+    theta: -0.2137932436838563
+    kappa: -0.010098603574803173
+    s: 0.10900153084908426
+    dkappa: -0.00036581620037551353
     ddkappa: 0
   }
-  v: 5.4501603208811105
-  a: 0.011333203537609214
+  v: 5.4501755876105555
+  a: 0.01208954353824283
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 587695.32251952926
-    y: 4141446.8610999486
-    theta: -0.21490432118122893
-    kappa: -0.010204868373756358
-    s: 0.21800727040506107
-    dkappa: -0.00074264837843282289
+    x: 587695.32251922193
+    y: 4141446.861095693
+    theta: -0.21492079153617391
+    kappa: -0.010281238679559226
+    s: 0.21800787914438069
+    dkappa: -0.00073164856841266568
     ddkappa: 0
   }
-  v: 5.4504494728076294
-  a: 0.017490978895199009
+  v: 5.4504797167660337
+  a: 0.018232118030045711
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 587695.42901667312
-    y: 4141446.8378155245
-    theta: -0.21602370454997985
-    kappa: -0.010390261005616366
-    s: 0.32702014139726709
-    dkappa: -0.00111400402964687
+    x: 587695.42901664926
+    y: 4141446.8378090453
+    theta: -0.21604841546860251
+    kappa: -0.010463885972767512
+    s: 0.32702150215650044
+    dkappa: -0.0010975053508708481
     ddkappa: 0
   }
-  v: 5.450856343818403
-  a: 0.023106321475427724
+  v: 5.4509012462540305
+  a: 0.023830780862810663
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 587695.53552282869
-    y: 4141446.8145291302
-    theta: -0.2171431806489986
-    kappa: -0.010575669325435329
-    s: 0.43604223707887324
-    dkappa: -0.0014853911050451282
+    x: 587695.53552337142
+    y: 4141446.8145203646
+    theta: -0.21717613578056758
+    kappa: -0.010646549206951959
+    s: 0.43604463959139128
+    dkappa: -0.0014633940643483455
     ddkappa: 0
   }
-  v: 5.4513701578286264
-  a: 0.028186488352661111
+  v: 5.4514293700857248
+  a: 0.028892756472448562
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 587695.641987365
-    y: 4141446.7910027015
-    theta: -0.21830217449180916
-    kappa: -0.010764351061706602
-    s: 0.54507558975853276
-    dkappa: -0.0017071683886577408
+    x: 587695.64198839385
+    y: 4141446.7909900211
+    theta: -0.21834281879587569
+    kappa: -0.010832544396925626
+    s: 0.54507931648011154
+    dkappa: -0.0016820901178966003
     ddkappa: 0
   }
-  v: 5.4519802838949847
-  a: 0.032738736601264924
+  v: 5.4520534267610135
+  a: 0.0334252692948703
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 587695.74839213723
-    y: 4141446.7671458675
-    theta: -0.21951563398759824
-    kappa: -0.0109575415859921
-    s: 0.65412202057744762
-    dkappa: -0.0017226757814290689
+    x: 587695.748393438
+    y: 4141446.7671270939
+    theta: -0.21956319419978104
+    kappa: -0.011023128365275789
+    s: 0.65412734606860468
+    dkappa: -0.0016978744207952925
     ddkappa: 0
   }
-  v: 5.4526762362156491
-  a: 0.03677032329560493
+  v: 5.4527628992685111
+  a: 0.037435543765986762
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 587695.85481124453
-    y: 4141446.74328582
-    theta: -0.22072926006753263
-    kappa: -0.011150758137402286
-    s: 0.7631831424121982
-    dkappa: -0.0017381852633962659
+    x: 587695.85481307679
+    y: 4141446.743260893
+    theta: -0.22078374014751684
+    kappa: -0.011213738474162615
+    s: 0.76319033270747372
+    dkappa: -0.001713660888672247
     ddkappa: 0
   }
-  v: 5.4534476741302766
-  a: 0.040288505510046868
+  v: 5.45354741508555
+  a: 0.040930804321708814
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 587695.96124606044
-    y: 4141446.7194222496
-    theta: -0.22194306481980342
-    kappa: -0.011344003209540276
-    s: 0.87226036277757246
-    dkappa: -0.0017536970347207166
+    x: 587695.96124867466
+    y: 4141446.7193911136
+    theta: -0.22200446865220658
+    kappa: -0.011404377167483047
+    s: 0.87226967474175543
+    dkappa: -0.0017294497239327866
     ddkappa: 0
   }
-  v: 5.4542844021200141
-  a: 0.0433005403189565
+  v: 5.4543967461781833
+  a: 0.043918275397947334
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 587696.06769776077
-    y: 4141446.6955548939
-    theta: -0.22315705807648098
-    kappa: -0.01153727893732571
-    s: 0.98135488672939652
-    dkappa: -0.0017692112667723038
+    x: 587696.0677013977
+    y: 4141446.6955174939
+    theta: -0.22322538945058978
+    kappa: -0.011595046534147207
+    s: 0.98136656740069472
+    dkappa: -0.0017452410995819666
     ddkappa: 0
   }
-  v: 5.4551763698074938
-  a: 0.045813684796699582
+  v: 5.4553008090011783
+  a: 0.046405181430613215
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 587696.17405014858
-    y: 4141446.6711678454
-    theta: -0.22445377972959274
-    kappa: -0.011737550467176906
-    s: 1.0904677197673642
-    dkappa: -0.0017855869026785376
+    x: 587696.1740543996
+    y: 4141446.6711214771
+    theta: -0.22452798563106965
+    kappa: -0.011792830167478514
+    s: 1.0904820056875195
+    dkappa: -0.0017618955476619142
     ddkappa: 0
   }
-  v: 5.4561136719568362
-  a: 0.047835196017641884
+  v: 5.4562496644980234
+  a: 0.048398746855617347
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 587696.28039947059
-    y: 4141446.6466810238
-    theta: -0.22576601782078271
-    kappa: -0.011939146631974484
-    s: 1.199599670737866
-    dkappa: -0.0018021244489116854
+    x: 587696.28040445677
+    y: 4141446.6466252347
+    theta: -0.22584589229480209
+    kappa: -0.011991959129774965
+    s: 1.1996167872692141
+    dkappa: -0.0017787121349951795
     ddkappa: 0
   }
-  v: 5.45708654847365
-  a: 0.049372331056149146
+  v: 5.4572335181009244
+  a: 0.049906196108870593
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587696.38676802244
-    y: 4141446.6221897737
-    theta: -0.22707849519120896
-    kappa: -0.012140779249000953
-    s: 1.3087513547368201
-    dkappa: -0.0018186649854319806
+    x: 587696.38677395135
+    y: 4141446.622124515
+    theta: -0.22716404185984035
+    kappa: -0.012191124486783098
+    s: 1.3087715153662942
+    dkappa: -0.0017955317958886162
     ddkappa: 0
   }
-  v: 5.4580853844050283
-  a: 0.050432346986587134
+  v: 5.4582427197308059
+  a: 0.050934753626283844
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 587696.4931562175
-    y: 4141446.5976940016
-    theta: -0.22839121242028929
-    kappa: -0.012342449101957365
-    s: 1.4179231960125012
-    dkappa: -0.0018352085765288033
+    x: 587696.49316328485
+    y: 4141446.5976192257
+    theta: -0.22848243471665375
+    kappa: -0.012390326989630909
+    s: 1.4179466016425817
+    dkappa: -0.0018123545937755361
     ddkappa: 0
   }
-  v: 5.4591007099395554
-  a: 0.051022500883321595
+  v: 5.4592677637973095
+  a: 0.051491643843767976
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587696.59952255851
-    y: 4141446.5730149592
-    theta: -0.22973186732598613
-    kappa: -0.012546550019578577
-    s: 1.52711543086837
-    dkappa: -0.0018520656045111598
+    x: 587696.59953073284
+    y: 4141446.5729297847
+    theta: -0.22982847067576789
+    kappa: -0.012592000805661242
+    s: 1.5271422690949781
+    dkappa: -0.0018294916694313792
     ddkappa: 0
   }
-  v: 5.460123200407299
-  a: 0.051150049820718295
+  v: 5.4602992891987965
+  a: 0.051584091197233867
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587696.7057965874
-    y: 4141446.5478506954
-    theta: -0.23114727453375011
-    kappa: -0.012757126233071858
-    s: 1.6363281105659049
-    dkappa: -0.0018697604724399706
+    x: 587696.70580561331
+    y: 4141446.5477534691
+    theta: -0.23124836292998263
+    kappa: -0.012800249629780467
+    s: 1.6363585549432389
+    dkappa: -0.0018474675932308451
     ddkappa: 0
   }
-  v: 5.4611436762798178
-  a: 0.050822250873142988
+  v: 5.4613280793223451
+  a: 0.051219320122592407
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 587696.81209038373
-    y: 4141446.5226817513
-    theta: -0.23256295076334768
-    kappa: -0.012967741614522832
-    s: 1.7455611042274282
-    dkappa: -0.0018874586316799265
+    x: 587696.81210041512
+    y: 4141446.5225724336
+    theta: -0.23266852714845337
+    kappa: -0.013008537490400074
+    s: 1.7455953135197491
+    dkappa: -0.0018654468866395235
     ddkappa: 0
   }
-  v: 5.462153103170154
-  a: 0.05004636111496142
+  v: 5.4623450620437515
+  a: 0.050404555055754474
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 587696.91840364563
-    y: 4141446.4975081971
-    theta: -0.23397888632288386
-    kappa: -0.013178395565991475
-    s: 1.8548141017389397
-    dkappa: -0.0019051600319857055
+    x: 587696.91841482138
+    y: 4141446.4973867545
+    theta: -0.23408895337039365
+    kappa: -0.013216863766555922
+    s: 1.8548522191592962
+    dkappa: -0.001883429496056132
     ddkappa: 0
   }
-  v: 5.4631425918328409
-  a: 0.048829637620539365
+  v: 5.4633413097275332
+  a: 0.049147020432630958
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 587697.02473589987
-    y: 4141446.472330146
-    theta: -0.23539506922893225
-    kappa: -0.013389087149543918
-    s: 1.964086616652944
-    dkappa: -0.0019228645945420617
+    x: 587697.02474834281
+    y: 4141446.4721965464
+    theta: -0.23550962932614911
+    kappa: -0.013425227499582436
+    s: 1.9641287690888458
+    dkappa: -0.001901415338729186
     ddkappa: 0
   }
-  v: 5.4641033981638971
-  a: 0.047179337464242563
+  v: 5.4643080392269212
+  a: 0.047453940689132723
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587697.13096669177
-    y: 4141446.4466491872
-    theta: -0.23688660638297082
-    kappa: -0.013606472701529841
-    s: 2.0733779890912816
-    dkappa: -0.0019414709208636946
+    x: 587697.1309800545
+    y: 4141446.4465017654
+    theta: -0.23700491655800007
+    kappa: -0.013640388458572352
+    s: 2.0734242863173153
+    dkappa: -0.0019203047259766033
     ddkappa: 0
   }
-  v: 5.4650269232008277
-  a: 0.045102717720436766
+  v: 5.4652366118838689
+  a: 0.045332540261170645
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587697.23716429831
-    y: 4141446.4207529863
-    theta: -0.23841018969527816
-    kappa: -0.013826711787220305
-    s: 2.1826873886479561
-    dkappa: -0.0019604606735080829
+    x: 587697.23717858514
+    y: 4141446.4205914754
+    theta: -0.23853186981173979
+    kappa: -0.013858440482271141
+    s: 2.1827379225253472
+    dkappa: -0.0019395775198309245
     ddkappa: 0
   }
-  v: 5.4659047131226268
-  a: 0.042607035463487743
+  v: 5.4661185335290448
+  a: 0.042790043584655629
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587697.34337844909
-    y: 4141446.3948527519
-    theta: -0.23993401446729004
-    kappa: -0.014046985183506572
-    s: 2.2920138172919686
-    dkappa: -0.0019794533845271184
+    x: 587697.3433937307
+    y: 4141446.394677131
+    theta: -0.24005906608596461
+    kappa: -0.014076526620415138
+    s: 2.2920686609550858
+    dkappa: -0.0019588533289320604
     ddkappa: 0
   }
-  v: 5.4667284592497749
-  a: 0.039699547767761252
+  v: 5.466945454481837
+  a: 0.039833675095498566
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 587697.44960801466
-    y: 4141446.3689487581
-    theta: -0.24145805741578932
-    kappa: -0.014267290547646867
-    s: 2.4013561122701423
-    dkappa: -0.0019984488519217474
+    x: 587697.44962434249
+    y: 4141446.3687590137
+    theta: -0.24158648174203243
+    kappa: -0.014294644514612919
+    s: 2.40141531929995
+    dkappa: -0.0019781319448307112
     ddkappa: 0
   }
-  v: 5.4674899980442406
-  a: 0.036387511707623055
+  v: 5.4677091695503526
+  a: 0.036470659229610285
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587697.55582562636
-    y: 4141446.3429360068
-    theta: -0.24299776768608058
-    kappa: -0.014489033663882789
-    s: 2.5107129490099567
-    dkappa: -0.0020176455331815855
+    x: 587697.55584284919
+    y: 4141446.3427314484
+    theta: -0.24312949141295551
+    kappa: -0.014514226243014036
+    s: 2.5107765525944088
+    dkappa: -0.0019976129661325439
     ddkappa: 0
   }
-  v: 5.46818131110948
-  a: 0.032678184357438876
+  v: 5.4684016180314163
+  a: 0.03270822042290171
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587697.66189314367
-    y: 4141446.31626288
-    theta: -0.24463424381005971
-    kappa: -0.014719593543799167
-    s: 2.6200828440223729
-    dkappa: -0.0020380845343795778
+    x: 587697.66191101749
+    y: 4141446.3160428377
+    theta: -0.24476828459958927
+    kappa: -0.014742740228816659
+    s: 2.6201508561037525
+    dkappa: -0.0020183367591437035
     ddkappa: 0
   }
-  v: 5.4687945251904342
-  a: 0.028578822791574504
+  v: 5.4690148837105692
+  a: 0.028553583111283715
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587697.767971735
-    y: 4141446.2895869678
-    theta: -0.24627090081138683
-    kappa: -0.014950177495332432
-    s: 2.7294641578046677
-    dkappa: -0.0020585256695126097
+    x: 587697.76799024944
+    y: 4141446.2893514424
+    theta: -0.24640725878220468
+    kappa: -0.014971278050426706
+    s: 2.7295365682138715
+    dkappa: -0.0020390627138095977
     ddkappa: 0
   }
-  v: 5.4693219121735339
-  a: 0.024096684084395692
+  v: 5.4695411948620745
+  a: 0.024013971730667182
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587697.87405966176
-    y: 4141446.2629087083
-    theta: -0.24790770310123178
-    kappa: -0.015180781739519722
-    s: 2.8388550977432612
-    dkappa: -0.0020789686035777885
+    x: 587697.8740787846
+    y: 4141446.2626577076
+    theta: -0.24804637795331483
+    kappa: -0.015199835914532282
+    s: 2.8389318733210289
+    dkappa: -0.0020597904861171007
     ddkappa: 0
   }
-  v: 5.4697558890866969
-  a: 0.019239025310268207
+  v: 5.4699729242489088
+  a: 0.019096610716962968
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587697.98015504
-    y: 4141446.236228575
-    theta: -0.24954461283848819
-    kappa: -0.015411402180748469
-    s: 2.9482537210165454
-    dkappa: -0.002099412973501409
+    x: 587697.98017471528
+    y: 4141446.2359621106
+    theta: -0.24968560383669819
+    kappa: -0.015428409712130978
+    s: 2.9483348047216333
+    dkappa: -0.0020805197034233077
     ddkappa: 0
   }
-  v: 5.4700890180993271
-  a: 0.014013103543557764
+  v: 5.4703025891227712
+  a: 0.013808724506081982
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587698.0861318917
-    y: 4141446.2090623192
-    theta: -0.25125158759175559
-    kappa: -0.015648597134746207
-    s: 3.0576579374977162
-    dkappa: -0.0021212272673146204
+    x: 587698.08615165867
+    y: 4141446.2087792018
+    theta: -0.25139442851064558
+    kappa: -0.015663649939412987
+    s: 3.0577432475020143
+    dkappa: -0.0021026217372683547
     ddkappa: 0
   }
-  v: 5.4703140065223179
-  a: 0.0084261758586301229
+  v: 5.4705228512240769
+  a: 0.0081575375339351
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587698.19202088751
-    y: 4141446.1815389097
-    theta: -0.25301007518335705
-    kappa: -0.015890623264033739
-    s: 3.1670655126576031
-    dkappa: -0.0021440484177094521
+    x: 587698.19204068242
+    y: 4141446.1812395318
+    theta: -0.25315424300283729
+    kappa: -0.015903771627759453
+    s: 3.1671549414281981
+    dkappa: -0.0021257291986389857
     ddkappa: 0
   }
-  v: 5.470423706808047
-  a: 0.0024854993298510696
+  v: 5.47062651678196
+  a: 0.0021502742364331775
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587698.29791083443
-    y: 4141446.1540152533
-    theta: -0.25476858622463217
-    kappa: -0.016132651567091937
-    s: 3.2764740704674953
-    dkappa: -0.0021668697730736707
+    x: 587698.29793052748
+    y: 4141446.153699649
+    theta: -0.25491407880484324
+    kappa: -0.0161438951782356
+    s: 3.2765674838356809
+    dkappa: -0.0021488368392066303
     ddkappa: 0
   }
-  v: 5.4704111165503821
-  a: -0.0038016689684136403
+  v: 5.4706065365142713
+  a: -0.0042058409505128427
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587698.40379929857
-    y: 4141446.1264919816
-    theta: -0.25652706942269682
-    kappa: -0.016374676481188462
-    s: 3.3858810963019779
-    dkappa: -0.0021896908088855989
+    x: 587698.40381873329
+    y: 4141446.1261601923
+    theta: -0.25667388412659542
+    kappa: -0.016384015011560102
+    s: 3.3859783325192003
+    dkappa: -0.0021719441220642384
     ddkappa: 0
   }
-  v: 5.4702693784846765
-  a: -0.01042807196179828
+  v: 5.4704560056275824
+  a: -0.010903583590992146
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587698.50968007208
-    y: 4141446.0989559824
-    theta: -0.25828741104596148
-    kappa: -0.016616878756133911
-    s: 3.4952839398417543
-    dkappa: -0.0022125254994285535
+    x: 587698.50969881564
+    y: 4141446.0986071229
+    theta: -0.25843565971228194
+    kappa: -0.01662432681597709
+    s: 3.4953848086225174
+    dkappa: -0.0021950660086205512
     ddkappa: 0
   }
-  v: 5.4699917804877716
-  a: -0.017386452575937066
+  v: 5.4701681638171813
+  a: -0.017935729249093851
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587698.615326519
-    y: 4141446.0705607994
-    theta: -0.26016882822800624
-    kappa: -0.016870724210958126
-    s: 3.604679817976483
-    dkappa: -0.00223626631500897
+    x: 587698.61534437374
+    y: 4141446.07019549
+    theta: -0.26031747607629463
+    kappa: -0.0168764098343544
+    s: 3.6047840995281817
+    dkappa: -0.0022190943545474131
     ddkappa: 0
   }
-  v: 5.4695717555779959
-  a: -0.024669553736464234
+  v: 5.4697363952670743
+  a: -0.025295053488907003
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587698.72096342617
-    y: 4141446.0421681814
-    theta: -0.26205009236196453
-    kappa: -0.0171245467436403
-    s: 3.7140658177076005
-    dkappa: -0.0022600049868033282
+    x: 587698.72098015074
+    y: 4141446.0417864872
+    theta: -0.26219913505160369
+    kappa: -0.017128469513733331
+    s: 3.714173261747312
+    dkappa: -0.00224312047582015
     ddkappa: 0
   }
-  v: 5.4690028819151664
-  a: -0.03227011836901407
+  v: 5.4691542286499857
+  a: -0.0329743318745208
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587698.82658785768
-    y: 4141446.0137789156
-    theta: -0.26393113784616667
-    kappa: -0.017378339300111102
-    s: 3.8234388990511574
-    dkappa: -0.0022837408550820336
+    x: 587698.82660318061
+    y: 4141446.0133809131
+    theta: -0.26408057047700778
+    kappa: -0.017380498776686241
+    s: 3.8235492238093687
+    dkappa: -0.0022671436978241822
     ddkappa: 0
   }
-  v: 5.4682788828005844
-  a: -0.040180889399220737
+  v: 5.46841533712736
+  a: -0.040966339970024288
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587698.932196758
-    y: 4141445.985393825
-    theta: -0.26581189695383178
-    kappa: -0.017632094538376338
-    s: 3.9327958979406441
-    dkappa: -0.0023074732331874103
+    x: 587698.93221037625
+    y: 4141445.9849795969
+    theta: -0.26596171404865609
+    kappa: -0.01763249025754287
+    s: 3.932908789151929
+    dkappa: -0.0022911633184698791
     ddkappa: 0
   }
-  v: 5.4673936266770422
-  a: -0.048394609752718559
+  v: 5.4675135383493583
+  a: -0.049263853339506683
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 587699.03767108463
-    y: 4141445.956590144
-    theta: -0.26775098798704983
-    kappa: -0.017891600559423345
-    s: 4.0421335291298215
-    dkappa: -0.0023320462419271068
+    x: 587699.03768229275
+    y: 4141445.9561586753
+    theta: -0.26790095669627462
+    kappa: -0.017890313079580764
+    s: 4.0422486390104577
+    dkappa: -0.0023160262959391964
     ddkappa: 0
   }
-  v: 5.4663411271288167
-  a: -0.056904022355141783
+  v: 5.46644279445486
+  a: -0.057859647547057011
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 587699.14298410038
-    y: 4141445.9272830235
-    theta: -0.26976026427222244
-    kappa: -0.018158022427299254
-    s: 4.15144838909555
-    dkappa: -0.0023576301234906841
+    x: 587699.14299277344
+    y: 4141445.9268356003
+    theta: -0.26990975882526635
+    kappa: -0.01815511385799647
+    s: 4.1515653353080895
+    dkappa: -0.0023418984098367247
     ddkappa: 0
   }
-  v: 5.4651155428816729
-  a: -0.065701870132124612
+  v: 5.4651972120714616
+  a: -0.06674649815676445
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 587699.2482717887
-    y: 4141445.8979829522
-    theta: -0.2717690707581063
-    kappa: -0.018424380220967373
-    s: 4.2607369589406234
-    dkappa: -0.00238320785215489
+    x: 587699.248277525
+    y: 4141445.8975196891
+    theta: -0.27191808356663383
+    kappa: -0.018419849940758119
+    s: 4.2608553235453908
+    dkappa: -0.0023677642027055148
     ddkappa: 0
   }
-  v: 5.4637111778028631
-  a: -0.074780896009301281
+  v: 5.4637710423154813
+  a: -0.07591718073271804
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 587699.35353065073
-    y: 4141445.8686909019
-    theta: -0.27377732448169795
-    kappa: -0.018690665090066783
-    s: 4.369995607296592
-    dkappa: -0.0024087785780398433
+    x: 587699.35353301349
+    y: 4141445.868211926
+    theta: -0.2739258473121311
+    kappa: -0.01868451244269248
+    s: 4.3701149356901494
+    dkappa: -0.0023936228064279745
     ddkappa: 0
   }
-  v: 5.4621224809011286
-  a: -0.084133842912306078
+  v: 5.4621586807919513
+  a: -0.085364470839007028
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 587699.4587570827
-    y: 4141445.8394078771
-    theta: -0.27578494049320795
-    kappa: -0.018956867917196734
-    s: 4.4792205932265974
-    dkappa: -0.0024343414256224388
+    x: 587699.4587555991
+    y: 4141445.8389133252
+    theta: -0.27593296444788051
+    kappa: -0.01894909221061274
+    s: 4.4793403930671349
+    dkappa: -0.0024194733267004953
     ddkappa: 0
   }
-  v: 5.4603440463266946
-  a: -0.093753453766773256
+  v: 5.4603546675946246
+  a: -0.095081144039720428
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 587699.56369731878
-    y: 4141445.8092525136
-    theta: -0.27791246787360924
-    kappa: -0.019235195552312841
-    s: 4.5884080691281994
-    dkappa: -0.0024616827313068261
+    x: 587699.5636910894
+    y: 4141445.8087416482
+    theta: -0.27805947329355241
+    kappa: -0.019225942210603794
+    s: 4.5885278092478838
+    dkappa: -0.0024471057398699958
     ddkappa: 0
   }
-  v: 5.4583706133712768
-  a: -0.10363247149833697
+  v: 5.4583536873059719
+  a: -0.10505997589894736
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 587699.66856625734
-    y: 4141445.7789976266
-    theta: -0.28005438215218448
-    kappa: -0.019514953878569347
-    s: 4.6975540836362093
-    dkappa: -0.0024892384274548688
+    x: 587699.6685551065
+    y: 4141445.7784719821
+    theta: -0.28020009258316403
+    kappa: -0.019504219653706134
+    s: 4.6976731929404654
+    dkappa: -0.0024749497438089408
     ddkappa: 0
   }
-  v: 5.4561970664680768
-  a: -0.11376363903263155
+  v: 5.4561505689971828
+  a: -0.11529374198077702
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 587699.77339146589
-    y: 4141445.7487553554
-    theta: -0.28219541105723916
-    kappa: -0.019794595546294182
-    s: 4.8066545845255151
-    dkappa: -0.0025167826329456186
+    x: 587699.77337480732
+    y: 4141445.7482151082
+    theta: -0.28233981498802568
+    kappa: -0.019782379494429056
+    s: 4.8067724508792606
+    dkappa: -0.0025027819806384883
     ddkappa: 0
   }
-  v: 5.4538184351917822
-  a: -0.12413969929529114
+  v: 5.4537402862281628
+  a: -0.12577521784929846
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 587699.87816895684
-    y: 4141445.7185268509
-    theta: -0.2843354535186382
-    kappa: -0.020074109917920482
-    s: 4.9157054216139144
-    dkappa: -0.0025443142999977135
+    x: 587699.87814616365
+    y: 4141445.7179721892
+    theta: -0.28447853869574585
+    kappa: -0.020060411043943877
+    s: 4.915821390714739
+    dkappa: -0.0025306013808512272
     ddkappa: 0
   }
-  v: 5.4512298942585717
-  a: -0.13475339521195009
+  v: 5.451117957047539
+  a: -0.13649717906860084
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 587699.98279678321
-    y: 4141445.6879803827
-    theta: -0.28651941557758281
-    kappa: -0.020358154496652108
-    s: 5.0247023496649428
-    dkappa: -0.0025725136952358635
+    x: 587699.98276680417
+    y: 4141445.6874103742
+    theta: -0.2866611356045643
+    kappa: -0.020343036966754047
+    s: 5.0248157239032256
+    dkappa: -0.002559091366014672
     ddkappa: 0
   }
-  v: 5.4484267635261068
-  a: -0.14559746970824264
+  v: 5.4482788439926528
+  a: -0.14745240120277328
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 587700.08715625945
-    y: 4141445.6567276055
-    theta: -0.28879992276722982
-    kappa: -0.020652180803206327
-    s: 5.1336410312907041
-    dkappa: -0.0026021769812561617
+    x: 587700.08711886476
+    y: 4141445.656144469
+    theta: -0.28893959026296784
+    kappa: -0.020635721777551855
+    s: 5.1337510685966823
+    dkappa: -0.0025890426456707037
     ddkappa: 0
   }
-  v: 5.44540450799354
-  a: -0.15666466570980303
+  v: 5.445218354089568
+  a: -0.15863365981590477
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 587700.191455697
-    y: 4141445.6254928079
-    theta: -0.29107914106695976
-    kappa: -0.020946037954703817
-    s: 5.2425170398547012
-    dkappa: -0.0026318232018142278
+    x: 587700.19141013455
+    y: 4141445.6248967787
+    theta: -0.29121674060378927
+    kappa: -0.020928236083543342
+    s: 5.2426229525324777
+    dkappa: -0.00261897647707883
     ddkappa: 0
   }
-  v: 5.4421587378015088
-  a: -0.16794772614226539
+  v: 5.4419320388530634
+  a: -0.17003373047208462
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 587700.29569077259
-    y: 4141445.594277285
-    theta: -0.29335695230250658
-    kappa: -0.021239713770572079
-    s: 5.3513258623746625
-    dkappa: -0.0026614511280546919
+    x: 587700.29563624575
+    y: 4141445.5936686117
+    theta: -0.29349246762019632
+    kappa: -0.021220567633603478
+    s: 5.3514268159231637
+    dkappa: -0.0026488916065461427
     ddkappa: 0
   }
-  v: 5.4386852082321386
-  a: -0.1794393939312641
+  v: 5.4384155942866368
+  a: -0.18164538873540195
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 587700.3998570831
-    y: 4141445.5630823555
-    theta: -0.29563323664058849
-    kappa: -0.021533195845025502
-    s: 5.4600629024253751
-    dkappa: -0.00269105950840122
+    x: 587700.3997927492
+    y: 4141445.5624613
+    theta: -0.29576665062378354
+    kappa: -0.021512703949211905
+    s: 5.4601580143462494
+    dkappa: -0.0026787867571097129
     ddkappa: 0
   }
-  v: 5.434979819709044
-  a: -0.19113241200243325
+  v: 5.4346648608825054
+  a: -0.19346141016994564
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587700.91783306818
-    y: 4141445.4020187552
-    theta: -0.30771832928210113
-    kappa: -0.023074661102311225
-    s: 6.0025050155083148
-    dkappa: -0.0028499559735264439
+    x: 587700.91770572134
+    y: 4141445.4013439412
+    theta: -0.30783816136579828
+    kappa: -0.023048142757744309
+    s: 6.002555916819075
+    dkappa: -0.0028391177982346754
     ddkappa: 0
   }
-  v: 5.4128416675182471
-  a: -0.25236375898803609
+  v: 5.41226139273412
+  a: -0.25535410965418126
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587701.43126452109
-    y: 4141445.2350081671
-    theta: -0.32063863450630636
-    kappa: -0.024705420468691931
-    s: 6.5424202033381258
-    dkappa: -0.0030218244621558238
+    x: 587701.43104826147
+    y: 4141445.2342998111
+    theta: -0.32074077847422788
+    kappa: -0.024673586675753277
+    s: 6.5423969236837793
+    dkappa: -0.0030124031369671857
     ddkappa: 0
   }
-  v: 5.3843755613814563
-  a: -0.31754030187303905
+  v: 5.3834598584885347
+  a: -0.32127222303244762
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587701.939427064
-    y: 4141445.0622346462
-    theta: -0.33437637051273356
-    kappa: -0.026425073839611238
-    s: 7.0791574564312034
-    dkappa: -0.0032062864111616117
+    x: 587701.93908955657
+    y: 4141445.0615162118
+    theta: -0.33445718642692529
+    kappa: -0.026388569557595061
+    s: 7.0790226063519892
+    dkappa: -0.0031982588754284704
     ddkappa: 0
   }
-  v: 5.3492323384235165
-  a: -0.38575490636172249
+  v: 5.3479028694807909
+  a: -0.39031269581588446
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587702.44146782649
-    y: 4141444.8836176377
-    theta: -0.348951196575446
-    kappa: -0.028236948062895041
-    s: 7.6120353846879061
-    dkappa: -0.0034033083970067251
+    x: 587702.44097017928
+    y: 4141444.8829180282
+    theta: -0.34900706549530514
+    kappa: -0.028196356121092309
+    s: 7.611743312641277
+    dkappa: -0.0033966422607589475
     ddkappa: 0
   }
-  v: 5.3071535491988469
-  a: -0.45610043815836715
+  v: 5.3053233424948143
+  a: -0.46157247351563174
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587702.9365078921
-    y: 4141444.6991173392
-    theta: -0.36437913895230234
-    kappa: -0.030143598509109547
-    s: 8.1403512887355163
-    dkappa: -0.003612507606179146
+    x: 587702.93580408779
+    y: 4141444.698471528
+    theta: -0.3644063467937057
+    kappa: -0.030099422251257037
+    s: 8.1398471973200515
+    dkappa: -0.0036071597819925818
     ddkappa: 0
   }
-  v: 5.2579714576914354
-  a: -0.52766976296725321
+  v: 5.2555444997634169
+  a: -0.53414850164282923
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587703.4236534792
-    y: 4141444.5087395716
-    theta: -0.38067170238185088
-    kappa: -0.032146548718057263
-    s: 8.6633902312711886
-    dkappa: -0.0038330686590637361
+    x: 587703.42268984916
+    y: 4141444.508189403
+    theta: -0.38066629799767637
+    kappa: -0.032099192485006631
+    s: 8.66260925265245
+    dkappa: -0.0038289842472376415
     ddkappa: 0
   }
-  v: 5.2016090413148452
-  a: -0.5995557464926613
+  v: 5.1984798689682954
+  a: -0.6071377257086169
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 587703.9020077904
-    y: 4141444.3125408161
-    theta: -0.39783486829975551
-    kappa: -0.034245998947626685
-    s: 9.1804341084049188
-    dkappa: -0.0040636578290528056
+    x: 587703.90072255244
+    y: 4141444.3121359823
+    theta: -0.39779249296449809
+    kappa: -0.034195746710842863
+    s: 9.17930033894322
+    dkappa: -0.0040607695682479024
     ddkappa: 0
   }
-  v: 5.1380799909122095
-  a: -0.670851254438872
+  v: 5.1341332832400353
+  a: -0.67963709122413429
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587704.37068363454
-    y: 4141444.1106332792
-    theta: -0.41586799366870991
-    kappa: -0.036440513737384661
-    s: 9.6907707210024885
-    dkappa: -0.0043023416050068245
+    x: 587704.36900652142
+    y: 4141444.1104325019
+    theta: -0.41578367950330658
+    kappa: -0.036387506534064293
+    s: 9.6891962150826139
+    dkappa: -0.0043005703718976915
     ddkappa: 0
   }
-  v: 5.0674887107562343
-  a: -0.74064915251016539
+  v: 5.062598881158106
+  a: -0.75074354370052165
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587704.82881691039
-    y: 4141443.9031897541
-    theta: -0.43476263971463719
-    kappa: -0.038726705823117477
-    s: 10.193702846028431
-    dkappa: -0.0045465194214620759
+    x: 587704.82666892116
+    y: 4141443.9032621905
+    theta: -0.43463057522839732
+    kappa: -0.038670919131589029
+    s: 10.191586569091276
+    dkappa: -0.0045457764657677565
     ddkappa: 0
   }
-  v: 4.9900303185491977
-  a: -0.80804230641082231
+  v: 4.9840611067508638
+  a: -0.8195540286489188
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587705.27558102645
-    y: 4141443.6904480038
-    theta: -0.45450138067961549
-    kappa: -0.041098942748580607
-    s: 10.68855730788899
-    dkappa: -0.0047928836134337894
+    x: 587705.272874338
+    y: 4141443.6908749114
+    theta: -0.45431464276420674
+    kappa: -0.041040165625731685
+    s: 10.685784048665132
+    dkappa: -0.00479307547792067
     ddkappa: 0
   }
-  v: 4.90599064542295
-  a: -0.87212358184511951
+  v: 4.89879470949555
+  a: -0.88516549158046309
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587705.71020873636
-    y: 4141443.4727109792
-    theta: -0.47505698623814635
-    kappa: -0.043549150425262972
-    s: 11.174701305828037
-    dkappa: -0.0050374259699623638
+    x: 587705.706846812
+    y: 4141443.4735876471
+    theta: -0.47480723880064141
+    kappa: -0.043486969077037668
+    s: 11.171140538924405
+    dkappa: -0.00503846327772896
     ddkappa: 0
   }
-  v: 4.8160345635520425
-  a: -0.92343174141290407
+  v: 4.8074527546495744
+  a: -0.93812845977507764
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587706.13201519416
-    y: 4141443.2502011941
-    theta: -0.49641079104232638
-    kappa: -0.046068468374160045
-    s: 11.651628238643484
-    dkappa: -0.0052782570952687848
+    x: 587706.12789462518
+    y: 4141443.2516419077
+    theta: -0.496087561905966
+    kappa: -0.046002224795334751
+    s: 11.647132907669379
+    dkappa: -0.005280002055649745
     ddkappa: 0
   }
-  v: 4.7219943887615132
-  a: -0.95403715234634
+  v: 4.71185554087859
+  a: -0.97049572782800542
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587706.54052214348
-    y: 4141443.0231308537
-    theta: -0.51854042103892162
-    kappa: -0.048646086046854373
-    s: 12.119030893665453
-    dkappa: -0.0055102181604509788
+    x: 587706.53553024
+    y: 4141443.025265119
+    theta: -0.51813164336836026
+    kappa: -0.048574973145822709
+    s: 12.113436356930787
+    dkappa: -0.0055127656678847633
     ddkappa: 0
   }
-  v: 4.62587088228216
-  a: -0.96533008729819758
+  v: 4.6139951208725583
+  a: -0.98361764222337467
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587706.93558519811
-    y: 4141442.7919367766
-    theta: -0.54138599836473211
-    kappa: -0.051266403961812958
-    s: 12.576795182983879
-    dkappa: -0.0057203115830592732
+    x: 587706.92960376793
+    y: 4141442.7949161846
+    theta: -0.54087704816417537
+    kappa: -0.05118943369454286
+    s: 12.569918542275939
+    dkappa: -0.00572367214505443
     ddkappa: 0
   }
-  v: 4.5295257780795009
-  a: -0.9587008189212467
+  v: 4.5157285126730295
+  a: -0.97884454944531374
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 587707.31720002159
-    y: 4141442.5569227375
-    theta: -0.56490148042902766
-    kappa: -0.0539144464143909
-    s: 13.02498624072199
-    dkappa: -0.0059062866337522262
+    x: 587707.31010758784
+    y: 4141442.5609230129
+    theta: -0.5642749446807569
+    kappa: -0.053830521919362308
+    s: 13.01662606934388
+    dkappa: -0.0059106068995391685
     ddkappa: 0
   }
-  v: 4.4346817828537786
-  a: -0.93553961986825773
+  v: 4.4187776996731394
+  a: -0.9575267959779511
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587707.68534514168
-    y: 4141442.3180567021
-    theta: -0.58908047772946515
-    kappa: -0.056576799199147626
-    s: 13.463834520309771
-    dkappa: -0.00606261528787339
+    x: 587707.67686802521
+    y: 4141442.3230523267
+    theta: -0.58834570940582953
+    kappa: -0.05648615100173001
+    s: 13.453770990380544
+    dkappa: -0.00606732617435238
     ddkappa: 0
   }
-  v: 4.3429225760399577
-  a: -0.8972367627920006
+  v: 4.3247296306176111
+  a: -0.921014728305415
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 587708.03937193868
-    y: 4141442.0742035983
-    theta: -0.61405291649751548
-    kappa: -0.0592409236947107
-    s: 13.893721891757442
-    dkappa: -0.0061790670473028881
+    x: 587708.02950083872
+    y: 4141442.0806039758
+    theta: -0.61316263530246073
+    kappa: -0.05914090283910322
+    s: 13.881717300773925
+    dkappa: -0.0061850632320226608
     ddkappa: 0
   }
-  v: 4.2556928098077282
-  a: -0.84518252034524544
+  v: 4.2350362196027573
+  a: -0.870658692911834
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 587708.38025094732
-    y: 4141441.8264284916
-    theta: -0.63965395979221751
-    kappa: -0.061878734303382324
-    s: 14.315167738928924
-    dkappa: -0.0062498685396959264
+    x: 587708.36887151608
+    y: 4141441.8344939779
+    theta: -0.6385810380607676
+    kappa: -0.061768365033401121
+    s: 14.300967435589214
+    dkappa: -0.0062579128235991264
     ddkappa: 0
   }
-  v: 4.1742981090615
-  a: -0.78076716518076239
+  v: 4.151014346076475
+  a: -0.80780903628133593
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 587708.70866035251
-    y: 4141441.5750070317
-    theta: -0.665817871923231
-    kappa: -0.064471050126541765
-    s: 14.72881505681532
-    dkappa: -0.0062679716862770341
+    x: 587708.69565668469
+    y: 4141441.5850099605
+    theta: -0.66453462171061262
+    kappa: -0.064349898692966673
+    s: 14.712148766103986
+    dkappa: -0.00627919657819459
     ddkappa: 0
   }
-  v: 4.0999050714404071
-  a: -0.7053809699513216
+  v: 4.0738458548382495
+  a: -0.73381610489804983
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 587709.02523451787
-    y: 4141441.3199316002
-    theta: -0.6925140288936148
-    kappa: -0.067001014733186648
-    s: 15.135416548808379
-    dkappa: -0.0062298758251807182
+    x: 587709.01051862584
+    y: 4141441.3321893043
+    theta: -0.69098816997197154
+    kappa: -0.066869390295571526
+    s: 15.116000096343335
+    dkappa: -0.0062455862106993131
     ddkappa: 0
   }
-  v: 4.0335412673183084
-  a: -0.62041420730969321
+  v: 4.0045775560391537
+  a: -0.65003024524610364
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587709.330571604
-    y: 4141441.06096035
-    theta: -0.719740182692806
-    kappa: -0.069453273717481792
-    s: 15.535820723973973
-    dkappa: -0.0061314948267917743
+    x: 587709.31409986119
+    y: 4141441.075842422
+    theta: -0.71793491345114668
+    kappa: -0.0693129286881272
+    s: 15.513358159615041
+    dkappa: -0.0061541480723456641
     ddkappa: 0
   }
-  v: 3.9760952398037825
-  a: -0.52725714990864714
+  v: 3.9441212251818465
+  a: -0.557801803809626
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587709.62410984992
-    y: 4141440.7964442628
-    theta: -0.74765119557523207
-    kappa: -0.071797427777168729
-    s: 15.930957994325571
-    dkappa: -0.00595168048562401
+    x: 587709.60521112941
+    y: 4141440.8136393242
+    theta: -0.74560692850221322
+    kappa: -0.0716424839219891
+    s: 15.905144115044738
+    dkappa: -0.005976655318354427
     ddkappa: 0
   }
-  v: 3.9283165047401338
-  a: -0.42730007040095358
+  v: 3.8932536031205753
+  a: -0.45848112707274485
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587709.90612289647
-    y: 4141440.5258358149
-    theta: -0.776239439381122
-    kappa: -0.073996697646448881
-    s: 16.3218267720977
-    dkappa: -0.0056932317188660188
+    x: 587709.88526706747
+    y: 4141440.5462997509
+    theta: -0.77385404791941392
+    kappa: -0.073835591081784085
+    s: 16.292350044111053
+    dkappa: -0.0057274242773030608
     ddkappa: 0
   }
-  v: 3.8908155507053879
-  a: -0.32193324143938284
+  v: 3.8526163960611735
+  a: -0.35341856151958895
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587710.17848047777
-    y: 4141440.2500731023
-    theta: -0.80534909958040146
-    kappa: -0.076046744656930565
-    s: 16.709479567019443
-    dkappa: -0.0053559250735115641
+    x: 587710.155718396
+    y: 4141440.2742508817
+    theta: -0.80258363300889568
+    kappa: -0.075885458120179439
+    s: 16.676025447180781
+    dkappa: -0.0054034305094647706
     ddkappa: 0
   }
-  v: 3.8640638390122941
-  a: -0.21254693567670502
+  v: 3.8227162755610626
+  a: -0.24396445363428632
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 587710.44183886913
-    y: 4141439.968600255
-    theta: -0.83499979577012817
-    kappa: -0.077934713149321738
-    s: 17.095009083587872
-    dkappa: -0.004942587814720594
+    x: 587710.41724006855
+    y: 4141439.9969221782
+    theta: -0.8318216654179682
+    kappa: -0.07778066510539268
+    s: 17.057263740044043
+    dkappa: -0.0050043954532351127
     ddkappa: 0
   }
-  v: 3.8483938037083245
-  a: -0.10053142576568985
+  v: 3.80392487852925
+  a: -0.13146914990096548
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587710.69682099181
-    y: 4141439.6807816364
-    theta: -0.86521999685720885
-    kappa: -0.079647884080822892
-    s: 17.479534318341557
-    dkappa: -0.0044585891544597763
+    x: 587710.66945165815
+    y: 4141439.7127878303
+    theta: -0.86167254933976467
+    kappa: -0.079483969362384679
+    s: 17.437188750449426
+    dkappa: -0.0045270126289032146
     ddkappa: 0
   }
-  v: 3.8439988515756749
-  a: 0.012723015640892177
+  v: 3.7964788072263325
+  a: -0.017282996803755224
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587710.940656747
-    y: 4141439.383289542
-    theta: -0.89621132393323
-    kappa: -0.081077291666733761
-    s: 17.864186657134024
-    dkappa: -0.0038786409770222682
+    x: 587710.91128166264
+    y: 4141439.42002206
+    theta: -0.89220635360346445
+    kappa: -0.080926109820862013
+    s: 17.816941214639172
+    dkappa: -0.0039622576348745074
     ddkappa: 0
   }
-  v: 3.8509333621312627
-  a: 0.12582611589027126
+  v: 3.8004796292644913
+  a: 0.097243659173216757
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587711.17615755834
-    y: 4141439.0776539631
-    theta: -0.92776875259843183
-    kappa: -0.082255362287508
-    s: 18.250095972407212
-    dkappa: -0.0032038595356461749
+    x: 587711.14527323
+    y: 4141439.1197925727
+    theta: -0.9232678225002201
+    kappa: -0.082137372489518773
+    s: 18.197665273884319
+    dkappa: -0.003311772191226298
     ddkappa: 0
   }
-  v: 3.86911268762673
-  a: 0.23738760232967704
+  v: 3.8158938776074969
+  a: 0.21076047154582211
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587711.40414011711
-    y: 4141438.763445708
-    theta: -0.95988778774967143
-    kappa: -0.0831793229782415
-    s: 18.638376720464983
-    dkappa: -0.0024724237360333507
+    x: 587711.37191206752
+    y: 4141438.8113356661
+    theta: -0.9548858383701061
+    kappa: -0.083106113442251439
+    s: 18.580494971019846
+    dkappa: -0.0025950968095558281
     ddkappa: 0
   }
-  v: 3.8983131530484405
-  a: 0.34601720230633903
+  v: 3.8425530505707064
+  a: 0.32191709382993217
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 587711.62476949207
-    y: 4141438.4398042457
-    theta: -0.99258261325810115
-    kappa: -0.083828899886648475
-    s: 19.030114038746554
-    dkappa: -0.0016945224949421636
+    x: 587711.59149268374
+    y: 4141438.4938207939
+    theta: -0.98708168229117221
+    kappa: -0.083814908205522634
+    s: 18.966540746979859
+    dkappa: -0.0018455094575894812
     ddkappa: 0
   }
-  v: 3.9381720561174829
-  a: 0.45032464316749404
+  v: 3.880153611821064
+  a: 0.42936317954141656
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587711.8356784475
-    y: 4141438.1043652152
-    theta: -1.0258783995295491
-    kappa: -0.084107098934728453
-    s: 19.426348730028803
-    dkappa: -0.00080335329923754374
+    x: 587711.79949243111
+    y: 4141438.1635286468
+    theta: -1.0198925987677891
+    kappa: -0.084097885519287846
+    s: 19.356874408710031
+    dkappa: -0.00096837550832091688
     ddkappa: 0
   }
-  v: 3.9881445894750933
-  a: 0.547699569468744
+  v: 3.9281972693555773
+  a: 0.53002794589785118
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587712.03709570307
-    y: 4141437.7568435189
-    theta: -1.0596594019273073
-    kappa: -0.084004993470252351
-    s: 19.828053427969341
-    dkappa: 6.647982262350657e-05
+    x: 587712.00034826377
+    y: 4141437.8227583375
+    theta: -1.0531724319221785
+    kappa: -0.084075973661144582
+    s: 19.75250108907203
+    dkappa: -0.00010438714083189473
     ddkappa: 0
   }
-  v: 4.0474324085553572
-  a: 0.63668765209375422
+  v: 3.9858687679146336
+  a: 0.621979719674283
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 587712.23008099524
-    y: 4141437.39739518
-    theta: -1.0938487357811182
-    kappa: -0.083549403633678326
-    s: 20.236118299866789
-    dkappa: 0.00098033838636306186
+    x: 587712.19337540632
+    y: 4141437.4704219252
+    theta: -1.0868954447537897
+    kappa: -0.083719030825335813
+    s: 20.154340604401831
+    dkappa: 0.00078606765587687592
     ddkappa: 0
   }
-  v: 4.1152140173326126
-  a: 0.71763265788182129
+  v: 4.0523147241328363
+  a: 0.70557681872279732
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587712.41436347144
-    y: 4141437.0253872937
-    theta: -1.1283875251250071
-    kappa: -0.082731853099832386
-    s: 20.651353082251386
-    dkappa: 0.0018924309855139366
+    x: 587712.37799448892
+    y: 4141437.1056516715
+    theta: -1.1210114567389082
+    kappa: -0.083005504587911588
+    s: 20.563229224288129
+    dkappa: 0.0017022834502088947
     ddkappa: 0
   }
-  v: 4.1907022964651315
-  a: 0.79087835367224157
+  v: 4.1267175864299981
+  a: 0.78117756089547929
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587712.58948939131
-    y: 4141436.6402649162
-    theta: -1.1631855586555726
-    kappa: -0.081562350486449678
-    s: 21.074490518553414
-    dkappa: 0.0027321802722299871
+    x: 587712.55401390733
+    y: 4141436.7279683654
+    theta: -1.1554443017381848
+    kappa: -0.081934768373199726
+    s: 20.979923254750869
+    dkappa: 0.0025665244839557016
     ddkappa: 0
   }
-  v: 4.2731445032951152
-  a: 0.85676850630431167
+  v: 4.20829563501114
+  a: 0.84914026404441489
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587712.7544773065
-    y: 4141436.2413654285
-    theta: -1.19812380748875
-    kappa: -0.080076459252113874
-    s: 21.506189796771547
-    dkappa: 0.0034363116302225574
+    x: 587712.71783480817
+    y: 4141436.33561622
+    theta: -1.1899406518889357
+    kappa: -0.080491141097564889
+    s: 21.405102621419733
+    dkappa: 0.0032846215438874348
     ddkappa: 0
   }
-  v: 4.3618222718486921
-  a: 0.91564688261732818
+  v: 4.296302981866492
+  a: 0.90982324602168885
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587712.90794359369
-    y: 4141435.8280895054
-    theta: -1.2330762080117772
-    kappa: -0.078355721031140177
-    s: 21.947039987141288
-    dkappa: 0.0038797198357334579
+    x: 587712.87145884312
+    y: 4141435.9294720064
+    theta: -1.2245117319187946
+    kappa: -0.078830907867660338
+    s: 21.839374452712718
+    dkappa: 0.0037669146250623388
     ddkappa: 0
   }
-  v: 4.456051612835922
-  a: 0.96785724945058726
+  v: 4.3900295707714916
+  a: 0.96358482467938689
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 587713.04916506726
-    y: 4141435.4002893791
-    theta: -1.2678840782857883
-    kappa: -0.076261071622458529
-    s: 22.397563479803331
-    dkappa: 0.00457579209372711
+    x: 587713.01455929957
+    y: 4141435.50934125
+    theta: -1.2590623330388171
+    kappa: -0.076853658806923464
+    s: 22.283276663014611
+    dkappa: 0.0043921031310770993
     ddkappa: 0
   }
-  v: 4.5551829136507935
-  a: 1.0137433736433858
+  v: 4.4888011772867857
+  a: 1.0107833178695942
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 587713.1783062066
-    y: 4141434.9581535575
-    theta: -1.3024247669322506
-    kappa: -0.073839529643995427
-    s: 22.858219422471979
-    dkappa: 0.0052123450578930863
+    x: 587713.14610791439
+    y: 4141435.0748988544
+    theta: -1.293426777122866
+    kappa: -0.074545908968146
+    s: 22.737281535855519
+    dkappa: 0.0050467672010195731
     ddkappa: 0
   }
-  v: 4.6586009383712259
-  a: 1.05364902203502
+  v: 4.5919794087582293
+  a: 1.0517770434443963
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587713.29526149225
-    y: 4141434.5017674817
-    theta: -1.3365786325169124
-    kappa: -0.071132591720142524
-    s: 23.329407158103507
-    dkappa: 0.0057230427080221958
+    x: 587713.26561390958
+    y: 4141434.626093369
+    theta: -1.327459963053232
+    kappa: -0.071939471229927748
+    s: 23.201799307089402
+    dkappa: 0.0055933658050671548
     ddkappa: 0
   }
-  v: 4.765724827759068
-  a: 1.0879179614647869
+  v: 4.6989617043168872
+  a: 1.0869243192558786
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 587713.39905145194
-    y: 4141434.0310760536
-    theta: -1.3701520080454472
-    kappa: -0.068175993206596508
-    s: 23.81146966256458
-    dkappa: 0.0061236588787510543
+    x: 587713.37245860416
+    y: 4141434.1629504673
+    theta: -1.3609962623037031
+    kappa: -0.069073364262378784
+    s: 23.677181748072581
+    dkappa: 0.0060209212534102947
     ddkappa: 0
   }
-  v: 4.8760080992600976
-  a: 1.1168939587719824
+  v: 4.80918133487903
+  a: 1.116583463156126
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 587713.48917940352
-    y: 4141433.5462148846
-    theta: -1.4029908169975385
-    kappa: -0.065016439576932622
-    s: 24.304696982300637
-    dkappa: 0.0064037437357555814
+    x: 587713.46582227584
+    y: 4141433.68551548
+    theta: -1.3938538451131604
+    kappa: -0.065988783039663809
+    s: 24.163725748842275
+    dkappa: 0.0063372376809766292
     ddkappa: 0
   }
-  v: 4.98893864700402
-  a: 1.140920780795903
+  v: 4.9221074031461391
+  a: 1.1411127929972247
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 587713.56551568978
-    y: 4141433.0474446621
-    theta: -1.4349797753187674
-    kappa: -0.061705263866623353
-    s: 24.809329672004282
-    dkappa: 0.0065623193730006118
+    x: 587713.54552741011
+    y: 4141433.1940437644
+    theta: -1.4259087060338294
+    kappa: -0.0627372210627664
+    s: 24.6616769012951
+    dkappa: 0.0065297073903368662
     ddkappa: 0
   }
-  v: 5.1040387418044775
-  a: 1.1603421943758458
+  v: 5.0372448436049053
+  a: 1.1608706266312594
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 587713.6275707318
-    y: 4141432.535007427
-    theta: -1.4659736057810793
-    kappa: -0.058291885239402923
-    s: 25.325562232283684
-    dkappa: 0.0066124926930523163
+    x: 587713.61124736536
+    y: 4141432.6888010493
+    theta: -1.4570300051848049
+    kappa: -0.059368749848728058
+    s: 25.171233082365607
+    dkappa: 0.006610744577539825
     ddkappa: 0
   }
-  v: 5.2208650311590334
-  a: 1.175501966351107
+  v: 5.1541344225272248
+  a: 1.1762152819103153
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 587713.67494267062
-    y: 4141432.0091969762
-    theta: -1.4958418619094509
-    kappa: -0.054825115156267527
-    s: 25.853546547330957
-    dkappa: 0.0065631474849310312
+    x: 587713.66245004314
+    y: 4141432.1700610826
+    theta: -1.4870714934976557
+    kappa: -0.0559319873352931
+    s: 25.6925480372048
+    dkappa: 0.0065912162391768281
     ddkappa: 0
   }
-  v: 5.3390085392491855
-  a: 1.1867438635609833
+  v: 5.2723527379702047
+  a: 1.1875050766864785
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 587713.70782046043
-    y: 4141431.4703827039
-    theta: -1.5245302985022606
-    kappa: -0.051348925068822396
-    s: 26.393395322590571
-    dkappa: 0.006430577039534327
+    x: 587713.69913960923
+    y: 4141431.6381856622
+    theta: -1.5159537348221264
+    kappa: -0.052472775877105471
+    s: 26.225734962358647
+    dkappa: 0.00648346141922302
     ddkappa: 0
   }
-  v: 5.45809466694036
-  a: 1.1944116528447706
+  v: 5.3915122197761587
+  a: 1.1950983288118338
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 587713.72627664555
-    y: 4141430.91892197
-    theta: -1.5519824267154267
-    kappa: -0.04790162675287167
-    s: 26.945185522427725
-    dkappa: 0.00623059911225558
+    x: 587713.72149451694
+    y: 4141431.093552073
+    theta: -1.5436287214719195
+    kappa: -0.049030650121437244
+    s: 26.770870088946619
+    dkappa: 0.00630490652017684
     ddkappa: 0
   }
-  v: 5.5777831917819123
-  a: 1.1988491010417659
+  v: 5.5112611295726133
+  a: 1.1993533561384668
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 587713.72943483968
-    y: 4141430.3551714267
-    theta: -1.5780270979750906
-    kappa: -0.044526548083776242
-    s: 27.508961807796755
-    dkappa: 0.0059747960080886618
+    x: 587713.72952725575
+    y: 4141430.5365199391
+    theta: -1.5700389498991312
+    kappa: -0.045639885679511161
+    s: 27.327996265840181
+    dkappa: 0.0060695877606638821
     ddkappa: 0
   }
-  v: 5.6977682680071284
-  a: 1.2003999749912653
+  v: 5.6312835607722969
+  a: 1.2006284765184629
   relative_time: 5.7999999999999963
 }
 trajectory_point {
   path_point {
-    x: 587713.71752245422
-    y: 4141429.7795372573
-    theta: -1.6026377491809813
-    kappa: -0.041253822723926033
-    s: 28.084739973909524
-    dkappa: 0.005675646555652435
+    x: 587713.72320795571
+    y: 4141429.9674520036
+    theta: -1.5951280231026796
+    kappa: -0.04233073216348731
+    s: 27.897126542841352
+    dkappa: 0.0057878237418438668
     ddkappa: 0
   }
-  v: 5.8177784265332244
-  a: 1.1994080415325659
+  v: 5.7512994385731533
+  a: 1.1992820078039077
   relative_time: 5.8999999999999959
 }
 trajectory_point {
   path_point {
-    x: 587713.6914751837
-    y: 4141429.1923696054
-    theta: -1.625892780038489
-    kappa: -0.038097129903090443
-    s: 28.672510387903813
-    dkappa: 0.0053537312119157649
+    x: 587713.70253218524
+    y: 4141429.3867150694
+    theta: -1.618847762478635
+    kappa: -0.039129161448252611
+    s: 28.478247753861194
+    dkappa: 0.0054751770280285546
     ddkappa: 0
   }
-  v: 5.9375765749613443
-  a: 1.1962170675049639
+  v: 5.8710645199583293
+  a: 1.1956722678468856
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 587713.65175029484
-    y: 4141428.5939806136
-    theta: -1.6478170789255848
-    kappa: -0.035068992679294836
-    s: 29.272241426511712
-    dkappa: 0.0050209895441164967
+    x: 587713.66665530915
+    y: 4141428.7947390168
+    theta: -1.6410370096905469
+    kappa: -0.0360703601111142
+    s: 29.071324100098341
+    dkappa: 0.0051485778747803633
     ddkappa: 0
   }
-  v: 6.0569599975765627
-  a: 1.1911708197477555
+  v: 5.9903703936961836
+  a: 1.190157574499483
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 587713.59869874222
-    y: 4141427.9846635824
-    theta: -1.6684266095724094
-    kappa: -0.032179003626227075
-    s: 29.883882913728009
-    dkappa: 0.0046831674923239362
+    x: 587713.61743275868
+    y: 4141428.1917879926
+    theta: -1.6619110607975691
+    kappa: -0.033145460433274911
+    s: 29.676300733217534
+    dkappa: 0.004814968639246831
     ddkappa: 0
   }
-  v: 6.1757603553478839
-  a: 1.1846130651002376
+  v: 6.1090444803402821
+  a: 1.1830962456137846
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
-    x: 587713.53213105188
-    y: 4141427.3647552859
-    theta: -1.6876631392779904
-    kappa: -0.029443040526659733
-    s: 30.507369558478604
-    dkappa: 0.0043478986150012021
+    x: 587713.5552480109
+    y: 4141427.5781435231
+    theta: -1.6814918001893278
+    kappa: -0.030360901077842478
+    s: 30.293107338528131
+    dkappa: 0.0044815978467479187
     ddkappa: 0
   }
-  v: 6.2938436859282421
-  a: 1.176887570401707
+  v: 6.2269500322293991
+  a: 1.1748465990418768
   relative_time: 6.2999999999999945
 }
 trajectory_point {
   path_point {
-    x: 587713.45182271721
-    y: 4141426.7346092574
-    theta: -1.7054625789153692
-    kappa: -0.026876219389895609
-    s: 31.142624392288869
-    dkappa: 0.0040221058587647787
+    x: 587713.48051783314
+    y: 4141426.9540612823
+    theta: -1.6998089128753002
+    kappa: -0.02771974014083085
+    s: 30.921661718162618
+    dkappa: 0.0041540863956878709
     ddkappa: 0
   }
-  v: 6.4111104036545008
-  a: 1.1683381024914596
+  v: 6.3439861334875189
+  a: 1.1657669526358434
   relative_time: 6.3999999999999941
 }
 trajectory_point {
   path_point {
-    x: 587713.35955843353
-    y: 4141426.0942978533
-    theta: -1.7220549589574776
-    kappa: -0.024454919356065235
-    s: 31.78956220695207
-    dkappa: 0.0037080922559099365
+    x: 587713.39249725768
+    y: 4141426.3199412543
+    theta: -1.7167219281279054
+    kappa: -0.025244401300968354
+    s: 31.561873374255143
+    dkappa: 0.0038388105034905112
     ddkappa: 0
   }
-  v: 6.5274952995474536
-  a: 1.1593084282087924
+  v: 6.4600877000238324
+  a: 1.156215624247771
   relative_time: 6.4999999999999938
 }
 trajectory_point {
   path_point {
-    x: 587713.255804637
-    y: 4141425.4440011173
-    theta: -1.7374846054929136
-    kappa: -0.022176375796673056
-    s: 32.448092992197751
-    dkappa: 0.0034081250497264913
+    x: 587713.29232351528
+    y: 4141425.6759225018
+    theta: -1.7323666038439873
+    kappa: -0.022921751122527232
+    s: 32.213647092120041
+    dkappa: 0.0035380647774258612
     ddkappa: 0
   }
-  v: 6.6429675413118208
-  a: 1.1501423143930014
+  v: 6.57522547953274
+  a: 1.1465509317297444
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
-    x: 587713.13951603509
-    y: 4141424.7841451112
-    theta: -1.7515668728607234
-    kappa: -0.020066534945765588
-    s: 33.118125373360122
-    dkappa: 0.0031274710505856769
+    x: 587713.18110509508
+    y: 4141425.0220842878
+    theta: -1.7468806485067818
+    kappa: -0.020736540641695077
+    s: 32.876886523430329
+    dkappa: 0.0032523479841988544
     ddkappa: 0
   }
-  v: 6.7575306733362588
-  a: 1.1411835278833833
+  v: 6.6894060514938518
+  a: 1.137131192933849
   relative_time: 6.6999999999999931
 }
 trajectory_point {
   path_point {
-    x: 587713.01257425651
-    y: 4141424.1146363951
-    theta: -1.7645583905947821
-    kappa: -0.01808926136834859
-    s: 33.799570049046437
-    dkappa: 0.0028700804623931248
+    x: 587713.05853069876
+    y: 4141424.3587108473
+    theta: -1.7601868875549191
+    kappa: -0.018698989558446337
+    s: 33.551497769396278
+    dkappa: 0.0029857647522931389
     ddkappa: 0
   }
-  v: 6.8712226166933457
-  a: 1.1327758355192348
+  v: 6.8026718271719835
+  a: 1.1283147257121702
   relative_time: 6.7999999999999927
 }
 trajectory_point {
   path_point {
-    x: 587712.87537053868
-    y: 4141423.435592154
-    theta: -1.7764877960868604
-    kappa: -0.016232445975670116
-    s: 34.492343228805446
-    dkappa: 0.0026383078067755287
+    x: 587712.92497970932
+    y: 4141423.6859497624
+    theta: -1.7723090972893369
+    kappa: -0.016800277305210533
+    s: 34.237392963943861
+    dkappa: 0.0027435386824010179
     ddkappa: 0
   }
-  v: 6.9841156691395962
-  a: 1.1252630041398524
+  v: 6.915101049617161
+  a: 1.1204598479167938
   relative_time: 6.8999999999999924
 }
 trajectory_point {
   path_point {
-    x: 587712.7273842725
-    y: 4141422.7472987669
-    theta: -1.7872307404611878
-    kappa: -0.014507861936339121
-    s: 35.196370070795687
-    dkappa: 0.0024309717025613373
+    x: 587712.78187970677
+    y: 4141423.0036997511
+    theta: -1.7834376549423816
+    kappa: -0.015014406455586408
+    s: 34.934493856893354
+    dkappa: 0.0025209360840180374
     ddkappa: 0
   }
-  v: 7.0963165051154506
-  a: 1.1189888005845325
+  v: 7.0268077936646209
+  a: 1.1139248773998049
   relative_time: 6.999999999999992
 }
 trajectory_point {
   path_point {
-    x: 587712.57053764025
-    y: 4141422.0494948626
-    theta: -1.7970591062887713
-    kappa: -0.012881971641842814
-    s: 35.911588119454009
-    dkappa: 0.0022382114517837965
+    x: 587712.62859286112
+    y: 4141422.3122497862
+    theta: -1.7934261748071345
+    kappa: -0.013357954147042491
+    s: 35.642735397137777
+    dkappa: 0.0023180204179871744
     ddkappa: 0
   }
-  v: 7.20796617574528
-  a: 1.1142969916925713
+  v: 7.1379419659348047
+  a: 1.1090681320132882
   relative_time: 7.0999999999999917
 }
 trajectory_point {
   path_point {
-    x: 587712.40429520188
-    y: 4141421.3424158976
-    theta: -1.8058371914234077
-    kappa: -0.011368919386260813
-    s: 36.637950743163834
-    dkappa: 0.002061281025773506
+    x: 587712.46664726886
+    y: 4141421.611386118
+    theta: -1.8024903498680291
+    kappa: -0.011804235119090019
+    s: 36.362069315821493
+    dkappa: 0.0021293351102371011
     ddkappa: 0
   }
-  v: 7.3192401088373851
-  a: 1.1115313443032659
+  v: 7.2486893048333636
+  a: 1.1062479296093306
   relative_time: 7.1999999999999913
 }
 trajectory_point {
   path_point {
-    x: 587712.22973717016
-    y: 4141420.6258952641
-    theta: -1.8137049188413419
-    kappa: -0.00995155031901331
-    s: 37.375430571923644
-    dkappa: 0.0018920128172985749
+    x: 587712.29601142614
+    y: 4141420.9012031285
+    theta: -1.8105796850444851
+    kappa: -0.010358652875324368
+    s: 37.092467709518665
+    dkappa: 0.0019524126337647249
     ddkappa: 0
   }
-  v: 7.4303481088839982
-  a: 1.1110356252559119
+  v: 7.3592713805511574
+  a: 1.105822588040015
   relative_time: 7.2999999999999909
 }
 trajectory_point {
   path_point {
-    x: 587712.04700671451
-    y: 4141419.8999501434
-    theta: -1.820633805691652
-    kappa: -0.0086322261594193248
-    s: 38.124022935015333
-    dkappa: 0.0017400403700725453
+    x: 587712.1172057651
+    y: 4141420.1816291227
+    theta: -1.8177401204919563
+    kappa: -0.0090156296606823273
+    s: 37.8339266234118
+    dkappa: 0.0017861200989906915
     ddkappa: 0
   }
-  v: 7.5415343570612761
-  a: 1.1131536013898069
+  v: 7.4699455950642557
+  a: 1.1081504251574301
   relative_time: 7.3999999999999906
 }
 trajectory_point {
   path_point {
-    x: 587711.85696814943
-    y: 4141419.1643774188
-    theta: -1.8267389141942925
-    kappa: -0.0073976452368642153
-    s: 38.883749298672605
-    dkappa: 0.0015993525508491272
+    x: 587711.93092583539
+    y: 4141419.4525080575
+    theta: -1.8240536668700644
+    kappa: -0.0077658997676191216
+    s: 38.586469634470291
+    dkappa: 0.0016353348382891462
     ddkappa: 0
   }
-  v: 7.65307741122931
-  a: 1.1182290395442469
+  v: 7.5810051821339366
+  a: 1.1135897588136581
   relative_time: 7.49999999999999
 }
 trajectory_point {
   path_point {
-    x: 587711.65971358086
-    y: 4141418.4191307104
-    theta: -1.8319742954293687
-    kappa: -0.0062538695654560349
-    s: 39.654660703749371
-    dkappa: 0.001462792065723582
+    x: 587711.73751302436
+    y: 4141418.7137256842
+    theta: -1.8295396133809549
+    kappa: -0.0066083583780596434
+    s: 39.35015143462892
+    dkappa: 0.0014908167056968461
     ddkappa: 0
   }
-  v: 7.7652902059321187
-  a: 1.1266057065585284
+  v: 7.6927792073066845
+  a: 1.1224989068607867
   relative_time: 7.59999999999999
 }
 trajectory_point {
   path_point {
-    x: 587711.45609366137
-    y: 4141417.6639194326
-    theta: -1.8364684575087968
-    kappa: -0.0051876132861280178
-    s: 40.436841203388134
-    dkappa: 0.0013368017644781655
+    x: 587711.537383268
+    y: 4141417.9651058656
+    theta: -1.8342280649198741
+    kappa: -0.0055417915212550711
+    s: 40.12506141396635
+    dkappa: 0.0013536545437224097
     ddkappa: 0
   }
-  v: 7.8785200523976524
-  a: 1.138627369271948
+  v: 7.8056325679141914
+  a: 1.1352361871508994
   relative_time: 7.6999999999999895
 }
 trajectory_point {
   path_point {
-    x: 587711.24618374114
-    y: 4141416.8986153649
-    theta: -1.8401663655086455
-    kappa: -0.0042079809647894272
-    s: 41.2304113006884
-    dkappa: 0.0012165806399686457
+    x: 587711.3311727111
+    y: 4141417.2063633474
+    theta: -1.8382160144976609
+    kappa: -0.0045571940507628476
+    s: 40.911327243883711
+    dkappa: 0.0012265701521810167
     ddkappa: 0
   }
-  v: 7.9931486385377912
-  a: 1.154637794523802
+  v: 7.919965993073367
+  a: 1.1521599175360828
   relative_time: 7.7999999999999892
 }
 trajectory_point {
   path_point {
-    x: 587711.03074202058
-    y: 4141416.1228560894
-    theta: -1.84319710344462
-    kappa: -0.0033038284883224914
-    s: 42.035531386375048
-    dkappa: 0.0010980620889833959
+    x: 587711.11898039235
+    y: 4141416.4373091403
+    theta: -1.841464191363323
+    kappa: -0.0036649182957875135
+    s: 41.709118460283051
+    dkappa: 0.0010987401624607341
     ddkappa: 0
   }
-  v: 8.1095920289483381
-  a: 1.1749807491533875
+  v: 8.0362160436863128
+  a: 1.1736284158684218
   relative_time: 7.8999999999999888
 }
 trajectory_point {
   path_point {
-    x: 587710.80999191967
-    y: 4141415.3363754139
-    theta: -1.8455529080089139
-    kappa: -0.0024811058223746536
-    s: 42.852405176466746
-    dkappa: 0.00098704556145650388
+    x: 587710.90151948191
+    y: 4141415.6575324973
+    theta: -1.844115774280874
+    kappa: -0.00285167775304138
+    s: 42.518650046745918
+    dkappa: 0.00097845238257211536
     ddkappa: 0
   }
-  v: 8.2283006649090336
-  a: 1.1999999999999997
+  v: 8.154855112440357
+  a: 1.2000000000000022
   relative_time: 7.9999999999999885
 }
 decision {
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_slowdown_01_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_slowdown_01_0.pb.txt
index 762def30b6..a03194f8f9 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_slowdown_01_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_slowdown_01_0.pb.txt
@@ -9,1693 +9,1693 @@ trajectory_point {
     y: 4140947.076827547
     z: 0
     theta: -1.7937387314394067
-    kappa: 0.00083016114539450128
+    kappa: 0.000829971079400957
     s: 0
     dkappa: 0
     ddkappa: 0
   }
   v: 1.7111110687255859
-  a: -0.29289365613175733
+  a: -0.29289366195637317
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 587258.37714730331
-    y: 4140947.0435085571
-    theta: -1.7937341132353632
-    kappa: 0.00073600712769246739
-    s: 0.034163989172098827
-    dkappa: -0.00018854801545852207
+    x: 587258.37714730168
+    y: 4140947.0435085576
+    theta: -1.7937341194086582
+    kappa: 0.00073581833500059764
+    s: 0.034163989170926307
+    dkappa: -0.00018854546581177395
     ddkappa: 0
   }
-  v: 1.7053050341816389
-  a: -0.287737913573519
+  v: 1.70530503406401
+  a: -0.28773791951124739
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 587258.36962138175
+    x: 587258.3696213786
     y: 4140947.0103018219
-    theta: -1.7937295105903557
-    kappa: 0.00064217032114753778
-    s: 0.068212877589061791
-    dkappa: -0.0003764608000940474
+    theta: -1.7937295229161476
+    kappa: 0.00064198279747406067
+    s: 0.068212877584341775
+    dkappa: -0.000376455709377352
     ddkappa: 0
   }
-  v: 1.6996004342413522
-  a: -0.28274986220923742
+  v: 1.6996004340038648
+  a: -0.28274986825678655
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 587258.36212046014
-    y: 4140946.9772053938
-    theta: -1.7937249232346659
-    kappa: 0.00054864522686112749
-    s: 0.10214866053814588
-    dkappa: -0.00056374936571935573
+    x: 587258.3621204556
+    y: 4140946.9772053948
+    theta: -1.7937249416925178
+    kappa: 0.00054845896799724674
+    s: 0.10214866052745944
+    dkappa: -0.000563741742360364
     ddkappa: 0
   }
-  v: 1.6939939350942361
-  a: -0.27792750069994421
+  v: 1.693993934734727
+  a: -0.27792750685405959
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 587258.35464411217
-    y: 4140946.9442173922
-    theta: -1.7937203509075355
-    kappa: 0.00045542652858739086
-    s: 0.13597326703066615
-    dkappa: -0.00075042435837620249
+    x: 587258.35464410624
+    y: 4140946.9442173936
+    theta: -1.7937203754773583
+    kappa: 0.00045524153039632224
+    s: 0.13597326701155177
+    dkappa: -0.00075041421065836936
     ddkappa: 0
   }
-  v: 1.6884822429565809
-  a: -0.27326882770667094
+  v: 1.6884822424729506
+  a: -0.27326883396413532
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 587258.34719192609
-    y: 4140946.9113360005
-    theta: -1.7937157933570562
-    kappa: 0.00036250909052699162
-    s: 0.16968856060253115
-    dkappa: -0.00093649606275340185
+    x: 587258.34719191852
+    y: 4140946.9113360019
+    theta: -1.7937158240190978
+    kappa: 0.00036232534894151922
+    s: 0.16968856057248594
+    dkappa: -0.00093648339882087931
     ddkappa: 0
   }
-  v: 1.6830621040714557
-  a: -0.26877184189044923
+  v: 1.6830621034616691
+  a: -0.26877184824808259
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 587258.339763504
-    y: 4140946.8785594632
-    theta: -1.7937112503400627
-    kappa: 0.00026988795512087075
-    s: 0.20329634011477873
-    dkappa: -0.0011219744066049131
+    x: 587258.33976349491
+    y: 4140946.8785594651
+    theta: -1.7937112870748961
+    kappa: 0.00026970546614093322
+    s: 0.20329634007125971
+    dkappa: -0.0011219592344673837
     ddkappa: 0
   }
-  v: 1.6777303047087091
-  a: -0.26443454191231075
+  v: 1.6777303039707943
+  a: -0.26443454836697033
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 587258.33235846262
-    y: 4140946.8458860889
-    theta: -1.7937067216220239
-    kappa: 0.00017755834084401753
-    s: 0.23679834055411139
-    dkappa: -0.0013068689651679244
+    x: 587258.332358452
+    y: 4140946.8458860908
+    theta: -1.7937067644105356
+    kappa: 0.00017737710053432343
+    s: 0.23679834049453685
+    dkappa: -0.0013068512927053757
     ddkappa: 0
   }
-  v: 1.6724836711649691
-  a: -0.26025492643328707
+  v: 1.6724836702970167
+  a: -0.2602549329818673
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 587258.32497643237
-    y: 4140946.8133142465
-    theta: -1.7937022069769348
-    kappa: 8.5515639999237563e-05
-    s: 0.27019623383343205
-    dkappa: -0.0014911889655809371
+    x: 587258.32497642026
+    y: 4140946.8133142493
+    theta: -1.7937022558003135
+    kappa: 8.5335644486911669e-05
+    s: 0.27019623375518265
+    dkappa: -0.0014911688005483754
     ddkappa: 0
   }
-  v: 1.6673190697636433
-  a: -0.2562309941144098
+  v: 1.6673190687638058
+  a: -0.25623100075384253
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 587258.31761705736
-    y: 4140946.7808423666
-    theta: -1.7936977061872088
-    kappa: -6.2445834890776687e-06
-    s: 0.30349162959237957
-    dkappa: -0.0016749432913018517
+    x: 587258.31761704374
+    y: 4140946.7808423704
+    theta: -1.7936977610269338
+    kappa: -6.4233380168189375e-06
+    s: 0.30349162949279967
+    dkappa: -0.0016749206413339561
     ddkappa: 0
   }
-  v: 1.6622334068549183
-  a: -0.25236074361671057
+  v: 1.6622334057214088
+  a: -0.25236075034396471
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 587258.31027999544
-    y: 4140946.7484689392
-    theta: -1.7936932190435682
-    kappa: -9.7726596281181189e-05
-    s: 0.33668607599786438
-    dkappa: -0.0018581404865260513
+    x: 587258.31027998042
+    y: 4140946.7484689429
+    theta: -1.7936932798813989
+    kappa: -9.7904113579323738e-05
+    s: 0.33668607587426308
+    dkappa: -0.0018581153591417676
     ddkappa: 0
   }
-  v: 1.65722362881576
-  a: -0.24864217360122098
+  v: 1.6572236275468524
+  a: -0.2486421804133028
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 587258.302964918
-    y: 4140946.7161925137
-    theta: -1.7936887453449377
-    kappa: -0.0001889344978282013
-    s: 0.36978106054460391
-    dkappa: -0.0020407887606044865
+    x: 587258.30296490144
+    y: 4140946.7161925174
+    theta: -1.7936888121629022
+    kappa: -0.00018911078159619877
+    s: 0.36978106039425651
+    dkappa: -0.0020407611632115626
     ddkappa: 0
   }
-  v: 1.6522867220499144
-  a: -0.24507328272897266
+  v: 1.6522867206439416
+  a: -0.24507328962292566
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587258.29567150911
-    y: 4140946.6840116978
-    theta: -1.7936842848983348
-    kappa: -0.00027987222257837253
-    s: 0.40277801085565834
-    dkappa: -0.00222289599246176
+    x: 587258.29567149119
+    y: 4140946.6840117024
+    theta: -1.7936843576787189
+    kappa: -0.00028004727646238051
+    s: 0.40277801067580737
+    dkappa: -0.0022228659323612209
     ddkappa: 0
   }
-  v: 1.647419712987906
-  a: -0.24165206966099723
+  v: 1.6474197114432596
+  a: -0.24165207663390215
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 587258.28839946678
-    y: 4140946.6519251568
-    theta: -1.7936798375187621
-    kappa: -0.00037054354218326604
-    s: 0.43567829548296616
-    dkappa: -0.0024044697350142112
+    x: 587258.28839944722
+    y: 4140946.6519251615
+    theta: -1.7936799162440991
+    kappa: -0.00037071736977834668
+    s: 0.43567829527082264
+    dkappa: -0.0024044372194047736
     ddkappa: 0
   }
-  v: 1.6426196680870391
-  a: -0.23837653305832629
+  v: 1.6426196664021693
+  a: -0.23837654010730111
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587258.281148501
-    y: 4140946.6199316122
-    theta: -1.7936754030290991
-    kappa: -0.00046095206770402125
-    s: 0.4684832247078799
-    dkappa: -0.0025855172195880034
+    x: 587258.28114848
+    y: 4140946.6199316173
+    theta: -1.793675487682159
+    kappa: -0.00046112467255631808
+    s: 0.46848322446062418
+    dkappa: -0.0025854822555704282
     ddkappa: 0
   }
-  v: 1.6378836938313974
-  a: -0.23524467158199144
+  v: 1.637883692004811
+  a: -0.23524467870419141
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587258.27391786466
-    y: 4140946.58802995
-    theta: -1.7936734986553988
-    kappa: -0.00055112906327210883
-    s: 0.50119405134170114
-    dkappa: -0.0027559990215402252
+    x: 587258.27391784219
+    y: 4140946.588029955
+    theta: -1.7936735891861568
+    kappa: -0.00055130045171776824
+    s: 0.50119405105648462
+    dkappa: -0.002755961757029907
     ddkappa: 0
   }
-  v: 1.6332089367318436
-  a: -0.2322544838930243
+  v: 1.6332089347621046
+  a: -0.23225449108564195
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 587258.2666998544
-    y: 4140946.55622069
-    theta: -1.7937139486886595
-    kappa: -0.00064151779438746583
-    s: 0.53381197152621707
-    dkappa: -0.002756991628119587
+    x: 587258.26669982763
+    y: 4140946.5562206958
+    theta: -1.7937140445187407
+    kappa: -0.00064168801752666432
+    s: 0.53381197120016266
+    dkappa: -0.0027569544647222566
     ddkappa: 0
   }
-  v: 1.6285925833260204
-  a: -0.22940396865245652
+  v: 1.6285925812117479
+  a: -0.22940397591272157
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 587258.259502151
-    y: 4140946.52450092
-    theta: -1.7937542849220058
-    kappa: -0.000731652228839944
-    s: 0.5663381255342349
-    dkappa: -0.0027579814421324679
+    x: 587258.25950212008
+    y: 4140946.5245009274
+    theta: -1.7937543860364979
+    kappa: -0.00073182128994275049
+    s: 0.5663381251644386
+    dkappa: -0.0027579443795635664
     ddkappa: 0
   }
-  v: 1.6240318601783492
-  a: -0.22669112452131968
+  v: 1.6240318579182176
+  a: -0.22669113184649914
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 587258.25232451444
-    y: 4140946.4928695839
-    theta: -1.79379450870108
-    kappa: -0.00082153537387969638
-    s: 0.59877359857011869
-    dkappa: -0.0027589684966030778
+    x: 587258.2523244794
+    y: 4140946.4928695923
+    theta: -1.7937946150852471
+    kappa: -0.00082170327617734132
+    s: 0.59877359815365039
+    dkappa: -0.0027589315345814078
     ddkappa: 0
   }
-  v: 1.6195240338800312
-  a: -0.22411395016064539
+  v: 1.6195240314727695
+  a: -0.22411395754804353
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587258.2451667164
-    y: 4140946.4613256748
-    theta: -1.7938346213042282
-    kappa: -0.00091117008637305649
-    s: 0.63111942157032364
-    dkappa: -0.0027599528229041788
+    x: 587258.24516667728
+    y: 4140946.4613256841
+    theta: -1.7938347329435014
+    kappa: -0.00091133683305987478
+    s: 0.6311194211042287
+    dkappa: -0.0027599159611517406
     ddkappa: 0
   }
-  v: 1.6150664110490467
-  a: -0.22167044423146526
+  v: 1.615066408493437
+  a: -0.22167045167842359
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587258.23802854074
-    y: 4140946.42986824
-    theta: -1.7938746239434928
-    kappa: -0.0010005590750209363
-    s: 0.66337657200393307
-    dkappa: -0.0027609344507814539
+    x: 587258.23802849744
+    y: 4140946.4298682506
+    theta: -1.7938747408234621
+    kappa: -0.0010007246692562779
+    s: 0.66337657148523266
+    dkappa: -0.0027608976890232742
     ddkappa: 0
   }
-  v: 1.6106563383301558
-  a: -0.21935860539481095
+  v: 1.6106563356250332
+  a: -0.2193586128987082
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587258.23090978281
-    y: 4140946.3984963777
-    theta: -1.7939145177656053
-    kappa: -0.0010897049025772162
-    s: 0.69554597467319268
-    dkappa: -0.0027619134083778606
+    x: 587258.23090973543
+    y: 4140946.3984963885
+    theta: -1.7939146398720109
+    kappa: -0.0010898693474873326
+    s: 0.69554597409888541
+    dkappa: -0.0027618767463418337
     ddkappa: 0
   }
-  v: 1.6062912023948972
-  a: -0.21717643231171402
+  v: 1.6062911995391489
+  a: -0.21717643986996621
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 587258.22381024924
-    y: 4140946.3672092361
-    theta: -1.7939543038529795
-    kappa: -0.0011786099880671413
-    s: 0.72762850251404709
-    dkappa: -0.0027628897222579983
+    x: 587258.22381019779
+    y: 4140946.3672092487
+    theta: -1.7939544311717033
+    kappa: -0.0011787732867470399
+    s: 0.72762850188110972
+    dkappa: -0.002762853159674724
     ddkappa: 0
   }
-  v: 1.60196842994159
-  a: -0.21512192364320609
+  v: 1.6019684269341541
+  a: -0.21512193125326654
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587258.21672975854
-    y: 4140946.3360060146
-    theta: -1.7939939832247029
-    kappa: -0.0012672766090057153
-    s: 0.75962497739667512
-    dkappa: -0.0027638634174324667
+    x: 587258.21672970289
+    y: 4140946.3360060276
+    theta: -1.7939941157417605
+    kappa: -0.0012674387645209867
+    s: 0.75962497670206364
+    dkappa: -0.0027638269540350914
     ddkappa: 0
   }
-  v: 1.5976854876953315
-  a: -0.21319307805031881
+  v: 1.5976854845351975
+  a: -0.213193085709678
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587258.20966813958
-    y: 4140946.3048859593
-    theta: -1.794033556837531
-    kappa: -0.0013557069036160931
-    s: 0.791536170926025
-    dkappa: -0.0027648345173822281
+    x: 587258.20966807986
+    y: 4140946.3048859737
+    theta: -1.7940336945390629
+    kappa: -0.0013558679190047104
+    s: 0.791536170166676
+    dkappa: -0.00276479815290629
     ddkappa: 0
   }
-  v: 1.5934398824079996
-  a: -0.21138789419408377
+  v: 1.5934398790942059
+  a: -0.21138790190026943
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587258.20262523275
-    y: 4140946.2738483679
-    theta: -1.7940730255868793
-    kappa: -0.0014439028730479763
-    s: 0.82336280524235084
-    dkappa: -0.002765803044082968
+    x: 587258.202625169
+    y: 4140946.2738483828
+    theta: -1.7940731684591436
+    kappa: -0.0014440627513220622
+    s: 0.8233628044151815
+    dkappa: -0.0027657667782662429
     ddkappa: 0
   }
-  v: 1.5892291608582503
-  a: -0.20970437073553261
+  v: 1.5892291573898849
+  a: -0.20970437848610976
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587258.19560088892
-    y: 4140946.2428925815
-    theta: -1.7941123903078156
-    kappa: -0.0015318663835960048
-    s: 0.855105553821747
-    dkappa: -0.0027667690180294578
+    x: 587258.195600821
+    y: 4140946.2428925978
+    theta: -1.7941125383371792
+    kappa: -0.001532025127743577
+    s: 0.85510555292365753
+    dkappa: -0.0027667328506118085
     ddkappa: 0
   }
-  v: 1.5850509098515202
-  a: -0.20814050633569689
+  v: 1.5850509062277194
+  a: -0.20814051412826781
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587258.18859496969
-    y: 4140946.2120179916
-    theta: -1.794151651776053
-    kappa: -0.0016195991689181542
-    s: 0.88676504227668507
-    dkappa: -0.0027677324582599137
+    x: 587258.18859489786
+    y: 4140946.2120180093
+    theta: -1.7941518049489851
+    kappa: -0.0016197567819048338
+    s: 0.88676504130455835
+    dkappa: -0.0027676963889831435
     ddkappa: 0
   }
-  v: 1.5809027562200242
-  a: -0.20669429965560826
+  v: 1.5809027524399717
+  a: -0.20669430748781248
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587258.1816073471
-    y: 4140946.1812240337
-    theta: -1.7941908107089433
-    kappa: -0.0017071028322541266
-    s: 0.91834184915654815
-    dkappa: -0.0027686933823803626
+    x: 587258.1816072712
+    y: 4140946.1812240519
+    theta: -1.7941909690120066
+    kappa: -0.0017072593170248233
+    s: 0.91834184810725128
+    dkappa: -0.0027686574109880655
     ddkappa: 0
   }
-  v: 1.5767823668227572
-  a: -0.20536374935629834
+  v: 1.5767823628856839
+  a: -0.20536375722581263
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587258.17463790323
-    y: 4140946.1505101877
-    theta: -1.794229867766469
-    kappa: -0.0017943788486437455
-    s: 0.94983650674816722
-    dkappa: -0.0027696518065889976
+    x: 587258.17463782337
+    y: 4140946.1505102073
+    theta: -1.7942300311863124
+    kappa: -0.0017945342081243146
+    s: 0.94983650561855271
+    dkappa: -0.002769615932826419
     ddkappa: 0
   }
-  v: 1.5726874485454934
-  a: -0.20414685409879874
+  v: 1.5726874444506758
+  a: -0.20414686200333709
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587258.16768653051
-    y: 4140946.1198759805
-    theta: -1.7942688235522357
-    kappa: -0.0018814285671453507
-    s: 0.98124950187635673
-    dkappa: -0.0027706077457005443
+    x: 587258.16768644645
+    y: 4140946.1198760006
+    theta: -1.7942689920755879
+    kappa: -0.0018815828042442171
+    s: 0.98124950066326277
+    dkappa: -0.0027705719693144376
     ddkappa: 0
   }
-  v: 1.5686157483007863
-  a: -0.20304161254414105
+  v: 1.5686157440475463
+  a: -0.20304162048145474
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587258.16074024444
-    y: 4140946.08932391
-    theta: -1.7943269425243717
-    kappa: -0.001968451803423758
-    s: 1.01258127670445
-    dkappa: -0.0027715349842377126
+    x: 587258.16074015538
+    y: 4140946.0893239314
+    theta: -1.7943271158962713
+    kappa: -0.00196860494273374
+    s: 1.0125812754047014
+    dkappa: -0.0027714993052209403
     ddkappa: 0
   }
-  v: 1.5645650530279687
-  a: -0.20204602335335686
+  v: 1.564565048615673
+  a: -0.20204603132123447
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587258.15379582881
-    y: 4140946.0588542996
-    theta: -1.7944089010840545
-    kappa: -0.0020554978647803761
-    s: 1.0438322295348341
-    dkappa: -0.0027724271678107306
+    x: 587258.15379573347
+    y: 4140946.0588543229
+    theta: -1.7944090789875811
+    kappa: -0.0020556499361128423
+    s: 1.0438322281452443
+    dkappa: -0.0027723915862213435
     ddkappa: 0
   }
-  v: 1.5605331896931527
-  a: -0.20115808518747788
+  v: 1.5605331851212121
+  a: -0.20115809318374517
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587258.14686929411
-    y: 4140946.0284631439
-    theta: -1.7944906486192862
-    kappa: -0.0021423197949178739
-    s: 1.075002715609487
-    dkappa: -0.0027733170541380282
+    x: 587258.1468691926
+    y: 4140946.028463169
+    theta: -1.7944908310427641
+    kappa: -0.0021424708010131612
+    s: 1.0750027141268574
+    dkappa: -0.0027732815697250673
     ddkappa: 0
   }
-  v: 1.55651802528923
-  a: -0.20037579670753566
+  v: 1.5565180205570979
+  a: -0.20037580473005562
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587258.13996057084
-    y: 4140945.9981501391
-    theta: -1.7945721859497483
-    kappa: -0.0022289184656131707
-    s: 1.1060930479105122
-    dkappa: -0.0027742046521549335
+    x: 587258.13996046293
+    y: 4140945.9981501647
+    theta: -1.7945723728815461
+    kappa: -0.0022290684092008931
+    s: 1.1060930463316339
+    dkappa: -0.0027741692646684139
     ddkappa: 0
   }
-  v: 1.5525174668358714
-  a: -0.1996971565745618
+  v: 1.552517461943044
+  a: -0.19969716462123474
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587258.13306959847
-    y: 4140945.9679150186
-    theta: -1.7946535137864081
-    kappa: -0.0023152946331627968
-    s: 1.137103497960674
-    dkappa: -0.0027750899696131503
+    x: 587258.13306948438
+    y: 4140945.967915046
+    theta: -1.7946537052149345
+    kappa: -0.0023154435169632651
+    s: 1.1371034962823281
+    dkappa: -0.0027750546788039353
     ddkappa: 0
   }
-  v: 1.5485294613795275
-  a: -0.19912016344958797
+  v: 1.5485294563255423
+  a: -0.19912017151835137
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587258.12619632587
-    y: 4140945.9377575582
-    theta: -1.7947346327336184
-    kappa: -0.0024014489406126984
-    s: 1.1680342966239334
-    dkappa: -0.0027759730131036147
+    x: 587258.12619620538
+    y: 4140945.9377575866
+    theta: -1.7947348286473146
+    kappa: -0.0024015967673383107
+    s: 1.168034294842893
+    dkappa: -0.0027759378187232854
     ddkappa: 0
   }
-  v: 1.5445519959934277
-  a: -0.19864281599364572
+  v: 1.5445519907778633
+  a: -0.19864282408247441
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 587258.11934071046
-    y: 4140945.9076775704
-    theta: -1.7948155432912167
-    kappa: -0.0024873819199880414
-    s: 1.1988856349059847
-    dkappa: -0.0027768537880793493
+    x: 587258.1193405838
+    y: 4140945.907677602
+    theta: -1.7948157436785519
+    kappa: -0.002487528692344647
+    s: 1.1988856330190136
+    dkappa: -0.0027768186898800822
     ddkappa: 0
   }
-  v: 1.5405830977775812
-  a: -0.19826311286776671
+  v: 1.5405830924000561
+  a: -0.19826312097467275
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 587258.11250271858
-    y: 4140945.8776749093
-    theta: -1.7948962458566242
-    kappa: -0.00257309399452301
-    s: 1.2296576647547888
-    dkappa: -0.0027777322988783168
+    x: 587258.11250258563
+    y: 4140945.8776749414
+    theta: -1.7948964507060892
+    kappa: -0.0025732397152112485
+    s: 1.2296576627586451
+    dkappa: -0.002777697296612763
     ddkappa: 0
   }
-  v: 1.5366208338587763
-  a: -0.1979790527329825
+  v: 1.5366208283189486
+  a: -0.19797906085601513
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 587258.10568232485
-    y: 4140945.8477494628
-    theta: -1.7949767407269457
-    kappa: -0.0026585854808906188
-    s: 1.2603504998611117
-    dkappa: -0.0027786085487462755
+    x: 587258.10568218585
+    y: 4140945.8477494968
+    theta: -1.7949769500270476
+    kappa: -0.0026587301526072235
+    s: 1.2603504977525455
+    dkappa: -0.0027785736421674393
     ddkappa: 0
   }
-  v: 1.532663311390581
-  a: -0.19778863425032478
+  v: 1.5326633056881473
+  a: -0.19778864238757055
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 587258.0988795124
-    y: 4140945.8179011564
-    theta: -1.7950570281010685
-    kappa: -0.0027438565914325022
-    s: 1.2909642164590571
-    dkappa: -0.0027794825398596333
+    x: 587258.09887936723
+    y: 4140945.8179011918
+    theta: -1.7950572418403252
+    kappa: -0.0027440002168715905
+    s: 1.2909642142348141
+    dkappa: -0.0027794477287207569
     ddkappa: 0
   }
-  v: 1.5287086775533423
-  a: -0.19768985608082509
+  v: 1.5287086716880374
+  a: -0.1976898642304078
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 587258.09209427249
-    y: 4140945.7881299518
-    theta: -1.7951371080817613
-    kappa: -0.0028289074363887314
-    s: 1.3214988541266044
-    dkappa: -0.0027803542733483015
+    x: 587258.092094121
+    y: 4140945.7881299891
+    theta: -1.7951373262486965
+    kappa: -0.0028290500182430517
+    s: 1.3214988517834245
+    dkappa: -0.0027803195574027505
     ddkappa: 0
   }
-  v: 1.5247551195541869
-  a: -0.19768071688551514
+  v: 1.5247551135257822
+  a: -0.19768072504559578
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 587258.0853266042
-    y: 4140945.7584358454
-    theta: -1.7952169806777747
-    kappa: -0.0029137380261276082
-    s: 1.351954416586143
-    dkappa: -0.002781223749318551
+    x: 587258.08532644669
+    y: 4140945.758435884
+    theta: -1.7952172032609124
+    kappa: -0.0029138795670897717
+    s: 1.3519544141207624
+    dkappa: -0.002781189128319702
     ddkappa: 0
   }
-  v: 1.5208008646270204
-  a: -0.19775921532542642
+  v: 1.5208008584353241
+  a: -0.19775922349420338
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 587258.07857651461
-    y: 4140945.7288188683
-    theta: -1.7952966458059398
-    kappa: -0.0029983482733754729
-    s: 1.3823308725050083
-    dkappa: -0.0027820909668758655
+    x: 587258.07857635093
+    y: 4140945.7288189079
+    theta: -1.795296872793799
+    kappa: -0.0029984887761391509
+    s: 1.3823308699141597
+    dkappa: -0.0027820564405769983
     ddkappa: 0
   }
-  v: 1.5168441800325281
-  a: -0.19792335006159062
+  v: 1.5168441736773841
+  a: -0.19792335823729937
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 587258.07184401818
-    y: 4140945.6992790829
-    theta: -1.7953761032932676
-    kappa: -0.0030827379954465042
-    s: 1.412628156296017
-    dkappa: -0.0027829559241477962
+    x: 587258.07184384833
+    y: 4140945.6992791248
+    theta: -1.7953763346743579
+    kappa: -0.0030828774627076005
+    s: 1.4126281535764298
+    dkappa: -0.0027829214923019846
     ddkappa: 0
   }
-  v: 1.5128833730581748
-  a: -0.19817111975503937
+  v: 1.5128833665394616
+  a: -0.19817112793595271
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 587258.0651291369
-    y: 4140945.6698165876
-    theta: -1.7954553528790489
-    kappa: -0.003166906916472523
-    s: 1.442846168918003
-    dkappa: -0.0027838186183068159
+    x: 587258.065128961
+    y: 4140945.6698166304
+    theta: -1.7954555886418653
+    kappa: -0.0031670453509303215
+    s: 1.4428461660664054
+    dkappa: -0.0027837842806668258
     ddkappa: 0
   }
-  v: 1.5089167910182044
-  a: -0.19850052306680424
+  v: 1.5089167843358351
+  a: -0.19850053125123224
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 587258.05843190011
-    y: 4140945.6404315089
-    theta: -1.7955343942169533
-    kappa: -0.0032508546696327994
-    s: 1.472984778676353
-    dkappa: -0.0027846790455931759
+    x: 587258.05843171827
+    y: 4140945.6404315541
+    theta: -1.7955346343499716
+    kappa: -0.0032509920739910766
+    s: 1.4729847756894709
+    dkappa: -0.0027846448019113626
     ddkappa: 0
   }
-  v: 1.5049428212536402
-  a: -0.19890955865791682
+  v: 1.5049428144075612
+  a: -0.19890956684420677
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 587258.05174406082
-    y: 4140945.6111259032
-    theta: -1.795620437187087
-    kappa: -0.0033346499035264211
-    s: 1.503043822023542
-    dkappa: -0.0027855274829876038
+    x: 587258.05174387246
+    y: 4140945.6111259493
+    theta: -1.7956206815960425
+    kappa: -0.0033347862886341979
+    s: 1.503043818898101
+    dkappa: -0.0027854933330654467
     ddkappa: 0
   }
-  v: 1.5009598911322852
-  a: -0.1993962251894088
+  v: 1.5009598841224752
+  a: -0.1993962333759452
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 587258.04503438028
-    y: 4140945.5819071154
-    theta: -1.7957407109163128
-    kappa: -0.0034185530467756822
-    s: 1.5330231043596683
-    dkappa: -0.0027863272237448346
+    x: 587258.04503418377
+    y: 4140945.5819071634
+    theta: -1.7957409591721423
+    kappa: -0.0034186884540132459
+    s: 1.5330231010923938
+    dkappa: -0.0027862931676008334
     ddkappa: 0
   }
-  v: 1.4969664680487211
-  a: -0.19995852132231176
+  v: 1.4969664608751909
+  a: -0.19995852950751647
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 587258.03834260139
-    y: 4140945.5527662844
-    theta: -1.7958606637754544
-    kappa: -0.00350223233325514
-    s: 1.5629224008329903
-    dkappa: -0.0027871248307633797
+    x: 587258.038342397
+    y: 4140945.5527663343
+    theta: -1.7958609158678822
+    kappa: -0.0035023667652212638
+    s: 1.5629223974206083
+    dkappa: -0.0027870908681472345
     ddkappa: 0
   }
-  v: 1.4929610594243097
-  a: -0.20059444571765728
+  v: 1.4929610520871017
+  a: -0.20059445389998937
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 587258.03166878107
-    y: 4140945.5237036585
-    theta: -1.7959802947406374
-    kappa: -0.0035856870512457908
-    s: 1.592741457140461
-    dkappa: -0.002787920297259338
+    x: 587258.03166856861
+    y: 4140945.5237037106
+    theta: -1.7959805506593545
+    kappa: -0.0035858205105475413
+    s: 1.5927414535796987
+    dkappa: -0.0027878864279199506
     ddkappa: 0
   }
-  v: 1.4889422127071916
-  a: -0.201301997036477
+  v: 1.4889422052063783
+  a: -0.20130200521443276
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587257.99857148936
-    y: 4140945.3795741829
-    theta: -1.7965735774592033
-    kappa: -0.0039995617162156853
-    s: 1.7406222748595439
-    dkappa: -0.0027918652321585989
+    x: 587257.9985712372
+    y: 4140945.3795742458
+    theta: -1.7965738523533288
+    kappa: -0.0039996903516559459
+    s: 1.740622270507868
+    dkappa: -0.0027918318254025803
     ddkappa: 0
   }
-  v: 1.4685983702904875
-  a: -0.20584411061879521
+  v: 1.468598361973749
+  a: -0.20584411875361763
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587257.96593517659
-    y: 4140945.2374521429
-    theta: -1.7971585969602291
-    kappa: -0.0044076719372666056
-    s: 1.8864434094122373
-    dkappa: -0.0027957552220276
+    x: 587257.96593488532
+    y: 4140945.2374522146
+    theta: -1.7971588905651052
+    kappa: -0.00440779581577367
+    s: 1.8864434041883265
+    dkappa: -0.002795722271409627
     ddkappa: 0
   }
-  v: 1.4477237081753795
-  a: -0.21187669644271051
+  v: 1.4477236990486684
+  a: -0.21187670450239493
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587257.93369802576
-    y: 4140945.0974135068
-    theta: -1.7977813887190763
-    kappa: -0.00481025936188271
-    s: 2.0301447434131119
-    dkappa: -0.0027995255206179439
+    x: 587257.933697694
+    y: 4140945.0974135892
+    theta: -1.7977817002418888
+    kappa: -0.0048103786044611041
+    s: 2.0301447372363914
+    dkappa: -0.002799493020092862
     ddkappa: 0
   }
-  v: 1.4261816875062603
-  a: -0.2191495871371747
+  v: 1.4261816775784966
+  a: -0.21914959509437293
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587257.90170554677
-    y: 4140944.9595683687
-    theta: -1.7985466215896466
-    kappa: -0.0052080506989958478
-    s: 2.1716537564280314
-    dkappa: -0.0028030309825538982
+    x: 587257.90170516889
+    y: 4140944.9595684623
+    theta: -1.7985469490179788
+    kappa: -0.0052081655469488125
+    s: 2.1716537492189487
+    dkappa: -0.0028029989272248023
     ddkappa: 0
   }
-  v: 1.4038607861646277
-  a: -0.22741261533113954
+  v: 1.4038607754472334
+  a: -0.22741262316315969
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587257.87022734375
-    y: 4140944.8239390776
-    theta: -1.7992995538882035
-    kappa: -0.0055994475813759157
-    s: 2.310888026647866
-    dkappa: -0.0028064800945506339
+    x: 587257.87022692047
+    y: 4140944.8239391828
+    theta: -1.7992998969658915
+    kappa: -0.0055995581050798347
+    s: 2.3108880183281162
+    dkappa: -0.002806448477258694
     ddkappa: 0
   }
-  v: 1.380674498769084
-  a: -0.23641561365355665
+  v: 1.3806744872755174
+  a: -0.23641562134236355
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587257.83928372327
-    y: 4140944.6906131282
-    theta: -1.8000396990350502
-    kappa: -0.0059841975147261308
-    s: 2.4477577325622004
-    dkappa: -0.0028098706315492728
+    x: 587257.83928325551
+    y: 4140944.6906132451
+    theta: -1.8000400574957853
+    kappa: -0.0059843037873490053
+    s: 2.4477577230549059
+    dkappa: -0.002809839444853092
     ddkappa: 0
   }
-  v: 1.3565613366753362
-  a: -0.24590841473337782
+  v: 1.3565613244206265
+  a: -0.2459084222655924
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 587257.8086414116
-    y: 4140944.5597423064
-    theta: -1.8008874550458009
-    kappa: -0.0063630278584664518
-    s: 2.5821681546330457
-    dkappa: -0.0028130583352189934
+    x: 587257.80864089576
+    y: 4140944.5597424367
+    theta: -1.800887827321016
+    kappa: -0.0063631300921259057
+    s: 2.5821681438628925
+    dkappa: -0.0028130275728679606
     ddkappa: 0
   }
-  v: 1.3314848279761966
-  a: -0.25564085119955482
+  v: 1.3314848149764777
+  a: -0.2556408585664548
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587257.778441753
-    y: 4140944.4313933142
-    theta: -1.8017856205498297
-    kappa: -0.00673519984104707
-    s: 2.7140221769685495
-    dkappa: -0.0028161073227044916
+    x: 587257.77844118734
+    y: 4140944.4313934576
+    theta: -1.8017860056483617
+    kappa: -0.0067352981869834251
+    s: 2.7140221648618712
+    dkappa: -0.0028160769774648446
     ddkappa: 0
   }
-  v: 1.3054335175015817
-  a: -0.26536275568103918
+  v: 1.3054335037736275
+  a: -0.26536276287855848
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587257.74884982756
-    y: 4140944.305627198
-    theta: -1.8026657118534306
-    kappa: -0.0070998822912099937
-    s: 2.8432227889967061
-    dkappa: -0.0028190949528291926
+    x: 587257.74884921336
+    y: 4140944.3056273535
+    theta: -1.8026661095165661
+    kappa: -0.0070999768273801271
+    s: 2.8432227754815274
+    dkappa: -0.0028190650163047591
     ddkappa: 0
   }
-  v: 1.2784209668185134
-  a: -0.274823960806783
+  v: 1.2784209523792711
+  a: -0.274823967835512
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587257.71988725755
-    y: 4140944.1825358504
-    theta: -1.8035270845551157
-    kappa: -0.0074568087448282161
-    s: 2.9696755871390672
-    dkappa: -0.0028220190426082761
+    x: 587257.71988659585
+    y: 4140944.182536019
+    theta: -1.803527494514908
+    kappa: -0.0074568995519745471
+    s: 2.9696755721450958
+    dkappa: -0.0028219895061042605
     ddkappa: 0
   }
-  v: 1.2504857542311181
-  a: -0.28377429920573755
+  v: 1.2504857390972433
+  a: -0.28377430607092308
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587257.69120392308
-    y: 4140944.0622942452
-    theta: -1.8045102566365689
-    kappa: -0.0078069366627404281
-    s: 3.0932911803363492
-    dkappa: -0.0028250308589216132
+    x: 587257.69120321074
+    y: 4140944.0622944273
+    theta: -1.8045106772229524
+    kappa: -0.0078070239800774842
+    s: 3.0932911637949521
+    dkappa: -0.0028250017148528954
     ddkappa: 0
   }
-  v: 1.2216874151706723
-  a: -0.29209607609700083
+  v: 1.2216873993591282
+  a: -0.29209608277665056
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587257.66310470609
-    y: 4140943.94491561
-    theta: -1.8055058245094926
-    kappa: -0.0081490979876972028
-    s: 3.2139862921956817
-    dkappa: -0.0028280102222716747
+    x: 587257.66310394322
+    y: 4140943.9449158055
+    theta: -1.8055062550986376
+    kappa: -0.008149181936540649
+    s: 3.2139862740401024
+    dkappa: -0.0028279814616936584
     ddkappa: 0
   }
-  v: 1.1920857159325373
-  a: -0.29984254536368404
+  v: 1.1920856994643394
+  a: -0.299842551809588
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587257.63570344681
-    y: 4140943.8304525474
-    theta: -1.8064766639247196
-    kappa: -0.0084827603523077236
-    s: 3.3316834528113768
-    dkappa: -0.0028309155810831684
+    x: 587257.6357026347
+    y: 4140943.830452757
+    theta: -1.8064771042673196
+    kappa: -0.008482841016031431
+    s: 3.3316834329771918
+    dkappa: -0.0028308871944677304
     ddkappa: 0
   }
-  v: 1.1617378744979223
-  a: -0.30701996263075926
+  v: 1.1617378573985981
+  a: -0.30701996880089694
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587257.60901685641
-    y: 4140943.7189748655
-    theta: -1.8074221810045243
-    kappa: -0.0088077202675546327
-    s: 3.4463108827977433
-    dkappa: -0.0028337451634792734
+    x: 587257.60901599634
+    y: 4140943.7189750881
+    theta: -1.8074226308452848
+    kappa: -0.00880779773154084
+    s: 3.4463108612232807
+    dkappa: -0.0028337171410702674
     ddkappa: 0
   }
-  v: 1.1307004832855398
-  a: -0.31363458352319862
+  v: 1.130700465584509
+  a: -0.3136345893817386
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 587257.58278430277
-    y: 4140943.6106136264
-    theta: -1.8084319481724052
-    kappa: -0.0091243621556399528
-    s: 3.5578024307328362
-    dkappa: -0.0028360692288689556
+    x: 587257.58278339368
+    y: 4140943.610613863
+    theta: -1.8084324064074333
+    kappa: -0.0091244366095426088
+    s: 3.5578024073595351
+    dkappa: -0.0028360415629884186
     ddkappa: 0
   }
-  v: 1.0990295091516047
-  a: -0.31969266366597415
+  v: 1.0990294908815605
+  a: -0.31969266918327455
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587257.55710330349
-    y: 4140943.5054075881
-    theta: -1.8094782381349608
-    kappa: -0.0094323414244873484
-    s: 3.666097510602206
-    dkappa: -0.0028380156484669652
+    x: 587257.55710234528
+    y: 4140943.5054078382
+    theta: -1.8094787038743274
+    kappa: -0.0094324130284812683
+    s: 3.6660974853749138
+    dkappa: -0.0028379883307237
     ddkappa: 0
   }
-  v: 1.0667802933898347
-  a: -0.32520045868405795
+  v: 1.0667802745861255
+  a: -0.32520046383666612
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 587257.53219337424
-    y: 4140943.4033603524
-    theta: -1.8104931149095509
-    kappa: -0.0097310736400862338
-    s: 3.7711410392426523
-    dkappa: -0.0028399036269701108
+    x: 587257.53219236829
+    y: 4140943.4033606155
+    theta: -1.810493587926937
+    kappa: -0.0097311424794349
+    s: 3.7711410121098563
+    dkappa: -0.002839876646909397
     ddkappa: 0
   }
-  v: 1.0340075517314504
-  a: -0.3301642242024222
+  v: 1.0340075324314602
+  a: -0.33016422897307468
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 587257.50806628715
-    y: 4140943.3045201465
-    theta: -1.811476096733365
-    kappa: -0.010020417623591996
-    s: 3.8728833737859696
-    dkappa: -0.0028417322721323956
+    x: 587257.5080652351
+    y: 4140943.3045204235
+    theta: -1.8114765767989942
+    kappa: -0.010020483784873736
+    s: 3.8728833446999733
+    dkappa: -0.0028417056191399779
     ddkappa: 0
   }
-  v: 1.0007653743451752
-  a: -0.33459021584603887
+  v: 1.0007653545877044
+  a: -0.33459022022366169
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 587257.48473253928
-    y: 4140943.2089299727
-    theta: -1.8124267541815344
-    kappa: -0.010300247489856185
-    s: 3.9712802491027013
-    dkappa: -0.0028435007883635148
+    x: 587257.48473144264
+    y: 4140943.2089302628
+    theta: -1.8124272410625561
+    kappa: -0.010300311060823219
+    s: 3.9712802180197322
+    dkappa: -0.0028434744516828963
     ddkappa: 0
   }
-  v: 0.96710722583723507
-  a: -0.33848468923988018
+  v: 0.967107205661883
+  a: -0.33848469321958852
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 587257.46183402452
-    y: 4140943.1167183439
-    theta: -1.81344869409061
-    kappa: -0.010571015839837153
-    s: 4.0662927152458881
-    dkappa: -0.0028450190597815147
+    x: 587257.4618328826
+    y: 4140943.1167186471
+    theta: -1.813449186640312
+    kappa: -0.010571077027307285
+    s: 4.0662926821261483
+    dkappa: -0.0028449930289581424
     ddkappa: 0
   }
-  v: 0.93308594525135913
-  a: -0.34185390000891819
+  v: 0.9330859246979033
+  a: -0.34185390359201656
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587257.4396548369
-    y: 4140943.0278498433
-    theta: -1.8144634406294786
-    kappa: -0.010832203400018723
-    s: 4.15788707489482
-    dkappa: -0.0028464288500119912
+    x: 587257.43965365086
+    y: 4140943.02785016
+    theta: -1.8144639383496675
+    kappa: -0.010832262322806128
+    s: 4.1578870397024712
+    dkappa: -0.0028464031142242524
     ddkappa: 0
   }
-  v: 0.89875374606877911
-  a: -0.344704103778125
+  v: 0.89875372517655761
+  a: -0.34470410697210724
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587257.41831023234
-    y: 4140942.9423253853
-    theta: -1.815440005009195
-    kappa: -0.01108356270616383
-    s: 4.2460348207987817
-    dkappa: -0.0028477855910983446
+    x: 587257.41830900381
+    y: 4140942.9423257145
+    theta: -1.8154405077041194
+    kappa: -0.011083619449172988
+    s: 4.246034783501873
+    dkappa: -0.0028477601392426196
     ddkappa: 0
   }
-  v: 0.86416221620822942
-  a: -0.34704155617247268
+  v: 0.864162195015521
+  a: -0.34704155899102196
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587257.397805872
-    y: 4140942.8601676533
-    theta: -1.8163781262452758
-    kappa: -0.011325027089358813
-    s: 4.3307125732208105
-    dkappa: -0.0028490889231873604
+    x: 587257.39780460263
+    y: 4140942.8601679946
+    theta: -1.8163786337178709
+    kappa: -0.011325081738081458
+    s: 4.3307125337911412
+    dkappa: -0.002849063744084771
     ddkappa: 0
   }
-  v: 0.8293623180259474
-  a: -0.34887251281693332
+  v: 0.82936229656935367
+  a: -0.34887251527992214
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587257.37814619078
-    y: 4140942.7813944165
-    theta: -1.8172775998889625
-    kappa: -0.011556544323761579
-    s: 4.4119020173814407
-    dkappa: -0.0028503385643839818
+    x: 587257.37814488239
+    y: 4140942.78139477
+    theta: -1.8172781119411405
+    kappa: -0.011556596964151214
+    s: 4.4119019757943576
+    dkappa: -0.0028503136467967045
     ddkappa: 0
   }
-  v: 0.79440438831567317
-  a: -0.35020322933647913
+  v: 0.79440436662949843
+  a: -0.35020323146996918
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 587257.35933441215
-    y: 4140942.7060185922
-    theta: -1.8181382772956791
-    kappa: -0.011778076448218185
-    s: 4.4895898409024557
-    dkappa: -0.0028515343097884663
+    x: 587257.3593330665
+    y: 4140942.7060189573
+    theta: -1.8181387937286158
+    kappa: -0.011778127166565995
+    s: 4.489589797136599
+    dkappa: -0.0028515096424358418
     ddkappa: 0
   }
-  v: 0.7593381383086496
-  a: -0.35103996135608206
+  v: 0.75933811642428273
+  a: -0.35103996319232439
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587257.340963318
-    y: 4140942.6341517149
-    theta: -1.8190623479784902
-    kappa: -0.011990047310737769
-    s: 4.56376767125064
-    dkappa: -0.0028524656950239453
+    x: 587257.34096193477
+    y: 4140942.634152092
+    theta: -1.819062867762282
+    kappa: -0.011990096310710059
+    s: 4.563767625287614
+    dkappa: -0.0028524412693142244
     ddkappa: 0
   }
-  v: 0.72421265367362231
-  a: -0.35138896450071427
+  v: 0.7242126316189178
+  a: -0.35138896607814929
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587257.32345047034
-    y: 4140942.5656918781
-    theta: -1.8199456305328368
-    kappa: -0.01219199104918273
-    s: 4.6344320131815255
-    dkappa: -0.0028533468410874611
+    x: 587257.32344905147
+    y: 4140942.5656922655
+    theta: -1.8199461534522752
+    kappa: -0.012192038415104809
+    s: 4.6344319650055175
+    dkappa: -0.0028533226457160567
     ddkappa: 0
   }
-  v: 0.68907639451684
-  a: -0.35125649439534784
+  v: 0.68907637231549845
+  a: -0.35125649575860524
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587257.30680804851
-    y: 4140942.5006346405
-    theta: -1.8207850140061723
-    kappa: -0.012383897751914261
-    s: 4.701584186183144
-    dkappa: -0.0028541841923070445
+    x: 587257.30680659576
+    y: 4140942.5006350391
+    theta: -1.8207855399038835
+    kappa: -0.012383943564648519
+    s: 4.7015841357804806
+    dkappa: -0.0028541602158241074
     ddkappa: 0
   }
-  v: 0.65397719538205368
-  a: -0.3506488066649549
+  v: 0.65397717305300362
+  a: -0.35064880786485364
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587257.29103454784
-    y: 4140942.4389741207
-    theta: -1.8215805729027188
-    kappa: -0.012565784770464276
-    s: 4.7652302619197808
-    dkappa: -0.0028549778243930611
+    x: 587257.29103306332
+    y: 4140942.4389745304
+    theta: -1.8215811016216064
+    kappa: -0.012565829110737892
+    s: 4.7652302092784149
+    dkappa: -0.0028549540553685559
     ddkappa: 0
   }
-  v: 0.61896226525051745
-  a: -0.34957215693450755
+  v: 0.618962242807296
+  a: -0.34957215802805586
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 587257.27612730162
-    y: 4140942.3806998944
-    theta: -1.8223324407034813
-    kappa: -0.012737682858306438
-    s: 4.8253810016757175
-    dkappa: -0.0028557278715328951
+    x: 587257.27612578683
+    y: 4140942.3807003144
+    theta: -1.8223329720869328
+    kappa: -0.012737725806601733
+    s: 4.8253809467846613
+    dkappa: -0.0028557042985718809
     ddkappa: 0
   }
-  v: 0.58407818754098817
-  a: -0.34803280082897786
+  v: 0.584078164991122
+  a: -0.34803280187937341
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587257.26208238781
-    y: 4140942.3257966288
-    theta: -1.8230408144906103
-    kappa: -0.012899637238934088
-    s: 4.882052230098064
-    dkappa: -0.0028564345310513237
+    x: 587257.26208084449
+    y: 4140942.3257970586
+    theta: -1.823041348382721
+    kappa: -0.012899678875370335
+    s: 4.8820521729468407
+    dkappa: -0.002856411142810455
     ddkappa: 0
   }
-  v: 0.5493883026756502
-  a: -0.34551898684841786
+  v: 0.54938828002342788
+  a: -0.34551898782351942
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587257.24889348319
-    y: 4140942.2742396076
-    theta: -1.8237060127218014
-    kappa: -0.013051720825127249
-    s: 4.9352694608950349
-    dkappa: -0.0028570981210904673
+    x: 587257.24889191333
+    y: 4140942.2742400463
+    theta: -1.8237065489678135
+    kappa: -0.013051761229238382
+    s: 4.9352694014739962
+    dkappa: -0.0028570749063095832
     ddkappa: 0
   }
-  v: 0.51502207989563331
-  a: -0.34157448113800348
+  v: 0.51502205715488714
+  a: -0.34157448191360074
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 587257.23655083077
-    y: 4140942.225990688
-    theta: -1.8243285272808289
-    kappa: -0.013194046129293723
-    s: 4.9850720645136564
-    dkappa: -0.0028577191325784724
+    x: 587257.23654923588
+    y: 4140942.2259911355
+    theta: -1.82432906572777
+    kappa: -0.013194085379712483
+    s: 4.9850720028151416
+    dkappa: -0.0028576960801257852
     ddkappa: 0
   }
-  v: 0.4811181117692897
-  a: -0.33628881579039266
+  v: 0.48111808896571845
+  a: -0.33628881625336887
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587257.2247811706
-    y: 4140942.1810662029
-    theta: -1.8249672395591123
-    kappa: -0.01332694998200085
-    s: 5.0315128229973283
-    dkappa: -0.0028581612857960445
+    x: 587257.22477955173
+    y: 4140942.1810666593
+    theta: -1.8249677796294734
+    kappa: -0.013326988223102489
+    s: 5.0315127590167918
+    dkappa: -0.0028581383854102628
     ddkappa: 0
   }
-  v: 0.44780603765570626
-  a: -0.32975152289824339
+  v: 0.44780601482576565
+  a: -0.3297515229465755
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587257.21381411329
-    y: 4140942.1393391537
-    theta: -1.8255679672991103
-    kappa: -0.013450443284694222
-    s: 5.0746570346648934
-    dkappa: -0.0028585547616407966
+    x: 587257.21381247218
+    y: 4140942.1393396179
+    theta: -1.8255685087881985
+    kappa: -0.013450480596006717
+    s: 5.0746569684019285
+    dkappa: -0.0028585320026926408
     ddkappa: 0
   }
-  v: 0.41520654370470395
-  a: -0.3220521345542135
+  v: 0.41520652089449728
+  a: -0.32205213409697203
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587257.20366547036
-    y: 4140942.1007259758
-    theta: -1.8261238673505529
-    kappa: -0.013564720926159169
-    s: 5.1145816187897166
-    dkappa: -0.0028589188744350216
+    x: 587257.20366380876
+    y: 4140942.100726448
+    theta: -1.8261244101501992
+    kappa: -0.01356475737659359
+    s: 5.1145815502489622
+    dkappa: -0.0028588962463682276
     ddkappa: 0
   }
-  v: 0.38343136285683788
-  a: -0.31328018285096093
+  v: 0.38343134012100677
+  a: -0.31328018180831013
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587257.1943129627
-    y: 4140942.0651419051
-    theta: -1.8266361594129799
-    kappa: -0.013670033775715254
-    s: 5.1513742202787522
-    dkappa: -0.0028592544235015592
+    x: 587257.19431128225
+    y: 4140942.0651423847
+    theta: -1.8266367034180393
+    kappa: -0.013670069432323852
+    s: 5.1513741494706915
+    dkappa: -0.0028592319160472804
     ddkappa: 0
   }
-  v: 0.35258327484339747
-  a: -0.30352519988114368
+  v: 0.35258325224401232
+  a: -0.30352519818434132
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 587257.18573181285
-    y: 4140942.0324926665
-    theta: -1.8271062002404515
-    kappa: -0.013766660840356378
-    s: 5.1851323143516206
-    dkappa: -0.0028595622978158453
+    x: 587257.185730115
+    y: 4140942.0324931527
+    theta: -1.8271067453491432
+    kappa: -0.013766695768124207
+    s: 5.18513224129327
+    dkappa: -0.00285953990102489
     ddkappa: 0
   }
-  v: 0.32275610618640627
-  a: -0.29287671773741963
+  v: 0.32275608379185716
+  a: -0.29287671532881715
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 587257.17789497157
-    y: 4140942.0026753452
-    theta: -1.8275354711390992
-    kappa: -0.013854906702039965
-    s: 5.2159623112196831
-    dkappa: -0.0028598434678405718
+    x: 587257.17789325793
+    y: 4140942.0026758378
+    theta: -1.827536017253317
+    kappa: -0.013854940963695321
+    s: 5.2159622359351667
+    dkappa: -0.0028598211721127054
     ddkappa: 0
   }
-  v: 0.29403473019862209
-  a: -0.28142426851244684
+  v: 0.29403470808250926
+  a: -0.28142426534548931
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587257.17077334621
-    y: 4140941.975579252
-    theta: -1.8279255654693038
-    kappa: -0.013935098954976135
-    s: 5.2439786607651122
-    dkappa: -0.0028600989773603473
+    x: 587257.17077161837
+    y: 4140941.9755797507
+    theta: -1.8279261124948933
+    kappa: -0.013935132610819847
+    s: 5.24397858328613
+    dkappa: -0.0028600767734706534
     ddkappa: 0
   }
-  v: 0.26649506698353681
-  a: -0.26925738429888307
+  v: 0.26649504522356132
+  a: -0.2692573803381092
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 587257.16433602828
-    y: 4140941.9510867847
-    theta: -1.8282781761524585
-    kappa: -0.014007585642916872
-    s: 5.2693029572199652
-    dkappa: -0.0028603299353163556
+    x: 587257.16433428752
+    y: 4140941.9510872895
+    theta: -1.8282787239994627
+    kappa: -0.014007618750670432
+    s: 5.2693028775861448
+    dkappa: -0.002860307814438667
     ddkappa: 0
   }
-  v: 0.24020408343537675
-  a: -0.25646559718938633
+  v: 0.24020406211223094
+  a: -0.25646559241042843
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 587257.15855052043
-    y: 4140941.9290743014
-    theta: -1.8285950831821367
-    kappa: -0.014072732696445213
-    s: 5.2920630438452605
-    dkappa: -0.0028605375076410176
+    x: 587257.15854876826
+    y: 4140941.9290748108
+    theta: -1.8285956317650105
+    kappa: -0.014072765311117747
+    s: 5.2920629621044029
+    dkappa: -0.0028605154613684045
     ddkappa: 0
   }
-  v: 0.21521979323910218
-  a: -0.24313843927661452
+  v: 0.21521977243536078
+  a: -0.24313843366619861
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 587257.15338296478
-    y: 4140941.9094129787
-    theta: -1.8288781411393731
-    kappa: -0.014130921370264424
-    s: 5.3123921176100488
-    dkappa: -0.002860722909092648
+    x: 587257.15338120237
+    y: 4140941.9094134932
+    theta: -1.8288786903771646
+    kappa: -0.014130953544038507
+    s: 5.3123920338182593
+    dkappa: -0.0028607009294549755
     ddkappa: 0
   }
-  v: 0.1915912568704079
-  a: -0.22936544265322562
+  v: 0.19159123666941802
+  a: -0.22936543620917133
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 587257.1487983698
-    y: 4140941.8919696836
-    theta: -1.829129266711681
-    kappa: -0.014182545680487163
-    s: 5.3304278338704858
-    dkappa: -0.0028608873950901202
+    x: 587257.14879659843
+    y: 4140941.8919702023
+    theta: -1.8291298165281824
+    kappa: -0.014182577462623481
+    s: 5.3304277480921982
+    dkappa: -0.0028608654745686636
     ddkappa: 0
   }
-  v: 0.16935858159572259
-  a: -0.21523613941187755
+  v: 0.16935856208049482
+  a: -0.21523613214309803
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 587257.14476083859
-    y: 4140941.8766078353
-    theta: -1.8293504262153868
-    kappa: -0.014228009841924679
-    s: 5.3463114110489087
-    dkappa: -0.0028610322535475219
+    x: 587257.1447590593
+    y: 4140941.8766083587
+    theta: -1.829350976539253
+    kappa: -0.014228041278685518
+    s: 5.3463113233567938
+    dkappa: -0.00286101038508665
     ddkappa: 0
   }
-  v: 0.14855292147220925
-  a: -0.20084006164522825
+  v: 0.14855290272430841
+  a: -0.20084005357173038
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 587257.141233796
-    y: 4140941.8631882761
-    theta: -1.8295436231208375
-    kappa: -0.014267725705375971
-    s: 5.3601867353129071
-    dkappa: -0.0028611587967088169
+    x: 587257.14123200974
+    y: 4140941.8631888023
+    theta: -1.8295441738856733
+    kappa: -0.014267756839967559
+    s: 5.3601866457876737
+    dkappa: -0.0028611369737247356
     ddkappa: 0
   }
-  v: 0.12919647734776521
-  a: -0.18626674144593561
+  v: 0.12919645944620037
+  a: -0.1862667325988199
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 587257.13818021608
-    y: 4140941.8515701289
-    theta: -1.829710885580027
-    kappa: -0.014302110194916968
-    s: 5.3721994652543952
-    dkappa: -0.0028612683529825049
+    x: 587257.138178424
+    y: 4140941.8515706589
+    theta: -1.8297114367244423
+    kappa: -0.014302141067450664
+    s: 5.3721993739844809
+    dkappa: -0.0028612465693690684
     ddkappa: 0
   }
-  v: 0.11130249686102187
-  a: -0.1716057109066576
+  v: 0.11130247988113764
+  a: -0.17160570132811814
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 587257.13556285028
-    y: 4140941.8416116736
-    theta: -1.8298542539562155
-    kappa: -0.014331582745189714
-    s: 5.3824961365686894
-    dkappa: -0.0028613622587762822
+    x: 587257.13556105318
+    y: 4140941.8416122058
+    theta: -1.8298548054238475
+    kappa: -0.014331613392662017
+    s: 5.3824960436498337
+    dkappa: -0.0028613405089078611
     ddkappa: 0
   }
-  v: 0.094875274441344665
-  a: -0.15694650212005218
+  v: 0.094875258453711558
+  a: -0.15694649186337672
   relative_time: 5.7999999999999963
 }
 trajectory_point {
   path_point {
-    x: 587257.13334445446
-    y: 4140941.8331712042
-    theta: -1.8299757683551277
-    kappa: -0.014356562738691542
-    s: 5.39122326673358
-    dkappa: -0.0028614418503317
+    x: 587257.13334265316
+    y: 4140941.83317174
+    theta: -1.8299763200946357
+    kappa: -0.014356593194982965
+    s: 5.3912231722682957
+    dkappa: -0.00286142012906312
     ddkappa: 0
   }
-  v: 0.079910151308833846
-  a: -0.14237864717877718
+  v: 0.07991013637813861
+  a: -0.14237863630834718
   relative_time: 5.8999999999999959
 }
 trajectory_point {
   path_point {
-    x: 587257.131488017
-    y: 4140941.8261078997
-    theta: -1.8300774561573743
-    kappa: -0.014377466943064241
-    s: 5.3985264596884006
-    dkappa: -0.0028615084555588259
+    x: 587257.13148621214
+    y: 4140941.826108438
+    theta: -1.8300780081223973
+    kappa: -0.014377497238957015
+    s: 5.39852636378533
+    dkappa: -0.002861486758222365
     ddkappa: 0
   }
-  v: 0.0663935154743231
-  a: -0.12799167817549065
+  v: 0.066393501658260023
+  a: -0.12799166676678106
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 587257.12995698559
-    y: 4140941.8202826888
-    theta: -1.8301613195517639
-    kappa: -0.014394706948383244
-    s: 5.4045495105131067
-    dkappa: -0.0028615633858709039
+    x: 587257.12995517789
+    y: 4140941.820283229
+    theta: -1.8301618717008508
+    kappa: -0.014394737111597869
+    s: 5.404549413286265
+    dkappa: -0.0028615417082703547
     ddkappa: 0
   }
-  v: 0.054302801739380867
-  a: -0.11387512720285048
+  v: 0.054302789087542068
+  a: -0.11387511534242994
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 587257.1287154957
-    y: 4140941.8155591143
-    theta: -1.8302293230692401
-    kappa: -0.014408686604446807
-    s: 5.4094335101073492
-    dkappa: -0.0028616079280190118
+    x: 587257.12871368567
+    y: 4140941.8155596564
+    theta: -1.8302298753657458
+    kappa: -0.014408716659697434
+    s: 5.4094334116752583
+    dkappa: -0.0028615862664208088
     ddkappa: 0
   }
-  v: 0.043606491696309618
-  a: -0.10011852635351456
+  v: 0.043606480249075347
+  a: -0.10011851413904541
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
-    x: 587257.12772859761
-    y: 4140941.8118042015
-    theta: -1.8302833811172114
-    kappa: -0.014419799458065173
-    s: 5.4133159498695438
-    dkappa: -0.0028616433359267262
+    x: 587257.12772678584
+    y: 4140941.8118047453
+    theta: -1.8302839335291627
+    kappa: -0.014419829427133843
+    s: 5.4133158503542571
+    dkappa: -0.0028616216870481336
     ddkappa: 0
   }
-  v: 0.034264113728145924
-  a: -0.086811407720140912
+  v: 0.03426410351557569
+  a: -0.086811395260378968
   relative_time: 6.2999999999999945
 }
 trajectory_point {
   path_point {
-    x: 587257.12696248421
-    y: 4140941.808889322
-    theta: -1.8303253455141013
-    kappa: -0.014428426190349754
-    s: 5.4163298263759447
-    dkappa: -0.0028616708225247772
+    x: 587257.12696067116
+    y: 4140941.8088898673
+    theta: -1.8303258980140307
+    kappa: -0.014428456092179472
+    s: 5.4163297259019618
+    dkappa: -0.0028616491835191413
     ddkappa: 0
   }
-  v: 0.026226243008660836
-  a: -0.07404330339538738
+  v: 0.02622623404938396
+  a: -0.07404329081018228
   relative_time: 6.3999999999999941
 }
 trajectory_point {
   path_point {
-    x: 587257.12638471869
-    y: 4140941.8066910617
-    theta: -1.8303569930239763
-    kappa: -0.014434932054002324
-    s: 5.418602746059725
-    dkappa: -0.0028616915515857115
+    x: 587257.12638290459
+    y: 4140941.8066916075
+    theta: -1.8303575455887233
+    kappa: -0.014434961904808963
+    s: 5.418602644752788
+    dkappa: -0.002861669920024778
     ddkappa: 0
   }
-  v: 0.019434501502359338
-  a: -0.061903745471911922
+  v: 0.019434493802465282
+  a: -0.061903732892206756
   relative_time: 6.4999999999999938
 }
 trajectory_point {
   path_point {
-    x: 587257.12596446159
-    y: 4140941.8050920833
-    theta: -1.8303800128911614
-    kappa: -0.014439664310604172
-    s: 5.4202560298900417
-    dkappa: -0.0028617066295585523
+    x: 587257.12596264691
+    y: 4140941.80509263
+    theta: -1.8303805655016439
+    kappa: -0.014439694124007241
+    s: 5.4202559278758287
+    dkappa: -0.002861685003411846
     ddkappa: 0
   }
-  v: 0.013821557964480641
-  a: -0.050482266042372485
+  v: 0.013821551516410119
+  a: -0.050482253610204079
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
-    x: 587257.12567269919
-    y: 4140941.8039819966
-    theta: -1.8303959943747738
-    kappa: -0.014442949667905287
-    s: 5.421403818051111
-    dkappa: -0.002861717097403457
+    x: 587257.12567088392
+    y: 4140941.8039825438
+    theta: -1.8303965470157262
+    kappa: -0.014442979455077524
+    s: 5.4214037154538168
+    dkappa: -0.0028616954750147246
     ddkappa: 0
   }
-  v: 0.0093111279409984447
-  a: -0.039868397199426968
+  v: 0.0093111227224337532
+  a: -0.039868385067925831
   relative_time: 6.6999999999999931
 }
 trajectory_point {
   path_point {
-    x: 587257.1254824705
-    y: 4140941.8032582216
-    theta: -1.8304064142831431
-    kappa: -0.014445091717113542
-    s: 5.4221521746212877
-    dkappa: -0.00286172392242638
+    x: 587257.125480655
+    y: 4140941.8032587692
+    theta: -1.8304069669428238
+    kappa: -0.014445121486949358
+    s: 5.42215207156209
+    dkappa: -0.0028617023024870968
     ddkappa: 0
   }
-  v: 0.0058179737686201334
-  a: -0.030151671035733414
+  v: 0.0058179697413758176
+  a: -0.030151659369123458
   relative_time: 6.7999999999999927
 }
 trajectory_point {
   path_point {
-    x: 587257.12536909489
-    y: 4140941.8028268544
-    theta: -1.8304126245080998
-    kappa: -0.014446368370183871
-    s: 5.4225981922521305
-    dkappa: -0.0028617279901137311
+    x: 587257.12536727916
+    y: 4140941.8028274029
+    theta: -1.8304131771779666
+    kappa: -0.014446398129486622
+    s: 5.42259808884755
+    dkappa: -0.0028617063716336703
     ddkappa: 0
   }
-  v: 0.0032479045747880117
-  a: -0.021421619643949612
+  v: 0.0032479016837015304
+  a: -0.021421608617548653
   relative_time: 6.8999999999999924
 }
 trajectory_point {
   path_point {
-    x: 587257.1253104
-    y: 4140941.802603534
-    theta: -1.8304158395591361
-    kappa: -0.014447029297107427
-    s: 5.4228290968474795
-    dkappa: -0.0028617300959670354
+    x: 587257.12530858431
+    y: 4140941.802604083
+    theta: -1.8304163922334902
+    kappa: -0.014447059050795552
+    s: 5.4228289932076246
+    dkappa: -0.0028617084782419037
     ddkappa: 0
   }
-  v: 0.0014977762776780788
-  a: -0.013767775116733555
+  v: 0.0014977744495002475
+  a: -0.013767764916952923
   relative_time: 6.999999999999992
 }
 trajectory_point {
   path_point {
-    x: 587257.1252869491
-    y: 4140941.8025143091
-    theta: -1.8304171240974523
-    kappa: -0.014447293363200789
-    s: 5.4229213522425308
-    dkappa: -0.002861730937337595
+    x: 587257.12528513337
+    y: 4140941.8025148581
+    theta: -1.8304176767730498
+    kappa: -0.01444732311453276
+    s: 5.4229212484692342
+    dkappa: -0.0028617093199137266
     ddkappa: 0
   }
-  v: 0.00045549158620058225
-  a: -0.0072796695467432487
+  v: 0.00045549072848657132
+  a: -0.007279660371087791
   relative_time: 7.0999999999999917
 }
 trajectory_point {
   path_point {
-    x: 587257.12528226874
-    y: 4140941.8024965012
-    theta: -1.8304173804699058
-    kappa: -0.014447346066395106
-    s: 5.4229397648829059
-    dkappa: -0.0028617311052611459
+    x: 587257.12528045289
+    y: 4140941.8024970503
+    theta: -1.8304179331455395
+    kappa: -0.014447375817213247
+    s: 5.4229396610677512
+    dkappa: -0.0028617094878972656
     ddkappa: 0
   }
-  v: 0
-  a: -0.0020468350266364694
+  v: 1.1102230246220011e-16
+  a: -0.0020468270837048236
   relative_time: 7.1999999999999913
 }
 trajectory_point {
   path_point {
-    x: 587257.12528307608
-    y: 4140941.8024995732
-    theta: -1.8304173362428908
-    kappa: -0.014447336974525306
-    s: 5.4229365885037293
-    dkappa: -0.0028617310762925211
+    x: 587257.12528126035
+    y: 4140941.8025001222
+    theta: -1.8304178889189382
+    kappa: -0.014447366725518437
+    s: 5.4229364847259625
+    dkappa: -0.0028617094589185677
     ddkappa: 0
   }
-  v: 1.2978094550292525e-06
-  a: 0.0018411963509286711
+  v: 1.2985330044718357e-06
+  a: 0.0018412028414442406
   relative_time: 7.2999999999999909
 }
 trajectory_point {
   path_point {
-    x: 587257.12527950713
-    y: 4140941.8024859941
-    theta: -1.8304175317361691
-    kappa: -0.014447377162619249
-    s: 5.4229506288086995
-    dkappa: -0.0028617312043403087
+    x: 587257.12527769129
+    y: 4140941.8024865426
+    theta: -1.83041808441411
+    kappa: -0.014447406913604171
+    s: 5.4229505251330332
+    dkappa: -0.0028617095870133212
     ddkappa: 0
   }
-  v: 0.00032042809567905104
-  a: 0.0042948924932943392
+  v: 0.00032042938608927416
+  a: 0.0042948973006081085
   relative_time: 7.3999999999999906
 }
 trajectory_point {
   path_point {
-    x: 587257.12526534358
-    y: 4140941.8024321049
-    theta: -1.8304183075566747
-    kappa: -0.014447536650186915
-    s: 5.4230063481491628
-    dkappa: -0.0028617317125015128
+    x: 587257.12526352773
+    y: 4140941.8024326535
+    theta: -1.8304188602385774
+    kappa: -0.014447566400408748
+    s: 5.4230062446234681
+    dkappa: -0.002861710095358583
     ddkappa: 0
   }
-  v: 0.00080948073051877764
-  a: 0.0052247213078027007
+  v: 0.00080948240746813479
+  a: 0.0052247241900349314
   relative_time: 7.49999999999999
 }
 trajectory_point {
   path_point {
-    x: 587257.1252382407
-    y: 4140941.8023289852
-    theta: -1.8304197921323124
-    kappa: -0.014447841838509575
-    s: 5.4231129702031868
-    dkappa: -0.0028617326848962132
+    x: 587257.12523642473
+    y: 4140941.8023295337
+    theta: -1.8304203448202863
+    kappa: -0.014447871586960936
+    s: 5.4231128668560764
+    dkappa: -0.002861711068104498
     ddkappa: 0
   }
-  v: 0.001311592376555706
-  a: 0.0045411507017955885
+  v: 0.0013115942349800093
+  a: 0.0045411514059734157
   relative_time: 7.59999999999999
 }
 trajectory_point {
   path_point {
-    x: 587257.12519995519
-    y: 4140941.8021833175
-    theta: -1.8304218892457595
-    kappa: -0.01444827294792898
-    s: 5.4232635846546344
-    dkappa: -0.0028617340585022251
+    x: 587257.12519813911
+    y: 4140941.8021838665
+    theta: -1.8304224419413786
+    kappa: -0.01444830269368798
+    s: 5.4232634814929286
+    dkappa: -0.0028617124422060259
     ddkappa: 0
   }
-  v: 0.0016609464871054718
-  a: 0.0021546485826152251
+  v: 0.0016609482960882132
+  a: 0.0021546468446719358
   relative_time: 7.6999999999999895
 }
 trajectory_point {
   path_point {
-    x: 587257.12515657244
-    y: 4140941.8020182564
-    theta: -1.8304242655682805
-    kappa: -0.01444876145513652
-    s: 5.4234342518722372
-    dkappa: -0.002861735614989759
+    x: 587257.12515475624
+    y: 4140941.8020188049
+    theta: -1.8304248182719738
+    kappa: -0.014448791197723647
+    s: 5.4234341488783278
+    dkappa: -0.0028617139992546631
     ddkappa: 0
   }
-  v: 0.0016827733062178497
-  a: -0.0020243171423962245
+  v: 0.00168277480788131
+  a: -0.0020243215976212467
   relative_time: 7.7999999999999892
 }
 trajectory_point {
   path_point {
-    x: 587257.125118734
-    y: 4140941.80187429
-    theta: -1.8304263381935466
-    kappa: -0.014449187530462412
-    s: 5.4235831075886667
-    dkappa: -0.0028617369725560818
+    x: 587257.12511691765
+    y: 4140941.8018748383
+    theta: -1.8304268909039569
+    kappa: -0.014449217270216213
+    s: 5.4235830047177629
+    dkappa: -0.0028617153573101662
     ddkappa: 0
   }
-  v: 0.0011933498686765307
-  a: -0.0080852785658969816
+  v: 0.0011933507770725557
+  a: -0.0080852860246575353
   relative_time: 7.8999999999999888
 }
 trajectory_point {
   path_point {
-    x: 587257.12510186562
-    y: 4140941.80181011
-    theta: -1.8304272621714504
-    kappa: -0.014449377475164882
-    s: 5.4236494675796116
-    dkappa: -0.0028617375777601744
+    x: 587257.12510004919
+    y: 4140941.8018106581
+    theta: -1.8304278148847626
+    kappa: -0.014449407213636507
+    s: 5.4236493647568791
+    dkappa: -0.0028617159627322758
     ddkappa: 0
   }
-  v: 1.4432899320075759e-15
-  a: -0.016117767780544658
+  v: 1.3322676295454547e-15
+  a: -0.016117778540188445
   relative_time: 7.9999999999999885
 }
 decision {
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_stop_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_stop_0.pb.txt
index e7ce6bbb8e..552d1bc7cb 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_stop_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_stop_0.pb.txt
@@ -9,7 +9,7 @@ trajectory_point {
     y: 4141549.0626593772
     z: 0
     theta: 1.2102934925174473
-    kappa: -0.030546891083324958
+    kappa: -0.030640801125550045
     s: 0
     dkappa: 0
     ddkappa: 0
@@ -20,12 +20,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.68194682128
-    y: 4141549.1130769788
-    theta: 1.2086616300170123
-    kappa: -0.030473786813074642
+    x: 587045.68194795167
+    y: 4141549.113076543
+    theta: 1.2086569378213541
+    kappa: -0.030566186589195066
     s: 0.054037806686823439
-    dkappa: 0.00014539190758161333
+    dkappa: 0.00014839557405833055
     ddkappa: 0
   }
   v: 2.6926029332900914
@@ -34,12 +34,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.70125825389
-    y: 4141549.1631454844
-    theta: 1.2070410352450165
-    kappa: -0.030401188723468073
+    x: 587045.70126050687
+    y: 4141549.1631446155
+    theta: 1.2070316830709262
+    kappa: -0.030492088690704269
     s: 0.10770145066493698
-    dkappa: 0.00028977710831665693
+    dkappa: 0.00029576364360908524
     ddkappa: 0
   }
   v: 2.6736947978545964
@@ -48,12 +48,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.72043216112
-    y: 4141549.21285743
-    theta: 1.2054319588772524
-    kappa: -0.030329107637190281
+    x: 587045.72043552867
+    y: 4141549.2128561311
+    theta: 1.2054179797152242
+    kappa: -0.030418518476349281
     s: 0.1609829319343406
-    dkappa: 0.00043313407773306267
+    dkappa: 0.00044208223950389811
     ddkappa: 0
   }
   v: 2.6543866624191019
@@ -62,12 +62,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.73946566437
-    y: 4141549.2622053516
-    theta: 1.2038346512392388
-    kappa: -0.030257554376926309
+    x: 587045.7394701381
+    y: 4141549.2622036254
+    theta: 1.2038160788500087
+    kappa: -0.030345486992401728
     s: 0.21387425049503439
-    dkappa: 0.00057544129135876263
+    dkappa: 0.00058732939259440364
     ddkappa: 0
   }
   v: 2.634678526983607
@@ -76,12 +76,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.75835610367
-    y: 4141549.3111823536
-    theta: 1.2022493438925022
-    kappa: -0.030186538940788918
+    x: 587045.75836167554
+    y: 4141549.3111802042
+    theta: 1.2022262127510326
+    kappa: -0.030273004443526069
     s: 0.26636801586103515
-    dkappa: 0.00071667886465511747
+    dkappa: 0.0007314848075452193
     ddkappa: 0
   }
   v: 2.6146890758563712
@@ -90,12 +90,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.77710260451
-    y: 4141549.3597861677
-    theta: 1.2006761182502768
-    kappa: -0.030116064617053691
+    x: 587045.77710926614
+    y: 4141549.3597835982
+    theta: 1.200648463111897
+    kappa: -0.030201074185930592
     s: 0.31846179737816255
-    dkappa: 0.00085684025780362555
+    dkappa: 0.00087454180943105577
     ddkappa: 0
   }
   v: 2.5946890758563712
@@ -104,12 +104,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.795705161
-    y: 4141549.40801678
-    theta: 1.1991149824473204
-    kappa: -0.030046131427570441
+    x: 587045.79571290407
+    y: 4141549.4080137932
+    theta: 1.199082838134921
+    kappa: -0.030129696241916505
     s: 0.37015557889529
-    dkappa: 0.000995925427348728
+    dkappa: 0.0010165003538986012
     ddkappa: 0
   }
   v: 2.5746890758563712
@@ -118,12 +118,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.81416377344
-    y: 4141549.4558741893
-    theta: 1.1975659437760793
-    kappa: -0.029976739372339176
+    x: 587045.81417258945
+    y: 4141549.4558707885
+    theta: 1.1975293451756412
+    kappa: -0.030058870611483816
     s: 0.42144936041241737
-    dkappa: 0.0011339343732904242
+    dkappa: 0.0011573604409478551
     ddkappa: 0
   }
   v: 2.5546890758563712
@@ -132,12 +132,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.83247844153
-    y: 4141549.5033583962
-    theta: 1.1960290091810943
-    kappa: -0.029907888451359884
+    x: 587045.83248832228
+    y: 4141549.5033545853
+    theta: 1.1959879912386755
+    kappa: -0.029988597294632525
     s: 0.4723431419295448
-    dkappa: 0.0012708670956287148
+    dkappa: 0.0012971220705788179
     ddkappa: 0
   }
   v: 2.5346890758563712
@@ -146,12 +146,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.85092904675
-    y: 4141549.5503590293
-    theta: 1.194517121689499
-    kappa: -0.029842469008075762
+    x: 587045.85094072809
+    y: 4141549.5503545026
+    theta: 1.1944719950327036
+    kappa: -0.029921790498052228
     s: 0.5228369234466721
-    dkappa: 0.0013470933510321573
+    dkappa: 0.0013749942675008275
     ddkappa: 0
   }
   v: 2.5146890758563711
@@ -160,12 +160,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.869655843
-    y: 4141549.5968207773
-    theta: 1.1930367229525212
-    kappa: -0.029781929451906816
+    x: 587045.86967043381
+    y: 4141549.596815079
+    theta: 1.1929879359704136
+    kappa: -0.0298599105831761
     s: 0.57293070496379961
-    dkappa: 0.0013327312213631084
+    dkappa: 0.0013605135796476101
     ddkappa: 0
   }
   v: 2.4946890758563711
@@ -174,12 +174,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.8882331053
-    y: 4141549.6429115278
-    theta: 1.1915681219794194
-    kappa: -0.02972187330548922
+    x: 587045.88825058227
+    y: 4141549.6429046653
+    theta: 1.1915157037268485
+    kappa: -0.0297985247808465
     s: 0.62262448648092694
-    dkappa: 0.0013184837736304926
+    dkappa: 0.0013461485204206587
     ddkappa: 0
   }
   v: 2.4746890758563715
@@ -188,12 +188,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.90666083375
-    y: 4141549.6886312794
-    theta: 1.1901113255877023
-    kappa: -0.029662300568822984
+    x: 587045.90668117371
+    y: 4141549.6886232635
+    theta: 1.1900553051702105
+    kappa: -0.02973763309106342
     s: 0.67191826799805432
-    dkappa: 0.0013043510078343092
+    dkappa: 0.0013318990898199732
     ddkappa: 0
   }
   v: 2.4546890758563715
@@ -202,12 +202,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.9249390281
-    y: 4141549.7339800331
-    theta: 1.1886663402885491
-    kappa: -0.029603211241908095
+    x: 587045.924962208
+    y: 4141549.7339708721
+    theta: 1.1886067468600985
+    kappa: -0.029677235513826859
     s: 0.72081204951518174
-    dkappa: 0.0012903329239745583
+    dkappa: 0.0013177652878455532
     ddkappa: 0
   }
   v: 2.4346890758563715
@@ -216,12 +216,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.94306768884
-    y: 4141549.7789577888
-    theta: 1.1872331722927334
-    kappa: -0.029544605324744566
+    x: 587045.9430936852
+    y: 4141549.7789474926
+    theta: 1.1871700350534717
+    kappa: -0.02961733204913683
     s: 0.76930583103230943
-    dkappa: 0.0012764295220512405
+    dkappa: 0.0013037471144973993
     ddkappa: 0
   }
   v: 2.4146890758563715
@@ -230,12 +230,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.9610468155
-    y: 4141549.8235645466
-    theta: 1.185811827516513
-    kappa: -0.029486482817332391
+    x: 587045.96107560513
+    y: 4141549.8235531244
+    theta: 1.1857451757105844
+    kappa: -0.029557922696993328
     s: 0.81739961254943683
-    dkappa: 0.0012626408020643552
+    dkappa: 0.0012898445697755111
     ddkappa: 0
   }
   v: 2.3946890758563715
@@ -244,12 +244,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.97887640842
-    y: 4141549.8678003061
-    theta: 1.1844023115874927
-    kappa: -0.029428843719671569
+    x: 587045.978907968
+    y: 4141549.8677877672
+    theta: 1.1843321745008903
+    kappa: -0.029499007457396344
     s: 0.86509339406656427
-    dkappa: 0.0012489667640139025
+    dkappa: 0.001276057653679889
     ddkappa: 0
   }
   v: 2.3746890758563715
@@ -258,12 +258,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587045.99655646726
-    y: 4141549.9116650671
-    theta: 1.1830046298504502
-    kappa: -0.029371688031762096
+    x: 587045.99659077381
+    y: 4141549.9116514213
+    theta: 1.1829310368089136
+    kappa: -0.029440586330345887
     s: 0.91238717558369165
-    dkappa: 0.0012354074078998827
+    dkappa: 0.0012623863662105327
     ddkappa: 0
   }
   v: 2.3546890758563714
@@ -272,12 +272,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.01408699236
-    y: 4141549.95515883
-    theta: 1.1816187873731345
-    kappa: -0.029315015753603981
+    x: 587046.01412402245
+    y: 4141549.9551440869
+    theta: 1.181541767740087
+    kappa: -0.029382659315841955
     s: 0.95928095710081929
-    dkappa: 0.0012219627337222952
+    dkappa: 0.001248830707367442
     ddkappa: 0
   }
   v: 2.3346890758563714
@@ -286,12 +286,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.0314679835
-    y: 4141549.9982815953
-    theta: 1.1802447889520271
-    kappa: -0.029258826885197219
+    x: 587046.03150771384
+    y: 4141549.9982657633
+    theta: 1.1801643721265562
+    kappa: -0.029325226413884544
     s: 1.0057747386179468
-    dkappa: 0.0012086327414811407
+    dkappa: 0.0012353906771506173
     ddkappa: 0
   }
   v: 2.3146890758563714
@@ -300,12 +300,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.04932214064
-    y: 4141550.0407770309
-    theta: 1.1789086782428138
-    kappa: -0.029210391238079808
+    x: 587046.04936594027
+    y: 4141550.0407594885
+    theta: 1.1788254948940542
+    kappa: -0.029275615165302277
     s: 1.0518685201350741
-    dkappa: 0.0011940967101269444
+    dkappa: 0.001220739144713056
     ddkappa: 0
   }
   v: 2.2946890758563714
@@ -314,12 +314,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.0670264638
-    y: 4141550.0829015919
-    theta: 1.1775843548100571
-    kappa: -0.029162435495948043
+    x: 587046.0670743112
+    y: 4141550.082882348
+    theta: 1.1774984338512853
+    kappa: -0.029226494526162751
     s: 1.0975623016522016
-    dkappa: 0.0011796759974269017
+    dkappa: 0.0012062038753679763
     ddkappa: 0
   }
   v: 2.2746890758563714
@@ -328,12 +328,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.08457580442
-    y: 4141550.1246573972
-    theta: 1.1762716082870937
-    kappa: -0.029114899554853003
+    x: 587046.0846276643
+    y: 4141550.1246364666
+    theta: 1.176182973582921
+    kappa: -0.0291778038854826
     s: 1.1428560831693289
-    dkappa: 0.0011653815225843641
+    dkappa: 0.00119179584670084
     ddkappa: 0
   }
   v: 2.2546890758563713
@@ -342,12 +342,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.10197016282
-    y: 4141550.1660444471
-    theta: 1.1749704435636632
-    kappa: -0.029067783414794688
+    x: 587046.10202599957
+    y: 4141550.1660218444
+    theta: 1.1748791190090884
+    kappa: -0.029129543243261835
     s: 1.1877498646864566
-    dkappa: 0.0011512132855993312
+    dkappa: 0.0011775150587116467
     ddkappa: 0
   }
   v: 2.2346890758563713
@@ -356,12 +356,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.11920953891
-    y: 4141550.2070627417
-    theta: 1.1736808652894148
-    kappa: -0.02902108707577309
+    x: 587046.11926931713
+    y: 4141550.2070384827
+    theta: 1.1735868748083329
+    kappa: -0.029081712599500459
     s: 1.2322436462035837
-    dkappa: 0.0011371712864718031
+    dkappa: 0.001163361511400397
     ddkappa: 0
   }
   v: 2.2146890758563713
@@ -370,12 +370,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.13629393268
-    y: 4141550.2477122811
-    theta: 1.1724028778790463
-    kappa: -0.028974810537788227
+    x: 587046.13635761687
+    y: 4141550.2476863805
+    theta: 1.1723062454227902
+    kappa: -0.029034311954198463
     s: 1.2763374277207116
-    dkappa: 0.00112325552520178
+    dkappa: 0.0011493352047670902
     ddkappa: 0
   }
   v: 2.1946890758563713
@@ -384,12 +384,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.15322334412
-    y: 4141550.2879930651
-    theta: 1.1711364855174111
-    kappa: -0.028928953800840082
+    x: 587046.153290899
+    y: 4141550.2879655375
+    theta: 1.1710372350633218
+    kappa: -0.028987341307355856
     s: 1.3200312092378388
-    dkappa: 0.0011094660017892618
+    dkappa: 0.0011354361388117272
     ddkappa: 0
   }
   v: 2.1746890758563713
@@ -398,12 +398,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.16999777313
-    y: 4141550.3279050943
-    theta: 1.1698816921645885
-    kappa: -0.028883516864928658
+    x: 587046.1700691632
+    y: 4141550.3278759541
+    theta: 1.1697798477146186
+    kappa: -0.028940800658972633
     s: 1.3633249907549663
-    dkappa: 0.0010958027162342484
+    dkappa: 0.0011216643135343071
     ddkappa: 0
   }
   v: 2.1546890758563713
@@ -412,12 +412,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.18661721982
-    y: 4141550.3674483681
-    theta: 1.1686385015609171
-    kappa: -0.028838499730053958
+    x: 587046.18669240968
+    y: 4141550.3674176307
+    theta: 1.1685340871402656
+    kappa: -0.02889469000904879
     s: 1.4062187722720938
-    dkappa: 0.0010822656685367399
+    dkappa: 0.0011080197289348305
     ddkappa: 0
   }
   v: 2.1346890758563712
@@ -426,12 +426,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.20308168419
-    y: 4141550.4066228857
-    theta: 1.1674069172319941
-    kappa: -0.028793902396215983
+    x: 587046.20316063834
+    y: 4141550.406590567
+    theta: 1.1672999568877702
+    kappa: -0.028849009357584336
     s: 1.4487125537892211
-    dkappa: 0.0010688548586967361
+    dkappa: 0.0010945023850132971
     ddkappa: 0
   }
   v: 2.1146890758563712
@@ -440,12 +440,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.21939116623
-    y: 4141550.445428649
-    theta: 1.1661869424936355
-    kappa: -0.028749724863414733
+    x: 587046.21947384928
+    y: 4141550.4453947623
+    theta: 1.1660774602935542
+    kappa: -0.028803758704579265
     s: 1.4908063353063488
-    dkappa: 0.0010555702867142372
+    dkappa: 0.0010811122817697069
     ddkappa: 0
   }
   v: 2.0946890758563712
@@ -454,12 +454,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.23584462574
-    y: 4141550.4837374543
-    theta: 1.1649895255588238
-    kappa: -0.028709795260208736
+    x: 587046.23593153642
+    y: 4141550.4837017464
+    theta: 1.1648778266343718
+    kappa: -0.028762795019447295
     s: 1.5325001168234762
-    dkappa: 0.0010419130349236022
+    dkappa: 0.001067349426050989
     ddkappa: 0
   }
   v: 2.0746890758563712
@@ -468,12 +468,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.25239361008
-    y: 4141550.5215700786
-    theta: 1.1638128638590166
-    kappa: -0.028673493160575346
+    x: 587046.25248517923
+    y: 4141550.5215323339
+    theta: 1.1636992083163058
+    kappa: -0.028725493437261732
     s: 1.5737938983406035
-    dkappa: 0.0010279639630708375
+    dkappa: 0.0010532948213994168
     ddkappa: 0
   }
   v: 2.0546890758563712
@@ -482,12 +482,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.2687822897
-    y: 4141550.5590362307
-    theta: 1.1626475854505032
-    kappa: -0.028637542708074282
+    x: 587046.268878472
+    y: 4141550.5589964674
+    theta: 1.1625319921693147
+    kappa: -0.028688553183884666
     s: 1.614687679857731
-    dkappa: 0.0010141500115324665
+    dkappa: 0.0010393763593255904
     ddkappa: 0
   }
   v: 2.0346890758563712
@@ -496,12 +496,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.28501066449
-    y: 4141550.5961359106
-    theta: 1.1614936938795981
-    kappa: -0.028601943902705547
+    x: 587046.28511141473
+    y: 4141550.5960941482
+    theta: 1.1613761817574366
+    kappa: -0.028651974259316107
     s: 1.6551814613748583
-    dkappa: 0.0010004711803084894
+    dkappa: 0.0010255940398295105
     ddkappa: 0
   }
   v: 2.0146890758563711
@@ -510,12 +510,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.30107873422
-    y: 4141550.632869117
-    theta: 1.1603511925056087
-    kappa: -0.028566696744469128
+    x: 587046.30118400755
+    y: 4141550.6328253755
+    theta: 1.1602317804567668
+    kappa: -0.028615756663556043
     s: 1.6952752428919859
-    dkappa: 0.000986927469398906
+    dkappa: 0.0010119478629111766
     ddkappa: 0
   }
   v: 1.9946890758563711
@@ -524,12 +524,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.31698649924
-    y: 4141550.66923585
-    theta: 1.1592200845052663
-    kappa: -0.028531801233365038
+    x: 587046.31709625036
+    y: 4141550.66919015
+    theta: 1.1590987914599147
+    kappa: -0.028579900396604494
     s: 1.7349690244091134
-    dkappa: 0.00097351887880371679
+    dkappa: 0.00099843782857058868
     ddkappa: 0
   }
   v: 1.9746890758563711
@@ -538,12 +538,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.33273395943
-    y: 4141550.7052361104
-    theta: 1.1581003728771233
-    kappa: -0.028497257369393278
+    x: 587046.33284814318
+    y: 4141550.7051884718
+    theta: 1.1579772177804191
+    kappa: -0.028544405458461451
     s: 1.7742628059262406
-    dkappa: 0.00096024540852292142
+    dkappa: 0.00098506393680774726
     ddkappa: 0
   }
   v: 1.9546890758563711
@@ -552,12 +552,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.34832111467
-    y: 4141550.7408698983
-    theta: 1.1569920604459119
-    kappa: -0.028463065152553837
+    x: 587046.348439686
+    y: 4141550.7408203394
+    theta: 1.1568670622571307
+    kappa: -0.028509271849126907
     s: 1.8131565874433682
-    dkappa: 0.00094710705855651964
+    dkappa: 0.00097182618762265175
     ddkappa: 0
   }
   v: 1.9346890758563711
@@ -566,12 +566,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.36374796508
-    y: 4141550.7761372132
-    theta: 1.1558951498668646
-    kappa: -0.028429224582846722
+    x: 587046.36387087882
+    y: 4141550.7760857549
+    theta: 1.1557683275585535
+    kappa: -0.028474499568600865
     s: 1.851650368960496
-    dkappa: 0.0009341038289045118
+    dkappa: 0.00095872458101530248
     ddkappa: 0
   }
   v: 1.914689075856371
@@ -580,12 +580,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.37901451066
-    y: 4141550.811038055
-    theta: 1.1548096436299977
-    kappa: -0.028395735660271933
+    x: 587046.37914172176
+    y: 4141550.8109847168
+    theta: 1.1546810161871492
+    kappa: -0.028440088616883335
     s: 1.8897441504776231
-    dkappa: 0.000921235719566898
+    dkappa: 0.00094575911698569946
     ddkappa: 0
   }
   v: 1.894689075856371
@@ -594,12 +594,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.39412075141
-    y: 4141550.8455724246
-    theta: 1.1537355440643553
-    kappa: -0.02836259838482947
+    x: 587046.3942522147
+    y: 4141550.8455172256
+    theta: 1.1536051304836028
+    kappa: -0.028406038993974304
     s: 1.9274379319947508
-    dkappa: 0.0009085027305436779
+    dkappa: 0.00093292979553384256
     ddkappa: 0
   }
   v: 1.874689075856371
@@ -608,12 +608,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.40906668722
-    y: 4141550.8797403206
-    theta: 1.1526728533422153
-    kappa: -0.028329812756519333
+    x: 587046.40920235764
+    y: 4141550.8796832813
+    theta: 1.152540672631049
+    kappa: -0.028372350699873778
     s: 1.9647317135118783
-    dkappa: 0.00089590486183485185
+    dkappa: 0.0009202366166597318
     ddkappa: 0
   }
   v: 1.854689075856371
@@ -622,12 +622,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.4238523182
-    y: 4141550.9135417445
-    theta: 1.1516215734832542
-    kappa: -0.028297378775341518
+    x: 587046.42399215058
+    y: 4141550.9134828839
+    theta: 1.1514876446592588
+    kappa: -0.028339023734581761
     s: 2.0016254950290056
-    dkappa: 0.00088344211344041951
+    dkappa: 0.00090767958036336738
     ddkappa: 0
   }
   v: 1.834689075856371
@@ -636,12 +636,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.43878639292
-    y: 4141550.946839008
-    theta: 1.1505911621610694
-    kappa: -0.028269543355863506
+    x: 587046.43893075455
+    y: 4141550.946778113
+    theta: 1.150455785237551
+    kappa: -0.028310335107380652
     s: 2.0381192765461331
-    dkappa: 0.00087070594516256957
+    dkappa: 0.00089485054526713666
     ddkappa: 0
   }
   v: 1.814689075856371
@@ -650,12 +650,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.45372176974
-    y: 4141550.9796977295
-    theta: 1.1495770899869606
-    kappa: -0.028244282530729106
+    x: 587046.45387086447
+    y: 4141550.9796346826
+    theta: 1.1494404319967584
+    kappa: -0.028284246961710675
     s: 2.0742130580632603
-    dkappa: 0.00085789105647333616
+    dkappa: 0.000881943977533904
     ddkappa: 0
   }
   v: 1.7946890758563709
@@ -664,12 +664,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.46849162912
-    y: 4141551.0121923033
-    theta: 1.1485742450498304
-    kappa: -0.028219301652156938
+    x: 587046.46864540444
+    y: 4141551.012127128
+    theta: 1.148436320148335
+    kappa: -0.028258447931168836
     s: 2.1099068395803879
-    dkappa: 0.00084521818547413769
+    dkappa: 0.00086918044350000386
     ddkappa: 0
   }
   v: 1.7746890758563709
@@ -678,12 +678,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.483095971
-    y: 4141551.0443227282
-    theta: 1.1475826296991389
-    kappa: -0.028194600720147005
+    x: 587046.48325437447
+    y: 4141551.0442554494
+    theta: 1.1474434520506605
+    kappa: -0.028232938015755122
     s: 2.1452006210975156
-    dkappa: 0.00083268733216497427
+    dkappa: 0.00085655994316543617
     ddkappa: 0
   }
   v: 1.7546890758563709
@@ -692,12 +692,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.49753479566
-    y: 4141551.0760890041
-    theta: 1.1466022461469967
-    kappa: -0.028170179734699305
+    x: 587046.49769777455
+    y: 4141551.0760196452
+    theta: 1.1464618299242439
+    kappa: -0.028207717215469535
     s: 2.1800944026146425
-    dkappa: 0.000820298496545846
+    dkappa: 0.000844082476530201
     ddkappa: 0
   }
   v: 1.7346890758563709
@@ -706,12 +706,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.5118081026
-    y: 4141551.1074911328
-    theta: 1.1456330964718875
-    kappa: -0.028146038695813839
+    x: 587046.51197560481
+    y: 4141551.1074197181
+    theta: 1.1454914558554605
+    kappa: -0.028182785530312081
     s: 2.21458818413177
-    dkappa: 0.00080805167861675291
+    dkappa: 0.0008317480435942983
     ddkappa: 0
   }
   v: 1.7146890758563709
@@ -720,12 +720,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.58069187624
-    y: 4141551.259039551
-    theta: 1.1409559201060373
-    kappa: -0.028029532699820038
+    x: 587046.5808812076
+    y: 4141551.2589582112
+    theta: 1.1408083704726997
+    kappa: -0.028062463831446737
     s: 2.3810570917174072
-    dkappa: 0.00074894785432181319
+    dkappa: 0.00077222138440477192
     ddkappa: 0
   }
   v: 1.6146890758563708
@@ -734,12 +734,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.64567100035
-    y: 4141551.401376233
-    theta: 1.1365654263963205
-    kappa: -0.027923421568782626
+    x: 587046.64588104759
+    y: 4141551.4012854318
+    theta: 1.1364125275199786
+    kappa: -0.027952788568029442
     s: 2.5375259993030443
-    dkappa: 0.00069314519952924257
+    dkappa: 0.00071602238440311879
     ddkappa: 0
   }
   v: 1.5146890758563707
@@ -748,12 +748,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.708153396
-    y: 4141551.5338492161
-    theta: 1.1324952426897681
-    kappa: -0.027848201876627208
+    x: 587046.7083845234
+    y: 4141551.5337484693
+    theta: 1.1323387942052636
+    kappa: -0.027874395374886819
     s: 2.6839949068886817
-    dkappa: 0.00063913931639360559
+    dkappa: 0.00066165292040813674
     ddkappa: 0
   }
   v: 1.4146890758563706
@@ -762,12 +762,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.76636987634
-    y: 4141551.6572777545
-    theta: 1.1287028705617683
-    kappa: -0.027778117724389116
+    x: 587046.76662064448
+    y: 4141551.6571677425
+    theta: 1.1285431146138183
+    kappa: -0.02780135438887367
     s: 2.8204638144743188
-    dkappa: 0.00058882062421377343
+    dkappa: 0.00061099547044338111
     ddkappa: 0
   }
   v: 1.3146890758563705
@@ -776,12 +776,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.82032044139
-    y: 4141551.7716618492
-    theta: 1.1251884234375431
-    kappa: -0.027713169112068361
+    x: 587046.82058941107
+    y: 4141551.7715432504
+    theta: 1.1250256024684633
+    kappa: -0.027733665609989997
     s: 2.9469327220599562
-    dkappa: 0.000542189122989746
+    dkappa: 0.0005640500345088518
     ddkappa: 0
   }
   v: 1.2146890758563704
@@ -790,12 +790,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.87049431319
-    y: 4141551.8767665573
-    theta: 1.1219601584364944
-    kappa: -0.027660811439934693
+    x: 587046.87078029313
+    y: 4141551.8766398323
+    theta: 1.1217949272264358
+    kappa: -0.027678830808351478
     s: 3.0634016296455933
-    dkappa: 0.00049881013449903911
+    dkappa: 0.00052038517556513323
     ddkappa: 0
   }
   v: 1.1146890758563703
@@ -804,12 +804,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.91725522443
-    y: 4141551.9724171995
-    theta: 1.1190240302853505
-    kappa: -0.027626587739053507
+    x: 587046.91755733907
+    y: 4141551.9722825857
+    theta: 1.1188573520886553
+    kappa: -0.027642429507539187
     s: 3.16987053723123
-    dkappa: 0.00045836047892521095
+    dkappa: 0.00047968000759491885
     ddkappa: 0
   }
   v: 1.0146890758563702
@@ -818,12 +818,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.95962415729
-    y: 4141552.0590839386
-    theta: 1.116363629899028
-    kappa: -0.027595578469658218
+    x: 587046.95994089148
+    y: 4141552.0589421764
+    theta: 1.1161956405536013
+    kappa: -0.027609447167401685
     s: 3.2663394448168672
-    dkappa: 0.00042171002224344358
+    dkappa: 0.00044279803729745906
     ddkappa: 0
   }
   v: 0.91468907585637016
@@ -832,12 +832,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587046.99760111235
-    y: 4141552.1367667736
-    theta: 1.1139790027442364
-    kappa: -0.027567783631748818
+    x: 587046.99793095037
+    y: 4141552.136618604
+    theta: 1.1138098381557446
+    kappa: -0.027579883787938974
     s: 3.3528083524025041
-    dkappa: 0.00038885876445373719
+    dkappa: 0.0004097392646727542
     ddkappa: 0
   }
   v: 0.81468907585637007
@@ -846,12 +846,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.03118608927
-    y: 4141552.2054657051
-    theta: 1.1118701729866134
-    kappa: -0.027543203225325313
+    x: 587047.03152751573
+    y: 4141552.2053118688
+    theta: 1.111699969096924
+    kappa: -0.027553739369151044
     s: 3.4292772599881411
-    dkappa: 0.00035980670555609158
+    dkappa: 0.00038050368972080383
     ddkappa: 0
   }
   v: 0.71468907585637
@@ -860,12 +860,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.06037908792
-    y: 4141552.2651807326
-    theta: 1.1100371489299712
-    kappa: -0.027521837250387697
+    x: 587047.06073058746
+    y: 4141552.2650219705
+    theta: 1.1098660416936357
+    kappa: -0.027531013911037905
     s: 3.4957461675737784
-    dkappa: 0.00033455384555050664
+    dkappa: 0.00035509131244160815
     ddkappa: 0
   }
   v: 0.61468907585636989
@@ -874,12 +874,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.08545366151
-    y: 4141552.3157757581
-    theta: 1.1084823998378537
-    kappa: -0.027508013070484713
+    x: 587047.08581372374
+    y: 4141552.3156127511
+    theta: 1.108310745289399
+    kappa: -0.027516059501516702
     s: 3.5522150751594155
-    dkappa: 0.00031289035558346314
+    dkappa: 0.00033329440916234564
     ddkappa: 0
   }
   v: 0.5146890758563698
@@ -888,12 +888,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.10644156637
-    y: 4141552.3572349814
-    theta: 1.1072061695236701
-    kappa: -0.02750223304884019
+    x: 587047.10680880118
+    y: 4141552.3570683426
+    theta: 1.1070343503761353
+    kappa: -0.02750938327251198
     s: 3.5986839827450523
-    dkappa: 0.000294791876643262
+    dkappa: 0.00031508877464911936
     ddkappa: 0
   }
   v: 0.41468907585636972
@@ -902,12 +902,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.12291292334
-    y: 4141552.3897722773
-    theta: 1.1062045692089868
-    kappa: -0.027497696874306529
+    x: 587047.12328578718
+    y: 4141552.3896027883
+    theta: 1.1060326208788667
+    kappa: -0.027504143752341531
     s: 3.6351528903306893
-    dkappa: 0.00028058814795232104
+    dkappa: 0.00030080095001332034
     ddkappa: 0
   }
   v: 0.31468907585636963
@@ -916,12 +916,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.1348677323
-    y: 4141552.413387645
-    theta: 1.1054776086692342
-    kappa: -0.027494404546883733
+    x: 587047.13524468173
+    y: 4141552.4132160875
+    theta: 1.1053055665770131
+    kappa: -0.027500340941005355
     s: 3.6616217979163266
-    dkappa: 0.00027027916951064023
+    dkappa: 0.00029043093525494841
     ddkappa: 0
   }
   v: 0.21468907585636954
@@ -930,12 +930,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.14230599336
-    y: 4141552.4280810859
-    theta: 1.1050252932739295
-    kappa: -0.027492356066571789
+    x: 587047.14268548484
+    y: 4141552.4279082413
+    theta: 1.1048531928423195
+    kappa: -0.027497974838503449
     s: 3.6780907055019636
-    dkappa: 0.00026386494131821982
+    dkappa: 0.0002839787303740039
     ddkappa: 0
   }
   v: 0.11468907585636945
@@ -944,12 +944,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.14522770653
-    y: 4141552.4338525985
-    theta: 1.1048476256315274
-    kappa: -0.027491551433370707
+    x: 587047.1456081965
+    y: 4141552.4336792482
+    theta: 1.1046755022843433
+    kappa: -0.027497045444835812
     s: 3.6845596130876004
-    dkappa: 0.0002613454633750598
+    dkappa: 0.0002814443353704867
     ddkappa: 0
   }
   v: 0.01468907585636936
@@ -958,12 +958,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.14527006331
-    y: 4141552.43393627
-    theta: 1.1048450499339861
-    kappa: -0.027491539768383796
+    x: 587047.14565056772
+    y: 4141552.4337629117
+    theta: 1.104672926254588
+    kappa: -0.027497031971162392
     s: 3.684653394604728
-    dkappa: 0.00026130893781633894
+    dkappa: 0.00028140759355507441
     ddkappa: 0
   }
   v: 0
@@ -972,12 +972,12 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587047.14527006331
-    y: 4141552.43393627
-    theta: 1.1048450499339861
-    kappa: -0.027491539768383796
+    x: 587047.14565056772
+    y: 4141552.4337629117
+    theta: 1.104672926254588
+    kappa: -0.027497031971162392
     s: 3.684653394604728
-    dkappa: 0.00026130893781633894
+    dkappa: 0.00028140759355507441
     ddkappa: 0
   }
   v: 0
-- 
GitLab