diff --git a/modules/planning/common/speed/speed_data.h b/modules/planning/common/speed/speed_data.h
index aaacee8c02387622da88f5d2ac62661e19b9d8dd..e29c4ae052f6477f910c56aca18eac50a3c25147 100644
--- a/modules/planning/common/speed/speed_data.h
+++ b/modules/planning/common/speed/speed_data.h
@@ -48,6 +48,8 @@ class SpeedData {
 
   double TotalTime() const;
 
+  bool Empty() const { return speed_vector_.empty(); }
+
   void Clear();
 
   virtual std::string DebugString() const;
diff --git a/modules/planning/tasks/qp_spline_st_speed/qp_spline_st_graph.cc b/modules/planning/tasks/qp_spline_st_speed/qp_spline_st_graph.cc
index 57c1f0008be4d7cf8cd1ead47cd480d929569af2..cf5a48c14cce81408d9c5392dc0f3e9d693853ed 100644
--- a/modules/planning/tasks/qp_spline_st_speed/qp_spline_st_graph.cc
+++ b/modules/planning/tasks/qp_spline_st_speed/qp_spline_st_graph.cc
@@ -26,6 +26,7 @@
 #include <utility>
 
 #include "modules/common/log.h"
+#include "modules/planning/common/frame.h"
 #include "modules/planning/common/planning_gflags.h"
 
 namespace apollo {
@@ -35,6 +36,7 @@ using apollo::common::ErrorCode;
 using apollo::common::Status;
 using apollo::common::VehicleParam;
 using apollo::planning_internal::STGraphDebug;
+using apollo::common::SpeedPoint;
 
 QpSplineStGraph::QpSplineStGraph(Spline1dGenerator* spline_generator,
                                  const QpStSpeedConfig& qp_st_speed_config,
@@ -452,6 +454,21 @@ Status QpSplineStGraph::GetSConstraintByTime(
   return Status::OK();
 }
 
+const SpeedData QpSplineStGraph::GetHistorySpeed() const {
+  const auto* last_frame = FrameHistory::instance()->Latest();
+  if (!last_frame) {
+    AWARN << "last frame is empty";
+    return SpeedData();
+  }
+  const ReferenceLineInfo* last_reference_line_info =
+      last_frame->DriveReferenceLineInfo();
+  if (!last_reference_line_info) {
+    ADEBUG << "last reference line info is empty";
+    return SpeedData();
+  }
+  return last_reference_line_info->speed_data();
+}
+
 Status QpSplineStGraph::EstimateSpeedUpperBound(
     const common::TrajectoryPoint& init_point, const SpeedLimit& speed_limit,
     std::vector<double>* speed_upper_bound) const {
@@ -463,6 +480,7 @@ Status QpSplineStGraph::EstimateSpeedUpperBound(
   // processing. We can do the following process multiple times and use
   // previous cycle's results for better estimation.
   const double v = init_point.v();
+  auto last_speed_data = GetHistorySpeed();
 
   if (static_cast<double>(t_evaluated_.size() +
                           speed_limit.speed_limit_points().size()) <
@@ -473,7 +491,12 @@ Status QpSplineStGraph::EstimateSpeedUpperBound(
     const double kDistanceEpsilon = 1e-6;
     while (i < t_evaluated_.size() &&
            j + 1 < speed_limit.speed_limit_points().size()) {
-      const double distance = v * t_evaluated_[i];
+      double distance = v * t_evaluated_[i];
+      if (!last_speed_data.Empty()) {
+        SpeedPoint p;
+        last_speed_data.EvaluateByTime(t_evaluated_[i], &p);
+        distance = p.s();
+      }
       if (fabs(distance - speed_limit.speed_limit_points()[j].first) <
           kDistanceEpsilon) {
         speed_upper_bound->push_back(
@@ -502,7 +525,12 @@ Status QpSplineStGraph::EstimateSpeedUpperBound(
 
     const auto& speed_limit_points = speed_limit.speed_limit_points();
     for (const double t : t_evaluated_) {
-      const double s = v * t;
+      double s = v * t;
+      if (!last_speed_data.Empty()) {
+        SpeedPoint p;
+        last_speed_data.EvaluateByTime(t, &p);
+        s = p.s();
+      }
 
       // NOTICE: we are using binary search here based on two assumptions:
       // (1) The s in speed_limit_points increase monotonically.
diff --git a/modules/planning/tasks/qp_spline_st_speed/qp_spline_st_graph.h b/modules/planning/tasks/qp_spline_st_speed/qp_spline_st_graph.h
index 242fcc598296953015ae2b6b00c98a7c06e4e7b1..6a09c40a7418739cf58723b56e2dfcbca05fd1c6 100644
--- a/modules/planning/tasks/qp_spline_st_speed/qp_spline_st_graph.h
+++ b/modules/planning/tasks/qp_spline_st_speed/qp_spline_st_graph.h
@@ -84,6 +84,7 @@ class QpSplineStGraph {
   common::Status AddFollowReferenceLineKernel(
       const std::vector<const StBoundary*>& boundaries, const double weight);
 
+  const SpeedData GetHistorySpeed() const;
   common::Status EstimateSpeedUpperBound(
       const common::TrajectoryPoint& init_point, const SpeedLimit& speed_limit,
       std::vector<double>* speed_upper_bound) const;
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 e2272742ca897306bd30716f4d281b523a491cc5..e5246a0b854ab009e3c34f152dfd9a60777d547c 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
@@ -1,7 +1,7 @@
 header {
   module_name: "planning"
 }
-total_path_length: 33.843359326879273
+total_path_length: 33.843359326879281
 total_path_time: 7.9999999999999885
 is_replan: true
 gear: GEAR_DRIVE
@@ -10,13 +10,13 @@ trajectory_point {
     x: 587581.22478024359
     y: 4140851.2261321167
     theta: 1.2656533435800004
-    kappa: 6.0568115638103012e-20
-    s: 6.9388939039072284e-18
+    kappa: 1.0744631395338823e-19
+    s: 1.3877787807814457e-17
     dkappa: 0
     ddkappa: 0
   }
   v: 0
-  a: 1.0397336338303285
+  a: 1.0397336338303294
   relative_time: 0
 }
 trajectory_point {
@@ -24,13 +24,13 @@ trajectory_point {
     x: 587581.2248418479
     y: 4140851.2263313192
     theta: 1.2656548971655508
-    kappa: 1.4086672867258991e-06
-    s: 0.00020851042061718176
+    kappa: 1.4086672867259473e-06
+    s: 0.00020851042061718894
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.020878992553343387
-  a: 1.0481188522975111
+  v: 0.020878992553343408
+  a: 1.0481188522975118
   relative_time: 0.02
 }
 trajectory_point {
@@ -38,27 +38,27 @@ trajectory_point {
     x: 587581.22502731613
     y: 4140851.2269310439
     theta: 1.2656595744441201
-    kappa: 5.6496497053331663e-06
-    s: 0.00083625909929147359
+    kappa: 5.6496497053332188e-06
+    s: 0.00083625909929148129
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.041922897519522925
-  a: 1.0562255849080673
+  v: 0.041922897519522967
+  a: 1.0562255849080682
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 587581.22533760639
-    y: 4140851.22793439
+    x: 587581.22533760627
+    y: 4140851.2279343894
     theta: 1.2656673995783323
-    kappa: 1.2744855396295229e-05
-    s: 0.0018864888710259293
+    kappa: 1.2744855396295287e-05
+    s: 0.0018864888710259378
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.063126187769053516
-  a: 1.0640580904267394
+  v: 0.063126187769053571
+  a: 1.06405809042674
   relative_time: 0.06
 }
 trajectory_point {
@@ -66,13 +66,13 @@ trajectory_point {
     x: 587581.225773644
     y: 4140851.2293443494
     theta: 1.2656783959139681
-    kappa: 2.2715451444108826e-05
-    s: 0.0033623328799868429
+    kappa: 2.2715451444108894e-05
+    s: 0.0033623328799868529
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.084483421347744891
-  a: 1.0716206276182689
+  v: 0.084483421347744961
+  a: 1.0716206276182696
   relative_time: 0.08
 }
 trajectory_point {
@@ -80,13 +80,13 @@ trajectory_point {
     x: 587581.22633632342
     y: 4140851.2311638128
     theta: 1.2656925859926671
-    kappa: 3.5581875385942028e-05
-    s: 0.005266816283009651
+    kappa: 3.558187538594211e-05
+    s: 0.0052668162830096623
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.10598924147670162
-  a: 1.0789174552473983
+  v: 0.10598924147670169
+  a: 1.0789174552473988
   relative_time: 0.1
 }
 trajectory_point {
@@ -94,13 +94,13 @@ trajectory_point {
     x: 587581.22702650633
     y: 4140851.23339557
     theta: 1.2657099915646259
-    kappa: 5.1363846720282249e-05
-    s: 0.0076028579531048314
+    kappa: 5.1363846720282331e-05
+    s: 0.0076028579531048435
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.12763837655232313
-  a: 1.0859528320788692
+  v: 0.12763837655232319
+  a: 1.0859528320788696
   relative_time: 0.12000000000000001
 }
 trajectory_point {
@@ -108,27 +108,27 @@ trajectory_point {
     x: 587581.22784502478
     y: 4140851.2360423082
     theta: 1.2657306336012963
-    kappa: 7.0080378415584126e-05
-    s: 0.010373272182963795
+    kappa: 7.0080378415584207e-05
+    s: 0.010373272182963807
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.14942564014630363
-  a: 1.0927310168774238
+  v: 0.14942564014630369
+  a: 1.092731016877424
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
     x: 587581.22879267938
-    y: 4140851.2391066183
-    theta: 1.2657545323080812
-    kappa: 9.1749788418917573e-05
-    s: 0.013580770388464784
+    y: 4140851.2391066179
+    theta: 1.2657545323080814
+    kappa: 9.1749788418917682e-05
+    s: 0.0135807703884648
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.17134593100563222
-  a: 1.099256268407804
+  v: 0.17134593100563231
+  a: 1.0992562684078042
   relative_time: 0.16
 }
 trajectory_point {
@@ -136,13 +136,13 @@ trajectory_point {
     x: 587581.2298702416
     y: 4140851.2425909932
     theta: 1.2657817071370292
-    kappa: 0.0001163897111646158
-    s: 0.017227962812178782
+    kappa: 0.00011638971116461593
+    s: 0.0172279628121788
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.19339423305259287
-  a: 1.1055328454347519
+  v: 0.19339423305259296
+  a: 1.1055328454347522
   relative_time: 0.18
 }
 trajectory_point {
@@ -150,13 +150,13 @@ trajectory_point {
     x: 587581.23107845336
     y: 4140851.2464978332
     theta: 1.2658121767995267
-    kappa: 0.0001440171090829232
-    s: 0.021317360226875394
+    kappa: 0.00014401710908292333
+    s: 0.021317360226875411
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.21556561538476435
-  a: 1.1115650067230094
+  v: 0.21556561538476443
+  a: 1.1115650067230096
   relative_time: 0.19999999999999998
 }
 trajectory_point {
@@ -164,12 +164,12 @@ trajectory_point {
     x: 587581.23241802724
     y: 4140851.2508294419
     theta: 1.2658459592789906
-    kappa: 0.00017464828410864323
-    s: 0.025851375639028739
+    kappa: 0.00017464828410864336
+    s: 0.025851375639028756
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.23785523227502026
+  v: 0.23785523227502037
   a: 1.1173570110373185
   relative_time: 0.21999999999999997
 }
@@ -178,12 +178,12 @@ trajectory_point {
     x: 587581.233889648
     y: 4140851.2555880346
     theta: 1.2658830718435561
-    kappa: 0.00020829888918978668
-    s: 0.030832325992323373
+    kappa: 0.00020829888918978687
+    s: 0.0308323259923234
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.260258323171529
+  v: 0.26025832317152914
   a: 1.1229131171424211
   relative_time: 0.23999999999999996
 }
@@ -191,13 +191,13 @@ trajectory_point {
   path_point {
     x: 587581.2354939722
     y: 4140851.2607757328
-    theta: 1.2659235310587653
-    kappa: 0.0002449839397962193
-    s: 0.036262433871160162
+    theta: 1.2659235310587651
+    kappa: 0.00024498393979621947
+    s: 0.036262433871160182
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.28277021269775393
+  v: 0.28277021269775404
   a: 1.1282375838030596
   relative_time: 0.25999999999999995
 }
@@ -206,26 +206,26 @@ trajectory_point {
     x: 587581.23723162932
     y: 4140851.2663945728
     theta: 1.2659673528002524
-    kappa: 0.00028471782542831004
-    s: 0.042143829204162181
+    kappa: 0.00028471782542831032
+    s: 0.042143829204162216
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.30538631065245325
+  v: 0.30538631065245336
   a: 1.1333346697839755
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
     x: 587581.23910322157
-    y: 4140851.2724465015
+    y: 4140851.2724465011
     theta: 1.2660145522664288
-    kappa: 0.00032751432112557903
-    s: 0.048478550967680639
+    kappa: 0.0003275143211255793
+    s: 0.048478550967680674
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.32810211200967981
+  v: 0.32810211200967992
   a: 1.1382086338499109
   relative_time: 0.3
 }
@@ -234,12 +234,12 @@ trajectory_point {
     x: 587581.241109325
     y: 4140851.2789333821
     theta: 1.266065143991165
-    kappa: 0.00037338659897534504
-    s: 0.05526854888930071
+    kappa: 0.00037338659897534536
+    s: 0.055268548889300752
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.35091319691878148
+  v: 0.35091319691878153
   a: 1.1428637347656081
   relative_time: 0.32
 }
@@ -248,12 +248,12 @@ trajectory_point {
     x: 587581.24325048993
     y: 4140851.2858569934
     theta: 1.2661191418564708
-    kappa: 0.00042234723962137445
-    s: 0.062515685151347533
+    kappa: 0.00042234723962137466
+    s: 0.062515685151347561
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.37381523070440087
+  v: 0.373815230704401
   a: 1.1473042312958088
   relative_time: 0.34
 }
@@ -262,12 +262,12 @@ trajectory_point {
     x: 587581.24552724115
     y: 4140851.2932190327
     theta: 1.2661765591051746
-    kappa: 0.00047440824377252777
-    s: 0.070221736094391946
+    kappa: 0.0004744082437725281
+    s: 0.070221736094391987
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.39680396386647548
+  v: 0.3968039638664756
   a: 1.1515343822052551
   relative_time: 0.36000000000000004
 }
@@ -276,13 +276,13 @@ trajectory_point {
     x: 587581.24794007849
     y: 4140851.3010211173
     theta: 1.2662374083536005
-    kappa: 0.00052958104371140949
-    s: 0.078388393920756638
+    kappa: 0.00052958104371140981
+    s: 0.07838839392075668
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.41987523208023769
-  a: 1.155558446258689
+  v: 0.4198752320802378
+  a: 1.1555584462586888
   relative_time: 0.38000000000000006
 }
 trajectory_point {
@@ -290,12 +290,12 @@ trajectory_point {
     x: 587581.25048947765
     y: 4140851.3092647833
     theta: 1.2663017016042433
-    kappa: 0.00058787651480301433
-    s: 0.087017268398021735
+    kappa: 0.00058787651480301443
+    s: 0.087017268398021763
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.44302495619621463
+  v: 0.44302495619621468
   a: 1.1593806822208523
   relative_time: 0.40000000000000008
 }
@@ -304,13 +304,13 @@ trajectory_point {
     x: 587581.25317589042
     y: 4140851.317951493
     theta: 1.2663694502584413
-    kappa: 0.00064930498700337613
-    s: 0.096109888562530971
+    kappa: 0.00064930498700337634
+    s: 0.096109888562531
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.46624914224022834
-  a: 1.1630053488564873
+  v: 0.4662491422402284
+  a: 1.1630053488564871
   relative_time: 0.4200000000000001
 }
 trajectory_point {
@@ -318,27 +318,27 @@ trajectory_point {
     x: 587581.25599974522
     y: 4140851.3270826316
     theta: 1.2664406651290492
-    kappa: 0.00071387625636821562
-    s: 0.10566770442289741
+    kappa: 0.00071387625636821583
+    s: 0.10566770442289744
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.48954388141339561
-  a: 1.1664367049303359
+  v: 0.48954388141339572
+  a: 1.1664367049303357
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587581.25896144751
-    y: 4140851.33665951
+    x: 587581.2589614474
+    y: 4140851.3366595097
     theta: 1.2665153564531064
-    kappa: 0.00078159959656158812
-    s: 0.11569208866350938
+    kappa: 0.00078159959656158844
+    s: 0.11569208866350943
     dkappa: 0
     ddkappa: 0
   }
   v: 0.51290535009212823
-  a: 1.16967900920714
+  a: 1.1696790092071399
   relative_time: 0.46000000000000013
 }
 trajectory_point {
@@ -346,27 +346,27 @@ trajectory_point {
     x: 587581.26206138043
     y: 4140851.3466833676
     theta: 1.2665935339045069
-    kappa: 0.00085248377036453238
-    s: 0.1261843383480365
+    kappa: 0.00085248377036453271
+    s: 0.12618433834803655
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.53632980982813261
-  a: 1.1727365204516416
+  v: 0.53632980982813272
+  a: 1.1727365204516413
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587581.26529990567
-    y: 4140851.3571553729
+    x: 587581.26529990556
+    y: 4140851.3571553724
     theta: 1.2666752066066631
-    kappa: 0.00092653704118371731
-    s: 0.13714567662293531
+    kappa: 0.00092653704118371774
+    s: 0.13714567662293536
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.55981360734841
-  a: 1.1756134974285828
+  v: 0.55981360734841012
+  a: 1.1756134974285826
   relative_time: 0.50000000000000011
 }
 trajectory_point {
@@ -374,27 +374,27 @@ trajectory_point {
     x: 587581.26867736294
     y: 4140851.3680766253
     theta: 1.2667603831451724
-    kappa: 0.0010037671845600912
-    s: 0.14857725442095551
+    kappa: 0.0010037671845600914
+    s: 0.14857725442095554
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.583353174555257
-  a: 1.1783141989027055
+  v: 0.58335317455525715
+  a: 1.1783141989027053
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587581.27219407156
+    x: 587581.27219407167
     y: 4140851.3794481568
     theta: 1.2668490715804788
-    kappa: 0.0010841814996775283
-    s: 0.16048015216464551
+    kappa: 0.0010841814996775288
+    s: 0.16048015216464556
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.60694502852626453
-  a: 1.1808428836387517
+  v: 0.60694502852626464
+  a: 1.1808428836387515
   relative_time: 0.54000000000000015
 }
 trajectory_point {
@@ -402,13 +402,13 @@ trajectory_point {
     x: 587581.2758503306
     y: 4140851.3912709341
     theta: 1.2669412794605344
-    kappa: 0.0011677868208714789
-    s: 0.1728553814698586
+    kappa: 0.0011677868208714792
+    s: 0.17285538146985863
     dkappa: 0
     ddkappa: 0
   }
   v: 0.63058577151431827
-  a: 1.1832038104014633
+  a: 1.1832038104014631
   relative_time: 0.56000000000000016
 }
 trajectory_point {
@@ -416,27 +416,27 @@ trajectory_point {
     x: 587581.27964641887
     y: 4140851.40354586
     theta: 1.2670370138334579
-    kappa: 0.0012545895291376141
-    s: 0.18570388684925862
+    kappa: 0.0012545895291376145
+    s: 0.1857038868492587
     dkappa: 0
     ddkappa: 0
   }
   v: 0.65427209094759919
-  a: 1.1854012379555825
+  a: 1.1854012379555823
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
     x: 587581.28358259611
-    y: 4140851.4162737736
+    y: 4140851.4162737741
     theta: 1.2671362812601932
-    kappa: 0.001344595563640477
-    s: 0.19902654741582607
+    kappa: 0.0013445955636404773
+    s: 0.19902654741582609
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.6780007594295826
-  a: 1.1874394250658513
+  v: 0.67800075942958271
+  a: 1.1874394250658511
   relative_time: 0.6000000000000002
 }
 trajectory_point {
@@ -444,13 +444,13 @@ trajectory_point {
     x: 587581.28765910352
     y: 4140851.4294554549
     theta: 1.2672390878271647
-    kappa: 0.0014378104332221293
-    s: 0.2128241785863639
+    kappa: 0.0014378104332221297
+    s: 0.212824178586364
     dkappa: 0
     ddkappa: 0
   }
-  v: 0.701768634739039
-  a: 1.1893226304970115
+  v: 0.70176863473903928
+  a: 1.1893226304970113
   relative_time: 0.62000000000000022
 }
 trajectory_point {
@@ -458,13 +458,13 @@ trajectory_point {
     x: 587581.29187616333
     y: 4140851.4430916235
     theta: 1.2673454391589314
-    kappa: 0.0015342392279107985
-    s: 0.22709753378500341
+    kappa: 0.0015342392279107993
+    s: 0.22709753378500352
     dkappa: 0
     ddkappa: 0
   }
   v: 0.725572659830034
-  a: 1.1910551130138052
+  a: 1.191055113013805
   relative_time: 0.64000000000000024
 }
 trajectory_point {
@@ -472,13 +472,13 @@ trajectory_point {
     x: 587581.29623398057
     y: 4140851.45718294
     theta: 1.2674553404308395
-    kappa: 0.0016338866304295276
-    s: 0.2418473061467101
+    kappa: 0.0016338866304295282
+    s: 0.24184730614671018
     dkappa: 0
     ddkappa: 0
   }
   v: 0.74940986283192734
-  a: 1.1926411313809744
+  a: 1.1926411313809742
   relative_time: 0.66000000000000025
 }
 trajectory_point {
@@ -492,21 +492,21 @@ trajectory_point {
     ddkappa: 0
   }
   v: 0.77327735704937384
-  a: 1.1940849443632611
+  a: 1.1940849443632608
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
     x: 587581.30537262023
-    y: 4140851.4867333919
+    y: 4140851.4867333914
     theta: 1.267685811326307
-    kappa: 0.001842854022375296
-    s: 0.27277858367439389
+    kappa: 0.0018428540223752968
+    s: 0.272778583674394
     dkappa: 0
     ddkappa: 0
   }
   v: 0.79717234096232348
-  a: 1.1953908107254072
+  a: 1.195390810725407
   relative_time: 0.70000000000000029
 }
 trajectory_point {
@@ -514,13 +514,13 @@ trajectory_point {
     x: 587581.31015376758
     y: 4140851.5021935776
     theta: 1.2678063891683466
-    kappa: 0.0019521814443003256
-    s: 0.28896118899602646
+    kappa: 0.0019521814443003264
+    s: 0.28896118899602657
     dkappa: 0
     ddkappa: 0
   }
   v: 0.82109209822602092
-  a: 1.1965629892321548
+  a: 1.1965629892321545
   relative_time: 0.72000000000000031
 }
 trajectory_point {
@@ -528,13 +528,13 @@ trajectory_point {
     x: 587581.31507632346
     y: 4140851.5181110175
     theta: 1.2679305334127826
-    kappa: 0.0020647423620686906
-    s: 0.30562241519904887
+    kappa: 0.0020647423620686911
+    s: 0.30562241519904892
     dkappa: 0
     ddkappa: 0
   }
   v: 0.8450339976710054
-  a: 1.1976057386482459
+  a: 1.1976057386482457
   relative_time: 0.74000000000000032
 }
 trajectory_point {
@@ -548,16 +548,16 @@ trajectory_point {
     ddkappa: 0
   }
   v: 0.86899549330311121
-  a: 1.1985233177384225
+  a: 1.1985233177384222
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 587581.32534613891
-    y: 4140851.5513192085
+    x: 587581.32534613879
+    y: 4140851.551319208
     theta: 1.2681895332115789
-    kappa: 0.0022995756221894695
-    s: 0.34038234914803384
+    kappa: 0.00229957562218947
+    s: 0.34038234914803389
     dkappa: 0
     ddkappa: 0
   }
@@ -568,10 +568,10 @@ trajectory_point {
 trajectory_point {
   path_point {
     x: 587581.330693601
-    y: 4140851.5686106132
+    y: 4140851.5686106137
     theta: 1.2683243938966231
-    kappa: 0.002421852598944309
-    s: 0.35848174287656176
+    kappa: 0.0024218525989443095
+    s: 0.35848174287656182
     dkappa: 0
     ddkappa: 0
   }
@@ -590,7 +590,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 0.94097337500992317
-  a: 1.2005676207008846
+  a: 1.2005676207008844
   relative_time: 0.8200000000000004
 }
 trajectory_point {
@@ -609,11 +609,11 @@ trajectory_point {
 }
 trajectory_point {
   path_point {
-    x: 587581.347587132
-    y: 4140851.6232370669
+    x: 587581.3475871319
+    y: 4140851.6232370664
     theta: 1.2687504425694307
-    kappa: 0.0028081461024451449
-    s: 0.41566076709018906
+    kappa: 0.0028081461024451453
+    s: 0.41566076709018912
     dkappa: 0
     ddkappa: 0
   }
@@ -638,10 +638,10 @@ trajectory_point {
 trajectory_point {
   path_point {
     x: 587581.35955928755
-    y: 4140851.6619498972
+    y: 4140851.6619498977
     theta: 1.2690523769226205
-    kappa: 0.0030819056807826939
-    s: 0.45618256053640277
+    kappa: 0.0030819056807826943
+    s: 0.45618256053640288
     dkappa: 0
     ddkappa: 0
   }
@@ -652,10 +652,10 @@ trajectory_point {
 trajectory_point {
   path_point {
     x: 587581.36575839727
-    y: 4140851.6819951669
+    y: 4140851.6819951665
     theta: 1.2692087169991777
-    kappa: 0.0032236567347690205
-    s: 0.47716450011016481
+    kappa: 0.0032236567347690213
+    s: 0.47716450011016492
     dkappa: 0
     ddkappa: 0
   }
@@ -688,7 +688,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 1.109189021535204
-  a: 1.2017516689581258
+  a: 1.201751668958126
   relative_time: 0.96000000000000052
 }
 trajectory_point {
@@ -696,27 +696,27 @@ trajectory_point {
     x: 587581.38520792418
     y: 4140851.7448866181
     theta: 1.2696992306562112
-    kappa: 0.00366839655279957
-    s: 0.54299472659202941
+    kappa: 0.0036683965527995709
+    s: 0.54299472659202952
     dkappa: 0
     ddkappa: 0
   }
   v: 1.133222399745033
-  a: 1.2015734529362139
+  a: 1.2015734529362141
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 587581.39197512693
-    y: 4140851.7667688569
+    x: 587581.391975127
+    y: 4140851.7667688574
     theta: 1.2698698987218413
-    kappa: 0.0038231378257295879
-    s: 0.56589947366006776
+    kappa: 0.0038231378257295888
+    s: 0.56589947366006788
     dkappa: 0
     ddkappa: 0
   }
   v: 1.1572514658849924
-  a: 1.2013211717652914
+  a: 1.2013211717652916
   relative_time: 1.0000000000000004
 }
 trajectory_point {
@@ -730,7 +730,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 1.2772847416354178
-  a: 1.1990978454414889
+  a: 1.1990978454414893
   relative_time: 1.1000000000000005
 }
 trajectory_point {
@@ -744,7 +744,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 1.3970306078394916
-  a: 1.19566170510372
+  a: 1.1956617051037206
   relative_time: 1.2000000000000006
 }
 trajectory_point {
@@ -758,7 +758,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 1.5163944003754548
-  a: 1.1915450963447336
+  a: 1.1915450963447343
   relative_time: 1.3000000000000007
 }
 trajectory_point {
@@ -772,7 +772,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 1.635334689680823
-  a: 1.1872803647572792
+  a: 1.18728036475728
   relative_time: 1.4000000000000008
 }
 trajectory_point {
@@ -785,8 +785,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.7538632807523866
-  a: 1.1833998559341059
+  v: 1.7538632807523868
+  a: 1.1833998559341068
   relative_time: 1.5000000000000009
 }
 trajectory_point {
@@ -799,8 +799,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.8720452131462118
-  a: 1.1804359154679627
+  v: 1.8720452131462122
+  a: 1.1804359154679638
   relative_time: 1.600000000000001
 }
 trajectory_point {
@@ -808,13 +808,13 @@ trajectory_point {
     x: 587581.71773113473
     y: 4140852.8201244543
     theta: 1.2780854660481105
-    kappa: 0.011271990861226551
-    s: 1.668475997004345
+    kappa: 0.011271990861226554
+    s: 1.6684759970043455
     dkappa: 0
     ddkappa: 0
   }
-  v: 1.9899987609776388
-  a: 1.1789208889515992
+  v: 1.9899987609776391
+  a: 1.1789208889516003
   relative_time: 1.7000000000000011
 }
 trajectory_point {
@@ -822,13 +822,13 @@ trajectory_point {
     x: 587581.77826712211
     y: 4140853.0158719448
     theta: 1.2796121430339493
-    kappa: 0.012656228334947908
-    s: 1.8733703255654583
+    kappa: 0.012656228334947912
+    s: 1.8733703255654588
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.1078954329212829
-  a: 1.1793871219777643
+  v: 2.1078954329212833
+  a: 1.1793871219777654
   relative_time: 1.8000000000000012
 }
 trajectory_point {
@@ -841,8 +841,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.2259599722110344
-  a: 1.1823669601392073
+  v: 2.2259599722110353
+  a: 1.1823669601392086
   relative_time: 1.9000000000000012
 }
 trajectory_point {
@@ -856,7 +856,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 2.34447035664006
-  a: 1.1883927490286779
+  a: 1.1883927490286785
   relative_time: 2.0000000000000013
 }
 trajectory_point {
@@ -870,7 +870,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 2.4636548026981009
-  a: 1.1948869376386773
+  a: 1.1948869376386777
   relative_time: 2.1000000000000014
 }
 trajectory_point {
@@ -884,7 +884,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 2.5833667614079023
-  a: 1.1989516434518863
+  a: 1.1989516434518865
   relative_time: 2.2000000000000015
 }
 trajectory_point {
@@ -898,7 +898,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 2.7033658769061861
-  a: 1.2006387147963216
+  a: 1.2006387147963218
   relative_time: 2.3000000000000016
 }
 trajectory_point {
@@ -911,7 +911,7 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 2.8234169781624749
+  v: 2.8234169781624754
   a: 1.2000000000000002
   relative_time: 2.4000000000000017
 }
@@ -926,7 +926,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 2.943290078979095
-  a: 1.1970873473909387
+  a: 1.1970873473909385
   relative_time: 2.5000000000000018
 }
 trajectory_point {
@@ -939,7 +939,7 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.0627603779911721
+  v: 3.0627603779911725
   a: 1.1919526052971539
   relative_time: 2.6000000000000019
 }
@@ -953,7 +953,7 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.1816082586666354
+  v: 3.1816082586666359
   a: 1.1846476220466629
   relative_time: 2.700000000000002
 }
@@ -1010,7 +1010,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 3.6465647365089797
-  a: 1.1347622440379717
+  a: 1.1347622440379719
   relative_time: 3.1000000000000023
 }
 trajectory_point {
@@ -1023,8 +1023,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.75918774666836
-  a: 1.1173837799242012
+  v: 3.7591877466683603
+  a: 1.1173837799242015
   relative_time: 3.2000000000000024
 }
 trajectory_point {
@@ -1037,8 +1037,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.8699795207875329
-  a: 1.0981461646218253
+  v: 3.8699795207875334
+  a: 1.0981461646218256
   relative_time: 3.3000000000000025
 }
 trajectory_point {
@@ -1052,7 +1052,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 3.9787567361640392
-  a: 1.0771012464588607
+  a: 1.0771012464588612
   relative_time: 3.4000000000000026
 }
 trajectory_point {
@@ -1065,8 +1065,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0853412549282195
-  a: 1.0543008737633244
+  v: 4.08534125492822
+  a: 1.0543008737633248
   relative_time: 3.5000000000000027
 }
 trajectory_point {
@@ -1080,7 +1080,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 4.1895601240432185
-  a: 1.0297968948632332
+  a: 1.0297968948632334
   relative_time: 3.6000000000000028
 }
 trajectory_point {
@@ -1094,7 +1094,7 @@ trajectory_point {
     ddkappa: 0
   }
   v: 4.2912455753049823
-  a: 1.0036411580866038
+  a: 1.003641158086604
   relative_time: 3.7000000000000028
 }
 trajectory_point {
@@ -1107,8 +1107,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3902350253422551
-  a: 0.97588551176145288
+  v: 4.390235025342256
+  a: 0.97588551176145322
   relative_time: 3.8000000000000029
 }
 trajectory_point {
@@ -1121,8 +1121,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4863710756165887
-  a: 0.94658180421579763
+  v: 4.4863710756165895
+  a: 0.94658180421579774
   relative_time: 3.900000000000003
 }
 trajectory_point {
@@ -1130,13 +1130,13 @@ trajectory_point {
     x: 587583.44385588367
     y: 4140860.2841138532
     theta: 1.4043799162679214
-    kappa: 0.010099867606676204
-    s: 9.3357236499762823
+    kappa: 0.010099867606676197
+    s: 9.3357236499762841
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5795015124223317
-  a: 0.91578188377765424
+  v: 4.5795015124223326
+  a: 0.91578188377765479
   relative_time: 4.0000000000000027
 }
 trajectory_point {
@@ -1144,13 +1144,13 @@ trajectory_point {
     x: 587583.52158336388
     y: 4140860.740012804
     theta: 1.4096515069831463
-    kappa: 0.0084541535542427548
-    s: 9.79820112405032
+    kappa: 0.0084541535542427479
+    s: 9.7982011240503226
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6695417253938949
-  a: 0.8854067774097949
+  v: 4.6695417253938958
+  a: 0.88540677740979534
   relative_time: 4.1000000000000023
 }
 trajectory_point {
@@ -1158,41 +1158,41 @@ trajectory_point {
     x: 587583.597003207
     y: 4140861.2052592044
     theta: 1.4130672061774077
-    kappa: 0.0066568021342505749
-    s: 10.269534575744878
+    kappa: 0.0066568021342505689
+    s: 10.269534575744879
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7566592617611274
-  a: 0.85732348674706382
+  v: 4.7566592617611283
+  a: 0.85732348674706416
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 587583.67043558892
+    x: 587583.67043558881
     y: 4140861.6795164365
     theta: 1.4148135020160175
-    kappa: 0.0047204382736954244
-    s: 10.749443147828606
+    kappa: 0.0047204382736954183
+    s: 10.749443147828607
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8410818436113487
-  a: 0.83150282212558813
+  v: 4.84108184361135
+  a: 0.83150282212558846
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
     x: 587583.74514056533
-    y: 4140862.1619926211
+    y: 4140862.1619926207
     theta: 1.4165900675898269
-    kappa: 0.002750516929405861
-    s: 11.237668609330569
+    kappa: 0.0027505169294058537
+    s: 11.237668609330571
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.92303427406549
-  a: 0.80791559388149514
+  v: 4.9230342740654915
+  a: 0.80791559388149548
   relative_time: 4.4000000000000012
 }
 trajectory_point {
@@ -1200,13 +1200,13 @@ trajectory_point {
     x: 587583.8210820409
     y: 4140862.6524546365
     theta: 1.4183960333335888
-    kappa: 0.00074798991114628441
-    s: 11.73397506364361
+    kappa: 0.00074798991114627725
+    s: 11.733975063643612
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.0027384372780972
-  a: 0.78653261235091187
+  v: 5.002738437278099
+  a: 0.78653261235091221
   relative_time: 4.5000000000000009
 }
 trajectory_point {
@@ -1214,13 +1214,13 @@ trajectory_point {
     x: 587583.89839040581
     y: 4140863.1506658271
     theta: 1.4185553665847759
-    kappa: -0.001227026848237335
-    s: 12.238148656627702
+    kappa: -0.0012270268482373484
+    s: 12.238148656627706
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.0804132984373265
-  a: 0.76732468786996577
+  v: 5.0804132984373283
+  a: 0.76732468786996588
   relative_time: 4.6000000000000005
 }
 trajectory_point {
@@ -1228,13 +1228,13 @@ trajectory_point {
     x: 587583.977107294
     y: 4140863.6564253252
     theta: 1.4163378650308922
-    kappa: -0.0031479562018466981
-    s: 12.749997284713315
+    kappa: -0.0031479562018467046
+    s: 12.749997284713317
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.1562749037649489
-  a: 0.75026263077478372
+  v: 5.15627490376495
+  a: 0.750262630774784
   relative_time: 4.7
 }
 trajectory_point {
@@ -1242,13 +1242,13 @@ trajectory_point {
     x: 587584.05697827786
     y: 4140864.1695999382
     theta: 1.4140878396957532
-    kappa: -0.0050970489667389422
-    s: 13.269350303004776
+    kappa: -0.0050970489667389552
+    s: 13.26935030300478
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2305363805163463
-  a: 0.73531725140149318
+  v: 5.2305363805163472
+  a: 0.73531725140149329
   relative_time: 4.8
 }
 trajectory_point {
@@ -1256,27 +1256,27 @@ trajectory_point {
     x: 587584.13798036915
     y: 4140864.6900419667
     theta: 1.4118059609102658
-    kappa: -0.0070737441627928025
-    s: 13.796058233383636
+    kappa: -0.0070737441627928095
+    s: 13.796058233383638
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.3034079369805145
-  a: 0.72245936008622125
+  v: 5.3034079369805154
+  a: 0.72245936008622136
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
     x: 587584.22460301069
-    y: 4140865.2168862973
-    theta: 1.4075683189780006
-    kappa: -0.0087697120979148228
-    s: 14.329992472612018
+    y: 4140865.2168862978
+    theta: 1.4075683189780004
+    kappa: -0.0087697120979148262
+    s: 14.32999247261202
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.3750968624800617
-  a: 0.71165976716509516
+  v: 5.3750968624800626
+  a: 0.71165976716509527
   relative_time: 4.9999999999999991
 }
 trajectory_point {
@@ -1284,12 +1284,12 @@ trajectory_point {
     x: 587584.31617573148
     y: 4140865.7501332145
     theta: 1.4016540485455984
-    kappa: -0.010229186687875348
-    s: 14.871045000435991
+    kappa: -0.010229186687875362
+    s: 14.871045000435997
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4458075273712083
+  v: 5.4458075273712092
   a: 0.702889282974242
   relative_time: 5.0999999999999988
 }
@@ -1298,13 +1298,13 @@ trajectory_point {
     x: 587584.408938369
     y: 4140866.2903092634
     theta: 1.395662795350513
-    kappa: -0.011707626022129837
-    s: 15.419128087688932
+    kappa: -0.011707626022129847
+    s: 15.419128087688936
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.51574138304379
-  a: 0.696118717849789
+  v: 5.5157413830437907
+  a: 0.69611871784978907
   relative_time: 5.1999999999999984
 }
 trajectory_point {
@@ -1312,13 +1312,13 @@ trajectory_point {
     x: 587584.50287945988
     y: 4140866.8373476909
     theta: 1.3895957330091258
-    kappa: -0.013204847400896258
-    s: 15.974174004394873
+    kappa: -0.013204847400896268
+    s: 15.974174004394877
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.58509696192125
-  a: 0.69131888212786341
+  v: 5.5850969619212512
+  a: 0.69131888212786352
   relative_time: 5.299999999999998
 }
 trajectory_point {
@@ -1331,8 +1331,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.65406987746065
-  a: 0.68846058614459238
+  v: 5.6540698774606506
+  a: 0.68846058614459249
   relative_time: 5.3999999999999977
 }
 trajectory_point {
@@ -1340,55 +1340,55 @@ trajectory_point {
     x: 587584.7216009933
     y: 4140867.9467776418
     theta: 1.373368935782048
-    kappa: -0.014240285408513211
-    s: 17.104981650835381
+    kappa: -0.014240285408513216
+    s: 17.104981650835384
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.7228528241526613
-  a: 0.6875146402361032
+  v: 5.7228528241526622
+  a: 0.68751464023610331
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
     x: 587584.833894424
-    y: 4140868.5114438208
+    y: 4140868.5114438203
     theta: 1.3649746434585079
-    kappa: -0.014698411908945691
-    s: 17.680705289501592
+    kappa: -0.014698411908945693
+    s: 17.680705289501596
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.7916355775215669
-  a: 0.68845185473852288
+  v: 5.7916355775215678
+  a: 0.68845185473852311
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
     x: 587584.95362101321
-    y: 4140869.0815603314
-    theta: 1.3566998995473762
-    kappa: -0.014568256458587031
-    s: 18.263314991690812
+    y: 4140869.0815603309
+    theta: 1.356699899547376
+    kappa: -0.014568256458587024
+    s: 18.263314991690816
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.8606049941252669
-  a: 0.69124303998797887
+  v: 5.8606049941252678
+  a: 0.69124303998797909
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
     x: 587585.08537684276
-    y: 4140869.6561719729
-    theta: 1.3487076377710505
-    kappa: -0.013402267392278251
-    s: 18.852838644930813
+    y: 4140869.6561719738
+    theta: 1.3487076377710503
+    kappa: -0.013402267392278244
+    s: 18.852838644930817
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.9299450115552679
-  a: 0.695859006320598
+  v: 5.9299450115552688
+  a: 0.69585900632059827
   relative_time: 5.7999999999999963
 }
 trajectory_point {
@@ -1401,8 +1401,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.9998366484366956
-  a: 0.702270564072508
+  v: 5.9998366484366965
+  a: 0.70227056407250832
   relative_time: 5.8999999999999959
 }
 trajectory_point {
@@ -1415,22 +1415,22 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.0704580044282821
-  a: 0.71044852357983568
+  v: 6.0704580044282839
+  a: 0.7104485235798359
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 587585.50247845287
+    x: 587585.502478453
     y: 4140871.4179888326
     theta: 1.3287223161989641
-    kappa: -0.00761462347123239
-    s: 20.663444152015906
+    kappa: -0.0076146234712323709
+    s: 20.663444152015909
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.1419842602223795
-  a: 0.72036369517870813
+  v: 6.1419842602223813
+  a: 0.72036369517870846
   relative_time: 6.0999999999999952
 }
 trajectory_point {
@@ -1438,27 +1438,27 @@ trajectory_point {
     x: 587585.65309875284
     y: 4140872.0171663789
     theta: 1.3249447271854193
-    kappa: -0.004168213464236732
-    s: 21.281263062503832
+    kappa: -0.0041682134642367129
+    s: 21.281263062503836
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.2145876775449462
-  a: 0.73198688920525279
+  v: 6.214587677544948
+  a: 0.73198688920525312
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
     x: 587585.80550393113
     y: 4140872.623444289
-    theta: 1.3211224143852067
-    kappa: -0.00068096286367418525
-    s: 21.906403240911118
+    theta: 1.3211224143852065
+    kappa: -0.00068096286367414556
+    s: 21.906403240911125
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.2884375991555563
-  a: 0.74528891599559688
+  v: 6.2884375991555572
+  a: 0.74528891599559732
   relative_time: 6.2999999999999945
 }
 trajectory_point {
@@ -1466,13 +1466,13 @@ trajectory_point {
     x: 587585.96086567116
     y: 4140873.236663701
     theta: 1.32148939606489
-    kappa: 0.0011667933575799732
-    s: 22.538997683180945
+    kappa: 0.0011667933575799808
+    s: 22.538997683180948
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.3637004488473963
-  a: 0.76024058588586718
+  v: 6.3637004488473981
+  a: 0.76024058588586774
   relative_time: 6.3999999999999941
 }
 trajectory_point {
@@ -1485,8 +1485,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.4405397314472648
-  a: 0.7768127092121917
+  v: 6.4405397314472657
+  a: 0.776812709212192
   relative_time: 6.4999999999999938
 }
 trajectory_point {
@@ -1494,13 +1494,13 @@ trajectory_point {
     x: 587586.27792156138
     y: 4140874.4852015739
     theta: 1.324799272371749
-    kappa: 0.0039121604697928466
-    s: 23.827163533339306
+    kappa: 0.0039121604697928544
+    s: 23.827163533339309
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5191160328155728
-  a: 0.79497609631069677
+  v: 6.5191160328155746
+  a: 0.79497609631069732
   relative_time: 6.5999999999999934
 }
 trajectory_point {
@@ -1513,8 +1513,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5995870198463464
-  a: 0.81470155751751
+  v: 6.5995870198463482
+  a: 0.81470155751751061
   relative_time: 6.6999999999999931
 }
 trajectory_point {
@@ -1522,13 +1522,13 @@ trajectory_point {
     x: 587586.59404094215
     y: 4140875.7667686203
     theta: 1.3310065886499012
-    kappa: 0.004972992805275283
-    s: 25.147149255004869
+    kappa: 0.0049729928052752873
+    s: 25.147149255004877
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.6821074404672229
-  a: 0.83595990316875868
+  v: 6.6821074404672247
+  a: 0.83595990316875934
   relative_time: 6.7999999999999927
 }
 trajectory_point {
@@ -1536,13 +1536,13 @@ trajectory_point {
     x: 587586.75416263437
     y: 4140876.4198538139
     theta: 1.3344628834244958
-    kappa: 0.005330336333502733
-    s: 25.819577114437759
+    kappa: 0.0053303363335027347
+    s: 25.819577114437763
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.76682912363945
-  a: 0.85872194360057
+  v: 6.766829123639452
+  a: 0.85872194360057064
   relative_time: 6.8999999999999924
 }
 trajectory_point {
@@ -1550,55 +1550,55 @@ trajectory_point {
     x: 587586.91173991316
     y: 4140877.0823800606
     theta: 1.3379010267988873
-    kappa: 0.0051805640675549013
-    s: 26.500593422060923
+    kappa: 0.0051805640675548995
+    s: 26.500593422060927
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.853900979357892
-  a: 0.88295848914907071
+  v: 6.8539009793578938
+  a: 0.88295848914907138
   relative_time: 6.999999999999992
 }
 trajectory_point {
   path_point {
     x: 587587.068942555
-    y: 4140877.7540766792
+    y: 4140877.7540766788
     theta: 1.3413510063696825
-    kappa: 0.0047592759508166636
-    s: 27.190440519005122
+    kappa: 0.0047592759508166592
+    s: 27.190440519005129
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9434689986510243
-  a: 0.90864035015038846
+  v: 6.9434689986510261
+  a: 0.90864035015038924
   relative_time: 7.0999999999999917
 }
 trajectory_point {
   path_point {
-    x: 587587.22821607906
+    x: 587587.22821607918
     y: 4140878.4346217783
     theta: 1.3448464166078502
-    kappa: 0.0043324380666007216
-    s: 27.889375199555644
+    kappa: 0.0043324380666007146
+    s: 27.889375199555655
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0356762535809345
-  a: 0.93573833694065012
+  v: 7.0356762535809363
+  a: 0.935738336940651
   relative_time: 7.1999999999999913
 }
 trajectory_point {
   path_point {
     x: 587587.385252705
-    y: 4140879.1252828552
-    theta: 1.3473096133314184
-    kappa: 0.0035281682692150972
-    s: 28.59766841925568
+    y: 4140879.1252828548
+    theta: 1.3473096133314186
+    kappa: 0.0035281682692150837
+    s: 28.59766841925569
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.1306628972433224
-  a: 0.96422325985598334
+  v: 7.130662897243325
+  a: 0.96422325985598423
   relative_time: 7.2999999999999909
 }
 trajectory_point {
@@ -1606,13 +1606,13 @@ trajectory_point {
     x: 587587.5430921301
     y: 4140879.8256539386
     theta: 1.349476644883441
-    kappa: 0.0025993588666431838
-    s: 29.315605003009658
+    kappa: 0.00259935886664317
+    s: 29.315605003009669
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.2285661637675034
-  a: 0.9940659292325148
+  v: 7.2285661637675052
+  a: 0.994065929232516
   relative_time: 7.3999999999999906
 }
 trajectory_point {
@@ -1620,27 +1620,27 @@ trajectory_point {
     x: 587587.70311726735
     y: 4140880.5357235465
     theta: 1.3516736771643856
-    kappa: 0.0016576875952923256
-    s: 30.043483353186627
+    kappa: 0.0016576875952923117
+    s: 30.043483353186637
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.3295203683164019
-  a: 1.0252371554063717
+  v: 7.3295203683164054
+  a: 1.0252371554063731
   relative_time: 7.49999999999999
 }
 trajectory_point {
   path_point {
-    x: 587587.86311638285
-    y: 4140881.2563057821
+    x: 587587.863116383
+    y: 4140881.2563057817
     theta: 1.3517884100488506
-    kappa: 0.00053767905254736728
-    s: 30.781615157723607
+    kappa: 0.00053767905254735644
+    s: 30.781615157723614
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.4336569070865588
-  a: 1.0577077487136823
+  v: 7.4336569070865606
+  a: 1.0577077487136834
   relative_time: 7.59999999999999
 }
 trajectory_point {
@@ -1648,13 +1648,13 @@ trajectory_point {
     x: 587588.02537208109
     y: 4140881.9872227837
     theta: 1.3518710920197292
-    kappa: -0.000601012328469535
-    s: 31.530325098228946
+    kappa: -0.00060101232846954057
+    s: 31.53032509822895
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.5411042573081239
-  a: 1.0914485194905721
+  v: 7.5411042573081266
+  a: 1.0914485194905734
   relative_time: 7.6999999999999895
 }
 trajectory_point {
@@ -1667,8 +1667,8 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.6519879772448629
-  a: 1.1264302780731699
+  v: 7.6519879772448656
+  a: 1.126430278073171
   relative_time: 7.7999999999999892
 }
 trajectory_point {
@@ -1681,22 +1681,22 @@ trajectory_point {
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7664307061941518
-  a: 1.1626238347976017
+  v: 7.7664307061941544
+  a: 1.1626238347976028
   relative_time: 7.8999999999999888
 }
 trajectory_point {
   path_point {
     x: 587588.53140072094
-    y: 4140884.2442241902
+    y: 4140884.2442241907
     theta: 1.346985815635551
-    kappa: -0.0037132089634053008
-    s: 33.843359326879273
+    kappa: -0.00371320896340531
+    s: 33.843359326879281
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8845521644869807
-  a: 1.1999999999999953
+  v: 7.8845521644869834
+  a: 1.1999999999999962
   relative_time: 7.9999999999999885
 }
 decision {
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_qp_path_failure_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_qp_path_failure_0.pb.txt
index 5ffbc271a8355b85885fd6413b88c5c574aa0291..15679a0420f03a4727b31f8e642ad112c085c06a 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_qp_path_failure_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_qp_path_failure_0.pb.txt
@@ -1,7 +1,7 @@
 header {
   module_name: "planning"
 }
-total_path_length: 42.294144915274117
+total_path_length: 59.130759095790431
 total_path_time: 7.9999999999999885
 is_replan: true
 gear: GEAR_DRIVE
@@ -17,1687 +17,1687 @@ trajectory_point {
     ddkappa: 0
   }
   v: 4.8822846412658691
-  a: -0.44582038098355159
+  a: -0.23306851931528991
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 586916.5138189611
-    y: 4141003.5402172045
-    theta: 2.6096935920353492
-    kappa: -0.023864416470194726
-    s: 0.097556826756007561
+    x: 586916.51378334849
+    y: 4141003.5402399711
+    theta: 2.6096920179204508
+    kappa: -0.023865041637348906
+    s: 0.0975990949574055
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8734126429858611
-  a: -0.441437630190783
+  v: 4.877626124509014
+  a: -0.2326876752060355
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 586916.43177219282
-    y: 4141003.5926694591
-    theta: 2.606066618929443
-    kappa: -0.025304717349815414
-    s: 0.1949370669788047
+    x: 586916.43163058977
+    y: 4141003.592759985
+    theta: 2.606060358495367
+    kappa: -0.025307203139062914
+    s: 0.19510513377477057
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8646248398314142
-  a: -0.43739931342154664
+  v: 4.8729809202660261
+  a: -0.23173892983700534
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 586916.34987284441
-    y: 4141003.645027468
-    theta: 2.602445378825069
-    kappa: -0.026742430325215262
-    s: 0.29214233630610026
+    x: 586916.34955591522
+    y: 4141003.64523008
+    theta: 2.6024313641620123
+    kappa: -0.026747993903476798
+    s: 0.29251849561786114
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8559144364224105
-  a: -0.43369610064066427
+  v: 4.8683602868475946
+  a: -0.23023225465884911
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 586916.26811966789
-    y: 4141003.69729203
-    theta: 2.5988299152332739
-    kappa: -0.028177577309917809
-    s: 0.38917411633400784
+    x: 586916.26755881717
+    y: 4141003.6976505807
+    theta: 2.5988051099408915
+    kappa: -0.028187422839456494
+    s: 0.38983978282436682
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8472748239794328
-  a: -0.43031866181295764
+  v: 4.8637752831353955
+  a: -0.22817762112221648
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 586916.18651152449
-    y: 4141003.749463873
-    theta: 2.5952202755676583
-    kappa: -0.029610178290104802
-    s: 0.4860337583490596
+    x: 586916.18563860352
+    y: 4141003.7500219285
+    theta: 2.5951816622330814
+    kappa: -0.029625502097695641
+    s: 0.48706981691532053
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8386995803237722
-  a: -0.42725766690324857
+  v: 4.8592367685820914
+  a: -0.22558500067775716
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 586916.10504738241
-    y: 4141003.8015436558
-    theta: 2.5916165110108289
-    kappa: -0.031040251379814532
-    s: 0.58272248706022067
+    x: 586916.10379440081
+    y: 4141003.8023446831
+    theta: 2.5915610789551069
+    kappa: -0.031062247011722586
+    s: 0.58420963460651842
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8301824698774176
-  a: -0.42450378587635895
+  v: 4.8547554032113327
+  a: -0.22246436477612086
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 586916.02372631326
-    y: 4141003.8535319725
-    theta: 2.5880186763790958
-    kappa: -0.032467812876140104
-    s: 0.67924140433090285
+    x: 586916.0220251577
+    y: 4141003.854619517
+    theta: 2.5879434096772855
+    kappa: -0.032497676038907318
+    s: 0.68126048381993931
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8217174436630668
-  a: -0.42204768869711051
+  v: 4.850341647617757
+  a: -0.21882568486795725
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 586915.94254748919
-    y: 4141003.905429353
-    theta: 2.5844268299855169
-    kappa: -0.033892877314427736
-    s: 0.77559149291097906
+    x: 586915.940329648
+    y: 4141003.9068472125
+    theta: 2.5843286957654357
+    kappa: -0.033931810701468453
+    s: 0.77822381969516485
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8132986393041168
-  a: -0.41988004533032508
+  v: 4.8460057629669873
+  a: -0.21467893240391606
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 586915.86151017924
-    y: 4141003.9572362639
-    theta: 2.580841033501371
-    kappa: -0.035315457523475056
-    s: 0.87177362016879723
+    x: 586915.85870647454
+    y: 4141003.959028664
+    theta: 2.5807169705258546
+    kappa: -0.03536467552748021
+    s: 0.87510130060079894
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8049203810246714
-  a: -0.41799152574082449
+  v: 4.8417578109956354
+  a: -0.21003407883464695
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 586915.78061374661
-    y: 4141004.0089531122
-    theta: 2.5772613518161629
-    kappa: -0.036735564680729381
-    s: 0.9677885418231944
+    x: 586915.77715407242
+    y: 4141004.011164872
+    theta: 2.577108259353464
+    kappa: -0.036796297991879362
+    s: 0.97189478414588781
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7965771796495362
-  a: -0.41637279989343051
+  v: 4.8376076540112987
+  a: -0.20490109561079961
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 586915.69985764567
-    y: 4141004.0605802462
-    theta: 2.5736878528962444
-    kappa: -0.038153208367486036
-    s: 1.0636369056755106
+    x: 586915.69567071158
+    y: 4141004.0632569408
+    theta: 2.5735025798830296
+    kappa: -0.038226708457472204
+    s: 1.0686063231913392
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7882637326042214
-  a: -0.41501453775296504
+  v: 4.8335649548925623
+  a: -0.19928995418302378
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586915.61924141855
-    y: 4141004.1121179606
-    theta: 2.5701206076421435
-    kappa: -0.039568396624086635
-    s: 1.1593192553416032
+    x: 586915.61425450176
+    y: 4141004.1153060813
+    theta: 2.5698999421433482
+    kappa: -0.039655940115941535
+    s: 1.1652381618613423
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7799749239149394
-  a: -0.41390740928424979
+  v: 4.829639177089
+  a: -0.19321062600196909
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 586915.53876469145
-    y: 4141004.163566492
-    theta: 2.5665596897446994
-    kappa: -0.040981136005117366
-    s: 1.2548360339838611
+    x: 586915.532903394
+    y: 4141004.1673136018
+    theta: 2.5663003487143041
+    kappa: -0.041084028928853614
+    s: 1.2617927315547881
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7717058242086079
-  a: -0.41304208445210655
+  v: 4.8258395846211677
+  a: -0.18667308251828529
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586915.45842717285
-    y: 4141004.2149260286
-    theta: 2.5630051755400833
-    kappa: -0.042391431634607304
-    s: 1.3501875880432179
+    x: 586915.45161518548
+    y: 4141004.2192809116
+    theta: 2.5627037948866969
+    kappa: -0.042511013568665122
+    s: 1.3582726469566886
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7634516907128459
-  a: -0.41240923322135725
+  v: 4.8221752420806148
+  a: -0.17968729518262203
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 586915.378228649
-    y: 4141004.2661967054
-    theta: 2.5594571438638094
-    kappa: -0.043799287261226713
-    s: 1.4453741709711676
+    x: 586915.37038752227
+    y: 4141004.2712095142
+    theta: 2.5591102688247331
+    kappa: -0.043936935359730159
+    s: 1.454680702049596
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7552079672559788
-  a: -0.41199952555682362
+  v: 4.8186550146298721
+  a: -0.17226323544562902
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 586915.29816898156
-    y: 4141004.3173786122
-    theta: 2.5559156759038117
-    kappa: -0.04520470531348527
-    s: 1.5403959469617767
+    x: 586915.28921790246
+    y: 4141004.32310101
+    theta: 2.5555197517310848
+    kappa: -0.045361838219307178
+    s: 1.5510198661250232
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7469702842670332
-  a: -0.41180363142332749
+  v: 4.8152875680024607
+  a: -0.16441087475795596
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 586915.21824810433
-    y: 4141004.3684717906
-    theta: 2.5523808550526881
-    kappa: -0.04660768695493047
-    s: 1.6352529946836998
+    x: 586915.20810367994
+    y: 4141004.3749570907
+    theta: 2.5519322180144162
+    kappa: -0.046785768598565952
+    s: 1.6472932797948643
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.73873445877574
-  a: -0.41181222078569069
+  v: 4.8120813685028869
+  a: -0.15614018457025258
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 586915.13846602
-    y: 4141004.4194762395
-    theta: 2.5488527667591923
-    kappa: -0.048008232139345838
-    s: 1.7299453110121934
+    x: 586915.12704206782
+    y: 4141004.4267795379
+    theta: 2.5483476354592707
+    kappa: -0.048208775423594585
+    s: 1.7435042510028123
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7304964944125345
-  a: -0.41201596360873494
+  v: 4.809044683006646
+  a: -0.14746113633316849
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586915.0588227968
-    y: 4141004.4703919142
-    theta: 2.5453314983790691
-    kappa: -0.049406339665949241
-    s: 1.8244728147611293
+    x: 586915.04603014118
+    y: 4141004.478570221
+    theta: 2.5447659653982235
+    kappa: -0.049630910036406427
+    s: 1.83965625103578
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7222525814085534
-  a: -0.41240552985728213
+  v: 4.8061855789602186
+  a: -0.13838370149735346
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586914.97931856592
-    y: 4141004.521218732
-    theta: 2.5418171390253144
-    kappa: -0.050802007234591211
-    s: 1.9188353504150093
+    x: 586914.96506484132
+    y: 4141004.5303310961
+    theta: 2.5411871628862031
+    kappa: -0.051052226135947086
+    s: 1.9357529105353204
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.71399909659564
-  a: -0.41297158949615409
+  v: 4.8035119243810724
+  a: -0.12891785151345711
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586914.90120499232
-    y: 4141004.5737094851
-    theta: 2.5378125500121751
-    kappa: -0.052142013489499865
-    s: 2.0130326918609791
+    x: 586914.88651062234
+    y: 4141004.5853805114
+    theta: 2.5366702722946255
+    kappa: -0.05237209741857584
+    s: 2.0317980155090454
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.70573260340634
-  a: -0.41370481249017255
+  v: 4.8010313878576634
+  a: -0.11907355783212922
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 586914.82757243083
-    y: 4141004.6321922643
-    theta: 2.532087165289802
-    kappa: -0.053294949558824647
-    s: 2.1070645461208417
+    x: 586914.81133885239
+    y: 4141004.64508581
+    theta: 2.5308244164385707
+    kappa: -0.053549134361624576
+    s: 2.1277955033420461
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.697449851873901
-  a: -0.41459586880415938
+  v: 4.7987514385494325
+  a: -0.10886079190401944
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586914.75406973471
-    y: 4141004.6905718986
-    theta: 2.5263683552487963
-    kappa: -0.054445852203187227
-    s: 2.2009305570830739
+    x: 586914.736201171
+    y: 4141004.7047640341
+    theta: 2.5249776180522319
+    kappa: -0.054725637548998854
+    s: 2.2237494588083111
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6891477786322753
-  a: -0.41563542840293638
+  v: 4.79667934618681
+  a: -0.098289525179777487
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586914.680697229
-    y: 4141004.7488481281
-    theta: 2.5206565340319016
-    kappa: -0.055594716327933849
-    s: 2.2946303092348361
+    x: 586914.66109426692
+    y: 4141004.7644178122
+    theta: 2.5191300206862728
+    kappa: -0.055901658822861126
+    s: 2.3196641100821491
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.680823506916119
-  a: -0.41681416125132531
+  v: 4.7948221810712113
+  a: -0.087369729110053
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586914.60745528294
-    y: 4141004.8070206614
-    theta: 2.5149521164894288
-    kappa: -0.056741536155842674
-    s: 2.3881633313939905
+    x: 586914.58601472038
+    y: 4141004.8240498621
+    theta: 2.5132817591526142
+    kappa: -0.057077251734718289
+    s: 2.4155438247496064
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6724743465607927
-  a: -0.41812273731414806
+  v: 4.7931868140750407
+  a: -0.076111375145495791
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586914.5343443061
-    y: 4141004.8650891716
-    theta: 2.5092555177955465
-    kappa: -0.057886305272882438
-    s: 2.4815291004411133
+    x: 586914.5109590051
+    y: 4141004.8836629838
+    theta: 2.5074329597628742
+    kappa: -0.058252471496517091
+    s: 2.5113931058198871
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.664097794002358
-  a: -0.41955182655622641
+  v: 4.7917799166416861
+  a: -0.064524434736755371
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586914.46136474563
-    y: 4141004.9230533037
-    theta: 2.5035671530637678
-    kappa: -0.05902901667397116
-    s: 2.574727045051509
+    x: 586914.43592349219
+    y: 4141004.9432600606
+    theta: 2.5015837405762738
+    kappa: -0.059427374931739746
+    s: 2.6072165877367728
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6556915322775829
-  a: -0.42109209894238209
+  v: 4.7906079607855272
+  a: -0.052618879334481589
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586914.38851708418
-    y: 4141004.9809126751
-    theta: 2.49788743696213
-    kappa: -0.0601696628087348
-    s: 2.6677565494272244
+    x: 586914.36090445262
+    y: 4141005.0028440524
+    theta: 2.4957342116566443
+    kappa: -0.060602020426499412
+    s: 2.7030190323900429
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6472534310239366
-  a: -0.422734224437437
+  v: 4.7896772190919261
+  a: -0.040404680389324044
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586914.3158018355
-    y: 4141005.0386668788
-    theta: 2.4922167833285473
-    kappa: -0.061308235627265929
-    s: 2.760616957029062
+    x: 586914.285898061
+    y: 4141005.0624179989
+    theta: 2.489884475338163
+    kappa: -0.061776467880635795
+    s: 2.7988053251268941
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6387815464795938
-  a: -0.424468873006213
+  v: 4.7889937647172358
+  a: -0.027891809351932556
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 586914.24321954278
-    y: 4141005.0963154803
-    theta: 2.4865556047868136
-    kappa: -0.062444726625882435
-    s: 2.8533075743085958
+    x: 586914.21090039832
+    y: 4141005.1219850122
+    theta: 2.484034626499438
+    kappa: -0.062950778658810547
+    s: 2.8945804707633589
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6302741214834313
-  a: -0.42628671461353174
+  v: 4.7885634713887928
+  a: -0.015090237672956641
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 586914.17077077518
-    y: 4141005.1538580293
-    theta: 2.4809043123637342
-    kappa: -0.063579126892886209
-    s: 2.9458276744401841
+    x: 586914.13590745488
+    y: 4141005.1815482778
+    theta: 2.4781847528455536
+    kappa: -0.064125015541602923
+    s: 2.9903495895957271
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.62172958547503
-  a: -0.42817841922421518
+  v: 4.7883920134049234
+  a: -0.0020099368030461195
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 586914.098456125
-    y: 4141005.2112940559
-    theta: 2.4752633151078576
-    kappa: -0.064711427154321821
-    s: 3.038176501052984
+    x: 586914.060915134
+    y: 4141005.2411110485
+    theta: 2.4723349351976807
+    kappa: -0.065299242676605279
+    s: 3.0861179134119658
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6131465544946746
-  a: -0.43013465680308505
+  v: 4.7884848656349392
+  a: 0.011339121807149327
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586914.02627620474
-    y: 4141005.2686230736
-    theta: 2.4696330197102618
-    kappa: -0.06584161781973516
-    s: 3.130353271962965
+    x: 586913.98591925472
+    y: 4141005.3006766452
+    theta: 2.4664852477898416
+    kappa: -0.066473525529518546
+    s: 3.181890781503137
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6045238311833536
-  a: -0.43214609731496317
+  v: 4.78884730351914
+  a: 0.024946966706980017
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 586913.954231644
-    y: 4141005.3258445808
-    theta: 2.4640138301278558
-    kappa: -0.066969689027932169
-    s: 3.2223571829049233
+    x: 586913.910915555
+    y: 4141005.3602484539
+    theta: 2.4606357585724261
+    kappa: -0.06764793083524781
+    s: 3.2776736366748191
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5958604047827576
-  a: -0.43420341072467133
+  v: 4.789484403068812
+  a: 0.038803626445796296
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586913.88232308731
-    y: 4141005.3829580671
-    theta: 2.4584061472096388
-    kappa: -0.068095630692737524
-    s: 3.3141874112644958
+    x: 586913.83589969471
+    y: 4141005.4198299209
+    theta: 2.4547865295220377
+    kappa: -0.068822526548997856
+    s: 3.3734720212585261
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5871554511352821
-  a: -0.43629726699703136
+  v: 4.7904010408662279
+  a: 0.052899129572948372
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586913.81055118993
-    y: 4141005.4399630111
-    theta: 2.452810368326352
-    kappa: -0.069219432548753249
-    s: 3.405843119810176
+    x: 586913.76086725923
+    y: 4141005.4794245525
+    theta: 2.4489376169572479
+    kappa: -0.069997381797368613
+    s: 3.469291573123126
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5784083326840266
-  a: -0.43841833609686509
+  v: 4.7916018940646481
+  a: 0.067223504637786674
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 586913.73891661619
-    y: 4141005.496858886
-    theta: 2.4472268870039464
-    kappa: -0.070341084197117487
-    s: 3.4973234604253243
+    x: 586913.685813762
+    y: 4141005.5390359126
+    theta: 2.44308907185983
+    kappa: -0.071172566829450792
+    s: 3.5651380216862645
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5696185984727924
-  a: -0.44055728798899435
+  v: 4.7930914403883191
+  a: 0.081766780189661409
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 586913.66742003581
-    y: 4141005.5536451591
-    theta: 2.4416560925612796
-    kappa: -0.071460575151263125
-    s: 3.5886275778401844
+    x: 586913.61073464807
+    y: 4141005.5986676193
+    theta: 2.4372409402010513
+    kappa: -0.072348152967921381
+    s: 3.6610171839257797
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5607859841460856
-  a: -0.44270479263824086
+  v: 4.7948739581324773
+  a: 0.09651898477792295
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 586913.596062121
-    y: 4141005.6103212973
-    theta: 2.4360983697524405
-    kappa: -0.072577894882676491
-    s: 3.6797546133638988
+    x: 586913.53562529688
+    y: 4141005.6583233415
+    theta: 2.4313932632725694
+    kappa: -0.073524212560139079
+    s: 3.7569349603911255
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5519104119491161
-  a: -0.44485152000942652
+  v: 4.7969535261633416
+  a: 0.11147014695192162
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 586913.52484354412
-    y: 4141005.6668867655
-    theta: 2.430554098414087
-    kappa: -0.073693032866656044
-    s: 3.7707037086165203
+    x: 586913.46048102551
+    y: 4141005.718006799
+    theta: 2.4255460780215166
+    kappa: -0.07470081892924
+    s: 3.8528973312147907
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5429919907277965
-  a: -0.44698814006737309
+  v: 4.7993340239181208
+  a: 0.12661029526100762
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 586913.45376497414
-    y: 4141005.7233410338
-    theta: 2.42502365311817
-    kappa: -0.074805978628071035
-    s: 3.861474009261026
+    x: 586913.38529709226
+    y: 4141005.7777217585
+    theta: 2.4196994173893152
+    kappa: -0.075878046325233039
+    s: 3.9489103521237157
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5340310159287442
-  a: -0.4491053227769024
+  v: 4.8020191314050109
+  a: 0.14192945825453132
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 586913.38282707392
-    y: 4141005.7796835746
-    theta: 2.4195074028304004
-    kappa: -0.075916721787120181
-    s: 3.9520646687353342
+    x: 586913.31622275
+    y: 4141005.8441179907
+    theta: 2.4123356795413233
+    kappa: -0.076814553232154525
+    s: 4.0449801504507175
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5250279695992779
-  a: -0.45119373810283625
+  v: 4.8050123292031941
+  a: 0.157417664481843
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 586913.31792687892
-    y: 4141005.8422815637
-    theta: 2.4125517451798451
-    kappa: -0.076793943553486649
-    s: 4.0424748519843146
+    x: 586913.25083228562
+    y: 4141005.9145849817
+    theta: 2.4040392891665578
+    kappa: -0.077605383362795918
+    s: 4.1411129211459041
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5159835203874223
-  a: -0.4532440560099964
+  v: 4.8083168984628388
+  a: 0.17306494249229296
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 586913.25655229471
-    y: 4141005.908420905
-    theta: 2.4047654321811835
-    kappa: -0.077536205761700547
-    s: 4.1327037391918067
+    x: 586913.18539472972
+    y: 4141005.9851027196
+    theta: 2.395726822153442
+    kappa: -0.078396783017424532
+    s: 4.2373149227881
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5068985235419046
-  a: -0.45524694646320479
+  v: 4.8119359209051025
+  a: 0.18886132083523169
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 586913.19530157454
-    y: 4141005.9744267664
-    theta: 2.3969858831380471
-    kappa: -0.0782769699614452
-    s: 4.22275052951263
+    x: 586913.11990578449
+    y: 4141006.0556758363
+    theta: 2.3873988433294682
+    kappa: -0.079188804172467181
+    s: 4.3335924735962585
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4977740209121553
-  a: -0.45719307942728304
+  v: 4.8158722788221278
+  a: 0.20479682806000918
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 586913.13417524775
-    y: 4141006.0402985769
-    theta: 2.3892141445361368
-    kappa: -0.079016229751383338
-    s: 4.3126144448045993
+    x: 586913.05436111439
+    y: 4141006.1263090046
+    theta: 2.37905592311145
+    kappa: -0.079981499262164635
+    s: 4.4299519474408884
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.488611240948309
-  a: -0.45907312486705321
+  v: 4.8201286550770446
+  a: 0.22086149271597597
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 586913.07317382575
-    y: 4141006.1060357857
-    theta: 2.3814512552090776
-    kappa: -0.079753978947643675
-    s: 4.40229473336054
+    x: 586912.98875634873
+    y: 4141006.1970069334
+    theta: 2.3706986377211479
+    kappa: -0.080774921145759848
+    s: 4.5263997698554705
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4794115987012031
-  a: -0.46087775274733689
+  v: 4.82470753310397
+  a: 0.2370453433524822
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 586913.01229779935
-    y: 4141006.1716378648
-    theta: 2.373698245218371
-    kappa: -0.080490211614522
-    s: 4.4917906736403017
+    x: 586912.92308708432
+    y: 4141006.2677743686
+    theta: 2.362327569399032
+    kappa: -0.081569123074686167
+    s: 4.6229424140478788
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.47017669582238
-  a: -0.46259763303295592
+  v: 4.82961119690801
+  a: 0.25333840851887834
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 586912.95154763607
-    y: 4141006.2371043093
-    theta: 2.3659561347482545
-    kappa: -0.081224922095182367
-    s: 4.5811015780027722
+    x: 586912.85734888841
+    y: 4141006.3386160862
+    theta: 2.3539433066151716
+    kappa: -0.082364158659755438
+    s: 4.7195863969117964
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4609083205640836
-  a: -0.46422343568873226
+  v: 4.8348417310652536
+  a: 0.26973071676451454
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 586912.89092377829
-    y: 4141006.3024346419
-    theta: 2.3582259330177004
-    kappa: -0.081958105042358315
-    s: 4.6702267964378876
+    x: 586912.79153730092
+    y: 4141006.4095368935
+    theta: 2.3455464442762421
+    kappa: -0.083160081838346292
+    s: 4.8163382750381407
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4516084477792637
-  a: -0.46574583067948755
+  v: 4.8404010207227808
+  a: 0.28621229663874126
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 586912.58971243387
-    y: 4141006.6270302418
-    theta: 2.319787977763228
-    kappa: -0.085600945159855557
-    s: 5.1130479537584392
+    x: 586912.46122084931
+    y: 4141006.765497148
+    theta: 2.3033944364771775
+    kappa: -0.08715491816776183
+    s: 5.3019478515334528
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4047188028597573
-  a: -0.47148013942671269
+  v: 4.8731808216314931
+  a: 0.3696102696659922
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586912.29170588532
-    y: 4141006.9481722442
-    theta: 2.2817910156079875
-    kappa: -0.089205026579769017
-    s: 5.5511576302654762
+    x: 586912.12838985457
+    y: 4141007.1241671592
+    theta: 2.2610207101291504
+    kappa: -0.091180165297240517
+    s: 5.79125414411969
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3574452431239958
-  a: -0.4732297512716846
+  v: 4.9143476104265558
+  a: 0.45374431581055019
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 586911.99702650926
-    y: 4141007.265934105
-    theta: 2.2443358012037926
-    kappa: -0.092765613300023531
-    s: 5.984539301719213
+    x: 586911.83629866049
+    y: 4141007.5199060757
+    theta: 2.2155657469215342
+    kappa: -0.0934287304432054
+    s: 6.28509745456552
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3102445509820706
-  a: -0.46982841181712953
+  v: 4.96391267285314
+  a: 0.53736800374120353
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 586911.76777901046
-    y: 4141007.628176488
-    theta: 2.2032639344870524
-    kappa: -0.093711422925615948
-    s: 6.4132279533928571
+    x: 586911.56933588861
+    y: 4141007.9417437464
+    theta: 2.167554854138364
+    kappa: -0.0945301425014857
+    s: 6.7843129810574743
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.263690134283797
-  a: -0.4601098666657738
+  v: 5.0217626515232929
+  a: 0.61923490212674037
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 586911.54098868137
-    y: 4141007.9865362081
-    theta: 2.1624476970281075
-    kappa: -0.094647094972066872
-    s: 6.8373217426165906
+    x: 586911.299063006
+    y: 4141008.3688118435
+    theta: 2.1188580434927804
+    kappa: -0.095645211128988925
+    s: 7.2897183538866317
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2184720263187181
-  a: -0.44290786142034339
+  v: 5.0876595459159439
+  a: 0.69809857963594868
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586911.31656301185
-    y: 4141008.3411594424
-    theta: 2.1220094179498705
-    kappa: -0.09557301110899355
-    s: 7.25699366132153
+    x: 586911.02505883353
+    y: 4141008.801775889
+    theta: 2.0696310289149666
+    kappa: -0.0967756739920714
+    s: 7.80210117113531
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1753968858161059
-  a: -0.41705614168356459
+  v: 5.1612407123768982
+  a: 0.77271260493761718
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 586911.09436323645
-    y: 4141008.6922654593
-    theta: 2.0820555895485384
-    kappa: -0.096489743843423847
-    s: 7.6725031985837058
+    x: 586910.80430216854
+    y: 4141009.2702397611
+    theta: 2.0202071852161594
+    kappa: -0.095811167973983813
+    s: 8.32220653436375
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1353879969449592
-  a: -0.38138845305816355
+  v: 5.2420188641188421
+  a: 0.84183054670053359
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 586910.89059140114
-    y: 4141009.0484347362
-    theta: 2.0427586001948441
-    kappa: -0.09679465890566033
-    s: 8.08420800316804
+    x: 586910.612681587
+    y: 4141009.7627972518
+    theta: 1.9699278624684544
+    kappa: -0.093627151515037338
+    s: 8.8507245842968
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0994852693140036
-  a: -0.33473854114686646
+  v: 5.32938207122134
+  a: 0.90420597359348642
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586910.74253283232
-    y: 4141009.4290168039
-    theta: 2.004022575523007
-    kappa: -0.095107145233959967
-    s: 8.492575546072306
+    x: 586910.4177851117
+    y: 4141010.2637753733
+    theta: 1.9187199115343556
+    kappa: -0.09140579769874442
+    s: 9.3882780365106218
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0688452379716917
-  a: -0.27594015155239937
+  v: 5.4225937606308339
+  a: 0.95859245428526374
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586910.59547069494
-    y: 4141009.8070375612
-    theta: 1.9654047453415149
-    kappa: -0.093430988497623449
-    s: 8.89819478307112
+    x: 586910.21941593429
+    y: 4141010.7736800164
+    theta: 1.8668028398288197
+    kappa: -0.089144863382855169
+    s: 9.9354097171193523
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.044741063406212
-  a: -0.20382702987748527
+  v: 5.5207927161606474
+  a: 1.0037435574446543
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 586910.44914543862
-    y: 4141010.1831641784
-    theta: 1.9269533601684941
-    kappa: -0.09176323044341804
-    s: 9.3017815925743683
+    x: 586910.101606409
+    y: 4141011.3173534754
+    theta: 1.8190377870718495
+    kappa: -0.084866796108663378
+    s: 10.492571381316464
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0283121676534623
-  a: -0.12481266204642381
+  v: 5.6230457929084983
+  a: 1.0400642958285995
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 586910.303272891
-    y: 4141010.5581271155
-    theta: 1.8887070791526031
-    kappa: -0.09010063218557
-    s: 9.7041197642765233
+    x: 586909.99241508287
+    y: 4141011.8743070792
+    theta: 1.7708295748562615
+    kappa: -0.080255113677248413
+    s: 11.060127600832683
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.01975808377176
-  a: -0.046411710970383518
+  v: 5.72856206975912
+  a: 1.069079867195154
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 586910.17597968737
-    y: 4141010.9379967367
-    theta: 1.8517725421187885
-    kappa: -0.08800794250979542
-    s: 10.105992903095776
+    x: 586909.881168071
+    y: 4141012.4417461455
+    theta: 1.7217388280088266
+    kappa: -0.075556609606389144
+    s: 11.638368892920145
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.01899319453145
-  a: 0.03089031226758987
+  v: 5.8366327223192984
+  a: 1.091224117714775
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586910.09860471578
-    y: 4141011.3326642509
-    theta: 1.8177141944216255
-    kappa: -0.0847400199601535
-    s: 10.508173624671938
+    x: 586909.79977668577
+    y: 4141013.0237690713
+    theta: 1.6767423069994798
+    kappa: -0.070562370964465077
+    s: 12.227517061622526
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0258833315945735
-  a: 0.1066078965844505
+  v: 5.94659231081286
+  a: 1.10693089355792
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 586910.02102497476
-    y: 4141011.7283762353
-    theta: 1.783467801228219
-    kappa: -0.081463448993828874
-    s: 10.911418700255606
+    x: 586909.770908059
+    y: 4141013.6232868917
+    theta: 1.6389866410224232
+    kappa: -0.065124004590950746
+    s: 12.827729536236731
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.040245775514868
-  a: 0.1802555308971526
+  v: 6.057818780080682
+  a: 1.1166340408950459
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 586909.94309885229
-    y: 4141012.1258550119
-    theta: 1.74905045618739
-    kappa: -0.07817224865414521
-    s: 11.316464201597341
+    x: 586909.74150258419
+    y: 4141014.2339534927
+    theta: 1.600496195731868
+    kappa: -0.059584505017669424
+    s: 13.439103709774606
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0618492557377639
-  a: 0.25134770412265017
+  v: 6.1697334595806836
+  a: 1.12076740589661
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 586909.86468965327
-    y: 4141012.5257978234
-    theta: 1.7144836978779567
-    kappa: -0.074860645628613609
-    s: 11.724020645836827
+    x: 586909.717132329
+    y: 4141014.8557739248
+    theta: 1.5621195233275409
+    kappa: -0.053998314261998068
+    s: 14.061681277424643
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0904139506003894
-  a: 0.31939890517789771
+  v: 6.2818010633878334
+  a: 1.1197648347330693
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 586909.8042376627
-    y: 4141012.9311274923
-    theta: 1.6825667879853139
-    kappa: -0.071402744391764777
-    s: 12.134768140392048
+    x: 586909.75592472288
+    y: 4141015.48835689
+    theta: 1.5314884550632797
+    kappa: -0.048937186033398306
+    s: 14.695452575013682
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1256114873315672
-  a: 0.38392362297984917
+  v: 6.3935296901941427
+  a: 1.1140601735748812
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 586909.78429730225
-    y: 4141013.3452310632
-    theta: 1.6565082544068219
-    kappa: -0.0676463140463112
-    s: 12.549351527848453
+    x: 586909.79539880226
+    y: 4141016.1320560179
+    theta: 1.5002899784788126
+    kappa: -0.0437871203404981
+    s: 15.340360917468617
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1670649420518151
-  a: 0.44443634644545871
+  v: 6.5044708233086705
+  a: 1.1040872685925029
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 586909.76414336008
-    y: 4141013.76377011
-    theta: 1.6301307970600905
-    kappa: -0.063849648458723138
-    s: 12.96837553084813
+    x: 586909.8355484847
+    y: 4141016.7867721273
+    theta: 1.4685896075958376
+    kappa: -0.038548910705690484
+    s: 15.9963069372781
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2143488397733462
-  a: 0.50045156449168071
+  v: 6.6142193306575194
+  a: 1.0902799659563915
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 586909.74374891352
-    y: 4141014.1873037326
-    theta: 1.6034351492826537
-    kappa: -0.060007675904255792
-    s: 13.392399896978972
+    x: 586909.92663903139
+    y: 4141017.4473189902
+    theta: 1.4473911727926689
+    kappa: -0.034521964679925382
+    s: 16.663152922954239
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.26698915440007
-  a: 0.55148376603546923
+  v: 6.7224134647838394
+  a: 1.0730721118370043
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 586909.72308943723
-    y: 4141014.6163412589
-    theta: 1.5764278410156038
-    kappa: -0.056115776156100985
-    s: 13.821934543663847
+    x: 586910.02047408756
+    y: 4141018.1183643136
+    theta: 1.4261218165364131
+    kappa: -0.03046326392692484
+    s: 17.340727157494328
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3244633087275908
-  a: 0.59704743999377841
+  v: 6.8287348628478268
+  a: 1.0528975524047985
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 586909.7291141653
-    y: 4141015.051160309
-    theta: 1.5526650055304239
-    kappa: -0.052435079713438769
-    s: 14.257434703049771
+    x: 586910.1166124352
+    y: 4141018.7996936445
+    theta: 1.4046624156331873
+    kappa: -0.026380262683083921
+    s: 18.028828256842509
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.386200174443208
-  a: 0.63665707528356263
+  v: 6.9329085466267211
+  a: 1.0301901338302313
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 586909.75615997845
-    y: 4141015.4921931755
-    theta: 1.5313025478492497
-    kappa: -0.04890649293426081
-    s: 14.699296066897068
+    x: 586910.250256701
+    y: 4141019.4851887347
+    theta: 1.3884668852170976
+    kappa: -0.023889447515353674
+    s: 18.727229508351513
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4515800721259176
-  a: 0.66982716082177574
+  v: 7.03470292251481
+  a: 1.0053837022837597
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 586909.78361543175
-    y: 4141015.9399059932
-    theta: 1.5096025176449963
-    kappa: -0.045324461632643948
-    s: 15.147849931468556
+    x: 586910.38582457777
+    y: 4141020.1805505082
+    theta: 1.3720347355756286
+    kappa: -0.021362780760381832
+    s: 19.43568320924436
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.51993477124641
-  a: 0.69607218552537231
+  v: 7.1339297815234248
+  a: 0.97891210393584094
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586909.81149656454
-    y: 4141016.394560318
-    theta: 1.48757249852972
-    kappa: -0.041686893182014489
-    s: 15.603358342418698
+    x: 586910.52851548919
+    y: 4141020.8843919421
+    theta: 1.3565456450583786
+    kappa: -0.018826520784704193
+    s: 20.153925005076033
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.59054749016707
-  a: 0.71490663831130641
+  v: 7.2304442992809452
+  a: 0.95120918495693241
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 586909.843942541
-    y: 4141016.8559292131
-    theta: 1.4661281830047141
-    kappa: -0.038098882740554445
-    s: 16.066009239682788
+    x: 586910.694831989
+    y: 4141021.59288582
+    theta: 1.3456763373422405
+    kappa: -0.016361484944756342
+    s: 20.881678228195227
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6626528961419806
-  a: 0.72584500809653207
+  v: 7.3241450360327933
+  a: 0.922708791517491
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 586909.90901779383
-    y: 4141017.3213037308
-    theta: 1.4513848082418581
-    kappa: -0.035284146028267786
-    s: 16.535911602366106
+    x: 586910.86325712455
+    y: 4141022.310362305
+    theta: 1.3346687635680623
+    kappa: -0.013865196264253822
+    s: 21.618658236206024
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.735437105316918
-  a: 0.72840178379800347
+  v: 7.4149739366414407
+  a: 0.89384476978797411
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 586909.97510076337
-    y: 4141017.7938847616
-    theta: 1.4364064988743224
-    kappa: -0.032425821976902469
-    s: 17.0130905936331
+    x: 586911.042423559
+    y: 4141023.034381575
+    theta: 1.3248134938500571
+    kappa: -0.011695818601519805
+    s: 22.3645767504296
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8080376827293545
-  a: 0.72209145433267485
+  v: 7.5029163305864017
+  a: 0.86505096593883912
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 586910.04218265449
-    y: 4141018.27360941
-    theta: 1.4212018147964958
-    kappa: -0.029524291001616805
-    s: 17.497482705596539
+    x: 586911.23351774493
+    y: 4141023.7643528464
+    theta: 1.3162977372428959
+    kappa: -0.0099057439298158335
+    s: 23.119146194365939
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8795436423084491
-  a: 0.70642850861750339
+  v: 7.5880009319642356
+  a: 0.83676122614054316
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 586910.11024239659
-    y: 4141018.7603269913
-    theta: 1.4057827140903638
-    kappa: -0.026580464489181384
-    s: 17.988935788041118
+    x: 586911.42673130031
+    y: 4141024.5024200147
+    theta: 1.3076877168233627
+    kappa: -0.0080958160677358591
+    s: 23.882084345518344
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.9491911957098562
-  a: 0.68681967353385753
+  v: 7.6703120170281176
+  a: 0.80975687410364694
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 586910.2043364757
-    y: 4141019.2496523364
-    theta: 1.3940324972261207
-    kappa: -0.024745292641643865
-    s: 18.487258548224581
+    x: 586911.6317439744
+    y: 4141025.2456880677
+    theta: 1.3023689444166757
+    kappa: -0.0066999604197490294
+    s: 24.653121549689388
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.0169668171569652
-  a: 0.66898804270254708
+  v: 7.7500116778890362
+  a: 0.78453365518890916
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 586910.30097467639
-    y: 4141019.7453340003
-    theta: 1.3823193145189259
-    kappa: -0.022944183596428309
-    s: 18.992272660704469
+    x: 586911.84084940108
+    y: 4141025.9959781803
+    theta: 1.29769205142035
+    kappa: -0.0053790781635663643
+    s: 25.43200557925546
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.083047679026313
-  a: 0.65292265514967562
+  v: 7.8272783017914955
+  a: 0.76109704804208422
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 586910.39886257309
-    y: 4141020.24742567
-    theta: 1.3704545416008098
-    kappa: -0.02111978315547711
-    s: 19.503817462471105
+    x: 586912.0537691483
+    y: 4141026.7530992837
+    theta: 1.2935355637342529
+    kappa: -0.00414194031007207
+    s: 26.218502072710628
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.14760985759705
-  a: 0.6386125499013473
+  v: 7.9022908238445746
+  a: 0.7394525313089263
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 586910.49830894452
-    y: 4141020.7557146791
-    theta: 1.3585194880114964
-    kappa: -0.019274222698601629
-    s: 20.021749843337865
+    x: 586912.27373008279
+    y: 4141027.5159115251
+    theta: 1.2909508284259907
+    kappa: -0.0031682334044253347
+    s: 27.012394589453105
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2108283330509373
-  a: 0.62604676598366593
+  v: 7.9752287270219275
+  a: 0.71960558363518967
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 586910.61810526717
-    y: 4141021.2660366339
-    theta: 1.3506908834423184
-    kappa: -0.017498678903979884
-    s: 20.545944136331428
+    x: 586912.49568521883
+    y: 4141028.2856395459
+    theta: 1.2883426595857934
+    kappa: -0.002185698713975484
+    s: 27.813484664571686
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2728769894723415
-  a: 0.61521434242273565
+  v: 8.0462720421617835
+  a: 0.70156168366662852
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 586910.73930789321
-    y: 4141021.7823493187
-    theta: 1.3427695121173651
-    kappa: -0.015702291790911482
-    s: 21.076292008082035
+    x: 586912.72192088212
+    y: 4141029.0614312291
+    theta: 1.2869903866473746
+    kappa: -0.0014002924255896139
+    s: 28.621591863632226
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.3339286148482454
-  a: 0.60610431824466
+  v: 8.1156013479669511
+  a: 0.68532631004899691
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 586910.8618960008
-    y: 4141022.3045640355
-    theta: 1.3347577158239583
-    kappa: -0.013885369963837971
-    s: 21.612702349213762
+    x: 586912.95084582106
+    y: 4141029.8435798027
+    theta: 1.2860482086854763
+    kappa: -0.00067604996441737882
+    s: 29.436553837464089
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.394154901068239
-  a: 0.59870573247554348
+  v: 8.183397771004806
+  a: 0.67090494142804913
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 586910.9893740128
-    y: 4141022.8317346736
-    theta: 1.3271774127455975
-    kappa: -0.012192760192921294
-    s: 22.155101164734774
+    x: 586913.18175271526
+    y: 4141030.6321403543
+    theta: 1.2854852070464247
+    kappa: -7.7288461071046315e-06
+    s: 30.2582263769466
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4537264439245225
-  a: 0.59300762414148955
+  v: 8.249842985707307
+  a: 0.65830305644953946
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 586911.12823827914
-    y: 4141023.36218998
-    theta: 1.3209893780322053
-    kappa: -0.010891949261474717
-    s: 22.703431464427588
+    x: 586913.41473913682
+    y: 4141031.4269529963
+    theta: 1.2858322718219568
+    kappa: 0.00051967335728474152
+    s: 31.086483467795514
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.5128127431119083
-  a: 0.58899903226860229
+  v: 8.315119214370986
+  a: 0.64752613375922219
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 586911.26859453588
-    y: 4141023.8983446239
-    theta: 1.3147345877978496
-    kappa: -0.0095771621128255043
-    s: 23.257653153239321
+    x: 586913.64954746037
+    y: 4141032.2279808968
+    theta: 1.2861820505592698
+    kappa: 0.0010511997289051171
+    s: 31.921217345349461
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.5715822022278187
-  a: 0.58666899588298571
+  v: 8.37940922715695
+  a: 0.63857965200285127
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 586911.41043687949
-    y: 4141024.4401760539
-    theta: 1.3084137936469664
-    kappa: -0.008248454044391668
-    s: 23.817742921671982
+    x: 586913.88460301375
+    y: 4141033.0355906105
+    theta: 1.287392586365292
+    kappa: 0.00144114761054254
+    s: 32.762338549356421
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.6302021287722841
-  a: 0.58600655401074364
+  v: 8.4428963420908811
+  a: 0.631469089826181
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 586911.55941133935
-    y: 4141024.98615171
-    theta: 1.3039867387265243
-    kappa: -0.00715687297198309
-    s: 24.3836941361727
+    x: 586914.12123344664
+    y: 4141033.8493204396
+    theta: 1.2887175537302271
+    kappa: 0.0018161438883593285
+    s: 33.609775978760169
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.6888387341479492
-  a: 0.58700074567798
+  v: 8.5057644250630347
+  a: 0.62619992587496576
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 586911.7129273992
-    y: 4141025.5369818993
-    theta: 1.3005531799788148
-    kappa: -0.0061871389826202794
-    s: 24.955516729524
+    x: 586914.35790928011
+    y: 4141034.6695564319
+    theta: 1.2904068261605097
+    kappa: 0.002124370640198471
+    s: 34.463476946486722
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.7476571336600664
-  a: 0.58964060991079881
+  v: 8.5681978898282463
+  a: 0.62277763879495962
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 586911.86802682118
-    y: 4141026.0934933419
-    theta: 1.2970841996430102
-    kappa: -0.00520740317362776
-    s: 25.533237091234074
+    x: 586914.59477917524
+    y: 4141035.4962200597
+    theta: 1.2924274332358092
+    kappa: 0.0023722712239291244
+    s: 35.323407234230821
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.8068213465164984
-  a: 0.593915185735304
+  v: 8.6303816980059231
+  a: 0.62120770723191687
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 586912.02561806294
-    y: 4141026.6554728914
-    theta: 1.2938663620893791
-    kappa: -0.0042665574668288471
-    s: 26.116897957927012
+    x: 586914.83249187807
+    y: 4141036.3291034689
+    theta: 1.2944734295861688
+    kappa: 0.0025997546732699738
+    s: 36.189551147242376
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.8664942958277191
-  a: 0.59981351217759937
+  v: 8.6925013590800528
+  a: 0.62149560983159158
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 586912.18899312685
-    y: 4141027.2220484922
-    theta: 1.291946564409747
-    kappa: -0.0035433407358621445
-    s: 26.706558303733093
+    x: 586915.06850347412
+    y: 4141037.1689315652
+    theta: 1.2965764493461349
+    kappa: 0.0027417698802813528
+    s: 37.061911569112922
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.9268378086068143
-  a: 0.607324628263789
+  v: 8.7547429303991926
+  a: 0.62364682523973813
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 586912.35405125306
-    y: 4141027.7944608596
-    theta: 1.2900069818904283
-    kappa: -0.0028126735484670711
-    s: 27.302293230679041
+    x: 586915.30620272842
+    y: 4141038.0147650577
+    theta: 1.29869450615262
+    kappa: 0.0028848006016050248
+    s: 37.940510016562072
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.9880126157694766
-  a: 0.6164375730199767
+  v: 8.817293017176473
+  a: 0.62766683210211049
   relative_time: 5.7999999999999963
 }
 trajectory_point {
   path_point {
-    x: 586912.520817688
-    y: 4141028.372797546
-    theta: 1.2880473324554769
-    kappa: -0.0020744441458393241
-    s: 27.904193859078269
+    x: 586915.54108236381
+    y: 4141038.8678983068
+    theta: 1.301346710120957
+    kappa: 0.0029447064301002523
+    s: 38.825386694223994
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.0501783521340124
-  a: 0.6271413854722665
+  v: 8.8803387724896119
+  a: 0.63356110906446317
   relative_time: 5.8999999999999959
 }
 trajectory_point {
   path_point {
-    x: 586912.691239395
-    y: 4141028.9566043736
-    theta: 1.2871166612928093
-    kappa: -0.0014973584590617175
-    s: 28.512367217921149
+    x: 586915.77724070672
+    y: 4141039.7272535153
+    theta: 1.3040642575874186
+    kappa: 0.0029975279441092589
+    s: 39.716600549433856
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.1134935564213366
-  a: 0.63942510464676228
+  v: 8.9440678972808918
+  a: 0.64133513477255011
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 586912.86387340294
-    y: 4141029.5464284606
-    theta: 1.2864061578601791
-    kappa: -0.0009512018044579686
-    s: 29.126936135265293
+    x: 586916.01161755214
+    y: 4141040.5937398188
+    theta: 1.3068315710121188
+    kappa: 0.0029992776659421936
+    s: 40.614229327014264
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.1781156712549743
-  a: 0.65327776956956807
+  v: 9.0086686403571719
+  a: 0.65099438787212571
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 586913.03834284982
-    y: 4141030.1425235369
-    theta: 1.2856881005172052
-    kappa: -0.00039923843070963479
-    s: 29.748039128625773
+    x: 586916.24596965406
+    y: 4141041.4669802035
+    theta: 1.309633944600292
+    kappa: 0.0029755466108774063
+    s: 41.518369624061762
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.2442010431610635
-  a: 0.66868841926678757
+  v: 9.0743297983898916
+  a: 0.66254434700894416
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
-    x: 586913.21483437123
-    y: 4141030.7449955079
-    theta: 1.2855344866430167
-    kappa: 6.715679796028779e-05
-    s: 30.375830295365429
+    x: 586916.47966524283
+    y: 4141042.347250802
+    theta: 1.3124035884131275
+    kappa: 0.00292236653526462
+    s: 42.429136944733251
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.311904922568349
-  a: 0.68564609276452493
+  v: 9.14124071591506
+  a: 0.67599049082875962
   relative_time: 6.2999999999999945
 }
 trajectory_point {
   path_point {
-    x: 586913.393359348
-    y: 4141031.3540177313
-    theta: 1.2858004237349856
-    kappa: 0.00047127676989860089
-    s: 31.01047920308508
+    x: 586916.71225825022
+    y: 4141043.2348090317
+    theta: 1.3151301589824942
+    kappa: 0.0028338039788750307
+    s: 43.346665755032461
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.3813814638081885
-  a: 0.704139829088884
+  v: 9.2095912853332678
+  a: 0.69133829797732627
   relative_time: 6.3999999999999941
 }
 trajectory_point {
   path_point {
-    x: 586913.573865408
-    y: 4141031.9697982459
-    theta: 1.2860693119183955
-    kappa: 0.000879881242241048
-    s: 31.652170780013829
+    x: 586916.94522321876
+    y: 4141044.1294141896
+    theta: 1.3178075405369816
+    kappa: 0.0027300558979335612
+    s: 44.271109537596431
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.452783725114549
-  a: 0.72415866726596867
+  v: 9.2795719469096749
+  a: 0.70859324710039839
   relative_time: 6.4999999999999938
 }
 trajectory_point {
   path_point {
-    x: 586913.755812566
-    y: 4141032.5927032451
-    theta: 1.2866714488870279
-    kappa: 0.0012370490173158938
-    s: 32.3011052053993
+    x: 586917.17639029969
+    y: 4141045.0318067651
+    theta: 1.32032449834813
+    kappa: 0.0025878344267754871
+    s: 45.20264084648192
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.52626366862401
-  a: 0.74569164632188289
+  v: 9.3513736887740215
+  a: 0.72776081684373006
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
-    x: 586913.93909744173
-    y: 4141033.2229872225
-    theta: 1.2876977176576765
-    kappa: 0.001527506779957645
-    s: 32.957497799897929
+    x: 586917.40874945337
+    y: 4141045.9414066132
+    theta: 1.3227731811286132
+    kappa: 0.002439067554570626
+    s: 46.141451361951894
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.6019721603757571
-  a: 0.76872780528273066
+  v: 9.42518804692062
+  a: 0.74884648585307556
   relative_time: 6.6999999999999931
 }
 trajectory_point {
   path_point {
-    x: 586914.12452918757
-    y: 4141033.8606539038
-    theta: 1.2887360075954886
-    kappa: 0.0018213667607102529
-    s: 33.621578915965145
+    x: 586917.639071475
+    y: 4141046.8592500421
+    theta: 1.3246844268242093
+    kappa: 0.0022546959368300937
+    s: 47.087751945262006
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.68005897031159
-  a: 0.793256183174616
+  v: 9.501207105208362
+  a: 0.77185573277418928
   relative_time: 6.7999999999999927
 }
 trajectory_point {
   path_point {
-    x: 586914.31111456093
-    y: 4141034.5062452932
-    theta: 1.2900076468167705
-    kappa: 0.0020753967585034755
-    s: 34.29359382824574
+    x: 586917.87115432275
+    y: 4141047.784610874
+    theta: 1.3266099573181322
+    kappa: 0.0020696473213220735
+    s: 48.04177269344698
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.7606727722759175
-  a: 0.81926581902364271
+  v: 9.5796234953607087
+  a: 0.79679403625282519
   relative_time: 6.8999999999999924
 }
 trajectory_point {
   path_point {
-    x: 586914.49847973266
-    y: 4141035.1601400082
-    theta: 1.2916059553288994
-    kappa: 0.0022714872555130126
-    s: 34.973802623964055
+    x: 586918.10140322288
+    y: 4141048.7186402008
+    theta: 1.3285098729762717
+    kappa: 0.0019094537213826994
+    s: 49.003762994107156
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8439611440157586
-  a: 0.8467457518559145
+  v: 9.6606303969657024
+  a: 0.82366687493473756
   relative_time: 6.999999999999992
 }
 trajectory_point {
   path_point {
-    x: 586914.68817762134
-    y: 4141035.8221757831
-    theta: 1.2932241633958226
-    kappa: 0.0024700191005791261
-    s: 35.662480093314279
+    x: 586918.333623927
+    y: 4141049.660668361
+    theta: 1.3304260592054036
+    kappa: 0.001747888256676017
+    s: 49.973991580194884
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9300705671807439
-  a: 0.87568502069753573
+  v: 9.744421537475958
+  a: 0.85247972746568068
   relative_time: 7.0999999999999917
 }
 trajectory_point {
   path_point {
-    x: 586914.87858290889
-    y: 4141036.4931146586
-    theta: 1.2948841304253664
-    kappa: 0.0026274890193479089
-    s: 36.359915619850668
+    x: 586918.56470381422
+    y: 4141050.6117535327
+    theta: 1.3320496242789
+    kappa: 0.0016044116987371051
+    s: 50.952746584801012
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0191464273231148
-  a: 0.90607266457461
+  v: 9.8311911922086637
+  a: 0.88323807249140862
   relative_time: 7.1999999999999913
 }
 trajectory_point {
   path_point {
-    x: 586915.06972132693
-    y: 4141037.1732651964
-    theta: 1.2965873012354452
-    kappa: 0.0027425026986084827
-    s: 37.066413070877857
+    x: 586918.797737698
+    y: 4141051.5714551867
+    theta: 1.3336750363242655
+    kappa: 0.0014604475561165635
+    s: 51.940335595941349
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.1113330138977187
-  a: 0.93789772251324133
+  v: 9.921134184345588
+  a: 0.91594738865767578
   relative_time: 7.2999999999999909
 }
 trajectory_point {
   path_point {
-    x: 586915.26339748944
-    y: 4141037.8624460916
-    theta: 1.2983130842299166
-    kappa: 0.0028590434143711822
-    s: 37.782290687841076
+    x: 586919.03032583208
+    y: 4141052.5406883843
+    theta: 1.3350387591974586
+    kappa: 0.0013226369759549027
+    s: 52.937085711343066
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.2067735202620167
-  a: 0.97114923353953364
+  v: 10.014445884933075
+  a: 0.95061315461023632
   relative_time: 7.3999999999999906
 }
 trajectory_point {
   path_point {
-    x: 586915.4569480971
-    y: 4141038.5617426187
-    theta: 1.3003785509159826
-    kappa: 0.0029258881267186084
-    s: 38.507880976716464
+    x: 586919.26492695184
+    y: 4141053.5192164821
+    theta: 1.336393665328826
+    kappa: 0.0011841024251542519
+    s: 53.943343593231269
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.3056100436760829
-  a: 1.0058162366795911
+  v: 10.111322212882039
+  a: 0.98724084899484432
   relative_time: 7.49999999999999
 }
 trajectory_point {
   path_point {
-    x: 586915.65188424487
-    y: 4141039.2710946216
-    theta: 1.3026217432083531
-    kappa: 0.0029694894762380157
-    s: 39.243530598401257
+    x: 586919.49966482422
+    y: 4141054.5078629646
+    theta: 1.337498131535791
+    kappa: 0.0010508931691889632
+    s: 54.9594755231153
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.4079835853025964
-  a: 1.0418877709595173
+  v: 10.21195963496797
+  a: 1.0258359504572541
   relative_time: 7.59999999999999
 }
 trajectory_point {
   path_point {
-    x: 586915.849581547
-    y: 4141039.990494174
-    theta: 1.3048967016365944
-    kappa: 0.0030137084129209868
-    s: 39.989600259104122
+    x: 586919.7366130139
+    y: 4141055.5065300553
+    theta: 1.3385942790314285
+    kappa: 0.00091683239519652028
+    s: 55.985867456575349
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.5140340502068508
-  a: 1.0793528754054165
+  v: 10.316555165830941
+  a: 1.0664039376432197
   relative_time: 7.6999999999999895
 }
 trajectory_point {
   path_point {
-    x: 586916.0458927796
-    y: 4141040.721455818
-    theta: 1.3072414327276132
-    kappa: 0.002995806874326744
-    s: 40.746464600735372
+    x: 586919.97422556311
+    y: 4141056.5159995677
+    theta: 1.3394395881582375
+    kappa: 0.00078774305949374677
+    s: 57.0229250780488
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.6239002473567483
-  a: 1.1182005890433924
+  v: 10.42530636797559
+  a: 1.1089502891984957
   relative_time: 7.7999999999999892
 }
 trajectory_point {
   path_point {
-    x: 586916.24496978614
-    y: 4141041.4632545058
-    theta: 1.3096219882380138
-    kappa: 0.0029756478599012007
-    s: 41.514512091297235
+    x: 586920.21425541432
+    y: 4141057.536294342
+    theta: 1.34027641169586
+    kappa: 0.00065778156090595685
+    s: 58.071073855616746
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7377198896228014
-  a: 1.1584199508995487
+  v: 10.53841135177114
+  a: 1.1534804837688359
   relative_time: 7.8999999999999888
 }
 trajectory_point {
   path_point {
-    x: 586916.44544484373
-    y: 4141042.2166682137
-    theta: 1.3120024411282039
-    kappa: 0.0029353963596002951
-    s: 42.294144915274117
+    x: 586920.45562897425
+    y: 4141058.5681235977
+    theta: 1.340942483992182
+    kappa: 0.000532177415514455
+    s: 59.130759095790431
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8556295937781337
-  a: 1.1999999999999902
+  v: 10.656068775451383
+  a: 1.1999999999999946
   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 a2d6c352a2908127b24ec8718da6f30c3a5ec71d..6765c479889d1fc43df00e93627ebd3ed950f2d3 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
@@ -1,7 +1,7 @@
 header {
   module_name: "planning"
 }
-total_path_length: 43.033071327902569
+total_path_length: 45.564812647573824
 total_path_time: 7.9999999999999885
 is_replan: true
 gear: GEAR_DRIVE
@@ -17,1687 +17,1687 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.4499998092651367
-  a: -0.0023541296109925868
+  a: 0.6730933161243674
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 587695.21607340174
-    y: 4141446.883376237
-    theta: -0.21386496072687791
-    kappa: -0.0075139216403306416
-    s: 0.10900005613476842
+    x: 587695.21620596992
+    y: 4141446.8833461455
+    theta: -0.21386633155693027
+    kappa: -0.0075142498973099492
+    s: 0.10913599660044694
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4500317703372181
-  a: 0.0054358874280988258
+  v: 5.4636682314360483
+  a: 0.69360767410524926
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 587695.3223716697
-    y: 4141446.8592482544
-    theta: -0.21496415449466827
-    kappa: -0.00777713050379416
-    s: 0.21800226393401489
+    x: 587695.3229052414
+    y: 4141446.8591271425
+    theta: -0.21496967202529807
+    kappa: -0.0077784516992054744
+    s: 0.21854940818619731
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4502127066636232
-  a: 0.01254518967159125
+  v: 5.4777384993616129
+  a: 0.71327950192612732
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 587695.42867480963
-    y: 4141446.8351191669
-    theta: -0.21606341764892203
-    kappa: -0.0080403514305375443
-    s: 0.32700946747740683
+    x: 587695.42988272
+    y: 4141446.8348449906
+    theta: -0.21607590854887374
+    kappa: -0.0080433423796939553
+    s: 0.32824810381044678
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4505291115794536
-  a: 0.018984540220874778
+  v: 5.4921938590588173
+  a: 0.732118461605628
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 587695.53498533345
-    y: 4141446.8109884029
-    theta: -0.21716277351460334
-    kappa: -0.008303590641096148
-    s: 0.4360242428639341
+    x: 587695.53714575479
+    y: 4141446.8104980225
+    theta: -0.21718511455617673
+    kappa: -0.0083089401358126553
+    s: 0.43823961937913442
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4509676936818394
-  a: 0.02476470217733949
+  v: 5.50701774978502
+  a: 0.75013421516237777
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 587695.64130549645
-    y: 4141446.7868554508
-    theta: -0.21826224275780431
-    kappa: -0.0085668537193052674
-    s: 0.54504890251714921
+    x: 587695.64470137353
+    y: 4141446.7860846426
+    theta: -0.21829736011620751
+    kappa: -0.00857526236947679
+    s: 0.5485311615157501
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4515153768299376
-  a: 0.02989643864237549
+  v: 5.5221938040379523
+  a: 0.7673364246150034
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 587695.74763730064
-    y: 4141446.7627198575
-    theta: -0.21936184342845544
-    kappa: -0.00883014562269605
-    s: 0.65408549949040828
+    x: 587695.75255628687
+    y: 4141446.7616033275
+    theta: -0.21941271197517104
+    kappa: -0.0088423256968119118
+    s: 0.65912961142614235
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4521593001449347
-  a: 0.034390512717372847
+  v: 5.537705847555717
+  a: 0.78373475198213116
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 587695.85398249933
-    y: 4141446.7385812225
-    theta: -0.22046159100313334
-    kappa: -0.0090934706928914137
-    s: 0.76313583177211142
+    x: 587695.86071689171
+    y: 4141446.737052625
+    theta: -0.22053123359329457
+    kappa: -0.0091101459574863125
+    s: 0.77004152876332521
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4528868180100423
-  a: 0.03825768750372166
+  v: 5.5535378993167912
+  a: 0.79933885928238779
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 587695.96034260164
-    y: 4141446.7144392049
-    theta: -0.22156149842796474
-    kappa: -0.0093568326660019522
-    s: 0.8722014465909429
+    x: 587695.969189275
+    y: 4141446.7124311537
+    theta: -0.2216529851817412
+    kappa: -0.0093787382240434185
+    s: 0.88127315549228613
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4536855000705016
-  a: 0.041508726102812019
+  v: 5.569674171540024
+  a: 0.8141584085343998
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 587696.06671887566
-    y: 4141446.690293517
-    theta: -0.2226615761616233
-    kappa: -0.0096202346830218562
-    s: 0.98128364472111285
+    x: 587696.0779792181
+    y: 4141446.6877376018
+    theta: -0.22277802373961655
+    kappa: -0.0096481168112341718
+    s: 0.99283041975479314
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4545431312335815
-  a: 0.044154391616034017
+  v: 5.5860990696846367
+  a: 0.82820306175679359
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 587696.173112354
-    y: 4141446.666143924
-    theta: -0.22376183221841806
-    kappa: -0.0098836793002248235
-    s: 1.0903834847875962
+    x: 587696.18709219969
+    y: 4141446.6629707259
+    theta: -0.22390640309107013
+    kappa: -0.0099182952853494356
+    s: 1.1047189397342023
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.45544771166858
-  a: 0.046205447144777728
+  v: 5.6027971924502227
+  a: 0.84148248096819589
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 587696.27952383726
-    y: 4141446.6419902444
-    theta: -0.22486227221147093
-    kappa: -0.010147168499559968
-    s: 1.1995017875713749
+    x: 587696.2965334
+    y: 4141446.6381293489
+    theta: -0.2250381739224899
+    kappa: -0.010189286473552392
+    s: 1.2169440275202659
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4563874568068194
-  a: 0.047672655790433248
+  v: 5.6197533317767485
+  a: 0.85400632818723321
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587696.38595389784
-    y: 4141446.6178323473
-    theta: -0.22596289939598146
-    kappa: -0.010410703699047735
-    s: 1.3086391403146767
+    x: 587696.40630770475
+    y: 4141446.6132123633
+    theta: -0.22617338381979096
+    kappa: -0.010461102473210925
+    s: 1.3295106929739382
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4573507973416522
-  a: 0.048566780654390677
+  v: 5.6369524728445537
+  a: 0.86578426543253184
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 587696.49240288488
-    y: 4141446.5936701545
-    theta: -0.22706371471257622
-    kappa: -0.010674285763175822
-    s: 1.4177959010262178
+    x: 587696.51641970861
+    y: 4141446.5882187253
+    theta: -0.22731207730579681
+    kappa: -0.010733754661230019
+    s: 1.4424236475921854
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4583263792284589
-  a: 0.048898584838040091
+  v: 5.654379794074349
+  a: 0.87682595472271863
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587696.59887092828
-    y: 4141446.5695036361
-    theta: -0.22816471683074119
-    kappa: -0.010937915013295078
-    s: 1.5269722027864416
+    x: 587696.62687371881
+    y: 4141446.5631474573
+    theta: -0.22845429587771476
+    kappa: -0.011007253703384162
+    s: 1.5556873083727907
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.459303063684648
-  a: 0.048678831442771575
+  v: 5.6720206671272191
+  a: 0.88714105807642007
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587696.70535794261
-    y: 4141446.5453328118
-    theta: -0.22926590219233517
-    kappa: -0.011201591238015425
-    s: 1.6361679580527602
+    x: 587696.73767375946
+    y: 4141446.5379976458
+    theta: -0.22960007804470423
+    kappa: -0.011281609563649719
+    s: 1.6693058016791622
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4602699271896533
-  a: 0.047918283569975226
+  v: 5.68986065690462
+  a: 0.8967392375122627
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 587696.81186363159
-    y: 4141446.5211577485
-    theta: -0.23036726505518124
-    kappa: -0.011465313703601772
-    s: 1.7453828629647938
+    x: 587696.848823575
+    y: 4141446.5127684413
+    theta: -0.23074945936553842
+    kappa: -0.011556831513537353
+    s: 1.7832829671051422
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4612162614849389
-  a: 0.046627704321041151
+  v: 5.7078855215483815
+  a: 0.9056301550488729
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 587696.91838749207
-    y: 4141446.4969785609
-    theta: -0.23146879753673516
-    kappa: -0.011729081164369924
-    s: 1.8546164016496138
+    x: 587696.96032663365
+    y: 4141446.4874590565
+    theta: -0.23190247248635995
+    kappa: -0.011832928141424396
+    s: 1.8976223613398127
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4621315735739966
-  a: 0.044817856797359383
+  v: 5.7260812124407048
+  a: 0.91382347270487729
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 587697.02492881846
-    y: 4141446.4727954087
-    theta: -0.23257048965782789
-    kappa: -0.011992891873082496
-    s: 1.9638678505269798
+    x: 587697.07209365827
+    y: 4141446.4616830088
+    theta: -0.2331135740181525
+    kappa: -0.012113390874392357
+    s: 2.0123272620323025
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4630055857223443
-  a: 0.042499504100320058
+  v: 5.7444338742041632
+  a: 0.92132885249890251
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587697.1310517973
-    y: 4141446.4467942603
-    theta: -0.23392831822076482
-    kappa: -0.012273126776583482
-    s: 2.073136282614584
+    x: 587697.18366450758
+    y: 4141446.4335079282
+    theta: -0.2346553886286937
+    kappa: -0.012415670942851502
+    s: 2.1274006716565976
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4638282354575294
-  a: 0.039683409331313264
+  v: 5.7629298447017048
+  a: 0.92815595644957494
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587697.23700973322
-    y: 4141446.420036613
-    theta: -0.23539259440734692
-    kappa: -0.01256019972014528
-    s: 2.1824205718332883
+    x: 587697.29559529736
+    y: 4141446.4052419509
+    theta: -0.23620223277385946
+    kappa: -0.012718926201685507
+    s: 2.2428453213763446
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4645896755691261
-  a: 0.036380335591729075
+  v: 5.781555655036648
+  a: 0.93431444657552132
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587697.342981763
-    y: 4141446.3932754067
-    theta: -0.23685711199966181
-    kappa: -0.012847310848217256
-    s: 2.2917193973123666
+    x: 587697.4078884162
+    y: 4141446.3768844744
+    theta: -0.2377541325669971
+    kappa: -0.01302316312270123
+    s: 2.3586636749096619
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4652802741087374
-  a: 0.032601045982957558
+  v: 5.8002980295526854
+  a: 0.939813984895368
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 587697.44896642107
-    y: 4141446.366511011
-    theta: -0.23832184448299684
-    kappa: -0.01313445619069788
-    s: 2.4010312476947457
+    x: 587697.52054599742
+    y: 4141446.34843496
+    theta: -0.23931111044952411
+    kappa: -0.013328387485319691
+    s: 2.4748579323939444
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.465890614389993
-  a: 0.028356303606388848
+  v: 5.8191438858338795
+  a: 0.94466423342774175
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587697.55496206181
-    y: 4141446.3397438424
-    theta: -0.23978676284004677
-    kappa: -0.01342163128841662
-    s: 2.5103544254422459
+    x: 587697.63356992241
+    y: 4141446.3198929327
+    theta: -0.24087318523981707
+    kappa: -0.013634604386728326
+    s: 2.5914300342506724
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4664114949885505
-  a: 0.023656871563413
+  v: 5.838080334704669
+  a: 0.94887485419126882
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587697.660966863
-    y: 4141446.3129743603
-    theta: -0.24125183560942276
-    kappa: -0.013708831204443141
-    s: 2.619687051140819
+    x: 587697.74696182425
+    y: 4141446.29125798
+    theta: -0.24244037218233616
+    kappa: -0.013941818252033243
+    s: 2.7083816650502186
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.466833929742096
-  a: 0.018513512955420124
+  v: 5.8570946802298627
+  a: 0.95245550920457589
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587697.7669788301
-    y: 4141446.2862030677
-    theta: -0.24271702894432751
-    kappa: -0.013996050535396501
-    s: 2.7290270678057915
+    x: 587697.8607230921
+    y: 4141446.2625297504
+    theta: -0.2440126829969867
+    kappa: -0.01425003284441146
+    s: 2.8257142573766547
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4671491477503427
-  a: 0.01293699088380031
+  v: 5.8761744197146415
+  a: 0.95541586048628946
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587697.87299580092
-    y: 4141446.2594305128
-    theta: -0.24418230667139068
-    kappa: -0.01428328342275437
-    s: 2.8383722451871063
+    x: 587697.97485487442
+    y: 4141446.2337079551
+    theta: -0.2455901259287171
+    kappa: -0.014559251275263193
+    s: 2.9434289956925626
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.467348593375033
-  a: 0.0069380684499436192
+  v: 5.8953072437045613
+  a: 0.95776557005503626
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587697.97901544918
-    y: 4141446.2326572817
-    theta: -0.24564763034966086
-    kappa: -0.014570523564162217
-    s: 2.9477201840745582
+    x: 587698.08935808251
+    y: 4141446.2047923626
+    theta: -0.247172705797355
+    kappa: -0.014869476014364056
+    s: 3.0615268202038357
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4674239262399356
-  a: 0.00052750875524016846
+  v: 5.9144810359855473
+  a: 0.95951429992944259
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587698.085035289
-    y: 4141446.2058840012
-    theta: -0.24711295932974658
-    kappa: -0.014857764224742514
-    s: 3.0570683206030411
+    x: 587698.20423339517
+    y: 4141446.175782803
+    theta: -0.24876042404768064
+    kappa: -0.015180708900017357
+    s: 3.1800084307244916
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4673670212308458
-  a: -0.0062839250989199531
+  v: 5.9336838735839006
+  a: 0.96067171212813518
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587698.19105267921
-    y: 4141446.17911134
-    theta: -0.24857825081310278
-    kappa: -0.015144998248403943
-    s: 3.1664139305577823
+    x: 587698.31948126154
+    y: 4141446.1466791611
+    theta: -0.25035327879973929
+    kappa: -0.015492951149206326
+    s: 3.2988742905414767
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4671699684955914
-  a: -0.013485470011146687
+  v: 5.9529040267662925
+  a: 0.96124746866974042
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587698.29706482717
-    y: 4141446.1523400028
-    theta: -0.25004345991145616
-    kappa: -0.015432218069150594
-    s: 3.2757541336795866
+    x: 587698.43510190514
+    y: 4141446.1174813826
+    theta: -0.25195126489939135
+    kappa: -0.01580620336774638
+    s: 3.418124630279475
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4668250734440225
-  a: -0.021066362880049948
+  v: 5.9721299590397674
+  a: 0.96125123157288483
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587698.40306879312
-    y: 4141446.1255707312
-    theta: -0.25150853970636372
-    kappa: -0.015719415722391168
-    s: 3.385085897970078
+    x: 587698.55109532783
+    y: 4141446.0881894655
+    theta: -0.25355437396910191
+    kappa: -0.016120465560437357
+    s: 3.5377594517657154
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.46632485674802
-  a: -0.029015840604239611
+  v: 5.9913503271517419
+  a: 0.96069266285619515
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587698.50906149438
-    y: 4141446.0988043053
-    theta: -0.25297344130889932
-    kappa: -0.016006582856248175
-    s: 3.4944060439969351
+    x: 587698.66746131319
+    y: 4141446.0588034648
+    theta: -0.25516259445896833
+    kappa: -0.0164357371412158
+    s: 3.6577785318947775
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.465662054341494
-  a: -0.037323140082325554
+  v: 6.0105539810900073
+  a: 0.95958142453829787
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587698.61503970972
-    y: 4141446.0720415367
-    theta: -0.25443811391946236
-    kappa: -0.016293710742867146
-    s: 3.6037112491991374
+    x: 587698.78419943061
+    y: 4141446.0293234889
+    theta: -0.25677591169798647
+    kappa: -0.016752016943307173
+    s: 3.7781814264934033
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4648296174203779
-  a: -0.0459774982129178
+  v: 6.0297299640827235
+  a: 0.95792717863781918
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587698.72100008291
-    y: 4141446.0452832733
-    theta: -0.25590250488770183
-    kappa: -0.016580790289725818
-    s: 3.7129980521922037
+    x: 587698.90130903886
+    y: 4141445.9997497005
+    theta: -0.25839430794555512
+    kappa: -0.017069303229378139
+    s: 3.898967474185298
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4638207124426366
-  a: -0.054968151894626179
+  v: 6.0488675125984273
+  a: 0.95573958717338614
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587698.82693912752
-    y: 4141446.0185303972
-    theta: -0.2573665597725513
-    kappa: -0.016867812050943353
-    s: 3.8222628570734294
+    x: 587699.01853164681
+    y: 4141445.9691318334
+    theta: -0.26021613470446797
+    kappa: -0.017397792328827776
+    s: 4.020135800255944
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4626287211282607
-  a: -0.064284338026060567
+  v: 6.0679560563460235
+  a: 0.95302831216362494
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587698.93285323086
-    y: 4141445.9917838192
-    theta: -0.25883022240236736
-    kappa: -0.017154766238589535
-    s: 3.9315019377271341
+    x: 587699.135278312
+    y: 4141445.9353013434
+    theta: -0.26269420662913984
+    kappa: -0.0177607526876207
+    s: 4.1416853205174045
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4612472404592713
-  a: -0.073915293505831023
+  v: 6.0869852182747941
+  a: 0.94980301562716241
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 587699.03829427168
-    y: 4141445.9634050806
-    theta: -0.26063560116848772
-    kappa: -0.017459233479240378
-    s: 4.0407114421298944
+    x: 587699.25238987
+    y: 4141445.9013651153
+    theta: -0.26518023706239408
+    kappa: -0.018124847483102093
+    s: 4.2636147451731325
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4596700826797147
-  a: -0.083850255232547313
+  v: 6.1059448145743893
+  a: 0.94607335958262473
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 587699.14315629483
-    y: 4141445.9330184855
-    theta: -0.26286143316655813
-    kappa: -0.017785244997352827
-    s: 4.1498873966557905
+    x: 587699.36986488861
+    y: 4141445.8673235653
+    theta: -0.26767416673764044
+    kappa: -0.018490072261366914
+    s: 4.3859225826827775
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4578912752956663
-  a: -0.094078460104819392
+  v: 6.1248248546748361
+  a: 0.94184900604863875
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 587699.24798216438
-    y: 4141445.9026423665
-    theta: -0.26508666729799657
-    kappa: -0.01811114411585844
-    s: 4.2590257103816427
+    x: 587699.48770174477
+    y: 4141445.8331771623
+    theta: -0.270175931762079
+    kappa: -0.018856421977621479
+    s: 4.5086071436269926
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.455905061075228
-  a: -0.10458914502125724
+  v: 6.14361554124653
+  a: 0.93713961704383086
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 587699.35276784271
-    y: 4141445.872277895
-    theta: -0.26731119565627631
-    kappa: -0.018436918281399827
-    s: 4.3681221793922553
+    x: 587699.60589862976
+    y: 4141445.7989264322
+    theta: -0.27268546369585667
+    kappa: -0.019223891007724203
+    s: 4.6316665445722425
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.45370589804853
-  a: -0.1153715468804706
+  v: 6.162307270200241
+  a: 0.9319548545868277
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 587699.45750918752
-    y: 4141445.8419262692
-    theta: -0.26953490813775105
-    kappa: -0.018762554616068818
-    s: 4.4771724910856543
+    x: 587699.72445355181
+    y: 4141445.7645719508
+    theta: -0.27520268963216127
+    kappa: -0.019592473159726346
+    s: 4.7550987119356138
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4512884595077331
-  a: -0.12641490258106958
+  v: 6.1808906306871121
+  a: 0.92630438069625565
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 587699.56220195652
-    y: 4141445.8115887204
-    theta: -0.27175769253877513
-    kappa: -0.019088039930262393
-    s: 4.5861722284783291
+    x: 587699.84336434049
+    y: 4141445.7301143473
+    theta: -0.2777275322782472
+    kappa: -0.01996216168541276
+    s: 4.8789013858496171
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4486476340070213
-  a: -0.13770844902166396
+  v: 6.1993564050986585
+  a: 0.92019785739074134
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 587699.6668418108
-    y: 4141445.781266504
-    theta: -0.27397943465314784
-    kappa: -0.0194133607355386
-    s: 4.6951168745104743
+    x: 587699.96262864978
+    y: 4141445.6955543021
+    theta: -0.28025991003739148
+    kappa: -0.020332949291842609
+    s: 5.0030721240269953
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4457785253626092
-  a: -0.14924142310086361
+  v: 6.217695569066767
+  a: 0.91364494668891139
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 587699.77142432018
-    y: 4141445.7509609056
-    theta: -0.2762000183698598
-    kappa: -0.0197385032574725
-    s: 4.8040018163512279
+    x: 587700.08224396245
+    y: 4141445.6608925438
+    theta: -0.28279973709177347
+    kappa: -0.020704828152890153
+    s: 5.1276083056255377
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.442676452652738
-  a: -0.16100306171727866
+  v: 6.2358992914636966
+  a: 0.9066553106093922
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 587699.875944966
-    y: 4141445.7206732342
-    theta: -0.2784193257711155
-    kappa: -0.020063453448512056
-    s: 4.9128223497039132
+    x: 587700.20220759371
+    y: 4141445.6261298507
+    theta: -0.28534692348627194
+    kappa: -0.021077789920785465
+    s: 5.2525071351128778
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4393369502176787
-  a: -0.17298260176951888
+  v: 6.2539589344020818
+  a: 0.89923861117081039
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 587699.98039914621
-    y: 4141445.6904048221
-    theta: -0.28063723723061018
-    kappa: -0.0203881970008341
-    s: 5.0215736831112805
+    x: 587700.322516694
+    y: 4141445.5912670493
+    theta: -0.28790137521317344
+    kappa: -0.021451825737655174
+    s: 5.3777656461313086
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4357557676597281
-  a: -0.18516928015619416
+  v: 6.2718660532349269
+  a: 0.89140451039179247
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 587700.08478217921
-    y: 4141445.6601570267
-    theta: -0.28285363151203635
-    kappa: -0.020712719359200223
-    s: 5.1302509422607425
+    x: 587700.4431682541
+    y: 4141445.5563050103
+    theta: -0.29046299429778216
+    kappa: -0.021826926247063231
+    s: 5.5033807053625852
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4319288698432109
-  a: -0.19755233377591444
+  v: 6.2896123965556079
+  a: 0.883162670290965
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 587700.18908930756
-    y: 4141445.6299312278
-    theta: -0.28506838586779476
-    kappa: -0.021037005733812744
-    s: 5.2388491742896246
+    x: 587700.56415910786
+    y: 4141445.5212446526
+    theta: -0.29303167888492315
+    kappa: -0.022203081605551619
+    s: 5.6293490163927338
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.42785243689448
-  a: -0.21012099952728949
+  v: 6.3071899061978769
+  a: 0.87452275288695458
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 587700.293315703
-    y: 4141445.5997288227
-    theta: -0.2872813761378869
-    kappa: -0.021361041113170592
-    s: 5.3473633520903938
+    x: 587700.68548593589
+    y: 4141445.4860869367
+    theta: -0.29560732332632789
+    kappa: -0.022580281494181138
+    s: 5.7556671235768624
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4235228642019173
-  a: -0.22286451430892945
+  v: 6.3245907172358562
+  a: 0.86549442019838774
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 587700.39745647
-    y: 4141445.5695512309
-    theta: -0.28949247684896307
-    kappa: -0.021684810276925249
-    s: 5.4557883786159085
+    x: 587700.80714527017
+    y: 4141445.450832868
+    theta: -0.2981898182688909
+    kappa: -0.022958515130072108
+    s: 5.8823314159039608
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4189367624159308
-  a: -0.2357721150194442
+  v: 6.34180715798404
+  a: 0.856087334243891
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587700.91656411858
-    y: 4141445.4186541415
-    theta: -0.30057321733020492
-    kappa: -0.023304400954143271
-    s: 5.9963934921521371
+    x: 587701.41281953489
+    y: 4141445.2494288823
+    theta: -0.31439484772310028
+    kappa: -0.025154850261540049
+    s: 6.5207088806364979
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.392054758190886
-  a: -0.30239469895648557
+  v: 6.4248688252114405
+  a: 0.80370877613438685
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587701.42537340149
-    y: 4141445.2451479593
-    theta: -0.31474457894512264
-    kappa: -0.025201661425692586
-    s: 6.5339725856296234
+    x: 587702.0246333658
+    y: 4141445.0407977
+    theta: -0.33144085900278553
+    kappa: -0.027436196605327874
+    s: 7.1671167501896669
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.3583748998575214
-  a: -0.37150496590688187
+  v: 6.502281595582442
+  a: 0.74331068917061582
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587701.93066555343
-    y: 4141445.072841133
-    theta: -0.32882271549048014
-    kappa: -0.027085807133330832
-    s: 7.0678356775352587
+    x: 587702.64347713056
+    y: 4141444.82976928
+    theta: -0.34867984517137418
+    kappa: -0.029743756338541476
+    s: 7.8209520501541041
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.31771568849819
-  a: -0.44175752819687225
+  v: 6.5733039038280321
+  a: 0.67610082568089569
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587702.43177670718
-    y: 4141444.9019600465
-    theta: -0.34278351579603622
-    kappa: -0.02895436263300703
-    s: 7.5972813634025353
+    x: 587703.25805919222
+    y: 4141444.5881619295
+    theta: -0.36933476675725763
+    kappa: -0.032450323230789127
+    s: 8.4815436883551847
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.27003016396262
-  a: -0.51180699815269515
+  v: 6.6373149599120334
+  a: 0.60328693799354394
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587702.92506344034
-    y: 4141444.724785116
-    theta: -0.3575102956300823
-    kappa: -0.0309098229984456
-    s: 8.1216102696882917
+    x: 587703.87478966988
+    y: 4141444.33512662
+    theta: -0.39124409327465881
+    kappa: -0.035303433033438428
+    s: 9.1481645323763114
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2154059048679153
-  a: -0.58030798810058992
+  v: 6.6938147490310973
+  a: 0.52607677843687806
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587703.40478466521
-    y: 4141444.5279626581
-    theta: -0.37454654574008311
-    kappa: -0.033129102536351179
-    s: 8.6401385076494446
+    x: 587704.49638473755
+    y: 4141444.0800954425
+    theta: -0.41332079822974477
+    kappa: -0.038179047333618252
+    s: 9.8200434870821862
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.1540650285985574
-  a: -0.64591511036679528
+  v: 6.7424240316147088
+  a: 0.44567809933921576
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 587703.8785334148
-    y: 4141444.3335906174
-    theta: -0.3913770947056408
-    kappa: -0.03532075229298251
-    s: 9.1522111272197257
+    x: 587705.10530718579
+    y: 4141443.7865899871
+    theta: -0.44191732004395007
+    kappa: -0.041543116277835371
+    s: 10.496377572142112
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.0863641913064042
-  a: -0.70728297727755018
+  v: 6.7828843433251844
+  a: 0.36329865302887504
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587704.34574297548
-    y: 4141444.1419015089
-    theta: -0.407972226203128
-    kappa: -0.03748215054737198
-    s: 9.6572155708864216
+    x: 587705.71238007012
+    y: 4141443.4802946141
+    theta: -0.47264643111478644
+    kappa: -0.045068348160186913
+    s: 11.176343999553255
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.0127945879106877
-  a: -0.76306620115909363
+  v: 6.8150579950576722
+  a: 0.28014619183417305
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587704.8001643935
-    y: 4141443.9405481494
-    theta: -0.42648325049477381
-    kappa: -0.039771172367264455
-    s: 10.154595127567113
+    x: 587706.32195441832
+    y: 4141443.1727371407
+    theta: -0.503484837019203
+    kappa: -0.048608105877342683
+    s: 11.859112251163946
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.9339819520980175
-  a: -0.81191939433766425
+  v: 6.8389280729401509
+  a: 0.197428468083428
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587705.23698138038
-    y: 4141443.7201544773
-    theta: -0.44858099593296319
-    kappa: -0.042307739572059938
-    s: 10.643862386486409
+    x: 587706.90555018571
+    y: 4141442.8154439861
+    theta: -0.53982649148416562
+    kappa: -0.052606763143689166
+    s: 12.543856156196945
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8506865563223824
-  a: -0.85249716913949469
+  v: 6.8545984383334329
+  a: 0.11635323410495781
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587705.66619763139
-    y: 4141443.5035957163
-    theta: -0.47030861718307954
-    kappa: -0.044800169808360081
-    s: 11.124616260248043
+    x: 587707.48457504541
+    y: 4141442.4477464133
+    theta: -0.57737778406047369
+    kappa: -0.056702206712458236
+    s: 13.22976525606634
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.763943777704359
-  a: -0.8793472566711853
+  v: 6.8622645174418366
+  a: 0.03721682389183463
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587706.087576044
-    y: 4141443.2909914949
-    theta: -0.49163340308205566
-    kappa: -0.0472470862389565
-    s: 11.596591176796343
+    x: 587708.06391525606
+    y: 4141442.0798485829
+    theta: -0.61490968727525375
+    kappa: -0.0607998807594232
+    s: 13.916047918717378
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6753977191231959
-  a: -0.8887789201513524
+  v: 6.86209758482082
+  a: -0.040246135050707177
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587706.4968324214
-    y: 4141443.0749916164
-    theta: -0.513353538480295
-    kappa: -0.049715901876376041
-    s: 12.05969173873776
+    x: 587708.59768051957
+    y: 4141441.6497087772
+    theta: -0.65829002696728789
+    kappa: -0.064907706731726966
+    s: 14.601929818887799
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.58672537745933
-  a: -0.882089070074288
+  v: 6.8542832452272666
+  a: -0.11567045012610921
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587706.88033276587
-    y: 4141442.8314577788
-    theta: -0.53819180881825612
-    kappa: -0.052428400300434191
-    s: 12.51398376381432
+    x: 587709.126686824
+    y: 4141441.2149632573
+    theta: -0.70218622617075566
+    kappa: -0.069009687598442157
+    s: 15.286657017754012
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4994740585437691
-  a: -0.86057461693428372
+  v: 6.8390436226777211
+  a: -0.18869092873781321
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 587707.25657790178
-    y: 4141442.5925312093
-    theta: -0.56259044995075858
-    kappa: -0.055089582625086266
-    s: 12.959681315798676
+    x: 587709.65401527076
+    y: 4141440.7811812274
+    theta: -0.745918539604771
+    kappa: -0.073096676846581915
+    s: 15.969499614857062
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4150613771580947
-  a: -0.82553247122563111
+  v: 6.81663736044838
+  a: -0.25894237828926081
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587707.62586273253
-    y: 4141442.3580246354
-    theta: -0.58653866708952
-    kappa: -0.057701534702694658
-    s: 13.397133735389172
+    x: 587710.111314299
+    y: 4141440.2775687459
+    theta: -0.80025637881906686
+    kappa: -0.076033084460147893
+    s: 16.649755400028603
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3347752570344547
-  a: -0.778259543442622
+  v: 6.7873596210750957
+  a: -0.32605960618389362
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 587707.98858541029
-    y: 4141442.127685219
-    theta: -0.61003550702674858
-    kappa: -0.060267072664281059
-    s: 13.826812671104893
+    x: 587710.56642336608
+    y: 4141439.7763680159
+    theta: -0.85448331324906179
+    kappa: -0.078955429898543289
+    s: 17.326753505316844
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2597739308555722
-  a: -0.72005274407954822
+  v: 6.7515420863533793
+  a: -0.38967741982515347
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 587708.32524520764
-    y: 4141441.8736003079
-    theta: -0.63570837794865165
-    kappa: -0.0627952093422151
-    s: 14.249299110180722
+    x: 587711.01440173225
+    y: 4141439.2745744442
+    theta: -0.90839505734640946
+    kappa: -0.081734391469789627
+    s: 17.999858056912547
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1910859402547374
-  a: -0.65220898363070123
+  v: 6.7095529573383956
+  a: -0.44943062661648192
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 587708.64661617135
-    y: 4141441.6094927057
-    theta: -0.6623494213147737
-    kappa: -0.065287159878985743
-    s: 14.665270409462405
+    x: 587711.37810388254
+    y: 4141438.713535124
+    theta: -0.96494018682078353
+    kappa: -0.082215070230215515
+    s: 18.668471827074956
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1296101358158142
-  a: -0.57602517259037289
+  v: 6.6617969543449664
+  a: -0.50495403396132066
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 587708.96354141622
-    y: 4141441.3490386633
-    theta: -0.68864915969253437
-    kappa: -0.067744637757567921
-    s: 15.075487326301596
+    x: 587711.73906134511
+    y: 4141438.1567297
+    theta: -1.0212179907863135
+    kappa: -0.082692121535893226
+    s: 19.332039886057789
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0761156770732363
-  a: -0.49279822145285485
+  v: 6.608715316947567
+  a: -0.55588244926311126
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587709.27666310174
-    y: 4141441.091710445
-    theta: -0.7146241073830325
-    kappa: -0.070172622362986187
-    s: 15.480781049450929
+    x: 587712.0931606479
+    y: 4141437.6025056117
+    theta: -1.0767886659779928
+    kappa: -0.083062274883672529
+    s: 19.990053254035189
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0312420325120062
-  a: -0.40382504071243863
+  v: 6.5507858039803306
+  a: -0.60185067992529562
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587709.58666778181
-    y: 4141440.8369438294
-    theta: -0.74029703383603884
-    kappa: -0.072576437318436465
-    s: 15.882040229959063
+    x: 587712.35800297919
+    y: 4141437.0067190519
+    theta: -1.1308727873324809
+    kappa: -0.08112181936209123
+    s: 20.642052553027696
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9954989795677016
-  a: -0.31040254086341623
+  v: 6.4885226935370461
+  a: -0.64249353335131543
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587709.8628809331
-    y: 4141440.5511624697
-    theta: -0.77069758826631818
-    kappa: -0.074437844623429616
-    s: 16.280198012065739
+    x: 587712.62023742578
+    y: 4141436.4167991616
+    theta: -1.1845305974986664
+    kappa: -0.079200471375361944
+    s: 21.287631658828225
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9692666046264669
-  a: -0.21382763240007918
+  v: 6.4224767829711578
+  a: -0.677445816944612
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587710.12910438352
-    y: 4141440.2579769515
-    theta: -0.80237537111912172
-    kappa: -0.07614731811511527
-    s: 16.676219064096848
+    x: 587712.85680433817
+    y: 4141435.8248339291
+    theta: -1.2352125744732076
+    kappa: -0.076940038892363963
+    s: 21.926441352927995
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.95279530302502
-  a: -0.11539722581671952
+  v: 6.353235388895766
+  a: -0.70634233810862734
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 587710.394552394
-    y: 4141439.9656454069
-    theta: -0.83400738846879685
-    kappa: -0.077851812353215588
-    s: 17.07108660935948
+    x: 587713.02612132579
+    y: 4141435.21619457
+    theta: -1.2790680497980689
+    kappa: -0.073691424238232522
+    s: 22.558192974442534
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9462057790506475
-  a: -0.016408231607628165
+  v: 6.2814223471836268
+  a: -0.72881790424680293
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587710.6598896872
-    y: 4141439.6734357923
-    theta: -0.865609394526768
-    kappa: -0.079555595653969288
-    s: 17.465789457036976
+    x: 587713.19348650961
+    y: 4141434.6145713087
+    theta: -1.3224716083047532
+    kappa: -0.070480258026048986
+    s: 23.182662072037619
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9494890459412089
-  a: 0.081842439732902283
+  v: 6.2076980129671524
+  a: -0.74450732276258058
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587710.92577602249
-    y: 4141439.3806215306
-    theta: -0.89719674672931837
-    kappa: -0.081262904463002036
-    s: 17.861309033083998
+    x: 587713.35347767489
+    y: 4141434.019001429
+    theta: -1.3649135630098936
+    kappa: -0.067259866435921259
+    s: 23.799692055855264
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9625064258851328
-  a: 0.17805787771058079
+  v: 6.13275926063841
+  a: -0.75304540105940176
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587711.15515164938
-    y: 4141439.0574565693
-    theta: -0.93023550302002411
-    kappa: -0.081920410429783846
-    s: 18.258606411121583
+    x: 587713.43663285754
+    y: 4141433.4151947312
+    theta: -1.4021246945333754
+    kappa: -0.063417907523584907
+    s: 24.409197849439664
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.98498955002142
-  a: 0.270941171831115
+  v: 6.0573394838491224
+  a: -0.75406694654070838
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587711.37273904157
-    y: 4141438.7218108149
-    theta: -0.96410405297358515
-    kappa: -0.082207979908909309
-    s: 18.658609343332181
+    x: 587713.518760159
+    y: 4141432.8188516884
+    theta: -1.4389343527164198
+    kappa: -0.059623439066917111
+    s: 25.011169541663165
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0165403584396415
-  a: 0.35919541160021407
+  v: 5.98220859551067
+  a: -0.74720676660994212
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 587711.59227764281
-    y: 4141438.3831551662
-    theta: -0.99833629191829121
-    kappa: -0.082498128159802675
-    s: 19.062199291354744
+    x: 587713.59986898012
+    y: 4141432.2299040388
+    theta: -1.47524684077725
+    kappa: -0.055876026723029805
+    s: 25.605676038652231
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.05663110017994
-  a: 0.44152368652358737
+  v: 5.908173027794084
+  a: -0.73209966867054854
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587711.81421519048
-    y: 4141438.0407989528
-    theta: -1.0329223579648146
-    kappa: -0.082791446924022113
-    s: 19.470199354911497
+    x: 587713.63227854588
+    y: 4141431.6442918191
+    theta: -1.5067177455450169
+    kappa: -0.052307855282115977
+    s: 26.192865152496022
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1046414710498285
-  a: 0.51780464797467807
+  v: 5.83593289677014
+  a: -0.71268085409211768
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587712.03896706074
-    y: 4141437.694101424
-    theta: -1.0678421856885163
-    kappa: -0.083088485174455134
-    s: 19.883373143427765
+    x: 587713.64785840269
+    y: 4141431.0644386718
+    theta: -1.5363569951327773
+    kappa: -0.048828858718529322
+    s: 26.772927567119371
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1600191689655466
-  a: 0.58889978514591546
+  v: 5.7656412297948023
+  a: -0.69313085493943216
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 587712.22004889883
-    y: 4141437.3170591188
-    theta: -1.1026674720995329
-    kappa: -0.082132585864035018
-    s: 20.302431756063786
+    x: 587713.66325209488
+    y: 4141430.491514225
+    theta: -1.5656408933376167
+    kappa: -0.045391432899432704
+    s: 27.3460587801776
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2222544788108216
-  a: 0.65498644427148156
+  v: 5.6973110051531881
+  a: -0.67345247465870817
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587712.392929568
-    y: 4141436.9281485598
-    theta: -1.1380200622283843
-    kappa: -0.080865918080048532
-    s: 20.728036207199349
+    x: 587713.66505710746
+    y: 4141429.9255390144
+    theta: -1.5923279154106078
+    kappa: -0.042137434427863663
+    s: 27.912455573137304
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.290855420092794
-  a: 0.71624197158555858
+  v: 5.630954920785789
+  a: -0.65364851669616164
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587712.568718039
-    y: 4141436.5326966392
-    theta: -1.1739951933173105
-    kappa: -0.079577945316869014
-    s: 21.160799199896115
+    x: 587713.63189636276
+    y: 4141429.3666615286
+    theta: -1.6129531821276972
+    kappa: -0.039293786805870964
+    s: 28.472315983241913
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3653477469420228
-  a: 0.77284371332232848
+  v: 5.5665853942884764
+  a: -0.63372178449800887
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587712.74764428847
-    y: 4141436.130186005
-    theta: -1.2105458852798359
-    kappa: -0.078266982575138935
-    s: 21.601286899359984
+    x: 587713.59911096783
+    y: 4141428.8141100351
+    theta: -1.6333538168826669
+    kappa: -0.0364823267253335
+    s: 29.0258392754772
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.4452749481124876
-  a: 0.82496901571597314
+  v: 5.5042145629125008
+  a: -0.61367508151046579
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587712.88992508245
-    y: 4141435.7057756539
-    theta: -1.2437790890181475
-    kappa: -0.07630456494188019
-    s: 22.050020706403426
+    x: 587713.56668904936
+    y: 4141428.267684421
+    theta: -1.6535202183747424
+    kappa: -0.0337020359843224
+    s: 29.573225914536838
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5301982469815849
-  a: 0.87279522500067463
+  v: 5.44385428356449
+  a: -0.59351121117974825
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 587713.01252938248
-    y: 4141435.265053174
-    theta: -1.2755433087677677
-    kappa: -0.073952207201861048
-    s: 22.50747903090781
+    x: 587713.51110097475
+    y: 4141427.729358274
+    theta: -1.6707633490077829
+    kappa: -0.031306172254290852
+    s: 30.114677536787923
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6196966015501308
-  a: 0.916499687410615
+  v: 5.38551613280645
+  a: -0.57323297695207265
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 587713.13758913032
-    y: 4141434.81550416
-    theta: -1.3079797235220694
-    kappa: -0.071552737806028777
-    s: 22.974099065285767
+    x: 587713.44422179437
+    y: 4141427.1978298789
+    theta: -1.6864717074856415
+    kappa: -0.029114633146926062
+    s: 30.650396922236528
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7133667044423548
-  a: 0.95625974917997625
+  v: 5.3292114068557668
+  a: -0.55284318227365492
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587713.26521092735
-    y: 4141434.3567454149
-    theta: -1.3410478799327472
-    kappa: -0.069104111439013974
-    s: 23.450278557943516
+    x: 587713.37803277187
+    y: 4141426.671786576
+    theta: -1.7020194963309674
+    kappa: -0.026945709565991696
+    s: 31.180587966493235
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8108229829059113
-  a: 0.99225275654294043
+  v: 5.274951121585203
+  a: -0.5323446305907108
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 587713.37212575076
-    y: 4141433.883593956
-    theta: -1.3732479465775671
-    kappa: -0.066398282839653627
-    s: 23.93637758674322
+    x: 587713.31250831718
+    y: 4141426.1510249856
+    theta: -1.7174052775595163
+    kappa: -0.024798562959311193
+    s: 31.705455652738667
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.9116975988118687
-  a: 1.0246560557336897
+  v: 5.2227460125229026
+  a: -0.5117401253494569
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 587713.43984204181
-    y: 4141433.3918921915
-    theta: -1.4035626218389587
-    kappa: -0.063269635900489715
-    s: 24.432720332465315
+    x: 587713.22468000581
+    y: 4141425.6388055957
+    theta: -1.7294783960072933
+    kappa: -0.023070859423213737
+    s: 32.22520602368904
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.0156404486547137
-  a: 1.0536469929864056
+  v: 5.172606534852382
+  a: -0.49103246999610872
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 587713.50899546093
-    y: 4141432.8897551475
-    theta: -1.434558038418098
-    kappa: -0.060074590368160014
-    s: 24.939596852270849
+    x: 587713.13523596863
+    y: 4141425.1317946259
+    theta: -1.7411039896634328
+    kappa: -0.021401955176096242
+    s: 32.740046153561657
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.1223191635523539
-  a: 1.0794029145352706
+  v: 5.1245428634125432
+  a: -0.47022446797688272
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 587713.57962116727
-    y: 4141432.3769275267
-    theta: -1.4661909340677803
-    kappa: -0.056811521812295984
-    s: 25.457264853163849
+    x: 587713.04660884629
+    y: 4141424.6294143135
+    theta: -1.7526235085969555
+    kappa: -0.019748293447972756
+    s: 33.250184120040529
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2314191092461124
-  a: 1.1021011666144664
+  v: 5.0785648926976608
+  a: -0.44931892273799473
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 587713.62672106526
-    y: 4141431.851130859
-    theta: -1.4961546977128422
-    kappa: -0.05354884588594086
-    s: 25.985951465453624
+    x: 587712.95876115
+    y: 4141424.1314590136
+    theta: -1.7640384031837355
+    kappa: -0.018109218668747408
+    s: 33.755828976241837
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.3426433861007308
-  a: 1.1219190954581755
+  v: 5.0346822368573925
+  a: -0.42831863772566087
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 587713.64122229791
-    y: 4141431.3114220873
-    theta: -1.5237295607473111
-    kappa: -0.050310707109109051
-    s: 26.525855016217154
+    x: 587712.8537291626
+    y: 4141423.6412224565
+    theta: -1.771509598729937
+    kappa: -0.016822772730881539
+    s: 34.257190722679489
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4557128291043693
-  a: 1.1390340473005793
+  v: 4.99290422969677
+  a: -0.40722641638609719
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 587713.65602940647
-    y: 4141430.7603291888
-    theta: -1.5519043135860591
-    kappa: -0.047004265970070111
-    s: 27.07714680276138
+    x: 587712.74955027224
+    y: 4141423.1549677267
+    theta: -1.7789209330397959
+    kappa: -0.015546775639930847
+    s: 34.754480279230684
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.5703660078686079
-  a: 1.1536233683758603
+  v: 4.9532399246762076
+  a: -0.3860450621655197
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 587713.67114631319
-    y: 4141430.1977061736
-    theta: -1.58064146817164
-    kappa: -0.043628646569745147
-    s: 27.639972866085586
+    x: 587712.646180106
+    y: 4141422.6724877143
+    theta: -1.7862747385017781
+    kappa: -0.014280683908747614
+    s: 35.247909457101414
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.6863592266284426
-  a: 1.1658644049182003
+  v: 4.9156980949114937
+  a: -0.36477737851014458
   relative_time: 5.7999999999999963
 }
 trajectory_point {
   path_point {
-    x: 587713.64716952143
-    y: 4141429.6240690327
-    theta: -1.603451438763452
-    kappa: -0.040603512560794995
-    s: 28.214455764343729
+    x: 587712.54357410956
+    y: 4141422.1935744639
+    theta: -1.7935733980128663
+    kappa: -0.013023951835053971
+    s: 35.737690930792034
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.8034665242422889
-  a: 1.1759345031617809
+  v: 4.8802872331737985
+  a: -0.34342616886618771
   relative_time: 5.8999999999999959
 }
 trajectory_point {
   path_point {
-    x: 587713.61244626914
-    y: 4141429.03885769
-    theta: -1.6250559934245068
-    kappa: -0.037625874551878977
-    s: 28.800696346306761
+    x: 587712.42976636533
+    y: 4141421.7207365213
+    theta: -1.7989789983296025
+    kappa: -0.012023233666020268
+    s: 36.224038210062787
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.9214796741919784
-  a: 1.184011009340785
+  v: 4.847015551889668
+  a: -0.32199423667986526
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 587713.57702181221
-    y: 4141428.4418285349
-    theta: -1.6470951248938759
-    kappa: -0.034588105845499088
-    s: 29.39877552482502
+    x: 587712.31625632918
+    y: 4141421.2511329027
+    theta: -1.804278738888611
+    kappa: -0.011038591582795848
+    s: 36.707165611899306
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.0402081845827649
-  a: 1.1902712696893942
+  v: 4.81589098314103
+  a: -0.30048438539739319
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 587713.52432420768
-    y: 4141427.8344511255
-    theta: -1.6676583736688066
-    kappa: -0.031739479478697014
-    s: 30.0087560502905
+    x: 587712.20345226175
+    y: 4141420.7844499564
+    theta: -1.8095455482738381
+    kappa: -0.010060073420644543
+    s: 37.187288232478238
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.1594792981433164
-  a: 1.1948926304417902
+  v: 4.786921178665188
+  a: -0.27889941846498761
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
-    x: 587713.44668271916
-    y: 4141427.217388303
-    theta: -1.6858936191113063
-    kappa: -0.029195274278385142
-    s: 30.630684284099267
+    x: 587712.09130344994
+    y: 4141420.3204778745
+    theta: -1.8147815040401651
+    kappa: -0.0090872392699960849
+    s: 37.664621919132685
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.2791379922257216
-  a: 1.1980524378321553
+  v: 4.7601135098548237
+  a: -0.25724213932886453
   relative_time: 6.2999999999999945
 }
 trajectory_point {
   path_point {
-    x: 587713.36754571623
-    y: 4141426.5884397421
-    theta: -1.7044825449562733
-    kappa: -0.026602063118815824
-    s: 31.264591972113752
+    x: 587711.97307161009
+    y: 4141419.8606863306
+    theta: -1.818690528573532
+    kappa: -0.0082812531003180131
+    s: 38.139383242317813
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.3990469788054849
-  a: 1.1999280380946717
+  v: 4.7354750677579984
+  a: -0.23551535143524016
   relative_time: 6.3999999999999941
 }
 trajectory_point {
   path_point {
-    x: 587713.27935475414
-    y: 4141425.9487278457
-    theta: -1.7223733758477895
-    kappa: -0.024091015966211819
-    s: 31.910498018125104
+    x: 587711.85384724045
+    y: 4141419.4035723503
+    theta: -1.8222736785733178
+    kappa: -0.007517423372140679
+    s: 38.611789467576344
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.5190867044815324
-  a: 1.2006967774635211
+  v: 4.7130126630781515
+  a: -0.21372185823033027
   relative_time: 6.4999999999999938
 }
 trajectory_point {
   path_point {
-    x: 587713.16505456
-    y: 4141425.3008204596
-    theta: -1.7372281173192348
-    kappa: -0.021958329591342085
-    s: 32.568410257315534
+    x: 587711.73516224185
+    y: 4141418.9485263526
+    theta: -1.8258406463761445
+    kappa: -0.0067570492089546064
+    s: 39.082058527504124
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.6391553504762051
-  a: 1.2005360021728859
+  v: 4.692732826174101
+  a: -0.19186446316035111
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
-    x: 587713.0486687636
-    y: 4141424.6410908969
-    theta: -1.7523557877727396
-    kappa: -0.019786728708676492
-    s: 33.238327229720682
+    x: 587711.61696145171
+    y: 4141418.4953368418
+    theta: -1.8293929992513145
+    kappa: -0.0059997772041195628
+    s: 39.550408993715656
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.7591688326352655
-  a: 1.1996230584569476
+  v: 4.674641807060044
+  a: -0.16994596967151879
   relative_time: 6.6999999999999931
 }
 trajectory_point {
   path_point {
-    x: 587712.92431813187
-    y: 4141423.9706963068
-    theta: -1.7664882810555775
-    kappa: -0.017687355940719175
-    s: 33.9202399536919
+    x: 587711.49629379937
+    y: 4141418.044564758
+    theta: -1.8321875765193583
+    kappa: -0.0053369446534738624
+    s: 40.017060048809618
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.87906080142789
-  a: 1.1981352925498885
+  v: 4.6587455754055522
+  a: -0.14796918121004904
   relative_time: 6.7999999999999927
 }
 trajectory_point {
   path_point {
-    x: 587712.77895195747
-    y: 4141423.2922000242
-    theta: -1.7768292481409309
-    kappa: -0.015906891442248432
-    s: 34.614133699358653
+    x: 587711.37374227017
+    y: 4141417.5958270142
+    theta: -1.8343903757062907
+    kappa: -0.0047479802538402264
+    s: 40.482231458334411
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.9987826419466774
-  a: 1.1962500506858909
+  v: 4.6450498205355819
+  a: -0.12593690122215834
   relative_time: 6.8999999999999924
 }
 trajectory_point {
   path_point {
-    x: 587712.63107975631
-    y: 4141422.6020068675
-    theta: -1.7873489374919285
-    kappa: -0.014095732790692388
-    s: 35.319989762090827
+    x: 587711.25152251578
+    y: 4141417.1483041053
+    theta: -1.8365872239471706
+    kappa: -0.004160610315171967
+    s: 40.946143542753717
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.1183034739076421
-  a: 1.1941446790991361
+  v: 4.6335599514304615
+  a: -0.10385193315406237
   relative_time: 6.999999999999992
 }
 trajectory_point {
   path_point {
-    x: 587712.47352574382
-    y: 4141421.9017739268
-    theta: -1.796935960746433
-    kappa: -0.012402824114512625
-    s: 36.03778723596109
+    x: 587711.12957635324
+    y: 4141416.7017829875
+    theta: -1.838779145831368
+    kappa: -0.0035745552174817319
+    s: 41.409017149412009
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.2376101516502178
-  a: 1.1919965240238064
+  v: 4.6242810967259018
+  a: -0.081717080451977467
   relative_time: 7.0999999999999917
 }
 trajectory_point {
   path_point {
-    x: 587712.30207973323
-    y: 4141421.1924827527
-    theta: -1.80494064698085
-    kappa: -0.010915616791779281
-    s: 36.767504787207216
+    x: 587711.00715763727
+    y: 4141416.2562402678
+    theta: -1.8407315582772552
+    kappa: -0.00302375506201661
+    s: 41.871073624500106
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.3567072641372562
-  a: 1.1899829316940842
+  v: 4.6172181047129914
+  a: -0.059535146562119369
   relative_time: 7.1999999999999913
 }
 trajectory_point {
   path_point {
-    x: 587712.12783781509
-    y: 4141420.4716245979
-    theta: -1.8130758493289927
-    kappa: -0.0094041563866987948
-    s: 37.509122427694429
+    x: 587710.88283362391
+    y: 4141415.8118418627
+    theta: -1.8419747729856721
+    kappa: -0.0025763022548241227
+    s: 42.3325347850207
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.4756171349550264
-  a: 1.1882812483441512
+  v: 4.6123755433381968
+  a: -0.037308934930704363
   relative_time: 7.2999999999999909
 }
 trajectory_point {
   path_point {
-    x: 587711.94196868211
-    y: 4141419.7414356838
-    theta: -1.8196252831171353
-    kappa: -0.0080819872865980184
-    s: 38.26262328837776
+    x: 587710.75861011678
+    y: 4141415.3678027187
+    theta: -1.8432169853421396
+    kappa: -0.0021292111777752188
+    s: 42.7936228907539
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.5943798223132184
-  a: 1.1870688202081898
+  v: 4.6097577002033621
+  a: -0.015041249003948513
   relative_time: 7.3999999999999906
 }
 trajectory_point {
   path_point {
-    x: 587711.74880652258
-    y: 4141419.0008394113
-    theta: -1.8254305816339018
-    kappa: -0.0068444634454839243
-    s: 39.027995392764382
+    x: 587710.63442712417
+    y: 4141414.9239083943
+    theta: -1.8444587914146173
+    kappa: -0.0016822659159696782
+    s: 43.254560616222768
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7130531190449361
-  a: 1.1865229935203814
+  v: 4.6093685825657111
+  a: 0.0072651077719321265
   relative_time: 7.49999999999999
 }
 trajectory_point {
   path_point {
-    x: 587711.5521004881
-    y: 4141418.2489079251
-    theta: -1.8311844906471979
-    kappa: -0.0056051432886386147
-    s: 39.805233430375942
+    x: 587710.50982151437
+    y: 4141414.4800573983
+    theta: -1.8453763931919693
+    kappa: -0.001280497229293209
+    s: 43.715571022658864
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.831712552606704
-  a: 1.1868211145149079
+  v: 4.6112119173378456
+  a: 0.029607331950721717
   relative_time: 7.59999999999999
 }
 trajectory_point {
   path_point {
-    x: 587711.34420662385
-    y: 4141417.487678553
-    theta: -1.8349212660784544
-    kappa: -0.0046060363362665259
-    s: 40.594340530210914
+    x: 587710.38437657745
+    y: 4141414.036134813
+    theta: -1.8456834417351151
+    kappa: -0.00096371313368494684
+    s: 44.176877529967761
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.950451385078467
-  a: 1.1881405294259524
+  v: 4.615291151087745
+  a: 0.051982620086204086
   relative_time: 7.6999999999999895
 }
 trajectory_point {
   path_point {
-    x: 587711.13318228617
-    y: 4141416.7149865613
-    theta: -1.8387143312956522
-    kappa: -0.0035918847941160898
-    s: 41.395330034206935
+    x: 587710.25879027531
+    y: 4141413.5917119673
+    theta: -1.8459908363422652
+    kappa: -0.00064657205056691393
+    s: 44.638703888694607
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.0693806131635828
-  a: 1.1906585844876956
+  v: 4.6216094500387692
+  a: 0.074388168732163071
   relative_time: 7.7999999999999892
 }
 trajectory_point {
   path_point {
-    x: 587710.916323786
-    y: 4141415.9315530453
-    theta: -1.8416398777887979
-    kappa: -0.0026968362260336451
-    s: 42.208227270703162
+    x: 587710.13300168
+    y: 4141413.1465732493
+    theta: -1.8462987260865968
+    kappa: -0.00032892012025265518
+    s: 45.101274151989664
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1886289681888353
-  a: 1.1945526259343202
+  v: 4.6301697000696533
+  a: 0.096821174442382821
   relative_time: 7.8999999999999888
 }
 trajectory_point {
   path_point {
-    x: 587710.694099399
-    y: 4141415.1372080063
-    theta: -1.8438620805131749
-    kappa: -0.0018970315610569795
-    s: 43.033071327902569
+    x: 587710.00694863859
+    y: 4141412.7005031123
+    theta: -1.84659806494005
+    kappa: -1.3065761511259159e-05
+    s: 45.564812647573824
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.3083429161044151
-  a: 1.2000000000000088
+  v: 4.6409745067145138
+  a: 0.11927883377064706
   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 7750fea3002745285dc1eda182714a7ffc6a5cff..c33e7c0827e1d065f3cc63f5e7de83da8d979756 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
@@ -1,7 +1,7 @@
 header {
   module_name: "planning"
 }
-total_path_length: 43.092359205778088
+total_path_length: 45.59729788926429
 total_path_time: 7.9999999999999885
 is_replan: true
 gear: GEAR_DRIVE
@@ -17,1687 +17,1687 @@ trajectory_point {
     ddkappa: 0
   }
   v: 5.4499998092651367
-  a: -0.00039843966536349748
+  a: -0.14106175627928821
   relative_time: 0
 }
 trajectory_point {
   path_point {
-    x: 587695.21616281965
-    y: 4141446.8837718065
-    theta: -0.21352012833299841
-    kappa: -0.0027247385307119293
-    s: 0.10900044978883905
+    x: 587695.21613522223
+    y: 4141446.8837779625
+    theta: -0.21351993265454322
+    kappa: -0.0027246222465283189
+    s: 0.10897217400035646
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4500712613916322
-  a: 0.0074292714878739646
+  v: 5.4472365598409693
+  a: -0.13537083792089208
   relative_time: 0.02
 }
 trajectory_point {
   path_point {
-    x: 587695.32255128573
-    y: 4141446.860039229
-    theta: -0.21427447375756023
-    kappa: -0.0031730150216785903
-    s: 0.21800384858940192
+    x: 587695.32244034228
+    y: 4141446.8600639771
+    theta: -0.21427368711233336
+    kappa: -0.0031725475527327298
+    s: 0.21789017833240804
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4502924407503821
-  a: 0.014576076500612475
+  v: 5.4445807091209968
+  a: -0.13031991365812468
   relative_time: 0.04
 }
 trajectory_point {
   path_point {
-    x: 587695.42894542043
-    y: 4141446.8363053864
-    theta: -0.21502886552108053
-    kappa: -0.0036213153984256188
-    s: 0.32701305548228177
+    x: 587695.4286945645
+    y: 4141446.8363613454
+    theta: -0.2150270868109132
+    kappa: -0.0036202583965623748
+    s: 0.32675603376030016
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.45064983679501
-  a: 0.021052733016266926
+  v: 5.4420195755554053
+  a: -0.12589715028114143
   relative_time: 0.06
 }
 trajectory_point {
   path_point {
-    x: 587695.53534775286
-    y: 4141446.8125697151
-    theta: -0.21578332069706982
-    kappa: -0.004069650316527995
-    s: 0.436030661488673
+    x: 587695.53489961592
+    y: 4141446.8126696832
+    theta: -0.21578014313271993
+    kappa: -0.0040677620550853616
+    s: 0.435571509783824
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4511301541320076
-  a: 0.0268699986782522
+  v: 5.4395407142585785
+  a: -0.12209071458009767
   relative_time: 0.08
 }
 trajectory_point {
   path_point {
-    x: 587695.64176055428
-    y: 4141446.7888317085
-    theta: -0.21653785453376984
-    kappa: -0.0045180293468627137
-    s: 0.5450589938734286
+    x: 587695.6410569828
+    y: 4141446.7889886573
+    theta: -0.21653286575674457
+    kappa: -0.0045150647915107433
+    s: 0.54433812937170023
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4517203125207381
-  a: 0.032038631129983196
+  v: 5.4371319170090944
+  a: -0.1188887733451488
   relative_time: 0.1
 }
 trajectory_point {
   path_point {
-    x: 587695.748185843
-    y: 4141446.7650909163
-    theta: -0.21729248048369693
-    kappa: -0.0049664609933051046
-    s: 0.65410012044811716
+    x: 587695.74716791592
+    y: 4141446.7653179904
+    theta: -0.21728526269111786
+    kappa: -0.0049621718746541421
+    s: 0.65305717369486316
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.452407446873428
-  a: 0.0365693880148748
+  v: 5.4347812122497308
+  a: -0.11627949336645022
   relative_time: 0.12000000000000001
 }
 trajectory_point {
   path_point {
-    x: 587695.854625388
-    y: 4141446.7413469441
-    theta: -0.21804721023321733
-    kappa: -0.00541495271042516
-    s: 0.763155853874081
+    x: 587695.853233434
+    y: 4141446.7416574541
+    theta: -0.21803734030572564
+    kappa: -0.0054090875984033893
+    s: 0.76172968685974485
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4531789072551771
-  a: 0.040473026976341892
+  v: 5.4324768650874606
+  a: -0.11425104143415729
   relative_time: 0.14
 }
 trajectory_point {
   path_point {
-    x: 587695.961080714
-    y: 4141446.7175994515
-    theta: -0.21880205373215261
-    kappa: -0.0058635109211838581
-    s: 0.87222775596549273
+    x: 587695.95925432967
+    y: 4141446.7180068716
+    theta: -0.21878910336485358
+    kappa: -0.0058558153011841474
+    s: 0.87035648064155791
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4540222588839482
-  a: 0.043760305657799377
+  v: 5.4302073772934536
+  a: -0.11279158433842543
   relative_time: 0.16
 }
 trajectory_point {
   path_point {
-    x: 587696.0675531046
-    y: 4141446.6938481522
-    theta: -0.21955701922341597
-    kappa: -0.0063121410346294836
-    s: 0.98131714199241282
+    x: 587696.06523117272
+    y: 4141446.694366117
+    theta: -0.21954055505985984
+    kappa: -0.0063023573854255489
+    s: 0.97893813921758166
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4549252821305778
-  a: 0.04644198170266213
+  v: 5.4279614873030786
+  a: -0.11188928886940998
   relative_time: 0.18
 }
 trajectory_point {
   path_point {
-    x: 587696.17404360673
-    y: 4141446.6700928127
-    theta: -0.22031211327267775
-    kappa: -0.0067608474635939561
-    s: 1.0904250849838477
+    x: 587696.171164316
+    y: 4141446.6707351096
+    theta: -0.22029169704187554
+    kappa: -0.0067487153370258176
+    s: 1.0874750239004436
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4558759725187675
-  a: 0.048528812754345042
+  v: 5.4257281702158986
+  a: -0.11153232181726634
   relative_time: 0.19999999999999998
 }
 trajectory_point {
   path_point {
-    x: 587696.28055303579
-    y: 4141446.6463332511
-    theta: -0.22106734079805976
-    kappa: -0.0072096336423891532
-    s: 1.1995524200308059
+    x: 587696.27705389925
+    y: 4141446.64711382
+    theta: -0.22104252945453087
+    kappa: -0.0071948897448179108
+    s: 1.1959672778714052
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.456862540725087
-  a: 0.050031556456263018
+  v: 5.4234966377956741
+  a: -0.1117088499721499
   relative_time: 0.21999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587696.38708197873
-    y: 4141446.6225693366
-    theta: -0.22182270509985802
-    kappa: -0.00765850204450323
-    s: 1.3086997485893563
+    x: 587696.38289985387
+    y: 4141446.6235022629
+    theta: -0.2217930509667079
+    kappa: -0.0076408803200351373
+    s: 1.3044148309136445
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4578734125789756
-  a: 0.050960970451830928
+  v: 5.4212563384703643
+  a: -0.11240704012421604
   relative_time: 0.23999999999999996
 }
 trajectory_point {
   path_point {
-    x: 587696.49363079877
-    y: 4141446.5988009875
-    theta: -0.22257820789029253
-    kappa: -0.0081074542002969527
-    s: 1.4178674427836857
+    x: 587696.4887019078
+    y: 4141446.5999004981
+    theta: -0.22254325880531708
+    kappa: -0.0080866859157768031
+    s: 1.4128174041455415
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4588972290627407
-  a: 0.051327812384463663
+  v: 5.4189969573321237
+  a: -0.11361505906362013
   relative_time: 0.25999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587696.60019963945
-    y: 4141446.5750281727
-    theta: -0.223333849323284
-    kappa: -0.0085564907147000074
-    s: 1.5270556497091545
+    x: 587696.5944595898
+    y: 4141446.5763086323
+    theta: -0.22329314878809844
+    kappa: -0.0085323045464738255
+    s: 1.5211745147539604
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4599228463115574
-  a: 0.051142839897576134
+  v: 5.4167084161373031
+  a: -0.11532107358051756
   relative_time: 0.27999999999999997
 }
 trajectory_point {
   path_point {
-    x: 587696.70678842883
-    y: 4141446.5512509076
-    theta: -0.22408962802425678
-    kappa: -0.0090056112849073409
-    s: 1.636264295735357
+    x: 587696.700172234
+    y: 4141446.5527268136
+    theta: -0.22404271535644549
+    kappa: -0.0089777334073543776
+    s: 1.6294854807275347
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4609393356134692
-  a: 0.0504168106345832
+  v: 5.4143808733064516
+  a: -0.11751325046506372
   relative_time: 0.3
 }
 trajectory_point {
   path_point {
-    x: 587696.813396884
-    y: 4141446.527469256
-    theta: -0.22484554111996702
-    kappa: -0.00945481471807547
-    s: 1.745493090809175
+    x: 587696.80583898514
+    y: 4141446.529155232
+    theta: -0.22479195160825152
+    kappa: -0.0094229688939095144
+    s: 1.7377494255899513
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.461935983409389
-  a: 0.049160482238899759
+  v: 5.4120047239243148
+  a: -0.12017975650741401
   relative_time: 0.32
 }
 trajectory_point {
   path_point {
-    x: 587696.92002451478
-    y: 4141446.5036833263
-    theta: -0.22560158426835578
-    kappa: -0.0099040989490188219
-    s: 1.8547415327578396
+    x: 587696.91145880264
+    y: 4141446.5055941204
+    theta: -0.22554084933077717
+    kappa: -0.0098680066213588
+    s: 1.8459652831332334
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4629022912930969
-  a: 0.047384612353940717
+  v: 5.4095705997398351
+  a: -0.12330875849772378
   relative_time: 0.34
 }
 trajectory_point {
   path_point {
-    x: 587697.02667062811
-    y: 4141446.4798932741
-    theta: -0.22635775168842576
-    kappa: -0.010353461057906035
-    s: 1.9640089115919837
+    x: 587697.01703046518
+    y: 4141446.4820437506
+    theta: -0.22628939903353834
+    kappa: -0.01031284144411594
+    s: 1.954131802151025
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4638279760112409
-  a: 0.045099958623120946
+  v: 5.4070693691661518
+  a: -0.12688842322614843
   relative_time: 0.36000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587697.13296917686
-    y: 4141446.4545388869
-    theta: -0.22758026383642588
-    kappa: -0.010769834161819977
-    s: 2.0732943138087041
+    x: 587697.1222397479
+    y: 4141446.4571676361
+    theta: -0.227437004032113
+    kappa: -0.010729142394948722
+    s: 2.062247551171875
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4647029694633389
-  a: 0.042317278689855325
+  v: 5.4044921372806014
+  a: -0.13090691748284333
   relative_time: 0.38000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587697.23913162737
-    y: 4141446.4285287024
-    theta: -0.22899777502111948
-    kappa: -0.011172459291069527
-    s: 2.1825966266946146
+    x: 587697.227198848
+    y: 4141446.4314522767
+    theta: -0.22883844294540431
+    kappa: -0.011127203771494064
+    s: 2.1703109231925217
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4655174187017748
-  a: 0.03904733019755878
+  v: 5.4018302458247156
+  a: -0.13535240805796389
   relative_time: 0.40000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587697.34530923259
-    y: 4141446.4025148042
-    theta: -0.23041553088763353
-    kappa: -0.011575141895583833
-    s: 2.2919145426289074
+    x: 587697.3321053487
+    y: 4141446.4057498039
+    theta: -0.23023922150508308
+    kappa: -0.011525065663842076
+    s: 2.2783201404111746
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4662616859318041
-  a: 0.035300870789646173
+  v: 5.3990752732042253
+  a: -0.14021306174166548
   relative_time: 0.4200000000000001
 }
 trajectory_point {
   path_point {
-    x: 587697.45150053757
-    y: 4141446.37649755
-    theta: -0.23183350632369881
-    kappa: -0.011977876456512573
-    s: 2.4012465633864064
+    x: 587697.43695736234
+    y: 4141446.3800606807
+    theta: -0.23163930894558832
+    kappa: -0.011922720911579639
+    s: 2.3862732589608009
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4669263485115467
-  a: 0.031088658109532392
+  v: 5.3962190344890573
+  a: -0.14547704532410349
   relative_time: 0.44000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587697.557703906
-    y: 4141446.3504773406
-    theta: -0.23325167379344361
-    kappa: -0.012380656768747405
-    s: 2.510591004440629
+    x: 587697.54175284412
+    y: 4141446.3543854086
+    theta: -0.23303867242257972
+    kappa: -0.012320161760012509
+    s: 2.4941681736424077
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4675021989519932
-  a: 0.02642144980063232
+  v: 5.3932535814133349
+  a: -0.15113252559543328
   relative_time: 0.46000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587697.66391752509
-    y: 4141446.3244546195
-    theta: -0.23467000339392974
-    kappa: -0.012783475956772674
-    s: 2.6199459992668386
+    x: 587697.64648959716
+    y: 4141446.3287245245
+    theta: -0.23443727707526021
+    kappa: -0.012717379877600809
+    s: 2.6020026226583273
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4679802449170021
-  a: 0.021310003506360874
+  v: 5.39017120237538
+  a: -0.15716766934581025
   relative_time: 0.48000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587697.77013940923
-    y: 4141446.2984298728
-    theta: -0.23608846291183941
-    kappa: -0.013186326490516136
-    s: 2.7293095036451058
+    x: 587697.75116527721
+    y: 4141446.3030786039
+    theta: -0.23583508608882256
+    kappa: -0.013114366373394512
+    s: 2.7097741923454994
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4683517092233007
-  a: 0.01576507687013292
+  v: 5.3869644224377078
+  a: -0.16357064336538973
   relative_time: 0.50000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587697.87636740447
-    y: 4141446.2724036295
-    theta: -0.23750701788030762
-    kappa: -0.013589200201199639
-    s: 2.8386792999633648
+    x: 587697.85577739682
+    y: 4141446.2774482556
+    theta: -0.23723206075701406
+    kappa: -0.013511111814468939
+    s: 2.8174803219087567
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4686080298404844
-  a: 0.0097974275353633761
+  v: 5.3836260033270351
+  a: -0.1703296144443272
   relative_time: 0.52000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587697.982599193
-    y: 4141446.2463764567
-    theta: -0.23892563163589678
-    kappa: -0.013992088297189852
-    s: 2.9480530015204693
+    x: 587697.96032333083
+    y: 4141446.2518341234
+    theta: -0.23862816054481312
+    kappa: -0.013907606243360227
+    s: 2.9251183081541083
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4687408598910148
-  a: 0.0034178131454670729
+  v: 5.3801489434342713
+  a: -0.17743274937277803
   relative_time: 0.54000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587698.0888322962
-    y: 4141446.2203489617
-    theta: -0.24034426537570663
-    kappa: -0.014394981379848962
-    s: 3.0574280568292513
+    x: 587698.06480031973
+    y: 4141446.2262368826
+    theta: -0.24002334315121257
+    kappa: -0.014303839195500836
+    s: 3.032685310222023
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4687420676502247
-  a: -0.003363008656141012
+  v: 5.3765264778145259
+  a: -0.18486821494089753
   relative_time: 0.56000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587698.1950640803
-    y: 4141446.19432179
-    theta: -0.24176287821461651
-    kappa: -0.014797869459385396
-    s: 3.1668017539195796
+    x: 587698.16920547513
+    y: 4141446.200657241
+    theta: -0.24141756457210314
+    kappa: -0.014699799716655023
+    s: 3.1401783543207133
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4686037365463136
-  a: -0.010534280226046081
+  v: 5.3727520781871032
+  a: -0.19262417793884112
   relative_time: 0.58000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587698.30129175936
-    y: 4141446.1682956237
-    theta: -0.24318142724265321
-    kappa: -0.015200741970704494
-    s: 3.2761712246414119
+    x: 587698.27353578433
+    y: 4141446.1750959381
+    theta: -0.24281077916325236
+    kappa: -0.015095476380354328
+    s: 3.2475943384594208
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.46831816516035
-  a: -0.018085243920833216
+  v: 5.3688194529355062
+  a: -0.20068880515676421
   relative_time: 0.6000000000000002
 }
 trajectory_point {
   path_point {
-    x: 587698.40751240018
-    y: 4141446.142271182
-    theta: -0.24459986758248092
-    kappa: -0.015603587789259269
-    s: 3.3855334489678608
+    x: 587698.37778811436
+    y: 4141446.14955374
+    theta: -0.24420293970337145
+    kappa: -0.015490857305333077
+    s: 3.3549300371816981
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4678778672262709
-  a: -0.026005142097087493
+  v: 5.3647225471074318
+  a: -0.20905026338482219
   relative_time: 0.62000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587698.51372292626
-    y: 4141446.1162492186
-    theta: -0.24601815244700681
-    kappa: -0.016006395246901072
-    s: 3.4948852592982429
+    x: 587698.48195921723
+    y: 4141446.1240314422
+    theta: -0.2455939974572659
+    kappa: -0.015885930172963851
+    s: 3.4621821062986955
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4672755716308794
-  a: -0.034283217111394024
+  v: 5.360455542414778
+  a: -0.21769671941317037
   relative_time: 0.64000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587698.61992012174
-    y: 4141446.0902305213
-    theta: -0.24743623319709751
-    kappa: -0.016409152147730317
-    s: 3.6042233447611403
+    x: 587698.586045734
+    y: 4141446.0985298688
+    theta: -0.24698390223906255
+    kappa: -0.016280682244692979
+    s: 3.5693470876224405
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4665042224138505
-  a: -0.042908711320337987
+  v: 5.3560128572336358
+  a: -0.22661634003196418
   relative_time: 0.66000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587698.72610063571
-    y: 4141446.0642159106
-    theta: -0.24885405939940142
-    kappa: -0.01681184578394718
-    s: 3.7135442555174576
+    x: 587698.6900441997
+    y: 4141446.073049868
+    theta: -0.24837260247550777
+    kappa: -0.016675100379476032
+    s: 3.6764214136991273
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4655569787677249
-  a: -0.051870867080504424
+  v: 5.3513891466042951
+  a: -0.23579729203135902
   relative_time: 0.68000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587698.832260987
-    y: 4141446.03820624
-    theta: -0.25027157888427143
-    kappa: -0.017214462951702331
-    s: 3.8228444070634797
+    x: 587698.79395104782
+    y: 4141446.047592314
+    theta: -0.24976004526933074
+    kappa: -0.017069171051213305
+    s: 3.7834014125423963
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4644272150379116
-  a: -0.061158926748478518
+  v: 5.3465793022312429
+  a: -0.24522774220151022
   relative_time: 0.70000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587698.93839756725
-    y: 4141446.0122023937
-    theta: -0.2516887378037817
-    kappa: -0.017616989966947608
-    s: 3.9321200845339268
+    x: 587698.897762615
+    y: 4141446.022158104
+    theta: -0.25114617646266557
+    kappa: -0.017462880366185284
+    s: 3.8902833123666185
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.46310852072269
-  a: -0.0707621326808453
+  v: 5.3415784524831622
+  a: -0.25489585733257325
   relative_time: 0.72000000000000031
 }
 trajectory_point {
   path_point {
-    x: 587699.04406577593
-    y: 4141445.9845350105
-    theta: -0.25352068454861276
-    kappa: -0.01801119928546225
-    s: 4.0413674470050145
+    x: 587699.00143664517
+    y: 4141445.9966022968
+    theta: -0.25256719740221512
+    kappa: -0.017855496820429254
+    s: 3.9970632463201832
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4615947004732046
-  a: -0.080669727234189886
+  v: 5.3363819623929327
+  a: -0.26478980421470338
   relative_time: 0.74000000000000032
 }
 trajectory_point {
   path_point {
-    x: 587699.1491516158
-    y: 4141445.9547877242
-    theta: -0.25587129809101411
-    kappa: -0.018395024242421711
-    s: 4.1505825317975082
+    x: 587699.10407748492
+    y: 4141445.9675471317
+    theta: -0.25486303098994767
+    kappa: -0.018230391439791029
+    s: 4.1037372572187767
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4598797740934719
-  a: -0.090870952765097476
+  v: 5.3309854336576326
+  a: -0.27489774963805608
   relative_time: 0.76000000000000034
 }
 trajectory_point {
   path_point {
-    x: 587699.2542024724
-    y: 4141445.9250503411
-    theta: -0.25822132898191946
-    kappa: -0.018778721423671728
-    s: 4.2597612587797862
+    x: 587699.20661251643
+    y: 4141445.9385219184
+    theta: -0.25715669898385141
+    kappa: -0.018604899595750003
+    s: 4.2103013022786708
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4579579765403734
-  a: -0.101355051630153
+  v: 5.3253847046385347
+  a: -0.28520786039278667
   relative_time: 0.78000000000000036
 }
 trajectory_point {
   path_point {
-    x: 587699.359214311
-    y: 4141445.895324003
-    theta: -0.26057066083185981
-    kappa: -0.019162276092366315
-    s: 4.3688994346708885
+    x: 587699.30903777212
+    y: 4141445.9095277805
+    theta: -0.25944808792221885
+    kappa: -0.018979006796171382
+    s: 4.3167512578500045
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.45582375792366
-  a: -0.11211126618594183
+  v: 5.3195758503611108
+  a: -0.29570830326905062
   relative_time: 0.80000000000000038
 }
 trajectory_point {
   path_point {
-    x: 587699.46418299247
-    y: 4141445.8656098824
-    theta: -0.2629191749277362
-    kappa: -0.019545673129130678
-    s: 4.4779927573435812
+    x: 587699.41134921077
+    y: 4141445.8805658612
+    theta: -0.26173708282123215
+    kappa: -0.019352698281359458
+    s: 4.4230829241500658
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4534717835059539
-  a: -0.12312883878904882
+  v: 5.313555182515028
+  a: -0.30638724505700321
   relative_time: 0.8200000000000004
 }
 trajectory_point {
   path_point {
-    x: 587699.56910427625
-    y: 4141445.835909178
-    theta: -0.26526675033659514
-    kappa: -0.019928897047183845
-    s: 4.5870368201274108
+    x: 587699.51354272291
+    y: 4141445.8516373239
+    theta: -0.264023567287028
+    kappa: -0.019725959040692253
+    s: 4.5292920299965811
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4508969337027393
-  a: -0.13439701179605923
+  v: 5.3073192494541512
+  a: -0.3172328525468
   relative_time: 0.84000000000000041
 }
 trajectory_point {
   path_point {
-    x: 587699.67397382611
-    y: 4141445.8062231187
-    theta: -0.26761326400978785
-    kappa: -0.020311932007461314
-    s: 4.6960271161117646
+    x: 587699.61561413435
+    y: 4141445.8227433506
+    theta: -0.26630742362796894
+    kappa: -0.020098773829256129
+    s: 4.6353742375409954
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4480943040823746
-  a: -0.14590502756355811
+  v: 5.3008648361965429
+  a: -0.32823329252859607
   relative_time: 0.86000000000000043
 }
 trajectory_point {
   path_point {
-    x: 587699.77878721326
-    y: 4141445.7765529575
-    theta: -0.26995859088748531
-    kappa: -0.020694761833737692
-    s: 4.804959042448921
+    x: 587699.717559212
+    y: 4141445.7938851388
+    theta: -0.26858853296709184
+    kappa: -0.02047112718448043
+    s: 4.7413251470017546
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4450592053660829
-  a: -0.15764212844813066
+  v: 5.29418896442446
+  a: -0.33937673179254713
   relative_time: 0.88000000000000045
 }
 trajectory_point {
   path_point {
-    x: 587699.88353992044
-    y: 4141445.7468999731
-    theta: -0.27230260400352141
-    kappa: -0.021077370027749349
-    s: 4.9138279046571176
+    x: 587699.81937366724
+    y: 4141445.7650639042
+    theta: -0.27086677535470616
+    kappa: -0.02084300344277212
+    s: 4.8471403013975953
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.441787163427958
-  a: -0.16959755680636193
+  v: 5.2872888924843586
+  a: -0.3506513371288083
   relative_time: 0.90000000000000047
 }
 trajectory_point {
   path_point {
-    x: 587699.98822734714
-    y: 4141445.7172654686
-    theta: -0.27464517459053511
-    kappa: -0.021459739784317051
-    s: 5.0226289209235953
+    x: 587699.92105316115
+    y: 4141445.7362808725
+    theta: -0.27314202988111108
+    kappa: -0.021214386756150387
+    s: 4.9528151912808216
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4382739192949607
-  a: -0.18176055499483706
+  v: 5.280162115386891
+  a: -0.36204527532753511
   relative_time: 0.92000000000000048
 }
 trajectory_point {
   path_point {
-    x: 587700.092844812
-    y: 4141445.6876507686
-    theta: -0.27698617218538479
-    kappa: -0.021841854006468608
-    s: 5.13135722640767
+    x: 587700.02259330894
+    y: 4141445.7075372874
+    theta: -0.27541417478940439
+    kappa: -0.021585261108881312
+    s: 5.0583452594705953
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4345154291469191
-  a: -0.194120365370141
+  v: 5.2728063648069066
+  a: -0.373546713178883
   relative_time: 0.9400000000000005
 }
 trajectory_point {
   path_point {
-    x: 587700.19738755841
-    y: 4141445.6580572193
-    theta: -0.27932546473480274
-    kappa: -0.02222369532056152
-    s: 5.2400078775437793
+    x: 587700.123989684
+    y: 4141445.6788344011
+    theta: -0.27768308758835336
+    kappa: -0.021955610334112453
+    s: 5.163725905786217
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4305078643165317
-  a: -0.20666623028885917
+  v: 5.26521960908345
+  a: -0.38514381747300719
   relative_time: 0.96000000000000052
 }
 trajectory_point {
   path_point {
-    x: 587700.30185075826
-    y: 4141445.6284861881
-    theta: -0.28166291870126442
-    kappa: -0.0226052460914056
-    s: 5.3485758563445414
+    x: 587700.22523782379
+    y: 4141445.650173476
+    theta: -0.27994864516529905
+    kappa: -0.022325418130507507
+    s: 5.2689524917804071
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4262476112893649
-  a: -0.21938739210757621
+  v: 5.2574000532197669
+  a: -0.3968247550000632
   relative_time: 0.98000000000000054
 }
 trajectory_point {
   path_point {
-    x: 587700.40622951568
-    y: 4141445.59893906
-    theta: -0.28399839916904079
-    kappa: -0.022986488437385676
-    s: 5.4570560747038188
+    x: 587700.32633323246
+    y: 4141445.6215557856
+    theta: -0.28221072389906615
+    kappa: -0.022694668078880931
+    s: 5.3740203454725961
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4217312717038517
-  a: -0.23227309318287773
+  v: 5.2493461388832943
+  a: -0.40857769255020615
   relative_time: 1.0000000000000004
 }
 trajectory_point {
   path_point {
-    x: 587700.9265505278
-    y: 4141445.4511905387
-    theta: -0.29569994732858867
-    kappa: -0.024888572303307294
-    s: 5.9979583100350844
+    x: 587700.82936029136
+    y: 4141445.4791608658
+    theta: -0.29346458680137294
+    kappa: -0.024531969275446097
+    s: 5.8968133591468188
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.3952043934470746
-  a: -0.29879316988862814
+  v: 5.2055232149632822
+  a: -0.46800821830266726
   relative_time: 1.1000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587701.43621483631
-    y: 4141445.279167722
-    theta: -0.31082840251100019
-    kappa: -0.026848045814521354
-    s: 6.535870530471712
+    x: 587701.32162184024
+    y: 4141445.3178453567
+    theta: -0.30742616494476388
+    kappa: -0.026407477509623505
+    s: 6.4149262697887659
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.3618893098960472
-  a: -0.36781308149675213
+  v: 5.1557436612527443
+  a: -0.52746376315561438
   relative_time: 1.2000000000000006
 }
 trajectory_point {
   path_point {
-    x: 587701.94239325542
-    y: 4141445.1083214693
-    theta: -0.32585916112525476
-    kappa: -0.028794117350778359
-    s: 7.0701036574854754
+    x: 587701.8075300823
+    y: 4141445.1538407304
+    theta: -0.32185427735019712
+    kappa: -0.028275617603428095
+    s: 6.9277657545759315
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.3216032728318741
-  a: -0.43798812258038805
+  v: 5.1000789334031627
+  a: -0.58546517587847047
   relative_time: 1.3000000000000007
 }
 trajectory_point {
   path_point {
-    x: 587702.44442194782
-    y: 4141444.9388758396
-    theta: -0.34076567769483818
-    kappa: -0.030724234700904931
-    s: 7.5999570612533951
+    x: 587702.28789343976
+    y: 4141444.9917076239
+    theta: -0.3361184564017245
+    kappa: -0.030122439636663476
+    s: 7.4347530320071122
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2742980045783474
-  a: -0.50797358771267542
+  v: 5.0387484021890732
+  a: -0.64053330524065843
   relative_time: 1.4000000000000008
 }
 trajectory_point {
   path_point {
-    x: 587702.93851859018
-    y: 4141444.7627055626
-    theta: -0.35641024035051644
-    kappa: -0.032698682035432025
-    s: 8.1247320077120033
+    x: 587702.76219131751
+    y: 4141444.8316217484
+    theta: -0.35019752651098152
+    kappa: -0.031945942111559163
+    s: 7.9353386534147132
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.220059698001946
-  a: -0.57642477146675231
+  v: 4.9721193535080719
+  a: -0.69118900001160088
   relative_time: 1.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587703.41859358619
-    y: 4141444.5654681576
-    theta: -0.37433699498019518
-    kappa: -0.0348245509710543
-    s: 8.6437451056116146
+    x: 587703.219975375
+    y: 4141444.6470698677
+    theta: -0.36691870050998748
+    kappa: -0.033945029461573177
+    s: 8.4290172944770489
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.1591090165118327
-  a: -0.64199696841575871
+  v: 4.9007069883808132
+  a: -0.7359531089607213
   relative_time: 1.600000000000001
 }
 trajectory_point {
   path_point {
-    x: 587703.89273351594
-    y: 4141444.3706691572
-    theta: -0.39204961620670609
-    kappa: -0.036924138235441377
-    s: 9.1563417535705955
+    x: 587703.66981487721
+    y: 4141444.4622546267
+    theta: -0.3837217459095929
+    kappa: -0.03593700953020447
+    s: 8.9153425467306544
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.0918010940598588
-  a: -0.70334547313283213
+  v: 4.8251744229510081
+  a: -0.77334648085744206
   relative_time: 1.7000000000000011
 }
 trajectory_point {
   path_point {
-    x: 587704.36037195649
-    y: 4141444.1785412747
-    theta: -0.40951577344255224
-    kappa: -0.038994935594048485
-    s: 9.66190958712963
+    x: 587704.11250792653
+    y: 4141444.2803754844
+    theta: -0.40025922542431774
+    kappa: -0.037897343664306846
+    s: 9.39394170908259
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.0186255351405595
-  a: -0.75912558019111254
+  v: 4.746332688485424
+  a: -0.80188996447118677
   relative_time: 1.8000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587704.81506468472
-    y: 4141443.9763769363
-    theta: -0.4287681240104837
-    kappa: -0.041162366615463665
-    s: 10.15989192580599
+    x: 587704.54779164284
+    y: 4141444.1015404458
+    theta: -0.41651243711428471
+    kappa: -0.039824867776398938
+    s: 9.86453057932274
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.9402064147911577
-  a: -0.80799258416373843
+  v: 4.6651407313738886
+  a: -0.82010440857137734
   relative_time: 1.9000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587705.251690007
-    y: 4141443.7541845767
-    theta: -0.45144542818954586
-    kappa: -0.043525030976718775
-    s: 10.649801220147809
+    x: 587704.96393364435
+    y: 4141443.9006196596
+    theta: -0.43649679491245463
+    kappa: -0.041967925449427165
+    s: 10.326928245636127
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8573022785915621
-  a: -0.84860177962384575
+  v: 4.5827054131292844
+  a: -0.826510661927438
   relative_time: 2.0000000000000013
 }
 trajectory_point {
   path_point {
-    x: 587705.6807625785
-    y: 4141443.5358357038
-    theta: -0.47374609501569032
-    kappa: -0.045846825936872775
-    s: 11.131236056677524
+    x: 587705.3686859461
+    y: 4141443.6946470225
+    theta: -0.457525596383843
+    kappa: -0.04415811871061058
+    s: 10.781074866144074
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7709462603541342
-  a: -0.8755148243362173
+  v: 4.500398218794107
+  a: -0.81626895849042524
   relative_time: 2.1000000000000014
 }
 trajectory_point {
   path_point {
-    x: 587706.10204514256
-    y: 4141443.321451054
-    theta: -0.49563540024123159
-    kappa: -0.048126467655161514
-    s: 11.6039302251229
+    x: 587705.76617764425
+    y: 4141443.4923692029
+    theta: -0.47818536255529193
+    kappa: -0.046309023435151211
+    s: 11.227074828337777
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6827785200852716
-  a: -0.88505167821999686
+  v: 4.4200928722575847
+  a: -0.7867897302618263
   relative_time: 2.2000000000000015
 }
 trajectory_point {
   path_point {
-    x: 587706.51123530266
-    y: 4141443.1037238459
-    theta: -0.51782819596837215
-    kappa: -0.050425482724516218
-    s: 12.067787279655263
+    x: 587706.15667005989
+    y: 4141443.2936532181
+    theta: -0.49847207126548654
+    kappa: -0.04842205368465289
+    s: 11.665221344377983
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5944720394775977
-  a: -0.87850508907987634
+  v: 4.3436183187066248
+  a: -0.73996912308670693
   relative_time: 2.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587706.89415766392
-    y: 4141442.8578128624
-    theta: -0.54286717284273478
-    kappa: -0.0529534374443663
-    s: 12.522871608876176
+    x: 587706.53495842544
+    y: 4141443.0884889653
+    theta: -0.519377918518623
+    kappa: -0.05058209666505574
+    s: 12.095981040214907
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.50757052544327
-  a: -0.8571678047205481
+  v: 4.2726138887436251
+  a: -0.67770328281013292
   relative_time: 2.4000000000000017
 }
 trajectory_point {
   path_point {
-    x: 587707.2698770084
-    y: 4141442.6165276221
-    theta: -0.56746626977000114
-    kappa: -0.055433839705429346
-    s: 12.969395508339398
+    x: 587706.89172036061
+    y: 4141442.8593780873
+    theta: -0.54270766565585538
+    kappa: -0.0529373469959602
+    s: 12.519974994129775
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.423488410113972
-  a: -0.82233257294670414
+  v: 4.2085292983864786
+  a: -0.60188835527717022
   relative_time: 2.5000000000000018
 }
 trajectory_point {
   path_point {
-    x: 587707.6386855446
-    y: 4141442.3796804724
-    theta: -0.59161395686523677
-    kappa: -0.057868618590931245
-    s: 13.407706253072826
+    x: 587707.24342598743
+    y: 4141442.6335143466
+    theta: -0.56573409133352859
+    kappa: -0.055259216883534683
+    s: 12.937959775276383
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3435108508409224
-  a: -0.77529214156303661
+  v: 4.152624649068569
+  a: -0.51442048633288451
   relative_time: 2.6000000000000019
 }
 trajectory_point {
   path_point {
-    x: 587708.000978179
-    y: 4141442.147017804
-    theta: -0.61530863380492651
-    kappa: -0.060260381172549254
-    s: 13.838273170100461
+    x: 587707.590809959
+    y: 4141442.4104259536
+    theta: -0.58848025206006072
+    kappa: -0.057552556316020714
+    s: 13.350808482222632
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2687937301948686
-  a: -0.7173392583742374
+  v: 4.1059704276387761
+  a: -0.41719582182234177
   relative_time: 2.700000000000002
 }
 trajectory_point {
   path_point {
-    x: 587708.3371206267
-    y: 4141441.8907437292
-    theta: -0.64096239710171221
-    kappa: -0.062636696365941477
-    s: 14.261674710964353
+    x: 587707.9346890247
+    y: 4141442.1895883949
+    theta: -0.61097635721757626
+    kappa: -0.059822757263350242
+    s: 13.759491781492093
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.20036365596609
-  a: -0.64976667118499887
+  v: 4.069447506361473
+  a: -0.31211050759060777
   relative_time: 2.800000000000002
 }
 trajectory_point {
   path_point {
-    x: 587708.65785154619
-    y: 4141441.6243862556
-    theta: -0.66748885332337837
-    kappa: -0.064989000219561319
-    s: 14.678585524246561
+    x: 587708.26279378717
+    y: 4141441.9524699543
+    theta: -0.63482294846241616
+    kappa: -0.062091568658150492
+    s: 14.165058946105544
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.139117961164394
-  a: -0.573867127800013
+  v: 4.0437471429165255
+  a: -0.20106068948274824
   relative_time: 2.9000000000000021
 }
 trajectory_point {
   path_point {
-    x: 587708.97417219565
-    y: 4141441.3616913804
-    theta: -0.69367741872290556
-    kappa: -0.067308958282900164
-    s: 15.089763528091094
+    x: 587708.57325384021
+    y: 4141441.6946421303
+    theta: -0.66048791911806881
+    kappa: -0.064368543950332216
+    s: 14.568618894122528
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0858247040191218
-  a: -0.49093337602397197
+  v: 4.0293709803992925
+  a: -0.085942513343829186
   relative_time: 3.0000000000000022
 }
 trajectory_point {
   path_point {
-    x: 587709.28671975876
-    y: 4141441.1021299409
-    theta: -0.71954453941329155
-    kappa: -0.069601243785012909
-    s: 15.496036982725872
+    x: 587708.8830541271
+    y: 4141441.4373622215
+    theta: -0.6861330247973656
+    kappa: -0.06664068038752842
+    s: 14.971321227182894
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0411226679791445
-  a: -0.4022581636615678
+  v: 4.0266310473206266
+  a: 0.031347874981083712
   relative_time: 3.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587709.596175588
-    y: 4141440.8451360948
-    theta: -0.74511284146901213
-    kappa: -0.07187085390097446
-    s: 15.89829156298469
+    x: 587709.19309575134
+    y: 4141441.1798818903
+    theta: -0.711799030229229
+    kappa: -0.068914586839815675
+    s: 15.374337269048356
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.005521361712864
-  a: -0.30913423851749255
+  v: 4.0356497576068735
+  a: 0.14891432964692453
   relative_time: 3.2000000000000024
 }
 trajectory_point {
   path_point {
-    x: 587709.87175249215
-    y: 4141440.5573151661
-    theta: -0.77528228750698336
-    kappa: -0.073651645031939841
-    s: 16.297457430829148
+    x: 587709.50428193994
+    y: 4141440.9214510312
+    theta: -0.73752672099502314
+    kappa: -0.071196887753549681
+    s: 15.778841104144032
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9794010191082121
-  a: -0.21285434839643835
+  v: 4.0563599105998733
+  a: 0.26486070480862756
   relative_time: 3.3000000000000025
 }
 trajectory_point {
   path_point {
-    x: 587710.13753030589
-    y: 4141440.2623541118
-    theta: -0.80666187384563715
-    kappa: -0.075298291279252735
-    s: 16.694496307870615
+    x: 587709.79713660933
+    y: 4141440.6401241077
+    theta: -0.76648909030295576
+    kappa: -0.073189356770544986
+    s: 16.185990616100007
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9630125992726528
-  a: -0.11471124110309727
+  v: 4.0885046910569587
+  a: 0.37729085462112688
   relative_time: 3.4000000000000026
 }
 trajectory_point {
   path_point {
-    x: 587710.40254056081
-    y: 4141439.9682448967
-    theta: -0.83799549020490061
-    kappa: -0.076940182058889778
-    s: 17.090388547892193
+    x: 587710.07220504747
+    y: 4141440.3348522889
+    theta: -0.79894258633819726
+    kappa: -0.074893563773684516
+    s: 16.596908526292861
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9564777865331795
-  a: -0.015997664442161552
+  v: 4.1316376691509564
+  a: 0.48430863323935669
   relative_time: 3.5000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587710.66744332446
-    y: 4141439.6742549753
-    theta: -0.86929950374641984
-    kappa: -0.078581406868339213
-    s: 17.48612020937065
+    x: 587710.35051137058
+    y: 4141440.0259870552
+    theta: -0.8318431931643574
+    kappa: -0.076617831334139921
+    s: 17.012663432387242
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.959788990436317
-  a: 0.081993633781676567
+  v: 4.1851228004701859
+  a: 0.58401789481825106
   relative_time: 3.6000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587710.93289382942
-    y: 4141439.3796571693
-    theta: -0.90058989311941473
-    kappa: -0.080226025249909946
-    s: 17.882670127998388
+    x: 587710.63272197451
+    y: 4141439.7127888389
+    theta: -0.86519956079490778
+    kappa: -0.078366288158210742
+    s: 17.434250846877404
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9728093457481215
-  a: 0.17796990576372573
+  v: 4.24813442601846
+  a: 0.67452249351274451
   relative_time: 3.7000000000000028
 }
 trajectory_point {
   path_point {
-    x: 587711.16142018023
-    y: 4141439.0545627563
-    theta: -0.93334272607082847
-    kappa: -0.080882189882681629
-    s: 18.280996989205395
+    x: 587710.91944163956
+    y: 4141439.3945864518
+    theta: -0.899007034175646
+    kappa: -0.0801426812056102
+    s: 17.862574235628752
     dkappa: 0
     ddkappa: 0
   }
-  v: 3.9952727124541796
-  a: 0.27063840369929293
+  v: 4.3196572722150863
+  a: 0.75392628347777091
   relative_time: 3.8000000000000029
 }
 trajectory_point {
   path_point {
-    x: 587711.37894536729
-    y: 4141438.71765419
-    theta: -0.96688811630531957
-    kappa: -0.081214859935585818
-    s: 18.682026400681185
+    x: 587711.17087400332
+    y: 4141439.0399204339
+    theta: -0.93479844578750049
+    kappa: -0.080896647996092363
+    s: 18.2984260564194
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0267836757596083
-  a: 0.35870637978368614
+  v: 4.3984864508948638
+  a: 0.82033311886826454
   relative_time: 3.900000000000003
 }
 trajectory_point {
   path_point {
-    x: 587711.59841357521
-    y: 4141438.3777362239
-    theta: -1.0007899136059948
-    kappa: -0.081550501528500172
-    s: 19.086637964896759
+    x: 587711.41173035349
+    y: 4141438.6668759668
+    theta: -0.97195074724870567
+    kappa: -0.081264999338746535
+    s: 18.742468797481713
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.0668175460890552
-  a: 0.44088108621221428
+  v: 4.4832274593080852
+  a: 0.87184685383915872
   relative_time: 4.0000000000000027
 }
 trajectory_point {
   path_point {
-    x: 587711.820270432
-    y: 4141438.0341186575
-    theta: -1.035039512950849
-    kappa: -0.081889796178860857
-    s: 19.495653242235893
+    x: 587711.65731122158
+    y: 4141438.2865140447
+    theta: -1.0098877118715479
+    kappa: -0.081640576077542062
+    s: 19.195221659388849
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1147572077128416
-  a: 0.51703644663369519
+  v: 4.5725236122446766
+  a: 0.91347922250531743
   relative_time: 4.1000000000000023
 }
 trajectory_point {
   path_point {
-    x: 587712.04492944758
-    y: 4141437.6861610357
-    theta: -1.0696183630331175
-    kappa: -0.082233376283657914
-    s: 19.909834579024992
+    x: 587711.90784536966
+    y: 4141437.8984803571
+    theta: -1.0485356585305454
+    kappa: -0.082023728067419838
+    s: 19.657106387710691
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.1700526347607907
-  a: 0.58802578740506994
+  v: 4.6658062713952475
+  a: 0.9516006187544781
   relative_time: 4.200000000000002
 }
 trajectory_point {
   path_point {
-    x: 587712.2248523389
-    y: 4141437.3073563934
-    theta: -1.1042728406108075
-    kappa: -0.081412546348050049
-    s: 20.329892015635309
+    x: 587712.14310556068
+    y: 4141437.4914065776
+    theta: -1.0876761115420572
+    kappa: -0.0819162001619802
+    s: 20.128504314612904
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.2321960430807675
-  a: 0.6540254647838295
+  v: 4.7627314317088132
+  a: 0.986352886400889
   relative_time: 4.3000000000000016
 }
 trajectory_point {
   path_point {
-    x: 587712.39801414963
-    y: 4141436.9174882681
-    theta: -1.1395004914007723
-    kappa: -0.080345671196664054
-    s: 20.756485695804191
+    x: 587712.33845688286
+    y: 4141437.0515794968
+    theta: -1.1273770581348734
+    kappa: -0.080712612197396949
+    s: 20.609763080975139
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.300697284146386
-  a: 0.71521183502746488
+  v: 4.8629692725158078
+  a: 1.0178778692587982
   relative_time: 4.4000000000000012
 }
 trajectory_point {
   path_point {
-    x: 587712.57407756138
-    y: 4141436.5210872833
-    theta: -1.1753435050882852
-    kappa: -0.079260918858712781
-    s: 21.190227630197612
+    x: 587712.537938905
+    y: 4141436.6024522809
+    theta: -1.16798878129384
+    kappa: -0.07948357438811833
+    s: 21.101198054829148
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.3750838450570058
-  a: 0.77176125439346721
+  v: 4.9662041575280957
+  a: 1.0463174111424547
   relative_time: 4.5000000000000009
 }
 trajectory_point {
   path_point {
-    x: 587712.75327217777
-    y: 4141436.1176364939
-    theta: -1.2117554870879657
-    kappa: -0.078156874712922431
-    s: 21.631683459972784
+    x: 587712.741667116
+    y: 4141436.1437649094
+    theta: -1.209401319727418
+    kappa: -0.078228375189347332
+    s: 21.603093749796948
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.45490084853774
-  a: 0.82385007913932728
+  v: 5.0721346348389647
+  a: 1.0718133558661056
   relative_time: 4.6000000000000005
 }
 trajectory_point {
   path_point {
-    x: 587712.8938676829
-    y: 4141435.6916032904
-    theta: -1.2447993300239637
-    kappa: -0.076228919904346853
-    s: 22.081374220340717
+    x: 587712.90306880767
+    y: 4141435.6585282478
+    theta: -1.2471807259999668
+    kappa: -0.076052381750391237
+    s: 22.115705243528957
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.5397110529394489
-  a: 0.87165466552253645
+  v: 5.1804734369231245
+  a: 1.0945075472440005
   relative_time: 4.7
 }
 trajectory_point {
   path_point {
-    x: 587713.01672540419
-    y: 4141435.2499698438
-    theta: -1.2766314028909227
-    kappa: -0.073871699877250452
-    s: 22.539778104128786
+    x: 587713.04338763352
+    y: 4141435.1541278241
+    theta: -1.28354613884539
+    kappa: -0.073360142768786468
+    s: 22.639259596142129
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.6290948522387421
-  a: 0.91535136980058562
+  v: 5.2909474806367136
+  a: 1.1145418290903866
   relative_time: 4.8
 }
 trajectory_point {
   path_point {
-    x: 587713.14203549852
-    y: 4141434.7995209163
-    theta: -1.3091327928464733
-    kappa: -0.07146742718834824
-    s: 23.007332225343319
+    x: 587713.18669300224
+    y: 4141434.6389917526
+    theta: -1.3207108697117458
+    kappa: -0.0706106022289441
+    s: 23.173957268658103
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.7226502760379763
-  a: 0.95511654823096559
+  v: 5.4032978672172929
+  a: 1.1320580452195126
   relative_time: 4.8999999999999995
 }
 trajectory_point {
   path_point {
-    x: 587713.26990458078
-    y: 4141434.3398732613
-    theta: -1.3422624980814939
-    kappa: -0.0690140562634024
-    s: 23.484434382732172
+    x: 587713.33303189126
+    y: 4141434.1129511646
+    theta: -1.3585829674730585
+    kappa: -0.067802858798218124
+    s: 23.719973541441359
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.8199929895652618
-  a: 0.99112655707116781
+  v: 5.5172798822838489
+  a: 1.1471980394456267
   relative_time: 4.9999999999999991
 }
 trajectory_point {
   path_point {
-    x: 587713.37463938224
-    y: 4141433.865341967
-    theta: -1.374371923534621
-    kappa: -0.066282147340891509
-    s: 23.971444823347291
+    x: 587713.41638917779
+    y: 4141433.5621882081
+    theta: -1.3930564598635593
+    kappa: -0.064353211654271755
+    s: 24.277459932637328
     dkappa: 0
     ddkappa: 0
   }
-  v: 4.9207562936744536
-  a: 1.023557752578683
+  v: 5.6326629958367924
+  a: 1.1601036555829776
   relative_time: 5.0999999999999988
 }
 trajectory_point {
   path_point {
-    x: 587713.44247852033
-    y: 4141433.3727481849
-    theta: -1.4047440008588779
-    kappa: -0.063147824586765308
-    s: 24.468688006107303
+    x: 587713.49402982462
+    y: 4141432.9984236779
+    theta: -1.4278500154056935
+    kappa: -0.060766036867029541
+    s: 24.846545616610566
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.0245911248451574
-  a: 1.0525864910110023
+  v: 5.7492308622579591
+  a: 1.1709167374458127
   relative_time: 5.1999999999999984
 }
 trajectory_point {
   path_point {
-    x: 587713.51175334072
-    y: 4141432.8697296223
-    theta: -1.4357941073553522
-    kappa: -0.059947170040328054
-    s: 24.976454365360073
+    x: 587713.57326773717
+    y: 4141432.4230610752
+    theta: -1.4633478234752024
+    kappa: -0.057105064759023633
+    s: 25.427338842382873
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.1311660551827289
-  a: 1.0783891286256169
+  v: 5.8667813203106114
+  a: 1.1797791288483812
   relative_time: 5.299999999999998
 }
 trajectory_point {
   path_point {
-    x: 587713.582499066
-    y: 4141432.3560305187
-    theta: -1.4674785467486842
-    kappa: -0.056678556331710789
-    s: 25.495002074445306
+    x: 587713.62718663574
+    y: 4141431.8338032
+    theta: -1.4970392978954512
+    kappa: -0.053444883587424788
+    s: 26.019928352071464
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.2401672924182732
-  a: 1.1011420216800176
+  v: 5.9851263931394341
+  a: 1.1868326736049308
   relative_time: 5.3999999999999977
 }
 trajectory_point {
   path_point {
-    x: 587713.62731100479
-    y: 4141431.8291744133
-    theta: -1.4972756139134551
-    kappa: -0.05341711184384948
-    s: 26.02455880925708
+    x: 587713.64342169021
+    y: 4141431.2295648204
+    theta: -1.5279144288998092
+    kappa: -0.049819580775475589
+    s: 26.624384799327075
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.3512986799086413
-  a: 1.1210215264316954
+  v: 6.1040922882705377
+  a: 1.19221921552971
   relative_time: 5.4999999999999973
 }
 trajectory_point {
   path_point {
-    x: 587713.64183536707
-    y: 4141431.2886048006
-    theta: -1.5248960356095247
-    kappa: -0.050173808201947612
-    s: 26.565323511806472
+    x: 587713.6599769278
+    y: 4141430.61340982
+    theta: -1.5594130633828029
+    kappa: -0.046122780754247944
+    s: 27.240762167772136
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.4642816966364363
-  a: 1.1382039991381414
+  v: 6.223519397611458
+  a: 1.1960805984369671
   relative_time: 5.599999999999997
 }
 trajectory_point {
   path_point {
-    x: 587713.65666538232
-    y: 4141430.7366593541
-    theta: -1.5531141774412172
-    kappa: -0.046862251956245674
-    s: 27.117468153784085
+    x: 587713.66861089214
+    y: 4141429.9854330332
+    theta: -1.590118797710306
+    kappa: -0.0424421836621175
+    s: 27.8690991894389
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.57885545721001
-  a: 1.1528657960568467
+  v: 6.3432622974511546
+  a: 1.1985586661409502
   relative_time: 5.6999999999999966
 }
 trajectory_point {
   path_point {
-    x: 587713.6718049926
-    y: 4141430.1731913611
-    theta: -1.5818922649077993
-    kappa: -0.043481562865783752
-    s: 27.681139500122658
+    x: 587713.63068440219
+    y: 4141429.3462356464
+    theta: -1.6137072710261944
+    kappa: -0.039189857032667655
+    s: 28.509420763207579
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.6947767118634633
-  a: 1.1651832734453025
+  v: 6.4631897484600129
+  a: 1.1997952624559078
   relative_time: 5.7999999999999963
 }
 trajectory_point {
   path_point {
-    x: 587713.64566731267
-    y: 4141429.5987514257
-    theta: -1.6043858614247384
-    kappa: -0.040474692999844158
-    s: 28.256460872559625
+    x: 587713.59204732324
+    y: 4141428.6950622867
+    theta: -1.6377486526772722
+    kappa: -0.035876594957454142
+    s: 29.161739373244504
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.811819846456646
-  a: 1.1753327875609996
+  v: 6.5831846956898428
+  a: 1.1999322311960878
   relative_time: 5.8999999999999959
 }
 trajectory_point {
   path_point {
-    x: 587713.61089475336
-    y: 4141429.0127090858
-    theta: -1.6260214495737382
-    kappa: -0.037492826759935892
-    s: 28.843533913199686
+    x: 587713.54921011708
+    y: 4141428.0322341649
+    theta: -1.6618160540152747
+    kappa: -0.032554956636392275
+    s: 29.82605650744026
     dkappa: 0
     ddkappa: 0
   }
-  v: 5.9297768824751538
-  a: 1.1834906946614288
+  v: 6.7031442685738787
+  a: 1.1991114161757388
   relative_time: 5.9999999999999956
 }
 trajectory_point {
   path_point {
-    x: 587713.57542129781
-    y: 4141428.4148541274
-    theta: -1.6480904941771986
-    kappa: -0.034450856246218145
-    s: 29.4424403480774
+    x: 587713.46477991529
+    y: 4141427.3612174233
+    theta: -1.6816425621956859
+    kappa: -0.029788294602068457
+    s: 30.502364075847815
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.048457477030337
-  a: 1.1898333510040815
+  v: 6.82297978092678
+  a: 1.1974746612091092
   relative_time: 6.0999999999999952
 }
 trajectory_point {
   path_point {
-    x: 587713.52084807341
-    y: 4141427.8068242315
-    theta: -1.6684745736046538
-    kappa: -0.031625571324108967
-    s: 30.053243750719737
+    x: 587713.37885485683
+    y: 4141426.6783201713
+    theta: -1.701826410076426
+    kappa: -0.026972648164484372
+    s: 31.190645829120687
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.1676889228592939
-  a: 1.1945371128464488
+  v: 6.9426167309446321
+  a: 1.1951638101104469
   relative_time: 6.1999999999999948
 }
 trajectory_point {
   path_point {
-    x: 587713.44310430088
-    y: 4141427.1889484972
-    theta: -1.6867342152010891
-    kappa: -0.02907801441680908
-    s: 30.675991305708667
+    x: 587713.28565464937
+    y: 4141425.9844386219
+    theta: -1.7215548104598135
+    kappa: -0.024208563456498106
+    s: 31.890878776951059
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.2873161483248667
-  a: 1.1977783364460215
+  v: 7.0619948012049427
+  a: 1.1923207066940003
   relative_time: 6.2999999999999945
 }
 trajectory_point {
   path_point {
-    x: 587713.36386535643
-    y: 4141426.5591897471
-    theta: -1.7053468935150127
-    kappa: -0.026481462770016954
-    s: 31.31071557224374
+    x: 587713.16193062568
+    y: 4141425.2831125269
+    theta: -1.7376341677854144
+    kappa: -0.02190004121902013
+    s: 32.603034606507954
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.4072017174156537
-  a: 1.1997333780602906
+  v: 7.1810678586666468
+  a: 1.1890871947740178
   relative_time: 6.3999999999999941
 }
 trajectory_point {
   path_point {
-    x: 587713.27409150871
-    y: 4141425.9188932888
-    theta: -1.7230572656835434
-    kappa: -0.023992810955531376
-    s: 31.957436247704642
+    x: 587713.03614081687
+    y: 4141424.5700765881
+    theta: -1.7539839791038421
+    kappa: -0.019552974237549085
+    s: 33.327081100875354
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.527225829745996
-  a: 1.2005785939467475
+  v: 7.2998039546701028
+  a: 1.1856051181647478
   relative_time: 6.4999999999999938
 }
 trajectory_point {
   path_point {
-    x: 587713.15964999353
-    y: 4141425.2701848289
-    theta: -1.7379306062264208
-    kappa: -0.021857487721956361
-    s: 32.616161931213782
+    x: 587712.89790087962
+    y: 4141423.8473938424
+    theta: -1.7683674016302346
+    kappa: -0.017363793874156618
+    s: 34.062983557490334
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.64728632055599
-  a: 1.2004903403628826
+  v: 7.4181853249370935
+  a: 1.1820163206804384
   relative_time: 6.5999999999999934
 }
 trajectory_point {
   path_point {
-    x: 587713.04312295618
-    y: 4141424.6096546459
-    theta: -1.7530765548273484
-    kappa: -0.019683251474114892
-    s: 33.286891887198877
+    x: 587712.7412579041
+    y: 4141423.1162631167
+    theta: -1.779510866765992
+    kappa: -0.015445209595623278
+    s: 34.810706206581237
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.7672986607114778
-  a: 1.1996449735661872
+  v: 7.53620838957083
+  a: 1.1784626461353376
   relative_time: 6.6999999999999931
 }
 trajectory_point {
   path_point {
-    x: 587712.91746038967
-    y: 4141423.93868781
-    theta: -1.7669760802886778
-    kappa: -0.017603361389611993
-    s: 33.969617808955505
+    x: 587712.58214609581
+    y: 4141422.373609121
+    theta: -1.7908298027846714
+    kappa: -0.013496386722575436
+    s: 35.570213629605789
     dkappa: 0
     ddkappa: 0
   }
-  v: 6.8871959567040495
-  a: 1.1982188498141528
+  v: 7.6538837530559425
+  a: 1.1750859383436938
   relative_time: 6.7999999999999927
 }
 trajectory_point {
   path_point {
-    x: 587712.77192368184
-    y: 4141423.259395564
-    theta: -1.7773292503961069
-    kappa: -0.015820808174779061
-    s: 34.664325582209692
+    x: 587712.40608567733
+    y: 4141421.6227668868
+    theta: -1.8000846235365042
+    kappa: -0.011817815688465978
+    s: 36.341472177689241
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.0069289506510453
-  a: 1.19638832536427
+  v: 7.771236204258491
+  a: 1.1720280411197554
   relative_time: 6.8999999999999924
 }
 trajectory_point {
   path_point {
-    x: 587712.62388065888
-    y: 4141422.5684050974
-    theta: -1.7878610555840866
-    kappa: -0.01400755727578944
-    s: 35.370997048680493
+    x: 587712.22212592175
+    y: 4141420.8617049605
+    theta: -1.8086736874611231
+    kappa: -0.010222057965219423
+    s: 37.124451390062546
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.1264660202955579
-  a: 1.19432975647403
+  v: 7.8883047164259574
+  a: 1.1694307982777712
   relative_time: 6.999999999999992
 }
 trajectory_point {
   path_point {
-    x: 587712.46525989089
-    y: 4141421.8675771756
-    theta: -1.7973218723215447
-    kappa: -0.012331122025300045
-    s: 36.089611769642531
+    x: 587712.03285987594
+    y: 4141420.0899184276
+    theta: -1.816893701092511
+    kappa: -0.00866429605577612
+    s: 37.919125412500463
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.2457931790064221
-  a: 1.1922194994009234
+  v: 8.00514244718725
+  a: 1.1674360536319885
   relative_time: 7.0999999999999917
 }
 trajectory_point {
   path_point {
-    x: 587712.29362134729
-    y: 4141421.1574894711
-    theta: -1.8053355706884695
-    kappa: -0.0108422445773236
-    s: 36.820148789488655
+    x: 587711.829356098
+    y: 4141419.3096717186
+    theta: -1.8230097379115646
+    kappa: -0.0073605170049422658
+    s: 38.725474415759706
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.3649140757782288
-  a: 1.1902339104024413
+  v: 8.1218167385527025
+  a: 1.1661856509966562
   relative_time: 7.1999999999999913
 }
 trajectory_point {
   path_point {
-    x: 587712.1191863087
-    y: 4141420.4358323561
-    theta: -1.813479760954241
-    kappa: -0.0093291089503925052
-    s: 37.562588399292409
+    x: 587711.6229089516
+    y: 4141418.5181399435
+    theta: -1.8292142586739832
+    kappa: -0.0060378808004745658
+    s: 39.543486014017105
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.4838499952313136
-  a: 1.1885493457360756
+  v: 8.23840911691407
+  a: 1.1658214341860227
   relative_time: 7.2999999999999909
 }
 trajectory_point {
   path_point {
-    x: 587711.93246729183
-    y: 4141419.7050067363
-    theta: -1.8199108353507383
-    kappa: -0.0080211151315711035
-    s: 38.316913900370686
+    x: 587711.40686318453
+    y: 4141417.7171033826
+    theta: -1.8337950425847935
+    kappa: -0.0049071544359833887
+    s: 40.373156683307734
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.6026398576117638
-  a: 1.1873421616593169
+  v: 8.3550152930445378
+  a: 1.1664852470143354
   relative_time: 7.3999999999999906
 }
 trajectory_point {
   path_point {
-    x: 587711.73909632512
-    y: 4141418.9636098836
-    theta: -1.825722411652736
-    kappa: -0.0067822535339043888
-    s: 39.083113367846259
+    x: 587711.18520924752
+    y: 4141416.9054897837
+    theta: -1.837779174525461
+    kappa: -0.0038419186229125277
+    s: 41.214493179963043
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.7213402187914122
-  a: 1.1867887144296554
+  v: 8.4717451620987116
+  a: 1.1683189332958435
   relative_time: 7.49999999999999
 }
 trajectory_point {
   path_point {
-    x: 587711.541745404
-    y: 4141418.2109914897
-    theta: -1.8313706153562286
-    kappa: -0.0055553782967301818
-    s: 39.861181414210385
+    x: 587710.95871774375
+    y: 4141416.0830910038
+    theta: -1.8412159467399434
+    kappa: -0.0028494157231537965
+    s: 42.06751395904903
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.8400252702678461
-  a: 1.1870653603045831
+  v: 8.5887228036126189
+  a: 1.171464336844795
   relative_time: 7.59999999999999
 }
 trajectory_point {
   path_point {
-    x: 587711.33363223006
-    y: 4141417.448959088
-    theta: -1.8351113363208413
-    kappa: -0.0045552173740761957
-    s: 40.65112095288535
+    x: 587710.72574574547
+    y: 4141415.250328437
+    theta: -1.8435456230112122
+    kappa: -0.0020109294812791808
+    s: 42.932250592804373
     dkappa: 0
     ddkappa: 0
   }
-  v: 7.9587868391643983
-  a: 1.1883484555415911
+  v: 8.7060864815037267
+  a: 1.176063301475438
   relative_time: 7.6999999999999895
 }
 trajectory_point {
   path_point {
-    x: 587711.1223880382
-    y: 4141416.6754620732
-    theta: -1.8389083515132187
-    kappa: -0.0035400092457497307
-    s: 41.45294496178709
+    x: 587710.48900899128
+    y: 4141414.4064063663
+    theta: -1.8454273354966326
+    kappa: -0.001227939696977869
+    s: 43.808749189078547
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.077734388230148
-  a: 1.190814356398169
+  v: 8.823988644070905
+  a: 1.1822576710020209
   relative_time: 7.7999999999999892
 }
 trajectory_point {
   path_point {
-    x: 587710.9050601325
-    y: 4141415.8912909147
-    theta: -1.8417525121364642
-    kappa: -0.0026562973693027964
-    s: 42.266678246887707
+    x: 587710.24744384224
+    y: 4141413.5515593877
+    theta: -1.8460186087367789
+    kappa: -0.0006179190847698787
+    s: 44.69707180977
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.1969950158399332
-  a: 1.1946394191318095
+  v: 8.9425959239944675
+  a: 1.1901892892387922
   relative_time: 7.8999999999999888
 }
 trajectory_point {
   path_point {
-    x: 587710.68261027266
-    y: 4141415.096139919
-    theta: -1.8439769695649737
-    kappa: -0.0018556812080551385
-    s: 43.092359205778088
+    x: 587710.00263859716
+    y: 4141412.6852584081
+    theta: -1.8465927120918897
+    kappa: -6.4440222083405349e-06
+    s: 45.59729788926429
     dkappa: 0
     ddkappa: 0
   }
-  v: 8.3167134559943285
-  a: 1.2000000000000033
+  v: 9.0620891383361446
+  a: 1.1999999999999995
   relative_time: 7.9999999999999885
 }
 decision {