diff --git a/modules/planning/common/path/path_data.cc b/modules/planning/common/path/path_data.cc
index 5c76e86ee9d1bcbce088f092e99336e05ee21644..111d8b490df2fde141c90eef2099521a6d42567d 100644
--- a/modules/planning/common/path/path_data.cc
+++ b/modules/planning/common/path/path_data.cc
@@ -185,11 +185,14 @@ bool PathData::SLToXY(const FrenetFramePath &frenet_path,
 
     if (path_points.empty()) {
       path_point.set_s(0.0);
+      path_point.set_dkappa(0.0);
     } else {
       common::math::Vec2d last(path_points.back().x(), path_points.back().y());
       common::math::Vec2d current(path_point.x(), path_point.y());
       double distance = (last - current).Length();
       path_point.set_s(path_points.back().s() + distance);
+      path_point.set_dkappa((path_point.kappa() - path_points.back().kappa()) /
+                            distance);
     }
     path_points.push_back(std::move(path_point));
   }
diff --git a/modules/planning/testdata/garage_test/result_follow_0.pb.txt b/modules/planning/testdata/garage_test/result_follow_0.pb.txt
index 4e4681745918914811076098afaaa6374a9cd003..b16e084c7721d79c01a6664ad3018dc4671069d2 100644
--- a/modules/planning/testdata/garage_test/result_follow_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_follow_0.pb.txt
@@ -27,7 +27,7 @@ trajectory_point {
     theta: 1.3235162828684548
     kappa: -0.025155282755943451
     s: 0.1120030871188539
-    dkappa: 0
+    dkappa: 0.00044809232925312973
     ddkappa: 0
   }
   v: 5.6000060213297109
@@ -41,7 +41,7 @@ trajectory_point {
     theta: 1.3215317394927251
     kappa: -0.024260490549704149
     s: 0.22398837012847977
-    dkappa: 0
+    dkappa: 0.0008961134293555326
     ddkappa: 0
   }
   v: 5.5982314936159341
@@ -55,7 +55,7 @@ trajectory_point {
     theta: 1.3195480219652069
     kappa: -0.023366119435069024
     s: 0.33592095256456705
-    dkappa: 0
+    dkappa: 0.0013439236895305964
     ddkappa: 0
   }
   v: 5.5947477853589076
@@ -69,7 +69,7 @@ trajectory_point {
     theta: 1.3175657393818931
     kappa: -0.022472436909353029
     s: 0.44776735652877203
-    dkappa: 0
+    dkappa: 0.0017913891742785666
     ddkappa: 0
   }
   v: 5.5896252892876488
@@ -83,7 +83,7 @@ trajectory_point {
     theta: 1.3155854761182095
     kappa: -0.021579699306325265
     s: 0.55949550126594472
-    dkappa: 0
+    dkappa: 0.0022383815376701552
     ddkappa: 0
   }
   v: 5.5829333269925216
@@ -97,7 +97,7 @@ trajectory_point {
     theta: 1.3136077921580009
     kappa: -0.020688151967382662
     s: 0.671074681741356
-    dkappa: 0
+    dkappa: 0.0026847779376401528
     ddkappa: 0
   }
   v: 5.5747401489252395
@@ -111,7 +111,7 @@ trajectory_point {
     theta: 1.3116332234256975
     kappa: -0.0197980294127236
     s: 0.78247554721792456
-    dkappa: 0
+    dkappa: 0.0031304609502810364
     ddkappa: 0
   }
   v: 5.5651129343988641
@@ -125,7 +125,7 @@ trajectory_point {
     theta: 1.3096622821216672
     kappa: -0.018909555512521589
     s: 0.89367007983344349
-    dkappa: 0
+    dkappa: 0.0035753184841365803
     ddkappa: 0
   }
   v: 5.5541177915878031
@@ -139,7 +139,7 @@ trajectory_point {
     theta: 1.3076954570607564
     kappa: -0.018022943658098926
     s: 1.0046315731778079
-    dkappa: 0
+    dkappa: 0.0040192436944954668
     ddkappa: 0
   }
   v: 5.541819757527815
@@ -153,7 +153,7 @@ trajectory_point {
     theta: 1.305733214013999
     kappa: -0.01713839693310033
     s: 1.1153346108702413
-    dkappa: 0
+    dkappa: 0.0044621348976848956
     ddkappa: 0
   }
   v: 5.5282827981160034
@@ -167,7 +167,7 @@ trajectory_point {
     theta: 1.3037759960534634
     kappa: -0.016256108284666636
     s: 1.2257550451365229
-    dkappa: 0
+    dkappa: 0.00490389548536419
     ddkappa: 0
   }
   v: 5.5135698081108231
@@ -181,7 +181,7 @@ trajectory_point {
     theta: 1.3018242239001976
     kappa: -0.015376260694608405
     s: 1.3358699753862144
-    dkappa: 0
+    dkappa: 0.0053444338388184166
     ddkappa: 0
   }
   v: 5.4977426111320726
@@ -195,7 +195,7 @@ trajectory_point {
     theta: 1.299878296275216
     kappa: -0.014499027350579617
     s: 1.4456577267898871
-    dkappa: 0
+    dkappa: 0.005783663243251985
     ddkappa: 0
   }
   v: 5.4808619596609036
@@ -209,7 +209,7 @@ trajectory_point {
     theta: 1.2979385902534684
     kappa: -0.013624571817251314
     s: 1.5550978288563488
-    dkappa: 0
+    dkappa: 0.0062215018020822634
     ddkappa: 0
   }
   v: 5.4629875350398125
@@ -223,7 +223,7 @@ trajectory_point {
     theta: 1.2960054616207184
     kappa: -0.012753048207485252
     s: 1.6641709940098701
-    dkappa: 0
+    dkappa: 0.0066578723512331836
     ddkappa: 0
   }
   v: 5.4441779474726433
@@ -237,7 +237,7 @@ trajectory_point {
     theta: 1.2940792452332532
     kappa: -0.01188460135350757
     s: 1.7728590961674127
-    dkappa: 0
+    dkappa: 0.0070927023734288584
     ddkappa: 0
   }
   v: 5.4244907360245893
@@ -251,7 +251,7 @@ trajectory_point {
     theta: 1.2921602553803357
     kappa: -0.011019366978082425
     s: 1.8811451493158557
-    dkappa: 0
+    dkappa: 0.0075259239124871877
     ddkappa: 0
   }
   v: 5.4039823686221933
@@ -265,7 +265,7 @@ trajectory_point {
     theta: 1.2902487861493113
     kappa: -0.010157471865685679
     s: 1.9890132860892218
-    dkappa: 0
+    dkappa: 0.0079574734876134649
     ddkappa: 0
   }
   v: 5.3827082420533428
@@ -279,7 +279,7 @@ trajectory_point {
     theta: 1.2896277332875878
     kappa: -0.0095865814297870552
     s: 2.0964487363459057
-    dkappa: 0
+    dkappa: 0.0078465527139380091
     ddkappa: 0
   }
   v: 5.3607226819672746
@@ -293,7 +293,7 @@ trajectory_point {
     theta: 1.2891147623171058
     kappa: -0.009041714427061847
     s: 2.2034378057458994
-    dkappa: 0
+    dkappa: 0.0076916157137237942
     ddkappa: 0
   }
   v: 5.3380789428745761
@@ -307,7 +307,7 @@ trajectory_point {
     theta: 1.2886039901813613
     kappa: -0.008499185095826119
     s: 2.3099678543280215
-    dkappa: 0
+    dkappa: 0.00753734344785072
     ddkappa: 0
   }
   v: 5.3148292081471773
@@ -321,7 +321,7 @@ trajectory_point {
     theta: 1.2880954728223841
     kappa: -0.0079590525474837852
     s: 2.4160272750871417
-    dkappa: 0
+    dkappa: 0.0073837527250878183
     ddkappa: 0
   }
   v: 5.2910245900183623
@@ -335,7 +335,7 @@ trajectory_point {
     theta: 1.2875892613258604
     kappa: -0.007421370741967602
     s: 2.5216054725514083
-    dkappa: 0
+    dkappa: 0.0072308588893448328
     ddkappa: 0
   }
   v: 5.2667151295827583
@@ -349,7 +349,7 @@ trajectory_point {
     theta: 1.2870854020235825
     kappa: -0.0068861885968396572
     s: 2.6266928413594779
-    dkappa: 0
+    dkappa: 0.0070786758506957651
     ddkappa: 0
   }
   v: 5.2419497967963444
@@ -363,7 +363,7 @@ trajectory_point {
     theta: 1.2865839365959431
     kappa: -0.0063535500963919165
     s: 2.7312807448377363
-    dkappa: 0
+    dkappa: 0.0069272161164024206
     ddkappa: 0
   }
   v: 5.2167764904764429
@@ -377,7 +377,7 @@ trajectory_point {
     theta: 1.2860849021744727
     kappa: -0.0058234944007466939
     s: 2.8353614935775338
-    dkappa: 0
+    dkappa: 0.0067764908219379513
     ddkappa: 0
   }
   v: 5.1912420383017306
@@ -391,7 +391,7 @@ trajectory_point {
     theta: 1.2855883314444196
     kappa: -0.0052960559549571923
     s: 2.9389283240124047
-    dkappa: 0
+    dkappa: 0.0066265097620104019
     ddkappa: 0
   }
   v: 5.1653921968122258
@@ -405,7 +405,7 @@ trajectory_point {
     theta: 1.285094252747367
     kappa: -0.0047712645981080049
     s: 3.041975376995298
-    dkappa: 0
+    dkappa: 0.0064772814215862569
     ddkappa: 0
   }
   v: 5.139271651409298
@@ -419,7 +419,7 @@ trajectory_point {
     theta: 1.2846026901838894
     kappa: -0.0042491456724156268
     s: 3.1444976763758028
-    dkappa: 0
+    dkappa: 0.0063288130069139824
     ddkappa: 0
   }
   v: 5.1129240163556666
@@ -433,7 +433,7 @@ trajectory_point {
     theta: 1.2841136637162431
     kappa: -0.0037297201323289587
     s: 3.2464911075773779
-    dkappa: 0
+    dkappa: 0.0061811104765475688
     ddkappa: 0
   }
   v: 5.0863918347753945
@@ -447,7 +447,7 @@ trajectory_point {
     theta: 1.283627189271088
     kappa: -0.0032130046536298319
     s: 3.3479523961745747
-    dkappa: 0
+    dkappa: 0.0060341785723700813
     ddkappa: 0
   }
   v: 5.0597165786538962
@@ -461,7 +461,7 @@ trajectory_point {
     theta: 1.2831432788422421
     kappa: -0.0026990117425335063
     s: 3.4488790864702681
-    dkappa: 0
+    dkappa: 0.0058880208506172024
     ddkappa: 0
   }
   v: 5.0329386488379333
@@ -475,7 +475,7 @@ trajectory_point {
     theta: 1.2826619405934634
     kappa: -0.0021877498447891862
     s: 3.549269520072881
-    dkappa: 0
+    dkappa: 0.0057426397129007713
     ddkappa: 0
   }
   v: 5.0060973750356137
@@ -489,7 +489,7 @@ trajectory_point {
     theta: 1.2821831789612586
     kappa: -0.0016792234547805369
     s: 3.6491228144736114
-    dkappa: 0
+    dkappa: 0.005598036437232343
     ddkappa: 0
   }
   v: 4.9792310158163957
@@ -503,7 +503,7 @@ trajectory_point {
     theta: 1.2817069947577164
     kappa: -0.0011734332246261889
     s: 3.7484388416236603
-    dkappa: 0
+    dkappa: 0.0054542112090467144
     ddkappa: 0
   }
   v: 4.9523767586110843
@@ -517,7 +517,7 @@ trajectory_point {
     theta: 1.2812333852733635
     kappa: -0.00067037607328024264
     s: 3.8472182065114593
-    dkappa: 0
+    dkappa: 0.0053111631522254829
     ddkappa: 0
   }
   v: 4.9255707197118328
@@ -531,7 +531,7 @@ trajectory_point {
     theta: 1.2807623443800402
     kappa: -0.0001700452956327994
     s: 3.9454622257398948
-    dkappa: 0
+    dkappa: 0.0051688903601205894
     ddkappa: 0
   }
   v: 4.8988479442721422
@@ -545,7 +545,7 @@ trajectory_point {
     theta: 1.2806549638129328
     kappa: 0.00022253471129773339
     s: 4.0431729061035382
-    dkappa: 0
+    dkappa: 0.0050402979705601132
     ddkappa: 0
   }
   v: 4.8722424063068628
@@ -559,7 +559,7 @@ trajectory_point {
     theta: 1.2809667186759246
     kappa: 0.000491237330296744
     s: 4.1403529231658709
-    dkappa: 0
+    dkappa: 0.0049273656356173248
     ddkappa: 0
   }
   v: 4.8457870086921915
@@ -573,7 +573,7 @@ trajectory_point {
     theta: 1.2812767823550715
     kappa: 0.00075848185380827764
     s: 4.2370055998365119
-    dkappa: 0
+    dkappa: 0.0048150461198892634
     ddkappa: 0
   }
   v: 4.8195135831656728
@@ -587,7 +587,7 @@ trajectory_point {
     theta: 1.281585167450527
     kappa: 0.0010242792003503198
     s: 4.3331348849484428
-    dkappa: 0
+    dkappa: 0.004703334834459995
     ddkappa: 0
   }
   v: 4.7934528903262006
@@ -601,7 +601,7 @@ trajectory_point {
     theta: 1.2818918885037465
     kappa: 0.0012886419598841998
     s: 4.4287453318352386
-    dkappa: 0
+    dkappa: 0.004592226487926945
     ddkappa: 0
   }
   v: 4.7676346196340171
@@ -615,7 +615,7 @@ trajectory_point {
     theta: 1.2821969619287186
     kappa: 0.0015515843345806498
     s: 4.5238420769082941
-    dkappa: 0
+    dkappa: 0.004481715111296183
     ddkappa: 0
   }
   v: 4.7420873894107114
@@ -629,7 +629,7 @@ trajectory_point {
     theta: 1.2825004059431966
     kappa: 0.0018131220795858459
     s: 4.6184308182340441
-    dkappa: 0
+    dkappa: 0.0043717940828777091
     ddkappa: 0
   }
   v: 4.71683874683922
@@ -643,7 +643,7 @@ trajectory_point {
     theta: 1.2828022404999222
     kappa: 0.0020732724437874995
     s: 4.7125177941112009
-    dkappa: 0
+    dkappa: 0.0042624561531807246
     ddkappa: 0
   }
   v: 4.6919151679638285
@@ -657,7 +657,7 @@ trajectory_point {
     theta: 1.283102487217846
     kappa: 0.0023320541105808956
     s: 4.8061097616479733
-    dkappa: 0
+    dkappa: 0.0041536934698089157
     ddkappa: 0
   }
   v: 4.6673420576901705
@@ -671,7 +671,7 @@ trajectory_point {
     theta: 1.2834011693133445
     kappa: 0.0025894871386349621
     s: 4.8992139753392969
-    dkappa: 0
+    dkappa: 0.00404549760235574
     ddkappa: 0
   }
   v: 4.6431437497852279
@@ -685,7 +685,7 @@ trajectory_point {
     theta: 1.2836983115314338
     kappa: 0.0028455929026583292
     s: 4.99183816564406
-    dkappa: 0
+    dkappa: 0.0039378595672996978
     ddkappa: 0
   }
   v: 4.61934350687733
@@ -699,7 +699,7 @@ trajectory_point {
     theta: 1.2839939400769809
     kappa: 0.0031003940341653934
     s: 5.0839905175623317
-    dkappa: 0
+    dkappa: 0.0038307698528996185
     ddkappa: 0
   }
   v: 4.595963520456154
@@ -713,7 +713,7 @@ trajectory_point {
     theta: 1.2842880825459115
     kappa: 0.0033539143622423745
     s: 5.1756796492125892
-    dkappa: 0
+    dkappa: 0.0037242184440899372
     ddkappa: 0
   }
   v: 4.573024910872725
@@ -727,7 +727,7 @@ trajectory_point {
     theta: 1.2857375583047084
     kappa: 0.0046032143357449826
     s: 5.6275062566093794
-    dkappa: 0
+    dkappa: 0.00319915336070073
     ddkappa: 0
   }
   v: 4.4656167481020432
@@ -741,7 +741,7 @@ trajectory_point {
     theta: 1.2873575651963525
     kappa: 0.0056919018509300695
     s: 6.06925662830837
-    dkappa: 0
+    dkappa: 0.0026987379983968419
     ddkappa: 0
   }
   v: 4.3717951542039684
@@ -755,7 +755,7 @@ trajectory_point {
     theta: 1.2900362113687347
     kappa: 0.0060458251649115665
     s: 6.5023667506887035
-    dkappa: 0
+    dkappa: 0.0022776570118188092
     ddkappa: 0
   }
   v: 4.2929951420731953
@@ -769,7 +769,7 @@ trajectory_point {
     theta: 1.2926710255303007
     kappa: 0.0063939513041398176
     s: 6.9283826383361129
-    dkappa: 0
+    dkappa: 0.0018634732257585989
     ddkappa: 0
   }
   v: 4.2299822629469217
@@ -783,7 +783,7 @@ trajectory_point {
     theta: 1.2952717980538075
     kappa: 0.0067375788259914663
     s: 7.3488933878771814
-    dkappa: 0
+    dkappa: 0.0014546416789218131
     ddkappa: 0
   }
   v: 4.1828526064048548
@@ -797,7 +797,7 @@ trajectory_point {
     theta: 1.2978481746320178
     kappa: 0.0070779867868669974
     s: 7.7654642318135849
-    dkappa: 0
+    dkappa: 0.0010496406113403238
     ddkappa: 0
   }
   v: 4.1510328003692072
@@ -811,7 +811,7 @@ trajectory_point {
     theta: 1.300549938421047
     kappa: 0.0071212702498595478
     s: 8.1795695923563478
-    dkappa: 0
+    dkappa: 0.00066988122916517728
     ddkappa: 0
   }
   v: 4.1332800111046968
@@ -825,7 +825,7 @@ trajectory_point {
     theta: 1.3034359677764165
     kappa: 0.0067623016889114826
     s: 8.59252613526009
-    dkappa: 0
+    dkappa: 0.00032230472807457441
     ddkappa: 0
   }
   v: 4.1276819432185485
@@ -839,7 +839,7 @@ trajectory_point {
     theta: 1.3063216479704829
     kappa: 0.0064033825495811436
     s: 9.00542582365728
-    dkappa: 0
+    dkappa: -2.5223919819854234e-05
     ddkappa: 0
   }
   v: 4.1316568396604936
@@ -853,7 +853,7 @@ trajectory_point {
     theta: 1.3092125241544663
     kappa: 0.0060438171468297237
     s: 9.419068971892484
-    dkappa: 0
+    dkappa: -0.00037337832161899511
     ddkappa: 0
   }
   v: 4.1419534817227692
@@ -867,7 +867,7 @@ trajectory_point {
     theta: 1.3121117509601732
     kappa: 0.0056832070713271551
     s: 9.8339139090716579
-    dkappa: 0
+    dkappa: -0.00072254424291909173
     ddkappa: 0
   }
   v: 4.1553213301089151
@@ -881,7 +881,7 @@ trajectory_point {
     theta: 1.3144288501349395
     kappa: 0.0052474705873622289
     s: 10.250201943709515
-    dkappa: 0
+    dkappa: -0.00090615087040585274
     ddkappa: 0
   }
   v: 4.170751046833205
@@ -895,7 +895,7 @@ trajectory_point {
     theta: 1.3163267750095238
     kappa: 0.0047566004522323069
     s: 10.668120294661264
-    dkappa: 0
+    dkappa: -0.00096985898832460779
     ddkappa: 0
   }
   v: 4.1878667447271241
@@ -909,7 +909,7 @@ trajectory_point {
     theta: 1.3182327992470426
     kappa: 0.0042636384398174647
     s: 11.087819633786344
-    dkappa: 0
+    dkappa: -0.0010338386028270856
     ddkappa: 0
   }
   v: 4.2063133710438434
@@ -923,7 +923,7 @@ trajectory_point {
     theta: 1.3201474368000756
     kappa: 0.0037684480565019921
     s: 11.509416169390597
-    dkappa: 0
+    dkappa: -0.0010981074288866411
     ddkappa: 0
   }
   v: 4.2257567074582152
@@ -937,7 +937,7 @@ trajectory_point {
     theta: 1.3220710541921674
     kappa: 0.0032709308410505209
     s: 11.932993729668436
-    dkappa: 0
+    dkappa: -0.0011626782454025081
     ddkappa: 0
   }
   v: 4.2458833700667773
@@ -951,7 +951,7 @@ trajectory_point {
     theta: 1.3230103563158822
     kappa: 0.0027567126874095075
     s: 12.358605846145011
-    dkappa: 0
+    dkappa: -0.0011817086065563072
     ddkappa: 0
   }
   v: 4.2664008093877506
@@ -965,7 +965,7 @@ trajectory_point {
     theta: 1.3237303909806244
     kappa: 0.0022367819335746305
     s: 12.786277837118373
-    dkappa: 0
+    dkappa: -0.0011905023045514864
     ddkappa: 0
   }
   v: 4.2870373103610389
@@ -979,7 +979,7 @@ trajectory_point {
     theta: 1.3244538927087464
     kappa: 0.0017143479291048308
     s: 13.216008891101659
-    dkappa: 0
+    dkappa: -0.0011993383405501927
     ddkappa: 0
   }
   v: 4.3075419923482308
@@ -993,7 +993,7 @@ trajectory_point {
     theta: 1.3251808188920808
     kappa: 0.0011894408942398529
     s: 13.647774150265235
-    dkappa: 0
+    dkappa: -0.0012082162034311669
     ddkappa: 0
   }
   v: 4.3276848091325961
@@ -1007,7 +1007,7 @@ trajectory_point {
     theta: 1.3257830478706591
     kappa: 0.00067285931527401419
     s: 14.08152679387889
-    dkappa: 0
+    dkappa: -0.0012103559735276426
     ddkappa: 0
   }
   v: 4.3472565489190922
@@ -1021,7 +1021,7 @@ trajectory_point {
     theta: 1.3257040636385002
     kappa: 0.00021136153613816671
     s: 14.517200121753991
-    dkappa: 0
+    dkappa: -0.0011762984587743205
     ddkappa: 0
   }
   v: 4.3660688343343566
@@ -1035,7 +1035,7 @@ trajectory_point {
     theta: 1.3256247465184041
     kappa: -0.00025208127074199303
     s: 14.954709637685642
-    dkappa: 0
+    dkappa: -0.0011420974052780346
     ddkappa: 0
   }
   v: 4.3839541224267116
@@ -1049,7 +1049,7 @@ trajectory_point {
     theta: 1.325545114678444
     kappa: -0.0007173629567317325
     s: 15.393955132894877
-    dkappa: 0
+    dkappa: -0.00110776064657311
     ddkappa: 0
   }
   v: 4.4007657046661635
@@ -1063,7 +1063,7 @@ trajectory_point {
     theta: 1.3254651887575744
     kappa: -0.0011843629362002749
     s: 15.834822769470811
-    dkappa: 0
+    dkappa: -0.001073297081612228
     ddkappa: 0
   }
   v: 4.4163777069444015
@@ -1077,7 +1077,7 @@ trajectory_point {
     theta: 1.3249284315591532
     kappa: -0.0015258204002250495
     s: 16.277187163812808
-    dkappa: 0
+    dkappa: -0.000995730795481584
     ddkappa: 0
   }
   v: 4.4306850895747987
@@ -1091,7 +1091,7 @@ trajectory_point {
     theta: 1.3240776539418162
     kappa: -0.0017813514536276847
     s: 16.720913470072656
-    dkappa: 0
+    dkappa: -0.0008885159984819114
     ddkappa: 0
   }
   v: 4.443603647292413
@@ -1105,7 +1105,7 @@ trajectory_point {
     theta: 1.3232245369427345
     kappa: -0.0020375848948920566
     s: 17.165859463596732
-    dkappa: 0
+    dkappa: -0.00078100649611861136
     ddkappa: 0
   }
   v: 4.4550700092539826
@@ -1119,7 +1119,7 @@ trajectory_point {
     theta: 1.322369364649189
     kappa: -0.0022944357708730061
     s: 17.611877624368176
-    dkappa: 0
+    dkappa: -0.000673237932725675
     ddkappa: 0
   }
   v: 4.4650416390379331
@@ -1133,7 +1133,7 @@ trajectory_point {
     theta: 1.3214800274387433
     kappa: -0.0025160476338384782
     s: 18.058817220449043
-    dkappa: 0
+    dkappa: -0.00055799205217996509
     ddkappa: 0
   }
   v: 4.4734968346443731
@@ -1147,7 +1147,7 @@ trajectory_point {
     theta: 1.3202918482006203
     kappa: -0.0024098128177879024
     s: 18.506527067495568
-    dkappa: 0
+    dkappa: -0.00037597736022231231
     ddkappa: 0
   }
   v: 4.4804615183869672
@@ -1161,7 +1161,7 @@ trajectory_point {
     theta: 1.3191019992119606
     kappa: -0.0023034290509372423
     s: 18.954864644154682
-    dkappa: 0
+    dkappa: -0.00019370746723921457
     ddkappa: 0
   }
   v: 4.4860794853343844
@@ -1175,7 +1175,7 @@ trajectory_point {
     theta: 1.317910819321741
     kappa: -0.0021969263291837103
     s: 19.403703537456966
-    dkappa: 0
+    dkappa: -1.1233765930296488e-05
     ddkappa: 0
   }
   v: 4.4905143214748495
@@ -1189,7 +1189,7 @@ trajectory_point {
     theta: 1.3167186052524462
     kappa: -0.002090330879832705
     s: 19.852933216567756
-    dkappa: 0
+    dkappa: 0.00017139880782475125
     ddkappa: 0
   }
   v: 4.4939200835157189
@@ -1203,7 +1203,7 @@ trajectory_point {
     theta: 1.3161940255957942
     kappa: -0.0016154799354060228
     s: 20.302458079859093
-    dkappa: 0
+    dkappa: 0.00039069454178891779
     ddkappa: 0
   }
   v: 4.4964412988834814
@@ -1217,7 +1217,7 @@ trajectory_point {
     theta: 1.3160462096475032
     kappa: -0.00093273168870817243
     s: 20.752196501981622
-    dkappa: 0
+    dkappa: 0.00063070629739813181
     ddkappa: 0
   }
   v: 4.4982129657237575
@@ -1231,7 +1231,7 @@ trajectory_point {
     theta: 1.3158983460494647
     kappa: -0.00024976338293093106
     s: 21.202079880936509
-    dkappa: 0
+    dkappa: 0.00087079541207232759
     ddkappa: 0
   }
   v: 4.4993605529013019
@@ -1245,7 +1245,7 @@ trajectory_point {
     theta: 1.3157504533884277
     kappa: 0.00043333916129579869
     s: 21.652051685147342
-    dkappa: 0
+    dkappa: 0.0011109317166237405
     ddkappa: 0
   }
   v: 4.5000000000000009
@@ -1259,7 +1259,7 @@ trajectory_point {
     theta: 1.3156025465971519
     kappa: 0.0011165070008188989
     s: 22.102066500532064
-    dkappa: 0
+    dkappa: 0.0013510909749341574
     ddkappa: 0
   }
   v: 4.5002377173228734
@@ -1273,7 +1273,7 @@ trajectory_point {
     theta: 1.3156745349606329
     kappa: 0.0015690385643856582
     s: 22.55208907757488
-    dkappa: 0
+    dkappa: 0.0014027767054260374
     ddkappa: 0
   }
   v: 4.5001705858920715
@@ -1287,7 +1287,7 @@ trajectory_point {
     theta: 1.3162485272485471
     kappa: 0.0014950032161847736
     s: 23.002093378398154
-    dkappa: 0
+    dkappa: 0.0010241829644398843
     ddkappa: 0
   }
   v: 4.4998859574488792
@@ -1301,7 +1301,7 @@ trajectory_point {
     theta: 1.3168224738938914
     kappa: 0.0014209737998675006
     s: 23.452061623834343
-    dkappa: 0
+    dkappa: 0.00064561955726132828
     ddkappa: 0
   }
   v: 4.4994616544537145
@@ -1315,7 +1315,7 @@ trajectory_point {
     theta: 1.3173963611594133
     kappa: 0.0013469520385301396
     s: 23.901983340497903
-    dkappa: 0
+    dkappa: 0.00026709529526885344
     ddkappa: 0
   }
   v: 4.4989659700861262
@@ -1329,7 +1329,7 @@ trajectory_point {
     theta: 1.3179701834113233
     kappa: 0.0012729386100884211
     s: 24.351854407857196
-    dkappa: 0
+    dkappa: -0.00011138635488736629
     ddkappa: 0
   }
   v: 4.4984576682447974
@@ -1343,7 +1343,7 @@ trajectory_point {
     theta: 1.3184990112366295
     kappa: 0.001199468559070833
     s: 24.80167610530642
-    dkappa: 0
+    dkappa: -0.00016425379604566417
     ddkappa: 0
   }
   v: 4.4979859835475411
@@ -1357,7 +1357,7 @@ trajectory_point {
     theta: 1.3190204498072593
     kappa: 0.0011260930560624313
     s: 25.251454159237493
-    dkappa: 0
+    dkappa: -0.00016394248461748618
     ddkappa: 0
   }
   v: 4.497590621331307
@@ -1371,7 +1371,7 @@ trajectory_point {
     theta: 1.3195418485735153
     kappa: 0.0010527231687328471
     s: 25.701197790112005
-    dkappa: 0
+    dkappa: -0.00016363119701503912
     ddkappa: 0
   }
   v: 4.4973017576521741
@@ -1385,7 +1385,7 @@ trajectory_point {
     theta: 1.3200632208874195
     kappa: 0.00097935697832784519
     s: 26.150918759533091
-    dkappa: 0
+    dkappa: -0.00016331992509759354
     ddkappa: 0
   }
   v: 4.4971400392853544
@@ -1399,7 +1399,7 @@ trajectory_point {
     theta: 1.3205214221419852
     kappa: 0.000906723360435351
     s: 26.600630417317387
-    dkappa: 0
+    dkappa: -0.00016277160803065488
     ddkappa: 0
   }
   v: 4.4971165837251954
@@ -1413,7 +1413,7 @@ trajectory_point {
     theta: 1.3208897654343674
     kappa: 0.00083512911775293749
     s: 27.050346748566891
-    dkappa: 0
+    dkappa: -0.0001618860124087397
     ddkappa: 0
   }
   v: 4.4972329791851724
@@ -1427,7 +1427,7 @@ trajectory_point {
     theta: 1.3212581238300634
     kappa: 0.000763531955219696
     s: 27.50008142074093
-    dkappa: 0
+    dkappa: -0.0001610003806692959
     ddkappa: 0
   }
   v: 4.4974812845978978
@@ -1441,7 +1441,7 @@ trajectory_point {
     theta: 1.3216265073828719
     kappa: 0.00069192989926743621
     s: 27.949846830728035
-    dkappa: 0
+    dkappa: -0.00016011468839997848
     ddkappa: 0
   }
   v: 4.4978440296151128
@@ -1455,7 +1455,7 @@ trajectory_point {
     theta: 1.3219949243253768
     kappa: 0.0006203213303060327
     s: 28.399653151917882
-    dkappa: 0
+    dkappa: -0.00015922891556702684
     ddkappa: 0
   }
   v: 4.4982942146076956
@@ -1469,7 +1469,7 @@ trajectory_point {
     theta: 1.322224653299116
     kappa: 0.00055213835551868489
     s: 28.849507381273177
-    dkappa: 0
+    dkappa: -0.00015748206132717451
     ddkappa: 0
   }
   v: 4.4987953106656526
@@ -1483,7 +1483,7 @@ trajectory_point {
     theta: 1.3224495041050173
     kappa: 0.000484069052197468
     s: 29.299412386401585
-    dkappa: 0
+    dkappa: -0.00015570457326621173
     ddkappa: 0
   }
   v: 4.4993012595981261
@@ -1497,7 +1497,7 @@ trajectory_point {
     theta: 1.3226743791863325
     kappa: 0.00041599240172507867
     s: 29.749365952627649
-    dkappa: 0
+    dkappa: -0.000153926893349701
     ddkappa: 0
   }
   v: 4.4997564739333882
@@ -1511,7 +1511,7 @@ trajectory_point {
     theta: 1.3228992743972385
     kappa: 0.00034790965228509386
     s: 30.19935983006469
-    dkappa: 0
+    dkappa: -0.00015214905417135623
     ddkappa: 0
   }
   v: 4.5000958369188471
@@ -1525,7 +1525,7 @@ trajectory_point {
     theta: 1.3230581792420151
     kappa: 0.00028295961701006527
     s: 30.649378780686728
-    dkappa: 0
+    dkappa: -0.00014972880341684348
     ddkappa: 0
   }
   v: 4.500244702521039
@@ -1539,7 +1539,7 @@ trajectory_point {
     theta: 1.3231563525306638
     kappa: 0.0002208953509589963
     s: 31.099399625400395
-    dkappa: 0
+    dkappa: -0.0001467175216715878
     ddkappa: 0
   }
   v: 4.5001188954256373
@@ -1553,7 +1553,7 @@ trajectory_point {
     theta: 1.323254519237008
     kappa: 0.00015883524702027006
     s: 31.549390291116847
-    dkappa: 0
+    dkappa: -0.00014370644186689654
     ddkappa: 0
   }
   v: 4.4996247110374457
@@ -1567,7 +1567,7 @@ trajectory_point {
     theta: 1.3233526702141893
     kappa: 9.6785086558979821e-05
     s: 31.999308857823678
-    dkappa: 0
+    dkappa: -0.00014069584450748329
     ddkappa: 0
   }
   v: 4.4986589154804
@@ -1581,7 +1581,7 @@ trajectory_point {
     theta: 1.3234408506592246
     kappa: 3.5447622957445931e-05
     s: 32.449102605656819
-    dkappa: 0
+    dkappa: -0.00013756646153743218
     ddkappa: 0
   }
   v: 4.49710874559757
@@ -1595,7 +1595,7 @@ trajectory_point {
     theta: 1.3234077989943991
     kappa: -1.7387062232476606e-05
     s: 32.898707061972495
-    dkappa: 0
+    dkappa: -0.00013298038403827819
     ddkappa: 0
   }
   v: 4.4948519089511567
@@ -1609,7 +1609,7 @@ trajectory_point {
     theta: 1.3233747669184264
     kappa: -7.0190433560961808e-05
     s: 33.348045048419067
-    dkappa: 0
+    dkappa: -0.00012839702459819532
     ddkappa: 0
   }
   v: 4.4917565838224967
@@ -1623,7 +1623,7 @@ trajectory_point {
     theta: 1.3233417611090565
     kappa: -0.00012295181643710718
     s: 33.797025728009018
-    dkappa: 0
+    dkappa: -0.00012381730977709349
     ddkappa: 0
   }
   v: 4.4876814192120547
@@ -1637,7 +1637,7 @@ trajectory_point {
     theta: 1.3233087893181754
     kappa: -0.0001756588192109957
     s: 34.245543652190818
-    dkappa: 0
+    dkappa: -0.00011924231517647682
     ddkappa: 0
   }
   v: 4.4824755348394323
@@ -1651,7 +1651,7 @@ trajectory_point {
     theta: 1.3232219912614156
     kappa: -0.00022028100491389244
     s: 34.693477807920878
-    dkappa: 0
+    dkappa: -0.00011350559623187429
     ddkappa: 0
   }
   v: 4.47597852114336
@@ -1665,7 +1665,7 @@ trajectory_point {
     theta: 1.3231026023898587
     kappa: -0.00025996073318025213
     s: 35.140690664735395
-    dkappa: 0
+    dkappa: -0.00010706864053323111
     ddkappa: 0
   }
   v: 4.4680204392817027
@@ -1679,7 +1679,7 @@ trajectory_point {
     theta: 1.3229834474550104
     kappa: -0.00029956271024930204
     s: 35.587027221822346
-    dkappa: 0
+    dkappa: -0.00010064429784970839
     ddkappa: 0
   }
   v: 4.4584218211314592
@@ -1693,7 +1693,7 @@ trajectory_point {
     theta: 1.32286457275791
     kappa: -0.00033907154877115561
     s: 36.032314055093359
-    dkappa: 0
+    dkappa: -9.4235064359931008e-05
     ddkappa: 0
   }
   v: 4.4469936692887595
diff --git a/modules/planning/testdata/garage_test/result_out_of_map_0.pb.txt b/modules/planning/testdata/garage_test/result_out_of_map_0.pb.txt
index d0cc4d1bf19e5c2ad17e09ff059abee0f82f51bb..f34197bd781f9cdcfafe616bd598af291c8bbc78 100644
--- a/modules/planning/testdata/garage_test/result_out_of_map_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_out_of_map_0.pb.txt
@@ -27,7 +27,7 @@ trajectory_point {
     theta: 1.2870620504574257
     kappa: 0.0056573217742357518
     s: 0.088483600561966774
-    dkappa: 0
+    dkappa: -4.7140532124188658e-05
     ddkappa: 0
   }
   v: 4.4233467303087188
@@ -41,7 +41,7 @@ trajectory_point {
     theta: 1.2874731113168
     kappa: 0.0055630946016176352
     s: 0.17693334125124435
-    dkappa: 0
+    dkappa: -9.4263025059125431e-05
     ddkappa: 0
   }
   v: 4.4216140763774083
@@ -55,7 +55,7 @@ trajectory_point {
     theta: 1.287884008711617
     kappa: 0.0054689051959384864
     s: 0.26534763063846667
-    dkappa: 0
+    dkappa: -0.00014136663095473776
     ddkappa: 0
   }
   v: 4.4198014763171312
@@ -69,7 +69,7 @@ trajectory_point {
     theta: 1.2882947350365748
     kappa: 0.00537475526840254
     s: 0.35372486243994006
-    dkappa: 0
+    dkappa: -0.00018845049404716006
     ddkappa: 0
   }
   v: 4.41790819731892
@@ -83,7 +83,7 @@ trajectory_point {
     theta: 1.2887052826194416
     kappa: 0.0052806465456164452
     s: 0.44206341591393977
-    dkappa: 0
+    dkappa: -0.00023551375086986454
     ddkappa: 0
   }
   v: 4.4159335263886375
@@ -97,7 +97,7 @@ trajectory_point {
     theta: 1.289115643722923
     kappa: 0.0051865807691670769
     s: 0.53036165625700638
-    dkappa: 0
+    dkappa: -0.00028255553046479224
     ddkappa: 0
   }
   v: 4.4138767703469748
@@ -111,7 +111,7 @@ trajectory_point {
     theta: 1.2895258105465273
     kappa: 0.0050925596951993642
     s: 0.61861793500024309
-    dkappa: 0
+    dkappa: -0.00032957495459348435
     ddkappa: 0
   }
   v: 4.4117372558294576
@@ -125,7 +125,7 @@ trajectory_point {
     theta: 1.2899357752284331
     kappa: 0.0049985850939940979
     s: 0.70683059040561147
-    dkappa: 0
+    dkappa: -0.00037657113794821294
     ddkappa: 0
   }
   v: 4.4095143292864387
@@ -139,7 +139,7 @@ trajectory_point {
     theta: 1.2903455298473572
     kappa: 0.0049046587495457546
     s: 0.79499794786222844
-    dkappa: 0
+    dkappa: -0.0004235431883631118
     ddkappa: 0
   }
   v: 4.407207356983105
@@ -153,7 +153,7 @@ trajectory_point {
     theta: 1.2907550664244196
     kappa: 0.0048107824591403069
     s: 0.88311832028266357
-    dkappa: 0
+    dkappa: -0.00047049020702530825
     ddkappa: 0
   }
   v: 4.40481572499947
@@ -167,7 +167,7 @@ trajectory_point {
     theta: 1.2911643769250141
     kappa: 0.0047169580329330558
     s: 0.97119000849923431
-    dkappa: 0
+    dkappa: -0.00051741128868605321
     ddkappa: 0
   }
   v: 4.4023388392303824
@@ -181,7 +181,7 @@ trajectory_point {
     theta: 1.2915734532606733
     kappa: 0.0046231872935264288
     s: 1.0592113016603038
-    dkappa: 0
+    dkappa: -0.00056430552187185278
     ddkappa: 0
   }
   v: 4.399776125385519
@@ -195,7 +195,7 @@ trajectory_point {
     theta: 1.2919822872909386
     kappa: 0.0045294720755478159
     s: 1.1471804776265766
-    dkappa: 0
+    dkappa: -0.00061117198909559924
     ddkappa: 0
   }
   v: 4.3971270289893871
@@ -209,7 +209,7 @@ trajectory_point {
     theta: 1.2923908708252272
     kappa: 0.0044358142252273747
     s: 1.2350958033673964
-    dkappa: 0
+    dkappa: -0.00065800976706770209
     ddkappa: 0
   }
   v: 4.394391015381327
@@ -223,7 +223,7 @@ trajectory_point {
     theta: 1.2927991956247011
     kappa: 0.0043422155999758554
     s: 1.3229555353570415
-    dkappa: 0
+    dkappa: -0.00070481792690721846
     ddkappa: 0
   }
   v: 4.391567569715507
@@ -237,7 +237,7 @@ trajectory_point {
     theta: 1.2932072534041348
     kappa: 0.0042486780679624157
     s: 1.4107579199710218
-    dkappa: 0
+    dkappa: -0.00075159553435298523
     ddkappa: 0
   }
   v: 4.3886561969609268
@@ -251,7 +251,7 @@ trajectory_point {
     theta: 1.2936150358337852
     kappa: 0.0041552035076924372
     s: 1.4985011938823758
-    dkappa: 0
+    dkappa: -0.00079834164997474909
     ddkappa: 0
   }
   v: 4.38565642190142
@@ -265,7 +265,7 @@ trajectory_point {
     theta: 1.2940225345412582
     kappa: 0.0040617938075853469
     s: 1.5861835844579668
-    dkappa: 0
+    dkappa: -0.0008450553293842981
     ddkappa: 0
   }
   v: 4.3825677891356456
@@ -279,7 +279,7 @@ trajectory_point {
     theta: 1.2944297411133789
     kappa: 0.003968450865552431
     s: 1.67380331015478
-    dkappa: 0
+    dkappa: -0.00089173562344659258
     ddkappa: 0
   }
   v: 4.3793898630770967
@@ -293,7 +293,7 @@ trajectory_point {
     theta: 1.294836647098061
     kappa: 0.003875176588574657
     s: 1.7613585809162182
-    dkappa: 0
+    dkappa: -0.0009383815784908962
     ddkappa: 0
   }
   v: 4.3761222279540979
@@ -307,7 +307,7 @@ trajectory_point {
     theta: 1.2952432440061739
     kappa: 0.0037819728922804884
     s: 1.8488475985683988
-    dkappa: 0
+    dkappa: -0.0009849922365219063
     ddkappa: 0
   }
   v: 4.3727644878098024
@@ -321,7 +321,7 @@ trajectory_point {
     theta: 1.2956495233134122
     kappa: 0.0036888417005237
     s: 1.9362685572164515
-    dkappa: 0
+    dkappa: -0.0010315666354308863
     ddkappa: 0
   }
   v: 4.3693162665021941
@@ -335,7 +335,7 @@ trajectory_point {
     theta: 1.2960100023541661
     kappa: 0.0036012905834755644
     s: 2.0236196436408136
-    dkappa: 0
+    dkappa: -0.001062565350593916
     ddkappa: 0
   }
   v: 4.36577720770409
@@ -349,7 +349,7 @@ trajectory_point {
     theta: 1.2962502328434062
     kappa: 0.0035283341853809044
     s: 2.1108990376935264
-    dkappa: 0
+    dkappa: -0.0010525510262323627
     ddkappa: 0
   }
   v: 4.3621469749031343
@@ -363,7 +363,7 @@ trajectory_point {
     theta: 1.2964902612584637
     kappa: 0.0034554392414847075
     s: 2.1981049126945331
-    dkappa: 0
+    dkappa: -0.0010425451373513547
     ddkappa: 0
   }
   v: 4.3584252514018056
@@ -377,7 +377,7 @@ trajectory_point {
     theta: 1.2967300825262913
     kappa: 0.0033826072838283538
     s: 2.2852354358279747
-    dkappa: 0
+    dkappa: -0.0010325478942458039
     ddkappa: 0
   }
   v: 4.35461174031741
@@ -391,7 +391,7 @@ trajectory_point {
     theta: 1.2969696915581659
     kappa: 0.00330983984925085
     s: 2.3722887685384868
-    dkappa: 0
+    dkappa: -0.0010225595078691666
     ddkappa: 0
   }
   v: 4.3507061645820881
@@ -405,7 +405,7 @@ trajectory_point {
     theta: 1.2972090832507852
     kappa: 0.00323713847905756
     s: 2.4592630669274955
-    dkappa: 0
+    dkappa: -0.0010125801897879704
     ddkappa: 0
   }
   v: 4.346708266942807
@@ -419,7 +419,7 @@ trajectory_point {
     theta: 1.297448252487365
     kappa: 0.0031645047186889506
     s: 2.5461564821495148
-    dkappa: 0
+    dkappa: -0.0010026101521363468
     ddkappa: 0
   }
   v: 4.3426178099613661
@@ -433,7 +433,7 @@ trajectory_point {
     theta: 1.2976871941387333
     kappa: 0.003091940117389321
     s: 2.6329671608084437
-    dkappa: 0
+    dkappa: -0.000992649607570558
     ddkappa: 0
   }
   v: 4.3384345760143983
@@ -447,7 +447,7 @@ trajectory_point {
     theta: 1.2979259030644286
     kappa: 0.003019446227875546
     s: 2.7196932453538607
-    dkappa: 0
+    dkappa: -0.00098269876922352749
     ddkappa: 0
   }
   v: 4.3341583672933632
@@ -461,7 +461,7 @@ trajectory_point {
     theta: 1.2981643741137956
     kappa: 0.0029470246060058132
     s: 2.8063328744773228
-    dkappa: 0
+    dkappa: -0.00097275785065937008
     ddkappa: 0
   }
   v: 4.3297890058045523
@@ -475,7 +475,7 @@ trajectory_point {
     theta: 1.2984026021270807
     kappa: 0.0028746768104483569
     s: 2.8928841835086621
-    dkappa: 0
+    dkappa: -0.00096282706582791948
     ddkappa: 0
   }
   v: 4.32532633336909
@@ -489,7 +489,7 @@ trajectory_point {
     theta: 1.2986405819365288
     kappa: 0.0028024044023502027
     s: 2.9793453048122784
-    dkappa: 0
+    dkappa: -0.00095290662901926
     ddkappa: 0
   }
   v: 4.3207702116229285
@@ -503,7 +503,7 @@ trajectory_point {
     theta: 1.29887830836748
     kappa: 0.002730208945005896
     s: 3.0657143681834427
-    dkappa: 0
+    dkappa: -0.00094299675481825411
     ddkappa: 0
   }
   v: 4.3161205220168535
@@ -517,7 +517,7 @@ trajectory_point {
     theta: 1.2991157762394647
     kappa: 0.0026580920035262478
     s: 3.151989501244588
-    dkappa: 0
+    dkappa: -0.00093309765805907319
     ddkappa: 0
   }
   v: 4.3113771658164781
@@ -531,7 +531,7 @@ trajectory_point {
     theta: 1.2993529803673007
     kappa: 0.0025860551445070689
     s: 3.2381688298416065
-    dkappa: 0
+    dkappa: -0.00092320955377972632
     ddkappa: 0
   }
   v: 4.3065400641022489
@@ -545,7 +545,7 @@ trajectory_point {
     theta: 1.2995899155621888
     kappa: 0.0025140999356979077
     s: 3.3242504784401508
-    dkappa: 0
+    dkappa: -0.0009133326571765894
     ddkappa: 0
   }
   v: 4.301609157769442
@@ -559,7 +559,7 @@ trajectory_point {
     theta: 1.2998265766328092
     kappa: 0.002442227945670789
     s: 3.4102325705219245
-    dkappa: 0
+    dkappa: -0.00090346718355893577
     ddkappa: 0
   }
   v: 4.2965844075281652
@@ -573,7 +573,7 @@ trajectory_point {
     theta: 1.3000629583864172
     kappa: 0.0023704407434889476
     s: 3.4961132289809842
-    dkappa: 0
+    dkappa: -0.00089361334830346433
     ddkappa: 0
   }
   v: 4.291465793903356
@@ -587,7 +587,7 @@ trajectory_point {
     theta: 1.3002990556299399
     kappa: 0.0022987398983755746
     s: 3.5818905765200304
-    dkappa: 0
+    dkappa: -0.00088377136680882994
     ddkappa: 0
   }
   v: 4.2862533172347836
@@ -601,7 +601,7 @@ trajectory_point {
     theta: 1.3005348631710707
     kappa: 0.0022271269793825437
     s: 3.6675627360467113
-    dkappa: 0
+    dkappa: -0.00087394145445017209
     ddkappa: 0
   }
   v: 4.280946997677046
@@ -615,7 +615,7 @@ trajectory_point {
     theta: 1.3007703758193676
     kappa: 0.00215560355505916
     s: 3.7531278310699117
-    dkappa: 0
+    dkappa: -0.00086412382653364519
     ddkappa: 0
   }
   v: 4.2755468751995744
@@ -629,7 +629,7 @@ trajectory_point {
     theta: 1.3010055883873466
     kappa: 0.00208417119312089
     s: 3.8385839860960544
-    dkappa: 0
+    dkappa: -0.00085431869825094726
     ddkappa: 0
   }
   v: 4.2700530095866291
@@ -643,7 +643,7 @@ trajectory_point {
     theta: 1.3012404956915793
     kappa: 0.002012831460118103
     s: 3.9239293270253972
-    dkappa: 0
+    dkappa: -0.00084452628463384987
     ddkappa: 0
   }
   v: 4.2644654804373019
@@ -657,7 +657,7 @@ trajectory_point {
     theta: 1.3014597071104612
     kappa: 0.0019436273544836352
     s: 4.0091619815483268
-    dkappa: 0
+    dkappa: -0.00083487390577065539
     ddkappa: 0
   }
   v: 4.2587843871655151
@@ -671,7 +671,7 @@ trajectory_point {
     theta: 1.3015631088847031
     kappa: 0.0018898434753920777
     s: 4.0942800795416572
-    dkappa: 0
+    dkappa: -0.0008261888146212273
     ddkappa: 0
   }
   v: 4.2530098490000219
@@ -685,7 +685,7 @@ trajectory_point {
     theta: 1.3016663692500148
     kappa: 0.0018361331615969179
     s: 4.1792817534649238
-    dkappa: 0
+    dkappa: -0.000817515602892091
     ddkappa: 0
   }
   v: 4.2471420049844051
@@ -699,7 +699,7 @@ trajectory_point {
     theta: 1.3017694859390254
     kappa: 0.0017824975912653072
     s: 4.2641651387566855
-    dkappa: 0
+    dkappa: -0.00080885446083521918
     ddkappa: 0
   }
   v: 4.24118101397708
@@ -713,7 +713,7 @@ trajectory_point {
     theta: 1.3018724566884807
     kappa: 0.0017289379404316278
     s: 4.3489283742308134
-    dkappa: 0
+    dkappa: -0.00080020557835818221
     ddkappa: 0
   }
   v: 4.2351270546512927
@@ -727,7 +727,7 @@ trajectory_point {
     theta: 1.302385042477437
     kappa: 0.001462319459089749
     s: 4.7708774490280854
-    dkappa: 0
+    dkappa: -0.00075715167723226318
     ddkappa: 0
   }
   v: 4.2034703738471384
@@ -741,7 +741,7 @@ trajectory_point {
     theta: 1.3028936434998806
     kappa: 0.0011977737057919714
     s: 5.1895462347917052
-    dkappa: 0
+    dkappa: -0.00071443248294220368
     ddkappa: 0
   }
   v: 4.1695276795958591
@@ -755,7 +755,7 @@ trajectory_point {
     theta: 1.3033979843716392
     kappa: 0.00093544378300943246
     s: 5.6047082582667285
-    dkappa: 0
+    dkappa: -0.00067207110386223294
     ddkappa: 0
   }
   v: 4.1333435977376833
@@ -769,7 +769,7 @@ trajectory_point {
     theta: 1.3038802974174939
     kappa: 0.0006785995918108152
     s: 6.0161421279956953
-    dkappa: 0
+    dkappa: -0.00063030826794491181
     ddkappa: 0
   }
   v: 4.0949751383820452
@@ -783,7 +783,7 @@ trajectory_point {
     theta: 1.3039673709360706
     kappa: 0.000494087909003233
     s: 6.4236327727455462
-    dkappa: 0
+    dkappa: -0.00059381516607672716
     ddkappa: 0
   }
   v: 4.0544916959075907
@@ -797,7 +797,7 @@ trajectory_point {
     theta: 1.3040535575175374
     kappa: 0.00031145567933675268
     s: 6.8269726799345491
-    dkappa: 0
+    dkappa: -0.00055769378633416125
     ddkappa: 0
   }
   v: 4.0119750489621717
@@ -811,7 +811,7 @@ trajectory_point {
     theta: 1.3041388146985993
     kappa: 0.00013079288115178605
     s: 7.225963134059219
-    dkappa: 0
+    dkappa: -0.00052196192481929595
     ddkappa: 0
   }
   v: 3.9675193604628496
@@ -825,7 +825,7 @@ trajectory_point {
     theta: 1.3042231021604522
     kappa: -4.7815051285994347e-05
     s: 7.6204154551212335
-    dkappa: 0
+    dkappa: -0.00048663647889772262
     ddkappa: 0
   }
   v: 3.9212311775958937
@@ -839,7 +839,7 @@ trajectory_point {
     theta: 1.3042974900506965
     kappa: -0.00022245501234382787
     s: 8.01015223705436
-    dkappa: 0
+    dkappa: -0.00045188324828266795
     ddkappa: 0
   }
   v: 3.8732294318167817
@@ -853,7 +853,7 @@ trajectory_point {
     theta: 1.3040923566190512
     kappa: -0.0003374861104414603
     s: 8.3950085861513735
-    dkappa: 0
+    dkappa: -0.00042226204164768944
     ddkappa: 0
   }
   v: 3.8236454388501993
@@ -867,7 +867,7 @@ trajectory_point {
     theta: 1.3038899055343858
     kappa: -0.00045101303452117368
     s: 8.7748324620257847
-    dkappa: 0
+    dkappa: -0.00039302816896097869
     ddkappa: 0
   }
   v: 3.77258705951036
@@ -881,7 +881,7 @@ trajectory_point {
     theta: 1.303690215553732
     kappa: -0.0005629916303259634
     s: 9.1494761390066319
-    dkappa: 0
+    dkappa: -0.00036419300021197515
     ddkappa: 0
   }
   v: 3.7200335592876681
@@ -895,7 +895,7 @@ trajectory_point {
     theta: 1.3034933674751201
     kappa: -0.00067337659833901543
     s: 9.5187880597590269
-    dkappa: 0
+    dkappa: -0.00033576820030147681
     ddkappa: 0
   }
   v: 3.6659472858577766
@@ -909,7 +909,7 @@ trajectory_point {
     theta: 1.3032994435820462
     kappa: -0.00078212180529473887
     s: 9.8826138774979633
-    dkappa: 0
+    dkappa: -0.00030776564882565736
     ddkappa: 0
   }
   v: 3.610310102542698
@@ -923,7 +923,7 @@ trajectory_point {
     theta: 1.3029957810530208
     kappa: -0.0008579029952317307
     s: 10.240798407552957
-    dkappa: 0
+    dkappa: -0.00028325192683986797
     ddkappa: 0
   }
   v: 3.5531233883108024
@@ -937,7 +937,7 @@ trajectory_point {
     theta: 1.3026443955137197
     kappa: -0.00091785576042648022
     s: 10.593187578932671
-    dkappa: 0
+    dkappa: -0.00026056091149353626
     ddkappa: 0
   }
   v: 3.49440803777682
@@ -951,7 +951,7 @@ trajectory_point {
     theta: 1.3022989393055495
     kappa: -0.00097679685721891265
     s: 10.939630385889568
-    dkappa: 0
+    dkappa: -0.00023825279396964534
     ddkappa: 0
   }
   v: 3.4342044612018396
@@ -965,7 +965,7 @@ trajectory_point {
     theta: 1.3019595580870245
     kappa: -0.0010347014481812676
     s: 11.279980839484537
-    dkappa: 0
+    dkappa: -0.00021633697477628928
     ddkappa: 0
   }
   v: 3.3725725844933097
@@ -979,7 +979,7 @@ trajectory_point {
     theta: 1.3016263905595988
     kappa: -0.0010915458814308237
     s: 11.614099919151522
-    dkappa: 0
+    dkappa: -0.00019482240571464126
     ddkappa: 0
   }
   v: 3.3095918492050371
@@ -993,7 +993,7 @@ trajectory_point {
     theta: 1.3012995665217311
     kappa: -0.0011473080226539962
     s: 11.941857524262174
-    dkappa: 0
+    dkappa: -0.00017371746421395909
     ddkappa: 0
   }
   v: 3.2453612125371873
@@ -1007,7 +1007,7 @@ trajectory_point {
     theta: 1.3009176578478121
     kappa: -0.001174431655432719
     s: 12.263134425690474
-    dkappa: 0
+    dkappa: -0.00015636210064726941
     ddkappa: 0
   }
   v: 3.1799991473362867
@@ -1021,7 +1021,7 @@ trajectory_point {
     theta: 1.3005309415173929
     kappa: -0.0011953451290092219
     s: 12.577824217377374
-    dkappa: 0
+    dkappa: -0.00014004679932427106
     ddkappa: 0
   }
   v: 3.1136436420952185
@@ -1035,7 +1035,7 @@ trajectory_point {
     theta: 1.3001524324154932
     kappa: -0.001215814750638033
     s: 12.885835267895436
-    dkappa: 0
+    dkappa: -0.0001240777617743802
     ddkappa: 0
   }
   v: 3.046452200953226
@@ -1049,7 +1049,7 @@ trajectory_point {
     theta: 1.2997822227063927
     kappa: -0.0012358355423102859
     s: 13.187092672013458
-    dkappa: 0
+    dkappa: -0.00010845887150907386
     ddkappa: 0
   }
   v: 2.9786018436959121
@@ -1063,7 +1063,7 @@ trajectory_point {
     theta: 1.2994203815876462
     kappa: -0.0012554037672930264
     s: 13.481540202261122
-    dkappa: 0
+    dkappa: -9.3193043678906941e-05
     ddkappa: 0
   }
   v: 2.9102891057552376
@@ -1077,7 +1077,7 @@ trajectory_point {
     theta: 1.2990669528924277
     kappa: -0.0012745170598251767
     s: 13.76914226049362
-    dkappa: 0
+    dkappa: -7.82821238933477e-05
     ddkappa: 0
   }
   v: 2.8417300382095232
@@ -1091,7 +1091,7 @@ trajectory_point {
     theta: 1.2987193467153628
     kappa: -0.0012890340095550316
     s: 14.049885829456297
-    dkappa: 0
+    dkappa: -6.4386788918440087e-05
     ddkappa: 0
   }
   v: 2.7731602077834481
@@ -1105,7 +1105,7 @@ trajectory_point {
     theta: 1.2983692085121012
     kappa: -0.0012857040784332197
     s: 14.323782424349281
-    dkappa: 0
+    dkappa: -5.3618696173401954e-05
     ddkappa: 0
   }
   v: 2.7048346968480512
@@ -1119,7 +1119,7 @@ trajectory_point {
     theta: 1.2980277744198578
     kappa: -0.0012824569282502139
     s: 14.590870044392124
-    dkappa: 0
+    dkappa: -4.3118294498833607e-05
     ddkappa: 0
   }
   v: 2.6370281034207297
@@ -1133,7 +1133,7 @@ trajectory_point {
     theta: 1.2976949596029956
     kappa: -0.00127929175131682
     s: 14.851215124388437
-    dkappa: 0
+    dkappa: -3.2882972047271134e-05
     ddkappa: 0
   }
   v: 2.5700345411652403
@@ -1147,7 +1147,7 @@ trajectory_point {
     theta: 1.2973706445374755
     kappa: -0.0012762074101611144
     s: 15.104911224737908
-    dkappa: 0
+    dkappa: -2.2909050543482536e-05
     ddkappa: 0
   }
   v: 2.50403733850293
@@ -1161,7 +1161,7 @@ trajectory_point {
     theta: 1.297054718051097
     kappa: -0.0012732028468460039
     s: 15.352045363867285
-    dkappa: 0
+    dkappa: -1.3193108906274316e-05
     ddkappa: 0
   }
   v: 2.4387245845727108
@@ -1175,7 +1175,7 @@ trajectory_point {
     theta: 1.2967471190034889
     kappa: -0.0012702774793507932
     s: 15.592665414684671
-    dkappa: 0
+    dkappa: -3.7332650388112694e-06
     ddkappa: 0
   }
   v: 2.3736913169933569
@@ -1189,7 +1189,7 @@ trajectory_point {
     theta: 1.2964478355296032
     kappa: -0.0012674311943695353
     s: 15.826780696936311
-    dkappa: 0
+    dkappa: 5.4708474595621721e-06
     ddkappa: 0
   }
   v: 2.308571434908846
@@ -1203,7 +1203,7 @@ trajectory_point {
     theta: 1.2961616596041889
     kappa: -0.0012615838702054818
     s: 16.054365863359102
-    dkappa: 0
+    dkappa: 1.3698085619546217e-05
     ddkappa: 0
   }
   v: 2.243037698988354
@@ -1217,7 +1217,7 @@ trajectory_point {
     theta: 1.2958974365359115
     kappa: -0.001247057698574986
     s: 16.275364785833123
-    dkappa: 0
+    dkappa: 1.9618718678361804e-05
     ddkappa: 0
   }
   v: 2.1768017314262611
@@ -1231,7 +1231,7 @@ trajectory_point {
     theta: 1.2956411870297317
     kappa: -0.0012329698952076005
     s: 16.489694441534144
-    dkappa: 0
+    dkappa: 2.5360679909766476e-05
     ddkappa: 0
   }
   v: 2.1096140159421481
@@ -1245,7 +1245,7 @@ trajectory_point {
     theta: 1.2953930378980199
     kappa: -0.0012193274295036677
     s: 16.697248799086168
-    dkappa: 0
+    dkappa: 3.0921128698688504e-05
     ddkappa: 0
   }
   v: 2.0412638977807966
@@ -1259,7 +1259,7 @@ trajectory_point {
     theta: 1.2951531388093642
     kappa: -0.0012061385277712592
     s: 16.897902704713921
-    dkappa: 0
+    dkappa: 3.6296712134756139e-05
     ddkappa: 0
   }
   v: 1.9715795837121903
@@ -1273,7 +1273,7 @@ trajectory_point {
     theta: 1.2949216576426685
     kappa: -0.0011934124177909091
     s: 17.091515768395396
-    dkappa: 0
+    dkappa: 4.1483669123588808e-05
     ddkappa: 0
   }
   v: 1.9004281420315146
@@ -1287,7 +1287,7 @@ trajectory_point {
     theta: 1.2946987758412332
     kappa: -0.0011811590733803497
     s: 17.27793625001436
-    dkappa: 0
+    dkappa: 4.6477934498087659e-05
     ddkappa: 0
   }
   v: 1.8277155025591563
@@ -1301,7 +1301,7 @@ trajectory_point {
     theta: 1.2944846837668333
     kappa: -0.0011693889589592431
     s: 17.457004945512882
-    dkappa: 0
+    dkappa: 5.1275243129726631e-05
     ddkappa: 0
   }
   v: 1.7533864566407027
@@ -1315,7 +1315,7 @@ trajectory_point {
     theta: 1.2942795760538093
     kappa: -0.0011581127741139175
     s: 17.628559073043853
-    dkappa: 0
+    dkappa: 5.5871234039843109e-05
     ddkappa: 0
   }
   v: 1.6774246571469438
@@ -1329,7 +1329,7 @@ trajectory_point {
     theta: 1.2940836469631796
     kappa: -0.001147341198162102
     s: 17.792436159123486
-    dkappa: 0
+    dkappa: 6.0261554510928227e-05
     ddkappa: 0
   }
   v: 1.5998526184738706
@@ -1343,7 +1343,7 @@ trajectory_point {
     theta: 1.2938970857367824
     kappa: -0.0011370846347176587
     s: 17.94847792478388
-    dkappa: 0
+    dkappa: 6.4441964197918842e-05
     ddkappa: 0
   }
   v: 1.5207317165426757
@@ -1357,7 +1357,7 @@ trajectory_point {
     theta: 1.2937298562472963
     kappa: -0.0011244990220239388
     s: 18.096534171725491
-    dkappa: 0
+    dkappa: 6.7156844097871615e-05
     ddkappa: 0
   }
   v: 1.4401621887997527
@@ -1371,7 +1371,7 @@ trajectory_point {
     theta: 1.2935762474830625
     kappa: -0.0011113074390662861
     s: 18.236466668469685
-    dkappa: 0
+    dkappa: 6.915416398388956e-05
     ddkappa: 0
   }
   v: 1.3582831342166977
@@ -1385,7 +1385,7 @@ trajectory_point {
     theta: 1.2934316907447694
     kappa: -0.0010988932272939635
     s: 18.368153036511245
-    dkappa: 0
+    dkappa: 7.1033783141173e-05
     ddkappa: 0
   }
   v: 1.2752725132903069
@@ -1399,7 +1399,7 @@ trajectory_point {
     theta: 1.293296298715348
     kappa: -0.0010872660626287044
     s: 18.491490636470896
-    dkappa: 0
+    dkappa: 7.2794236551387441e-05
     ddkappa: 0
   }
   v: 1.1913471480425792
@@ -1413,7 +1413,7 @@ trajectory_point {
     theta: 1.2931701581410067
     kappa: -0.00107643339381251
     s: 18.606400454247819
-    dkappa: 0
+    dkappa: 7.4434396410495154e-05
     ddkappa: 0
   }
   v: 1.1067627220207146
@@ -1427,7 +1427,7 @@ trajectory_point {
     theta: 1.2930533255655428
     kappa: -0.0010664000760559817
     s: 18.712830987172175
-    dkappa: 0
+    dkappa: 7.595352759756966e-05
     ddkappa: 0
   }
   v: 1.0218137802971139
@@ -1441,7 +1441,7 @@ trajectory_point {
     theta: 1.2929458230646571
     kappa: -0.0010571680046866554
     s: 18.810762130157631
-    dkappa: 0
+    dkappa: 7.7351343143610313e-05
     ddkappa: 0
   }
   v: 0.93683372946938059
@@ -1455,7 +1455,7 @@ trajectory_point {
     theta: 1.2928476339802675
     kappa: -0.0010487357487973358
     s: 18.900209061853868
-    dkappa: 0
+    dkappa: 7.86280597003568e-05
     ddkappa: 0
   }
   v: 0.852194837660319
@@ -1469,7 +1469,7 @@ trajectory_point {
     theta: 1.2927586986548125
     kappa: -0.001041098184894429
     s: 18.981226130799111
-    dkappa: 0
+    dkappa: 7.9784453009103612e-05
     ddkappa: 0
   }
   v: 0.76830823451793462
@@ -1483,7 +1483,7 @@ trajectory_point {
     theta: 1.2926789101655434
     kappa: -0.0010342461305462775
     s: 19.053910741572643
-    dkappa: 0
+    dkappa: 8.0821913369514669e-05
     ddkappa: 0
   }
   v: 0.68562391121543442
@@ -1497,7 +1497,7 @@ trajectory_point {
     theta: 1.2926081100587967
     kappa: -0.0010281659780314919
     s: 19.118407240947334
-    dkappa: 0
+    dkappa: 8.1742501108437871e-05
     ddkappa: 0
   }
   v: 0.60463072045122823
@@ -1511,7 +1511,7 @@ trajectory_point {
     theta: 1.2925460840842427
     kappa: -0.0010228393279872868
     s: 19.174910804042145
-    dkappa: 0
+    dkappa: 8.25490020487194e-05
     ddkappa: 0
   }
   v: 0.52585637644892536
@@ -1525,7 +1525,7 @@ trajectory_point {
     theta: 1.2924925579291062
     kappa: -0.0010182426230578127
     s: 19.223671320474672
-    dkappa: 0
+    dkappa: 8.3244982978018675e-05
     ddkappa: 0
   }
   v: 0.44986745495733782
@@ -1539,7 +1539,7 @@ trajectory_point {
     theta: 1.2924471929523578
     kappa: -0.0010143467815424913
     s: 19.264997280513633
-    dkappa: 0
+    dkappa: 8.38348471176223e-05
     ddkappa: 0
   }
   v: 0.37726939325047915
@@ -1553,7 +1553,7 @@ trajectory_point {
     theta: 1.2924095819188703
     kappa: -0.0010111168310443469
     s: 19.299259661231428
-    dkappa: 0
+    dkappa: 8.4323889591259217e-05
     ddkappa: 0
   }
   v: 0.30870649012756424
@@ -1567,7 +1567,7 @@ trajectory_point {
     theta: 1.2923792447335487
     kappa: -0.0010085115421183434
     s: 19.32689581265662
-    dkappa: 0
+    dkappa: 8.4718352893914663e-05
     ddkappa: 0
   }
   v: 0.24486190591300827
@@ -1581,7 +1581,7 @@ trajectory_point {
     theta: 1.2923556241754235
     kappa: -0.001006483061919714
     s: 19.348413343926488
-    dkappa: 0
+    dkappa: 8.5025482360645235e-05
     ddkappa: 0
   }
   v: 0.18645766245642997
@@ -1595,7 +1595,7 @@ trajectory_point {
     theta: 1.2923380816317218
     kappa: -0.0010049765478522983
     s: 19.364394009439525
-    dkappa: 0
+    dkappa: 8.5253581635392984e-05
     ddkappa: 0
   }
   v: 0.13425464313264765
@@ -1609,7 +1609,7 @@ trajectory_point {
     theta: 1.2923258928319128
     kappa: -0.0010039298012168741
     s: 19.375497595007968
-    dkappa: 0
+    dkappa: 8.5412068139800119e-05
     ddkappa: 0
   }
   v: 0.089052592841682876
@@ -1623,7 +1623,7 @@ trajectory_point {
     theta: 1.2923182435817373
     kappa: -0.0010032729008594902
     s: 19.382465804010327
-    dkappa: 0
+    dkappa: 8.5511528542023789e-05
     ddkappa: 0
   }
   v: 0.0516901180087567
@@ -1637,7 +1637,7 @@ trajectory_point {
     theta: 1.2923142254972235
     kappa: -0.0010029278368198047
     s: 19.386126143543862
-    dkappa: 0
+    dkappa: 8.5563774225549862e-05
     ddkappa: 0
   }
   v: 0.023044686584292977
@@ -1651,7 +1651,7 @@ trajectory_point {
     theta: 1.2923128317386976
     kappa: -0.0010028081439794119
     s: 19.387395810577178
-    dkappa: 0
+    dkappa: 8.5581896758008487e-05
     ddkappa: 0
   }
   v: 0.0040326280439178111
@@ -1665,7 +1665,7 @@ trajectory_point {
     theta: 1.2923129527447936
     kappa: -0.0010028185357101814
     s: 19.387285578102674
-    dkappa: 0
+    dkappa: 8.5580323359987757e-05
     ddkappa: 0
   }
   v: 0
@@ -1679,7 +1679,7 @@ trajectory_point {
     theta: 1.2923133719664681
     kappa: -0.001002854537522589
     s: 19.3869036812891
-    dkappa: 0
+    dkappa: 8.557487237384866e-05
     ddkappa: 0
   }
   v: 3.0489416841993562e-15
@@ -1693,7 +1693,7 @@ trajectory_point {
     theta: 1.2923127616010233
     kappa: -0.0010028021207140526
     s: 19.387459703634061
-    dkappa: 0
+    dkappa: 8.5582808732539482e-05
     ddkappa: 0
   }
   v: 0.01454290736159085
diff --git a/modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt b/modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt
index d0cc4d1bf19e5c2ad17e09ff059abee0f82f51bb..f34197bd781f9cdcfafe616bd598af291c8bbc78 100644
--- a/modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_stop_dest_0.pb.txt
@@ -27,7 +27,7 @@ trajectory_point {
     theta: 1.2870620504574257
     kappa: 0.0056573217742357518
     s: 0.088483600561966774
-    dkappa: 0
+    dkappa: -4.7140532124188658e-05
     ddkappa: 0
   }
   v: 4.4233467303087188
@@ -41,7 +41,7 @@ trajectory_point {
     theta: 1.2874731113168
     kappa: 0.0055630946016176352
     s: 0.17693334125124435
-    dkappa: 0
+    dkappa: -9.4263025059125431e-05
     ddkappa: 0
   }
   v: 4.4216140763774083
@@ -55,7 +55,7 @@ trajectory_point {
     theta: 1.287884008711617
     kappa: 0.0054689051959384864
     s: 0.26534763063846667
-    dkappa: 0
+    dkappa: -0.00014136663095473776
     ddkappa: 0
   }
   v: 4.4198014763171312
@@ -69,7 +69,7 @@ trajectory_point {
     theta: 1.2882947350365748
     kappa: 0.00537475526840254
     s: 0.35372486243994006
-    dkappa: 0
+    dkappa: -0.00018845049404716006
     ddkappa: 0
   }
   v: 4.41790819731892
@@ -83,7 +83,7 @@ trajectory_point {
     theta: 1.2887052826194416
     kappa: 0.0052806465456164452
     s: 0.44206341591393977
-    dkappa: 0
+    dkappa: -0.00023551375086986454
     ddkappa: 0
   }
   v: 4.4159335263886375
@@ -97,7 +97,7 @@ trajectory_point {
     theta: 1.289115643722923
     kappa: 0.0051865807691670769
     s: 0.53036165625700638
-    dkappa: 0
+    dkappa: -0.00028255553046479224
     ddkappa: 0
   }
   v: 4.4138767703469748
@@ -111,7 +111,7 @@ trajectory_point {
     theta: 1.2895258105465273
     kappa: 0.0050925596951993642
     s: 0.61861793500024309
-    dkappa: 0
+    dkappa: -0.00032957495459348435
     ddkappa: 0
   }
   v: 4.4117372558294576
@@ -125,7 +125,7 @@ trajectory_point {
     theta: 1.2899357752284331
     kappa: 0.0049985850939940979
     s: 0.70683059040561147
-    dkappa: 0
+    dkappa: -0.00037657113794821294
     ddkappa: 0
   }
   v: 4.4095143292864387
@@ -139,7 +139,7 @@ trajectory_point {
     theta: 1.2903455298473572
     kappa: 0.0049046587495457546
     s: 0.79499794786222844
-    dkappa: 0
+    dkappa: -0.0004235431883631118
     ddkappa: 0
   }
   v: 4.407207356983105
@@ -153,7 +153,7 @@ trajectory_point {
     theta: 1.2907550664244196
     kappa: 0.0048107824591403069
     s: 0.88311832028266357
-    dkappa: 0
+    dkappa: -0.00047049020702530825
     ddkappa: 0
   }
   v: 4.40481572499947
@@ -167,7 +167,7 @@ trajectory_point {
     theta: 1.2911643769250141
     kappa: 0.0047169580329330558
     s: 0.97119000849923431
-    dkappa: 0
+    dkappa: -0.00051741128868605321
     ddkappa: 0
   }
   v: 4.4023388392303824
@@ -181,7 +181,7 @@ trajectory_point {
     theta: 1.2915734532606733
     kappa: 0.0046231872935264288
     s: 1.0592113016603038
-    dkappa: 0
+    dkappa: -0.00056430552187185278
     ddkappa: 0
   }
   v: 4.399776125385519
@@ -195,7 +195,7 @@ trajectory_point {
     theta: 1.2919822872909386
     kappa: 0.0045294720755478159
     s: 1.1471804776265766
-    dkappa: 0
+    dkappa: -0.00061117198909559924
     ddkappa: 0
   }
   v: 4.3971270289893871
@@ -209,7 +209,7 @@ trajectory_point {
     theta: 1.2923908708252272
     kappa: 0.0044358142252273747
     s: 1.2350958033673964
-    dkappa: 0
+    dkappa: -0.00065800976706770209
     ddkappa: 0
   }
   v: 4.394391015381327
@@ -223,7 +223,7 @@ trajectory_point {
     theta: 1.2927991956247011
     kappa: 0.0043422155999758554
     s: 1.3229555353570415
-    dkappa: 0
+    dkappa: -0.00070481792690721846
     ddkappa: 0
   }
   v: 4.391567569715507
@@ -237,7 +237,7 @@ trajectory_point {
     theta: 1.2932072534041348
     kappa: 0.0042486780679624157
     s: 1.4107579199710218
-    dkappa: 0
+    dkappa: -0.00075159553435298523
     ddkappa: 0
   }
   v: 4.3886561969609268
@@ -251,7 +251,7 @@ trajectory_point {
     theta: 1.2936150358337852
     kappa: 0.0041552035076924372
     s: 1.4985011938823758
-    dkappa: 0
+    dkappa: -0.00079834164997474909
     ddkappa: 0
   }
   v: 4.38565642190142
@@ -265,7 +265,7 @@ trajectory_point {
     theta: 1.2940225345412582
     kappa: 0.0040617938075853469
     s: 1.5861835844579668
-    dkappa: 0
+    dkappa: -0.0008450553293842981
     ddkappa: 0
   }
   v: 4.3825677891356456
@@ -279,7 +279,7 @@ trajectory_point {
     theta: 1.2944297411133789
     kappa: 0.003968450865552431
     s: 1.67380331015478
-    dkappa: 0
+    dkappa: -0.00089173562344659258
     ddkappa: 0
   }
   v: 4.3793898630770967
@@ -293,7 +293,7 @@ trajectory_point {
     theta: 1.294836647098061
     kappa: 0.003875176588574657
     s: 1.7613585809162182
-    dkappa: 0
+    dkappa: -0.0009383815784908962
     ddkappa: 0
   }
   v: 4.3761222279540979
@@ -307,7 +307,7 @@ trajectory_point {
     theta: 1.2952432440061739
     kappa: 0.0037819728922804884
     s: 1.8488475985683988
-    dkappa: 0
+    dkappa: -0.0009849922365219063
     ddkappa: 0
   }
   v: 4.3727644878098024
@@ -321,7 +321,7 @@ trajectory_point {
     theta: 1.2956495233134122
     kappa: 0.0036888417005237
     s: 1.9362685572164515
-    dkappa: 0
+    dkappa: -0.0010315666354308863
     ddkappa: 0
   }
   v: 4.3693162665021941
@@ -335,7 +335,7 @@ trajectory_point {
     theta: 1.2960100023541661
     kappa: 0.0036012905834755644
     s: 2.0236196436408136
-    dkappa: 0
+    dkappa: -0.001062565350593916
     ddkappa: 0
   }
   v: 4.36577720770409
@@ -349,7 +349,7 @@ trajectory_point {
     theta: 1.2962502328434062
     kappa: 0.0035283341853809044
     s: 2.1108990376935264
-    dkappa: 0
+    dkappa: -0.0010525510262323627
     ddkappa: 0
   }
   v: 4.3621469749031343
@@ -363,7 +363,7 @@ trajectory_point {
     theta: 1.2964902612584637
     kappa: 0.0034554392414847075
     s: 2.1981049126945331
-    dkappa: 0
+    dkappa: -0.0010425451373513547
     ddkappa: 0
   }
   v: 4.3584252514018056
@@ -377,7 +377,7 @@ trajectory_point {
     theta: 1.2967300825262913
     kappa: 0.0033826072838283538
     s: 2.2852354358279747
-    dkappa: 0
+    dkappa: -0.0010325478942458039
     ddkappa: 0
   }
   v: 4.35461174031741
@@ -391,7 +391,7 @@ trajectory_point {
     theta: 1.2969696915581659
     kappa: 0.00330983984925085
     s: 2.3722887685384868
-    dkappa: 0
+    dkappa: -0.0010225595078691666
     ddkappa: 0
   }
   v: 4.3507061645820881
@@ -405,7 +405,7 @@ trajectory_point {
     theta: 1.2972090832507852
     kappa: 0.00323713847905756
     s: 2.4592630669274955
-    dkappa: 0
+    dkappa: -0.0010125801897879704
     ddkappa: 0
   }
   v: 4.346708266942807
@@ -419,7 +419,7 @@ trajectory_point {
     theta: 1.297448252487365
     kappa: 0.0031645047186889506
     s: 2.5461564821495148
-    dkappa: 0
+    dkappa: -0.0010026101521363468
     ddkappa: 0
   }
   v: 4.3426178099613661
@@ -433,7 +433,7 @@ trajectory_point {
     theta: 1.2976871941387333
     kappa: 0.003091940117389321
     s: 2.6329671608084437
-    dkappa: 0
+    dkappa: -0.000992649607570558
     ddkappa: 0
   }
   v: 4.3384345760143983
@@ -447,7 +447,7 @@ trajectory_point {
     theta: 1.2979259030644286
     kappa: 0.003019446227875546
     s: 2.7196932453538607
-    dkappa: 0
+    dkappa: -0.00098269876922352749
     ddkappa: 0
   }
   v: 4.3341583672933632
@@ -461,7 +461,7 @@ trajectory_point {
     theta: 1.2981643741137956
     kappa: 0.0029470246060058132
     s: 2.8063328744773228
-    dkappa: 0
+    dkappa: -0.00097275785065937008
     ddkappa: 0
   }
   v: 4.3297890058045523
@@ -475,7 +475,7 @@ trajectory_point {
     theta: 1.2984026021270807
     kappa: 0.0028746768104483569
     s: 2.8928841835086621
-    dkappa: 0
+    dkappa: -0.00096282706582791948
     ddkappa: 0
   }
   v: 4.32532633336909
@@ -489,7 +489,7 @@ trajectory_point {
     theta: 1.2986405819365288
     kappa: 0.0028024044023502027
     s: 2.9793453048122784
-    dkappa: 0
+    dkappa: -0.00095290662901926
     ddkappa: 0
   }
   v: 4.3207702116229285
@@ -503,7 +503,7 @@ trajectory_point {
     theta: 1.29887830836748
     kappa: 0.002730208945005896
     s: 3.0657143681834427
-    dkappa: 0
+    dkappa: -0.00094299675481825411
     ddkappa: 0
   }
   v: 4.3161205220168535
@@ -517,7 +517,7 @@ trajectory_point {
     theta: 1.2991157762394647
     kappa: 0.0026580920035262478
     s: 3.151989501244588
-    dkappa: 0
+    dkappa: -0.00093309765805907319
     ddkappa: 0
   }
   v: 4.3113771658164781
@@ -531,7 +531,7 @@ trajectory_point {
     theta: 1.2993529803673007
     kappa: 0.0025860551445070689
     s: 3.2381688298416065
-    dkappa: 0
+    dkappa: -0.00092320955377972632
     ddkappa: 0
   }
   v: 4.3065400641022489
@@ -545,7 +545,7 @@ trajectory_point {
     theta: 1.2995899155621888
     kappa: 0.0025140999356979077
     s: 3.3242504784401508
-    dkappa: 0
+    dkappa: -0.0009133326571765894
     ddkappa: 0
   }
   v: 4.301609157769442
@@ -559,7 +559,7 @@ trajectory_point {
     theta: 1.2998265766328092
     kappa: 0.002442227945670789
     s: 3.4102325705219245
-    dkappa: 0
+    dkappa: -0.00090346718355893577
     ddkappa: 0
   }
   v: 4.2965844075281652
@@ -573,7 +573,7 @@ trajectory_point {
     theta: 1.3000629583864172
     kappa: 0.0023704407434889476
     s: 3.4961132289809842
-    dkappa: 0
+    dkappa: -0.00089361334830346433
     ddkappa: 0
   }
   v: 4.291465793903356
@@ -587,7 +587,7 @@ trajectory_point {
     theta: 1.3002990556299399
     kappa: 0.0022987398983755746
     s: 3.5818905765200304
-    dkappa: 0
+    dkappa: -0.00088377136680882994
     ddkappa: 0
   }
   v: 4.2862533172347836
@@ -601,7 +601,7 @@ trajectory_point {
     theta: 1.3005348631710707
     kappa: 0.0022271269793825437
     s: 3.6675627360467113
-    dkappa: 0
+    dkappa: -0.00087394145445017209
     ddkappa: 0
   }
   v: 4.280946997677046
@@ -615,7 +615,7 @@ trajectory_point {
     theta: 1.3007703758193676
     kappa: 0.00215560355505916
     s: 3.7531278310699117
-    dkappa: 0
+    dkappa: -0.00086412382653364519
     ddkappa: 0
   }
   v: 4.2755468751995744
@@ -629,7 +629,7 @@ trajectory_point {
     theta: 1.3010055883873466
     kappa: 0.00208417119312089
     s: 3.8385839860960544
-    dkappa: 0
+    dkappa: -0.00085431869825094726
     ddkappa: 0
   }
   v: 4.2700530095866291
@@ -643,7 +643,7 @@ trajectory_point {
     theta: 1.3012404956915793
     kappa: 0.002012831460118103
     s: 3.9239293270253972
-    dkappa: 0
+    dkappa: -0.00084452628463384987
     ddkappa: 0
   }
   v: 4.2644654804373019
@@ -657,7 +657,7 @@ trajectory_point {
     theta: 1.3014597071104612
     kappa: 0.0019436273544836352
     s: 4.0091619815483268
-    dkappa: 0
+    dkappa: -0.00083487390577065539
     ddkappa: 0
   }
   v: 4.2587843871655151
@@ -671,7 +671,7 @@ trajectory_point {
     theta: 1.3015631088847031
     kappa: 0.0018898434753920777
     s: 4.0942800795416572
-    dkappa: 0
+    dkappa: -0.0008261888146212273
     ddkappa: 0
   }
   v: 4.2530098490000219
@@ -685,7 +685,7 @@ trajectory_point {
     theta: 1.3016663692500148
     kappa: 0.0018361331615969179
     s: 4.1792817534649238
-    dkappa: 0
+    dkappa: -0.000817515602892091
     ddkappa: 0
   }
   v: 4.2471420049844051
@@ -699,7 +699,7 @@ trajectory_point {
     theta: 1.3017694859390254
     kappa: 0.0017824975912653072
     s: 4.2641651387566855
-    dkappa: 0
+    dkappa: -0.00080885446083521918
     ddkappa: 0
   }
   v: 4.24118101397708
@@ -713,7 +713,7 @@ trajectory_point {
     theta: 1.3018724566884807
     kappa: 0.0017289379404316278
     s: 4.3489283742308134
-    dkappa: 0
+    dkappa: -0.00080020557835818221
     ddkappa: 0
   }
   v: 4.2351270546512927
@@ -727,7 +727,7 @@ trajectory_point {
     theta: 1.302385042477437
     kappa: 0.001462319459089749
     s: 4.7708774490280854
-    dkappa: 0
+    dkappa: -0.00075715167723226318
     ddkappa: 0
   }
   v: 4.2034703738471384
@@ -741,7 +741,7 @@ trajectory_point {
     theta: 1.3028936434998806
     kappa: 0.0011977737057919714
     s: 5.1895462347917052
-    dkappa: 0
+    dkappa: -0.00071443248294220368
     ddkappa: 0
   }
   v: 4.1695276795958591
@@ -755,7 +755,7 @@ trajectory_point {
     theta: 1.3033979843716392
     kappa: 0.00093544378300943246
     s: 5.6047082582667285
-    dkappa: 0
+    dkappa: -0.00067207110386223294
     ddkappa: 0
   }
   v: 4.1333435977376833
@@ -769,7 +769,7 @@ trajectory_point {
     theta: 1.3038802974174939
     kappa: 0.0006785995918108152
     s: 6.0161421279956953
-    dkappa: 0
+    dkappa: -0.00063030826794491181
     ddkappa: 0
   }
   v: 4.0949751383820452
@@ -783,7 +783,7 @@ trajectory_point {
     theta: 1.3039673709360706
     kappa: 0.000494087909003233
     s: 6.4236327727455462
-    dkappa: 0
+    dkappa: -0.00059381516607672716
     ddkappa: 0
   }
   v: 4.0544916959075907
@@ -797,7 +797,7 @@ trajectory_point {
     theta: 1.3040535575175374
     kappa: 0.00031145567933675268
     s: 6.8269726799345491
-    dkappa: 0
+    dkappa: -0.00055769378633416125
     ddkappa: 0
   }
   v: 4.0119750489621717
@@ -811,7 +811,7 @@ trajectory_point {
     theta: 1.3041388146985993
     kappa: 0.00013079288115178605
     s: 7.225963134059219
-    dkappa: 0
+    dkappa: -0.00052196192481929595
     ddkappa: 0
   }
   v: 3.9675193604628496
@@ -825,7 +825,7 @@ trajectory_point {
     theta: 1.3042231021604522
     kappa: -4.7815051285994347e-05
     s: 7.6204154551212335
-    dkappa: 0
+    dkappa: -0.00048663647889772262
     ddkappa: 0
   }
   v: 3.9212311775958937
@@ -839,7 +839,7 @@ trajectory_point {
     theta: 1.3042974900506965
     kappa: -0.00022245501234382787
     s: 8.01015223705436
-    dkappa: 0
+    dkappa: -0.00045188324828266795
     ddkappa: 0
   }
   v: 3.8732294318167817
@@ -853,7 +853,7 @@ trajectory_point {
     theta: 1.3040923566190512
     kappa: -0.0003374861104414603
     s: 8.3950085861513735
-    dkappa: 0
+    dkappa: -0.00042226204164768944
     ddkappa: 0
   }
   v: 3.8236454388501993
@@ -867,7 +867,7 @@ trajectory_point {
     theta: 1.3038899055343858
     kappa: -0.00045101303452117368
     s: 8.7748324620257847
-    dkappa: 0
+    dkappa: -0.00039302816896097869
     ddkappa: 0
   }
   v: 3.77258705951036
@@ -881,7 +881,7 @@ trajectory_point {
     theta: 1.303690215553732
     kappa: -0.0005629916303259634
     s: 9.1494761390066319
-    dkappa: 0
+    dkappa: -0.00036419300021197515
     ddkappa: 0
   }
   v: 3.7200335592876681
@@ -895,7 +895,7 @@ trajectory_point {
     theta: 1.3034933674751201
     kappa: -0.00067337659833901543
     s: 9.5187880597590269
-    dkappa: 0
+    dkappa: -0.00033576820030147681
     ddkappa: 0
   }
   v: 3.6659472858577766
@@ -909,7 +909,7 @@ trajectory_point {
     theta: 1.3032994435820462
     kappa: -0.00078212180529473887
     s: 9.8826138774979633
-    dkappa: 0
+    dkappa: -0.00030776564882565736
     ddkappa: 0
   }
   v: 3.610310102542698
@@ -923,7 +923,7 @@ trajectory_point {
     theta: 1.3029957810530208
     kappa: -0.0008579029952317307
     s: 10.240798407552957
-    dkappa: 0
+    dkappa: -0.00028325192683986797
     ddkappa: 0
   }
   v: 3.5531233883108024
@@ -937,7 +937,7 @@ trajectory_point {
     theta: 1.3026443955137197
     kappa: -0.00091785576042648022
     s: 10.593187578932671
-    dkappa: 0
+    dkappa: -0.00026056091149353626
     ddkappa: 0
   }
   v: 3.49440803777682
@@ -951,7 +951,7 @@ trajectory_point {
     theta: 1.3022989393055495
     kappa: -0.00097679685721891265
     s: 10.939630385889568
-    dkappa: 0
+    dkappa: -0.00023825279396964534
     ddkappa: 0
   }
   v: 3.4342044612018396
@@ -965,7 +965,7 @@ trajectory_point {
     theta: 1.3019595580870245
     kappa: -0.0010347014481812676
     s: 11.279980839484537
-    dkappa: 0
+    dkappa: -0.00021633697477628928
     ddkappa: 0
   }
   v: 3.3725725844933097
@@ -979,7 +979,7 @@ trajectory_point {
     theta: 1.3016263905595988
     kappa: -0.0010915458814308237
     s: 11.614099919151522
-    dkappa: 0
+    dkappa: -0.00019482240571464126
     ddkappa: 0
   }
   v: 3.3095918492050371
@@ -993,7 +993,7 @@ trajectory_point {
     theta: 1.3012995665217311
     kappa: -0.0011473080226539962
     s: 11.941857524262174
-    dkappa: 0
+    dkappa: -0.00017371746421395909
     ddkappa: 0
   }
   v: 3.2453612125371873
@@ -1007,7 +1007,7 @@ trajectory_point {
     theta: 1.3009176578478121
     kappa: -0.001174431655432719
     s: 12.263134425690474
-    dkappa: 0
+    dkappa: -0.00015636210064726941
     ddkappa: 0
   }
   v: 3.1799991473362867
@@ -1021,7 +1021,7 @@ trajectory_point {
     theta: 1.3005309415173929
     kappa: -0.0011953451290092219
     s: 12.577824217377374
-    dkappa: 0
+    dkappa: -0.00014004679932427106
     ddkappa: 0
   }
   v: 3.1136436420952185
@@ -1035,7 +1035,7 @@ trajectory_point {
     theta: 1.3001524324154932
     kappa: -0.001215814750638033
     s: 12.885835267895436
-    dkappa: 0
+    dkappa: -0.0001240777617743802
     ddkappa: 0
   }
   v: 3.046452200953226
@@ -1049,7 +1049,7 @@ trajectory_point {
     theta: 1.2997822227063927
     kappa: -0.0012358355423102859
     s: 13.187092672013458
-    dkappa: 0
+    dkappa: -0.00010845887150907386
     ddkappa: 0
   }
   v: 2.9786018436959121
@@ -1063,7 +1063,7 @@ trajectory_point {
     theta: 1.2994203815876462
     kappa: -0.0012554037672930264
     s: 13.481540202261122
-    dkappa: 0
+    dkappa: -9.3193043678906941e-05
     ddkappa: 0
   }
   v: 2.9102891057552376
@@ -1077,7 +1077,7 @@ trajectory_point {
     theta: 1.2990669528924277
     kappa: -0.0012745170598251767
     s: 13.76914226049362
-    dkappa: 0
+    dkappa: -7.82821238933477e-05
     ddkappa: 0
   }
   v: 2.8417300382095232
@@ -1091,7 +1091,7 @@ trajectory_point {
     theta: 1.2987193467153628
     kappa: -0.0012890340095550316
     s: 14.049885829456297
-    dkappa: 0
+    dkappa: -6.4386788918440087e-05
     ddkappa: 0
   }
   v: 2.7731602077834481
@@ -1105,7 +1105,7 @@ trajectory_point {
     theta: 1.2983692085121012
     kappa: -0.0012857040784332197
     s: 14.323782424349281
-    dkappa: 0
+    dkappa: -5.3618696173401954e-05
     ddkappa: 0
   }
   v: 2.7048346968480512
@@ -1119,7 +1119,7 @@ trajectory_point {
     theta: 1.2980277744198578
     kappa: -0.0012824569282502139
     s: 14.590870044392124
-    dkappa: 0
+    dkappa: -4.3118294498833607e-05
     ddkappa: 0
   }
   v: 2.6370281034207297
@@ -1133,7 +1133,7 @@ trajectory_point {
     theta: 1.2976949596029956
     kappa: -0.00127929175131682
     s: 14.851215124388437
-    dkappa: 0
+    dkappa: -3.2882972047271134e-05
     ddkappa: 0
   }
   v: 2.5700345411652403
@@ -1147,7 +1147,7 @@ trajectory_point {
     theta: 1.2973706445374755
     kappa: -0.0012762074101611144
     s: 15.104911224737908
-    dkappa: 0
+    dkappa: -2.2909050543482536e-05
     ddkappa: 0
   }
   v: 2.50403733850293
@@ -1161,7 +1161,7 @@ trajectory_point {
     theta: 1.297054718051097
     kappa: -0.0012732028468460039
     s: 15.352045363867285
-    dkappa: 0
+    dkappa: -1.3193108906274316e-05
     ddkappa: 0
   }
   v: 2.4387245845727108
@@ -1175,7 +1175,7 @@ trajectory_point {
     theta: 1.2967471190034889
     kappa: -0.0012702774793507932
     s: 15.592665414684671
-    dkappa: 0
+    dkappa: -3.7332650388112694e-06
     ddkappa: 0
   }
   v: 2.3736913169933569
@@ -1189,7 +1189,7 @@ trajectory_point {
     theta: 1.2964478355296032
     kappa: -0.0012674311943695353
     s: 15.826780696936311
-    dkappa: 0
+    dkappa: 5.4708474595621721e-06
     ddkappa: 0
   }
   v: 2.308571434908846
@@ -1203,7 +1203,7 @@ trajectory_point {
     theta: 1.2961616596041889
     kappa: -0.0012615838702054818
     s: 16.054365863359102
-    dkappa: 0
+    dkappa: 1.3698085619546217e-05
     ddkappa: 0
   }
   v: 2.243037698988354
@@ -1217,7 +1217,7 @@ trajectory_point {
     theta: 1.2958974365359115
     kappa: -0.001247057698574986
     s: 16.275364785833123
-    dkappa: 0
+    dkappa: 1.9618718678361804e-05
     ddkappa: 0
   }
   v: 2.1768017314262611
@@ -1231,7 +1231,7 @@ trajectory_point {
     theta: 1.2956411870297317
     kappa: -0.0012329698952076005
     s: 16.489694441534144
-    dkappa: 0
+    dkappa: 2.5360679909766476e-05
     ddkappa: 0
   }
   v: 2.1096140159421481
@@ -1245,7 +1245,7 @@ trajectory_point {
     theta: 1.2953930378980199
     kappa: -0.0012193274295036677
     s: 16.697248799086168
-    dkappa: 0
+    dkappa: 3.0921128698688504e-05
     ddkappa: 0
   }
   v: 2.0412638977807966
@@ -1259,7 +1259,7 @@ trajectory_point {
     theta: 1.2951531388093642
     kappa: -0.0012061385277712592
     s: 16.897902704713921
-    dkappa: 0
+    dkappa: 3.6296712134756139e-05
     ddkappa: 0
   }
   v: 1.9715795837121903
@@ -1273,7 +1273,7 @@ trajectory_point {
     theta: 1.2949216576426685
     kappa: -0.0011934124177909091
     s: 17.091515768395396
-    dkappa: 0
+    dkappa: 4.1483669123588808e-05
     ddkappa: 0
   }
   v: 1.9004281420315146
@@ -1287,7 +1287,7 @@ trajectory_point {
     theta: 1.2946987758412332
     kappa: -0.0011811590733803497
     s: 17.27793625001436
-    dkappa: 0
+    dkappa: 4.6477934498087659e-05
     ddkappa: 0
   }
   v: 1.8277155025591563
@@ -1301,7 +1301,7 @@ trajectory_point {
     theta: 1.2944846837668333
     kappa: -0.0011693889589592431
     s: 17.457004945512882
-    dkappa: 0
+    dkappa: 5.1275243129726631e-05
     ddkappa: 0
   }
   v: 1.7533864566407027
@@ -1315,7 +1315,7 @@ trajectory_point {
     theta: 1.2942795760538093
     kappa: -0.0011581127741139175
     s: 17.628559073043853
-    dkappa: 0
+    dkappa: 5.5871234039843109e-05
     ddkappa: 0
   }
   v: 1.6774246571469438
@@ -1329,7 +1329,7 @@ trajectory_point {
     theta: 1.2940836469631796
     kappa: -0.001147341198162102
     s: 17.792436159123486
-    dkappa: 0
+    dkappa: 6.0261554510928227e-05
     ddkappa: 0
   }
   v: 1.5998526184738706
@@ -1343,7 +1343,7 @@ trajectory_point {
     theta: 1.2938970857367824
     kappa: -0.0011370846347176587
     s: 17.94847792478388
-    dkappa: 0
+    dkappa: 6.4441964197918842e-05
     ddkappa: 0
   }
   v: 1.5207317165426757
@@ -1357,7 +1357,7 @@ trajectory_point {
     theta: 1.2937298562472963
     kappa: -0.0011244990220239388
     s: 18.096534171725491
-    dkappa: 0
+    dkappa: 6.7156844097871615e-05
     ddkappa: 0
   }
   v: 1.4401621887997527
@@ -1371,7 +1371,7 @@ trajectory_point {
     theta: 1.2935762474830625
     kappa: -0.0011113074390662861
     s: 18.236466668469685
-    dkappa: 0
+    dkappa: 6.915416398388956e-05
     ddkappa: 0
   }
   v: 1.3582831342166977
@@ -1385,7 +1385,7 @@ trajectory_point {
     theta: 1.2934316907447694
     kappa: -0.0010988932272939635
     s: 18.368153036511245
-    dkappa: 0
+    dkappa: 7.1033783141173e-05
     ddkappa: 0
   }
   v: 1.2752725132903069
@@ -1399,7 +1399,7 @@ trajectory_point {
     theta: 1.293296298715348
     kappa: -0.0010872660626287044
     s: 18.491490636470896
-    dkappa: 0
+    dkappa: 7.2794236551387441e-05
     ddkappa: 0
   }
   v: 1.1913471480425792
@@ -1413,7 +1413,7 @@ trajectory_point {
     theta: 1.2931701581410067
     kappa: -0.00107643339381251
     s: 18.606400454247819
-    dkappa: 0
+    dkappa: 7.4434396410495154e-05
     ddkappa: 0
   }
   v: 1.1067627220207146
@@ -1427,7 +1427,7 @@ trajectory_point {
     theta: 1.2930533255655428
     kappa: -0.0010664000760559817
     s: 18.712830987172175
-    dkappa: 0
+    dkappa: 7.595352759756966e-05
     ddkappa: 0
   }
   v: 1.0218137802971139
@@ -1441,7 +1441,7 @@ trajectory_point {
     theta: 1.2929458230646571
     kappa: -0.0010571680046866554
     s: 18.810762130157631
-    dkappa: 0
+    dkappa: 7.7351343143610313e-05
     ddkappa: 0
   }
   v: 0.93683372946938059
@@ -1455,7 +1455,7 @@ trajectory_point {
     theta: 1.2928476339802675
     kappa: -0.0010487357487973358
     s: 18.900209061853868
-    dkappa: 0
+    dkappa: 7.86280597003568e-05
     ddkappa: 0
   }
   v: 0.852194837660319
@@ -1469,7 +1469,7 @@ trajectory_point {
     theta: 1.2927586986548125
     kappa: -0.001041098184894429
     s: 18.981226130799111
-    dkappa: 0
+    dkappa: 7.9784453009103612e-05
     ddkappa: 0
   }
   v: 0.76830823451793462
@@ -1483,7 +1483,7 @@ trajectory_point {
     theta: 1.2926789101655434
     kappa: -0.0010342461305462775
     s: 19.053910741572643
-    dkappa: 0
+    dkappa: 8.0821913369514669e-05
     ddkappa: 0
   }
   v: 0.68562391121543442
@@ -1497,7 +1497,7 @@ trajectory_point {
     theta: 1.2926081100587967
     kappa: -0.0010281659780314919
     s: 19.118407240947334
-    dkappa: 0
+    dkappa: 8.1742501108437871e-05
     ddkappa: 0
   }
   v: 0.60463072045122823
@@ -1511,7 +1511,7 @@ trajectory_point {
     theta: 1.2925460840842427
     kappa: -0.0010228393279872868
     s: 19.174910804042145
-    dkappa: 0
+    dkappa: 8.25490020487194e-05
     ddkappa: 0
   }
   v: 0.52585637644892536
@@ -1525,7 +1525,7 @@ trajectory_point {
     theta: 1.2924925579291062
     kappa: -0.0010182426230578127
     s: 19.223671320474672
-    dkappa: 0
+    dkappa: 8.3244982978018675e-05
     ddkappa: 0
   }
   v: 0.44986745495733782
@@ -1539,7 +1539,7 @@ trajectory_point {
     theta: 1.2924471929523578
     kappa: -0.0010143467815424913
     s: 19.264997280513633
-    dkappa: 0
+    dkappa: 8.38348471176223e-05
     ddkappa: 0
   }
   v: 0.37726939325047915
@@ -1553,7 +1553,7 @@ trajectory_point {
     theta: 1.2924095819188703
     kappa: -0.0010111168310443469
     s: 19.299259661231428
-    dkappa: 0
+    dkappa: 8.4323889591259217e-05
     ddkappa: 0
   }
   v: 0.30870649012756424
@@ -1567,7 +1567,7 @@ trajectory_point {
     theta: 1.2923792447335487
     kappa: -0.0010085115421183434
     s: 19.32689581265662
-    dkappa: 0
+    dkappa: 8.4718352893914663e-05
     ddkappa: 0
   }
   v: 0.24486190591300827
@@ -1581,7 +1581,7 @@ trajectory_point {
     theta: 1.2923556241754235
     kappa: -0.001006483061919714
     s: 19.348413343926488
-    dkappa: 0
+    dkappa: 8.5025482360645235e-05
     ddkappa: 0
   }
   v: 0.18645766245642997
@@ -1595,7 +1595,7 @@ trajectory_point {
     theta: 1.2923380816317218
     kappa: -0.0010049765478522983
     s: 19.364394009439525
-    dkappa: 0
+    dkappa: 8.5253581635392984e-05
     ddkappa: 0
   }
   v: 0.13425464313264765
@@ -1609,7 +1609,7 @@ trajectory_point {
     theta: 1.2923258928319128
     kappa: -0.0010039298012168741
     s: 19.375497595007968
-    dkappa: 0
+    dkappa: 8.5412068139800119e-05
     ddkappa: 0
   }
   v: 0.089052592841682876
@@ -1623,7 +1623,7 @@ trajectory_point {
     theta: 1.2923182435817373
     kappa: -0.0010032729008594902
     s: 19.382465804010327
-    dkappa: 0
+    dkappa: 8.5511528542023789e-05
     ddkappa: 0
   }
   v: 0.0516901180087567
@@ -1637,7 +1637,7 @@ trajectory_point {
     theta: 1.2923142254972235
     kappa: -0.0010029278368198047
     s: 19.386126143543862
-    dkappa: 0
+    dkappa: 8.5563774225549862e-05
     ddkappa: 0
   }
   v: 0.023044686584292977
@@ -1651,7 +1651,7 @@ trajectory_point {
     theta: 1.2923128317386976
     kappa: -0.0010028081439794119
     s: 19.387395810577178
-    dkappa: 0
+    dkappa: 8.5581896758008487e-05
     ddkappa: 0
   }
   v: 0.0040326280439178111
@@ -1665,7 +1665,7 @@ trajectory_point {
     theta: 1.2923129527447936
     kappa: -0.0010028185357101814
     s: 19.387285578102674
-    dkappa: 0
+    dkappa: 8.5580323359987757e-05
     ddkappa: 0
   }
   v: 0
@@ -1679,7 +1679,7 @@ trajectory_point {
     theta: 1.2923133719664681
     kappa: -0.001002854537522589
     s: 19.3869036812891
-    dkappa: 0
+    dkappa: 8.557487237384866e-05
     ddkappa: 0
   }
   v: 3.0489416841993562e-15
@@ -1693,7 +1693,7 @@ trajectory_point {
     theta: 1.2923127616010233
     kappa: -0.0010028021207140526
     s: 19.387459703634061
-    dkappa: 0
+    dkappa: 8.5582808732539482e-05
     ddkappa: 0
   }
   v: 0.01454290736159085
diff --git a/modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt b/modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt
index 9b8d15922bb3c7c783360f41457a770fca4a36f8..7b22d0dbfd3f44dbbfd7dfae8f92b2fede7df70a 100644
--- a/modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt
+++ b/modules/planning/testdata/garage_test/result_stop_obstacle_0.pb.txt
@@ -12,7 +12,7 @@ trajectory_point {
     theta: 1.6602776953500005
     kappa: -1.3193375626513692e-17
     s: 8.6820877093565785e-20
-    dkappa: 0
+    dkappa: -1.1095984692024601e-21
     ddkappa: 0
   }
   v: 4.2611112594604492
@@ -26,7 +26,7 @@ trajectory_point {
     theta: 1.6590842604797489
     kappa: -0.0019049604145966611
     s: 0.085455697767938335
-    dkappa: 0
+    dkappa: -0.0010921510424934373
     ddkappa: 0
   }
   v: 4.2841574905276776
@@ -40,7 +40,7 @@ trajectory_point {
     theta: 1.6578846075478355
     kappa: -0.0038197957571773286
     s: 0.17135438052119
-    dkappa: 0
+    dkappa: -0.0021899635742281503
     ddkappa: 0
   }
   v: 4.3054158448043074
@@ -54,7 +54,7 @@ trajectory_point {
     theta: 1.6566792338630896
     kappa: -0.0057437170431310159
     s: 0.25766065475239147
-    dkappa: 0
+    dkappa: -0.0032929852549041447
     ddkappa: 0
   }
   v: 4.3249226661200355
@@ -68,7 +68,7 @@ trajectory_point {
     theta: 1.6554686267755818
     kappa: -0.0076759514122934488
     s: 0.34433985028986414
-    dkappa: 0
+    dkappa: -0.0044007729886819244
     ddkappa: 0
   }
   v: 4.3427139442136333
@@ -82,7 +82,7 @@ trajectory_point {
     theta: 1.6542532637618796
     kappa: -0.0096157419710806609
     s: 0.43135801321579514
-    dkappa: 0
+    dkappa: -0.0055128928336745861
     ddkappa: 0
   }
   v: 4.3588253147329477
@@ -96,7 +96,7 @@ trajectory_point {
     theta: 1.6530336125103335
     kappa: -0.011562347634622605
     s: 0.51868189878441984
-    dkappa: 0
+    dkappa: -0.0066289199114399353
     ddkappa: 0
   }
   v: 4.3732920592348972
@@ -110,7 +110,7 @@ trajectory_point {
     theta: 1.6518101310064397
     kappa: -0.013515042968896732
     s: 0.60627896434020223
-    dkappa: 0
+    dkappa: -0.0077484383164725763
     ddkappa: 0
   }
   v: 4.3861491051854795
@@ -124,7 +124,7 @@ trajectory_point {
     theta: 1.650583267618327
     kappa: -0.015473118032861586
     s: 0.69411736223601728
-    dkappa: 0
+    dkappa: -0.0088710410256960248
     ddkappa: 0
   }
   v: 4.3974310259597624
@@ -138,7 +138,7 @@ trajectory_point {
     theta: 1.6493534611824003
     kappa: -0.017435878220590405
     s: 0.78216593275133184
-    dkappa: 0
+    dkappa: -0.0099963298079548055
     ddkappa: 0
   }
   v: 4.4071720408418908
@@ -152,7 +152,7 @@ trajectory_point {
     theta: 1.6481211410891869
     kappa: -0.019402644103404709
     s: 0.87039419701038667
-    dkappa: 0
+    dkappa: -0.01112391513350656
     ddkappa: 0
   }
   v: 4.4154060150250842
@@ -166,7 +166,7 @@ trajectory_point {
     theta: 1.6468867273694121
     kappa: -0.021372751272007905
     s: 0.95877234990037774
-    dkappa: 0
+    dkappa: -0.012253416083514154
     ddkappa: 0
   }
   v: 4.4221664596116348
@@ -180,7 +180,7 @@ trajectory_point {
     theta: 1.6456506307803445
     kappa: -0.023345550178618849
     s: 1.0472712529896371
-    dkappa: 0
+    dkappa: -0.013384460259537762
     ddkappa: 0
   }
   v: 4.42748653161291
@@ -194,7 +194,7 @@ trajectory_point {
     theta: 1.6444132528924327
     kappa: -0.025320405979105474
     s: 1.1358624274458153
-    dkappa: 0
+    dkappa: -0.014516683693026991
     ddkappa: 0
   }
   v: 4.4313990339493534
@@ -208,7 +208,7 @@ trajectory_point {
     theta: 1.6431749861762677
     kappa: -0.027296698375118382
     s: 1.2245180469540626
-    dkappa: 0
+    dkappa: -0.01564973075481298
     ddkappa: 0
   }
   v: 4.43393641545048
@@ -222,7 +222,7 @@ trajectory_point {
     theta: 1.6419362140898903
     kappa: -0.029273821456224397
     s: 1.31321093063521
-    dkappa: 0
+    dkappa: -0.016783254064600482
     ddkappa: 0
   }
   v: 4.4351307708548831
@@ -236,7 +236,7 @@ trajectory_point {
     theta: 1.6406973111664691
     kappa: -0.031251183542040206
     s: 1.4019145359639513
-    dkappa: 0
+    dkappa: -0.017916914400460011
     ddkappa: 0
   }
   v: 4.4350138408102264
@@ -250,7 +250,7 @@ trajectory_point {
     theta: 1.6394586431023677
     kappa: -0.033228207024365942
     s: 1.4906029516870241
-    dkappa: 0
+    dkappa: -0.019050380608319893
     ddkappa: 0
   }
   v: 4.4336170118732516
@@ -264,7 +264,7 @@ trajectory_point {
     theta: 1.6382205668456176
     kappa: -0.035204328209318743
     s: 1.5792508907413918
-    dkappa: 0
+    dkappa: -0.020183329511458414
     ddkappa: 0
   }
   v: 4.4309713165097735
@@ -278,7 +278,7 @@ trajectory_point {
     theta: 1.6369834306848157
     kappa: -0.03717899715946639
     s: 1.6678336831724248
-    dkappa: 0
+    dkappa: -0.021315445819995886
     ddkappa: 0
   }
   v: 4.4271074330946814
@@ -292,7 +292,7 @@ trajectory_point {
     theta: 1.6352634919613869
     kappa: -0.038976798224074315
     s: 1.7563272690520815
-    dkappa: 0
+    dkappa: -0.02219520165900114
     ddkappa: 0
   }
   v: 4.4220556859119391
@@ -306,7 +306,7 @@ trajectory_point {
     theta: 1.6304919612136237
     kappa: -0.039669536359954148
     s: 1.8447081913970917
-    dkappa: 0
+    dkappa: -0.021489663051105619
     ddkappa: 0
   }
   v: 4.4158460451545842
@@ -320,7 +320,7 @@ trajectory_point {
     theta: 1.6257257681637995
     kappa: -0.040361212240605626
     s: 1.9329535890871354
-    dkappa: 0
+    dkappa: -0.020785206326877567
     ddkappa: 0
   }
   v: 4.4085081269247306
@@ -334,7 +334,7 @@ trajectory_point {
     theta: 1.6209663404558003
     kappa: -0.041051651293300608
     s: 2.0210411897830256
-    dkappa: 0
+    dkappa: -0.020082009284814295
     ddkappa: 0
   }
   v: 4.4000711932335648
@@ -348,7 +348,7 @@ trajectory_point {
     theta: 1.616215074218158
     kappa: -0.041740683504725914
     s: 2.1089493028448913
-    dkappa: 0
+    dkappa: -0.019380245079748924
     ddkappa: 0
   }
   v: 4.39056415200135
@@ -362,7 +362,7 @@ trajectory_point {
     theta: 1.6114733338016061
     kappa: -0.042428143365475304
     s: 2.1966568122503554
-    dkappa: 0
+    dkappa: -0.01868008227938403
     ddkappa: 0
   }
   v: 4.3800155570574217
@@ -376,7 +376,7 @@ trajectory_point {
     theta: 1.6067424515436222
     kappa: -0.043113869814541593
     s: 2.2841431695127197
-    dkappa: 0
+    dkappa: -0.017981684920825313
     ddkappa: 0
   }
   v: 4.3684536081401912
@@ -390,7 +390,7 @@ trajectory_point {
     theta: 1.6020237275608318
     kappa: -0.0437977061838086
     s: 2.3713883865991447
-    dkappa: 0
+    dkappa: -0.017285212567115214
     ddkappa: 0
   }
   v: 4.3559061508971437
@@ -404,7 +404,7 @@ trajectory_point {
     theta: 1.597318429570002
     kappa: -0.044479500142543218
     s: 2.4583730288488304
-    dkappa: 0
+    dkappa: -0.016590820363766617
     ddkappa: 0
   }
   v: 4.3424006768848393
@@ -418,7 +418,7 @@ trajectory_point {
     theta: 1.5926277927382335
     kappa: -0.045159103641887438
     s: 2.5450782078912009
-    dkappa: 0
+    dkappa: -0.015898659095296447
     ddkappa: 0
   }
   v: 4.3279643235689127
@@ -432,7 +432,7 @@ trajectory_point {
     theta: 1.5879530195628182
     kappa: -0.045836372859350355
     s: 2.6314855745640813
-    dkappa: 0
+    dkappa: -0.015208875241759378
     ddkappa: 0
   }
   v: 4.312623874324073
@@ -446,7 +446,7 @@ trajectory_point {
     theta: 1.5832952797811135
     kappa: -0.046511168143300231
     s: 2.7175773118318833
-    dkappa: 0
+    dkappa: -0.014521611035281438
     ddkappa: 0
   }
   v: 4.2964057584341031
@@ -460,7 +460,7 @@ trajectory_point {
     theta: 1.5786557103106575
     kappa: -0.0471833539574565
     s: 2.8033361277037838
-    dkappa: 0
+    dkappa: -0.013837004516593698
     ddkappa: 0
   }
   v: 4.2793360510918621
@@ -474,7 +474,7 @@ trajectory_point {
     theta: 1.57403541521963
     kappa: -0.047852798825381779
     s: 2.8887452481519094
-    dkappa: 0
+    dkappa: -0.013155189591565896
     ddkappa: 0
   }
   v: 4.2614404733992819
@@ -488,7 +488,7 @@ trajectory_point {
     theta: 1.5694354657276606
     kappa: -0.04851937527497395
     s: 2.9737884100295138
-    dkappa: 0
+    dkappa: -0.012476296087740115
     ddkappa: 0
   }
   v: 4.24274439236737
@@ -502,7 +502,7 @@ trajectory_point {
     theta: 1.5648569002368644
     kappa: -0.049182959782958127
     s: 3.0584498539891625
-    dkappa: 0
+    dkappa: -0.011800449810864422
     ddkappa: 0
   }
   v: 4.2232728209162076
@@ -516,7 +516,7 @@ trajectory_point {
     theta: 1.5603007243929001
     kappa: -0.049843432719378751
     s: 3.1427143174009147
-    dkappa: 0
+    dkappa: -0.011127772601426507
     ddkappa: 0
   }
   v: 4.2030504178749517
@@ -530,7 +530,7 @@ trajectory_point {
     theta: 1.5557679111757283
     kappa: -0.050500678292091532
     s: 3.226567027270502
-    dkappa: 0
+    dkappa: -0.010458382391187366
     ddkappa: 0
   }
   v: 4.1821014879818312
@@ -544,7 +544,7 @@ trajectory_point {
     theta: 1.5512594010196792
     kappa: -0.051154584491255553
     s: 3.3099936931575118
-    dkappa: 0
+    dkappa: -0.0097923932597149387
     ddkappa: 0
   }
   v: 4.1604499818841534
@@ -558,7 +558,7 @@ trajectory_point {
     theta: 1.5467761019623329
     kappa: -0.051805043033825271
     s: 3.3929805000935676
-    dkappa: 0
+    dkappa: -0.00912991549091776
     ddkappa: 0
   }
   v: 4.1381194961382963
@@ -572,7 +572,7 @@ trajectory_point {
     theta: 1.5423188898216553
     kappa: -0.052451949308042541
     s: 3.4755141015005124
-    dkappa: 0
+    dkappa: -0.0084710556295786114
     ddkappa: 0
   }
   v: 4.1151332732097154
@@ -586,7 +586,7 @@ trajectory_point {
     theta: 1.5378997914993024
     kappa: -0.053071241204440706
     s: 3.5575816121085886
-    dkappa: 0
+    dkappa: -0.0078255860260034883
     ddkappa: 0
   }
   v: 4.091514201472938
@@ -600,7 +600,7 @@ trajectory_point {
     theta: 1.5338238453642417
     kappa: -0.053007098223303011
     s: 3.639170600874619
-    dkappa: 0
+    dkappa: -0.007458222432479969
     ddkappa: 0
   }
   v: 4.0672848152115693
@@ -614,7 +614,7 @@ trajectory_point {
     theta: 1.5297709630732717
     kappa: -0.052943340864060294
     s: 3.7202690839001917
-    dkappa: 0
+    dkappa: -0.0070930673961917294
     ddkappa: 0
   }
   v: 4.0424672946182847
@@ -628,7 +628,7 @@ trajectory_point {
     theta: 1.5257418791023272
     kappa: -0.05287997820215317
     s: 3.8008655173498371
-    dkappa: 0
+    dkappa: -0.0067301728945556409
     ddkappa: 0
   }
   v: 4.0170834657948378
@@ -642,7 +642,7 @@ trajectory_point {
     theta: 1.5217373022235972
     kappa: -0.052817018972624571
     s: 3.88094879036921
-    dkappa: 0
+    dkappa: -0.00636958895544239
     ddkappa: 0
   }
   v: 3.9911548007520561
@@ -656,7 +656,7 @@ trajectory_point {
     theta: 1.517757915674163
     kappa: -0.052754471575687314
     s: 3.9605082180032762
-    dkappa: 0
+    dkappa: -0.0060113636890631386
     ddkappa: 0
   }
   v: 3.9647024174098395
@@ -670,7 +670,7 @@ trajectory_point {
     theta: 1.5138043773441543
     kappa: -0.05269234408229162
     s: 4.0395335341144882
-    dkappa: 0
+    dkappa: -0.0056555433198562325
     ddkappa: 0
   }
   v: 3.9377470795971647
@@ -684,7 +684,7 @@ trajectory_point {
     theta: 1.50987731998393
     kappa: -0.052630644239692666
     s: 4.1180148843009672
-    dkappa: 0
+    dkappa: -0.0053021722183738892
     ddkappa: 0
   }
   v: 3.9103091970520816
@@ -698,7 +698,7 @@ trajectory_point {
     theta: 1.505977351429749
     kappa: -0.052569379477018043
     s: 4.1959428188146894
-    dkappa: 0
+    dkappa: -0.0049512929331688525
     ddkappa: 0
   }
   v: 3.8824088254217153
@@ -712,7 +712,7 @@ trajectory_point {
     theta: 1.5021050548473736
     kappa: -0.052508556910835349
     s: 4.2733082854796622
-    dkappa: 0
+    dkappa: -0.0046029462226811037
     ddkappa: 0
   }
   v: 3.8540656662622657
@@ -726,7 +726,7 @@ trajectory_point {
     theta: 1.4831773696395547
     kappa: -0.052211304929474442
     s: 4.6514087126467558
-    dkappa: 0
+    dkappa: -0.0029005065462291658
     ddkappa: 0
   }
   v: 3.7063728256309503
@@ -740,7 +740,7 @@ trajectory_point {
     theta: 1.4650152749266205
     kappa: -0.051926005366717459
     s: 5.0143058279330175
-    dkappa: 0
+    dkappa: -0.0012665214905596661
     ddkappa: 0
   }
   v: 3.5503619279202914
@@ -754,7 +754,7 @@ trajectory_point {
     theta: 1.4476657585652535
     kappa: -0.051653229568665075
     s: 5.3612728869657706
-    dkappa: 0
+    dkappa: 0.0002957366960944238
     ddkappa: 0
   }
   v: 3.3880973153640439
@@ -768,7 +768,7 @@ trajectory_point {
     theta: 1.4312902765250226
     kappa: -0.050647717980075183
     s: 5.6917785142542936
-    dkappa: 0
+    dkappa: 0.0011680870538960636
     ddkappa: 0
   }
   v: 3.2214220233675332
@@ -782,7 +782,7 @@ trajectory_point {
     theta: 1.4157971453691223
     kappa: -0.049345482776413423
     s: 6.0054645725069751
-    dkappa: 0
+    dkappa: 0.0017087480842162251
     ddkappa: 0
   }
   v: 3.05195778050766
@@ -796,7 +796,7 @@ trajectory_point {
     theta: 1.4011352094188012
     kappa: -0.048113931735427079
     s: 6.3021240319484733
-    dkappa: 0
+    dkappa: 0.0022200625171537028
     ddkappa: 0
   }
   v: 2.8811050085328982
@@ -810,7 +810,7 @@ trajectory_point {
     theta: 1.387315726234436
     kappa: -0.046953388885825925
     s: 6.5816788396368695
-    dkappa: 0
+    dkappa: 0.0027018958227130229
     ddkappa: 0
   }
   v: 2.7100428223632931
@@ -824,7 +824,7 @@ trajectory_point {
     theta: 1.3743425954588664
     kappa: -0.045863734695584281
     s: 6.8441577887808336
-    dkappa: 0
+    dkappa: 0.0031542976281104928
     ddkappa: 0
   }
   v: 2.5397290300904634
@@ -838,7 +838,7 @@ trajectory_point {
     theta: 1.362213737990164
     kappa: -0.044844497945180466
     s: 7.0896743880567712
-    dkappa: 0
+    dkappa: 0.0035774635739119608
     ddkappa: 0
   }
   v: 2.3709001329776038
@@ -852,7 +852,7 @@ trajectory_point {
     theta: 1.3509224830724247
     kappa: -0.043894947600835905
     s: 7.3184047309259892
-    dkappa: 0
+    dkappa: 0.0039716971701706117
     ddkappa: 0
   }
   v: 2.2040713254594788
@@ -866,7 +866,7 @@ trajectory_point {
     theta: 1.3415750693514499
     kappa: -0.042740310701854523
     s: 7.5305691203353415
-    dkappa: 0
+    dkappa: 0.0042901528032650311
     ddkappa: 0
   }
   v: 2.0396877792010857
@@ -880,7 +880,7 @@ trajectory_point {
     theta: 1.3339401742724843
     kappa: -0.041429972632526645
     s: 7.7264529572030476
-    dkappa: 0
+    dkappa: 0.004542042925350113
     ddkappa: 0
   }
   v: 1.8786212440291743
@@ -894,7 +894,7 @@ trajectory_point {
     theta: 1.3269223293080719
     kappa: -0.040226009558571821
     s: 7.9064346824491745
-    dkappa: 0
+    dkappa: 0.0047734842707427352
     ddkappa: 0
   }
   v: 1.7217910060784707
@@ -908,7 +908,7 @@ trajectory_point {
     theta: 1.3205046707531269
     kappa: -0.039125306133280663
     s: 8.07098001257917
-    dkappa: 0
+    dkappa: 0.0049850757116586437
     ddkappa: 0
   }
   v: 1.5700232909219061
@@ -922,7 +922,7 @@ trajectory_point {
     theta: 1.3146669045916797
     kappa: -0.038124225443468523
     s: 8.22063263362769
-    dkappa: 0
+    dkappa: 0.0051775163825636284
     ddkappa: 0
   }
   v: 1.4240512635706155
@@ -936,7 +936,7 @@ trajectory_point {
     theta: 1.3093857542892295
     kappa: -0.037218671261062312
     s: 8.3560048951024051
-    dkappa: 0
+    dkappa: 0.0053515937133687694
     ddkappa: 0
   }
   v: 1.2845150284739386
@@ -950,7 +950,7 @@ trajectory_point {
     theta: 1.3046353976341609
     kappa: -0.036404150294687382
     s: 8.47776850392784
-    dkappa: 0
+    dkappa: 0.0055081714626257078
     ddkappa: 0
   }
   v: 1.1519616295194197
@@ -964,7 +964,7 @@ trajectory_point {
     theta: 1.300387892036398
     kappa: -0.035675834441254384
     s: 8.5866452183891777
-    dkappa: 0
+    dkappa: 0.0056481777507219081
     ddkappa: 0
   }
   v: 1.026845050032807
@@ -978,7 +978,7 @@ trajectory_point {
     theta: 1.296613588450936
     kappa: -0.035028623037546204
     s: 8.6833975420760883
-    dkappa: 0
+    dkappa: 0.005772593093075911
     ddkappa: 0
   }
   v: 0.90952621277805346
@@ -992,7 +992,7 @@ trajectory_point {
     theta: 1.2932815346485744
     kappa: -0.034457205111804765
     s: 8.7688194178265455
-    dkappa: 0
+    dkappa: 0.0058824384333326027
     ddkappa: 0
   }
   v: 0.80027297995731628
@@ -1006,7 +1006,7 @@ trajectory_point {
     theta: 1.2903598689261342
     kappa: -0.033956121635317874
     s: 8.8437269216706547
-    dkappa: 0
+    dkappa: 0.0059787631765584794
     ddkappa: 0
   }
   v: 0.699260153210957
@@ -1020,7 +1020,7 @@ trajectory_point {
     theta: 1.2878162055594327
     kappa: -0.033519827774006222
     s: 8.90894895677446
-    dkappa: 0
+    dkappa: 0.0060626332224368966
     ddkappa: 0
   }
   v: 0.60656947361754177
@@ -1034,7 +1034,7 @@ trajectory_point {
     theta: 1.285618013382821
     kappa: -0.033142755140010083
     s: 8.9653179473837774
-    dkappa: 0
+    dkappa: 0.006135118998463346
     ddkappa: 0
   }
   v: 0.52218962169384064
@@ -1048,7 +1048,7 @@ trajectory_point {
     theta: 1.2837329888580853
     kappa: -0.032819374043276339
     s: 9.013660532768009
-    dkappa: 0
+    dkappa: 0.0061972834931407024
     ddkappa: 0
   }
   v: 0.44601621739482855
@@ -1062,7 +1062,7 @@ trajectory_point {
     theta: 1.282129424900591
     kappa: -0.032544255743145262
     s: 9.0547882611639636
-    dkappa: 0
+    dkappa: 0.0062501702891744953
     ddkappa: 0
   }
   v: 0.37785182011368423
@@ -1076,7 +1076,7 @@ trajectory_point {
     theta: 1.2807765765867758
     kappa: -0.032312134699937413
     s: 9.0894882837196782
-    dkappa: 0
+    dkappa: 0.0062947915966681664
     ddkappa: 0
   }
   v: 0.31740592868179185
@@ -1090,7 +1090,7 @@ trajectory_point {
     theta: 1.279645024696219
     kappa: -0.032117970826540478
     s: 9.1185140484382412
-    dkappa: 0
+    dkappa: 0.0063321162863183376
     ddkappa: 0
   }
   v: 0.26429498136873875
@@ -1104,7 +1104,7 @@ trajectory_point {
     theta: 1.2787070378614733
     kappa: -0.031957011739996218
     s: 9.1425759941216054
-    dkappa: 0
+    dkappa: 0.006363057922610057
     ddkappa: 0
   }
   v: 0.21804235588231791
@@ -1118,7 +1118,7 @@ trajectory_point {
     theta: 1.2779369339236941
     kappa: -0.031824855013087253
     s: 9.1623322443144151
-    dkappa: 0
+    dkappa: 0.0063884627970120789
     ddkappa: 0
   }
   v: 0.17807836936852528
@@ -1132,7 +1132,7 @@ trajectory_point {
     theta: 1.2773114409320834
     kappa: -0.031717510425923967
     s: 9.17837930124783
-    dkappa: 0
+    dkappa: 0.0064090979611721218
     ddkappa: 0
   }
   v: 0.14374027841156195
@@ -1146,7 +1146,7 @@ trajectory_point {
     theta: 1.2768099979727037
     kappa: -0.031631451900442489
     s: 9.1912442820959672
-    dkappa: 0
+    dkappa: 0.0064256412433963111
     ddkappa: 0
   }
   v: 0.11433491100126075
@@ -1160,7 +1160,7 @@ trajectory_point {
     theta: 1.2764143964222001
     kappa: -0.031563556208970854
     s: 9.20139408149779
-    dkappa: 0
+    dkappa: 0.006438693030880906
     ddkappa: 0
   }
   v: 0.089364140911061346
@@ -1174,7 +1174,7 @@ trajectory_point {
     theta: 1.2761081798485923
     kappa: -0.031511000205745168
     s: 9.2092507345274619
-    dkappa: 0
+    dkappa: 0.00644879602519871
     ddkappa: 0
   }
   v: 0.068402754128198462
@@ -1188,7 +1188,7 @@ trajectory_point {
     theta: 1.275876513510243
     kappa: -0.031471238605469652
     s: 9.21519473860113
-    dkappa: 0
+    dkappa: 0.0064564395139915182
     ddkappa: 0
   }
   v: 0.051045211505198647
@@ -1202,7 +1202,7 @@ trajectory_point {
     theta: 1.275706106855141
     kappa: -0.031441990822085236
     s: 9.2195670209634635
-    dkappa: 0
+    dkappa: 0.0064620619009921479
     ddkappa: 0
   }
   v: 0.036905648759880366
@@ -1216,7 +1216,7 @@ trajectory_point {
     theta: 1.2755851362076078
     kappa: -0.031421227807538192
     s: 9.2226709061741712
-    dkappa: 0
+    dkappa: 0.00646605323604646
     ddkappa: 0
   }
   v: 0.0256178764753538
@@ -1230,7 +1230,7 @@ trajectory_point {
     theta: 1.27550316761669
     kappa: -0.031407158890548736
     s: 9.2247740835945464
-    dkappa: 0
+    dkappa: 0.006468757745135399
     ddkappa: 0
   }
   v: 0.016835380100021023
@@ -1244,7 +1244,7 @@ trajectory_point {
     theta: 1.2754510798380134
     kappa: -0.0313982186153797
     s: 9.226110574873978
-    dkappa: 0
+    dkappa: 0.0064704763603969989
     ddkappa: 0
   }
   v: 0.010231319947575984
@@ -1258,7 +1258,7 @@ trajectory_point {
     theta: 1.2754209874215161
     kappa: -0.031393053580605082
     s: 9.2268827014364962
-    dkappa: 0
+    dkappa: 0.0064714692501484355
     ddkappa: 0
   }
   v: 0.0054985311970043712
@@ -1272,7 +1272,7 @@ trajectory_point {
     theta: 1.2754061638799465
     kappa: -0.031390509277878745
     s: 9.22726305196729
-    dkappa: 0
+    dkappa: 0.0064719583489080308
     ddkappa: 0
   }
   v: 0.0023495238925838645
@@ -1286,7 +1286,7 @@ trajectory_point {
     theta: 1.2754009649164471
     kappa: -0.031389616930702982
     s: 9.227396449899242
-    dkappa: 0
+    dkappa: 0.0064721298874172951
     ddkappa: 0
   }
   v: 0.00051648294388373026
@@ -1300,7 +1300,7 @@ trajectory_point {
     theta: 1.2754007516933101
     kappa: -0.031389580333197172
     s: 9.2274019208994567
-    dkappa: 0
+    dkappa: 0.0064721369226629494
     ddkappa: 0
   }
   v: 0
@@ -1314,7 +1314,7 @@ trajectory_point {
     theta: 1.275401814127703
     kappa: -0.031389762688866361
     s: 9.22737466035579
-    dkappa: 0
+    dkappa: 0.0064721018678989524
     ddkappa: 0
   }
   v: 0
@@ -1328,7 +1328,7 @@ trajectory_point {
     theta: 1.2754012942035777
     kappa: -0.031389673449369958
     s: 9.2273880008633782
-    dkappa: 0
+    dkappa: 0.0064721190226685269
     ddkappa: 0
   }
   v: 0.00053013030717779912
@@ -1342,7 +1342,7 @@ trajectory_point {
     theta: 1.2753971092919629
     kappa: -0.031388955153290293
     s: 9.2274953797111614
-    dkappa: 0
+    dkappa: 0.0064722571028261854
     ddkappa: 0
   }
   v: 0.0016763011828903277
@@ -1356,7 +1356,7 @@ trajectory_point {
     theta: 1.2753878754743566
     kappa: -0.031387370264901258
     s: 9.2277323063684253
-    dkappa: 0
+    dkappa: 0.0064725617705597661
     ddkappa: 0
   }
   v: 0.0030944859415478337
@@ -1370,7 +1370,7 @@ trajectory_point {
     theta: 1.2753728308659562
     kappa: -0.031384788012936933
     s: 9.22811832997132
-    dkappa: 0
+    dkappa: 0.0064730581644124475
     ddkappa: 0
   }
   v: 0.0046349186844708542
@@ -1384,7 +1384,7 @@ trajectory_point {
     theta: 1.2753517589370484
     kappa: -0.031381171229360239
     s: 9.2286590068093872
-    dkappa: 0
+    dkappa: 0.0064737534293047789
     ddkappa: 0
   }
   v: 0.0061675083782716253
@@ -1398,7 +1398,7 @@ trajectory_point {
     theta: 1.2753249118320424
     kappa: -0.031376563188131472
     s: 9.2293478678121037
-    dkappa: 0
+    dkappa: 0.0064746392465567211
     ddkappa: 0
   }
   v: 0.0075818388548540517
@@ -1412,7 +1412,7 @@ trajectory_point {
     theta: 1.2752929336864309
     kappa: -0.031371074443977021
     s: 9.2301683860353965
-    dkappa: 0
+    dkappa: 0.0064756943639096539
     ddkappa: 0
   }
   v: 0.0087871688114144579
@@ -1426,7 +1426,7 @@ trajectory_point {
     theta: 1.2752567839424647
     kappa: -0.03136486967115798
     s: 9.2310959441481746
-    dkappa: 0
+    dkappa: 0.0064768871255484126
     ddkappa: 0
   }
   v: 0.0097124318104401773
@@ -1440,7 +1440,7 @@ trajectory_point {
     theta: 1.2752176606645478
     kappa: -0.031358154502238694
     s: 9.2320998019188636
-    dkappa: 0
+    dkappa: 0.0064781780021233174
     ddkappa: 0
   }
   v: 0.010306236279711065
@@ -1454,7 +1454,7 @@ trajectory_point {
     theta: 1.2751769238553898
     kappa: -0.031351162366855444
     s: 9.233145063701933
-    dkappa: 0
+    dkappa: 0.0064795221207721984
     ddkappa: 0
   }
   v: 0.010536865512298323
@@ -1468,7 +1468,7 @@ trajectory_point {
     theta: 1.2751360187737941
     kappa: -0.03134414133048509
     s: 9.2341946459244184
-    dkappa: 0
+    dkappa: 0.0064808717951424155
     ddkappa: 0
   }
   v: 0.010392277666565513
@@ -1482,7 +1482,7 @@ trajectory_point {
     theta: 1.2750963992546911
     kappa: -0.031337340933213637
     s: 9.2352112445724615
-    dkappa: 0
+    dkappa: 0.0064821790554128985
     ddkappa: 0
   }
   v: 0.00988010576616753
@@ -1496,7 +1496,7 @@ trajectory_point {
     theta: 1.275059451031668
     kappa: -0.031330999028504856
     s: 9.2361593026778337
-    dkappa: 0
+    dkappa: 0.0064833981783161704
     ddkappa: 0
   }
   v: 0.0090276577000513378
@@ -1510,7 +1510,7 @@ trajectory_point {
     theta: 1.2750264150618633
     kappa: -0.031325328621969
     s: 9.2370069778044623
-    dkappa: 0
+    dkappa: 0.0064844882171603609
     ddkappa: 0
   }
   v: 0.0078819162224556157
@@ -1524,7 +1524,7 @@ trajectory_point {
     theta: 1.2749983108527094
     kappa: -0.031320504710131314
     s: 9.2377281095349648
-    dkappa: 0
+    dkappa: 0.0064854155318512519
     ddkappa: 0
   }
   v: 0.0065095389529113319
@@ -1538,7 +1538,7 @@ trajectory_point {
     theta: 1.2749758597897034
     kappa: -0.0313166511192007
     s: 9.238304186957178
-    dkappa: 0
+    dkappa: 0.0064861563189143011
     ddkappa: 0
   }
   v: 0.0049968583762404813
@@ -1552,7 +1552,7 @@ trajectory_point {
     theta: 1.2749594084641798
     kappa: -0.031313827343838377
     s: 9.2387263161506841
-    dkappa: 0
+    dkappa: 0.0064866991415166563
     ddkappa: 0
   }
   v: 0.0034498818425575655
@@ -1566,7 +1566,7 @@ trajectory_point {
     theta: 1.2749488520000225
     kappa: -0.031312015385926428
     s: 9.2389971876733412
-    dkappa: 0
+    dkappa: 0.0064870474594891977
     ddkappa: 0
   }
   v: 0.0019942915672687533
@@ -1580,7 +1580,7 @@ trajectory_point {
     theta: 1.2749435573784322
     kappa: -0.0313111065933365
     s: 9.2391330440478132
-    dkappa: 0
+    dkappa: 0.0064872221593485586
     ddkappa: 0
   }
   v: 0.000775444631072053
@@ -1594,7 +1594,7 @@ trajectory_point {
     theta: 1.2749422867603097
     kappa: -0.031310888498698383
     s: 9.2391656472480985
-    dkappa: 0
+    dkappa: 0.006487264084319151
     ddkappa: 0
   }
   v: 0
@@ -1608,7 +1608,7 @@ trajectory_point {
     theta: 1.2749431208065582
     kappa: -0.031311031658168641
     s: 9.239144246186056
-    dkappa: 0
+    dkappa: 0.0064872365643551923
     ddkappa: 0
   }
   v: 0
@@ -1622,7 +1622,7 @@ trajectory_point {
     theta: 1.2749433819977194
     kappa: -0.031311076490199236
     s: 9.2391375441979413
-    dkappa: 0
+    dkappa: 0.00648722794616274
     ddkappa: 0
   }
   v: 0.0002880576433899426
@@ -1636,7 +1636,7 @@ trajectory_point {
     theta: 1.2749395579558549
     kappa: -0.031310420114306126
     s: 9.2392356665309325
-    dkappa: 0
+    dkappa: 0.0064873541232217139
     ddkappa: 0
   }
   v: 0.0018632521763769436
@@ -1650,7 +1650,7 @@ trajectory_point {
     theta: 1.2749272247735421
     kappa: -0.031308303189837954
     s: 9.2395521278296542
-    dkappa: 0
+    dkappa: 0.0064877610658079163
     ddkappa: 0
   }
   v: 0.0046970984313232014
@@ -1664,7 +1664,7 @@ trajectory_point {
     theta: 1.2749009703573062
     kappa: -0.031303796754744609
     s: 9.2402257996227153
-    dkappa: 0
+    dkappa: 0.0064886273510150723
     ddkappa: 0
   }
   v: 0.0090530026806775778
@@ -1678,7 +1678,7 @@ trajectory_point {
     theta: 1.2748543177958918
     kappa: -0.031295789064345853
     s: 9.2414228778092333
-    dkappa: 0
+    dkappa: 0.0064901666927768484
     ddkappa: 0
   }
   v: 0.015214046062180939
@@ -1692,7 +1692,7 @@ trajectory_point {
     theta: 1.2747796487675673
     kappa: -0.03128297243009999
     s: 9.2433388501453635
-    dkappa: 0
+    dkappa: 0.0064926304718888805
     ddkappa: 0
   }
   v: 0.023482984578865471
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_avoid_change_left_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_avoid_change_left_0.pb.txt
index 1c973e4ca062a66cd116470544e23b645df8a67d..07ee767f93dcc3bfd839d5a4df63bed5872a4455 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_avoid_change_left_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_avoid_change_left_0.pb.txt
@@ -27,7 +27,7 @@ trajectory_point {
     theta: 2.8988193613366189
     kappa: 0.00010982317788408795
     s: 0.22499595167423742
-    dkappa: 0
+    dkappa: 5.4927266385156932e-05
     ddkappa: 0
   }
   v: 11.249689282117576
@@ -41,7 +41,7 @@ trajectory_point {
     theta: 2.8989313995839447
     kappa: 0.00021964605718808165
     s: 0.44999129164415375
-    dkappa: 0
+    dkappa: 0.00010985438343764391
     ddkappa: 0
   }
   v: 11.249936622713989
@@ -55,7 +55,7 @@ trajectory_point {
     theta: 2.8990434430256267
     kappa: 0.00032947402140165373
     s: 0.6749970491242151
-    dkappa: 0
+    dkappa: 0.00016478404367079947
     ddkappa: 0
   }
   v: 11.250728849084359
@@ -69,7 +69,7 @@ trajectory_point {
     theta: 2.8991554970204314
     kappa: 0.00043931232608734216
     s: 0.90002399124316068
-    dkappa: 0
+    dkappa: 0.00021971887561613297
     ddkappa: 0
   }
   v: 11.252052925360852
@@ -83,7 +83,7 @@ trajectory_point {
     theta: 2.89926756679798
     kappa: 0.00054916610021638577
     s: 1.1250826257807434
-    dkappa: 0
+    dkappa: 0.00027466144449143313
     ddkappa: 0
   }
   v: 11.253895952512684
@@ -97,7 +97,7 @@ trajectory_point {
     theta: 2.8993796574601105
     kappa: 0.00065904034750455937
     s: 1.3501832039044706
-    dkappa: 0
+    dkappa: 0.00032961425286887606
     ddkappa: 0
   }
   v: 11.256245168346112
@@ -111,7 +111,7 @@ trajectory_point {
     theta: 2.8994917739822359
     kappa: 0.00076893994774801
     s: 1.5753357229063456
-    dkappa: 0
+    dkappa: 0.00038457974134313452
     ddkappa: 0
   }
   v: 11.259087947504446
@@ -125,7 +125,7 @@ trajectory_point {
     theta: 2.8996039212147093
     kappa: 0.0008788696581590919
     s: 1.8005499289396085
-    dkappa: 0
+    dkappa: 0.00043956028919948551
     ddkappa: 0
   }
   v: 11.262411801468035
@@ -139,7 +139,7 @@ trajectory_point {
     theta: 2.8997383688501839
     kappa: 0.00098493251252862847
     s: 2.0258353197554761
-    dkappa: 0
+    dkappa: 0.00048616047732999071
     ddkappa: 0
   }
   v: 11.266204378554281
@@ -153,7 +153,7 @@ trajectory_point {
     theta: 2.9000406140900679
     kappa: 0.0010616376491556882
     s: 2.2512011474398852
-    dkappa: 0
+    dkappa: 0.00046950677575945674
     ddkappa: 0
   }
   v: 11.270453463917628
@@ -167,7 +167,7 @@ trajectory_point {
     theta: 2.9003429794722226
     kappa: 0.0011383732294827122
     s: 2.4766564211502304
-    dkappa: 0
+    dkappa: 0.00045284646445825546
     ddkappa: 0
   }
   v: 11.275146979549566
@@ -181,7 +181,7 @@ trajectory_point {
     theta: 2.900645476701897
     kappa: 0.0012151422381001811
     s: 2.7022099098521069
-    dkappa: 0
+    dkappa: 0.00043617889543224513
     ddkappa: 0
   }
   v: 11.280272984278637
@@ -195,7 +195,7 @@ trajectory_point {
     theta: 2.9009481171657567
     kappa: 0.0012919475787787694
     s: 2.927870145056052
-    dkappa: 0
+    dkappa: 0.00041950343823433478
     ddkappa: 0
   }
   v: 11.285819673770423
@@ -209,7 +209,7 @@ trajectory_point {
     theta: 2.9012509119355321
     kappa: 0.0013687920754008164
     s: 3.1536454235542846
-    dkappa: 0
+    dkappa: 0.00040281947976224979
     ddkappa: 0
   }
   v: 11.291775380527554
@@ -223,7 +223,7 @@ trajectory_point {
     theta: 2.9015538717716689
     kappa: 0.0014456784728917997
     s: 3.3795438101574464
-    dkappa: 0
+    dkappa: 0.00038612642405629624
     ddkappa: 0
   }
   v: 11.298128573889707
@@ -237,7 +237,7 @@ trajectory_point {
     theta: 2.9018570071269778
     kappa: 0.001522609438151807
     s: 3.6055731404313436
-    dkappa: 0
+    dkappa: 0.00036942369209712648
     ddkappa: 0
   }
   v: 11.304867860033609
@@ -251,7 +251,7 @@ trajectory_point {
     theta: 2.902160328150285
     kappa: 0.0015995875609870096
     s: 3.8317410234336862
-    dkappa: 0
+    dkappa: 0.00035271072160350352
     ddkappa: 0
   }
   v: 11.311981981973025
@@ -265,7 +265,7 @@ trajectory_point {
     theta: 2.9024955686268692
     kappa: 0.0016688200527467695
     s: 4.0580548444508322
-    dkappa: 0
+    dkappa: 0.00033646042069262046
     ddkappa: 0
   }
   v: 11.319459819558777
@@ -279,7 +279,7 @@ trajectory_point {
     theta: 2.9029207408479158
     kappa: 0.001716056878020987
     s: 4.2845217677345246
-    dkappa: 0
+    dkappa: 0.00032153789524534356
     ddkappa: 0
   }
   v: 11.327290389478721
@@ -293,7 +293,7 @@ trajectory_point {
     theta: 2.9033462140310968
     kappa: 0.001763327086403491
     s: 4.5111487392386342
-    dkappa: 0
+    dkappa: 0.00030660482378274876
     ddkappa: 0
   }
   v: 11.335462845257771
@@ -307,7 +307,7 @@ trajectory_point {
     theta: 2.9037720006589955
     kappa: 0.0018106320817301322
     s: 4.737942489355901
-    dkappa: 0
+    dkappa: 0.00029166076282083624
     ddkappa: 0
   }
   v: 11.343966477257879
@@ -321,7 +321,7 @@ trajectory_point {
     theta: 2.9041981128139942
     kappa: 0.0018579732234455166
     s: 4.9649095356546749
-    dkappa: 0
+    dkappa: 0.00027670528289918794
     ddkappa: 0
   }
   v: 11.352790712678051
@@ -335,7 +335,7 @@ trajectory_point {
     theta: 2.9046245621833644
     kappa: 0.0019053518271738403
     s: 5.1920561856156562
-    dkappa: 0
+    dkappa: 0.00026173796840063611
     ddkappa: 0
   }
   v: 11.36192511555433
@@ -349,7 +349,7 @@ trajectory_point {
     theta: 2.9050513600643608
     kappa: 0.001952769165289721
     s: 5.4193885393686347
-    dkappa: 0
+    dkappa: 0.00024675841737093195
     ddkappa: 0
   }
   v: 11.371359386759812
@@ -363,7 +363,7 @@ trajectory_point {
     theta: 2.9054785173693163
     kappa: 0.0020002264674890339
     s: 5.6469124924292355
-    dkappa: 0
+    dkappa: 0.00023176624133841405
     ddkappa: 0
   }
   v: 11.38108336400464
@@ -377,7 +377,7 @@ trajectory_point {
     theta: 2.9059060446307394
     kappa: 0.0020477249213597453
     s: 5.8746337384356559
-    dkappa: 0
+    dkappa: 0.00021676106513367728
     ddkappa: 0
   }
   v: 11.391087021835997
@@ -391,7 +391,7 @@ trajectory_point {
     theta: 2.9063651870254485
     kappa: 0.0020832547131340289
     s: 6.1025577718854063
-    dkappa: 0
+    dkappa: 0.00020257794039806143
     ddkappa: 0
   }
   v: 11.401360471638119
@@ -405,7 +405,7 @@ trajectory_point {
     theta: 2.90686214031777
     kappa: 0.0021044387012177784
     s: 6.330689890872053
-    dkappa: 0
+    dkappa: 0.00018938193526633396
     ddkappa: 0
   }
   v: 11.411893961632286
@@ -419,7 +419,7 @@ trajectory_point {
     theta: 2.9073595587341106
     kappa: 0.0021256424857846417
     s: 6.55903519982196
-    dkappa: 0
+    dkappa: 0.00017617359843900084
     ddkappa: 0
   }
   v: 11.422677876876824
@@ -433,7 +433,7 @@ trajectory_point {
     theta: 2.9078574527333756
     kappa: 0.0021468665230940844
     s: 6.7875986122310232
-    dkappa: 0
+    dkappa: 0.00016295264570129496
     ddkappa: 0
   }
   v: 11.433702739267105
@@ -447,7 +447,7 @@ trajectory_point {
     theta: 2.9083558323631453
     kappa: 0.0021681112519301993
     s: 7.0163848534014228
-    dkappa: 0
+    dkappa: 0.00014971880372426956
     ddkappa: 0
   }
   v: 11.444959207535547
@@ -461,7 +461,7 @@ trajectory_point {
     theta: 2.9088547072655815
     kappa: 0.0021893770938558378
     s: 7.245398463178355
-    dkappa: 0
+    dkappa: 0.00013647180990649537
     ddkappa: 0
   }
   v: 11.456438077251615
@@ -475,7 +475,7 @@ trajectory_point {
     theta: 2.9093540866833338
     kappa: 0.0022106644534667383
     s: 7.474643798686774
-    dkappa: 0
+    dkappa: 0.00012321141221575743
     ddkappa: 0
   }
   v: 11.468130280821823
@@ -489,7 +489,7 @@ trajectory_point {
     theta: 2.9098539794654559
     kappa: 0.0022319737186456564
     s: 7.7041250370681382
-    dkappa: 0
+    dkappa: 0.00010993736903075149
     ddkappa: 0
   }
   v: 11.480026887489727
@@ -503,7 +503,7 @@ trajectory_point {
     theta: 2.9103543940733174
     kappa: 0.0022533052608164927
     s: 7.9338461782171441
-    dkappa: 0
+    dkappa: 9.6649448982781322e-05
     ddkappa: 0
   }
   v: 11.492119103335932
@@ -517,7 +517,7 @@ trajectory_point {
     theta: 2.9108700225576873
     kappa: 0.00225828746799287
     s: 8.1638110475184735
-    dkappa: 0
+    dkappa: 8.46756596782817e-05
     ddkappa: 0
   }
   v: 11.504398271278088
@@ -531,7 +531,7 @@ trajectory_point {
     theta: 2.9113920656723136
     kappa: 0.0022567422365075539
     s: 8.3940232985835319
-    dkappa: 0
+    dkappa: 7.3218984142523445e-05
     ddkappa: 0
   }
   v: 11.516855871070893
@@ -545,7 +545,7 @@ trajectory_point {
     theta: 2.9119146784163443
     kappa: 0.0022551953211563136
     s: 8.6244864159871835
-    dkappa: 0
+    dkappa: 6.1749824065952068e-05
     ddkappa: 0
   }
   v: 11.529483519306089
@@ -559,7 +559,7 @@ trajectory_point {
     theta: 2.9124378680310281
     kappa: 0.002253646699666211
     s: 8.8552037180045069
-    dkappa: 0
+    dkappa: 5.0268014312260775e-05
     ddkappa: 0
   }
   v: 11.542272969412465
@@ -573,7 +573,7 @@ trajectory_point {
     theta: 2.9129616413848414
     kappa: 0.0022520963508621764
     s: 9.0861783593475209
-    dkappa: 0
+    dkappa: 3.87733978849742e-05
     ddkappa: 0
   }
   v: 11.555216111655859
@@ -587,7 +587,7 @@ trajectory_point {
     theta: 2.9134860049796525
     kappa: 0.0022505442546486409
     s: 9.3174133339019338
-    dkappa: 0
+    dkappa: 2.7265825791252054e-05
     ddkappa: 0
   }
   v: 11.568304973139155
@@ -601,7 +601,7 @@ trajectory_point {
     theta: 2.9140109649568831
     kappa: 0.0022489903919911619
     s: 9.5489114774638786
-    dkappa: 0
+    dkappa: 1.5745156905693631e-05
     ddkappa: 0
   }
   v: 11.581531717802276
@@ -615,7 +615,7 @@ trajectory_point {
     theta: 2.9145365271036789
     kappa: 0.00224743474489806
     s: 9.78067547047666
-    dkappa: 0
+    dkappa: 4.2112578341415841e-06
     ddkappa: 0
   }
   v: 11.594888646422202
@@ -629,7 +629,7 @@ trajectory_point {
     theta: 2.9150612784522596
     kappa: 0.0022448311433941786
     s: 10.012707840767497
-    dkappa: 0
+    dkappa: -7.2349419816420972e-06
     ddkappa: 0
   }
   v: 11.608368196612954
@@ -643,7 +643,7 @@ trajectory_point {
     theta: 2.9155617749093889
     kappa: 0.0022238835893047576
     s: 10.245010966284251
-    dkappa: 0
+    dkappa: -1.6922820908357365e-05
     ddkappa: 0
   }
   v: 11.621962942825599
@@ -657,7 +657,7 @@ trajectory_point {
     theta: 2.9160628603313423
     kappa: 0.0022029114191468233
     s: 10.47758707783218
-    dkappa: 0
+    dkappa: -2.6622084338026756e-05
     ddkappa: 0
   }
   v: 11.635665596348248
@@ -671,7 +671,7 @@ trajectory_point {
     theta: 2.9165645389640811
     kappa: 0.002181914444782725
     s: 10.710438261810676
-    dkappa: 0
+    dkappa: -3.6332819281036727e-05
     ddkappa: 0
   }
   v: 11.649469005306068
@@ -685,7 +685,7 @@ trajectory_point {
     theta: 2.9170668147465739
     kappa: 0.0021608924908495951
     s: 10.943566462949999
-    dkappa: 0
+    dkappa: -4.6055106839659317e-05
     ddkappa: 0
   }
   v: 11.663366154661261
@@ -699,7 +699,7 @@ trajectory_point {
     theta: 2.9175696913166642
     kappa: 0.0021398453945125654
     s: 11.176973487048027
-    dkappa: 0
+    dkappa: -5.5789022322184869e-05
     ddkappa: 0
   }
   v: 11.677350166213081
@@ -713,7 +713,7 @@ trajectory_point {
     theta: 2.9180731720169435
     kappa: 0.0021187730052179879
     s: 11.410661003706998
-    dkappa: 0
+    dkappa: -6.5534635357053793e-05
     ddkappa: 0
   }
   v: 11.691414298597829
@@ -727,7 +727,7 @@ trajectory_point {
     theta: 2.9204666453143533
     kappa: 0.0019738312502937589
     s: 12.583355222628397
-    dkappa: 0
+    dkappa: -0.00010975375189191221
     ddkappa: 0
   }
   v: 11.762710403879208
@@ -741,7 +741,7 @@ trajectory_point {
     theta: 2.92273883103252
     kappa: 0.0017879603268249286
     s: 13.763238672709441
-    dkappa: 0
+    dkappa: -0.00014945650139897132
     ddkappa: 0
   }
   v: 11.835077006730362
@@ -755,7 +755,7 @@ trajectory_point {
     theta: 2.9246463916932788
     kappa: 0.0015522794844338693
     s: 14.950383254574891
-    dkappa: 0
+    dkappa: -0.00018184221633944092
     ddkappa: 0
   }
   v: 11.907825378365892
@@ -769,7 +769,7 @@ trajectory_point {
     theta: 2.9264108015602184
     kappa: 0.0012978085421440925
     s: 16.144796272128659
-    dkappa: 0
+    dkappa: -0.00021138203319683954
     ddkappa: 0
   }
   v: 11.98035231315437
@@ -783,7 +783,7 @@ trajectory_point {
     theta: 2.9277425610485688
     kappa: 0.0010042447001314737
     s: 17.346428984869224
-    dkappa: 0
+    dkappa: -0.00023261199442616384
     ddkappa: 0
   }
   v: 12.05214012861834
@@ -797,7 +797,7 @@ trajectory_point {
     theta: 2.928776025726072
     kappa: 0.00069814141994061776
     s: 18.555185160205006
-    dkappa: 0
+    dkappa: -0.00024969634251339367
     ddkappa: 0
   }
   v: 12.122756665434322
@@ -811,7 +811,7 @@ trajectory_point {
     theta: 2.929442863272457
     kappa: 0.00037712694008859766
     s: 19.770929625769778
-    dkappa: 0
+    dkappa: -0.00026168150676262269
     ddkappa: 0
   }
   v: 12.191855287432801
@@ -825,7 +825,7 @@ trajectory_point {
     theta: 2.9296432832500283
     kappa: 5.0484635173674982e-05
     s: 20.99349682173807
-    dkappa: 0
+    dkappa: -0.00026595746047891632
     ddkappa: 0
   }
   v: 12.259174881598243
@@ -839,7 +839,7 @@ trajectory_point {
     theta: 2.9296054981247295
     kappa: -0.00027643667759934097
     s: 22.222699353140552
-    dkappa: 0
+    dkappa: -0.0002667271718858754
     ddkappa: 0
   }
   v: 12.324539858069082
@@ -853,7 +853,7 @@ trajectory_point {
     theta: 2.9289572990827666
     kappa: -0.000593076112338642
     s: 23.458336542179424
-    dkappa: 0
+    dkappa: -0.000259519515594513
     ddkappa: 0
   }
   v: 12.387860150137723
@@ -867,7 +867,7 @@ trajectory_point {
     theta: 2.9280241016094943
     kappa: -0.00089257877398100731
     s: 24.700201910168786
-    dkappa: 0
+    dkappa: -0.00024690672235067463
     ddkappa: 0
   }
   v: 12.4490878687699
@@ -881,7 +881,7 @@ trajectory_point {
     theta: 2.9268542808189282
     kappa: -0.0011780982387328259
     s: 25.948079106958911
-    dkappa: 0
+    dkappa: -0.00022980975711418348
     ddkappa: 0
   }
   v: 12.508066047025306
@@ -895,7 +895,7 @@ trajectory_point {
     theta: 2.9250840342185049
     kappa: -0.0014140251661337504
     s: 27.201734339202858
-    dkappa: 0
+    dkappa: -0.00020338680697540041
     ddkappa: 0
   }
   v: 12.564621543009061
@@ -909,7 +909,7 @@ trajectory_point {
     theta: 2.92317273571846
     kappa: -0.0016220297775554615
     s: 28.460917592651295
-    dkappa: 0
+    dkappa: -0.00017279910821212981
     ddkappa: 0
   }
   v: 12.618603080568002
@@ -923,7 +923,7 @@ trajectory_point {
     theta: 2.9210714716178066
     kappa: -0.0017820731264927636
     s: 29.725364818726653
-    dkappa: 0
+    dkappa: -0.00013551025470020449
     ddkappa: 0
   }
   v: 12.669881249290688
@@ -937,7 +937,7 @@ trajectory_point {
     theta: 2.91868410419317
     kappa: -0.0018703741343744527
     s: 30.994800121097303
-    dkappa: 0
+    dkappa: -9.04406008098491e-05
     ddkappa: 0
   }
   v: 12.718348504507395
@@ -951,7 +951,7 @@ trajectory_point {
     theta: 2.9162240715907504
     kappa: -0.0019146574019316843
     s: 32.268937942251732
-    dkappa: 0
+    dkappa: -4.0917224053784458e-05
     ddkappa: 0
   }
   v: 12.763919167290116
@@ -965,7 +965,7 @@ trajectory_point {
     theta: 2.9138591241570904
     kappa: -0.0018646104519697241
     s: 33.547485250072718
-    dkappa: 0
+    dkappa: 1.7031744467837197e-05
     ddkappa: 0
   }
   v: 12.806529424452565
@@ -979,7 +979,7 @@ trajectory_point {
     theta: 2.9115315954610566
     kappa: -0.0017296347295743322
     s: 34.830143724411485
-    dkappa: 0
+    dkappa: 8.1455712018078718e-05
     ddkappa: 0
   }
   v: 12.846137328550176
@@ -993,7 +993,7 @@ trajectory_point {
     theta: 2.909267405623845
     kappa: -0.0015325478741494666
     s: 36.116611943661894
-    dkappa: 0
+    dkappa: 0.00015054935231740646
     ddkappa: 0
   }
   v: 12.882722797880099
@@ -1007,7 +1007,7 @@ trajectory_point {
     theta: 2.9076590401787845
     kappa: -0.0011867092429123124
     s: 37.406587571334612
-    dkappa: 0
+    dkappa: 0.00022927423509011238
     ddkappa: 0
   }
   v: 12.916287616481203
@@ -1021,7 +1021,7 @@ trajectory_point {
     theta: 2.9063929473386949
     kappa: -0.00074931570568710138
     s: 38.699769542631259
-    dkappa: 0
+    dkappa: 0.00031336062275809406
     ddkappa: 0
   }
   v: 12.946855434134079
@@ -1035,7 +1035,7 @@ trajectory_point {
     theta: 2.905460020061716
     kappa: -0.00022292934523284412
     s: 39.995860251018627
-    dkappa: 0
+    dkappa: 0.00040264896621956104
     ddkappa: 0
   }
   v: 12.974471766361031
@@ -1049,7 +1049,7 @@ trajectory_point {
     theta: 2.9059216691169296
     kappa: 0.00049591664624222716
     s: 41.294567734802804
-    dkappa: 0
+    dkappa: 0.00050161137897554225
     ddkappa: 0
   }
   v: 12.99920399442609
@@ -1063,7 +1063,7 @@ trajectory_point {
     theta: 2.9068623013587658
     kappa: 0.0010117720427016449
     s: 42.595607863703378
-    dkappa: 0
+    dkappa: 0.00045371232546492812
     ddkappa: 0
   }
   v: 13.021141365334996
@@ -1077,7 +1077,7 @@ trajectory_point {
     theta: 2.9083908651484083
     kappa: 0.0012333771718098
     s: 43.898706525427585
-    dkappa: 0
+    dkappa: 0.00020059969471654783
     ddkappa: 0
   }
   v: 13.040394991835218
@@ -1091,7 +1091,7 @@ trajectory_point {
     theta: 2.9100669131123356
     kappa: 0.0014127033348968741
     s: 45.2036018122445
-    dkappa: 0
+    dkappa: 0.00014881456584873732
     ddkappa: 0
   }
   v: 13.057097852415934
@@ -1105,7 +1105,7 @@ trajectory_point {
     theta: 2.9119519173176625
     kappa: 0.0015711819373331929
     s: 46.510046207559213
-    dkappa: 0
+    dkappa: 0.0001253661643933832
     ddkappa: 0
   }
   v: 13.071404791308048
@@ -1119,7 +1119,7 @@ trajectory_point {
     theta: 2.9141954776157979
     kappa: 0.0017007974245707522
     s: 47.817808772486977
-    dkappa: 0
+    dkappa: 0.0001032984040631351
     ddkappa: 0
   }
   v: 13.08349251848418
@@ -1133,7 +1133,7 @@ trajectory_point {
     theta: 2.9164106328095052
     kappa: 0.0017982805583693566
     s: 49.126677332427391
-    dkappa: 0
+    dkappa: 8.3030432938167978e-05
     ddkappa: 0
   }
   v: 13.093559609658666
@@ -1147,7 +1147,7 @@ trajectory_point {
     theta: 2.9187348068482106
     kappa: 0.0018783761562121254
     s: 50.436460551173809
-    dkappa: 0
+    dkappa: 6.3778481955663866e-05
     ddkappa: 0
   }
   v: 13.101821731278211
@@ -1161,7 +1161,7 @@ trajectory_point {
     theta: 2.9213561895428848
     kappa: 0.0019329207742696506
     s: 51.746988438105483
-    dkappa: 0
+    dkappa: 4.6077217123834448e-05
     ddkappa: 0
   }
   v: 13.108486258755644
@@ -1175,7 +1175,7 @@ trajectory_point {
     theta: 2.9238249914877921
     kappa: 0.001964232241603657
     s: 53.058111031017894
-    dkappa: 0
+    dkappa: 3.0022135167553255e-05
     ddkappa: 0
   }
   v: 13.113747252206242
@@ -1189,7 +1189,7 @@ trajectory_point {
     theta: 2.926311206323319
     kappa: 0.0019818700798943313
     s: 54.369697208494429
-    dkappa: 0
+    dkappa: 1.4995350992472987e-05
     ddkappa: 0
   }
   v: 13.117787467272549
@@ -1203,7 +1203,7 @@ trajectory_point {
     theta: 2.9290414042542849
     kappa: 0.001978627255425401
     s: 55.6816335594591
-    dkappa: 0
+    dkappa: 1.6661855392139732e-06
     ddkappa: 0
   }
   v: 13.120778355124372
@@ -1217,7 +1217,7 @@ trajectory_point {
     theta: 2.9315385991393907
     kappa: 0.0019599480311251125
     s: 56.993823252729243
-    dkappa: 0
+    dkappa: -1.0167614875657659e-05
     ddkappa: 0
   }
   v: 13.122880062458785
@@ -1231,7 +1231,7 @@ trajectory_point {
     theta: 2.9339708184691067
     kappa: 0.0019314166292573832
     s: 58.306184906568269
-    dkappa: 0
+    dkappa: -2.0975272597465503e-05
     ddkappa: 0
   }
   v: 13.124241431500128
@@ -1245,7 +1245,7 @@ trajectory_point {
     theta: 2.9366104576696186
     kappa: 0.0018877827950469104
     s: 59.6186514582384
-    dkappa: 0
+    dkappa: -2.9944526150090832e-05
     ddkappa: 0
   }
   v: 13.125
@@ -1259,7 +1259,7 @@ trajectory_point {
     theta: 2.9389745325293859
     kappa: 0.0018354921168058261
     s: 60.931169033553374
-    dkappa: 0
+    dkappa: -3.7560030724179216e-05
     ddkappa: 0
   }
   v: 13.125282001237276
@@ -1273,7 +1273,7 @@ trajectory_point {
     theta: 2.941200490829869
     kappa: 0.001777198566428677
     s: 62.243695816431163
-    dkappa: 0
+    dkappa: -4.4159473668252965e-05
     ddkappa: 0
   }
   v: 13.125202364018088
@@ -1287,7 +1287,7 @@ trajectory_point {
     theta: 2.9436204592462012
     kappa: 0.0017105748788125924
     s: 63.556200918446734
-    dkappa: 0
+    dkappa: -4.8788342335996357e-05
     ddkappa: 0
   }
   v: 13.124864712675839
@@ -1301,7 +1301,7 @@ trajectory_point {
     theta: 2.9459660427635335
     kappa: 0.0016410874509655783
     s: 64.868663248384763
-    dkappa: 0
+    dkappa: -5.2188116708419075e-05
     ddkappa: 0
   }
   v: 13.124361367071192
@@ -1315,7 +1315,7 @@ trajectory_point {
     theta: 2.9482335988119597
     kappa: 0.0015694431334576982
     s: 66.181070381792338
-    dkappa: 0
+    dkappa: -5.4581590375155438e-05
     ddkappa: 0
   }
   v: 13.123773342592079
@@ -1329,7 +1329,7 @@ trajectory_point {
     theta: 2.9500073548156407
     kappa: 0.0014971093824547087
     s: 67.493417430531721
-    dkappa: 0
+    dkappa: -5.494272048312395e-05
     ddkappa: 0
   }
   v: 13.123170350153698
@@ -1343,7 +1343,7 @@ trajectory_point {
     theta: 2.9519023300502836
     kappa: 0.0014266804996082988
     s: 68.805705912333039
-    dkappa: 0
+    dkappa: -5.4170838365924024e-05
     ddkappa: 0
   }
   v: 13.12261079619851
@@ -1357,7 +1357,7 @@ trajectory_point {
     theta: 2.9539114498798038
     kappa: 0.0013580450246151732
     s: 70.117942620347037
-    dkappa: 0
+    dkappa: -5.2332130798707626e-05
     ddkappa: 0
   }
   v: 13.122141782696243
@@ -1371,7 +1371,7 @@ trajectory_point {
     theta: 2.9554040736445906
     kappa: 0.0012972056099540515
     s: 71.4301384926978
-    dkappa: 0
+    dkappa: -4.8420194497605745e-05
     ddkappa: 0
   }
   v: 13.12179910714389
@@ -1385,7 +1385,7 @@ trajectory_point {
     theta: 2.9570204600929886
     kappa: 0.0012420964968979642
     s: 72.742307482035471
-    dkappa: 0
+    dkappa: -4.3480379542726184e-05
     ddkappa: 0
   }
   v: 13.121607262565707
@@ -1399,7 +1399,7 @@ trajectory_point {
     theta: 2.9587943160568995
     kappa: 0.0011936914114120912
     s: 74.054465425088978
-    dkappa: 0
+    dkappa: -3.735023884905755e-05
     ddkappa: 0
   }
   v: 13.121579437513219
@@ -1413,7 +1413,7 @@ trajectory_point {
     theta: 2.9601090906248464
     kappa: 0.0011605768237392378
     s: 75.366628912218772
-    dkappa: 0
+    dkappa: -2.9270758733198329e-05
     ddkappa: 0
   }
   v: 13.121717516065216
@@ -1427,7 +1427,7 @@ trajectory_point {
     theta: 2.9615355656437967
     kappa: 0.0011369715706085392
     s: 76.678814156969551
-    dkappa: 0
+    dkappa: -2.0173673217892205e-05
     ddkappa: 0
   }
   v: 13.122012077827753
@@ -1441,7 +1441,7 @@ trajectory_point {
     theta: 2.9631898867417741
     kappa: 0.0011260085236841889
     s: 77.991035865622976
-    dkappa: 0
+    dkappa: -9.7625876854131789e-06
     ddkappa: 0
   }
   v: 13.122442397934147
@@ -1455,7 +1455,7 @@ trajectory_point {
     theta: 2.9648529563608483
     kappa: 0.0011368338617108852
     s: 79.303306106750384
-    dkappa: 0
+    dkappa: 2.4929293124467827e-06
     ddkappa: 0
   }
   v: 13.122976447044987
@@ -1469,7 +1469,7 @@ trajectory_point {
     theta: 2.9663878866533522
     kappa: 0.0011609262893380475
     s: 80.615633180765556
-    dkappa: 0
+    dkappa: 1.5757356740337285e-05
     ddkappa: 0
   }
   v: 13.123570891348121
@@ -1483,7 +1483,7 @@ trajectory_point {
     theta: 2.9676646585180961
     kappa: 0.0012046685342496756
     s: 81.928020489477447
-    dkappa: 0
+    dkappa: 3.0457299308180965e-05
     ddkappa: 0
   }
   v: 13.124171092558667
@@ -1497,7 +1497,7 @@ trajectory_point {
     theta: 2.9694002638093759
     kappa: 0.0012755561712114159
     s: 83.240465405642809
-    dkappa: 0
+    dkappa: 4.6841515644383482e-05
     ddkappa: 0
   }
   v: 13.124711107919005
@@ -1511,7 +1511,7 @@ trajectory_point {
     theta: 2.9711737784372358
     kappa: 0.0013634866986507387
     s: 84.55295814251906
-    dkappa: 0
+    dkappa: 6.4239412821396065e-05
     ddkappa: 0
   }
   v: 13.125113690198782
@@ -1525,7 +1525,7 @@ trajectory_point {
     theta: 2.9727522208226218
     kappa: 0.0014791145733198654
     s: 85.865480623416943
-    dkappa: 0
+    dkappa: 8.3239246346691832e-05
     ddkappa: 0
   }
   v: 13.125290287694913
@@ -1539,7 +1539,7 @@ trajectory_point {
     theta: 2.9748810168130038
     kappa: 0.0016261811040530435
     s: 87.178005351253262
-    dkappa: 0
+    dkappa: 0.00010374378710311505
     ddkappa: 0
   }
   v: 13.125141044231572
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 d203b022a0c882a6f0ddd5028e8851d5d7de1cc1..69bfb8ef492c02340895ade1d2ac1a39c493d6d4 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
@@ -12,7 +12,7 @@ trajectory_point {
     theta: 1.2656533435800004
     kappa: -1.5424258921372912e-20
     s: 1.0842021724855044e-19
-    dkappa: 0
+    dkappa: 1.8850388651231266e-22
     ddkappa: 0
   }
   v: 0
@@ -26,7 +26,7 @@ trajectory_point {
     theta: 1.265653399018605
     kappa: 5.018290812244888e-08
     s: 1.4410307354495851e-05
-    dkappa: 0
+    dkappa: 2.5054358044055182e-08
     ddkappa: 0
   }
   v: 0.0016170097301915552
@@ -40,7 +40,7 @@ trajectory_point {
     theta: 1.265653619166915
     kappa: 2.4946069483179465e-07
     s: 7.1634056691569976e-05
-    dkappa: 0
+    dkappa: 1.2454594203633007e-07
     ddkappa: 0
   }
   v: 0.0042770798454151949
@@ -54,7 +54,7 @@ trajectory_point {
     theta: 1.2656540833007253
     kappa: 6.6959364158837812e-07
     s: 0.00019227762078589933
-    dkappa: 0
+    dkappa: 3.3430184634653319e-07
     ddkappa: 0
   }
   v: 0.0079547781653041272
@@ -68,7 +68,7 @@ trajectory_point {
     theta: 1.2656548687508495
     kappa: 1.3805814348162805e-06
     s: 0.00039644180753860633
-    dkappa: 0
+    dkappa: 6.89270169286556e-07
     ddkappa: 0
   }
   v: 0.012624980382869685
@@ -82,7 +82,7 @@ trajectory_point {
     theta: 1.26565605092681
     kappa: 2.4506846088622492e-06
     s: 0.00070372801744482112
-    dkappa: 0
+    dkappa: 1.2235307187389622e-06
     ddkappa: 0
   }
   v: 0.018262870064501321
@@ -96,7 +96,7 @@ trajectory_point {
     theta: 1.2656577033405287
     kappa: 3.9464459889546396e-06
     s: 0.0011332444010612439
-    dkappa: 0
+    dkappa: 1.9703057177854764e-06
     ddkappa: 0
   }
   v: 0.024843938649966622
@@ -110,7 +110,7 @@ trajectory_point {
     theta: 1.2656598976300191
     kappa: 5.9327121341623645e-06
     s: 0.0017036120164737089
-    dkappa: 0
+    dkappa: 2.961970510335476e-06
     ddkappa: 0
   }
   v: 0.032343985452411281
@@ -124,7 +124,7 @@ trajectory_point {
     theta: 1.2656627035830759
     kappa: 8.4726547803538244e-06
     s: 0.0024329709867647433
-    dkappa: 0
+    dkappa: 4.2300642667544753e-06
     ddkappa: 0
   }
   v: 0.040739117658359129
@@ -138,7 +138,7 @@ trajectory_point {
     theta: 1.2656661891609677
     kappa: 1.1627792283155862e-05
     s: 0.0033389866574811346
-    dkappa: 0
+    dkappa: 5.8053006894926238e-06
     ddkappa: 0
   }
   v: 0.050005750327712116
@@ -152,7 +152,7 @@ trajectory_point {
     theta: 1.2656704205221292
     kappa: 1.5458011060912697e-05
     s: 0.0044388557541014881
-    dkappa: 0
+    dkappa: 7.7175787187131836e-06
     ddkappa: 0
   }
   v: 0.060120606393750317
@@ -166,7 +166,7 @@ trajectory_point {
     theta: 1.2656754620458524
     kappa: 2.0021587037644884e-05
     s: 0.0057493125395037953
-    dkappa: 0
+    dkappa: 9.9959932379210327e-06
     ddkappa: 0
   }
   v: 0.0710607166631319
@@ -180,7 +180,7 @@ trajectory_point {
     theta: 1.2656813763559784
     kappa: 2.5375207086008223e-05
     s: 0.007286634971432988
-    dkappa: 0
+    dkappa: 1.2668845779591138e-05
     ddkappa: 0
   }
   v: 0.082803419815893245
@@ -194,7 +194,7 @@ trajectory_point {
     theta: 1.2656882243445913
     kappa: 3.1573990470252749e-05
     s: 0.0090666508599685121
-    dkappa: 0
+    dkappa: 1.5763655230797064e-05
     ddkappa: 0
   }
   v: 0.095326362405448756
@@ -208,7 +208,7 @@ trajectory_point {
     theta: 1.2656960651957085
     kappa: 3.8671510289181627e-05
     s: 0.01110474402499188
-    dkappa: 0
+    dkappa: 1.930716853883945e-05
     ddkappa: 0
   }
   v: 0.10860749885859101
@@ -222,7 +222,7 @@ trajectory_point {
     theta: 1.2657049564089751
     kappa: 4.6719814919110123e-05
     s: 0.013415860453654234
-    dkappa: 0
+    dkappa: 2.3325371416874491e-05
     ddkappa: 0
   }
   v: 0.12262509147549071
@@ -236,7 +236,7 @@ trajectory_point {
     theta: 1.2657149538233543
     kappa: 5.5769449456824542e-05
     s: 0.016014514457843913
-    dkappa: 0
+    dkappa: 2.7843499049542441e-05
     ddkappa: 0
   }
   v: 0.13735771042969666
@@ -250,7 +250,7 @@ trajectory_point {
     theta: 1.2657261116408214
     kappa: 6.5869477162541158e-05
     s: 0.018914794831654019
-    dkappa: 0
+    dkappa: 3.2886046798596109e-05
     ddkappa: 0
   }
   v: 0.15278423376813582
@@ -264,7 +264,7 @@ trajectory_point {
     theta: 1.2657384824500544
     kappa: 7.7067500902865192e-05
     s: 0.022130371008849969
-    dkappa: 0
+    dkappa: 3.8476780908529338e-05
     ddkappa: 0
   }
   v: 0.1688838474111132
@@ -278,7 +278,7 @@ trajectory_point {
     theta: 1.2657521172501278
     kappa: 8.94096845937497e-05
     s: 0.025674499220337058
-    dkappa: 0
+    dkappa: 4.4638749212205468e-05
     ddkappa: 0
   }
   v: 0.18563604515231202
@@ -292,7 +292,7 @@ trajectory_point {
     theta: 1.2657670654742039
     kappa: 0.00010294077464345455
     s: 0.029560028651628035
-    dkappa: 0
+    dkappa: 5.1394291836485882e-05
     ddkappa: 0
   }
   v: 0.20302062865879361
@@ -306,7 +306,7 @@ trajectory_point {
     theta: 1.2657833750132241
     kappa: 0.00011770412139550539
     s: 0.033799407600310652
-    dkappa: 0
+    dkappa: 5.8765051907858448e-05
     ddkappa: 0
   }
   v: 0.22101770747099736
@@ -320,7 +320,7 @@ trajectory_point {
     theta: 1.2658010922396015
     kappa: 0.00013374170057165255
     s: 0.038404689633515232
-    dkappa: 0
+    dkappa: 6.6771986258066049e-05
     ddkappa: 0
   }
   v: 0.23960769900274084
@@ -334,7 +334,7 @@ trajectory_point {
     theta: 1.2658202620309116
     kappa: 0.00015109413471482995
     s: 0.043387539745382224
-    dkappa: 0
+    dkappa: 7.5435376129735009e-05
     ddkappa: 0
   }
   v: 0.25877132854121976
@@ -348,7 +348,7 @@ trajectory_point {
     theta: 1.2658409277935838
     kappa: 0.00016980071463211415
     s: 0.048759240514529789
-    dkappa: 0
+    dkappa: 8.4774837882003659e-05
     ddkappa: 0
   }
   v: 0.27848962924700787
@@ -362,7 +362,7 @@ trajectory_point {
     theta: 1.265863131486592
     kappa: 0.00018989942083768316
     s: 0.054530698261521307
-    dkappa: 0
+    dkappa: 9.4809333696150742e-05
     ddkappa: 0
   }
   v: 0.29874394215405708
@@ -376,7 +376,7 @@ trajectory_point {
     theta: 1.2658869136451454
     kappa: 0.00021142694499577555
     s: 0.060712449206333029
-    dkappa: 0
+    dkappa: 0.00010555718228122402
     ddkappa: 0
   }
   v: 0.31951591616969754
@@ -390,7 +390,7 @@ trajectory_point {
     theta: 1.2659123134043788
     kappa: 0.00023441871136364929
     s: 0.067314665625821571
-    dkappa: 0
+    dkappa: 0.00011703606957966867
     ddkappa: 0
   }
   v: 0.34078750807463737
@@ -404,7 +404,7 @@ trajectory_point {
     theta: 1.2659393685230413
     kappa: 0.00025890889823454059
     s: 0.074347162011191473
-    dkappa: 0
+    dkappa: 0.00012926305947295576
     ddkappa: 0
   }
   v: 0.36254098252296291
@@ -418,7 +418,7 @@ trajectory_point {
     theta: 1.2659681154071878
     kappa: 0.00028493045938062308
     s: 0.081819401225462787
-    dkappa: 0
+    dkappa: 0.0001422546044872108
     ddkappa: 0
   }
   v: 0.38475891204213847
@@ -432,7 +432,7 @@ trajectory_point {
     theta: 1.2659985891338656
     kappa: 0.00031251514549596654
     s: 0.08974050066093868
-    dkappa: 0
+    dkappa: 0.00015602655649884223
     ddkappa: 0
   }
   v: 0.40742417703300665
@@ -446,7 +446,7 @@ trajectory_point {
     theta: 1.2660308234748048
     kappa: 0.00034169352563949586
     s: 0.098119238396672873
-    dkappa: 0
+    dkappa: 0.0001705941774401698
     ddkappa: 0
   }
   v: 0.43051996576978824
@@ -460,7 +460,7 @@ trajectory_point {
     theta: 1.2660648509201056
     kappa: 0.00037249500867795021
     s: 0.10696405935593731
-    dkappa: 0
+    dkappa: 0.00018597215000505323
     ddkappa: 0
   }
   v: 0.4540297744000818
@@ -474,7 +474,7 @@ trajectory_point {
     theta: 1.266100702701926
     kappa: 0.00040494786472884177
     s: 0.11628308146368976
-    dkappa: 0
+    dkappa: 0.00020217458835452063
     ddkappa: 0
   }
   v: 0.47793740694486442
@@ -488,7 +488,7 @@ trajectory_point {
     theta: 1.2661384088181693
     kappa: 0.00043907924660341476
     s: 0.12608410180404123
-    dkappa: 0
+    dkappa: 0.0002192150488223969
     ddkappa: 0
   }
   v: 0.50222697529849114
@@ -502,7 +502,7 @@ trajectory_point {
     theta: 1.2661779980561698
     kappa: 0.00047491521124960441
     s: 0.13637460277772367
-    dkappa: 0
+    dkappa: 0.00023710654062093244
     ddkappa: 0
   }
   v: 0.52688289922869491
@@ -516,7 +516,7 @@ trajectory_point {
     theta: 1.2662194980163801
     kappa: 0.0005124807411949953
     s: 0.14716175825955735
-    dkappa: 0
+    dkappa: 0.00025586153654643106
     ddkappa: 0
   }
   v: 0.55188990637658741
@@ -530,7 +530,7 @@ trajectory_point {
     theta: 1.2662629351360561
     kappa: 0.00055179976598978154
     s: 0.15845243975591869
-    dkappa: 0
+    dkappa: 0.00027549198368487953
     ddkappa: 0
   }
   v: 0.57723303225665767
@@ -544,7 +544,7 @@ trajectory_point {
     theta: 1.266308334712942
     kappa: 0.000592895183649725
     s: 0.17025322256220768
-    dkappa: 0
+    dkappa: 0.00029600931411757512
     ddkappa: 0
   }
   v: 0.6028976202567734
@@ -558,7 +558,7 @@ trajectory_point {
     theta: 1.2663557209289549
     kappa: 0.00063578888209911357
     s: 0.18257039192031529
-    dkappa: 0
+    dkappa: 0.00031742445562675428
     ddkappa: 0
   }
   v: 0.62886932163818021
@@ -572,7 +572,7 @@ trajectory_point {
     theta: 1.2664051168738686
     kappa: 0.00068050176061372166
     s: 0.19540994917609139
-    dkappa: 0
+    dkappa: 0.00033974784240122153
     ddkappa: 0
   }
   v: 0.655134095535502
@@ -586,7 +586,7 @@ trajectory_point {
     theta: 1.2664565445689977
     kappa: 0.00072705375126376763
     s: 0.2087776179368119
-    dkappa: 0
+    dkappa: 0.00036298942574197746
     ddkappa: 0
   }
   v: 0.68167820895674047
@@ -600,7 +600,7 @@ trajectory_point {
     theta: 1.2665100249908792
     kappa: 0.0007754638403568735
     s: 0.22267885022864667
-    dkappa: 0
+    dkappa: 0.00038715868476784748
     ddkappa: 0
   }
   v: 0.70848823678327588
@@ -614,7 +614,7 @@ trajectory_point {
     theta: 1.2665655780949561
     kappa: 0.00082575008988102438
     s: 0.237118832654127
-    dkappa: 0
+    dkappa: 0.0004122646371211105
     ddkappa: 0
   }
   v: 0.735551061769866
@@ -628,7 +628,7 @@ trajectory_point {
     theta: 1.2666232228392582
     kappa: 0.00087792965894752616
     s: 0.25210249254961303
-    dkappa: 0
+    dkappa: 0.000438315849673127
     ddkappa: 0
   }
   v: 0.76285387454464759
@@ -642,7 +642,7 @@ trajectory_point {
     theta: 1.2666829772080836
     kappa: 0.00093201882523396477
     s: 0.2676345041427613
-    dkappa: 0
+    dkappa: 0.00046532044922996738
     ddkappa: 0
   }
   v: 0.79038417360913471
@@ -656,7 +656,7 @@ trajectory_point {
     theta: 1.2667448582356795
     kappa: 0.00098803300642716666
     s: 0.28371929470999274
-    dkappa: 0
+    dkappa: 0.00049328613323804148
     ddkappa: 0
   }
   v: 0.81812976533821991
@@ -670,7 +670,7 @@ trajectory_point {
     theta: 1.2668088820299215
     kappa: 0.0010459867816661559
     s: 0.30036105073395963
-    dkappa: 0
+    dkappa: 0.00052222018048972592
     ddkappa: 0
   }
   v: 0.846078763980174
@@ -684,7 +684,7 @@ trajectory_point {
     theta: 1.2668750637959942
     kappa: 0.001105893912985113
     s: 0.31756372406101352
-    dkappa: 0
+    dkappa: 0.00055212946182899307
     ddkappa: 0
   }
   v: 0.87421959165664564
@@ -698,7 +698,7 @@ trajectory_point {
     theta: 1.2669434178600696
     kappa: 0.0011677673667563361
     s: 0.33533103805867276
-    dkappa: 0
+    dkappa: 0.00058302045085703949
     ddkappa: 0
   }
   v: 0.90254097836266189
@@ -712,7 +712,7 @@ trajectory_point {
     theta: 1.2670139576929849
     kappa: 0.0012316193351331974
     s: 0.35366649377309
-    dkappa: 0
+    dkappa: 0.00061489923463791459
     ddkappa: 0
   }
   v: 0.93103196196662741
@@ -726,7 +726,7 @@ trajectory_point {
     theta: 1.2673998579794683
     kappa: 0.0015809321279581969
     s: 0.45397364805720386
-    dkappa: 0
+    dkappa: 0.0007892974133853237
     ddkappa: 0
   }
   v: 1.075668709986342
@@ -740,7 +740,7 @@ trajectory_point {
     theta: 1.2678419727304298
     kappa: 0.0019811289582263502
     s: 0.56889244296613184
-    dkappa: 0
+    dkappa: 0.000989099996551001
     ddkappa: 0
   }
   v: 1.223078087907967
@@ -754,7 +754,7 @@ trajectory_point {
     theta: 1.2683411656083003
     kappa: 0.0024329915065723485
     s: 0.69864734253793792
-    dkappa: 0
+    dkappa: 0.0012146972466213265
     ddkappa: 0
   }
   v: 1.3722361868325188
@@ -768,7 +768,7 @@ trajectory_point {
     theta: 1.268897943255811
     kappa: 0.0029369783889948856
     s: 0.84337004096385426
-    dkappa: 0
+    dkappa: 0.0014663181325792849
     ddkappa: 0
   }
   v: 1.52231151872234
@@ -782,7 +782,7 @@ trajectory_point {
     theta: 1.2695125292710552
     kappa: 0.0034932921661033536
     s: 1.0031187046744139
-    dkappa: 0
+    dkappa: 0.0017440637849934939
     ddkappa: 0
   }
   v: 1.6726650164011019
@@ -796,7 +796,7 @@ trajectory_point {
     theta: 1.2701849381778187
     kappa: 0.0041019463523645364
     s: 1.1778972144255828
-    dkappa: 0
+    dkappa: 0.0020479409511072336
     ddkappa: 0
   }
   v: 1.822850033553802
@@ -810,7 +810,7 @@ trajectory_point {
     theta: 1.2709150493918062
     kappa: 0.0047628324253493068
     s: 1.367674407384893
-    dkappa: 0
+    dkappa: 0.0023778954499274738
     ddkappa: 0
   }
   v: 1.9726123447267652
@@ -824,7 +824,7 @@ trajectory_point {
     theta: 1.2717026811828782
     kappa: 0.0054757868349793251
     s: 1.5724033192175759
-    dkappa: 0
+    dkappa: 0.0027338456273139091
     ddkappa: 0
   }
   v: 2.1218901453276433
@@ -838,7 +838,7 @@ trajectory_point {
     theta: 1.272547664632659
     kappa: 0.0062406580127737348
     s: 1.792040426172695
-    dkappa: 0
+    dkappa: 0.0031157158110679817
     ddkappa: 0
   }
   v: 2.2708140516254165
@@ -852,7 +852,7 @@ trajectory_point {
     theta: 1.2735660680316419
     kappa: 0.0070102679687154322
     s: 2.0265648871692759
-    dkappa: 0
+    dkappa: 0.0034589044890876181
     ddkappa: 0
   }
   v: 2.419707100750391
@@ -866,7 +866,7 @@ trajectory_point {
     theta: 1.2757530992071267
     kappa: 0.0073811045885742617
     s: 2.2759929760851083
-    dkappa: 0
+    dkappa: 0.0032102863122885411
     ddkappa: 0
   }
   v: 2.5688910134799254
@@ -880,7 +880,7 @@ trajectory_point {
     theta: 1.2780710283926788
     kappa: 0.0077741252650142777
     s: 2.5403422635588195
-    dkappa: 0
+    dkappa: 0.002946795387206616
     ddkappa: 0
   }
   v: 2.7180511697615075
@@ -894,7 +894,7 @@ trajectory_point {
     theta: 1.2805196135778234
     kappa: 0.0081892915954689845
     s: 2.819586919674328
-    dkappa: 0
+    dkappa: 0.0026684574598862094
     ddkappa: 0
   }
   v: 2.8667236478959239
@@ -908,7 +908,7 @@ trajectory_point {
     theta: 1.2830982110220042
     kappa: 0.00862649850707068
     s: 3.1136562714635838
-    dkappa: 0
+    dkappa: 0.0023753429738148269
     ddkappa: 0
   }
   v: 3.0144755085430095
@@ -922,7 +922,7 @@ trajectory_point {
     theta: 1.2858058012219162
     kappa: 0.0090855788629453527
     s: 3.4224379011424721
-    dkappa: 0
+    dkappa: 0.0020675639817474252
     ddkappa: 0
   }
   v: 3.160904794721648
@@ -936,7 +936,7 @@ trajectory_point {
     theta: 1.2886410149145791
     kappa: 0.0095663080685075563
     s: 3.74578074434672
-    dkappa: 0
+    dkappa: 0.001745271057530712
     ddkappa: 0
   }
   v: 3.3056405318097739
@@ -950,7 +950,7 @@ trajectory_point {
     theta: 1.2917065380402817
     kappa: 0.0099471294468779937
     s: 4.0834981883678
-    dkappa: 0
+    dkappa: 0.0014261350747888704
     ddkappa: 0
   }
   v: 3.44834272754437
@@ -964,7 +964,7 @@ trajectory_point {
     theta: 1.2952606301875942
     kappa: 0.0099256186558581479
     s: 4.4353711703888354
-    dkappa: 0
+    dkappa: 0.0011539280475511544
     ddkappa: 0
   }
   v: 3.5887023720214692
@@ -978,7 +978,7 @@ trajectory_point {
     theta: 1.2989553442020612
     kappa: 0.0099032576909445745
     s: 4.8011512757205033
-    dkappa: 0
+    dkappa: 0.00087096254527488219
     ddkappa: 0
   }
   v: 3.7264414376961543
@@ -992,7 +992,7 @@ trajectory_point {
     theta: 1.3027878167433782
     kappa: 0.0098800633432330909
     s: 5.1805638360369475
-    dkappa: 0
+    dkappa: 0.00057745104988471949
     ddkappa: 0
   }
   v: 3.8613128793825551
@@ -1006,7 +1006,7 @@ trajectory_point {
     theta: 1.3067549272425065
     kappa: 0.00985605381936152
     s: 5.5733110276116689
-    dkappa: 0
+    dkappa: 0.00027362395619747089
     ddkappa: 0
   }
   v: 3.993100634253854
@@ -1020,7 +1020,7 @@ trajectory_point {
     theta: 1.31085332796749
     kappa: 0.0098312485521075046
     s: 5.9790749695534462
-    dkappa: 0
+    dkappa: -4.0272824856396812e-05
     ddkappa: 0
   }
   v: 4.1216196218422807
@@ -1034,7 +1034,7 @@ trajectory_point {
     theta: 1.3145827158455388
     kappa: 0.0093787993549419857
     s: 6.397520822042229
-    dkappa: 0
+    dkappa: -0.0002744987207521945
     ddkappa: 0
   }
   v: 4.2467157440391148
@@ -1048,7 +1048,7 @@ trajectory_point {
     theta: 1.3183869310543381
     kappa: 0.00888274647934227
     s: 6.8282998845650509
-    dkappa: 0
+    dkappa: -0.00050928322835584944
     ddkappa: 0
   }
   v: 4.3682658850946856
@@ -1062,7 +1062,7 @@ trajectory_point {
     theta: 1.3222969329364811
     kappa: 0.008372905534220227
     s: 7.2710526941519271
-    dkappa: 0
+    dkappa: -0.00075059370380051518
     ddkappa: 0
   }
   v: 4.4861779116183715
@@ -1076,7 +1076,7 @@ trajectory_point {
     theta: 1.326309358844098
     kappa: 0.0078496992757278953
     s: 7.7254121236117674
-    dkappa: 0
+    dkappa: -0.00099823005431407962
     ddkappa: 0
   }
   v: 4.6003906725786
@@ -1090,7 +1090,7 @@ trajectory_point {
     theta: 1.3298631195636252
     kappa: 0.0071990707038469878
     s: 8.1910064797682729
-    dkappa: 0
+    dkappa: -0.0012087320944935407
     ddkappa: 0
   }
   v: 4.71087399930285
@@ -1104,7 +1104,7 @@ trajectory_point {
     theta: 1.3326289675435339
     kappa: 0.0063545003985749811
     s: 8.6674626016958456
-    dkappa: 0
+    dkappa: -0.0013566000430742632
     ddkappa: 0
   }
   v: 4.817628705477647
@@ -1118,7 +1118,7 @@ trajectory_point {
     theta: 1.3354557426505844
     kappa: 0.0054913350100433758
     s: 9.1544089589554964
-    dkappa: 0
+    dkappa: -0.0015077236315917922
     ddkappa: 0
   }
   v: 4.9206865871485661
@@ -1132,7 +1132,7 @@ trajectory_point {
     theta: 1.3383412709609221
     kappa: 0.0046102247341955321
     s: 9.65147874983074
-    dkappa: 0
+    dkappa: -0.0016619890232942084
     ddkappa: 0
   }
   v: 5.0201104227202347
@@ -1146,7 +1146,7 @@ trajectory_point {
     theta: 1.3407181522839358
     kappa: 0.0036885005396590908
     s: 10.158313420713649
-    dkappa: 0
+    dkappa: -0.0017842538367960213
     ddkappa: 0
   }
   v: 5.1160105773203162
@@ -1160,7 +1160,7 @@ trajectory_point {
     theta: 1.3417746310658767
     kappa: 0.0026933844740734893
     s: 10.674570010365791
-    dkappa: 0
+    dkappa: -0.0018242265114736349
     ddkappa: 0
   }
   v: 5.2085851502292364
@@ -1174,7 +1174,7 @@ trajectory_point {
     theta: 1.3428497344514847
     kappa: 0.001680727026463109
     s: 11.199926899344895
-    dkappa: 0
+    dkappa: -0.0018649038034128131
     ddkappa: 0
   }
   v: 5.2980532468792809
@@ -1188,7 +1188,7 @@ trajectory_point {
     theta: 1.3439428468445505
     kappa: 0.00065110571266526764
     s: 11.734084478020533
-    dkappa: 0
+    dkappa: -0.0019062625144626224
     ddkappa: 0
   }
   v: 5.3846359580750489
@@ -1202,7 +1202,7 @@ trajectory_point {
     theta: 1.3440772342461276
     kappa: -0.00031291709141274924
     s: 12.276765345111352
-    dkappa: 0
+    dkappa: -0.0018865726294334242
     ddkappa: 0
   }
   v: 5.4685563599934479
@@ -1216,7 +1216,7 @@ trajectory_point {
     theta: 1.3431949699798462
     kappa: -0.0012060260077330308
     s: 12.827714506222309
-    dkappa: 0
+    dkappa: -0.0018021854658053164
     ddkappa: 0
   }
   v: 5.5500395141836982
@@ -1230,7 +1230,7 @@ trajectory_point {
     theta: 1.3422998369333279
     kappa: -0.0021121614226056748
     s: 13.386699572381898
-    dkappa: 0
+    dkappa: -0.0017165674676790283
     ddkappa: 0
   }
   v: 5.62931246756733
@@ -1244,7 +1244,7 @@ trajectory_point {
     theta: 1.34139217217136
     kappa: -0.0030309835909960491
     s: 13.953510958579377
-    dkappa: 0
+    dkappa: -0.0016297507365365912
     ddkappa: 0
   }
   v: 5.7066042524381846
@@ -1258,7 +1258,7 @@ trajectory_point {
     theta: 1.3391618695265146
     kappa: -0.0035642320810724418
     s: 14.527962082302007
-    dkappa: 0
+    dkappa: -0.001422182962420379
     ddkappa: 0
   }
   v: 5.7821458864624153
@@ -1272,7 +1272,7 @@ trajectory_point {
     theta: 1.3367566022245894
     kappa: -0.0040601070641342416
     s: 15.109889562072281
-    dkappa: 0
+    dkappa: -0.0011985981068322609
     ddkappa: 0
   }
   v: 5.8561703726784859
@@ -1286,7 +1286,7 @@ trajectory_point {
     theta: 1.3343210165950585
     kappa: -0.0045622335553824759
     s: 15.699153415985151
-    dkappa: 0
+    dkappa: -0.00097219451143508548
     ddkappa: 0
   }
   v: 5.928912699497169
@@ -1300,7 +1300,7 @@ trajectory_point {
     theta: 1.3316997829004851
     kappa: -0.0047199082415667545
     s: 16.295637260245265
-    dkappa: 0
+    dkappa: -0.000676882825013102
     ddkappa: 0
   }
   v: 6.0006098407015518
@@ -1314,7 +1314,7 @@ trajectory_point {
     theta: 1.3288736619128396
     kappa: -0.0044890299438767167
     s: 16.899248507704193
-    dkappa: 0
+    dkappa: -0.00030439474677024139
     ddkappa: 0
   }
   v: 6.07150075544703
@@ -1328,7 +1328,7 @@ trajectory_point {
     theta: 1.3260144830152021
     kappa: -0.0042554516860245622
     s: 17.509918566397658
-    dkappa: 0
+    dkappa: 7.2449318943860455e-05
     ddkappa: 0
   }
   v: 6.141826388261312
@@ -1342,7 +1342,7 @@ trajectory_point {
     theta: 1.3233956091945618
     kappa: -0.0038232567886627351
     s: 18.127603038082778
-    dkappa: 0
+    dkappa: 0.00048043277883066395
     ddkappa: 0
   }
   v: 6.2118296690444135
@@ -1356,7 +1356,7 @@ trajectory_point {
     theta: 1.3219510747938252
     kappa: -0.0025224126123882993
     s: 18.752281916775271
-    dkappa: 0
+    dkappa: 0.001011229010583794
     ddkappa: 0
   }
   v: 6.2817555130686662
@@ -1370,7 +1370,7 @@ trajectory_point {
     theta: 1.3204903530902021
     kappa: -0.0012069935912486921
     s: 19.383959787286727
-    dkappa: 0
+    dkappa: 0.0015479723595043129
     ddkappa: 0
   }
   v: 6.35185082097871
@@ -1384,7 +1384,7 @@ trajectory_point {
     theta: 1.3190369536401954
     kappa: 0.00010279058027610303
     s: 20.0226660237618
-    dkappa: 0
+    dkappa: 0.0020722881766341242
     ddkappa: 0
   }
   v: 6.4223644787914953
@@ -1398,7 +1398,7 @@ trajectory_point {
     theta: 1.3191081501680519
     kappa: 0.00010211516896572674
     s: 20.668454988215451
-    dkappa: 0
+    dkappa: 0.0013997468774822085
     ddkappa: 0
   }
   v: 6.4935473578962846
@@ -1412,7 +1412,7 @@ trajectory_point {
     theta: 1.3191801363183557
     kappa: 0.00010143226684526291
     s: 21.321406229070202
-    dkappa: 0
+    dkappa: 0.00071974659878616849
     ddkappa: 0
   }
   v: 6.5656523150546509
@@ -1426,7 +1426,7 @@ trajectory_point {
     theta: 1.3192529236589878
     kappa: 0.00010074176416804975
     s: 21.981624679693326
-    dkappa: 0
+    dkappa: 3.2178060228386266e-05
     ddkappa: 0
   }
   v: 6.6389341924004794
@@ -1440,7 +1440,7 @@ trajectory_point {
     theta: 1.31931365291989
     kappa: 0.00010004000963993802
     s: 22.649240856934114
-    dkappa: 0
+    dkappa: -1.047626886059476e-06
     ddkappa: 0
   }
   v: 6.713649817439963
@@ -1454,7 +1454,7 @@ trajectory_point {
     theta: 1.3193744159726002
     kappa: 9.93301364332458e-05
     s: 23.324411059661081
-    dkappa: 0
+    dkappa: -1.0494928053823495e-06
     ddkappa: 0
   }
   v: 6.7900580030516107
@@ -1468,7 +1468,7 @@ trajectory_point {
     theta: 1.3194358752665818
     kappa: 9.8612129284100187e-05
     s: 24.007317567299229
-    dkappa: 0
+    dkappa: -1.0513801049746175e-06
     ddkappa: 0
   }
   v: 6.8684195474862362
@@ -1482,7 +1482,7 @@ trajectory_point {
     theta: 1.3195088153502057
     kappa: 9.7883759874344868e-05
     s: 24.698168838367227
-    dkappa: 0
+    dkappa: -1.0524031734483784e-06
     ddkappa: 0
   }
   v: 6.94899723436697
@@ -1496,7 +1496,7 @@ trajectory_point {
     theta: 1.3195827273222367
     kappa: 9.7146746445918485e-05
     s: 25.3971997090147
-    dkappa: 0
+    dkappa: -1.0534294413685651e-06
     ddkappa: 0
   }
   v: 7.0320558326892488
@@ -1510,7 +1510,7 @@ trajectory_point {
     theta: 1.3196557766159063
     kappa: 9.6400792091909477e-05
     s: 26.104671591559413
-    dkappa: 0
+    dkappa: -1.0543566486922897e-06
     ddkappa: 0
   }
   v: 7.1178620968208248
@@ -1524,7 +1524,7 @@ trajectory_point {
     theta: 1.3197175310248586
     kappa: 9.5645346999501042e-05
     s: 26.820872673024532
-    dkappa: 0
+    dkappa: -1.0545208463112296e-06
     ddkappa: 0
   }
   v: 7.2066847665017582
@@ -1538,7 +1538,7 @@ trajectory_point {
     theta: 1.31978006528344
     kappa: 9.48803619651154e-05
     s: 27.546118113675846
-    dkappa: 0
+    dkappa: -1.0546871174571183e-06
     ddkappa: 0
   }
   v: 7.2987945668444194
@@ -1552,7 +1552,7 @@ trajectory_point {
     theta: 1.3198473857208983
     kappa: 9.4106017306331955e-05
     s: 28.280750245558988
-    dkappa: 0
+    dkappa: -1.0545240587025506e-06
     ddkappa: 0
   }
   v: 7.3944642083334919
@@ -1566,7 +1566,7 @@ trajectory_point {
     theta: 1.3199227224925687
     kappa: 9.3322358480048718e-05
     s: 29.02513877103668
-    dkappa: 0
+    dkappa: -1.0537651666498882e-06
     ddkappa: 0
   }
   v: 7.49396838682597
@@ -1580,7 +1580,7 @@ trajectory_point {
     theta: 1.3199990868785694
     kappa: 9.2528010331219013e-05
     s: 29.779680961325965
-    dkappa: 0
+    dkappa: -1.0529959231003032e-06
     ddkappa: 0
   }
   v: 7.5975837835511584
@@ -1594,7 +1594,7 @@ trajectory_point {
     theta: 1.3200666053694174
     kappa: 9.1724913318441388e-05
     s: 30.544801855035416
-    dkappa: 0
+    dkappa: -1.0515615053044485e-06
     ddkappa: 0
   }
   v: 7.7055890651106722
@@ -1608,7 +1608,7 @@ trajectory_point {
     theta: 1.3201306048746806
     kappa: 9.0911318848065e-05
     s: 31.3209544567024
-    dkappa: 0
+    dkappa: -1.0498099475972066e-06
     ddkappa: 0
   }
   v: 7.818264883478439
@@ -1622,7 +1622,7 @@ trajectory_point {
     theta: 1.3201968663550592
     kappa: 9.008630529208338e-05
     s: 32.108619935330282
-    dkappa: 0
+    dkappa: -1.0479158518131855e-06
     ddkappa: 0
   }
   v: 7.9358938760006952
@@ -1636,7 +1636,7 @@ trajectory_point {
     theta: 1.3202741990164826
     kappa: 8.925367437679225e-05
     s: 32.908307822925671
-    dkappa: 0
+    dkappa: -1.0450995875741721e-06
     ddkappa: 0
   }
   v: 8.0587606653959885
@@ -1650,7 +1650,7 @@ trajectory_point {
     theta: 1.320352746323272
     kappa: 8.8407965530889123e-05
     s: 33.720556213035636
-    dkappa: 0
+    dkappa: -1.0422390889599675e-06
     ddkappa: 0
   }
   v: 8.1871518597551809
@@ -1664,7 +1664,7 @@ trajectory_point {
     theta: 1.3204230570123203
     kappa: 8.7553591160128181e-05
     s: 34.545931959284964
-    dkappa: 0
+    dkappa: -1.0386942892018596e-06
     ddkappa: 0
   }
   v: 8.3213560525414412
@@ -1678,7 +1678,7 @@ trajectory_point {
     theta: 1.3204891184078307
     kappa: 8.66878628847465e-05
     s: 35.385030873913365
-    dkappa: 0
+    dkappa: -1.0347268611934174e-06
     ddkappa: 0
   }
   v: 8.46166382259025
@@ -1692,7 +1692,7 @@ trajectory_point {
     theta: 1.3205592992832962
     kappa: 8.5809998723438929e-05
     s: 36.238477926312711
-    dkappa: 0
+    dkappa: -1.0304022573362328e-06
     ddkappa: 0
   }
   v: 8.6083677341094038
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
index f593197fd3e47934a4ae6e5853a30317e70218a5..f6a76159797fdd6b50cef9e074b8b925de96b93a 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_cruise_0.pb.txt
@@ -27,7 +27,7 @@ trajectory_point {
     theta: 2.3560433667672163
     kappa: 2.6158767697247165e-05
     s: 0.10855362546611023
-    dkappa: 0
+    dkappa: 1.3006894338600993e-05
     ddkappa: 0
   }
   v: 5.43325748021816
@@ -41,7 +41,7 @@ trajectory_point {
     theta: 2.35613198127371
     kappa: 5.2372399839165587e-05
     s: 0.21733492734445656
-    dkappa: 0
+    dkappa: 2.6041068862682948e-05
     ddkappa: 0
   }
   v: 5.4449870492333332
@@ -55,7 +55,7 @@ trajectory_point {
     theta: 2.3562207924357268
     kappa: 7.8644202934733391e-05
     s: 0.3263576269823818
-    dkappa: 0
+    dkappa: 3.91041676639499e-05
     ddkappa: 0
   }
   v: 5.4573945561779045
@@ -69,7 +69,7 @@ trajectory_point {
     theta: 2.3563098111655658
     kappa: 0.00010497740541691364
     s: 0.435635121727451
-    dkappa: 0
+    dkappa: 5.219779601245568e-05
     ddkappa: 0
   }
   v: 5.470463889392871
@@ -83,7 +83,7 @@ trajectory_point {
     theta: 2.3563990481144552
     kappa: 0.00013137515849406405
     s: 0.54518048846064682
-    dkappa: 0
+    dkappa: 6.5323520779951374e-05
     ddkappa: 0
   }
   v: 5.4841791138790228
@@ -97,7 +97,7 @@ trajectory_point {
     theta: 2.3564885136754263
     kappa: 0.0001578405370013509
     s: 0.655006487129566
-    dkappa: 0
+    dkappa: 7.8482870863232857e-05
     ddkappa: 0
   }
   v: 5.498524471296939
@@ -111,7 +111,7 @@ trajectory_point {
     theta: 2.3565782179861947
     kappa: 0.00018437654025216262
     s: 0.76512556428161427
-    dkappa: 0
+    dkappa: 9.1677337607488213e-05
     ddkappa: 0
   }
   v: 5.5134843799669868
@@ -125,7 +125,7 @@ trajectory_point {
     theta: 2.3566681709320312
     kappa: 0.00021098609288952358
     s: 0.87554985659720286
-    dkappa: 0
+    dkappa: 0.00010490837522964527
     ddkappa: 0
   }
   v: 5.5290434348693243
@@ -139,7 +139,7 @@ trajectory_point {
     theta: 2.3567583821486426
     kappa: 0.00023767204573750794
     s: 0.98629119442294433
-    dkappa: 0
+    dkappa: 0.00011817740124171909
     ddkappa: 0
   }
   v: 5.545186407643901
@@ -153,7 +153,7 @@ trajectory_point {
     theta: 2.3568488610250462
     kappa: 0.00026443717665265319
     s: 1.0973611053048478
-    dkappa: 0
+    dkappa: 0.00013148579687415944
     ddkappa: 0
   }
   v: 5.5618982465904523
@@ -167,7 +167,7 @@ trajectory_point {
     theta: 2.3569396167064451
     kappa: 0.000291284191375374
     s: 1.2087708175215146
-    dkappa: 0
+    dkappa: 0.0001448349074991982
     ddkappa: 0
   }
   v: 5.5791640766685076
@@ -181,7 +181,7 @@ trajectory_point {
     theta: 2.3570306580971074
     kappa: 0.000318215724381376
     s: 1.3205312636173354
-    dkappa: 0
+    dkappa: 0.000158226043054197
     ddkappa: 0
   }
   v: 5.5969691994973818
@@ -195,7 +195,7 @@ trajectory_point {
     theta: 2.3571219938632404
     kappa: 0.0003452343397330694
     s: 1.4326530839356848
-    dkappa: 0
+    dkappa: 0.00017166047846499464
     ddkappa: 0
   }
   v: 5.6152990933561817
@@ -209,7 +209,7 @@ trajectory_point {
     theta: 2.3572136324358683
     kappa: 0.00037234253193098288
     s: 1.5451466301521171
-    dkappa: 0
+    dkappa: 0.00018513945406925452
     ddkappa: 0
   }
   v: 5.6341394131838038
@@ -223,7 +223,7 @@ trajectory_point {
     theta: 2.3573055820137094
     kappa: 0.00039954272676517714
     s: 1.6580219688075628
-    dkappa: 0
+    dkappa: 0.00019866417603981216
     ddkappa: 0
   }
   v: 5.6534759905789347
@@ -237,7 +237,7 @@ trajectory_point {
     theta: 2.3573978505660507
     kappa: 0.00042683728216665864
     s: 1.7712888848415236
-    dkappa: 0
+    dkappa: 0.00021223581680802268
     ddkappa: 0
   }
   v: 5.67329483380005
@@ -251,7 +251,7 @@ trajectory_point {
     theta: 2.3574904458356256
     kappa: 0.00045422848905879372
     s: 1.8849568851252692
-    dkappa: 0
+    dkappa: 0.00022585551548710839
     ddkappa: 0
   }
   v: 5.6935821277654126
@@ -265,7 +265,7 @@ trajectory_point {
     theta: 2.3575833753414912
     kappa: 0.00048171857220872166
     s: 1.9990352019950313
-    dkappa: 0
+    dkappa: 0.000239524378295506
     ddkappa: 0
   }
   v: 5.71432423405308
@@ -279,7 +279,7 @@ trajectory_point {
     theta: 2.3576171301297841
     kappa: 0.00043146259283864517
     s: 2.113532796785202
-    dkappa: 0
+    dkappa: 0.00020203844517222897
     ddkappa: 0
   }
   v: 5.7355076909008957
@@ -293,7 +293,7 @@ trajectory_point {
     theta: 2.3576439446979855
     kappa: 0.0003717759475825211
     s: 2.2284583633615282
-    dkappa: 0
+    dkappa: 0.00015833280158047619
     ddkappa: 0
   }
   v: 5.7571192132064946
@@ -307,7 +307,7 @@ trajectory_point {
     theta: 2.3576708610882124
     kappa: 0.00031186265689653363
     s: 2.3438203316543058
-    dkappa: 0
+    dkappa: 0.00011446119650208254
     ddkappa: 0
   }
   v: 5.7791456925272993
@@ -321,7 +321,7 @@ trajectory_point {
     theta: 2.3576978812065525
     kappa: 0.00025171847793257834
     s: 2.459626871191579
-    dkappa: 0
+    dkappa: 7.0420523104585466e-05
     ddkappa: 0
   }
   v: 5.8015741970805257
@@ -335,7 +335,7 @@ trajectory_point {
     theta: 2.3577250068991584
     kappa: 0.00019133930124761386
     s: 2.5758858946323322
-    dkappa: 0
+    dkappa: 2.6207772241597968e-05
     ddkappa: 0
   }
   v: 5.8243919717431751
@@ -349,7 +349,7 @@ trajectory_point {
     theta: 2.3577522399530761
     kappa: 0.00013072114896869313
     s: 2.6926050612996897
-    dkappa: 0
+    dkappa: -1.8179968890850312e-05
     ddkappa: 0
   }
   v: 5.8475864380520406
@@ -363,7 +363,7 @@ trajectory_point {
     theta: 2.3577795820970637
     kappa: 6.9860172957998277e-05
     s: 2.8097917807141077
-    dkappa: 0
+    dkappa: -6.2745518097969267e-05
     ddkappa: 0
   }
   v: 5.8711451942037067
@@ -377,7 +377,7 @@ trajectory_point {
     theta: 2.35780703500242
     kappa: 8.7526529778712641e-06
     s: 2.9274532161265747
-    dkappa: 0
+    dkappa: -0.00010749159952986741
     ddkappa: 0
   }
   v: 5.8950560150545437
@@ -391,7 +391,7 @@ trajectory_point {
     theta: 2.3578346002838066
     kappa: -5.260500514414935e-05
     s: 3.0455962880518013
-    dkappa: 0
+    dkappa: -0.0001524208450252071
     ddkappa: 0
   }
   v: 5.9193068521207151
@@ -405,7 +405,7 @@ trajectory_point {
     theta: 2.3578622795000741
     kappa: -0.00011421627135029842
     s: 3.1642276778014211
-    dkappa: 0
+    dkappa: -0.00019753579545486407
     ddkappa: 0
   }
   v: 5.9438858335781726
@@ -419,7 +419,7 @@ trajectory_point {
     theta: 2.3578900741550837
     kappa: -0.00017608449318754971
     s: 3.2833538310171844
-    dkappa: 0
+    dkappa: -0.00024283890206558385
     ddkappa: 0
   }
   v: 5.9687812642626561
@@ -433,7 +433,7 @@ trajectory_point {
     theta: 2.3579179856985339
     kappa: -0.00023821289764258246
     s: 3.4029809612041535
-    dkappa: 0
+    dkappa: -0.00028833252782363943
     ddkappa: 0
   }
   v: 5.9939816256696981
@@ -447,7 +447,7 @@ trajectory_point {
     theta: 2.357946015526784
     kappa: -0.00030060459297674951
     s: 3.5231150532639006
-    dkappa: 0
+    dkappa: -0.00033401894875848909
     ddkappa: 0
   }
   v: 6.019475575954619
@@ -461,7 +461,7 @@ trajectory_point {
     theta: 2.3579741649836787
     kappa: -0.00036326257056104231
     s: 3.6437618670277008
-    dkappa: 0
+    dkappa: -0.00037990035530643306
     ddkappa: 0
   }
   v: 6.0452519499325295
@@ -475,7 +475,7 @@ trajectory_point {
     theta: 2.3580024353613718
     kappa: -0.00042618970671105957
     s: 3.76492694078973
-    dkappa: 0
+    dkappa: -0.00042597885365427192
     ddkappa: 0
   }
   v: 6.0712997590783289
@@ -489,7 +489,7 @@ trajectory_point {
     theta: 2.3580308279011515
     kappa: -0.00048938876452197285
     s: 3.8866155948402605
-    dkappa: 0
+    dkappa: -0.00047225646708296356
     ddkappa: 0
   }
   v: 6.0976081915267084
@@ -503,7 +503,7 @@ trajectory_point {
     theta: 2.3580593437942632
     kappa: -0.00055286239570349351
     s: 4.0088329349988543
-    dkappa: 0
+    dkappa: -0.00051873513731128017
     ddkappa: 0
   }
   v: 6.1241666120721465
@@ -517,7 +517,7 @@ trajectory_point {
     theta: 2.3578739486658753
     kappa: -0.00067856440721197333
     s: 4.1315838561475635
-    dkappa: 0
+    dkappa: -0.00055041800205725541
     ddkappa: 0
   }
   v: 6.1509645621689124
@@ -531,7 +531,7 @@ trajectory_point {
     theta: 2.3576848690199084
     kappa: -0.00080564875321250773
     s: 4.2548730457641231
-    dkappa: 0
+    dkappa: -0.00058203857785443628
     ddkappa: 0
   }
   v: 6.1779917599310652
@@ -545,7 +545,7 @@ trajectory_point {
     theta: 2.3574949569115788
     kappa: -0.0009332925585836308
     s: 4.3787049874551434
-    dkappa: 0
+    dkappa: -0.00061379835590578827
     ddkappa: 0
   }
   v: 6.205238100132453
@@ -559,7 +559,7 @@ trajectory_point {
     theta: 2.3573042057846378
     kappa: -0.0010615002384439928
     s: 4.5030839644893161
-    dkappa: 0
+    dkappa: -0.00064569843476198531
     ddkappa: 0
   }
   v: 6.2326936542067157
@@ -573,7 +573,7 @@ trajectory_point {
     theta: 2.3571126093850134
     kappa: -0.0011902760050493547
     s: 4.6280140633305971
-    dkappa: 0
+    dkappa: -0.000677739862498237
     ddkappa: 0
   }
   v: 6.2603486702472786
@@ -587,7 +587,7 @@ trajectory_point {
     theta: 2.3569201617554
     kappa: -0.0013196238714345571
     s: 4.7534991771714123
-    dkappa: 0
+    dkappa: -0.00070992363762046728
     ddkappa: 0
   }
   v: 6.2881935730073613
@@ -601,7 +601,7 @@ trajectory_point {
     theta: 2.356726857229849
     kappa: -0.0014495476550554717
     s: 4.8795430094658512
-    dkappa: 0
+    dkappa: -0.00074225070997148987
     ddkappa: 0
   }
   v: 6.3162189638999706
@@ -615,7 +615,7 @@ trajectory_point {
     theta: 2.3565326904283594
     kappa: -0.0015800509814309538
     s: 5.0061490774628563
-    dkappa: 0
+    dkappa: -0.00077472198163718194
     ddkappa: 0
   }
   v: 6.3444156209979035
@@ -629,7 +629,7 @@ trajectory_point {
     theta: 2.3563376562514677
     kappa: -0.0017111372877848081
     s: 5.1333207157394289
-    dkappa: 0
+    dkappa: -0.00080733830785266318
     ddkappa: 0
   }
   v: 6.3727744990337465
@@ -643,7 +643,7 @@ trajectory_point {
     theta: 2.3561417498748352
     kappa: -0.0018428098266877375
     s: 5.2610610797338175
-    dkappa: 0
+    dkappa: -0.00084010049790846932
     ddkappa: 0
   }
   v: 6.4012867293998754
@@ -657,7 +657,7 @@ trajectory_point {
     theta: 2.3559449667438406
     kappa: -0.0019750716696993031
     s: 5.3893731492787165
-    dkappa: 0
+    dkappa: -0.00087300931605672864
     ddkappa: 0
   }
   v: 6.4299436201484568
@@ -671,7 +671,7 @@ trajectory_point {
     theta: 2.3557473025681679
     kappa: -0.0021079257110098804
     s: 5.5182597321344611
-    dkappa: 0
+    dkappa: -0.00090606548241733791
     ddkappa: 0
   }
   v: 6.4587366559914461
@@ -685,7 +685,7 @@ trajectory_point {
     theta: 2.3555487533163944
     kappa: -0.0022413746710826157
     s: 5.6477234675222245
-    dkappa: 0
+    dkappa: -0.00093926967388413855
     ddkappa: 0
   }
   v: 6.4876574983005888
@@ -699,7 +699,7 @@ trajectory_point {
     theta: 2.355349315210582
     kappa: -0.0023754211002953822
     s: 5.777766829657212
-    dkappa: 0
+    dkappa: -0.00097262252503109187
     ddkappa: 0
   }
   v: 6.51669798510742
@@ -713,7 +713,7 @@ trajectory_point {
     theta: 2.3551489847208638
     kappa: -0.0025100673825827376
     s: 5.9083921312818566
-    dkappa: 0
+    dkappa: -0.0010061246290184547
     ddkappa: 0
   }
   v: 6.5458501311032649
@@ -727,7 +727,7 @@ trajectory_point {
     theta: 2.3527391556585706
     kappa: -0.0033398190266636469
     s: 6.5703178198238179
-    dkappa: 0
+    dkappa: -0.0011047593513412416
     ddkappa: 0
   }
   v: 6.6930186292993286
@@ -741,7 +741,7 @@ trajectory_point {
     theta: 2.3500330966601384
     kappa: -0.0042137502887451446
     s: 7.2470522432214173
-    dkappa: 0
+    dkappa: -0.0011932430213239637
     ddkappa: 0
   }
   v: 6.8418823902984975
@@ -755,7 +755,7 @@ trajectory_point {
     theta: 2.3472673227604477
     kappa: -0.0051069715915515526
     s: 7.9387240365858442
-    dkappa: 0
+    dkappa: -0.0012836797666474066
     ddkappa: 0
   }
   v: 6.9916419927797548
@@ -769,7 +769,7 @@ trajectory_point {
     theta: 2.3426898379183037
     kappa: -0.0060420624195605911
     s: 8.645387413514614
-    dkappa: 0
+    dkappa: -0.0013026803723675581
     ddkappa: 0
   }
   v: 7.1416084277906329
@@ -783,7 +783,7 @@ trajectory_point {
     theta: 2.3378522256403573
     kappa: -0.0069990729049149141
     s: 9.36703320732842
-    dkappa: 0
+    dkappa: -0.0013152552767038937
     ddkappa: 0
   }
   v: 7.2912030987472161
@@ -797,7 +797,7 @@ trajectory_point {
     theta: 2.3326321362847837
     kappa: -0.00795766873883718
     s: 10.103599912307994
-    dkappa: 0
+    dkappa: -0.0013170327932809965
     ddkappa: 0
   }
   v: 7.43995782143414
@@ -811,7 +811,7 @@ trajectory_point {
     theta: 2.3256882465806452
     kappa: -0.0088312906285112729
     s: 10.854984724930956
-    dkappa: 0
+    dkappa: -0.0012555107751454652
     ddkappa: 0
   }
   v: 7.5875148240045878
@@ -825,7 +825,7 @@ trajectory_point {
     theta: 2.3186085258205309
     kappa: -0.0097219865643721082
     s: 11.621054585108677
-    dkappa: 0
+    dkappa: -0.0011927863720976922
     ddkappa: 0
   }
   v: 7.7336267469802982
@@ -839,7 +839,7 @@ trajectory_point {
     theta: 2.3105669888026359
     kappa: -0.010487160830676676
     s: 12.401657217423127
-    dkappa: 0
+    dkappa: -0.0010914066234293861
     ddkappa: 0
   }
   v: 7.8781566432515584
@@ -853,7 +853,7 @@ trajectory_point {
     theta: 2.3016260878914028
     kappa: -0.011137282162211624
     s: 13.196632172363731
-    dkappa: 0
+    dkappa: -0.00095418770405614466
     ddkappa: 0
   }
   v: 8.0210779780772086
@@ -867,7 +867,7 @@ trajectory_point {
     theta: 2.2925044317542578
     kappa: -0.011789135046685419
     s: 14.005820085302451
-    dkappa: 0
+    dkappa: -0.00081284060972258026
     ddkappa: 0
   }
   v: 8.1624025653024272
@@ -881,7 +881,7 @@ trajectory_point {
     theta: 2.2823211315952507
     kappa: -0.012032912839490567
     s: 14.829052875553039
-    dkappa: 0
+    dkappa: -0.00059802691437991281
     ddkappa: 0
   }
   v: 8.3019334435592835
@@ -895,7 +895,7 @@ trajectory_point {
     theta: 2.2719660672231785
     kappa: -0.012280792870026062
     s: 15.666138841575567
-    dkappa: 0
+    dkappa: -0.00037959838260817613
     ddkappa: 0
   }
   v: 8.4394269358208813
@@ -909,7 +909,7 @@ trajectory_point {
     theta: 2.2614993824213916
     kappa: -0.012161145864685101
     s: 16.516862794877913
-    dkappa: 0
+    dkappa: -0.00011031064375905096
     ddkappa: 0
   }
   v: 8.5746569800703636
@@ -923,7 +923,7 @@ trajectory_point {
     theta: 2.2509013921607348
     kappa: -0.011812157453883323
     s: 17.380987821516776
-    dkappa: 0
+    dkappa: 0.00019217443599404426
     ddkappa: 0
   }
   v: 8.7074151293009
@@ -937,7 +937,7 @@ trajectory_point {
     theta: 2.2406031756736291
     kappa: -0.011218191349502875
     s: 18.258257043598672
-    dkappa: 0
+    dkappa: 0.00052370402282597391
     ddkappa: 0
   }
   v: 8.8375105515157024
@@ -951,7 +951,7 @@ trajectory_point {
     theta: 2.2311895300720948
     kappa: -0.010075878462820157
     s: 19.148395380780933
-    dkappa: 0
+    dkappa: 0.00091513319307786852
     ddkappa: 0
   }
   v: 8.9647700297280117
@@ -965,7 +965,7 @@ trajectory_point {
     theta: 2.2218734942459384
     kappa: -0.0088541177506941213
     s: 20.051111311772733
-    dkappa: 0
+    dkappa: 0.001320782937050901
     ddkappa: 0
   }
   v: 9.0890379619611039
@@ -979,7 +979,7 @@ trajectory_point {
     theta: 2.2154159644643854
     kappa: -0.0067953212938007615
     s: 20.966098635836062
-    dkappa: 0
+    dkappa: 0.0018445612678295751
     ddkappa: 0
   }
   v: 9.2101763612482923
@@ -993,7 +993,7 @@ trajectory_point {
     theta: 2.2093551432558525
     kappa: -0.00510158063718516
     s: 21.893038234286742
-    dkappa: 0
+    dkappa: 0.0020541064728728459
     ddkappa: 0
   }
   v: 9.32806485563292
@@ -1007,7 +1007,7 @@ trajectory_point {
     theta: 2.204797100094428
     kappa: -0.00467306959287501
     s: 22.83159983199544
-    dkappa: 0
+    dkappa: 0.0012124384245994697
     ddkappa: 0
   }
   v: 9.4426006881683691
@@ -1021,7 +1021,7 @@ trajectory_point {
     theta: 2.2002799788355696
     kappa: -0.0042437143289935147
     s: 23.781443758888656
-    dkappa: 0
+    dkappa: 0.00045440801625462731
     ddkappa: 0
   }
   v: 9.5536987169180509
@@ -1035,7 +1035,7 @@ trajectory_point {
     theta: 2.1964736458351415
     kappa: -0.0038437501823036378
     s: 24.742222711449742
-    dkappa: 0
+    dkappa: 0.00043506272315523546
     ddkappa: 0
   }
   v: 9.6612914149554161
@@ -1049,7 +1049,7 @@ trajectory_point {
     theta: 2.1926570509524961
     kappa: -0.0034407754187259373
     s: 25.713583514219884
-    dkappa: 0
+    dkappa: 0.00041559427677584056
     ddkappa: 0
   }
   v: 9.7653288703639483
@@ -1063,7 +1063,7 @@ trajectory_point {
     theta: 2.1895630336494412
     kappa: -0.0030671631238343945
     s: 26.695168881299121
-    dkappa: 0
+    dkappa: 0.00039808755540605891
     ddkappa: 0
   }
   v: 9.8657787862371613
@@ -1077,7 +1077,7 @@ trajectory_point {
     theta: 2.1864468094192251
     kappa: -0.0026901873823295258
     s: 27.686619177847358
-    dkappa: 0
+    dkappa: 0.000380425618351655
     ddkappa: 0
   }
   v: 9.96262648067861
@@ -1091,7 +1091,7 @@ trajectory_point {
     theta: 2.1840248343638828
     kappa: -0.0023414872351390091
     s: 28.68757418158534
-    dkappa: 0
+    dkappa: 0.000364283281614437
     ddkappa: 0
   }
   v: 10.055874886801879
@@ -1105,7 +1105,7 @@ trajectory_point {
     theta: 2.1815896186757633
     kappa: -0.0019902940294857295
     s: 29.697674844295683
-    dkappa: 0
+    dkappa: 0.00034802095906986361
     ddkappa: 0
   }
   v: 10.145544552730589
@@ -1119,7 +1119,7 @@ trajectory_point {
     theta: 2.1795147551419887
     kappa: -0.0016657920853252041
     s: 30.716565053323862
-    dkappa: 0
+    dkappa: 0.0003327978296041244
     ddkappa: 0
   }
   v: 10.231673641598393
@@ -1133,7 +1133,7 @@ trajectory_point {
     theta: 2.17749109628535
     kappa: -0.0013405532619983954
     s: 31.743893393079219
-    dkappa: 0
+    dkappa: 0.00031751041051573421
     ddkappa: 0
   }
   v: 10.314317931548981
@@ -1147,7 +1147,7 @@ trajectory_point {
     theta: 2.176402729706616
     kappa: -0.0010398830629186132
     s: 32.779315648484229
-    dkappa: 0
+    dkappa: 0.0003029619251175838
     ddkappa: 0
   }
   v: 10.393580256410067
@@ -1161,7 +1161,7 @@ trajectory_point {
     theta: 2.1753834422048128
     kappa: -0.00074095273678535648
     s: 33.8225043482962
-    dkappa: 0
+    dkappa: 0.000288386575280117
     ddkappa: 0
   }
   v: 10.469689337462047
@@ -1175,7 +1175,7 @@ trajectory_point {
     theta: 2.1748273302978647
     kappa: -0.00046421715026369261
     s: 34.8731567425372
-    dkappa: 0
+    dkappa: 0.00027420797297080717
     ddkappa: 0
   }
   v: 10.542894853472767
@@ -1189,7 +1189,7 @@ trajectory_point {
     theta: 2.1743864458423032
     kappa: -0.0001923598888759771
     s: 35.930994506318584
-    dkappa: 0
+    dkappa: 0.00026000859304004084
     ddkappa: 0
   }
   v: 10.61343562746019
@@ -1203,7 +1203,7 @@ trajectory_point {
     theta: 2.1743169522341579
     kappa: 5.9988028126106068e-05
     s: 36.995762654265995
-    dkappa: 0
+    dkappa: 0.00024595575551838794
     ddkappa: 0
   }
   v: 10.681539626692402
@@ -1217,7 +1217,7 @@ trajectory_point {
     theta: 2.1744520838151637
     kappa: 0.0003036366292201284
     s: 38.067228454944427
-    dkappa: 0
+    dkappa: 0.00023185467249296386
     ddkappa: 0
   }
   v: 10.74742396268762
@@ -1231,7 +1231,7 @@ trajectory_point {
     theta: 2.174944554749509
     kappa: 0.00053087579110822709
     s: 39.145180345283173
-    dkappa: 0
+    dkappa: 0.00021773803421774125
     ddkappa: 0
   }
   v: 10.811294891214178
@@ -1245,7 +1245,7 @@ trajectory_point {
     theta: 2.1756110095318983
     kappa: 0.000744824904930148
     s: 40.229426845000866
-    dkappa: 0
+    dkappa: 0.00020349790728996979
     ddkappa: 0
   }
   v: 10.87334781229054
@@ -1259,7 +1259,7 @@ trajectory_point {
     theta: 2.1764453507822372
     kappa: 0.00094596177244425357
     s: 41.319795471030496
-    dkappa: 0
+    dkappa: 0.00018913799467503974
     ddkappa: 0
   }
   v: 10.933767270185287
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_follow_01_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_follow_01_0.pb.txt
index 9ab4284bd69fbd26c6be4968a01206511d6bd8ac..b897afae073b266c987adff705f478ee5a82179f 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_follow_01_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_follow_01_0.pb.txt
@@ -27,7 +27,7 @@ trajectory_point {
     theta: -1.9285172580786012
     kappa: -0.0685548197012652
     s: 0.14198880057550067
-    dkappa: 0
+    dkappa: 0.0015740081379530646
     ddkappa: 0
   }
   v: 7.09881271937749
@@ -41,7 +41,7 @@ trajectory_point {
     theta: -1.9353090604235463
     kappa: -0.065380662085486749
     s: 0.28394984870823897
-    dkappa: 0
+    dkappa: 0.0031477086279044627
     ddkappa: 0
   }
   v: 7.0972265196998858
@@ -55,7 +55,7 @@ trajectory_point {
     theta: -1.9421023615490376
     kappa: -0.0622073009459669
     s: 0.42587527523103447
-    dkappa: 0
+    dkappa: 0.0047210142366841783
     ddkappa: 0
   }
   v: 7.0952523957596041
@@ -69,7 +69,7 @@ trajectory_point {
     theta: -1.9488961678487975
     kappa: -0.059034907302309869
     s: 0.56775743146187407
-    dkappa: 0
+    dkappa: 0.0062938401752969941
     ddkappa: 0
   }
   v: 7.0929013059135269
@@ -83,7 +83,7 @@ trajectory_point {
     theta: -1.95568949531607
     kappa: -0.055863647303154074
     s: 0.70958888656785257
-    dkappa: 0
+    dkappa: 0.0078661040697006069
     ddkappa: 0
   }
   v: 7.0901840767155724
@@ -97,7 +97,7 @@ trajectory_point {
     theta: -1.9624813701110997
     kappa: -0.052693682285112786
     s: 0.85136242492911374
-    dkappa: 0
+    dkappa: 0.0094377259315837746
     ddkappa: 0
   }
   v: 7.0871114029166975
@@ -111,7 +111,7 @@ trajectory_point {
     theta: -1.9692708291142453
     kappa: -0.049525168831714657
     s: 0.99307104350279052
-    dkappa: 0
+    dkappa: 0.011008628129144419
     ddkappa: 0
   }
   v: 7.0836938474648949
@@ -125,7 +125,7 @@ trajectory_point {
     theta: -1.9760569204637395
     kappa: -0.046358258832344346
     s: 1.1347079491869463
-    dkappa: 0
+    dkappa: 0.012578735357867769
     ddkappa: 0
   }
   v: 7.0799418415051933
@@ -139,7 +139,7 @@ trajectory_point {
     theta: -1.9828387040771667
     kappa: -0.0431930995411831
     s: 1.2762665561845157
-    dkappa: 0
+    dkappa: 0.014147974611304489
     ddkappa: 0
   }
   v: 7.075865684379659
@@ -153,7 +153,7 @@ trajectory_point {
     theta: -1.9896152521557859
     kappa: -0.040029833636149356
     s: 1.417740483367244
-    dkappa: 0
+    dkappa: 0.015716275151848782
     ddkappa: 0
   }
   v: 7.0714755436273951
@@ -167,7 +167,7 @@ trajectory_point {
     theta: -1.9963856496708765
     kappa: -0.036868599277839295
     s: 1.5591235516396302
-    dkappa: 0
+    dkappa: 0.017283568481516555
     ddkappa: 0
   }
   v: 7.0667814549845405
@@ -181,7 +181,7 @@ trajectory_point {
     theta: -2.0031489948313532
     kappa: -0.03370953016846745
     s: 1.7004097813028656
-    dkappa: 0
+    dkappa: 0.018849788312723503
     ddkappa: 0
   }
   v: 7.06179332238427
@@ -195,7 +195,7 @@ trajectory_point {
     theta: -2.0099043995319521
     kappa: -0.030552755610807288
     s: 1.8415933894187757
-    dkappa: 0
+    dkappa: 0.020414870539063278
     ddkappa: 0
   }
   v: 7.0565209179567985
@@ -209,7 +209,7 @@ trajectory_point {
     theta: -2.0166509897813532
     kappa: -0.027398400567131803
     s: 1.9826687871737607
-    dkappa: 0
+    dkappa: 0.021978753206085579
     ddkappa: 0
   }
   v: 7.0509738820293739
@@ -223,7 +223,7 @@ trajectory_point {
     theta: -2.0194041005030687
     kappa: -0.025069947384123884
     s: 2.1236305772427357
-    dkappa: 0
+    dkappa: 0.021955397221730018
     ddkappa: 0
   }
   v: 7.0451617231262817
@@ -237,7 +237,7 @@ trajectory_point {
     theta: -2.0208727017764927
     kappa: -0.023008350417466145
     s: 2.2644735511530731
-    dkappa: 0
+    dkappa: 0.021421816485018119
     ddkappa: 0
   }
   v: 7.0390938179688458
@@ -251,7 +251,7 @@ trajectory_point {
     theta: -2.0223400464261045
     kappa: -0.020948566142583971
     s: 2.40519268664854
-    dkappa: 0
+    dkappa: 0.020888704907617513
     ddkappa: 0
   }
   v: 7.0327794114754258
@@ -265,7 +265,7 @@ trajectory_point {
     theta: -2.0238060775945672
     kappa: -0.018890665385868394
     s: 2.5457831450532424
-    dkappa: 0
+    dkappa: 0.020356080820753221
     ddkappa: 0
   }
   v: 7.0262276167614166
@@ -279,7 +279,7 @@ trajectory_point {
     theta: -2.0252707403414889
     kappa: -0.016834716325128386
     s: 2.6862402686355651
-    dkappa: 0
+    dkappa: 0.01982396187014656
     ddkappa: 0
   }
   v: 7.0194474151392514
@@ -293,7 +293,7 @@ trajectory_point {
     theta: -2.0267339816182743
     kappa: -0.014780784528176329
     s: 2.8265595779721107
-    dkappa: 0
+    dkappa: 0.0192923650260018
     ddkappa: 0
   }
   v: 7.0124476561183995
@@ -307,7 +307,7 @@ trajectory_point {
     theta: -2.0281957502428458
     kappa: -0.012728932991413446
     s: 2.9667367693116429
-    dkappa: 0
+    dkappa: 0.018761306592992792
     ddkappa: 0
   }
   v: 7.0052370574053668
@@ -321,7 +321,7 @@ trajectory_point {
     theta: -2.0296559968742276
     kappa: -0.010679222178415304
     s: 3.1067677119390247
-    dkappa: 0
+    dkappa: 0.01823080222024967
     ddkappa: 0
   }
   v: 6.9978242049036963
@@ -335,7 +335,7 @@ trajectory_point {
     theta: -2.0311146739870019
     kappa: -0.0086317100585172665
     s: 3.24664844553916
-    dkappa: 0
+    dkappa: 0.017700866911345467
     ddkappa: 0
   }
   v: 6.9902175527139674
@@ -349,7 +349,7 @@ trajectory_point {
     theta: -2.032571735845635
     kappa: -0.0065864521453999521
     s: 3.3863751775609368
-    dkappa: 0
+    dkappa: 0.0171715150342828
     ddkappa: 0
   }
   v: 6.9824254231337957
@@ -363,7 +363,7 @@ trajectory_point {
     theta: -2.0340271384786792
     kappa: -0.0045435015356747122
     s: 3.5259442805811632
-    dkappa: 0
+    dkappa: 0.016642760331480509
     ddkappa: 0
   }
   v: 6.9744560066578343
@@ -377,7 +377,7 @@ trajectory_point {
     theta: -2.035480839652855
     kappa: -0.0025029089474690848
     s: 3.6653522896685122
-    dkappa: 0
+    dkappa: 0.016114615929760327
     ddkappa: 0
   }
   v: 6.9663173619777732
@@ -391,7 +391,7 @@ trajectory_point {
     theta: -2.03693279884701
     kappa: -0.00046472275901226004
     s: 3.8045958997474614
-    dkappa: 0
+    dkappa: 0.015587094350333517
     ddkappa: 0
   }
   v: 6.9580174159823374
@@ -405,7 +405,7 @@ trajectory_point {
     theta: -2.0383829772259645
     kappa: 0.0015710109527794153
     s: 3.9436719629622314
-    dkappa: 0
+    dkappa: 0.015060207518787561
     ddkappa: 0
   }
   v: 6.9495639637572895
@@ -419,7 +419,7 @@ trajectory_point {
     theta: -2.0391812020106657
     kappa: 0.003458970693214279
     s: 4.08257748604073
-    dkappa: 0
+    dkappa: 0.014566317950373641
     ddkappa: 0
   }
   v: 6.9409646685854307
@@ -433,7 +433,7 @@ trajectory_point {
     theta: -2.0373290824861545
     kappa: 0.004752572787671651
     s: 4.221309627658492
-    dkappa: 0
+    dkappa: 0.014204874679656232
     ddkappa: 0
   }
   v: 6.9322270619465955
@@ -447,7 +447,7 @@ trajectory_point {
     theta: -2.0354792355466973
     kappa: 0.0060445330923345319
     s: 4.3598656958026165
-    dkappa: 0
+    dkappa: 0.013843890138772127
     ddkappa: 0
   }
   v: 6.923358543517657
@@ -461,7 +461,7 @@ trajectory_point {
     theta: -2.0336317081056854
     kappa: 0.0073348278733101034
     s: 4.4982431451357128
-    dkappa: 0
+    dkappa: 0.013483370959169964
     ddkappa: 0
   }
   v: 6.9143663811725249
@@ -475,7 +475,7 @@ trajectory_point {
     theta: -2.0317865450626855
     kappa: 0.0086234347397954065
     s: 4.636439574359839
-    dkappa: 0
+    dkappa: 0.013123323397027813
     ddkappa: 0
   }
   v: 6.9052577109821449
@@ -489,7 +489,7 @@ trajectory_point {
     theta: -2.0299437893359009
     kappa: 0.0099103326194974924
     s: 4.77445272358044
-    dkappa: 0
+    dkappa: 0.012763753340120991
     ddkappa: 0
   }
   v: 6.8960395372145005
@@ -503,7 +503,7 @@ trajectory_point {
     theta: -2.0281034818948567
     kappa: 0.011195501734053644
     s: 4.9122804716702939
-    dkappa: 0
+    dkappa: 0.012404666314689866
     ddkappa: 0
   }
   v: 6.8867187323346108
@@ -517,7 +517,7 @@ trajectory_point {
     theta: -2.0262656617933072
     kappa: 0.012478923574451508
     s: 5.0499208336334469
-    dkappa: 0
+    dkappa: 0.012046067492307667
     ddkappa: 0
   }
   v: 6.8773020370045312
@@ -531,7 +531,7 @@ trajectory_point {
     theta: -2.0244303662023522
     kappa: 0.0137605808764493
     s: 5.187371957969158
-    dkappa: 0
+    dkappa: 0.011687961696748291
     ddkappa: 0
   }
   v: 6.8677960600833563
@@ -545,7 +545,7 @@ trajectory_point {
     theta: -2.0225976304437649
     kappa: 0.015040457595995961
     s: 5.3246321240358379
-    dkappa: 0
+    dkappa: 0.011330353410854119
     ddkappa: 0
   }
   v: 6.8582072786272139
@@ -559,7 +559,7 @@ trajectory_point {
     theta: -2.0207674880235147
     kappa: 0.016318538884651339
     s: 5.4616997394149909
-    dkappa: 0
+    dkappa: 0.010973246783403817
     ddkappa: 0
   }
   v: 6.8485420378892705
@@ -573,7 +573,7 @@ trajectory_point {
     theta: -2.0189399706654876
     kappa: 0.017594811065006367
     s: 5.5985733372751545
-    dkappa: 0
+    dkappa: 0.010616645635980148
     ddkappa: 0
   }
   v: 6.8388065513197294
@@ -587,7 +587,7 @@ trajectory_point {
     theta: -2.0171151083453935
     kappa: 0.018869261606103225
     s: 5.73525157373584
-    dkappa: 0
+    dkappa: 0.010260553469837789
     ddkappa: 0
   }
   v: 6.82900690056583
@@ -601,7 +601,7 @@ trajectory_point {
     theta: -2.0152929293248523
     kappa: 0.02014187909885555
     s: 5.8717332252314769
-    dkappa: 0
+    dkappa: 0.00990497347277112
     ddkappa: 0
   }
   v: 6.819149035471848
@@ -615,7 +615,7 @@ trajectory_point {
     theta: -2.0134734601856561
     kappa: 0.021412653231468546
     s: 6.0080171858753459
-    dkappa: 0
+    dkappa: 0.0095499085259820628
     ddkappa: 0
   }
   v: 6.8092387740790974
@@ -629,7 +629,7 @@ trajectory_point {
     theta: -2.0109273033595283
     kappa: 0.022479537496000485
     s: 6.1441024648235274
-    dkappa: 0
+    dkappa: 0.00922475884889526
     ddkappa: 0
   }
   v: 6.7992818026259263
@@ -643,7 +643,7 @@ trajectory_point {
     theta: -2.0071124221519856
     kappa: 0.023192545252352981
     s: 6.27998818363884
-    dkappa: 0
+    dkappa: 0.0089513494966575458
     ddkappa: 0
   }
   v: 6.7892836755477219
@@ -657,7 +657,7 @@ trajectory_point {
     theta: -2.0033024894818054
     kappa: 0.023904501860806934
     s: 6.4156735736547761
-    dkappa: 0
+    dkappa: 0.0086783432167054936
     ddkappa: 0
   }
   v: 6.7792498154769074
@@ -671,7 +671,7 @@ trajectory_point {
     theta: -1.999497637127112
     kappa: 0.024615403850229603
     s: 6.5511579733394507
-    dkappa: 0
+    dkappa: 0.008405741340076732
     ddkappa: 0
   }
   v: 6.7691855132429408
@@ -685,7 +685,7 @@ trajectory_point {
     theta: -1.995697993055533
     kappa: 0.025325248297804061
     s: 6.6864408256595391
-    dkappa: 0
+    dkappa: 0.0081335449875521732
     ddkappa: 0
   }
   v: 6.75909592787232
@@ -699,7 +699,7 @@ trajectory_point {
     theta: -1.9919036814749833
     kappa: 0.026034032815197408
     s: 6.8215216754442123
-    dkappa: 0
+    dkappa: 0.0078617550749599321
     ddkappa: 0
   }
   v: 6.7489860865885758
@@ -713,7 +713,7 @@ trajectory_point {
     theta: -1.9881148228861785
     kappa: 0.026741755534729156
     s: 6.9564001667490869
-    dkappa: 0
+    dkappa: 0.0075903723184791941
     ddkappa: 0
   }
   v: 6.73886088481228
@@ -727,7 +727,7 @@ trajectory_point {
     theta: -1.9692562898013606
     kappa: 0.030264411498518117
     s: 7.6277514017304044
-    dkappa: 0
+    dkappa: 0.0062395776368857087
     ddkappa: 0
   }
   v: 6.6881665820550138
@@ -741,7 +741,7 @@ trajectory_point {
     theta: -1.9493000165914329
     kappa: 0.033125662773719741
     s: 8.29404272515885
-    dkappa: 0
+    dkappa: 0.004931725563710204
     ddkappa: 0
   }
   v: 6.6377467933654568
@@ -755,7 +755,7 @@ trajectory_point {
     theta: -1.9259471366922791
     kappa: 0.034169239252497777
     s: 8.9553254573768371
-    dkappa: 0
+    dkappa: 0.0037263962648327181
     ddkappa: 0
   }
   v: 6.5880651679810622
@@ -769,7 +769,7 @@ trajectory_point {
     theta: -1.9027612058005363
     kappa: 0.035205059284584658
     s: 9.6116931648079174
-    dkappa: 0
+    dkappa: 0.0025300256504811351
     ddkappa: 0
   }
   v: 6.5395029782872021
@@ -783,7 +783,7 @@ trajectory_point {
     theta: -1.8798226034759129
     kappa: 0.035822192783885591
     s: 10.263273422271576
-    dkappa: 0
+    dkappa: 0.0013730150128215984
     ddkappa: 0
   }
   v: 6.4923591198171735
@@ -797,7 +797,7 @@ trajectory_point {
     theta: -1.8572862600455624
     kappa: 0.034786516249167052
     s: 10.910219575298024
-    dkappa: 0
+    dkappa: 0.00034705868996435195
     ddkappa: 0
   }
   v: 6.44685011125219
@@ -811,7 +811,7 @@ trajectory_point {
     theta: -1.8348974209088815
     kappa: 0.03375798475780796
     s: 11.552702502442989
-    dkappa: 0
+    dkappa: -0.0006718196484392779
     ddkappa: 0
   }
   v: 6.4031100944213879
@@ -825,7 +825,7 @@ trajectory_point {
     theta: -1.812967780213057
     kappa: 0.032616545201771124
     s: 12.190902377602512
-    dkappa: 0
+    dkappa: -0.0016605982322756277
     ddkappa: 0
   }
   v: 6.3611908343018255
@@ -839,7 +839,7 @@ trajectory_point {
     theta: -1.7944699419359407
     kappa: 0.030151064009966675
     s: 12.825000432327732
-    dkappa: 0
+    dkappa: -0.0023839212642423535
     ddkappa: 0
   }
   v: 6.3210617190184788
@@ -853,7 +853,7 @@ trajectory_point {
     theta: -1.7760804021605836
     kappa: 0.02770085465239867
     s: 13.455170718139685
-    dkappa: 0
+    dkappa: -0.0031027638444902357
     ddkappa: 0
   }
   v: 6.2826097598442461
@@ -867,7 +867,7 @@ trajectory_point {
     theta: -1.7578069417797533
     kappa: 0.025265296753489236
     s: 14.081572789671535
-    dkappa: 0
+    dkappa: -0.00381730797884655
     ddkappa: 0
   }
   v: 6.2456770964545782
@@ -881,7 +881,7 @@ trajectory_point {
     theta: -1.7447964576056949
     kappa: 0.022151146586948552
     s: 14.704354671535999
-    dkappa: 0
+    dkappa: -0.0042327789064357212
     ddkappa: 0
   }
   v: 6.21020039895846
@@ -895,7 +895,7 @@ trajectory_point {
     theta: -1.7324234846241744
     kappa: 0.018978060115544055
     s: 15.323660261426825
-    dkappa: 0
+    dkappa: -0.0046134334436021187
     ddkappa: 0
   }
   v: 6.1761454031019332
@@ -909,7 +909,7 @@ trajectory_point {
     theta: -1.7201182137488811
     kappa: 0.015822065561764631
     s: 15.939629944709825
-    dkappa: 0
+    dkappa: -0.0049920375746256357
     ddkappa: 0
   }
   v: 6.1434761265802385
@@ -923,7 +923,7 @@ trajectory_point {
     theta: -1.7121618649743724
     kappa: 0.012556396003095334
     s: 16.552400422617794
-    dkappa: 0
+    dkappa: -0.0051862064325525247
     ddkappa: 0
   }
   v: 6.1121548690378127
@@ -937,7 +937,7 @@ trajectory_point {
     theta: -1.7065329682330921
     kappa: 0.0092397155341408729
     s: 17.162104540445434
-    dkappa: 0
+    dkappa: -0.0052819173521646062
     ddkappa: 0
   }
   v: 6.0821422120682955
@@ -951,7 +951,7 @@ trajectory_point {
     theta: -1.7009311915684873
     kappa: 0.0059390147668928073
     s: 17.768871115744258
-    dkappa: 0
+    dkappa: -0.0053771671384357581
     ddkappa: 0
   }
   v: 6.053397019214521
@@ -965,7 +965,7 @@ trajectory_point {
     theta: -1.6975286699515233
     kappa: 0.0027452015829361092
     s: 18.372824766517542
-    dkappa: 0
+    dkappa: -0.0053943568306580062
     ddkappa: 0
   }
   v: 6.0258764359685237
@@ -979,7 +979,7 @@ trajectory_point {
     theta: -1.698356402217738
     kappa: -0.0002567207519011943
     s: 18.974085739415209
-    dkappa: 0
+    dkappa: -0.0052609110691910444
     ddkappa: 0
   }
   v: 5.9995358897715363
@@ -993,7 +993,7 @@ trajectory_point {
     theta: -1.6991805872436012
     kappa: -0.0032457769981203621
     s: 19.572769737928777
-    dkappa: 0
+    dkappa: -0.0051280372495670894
     ddkappa: 0
   }
   v: 5.97432909001399
@@ -1007,7 +1007,7 @@ trajectory_point {
     theta: -1.700001376705073
     kappa: -0.0062225212893774334
     s: 20.168987750586258
-    dkappa: 0
+    dkappa: -0.004995710738641297
     ddkappa: 0
   }
   v: 5.9502080280355152
@@ -1021,7 +1021,7 @@ trajectory_point {
     theta: -1.7060064074055679
     kappa: -0.0084740997646183449
     s: 20.76284587914709
-    dkappa: 0
+    dkappa: -0.004637905606538862
     ddkappa: 0
   }
   v: 5.927122977124939
@@ -1035,7 +1035,7 @@ trajectory_point {
     theta: -1.7121092670819855
     kappa: -0.010700563508160742
     s: 21.354445166797056
-    dkappa: 0
+    dkappa: -0.0042762182021151875
     ddkappa: 0
   }
   v: 5.9050224925202883
@@ -1049,7 +1049,7 @@ trajectory_point {
     theta: -1.7181896864641195
     kappa: -0.012918886769200933
     s: 21.943881426343196
-    dkappa: 0
+    dkappa: -0.0039158532131117153
     ddkappa: 0
   }
   v: 5.88385341140879
@@ -1063,7 +1063,7 @@ trajectory_point {
     theta: -1.7261365710589862
     kappa: -0.014419816095235511
     s: 22.531245068408744
-    dkappa: 0
+    dkappa: -0.0034095981599067604
     ddkappa: 0
   }
   v: 5.8635608529268648
@@ -1077,7 +1077,7 @@ trajectory_point {
     theta: -1.7354450946891027
     kappa: -0.015394322700071027
     s: 23.116620929628027
-    dkappa: 0
+    dkappa: -0.002796939020229405
     ddkappa: 0
   }
   v: 5.8440882181601381
@@ -1091,7 +1091,7 @@ trajectory_point {
     theta: -1.7447234855108162
     kappa: -0.01636565180624187
     s: 23.700088100841388
-    dkappa: 0
+    dkappa: -0.0021862775310436573
     ddkappa: 0
   }
   v: 5.8253771901434286
@@ -1105,7 +1105,7 @@ trajectory_point {
     theta: -1.7540067537131838
     kappa: -0.017080123549156143
     s: 24.281719755290126
-    dkappa: 0
+    dkappa: -0.0015379386920666403
     ddkappa: 0
   }
   v: 5.8073677338607563
@@ -1119,7 +1119,7 @@ trajectory_point {
     theta: -1.7634733159295561
     kappa: -0.016279393693540069
     s: 24.861582976811391
-    dkappa: 0
+    dkappa: -0.00065550662049980988
     ddkappa: 0
   }
   v: 5.7899980962453386
@@ -1133,7 +1133,7 @@ trajectory_point {
     theta: -1.7729129855096259
     kappa: -0.01548102186736584
     s: 25.439738588033123
-    dkappa: 0
+    dkappa: 0.00022432682084161512
     ddkappa: 0
   }
   v: 5.7732048061795922
@@ -1147,7 +1147,7 @@ trajectory_point {
     theta: -1.7823249760181361
     kappa: -0.014684932198613075
     s: 26.01624153307144
-    dkappa: 0
+    dkappa: 0.0011016452455554766
     ddkappa: 0
   }
   v: 5.7569447544832872
@@ -1161,7 +1161,7 @@ trajectory_point {
     theta: -1.7886385798346311
     kappa: -0.012171555681418063
     s: 26.59114664507522
-    dkappa: 0
+    dkappa: 0.0020973326312711338
     ddkappa: 0
   }
   v: 5.74125742943781
@@ -1175,7 +1175,7 @@ trajectory_point {
     theta: -1.7934996412743835
     kappa: -0.0088609203307571174
     s: 27.164513753038047
-    dkappa: 0
+    dkappa: 0.0031468389735067482
     ddkappa: 0
   }
   v: 5.7261916690513415
@@ -1189,7 +1189,7 @@ trajectory_point {
     theta: -1.7983483002857392
     kappa: -0.0055587957631408726
     s: 27.736406883185751
-    dkappa: 0
+    dkappa: 0.0041936473077056214
     ddkappa: 0
   }
   v: 5.7117825799606461
@@ -1203,7 +1203,7 @@ trajectory_point {
     theta: -1.8031848957925773
     kappa: -0.0022647959832223937
     s: 28.306892885839286
-    dkappa: 0
+    dkappa: 0.0052378799984660156
     ddkappa: 0
   }
   v: 5.698051537431077
@@ -1217,7 +1217,7 @@ trajectory_point {
     theta: -1.8058804052586819
     kappa: -0.00065808040193766437
     s: 28.876040062277546
-    dkappa: 0
+    dkappa: 0.0049342468399636279
     ddkappa: 0
   }
   v: 5.68500618535657
@@ -1231,7 +1231,7 @@ trajectory_point {
     theta: -1.8063169724958201
     kappa: -0.0008318588436141991
     s: 29.443916791600262
-    dkappa: 0
+    dkappa: 0.0032078905096750653
     ddkappa: 0
   }
   v: 5.672640436259651
@@ -1245,7 +1245,7 @@ trajectory_point {
     theta: -1.8067526146676169
     kappa: -0.0010052690388142871
     s: 30.010590157590844
-    dkappa: 0
+    dkappa: 0.0014851924273808249
     ddkappa: 0
   }
   v: 5.6609344712914309
@@ -1259,7 +1259,7 @@ trajectory_point {
     theta: -1.8071873811295776
     kappa: -0.0011783306995547802
     s: 30.576124575579236
-    dkappa: 0
+    dkappa: -0.00023404323077817291
     ddkappa: 0
   }
   v: 5.6498547402316053
@@ -1273,7 +1273,7 @@ trajectory_point {
     theta: -1.8079688824133331
     kappa: -0.0013738803604319806
     s: 31.140580419304793
-    dkappa: 0
+    dkappa: -0.00031742340791364964
     ddkappa: 0
   }
   v: 5.6393539614884576
@@ -1287,7 +1287,7 @@ trajectory_point {
     theta: -1.8087641554951623
     kappa: -0.0015700725285500573
     s: 31.70401264777912
-    dkappa: 0
+    dkappa: -0.00032931031640180529
     ddkappa: 0
   }
   v: 5.6293711220988563
@@ -1301,7 +1301,7 @@ trajectory_point {
     theta: -1.8095580515725602
     kappa: -0.0017659250382601331
     s: 32.266469432148945
-    dkappa: 0
+    dkappa: -0.00034117664563620238
     ddkappa: 0
   }
   v: 5.6198314777282565
@@ -1315,7 +1315,7 @@ trajectory_point {
     theta: -1.8105165956766023
     kappa: -0.00197165461316618
     s: 32.827990782558949
-    dkappa: 0
+    dkappa: -0.000353310391925955
     ddkappa: 0
   }
   v: 5.6106465526706994
@@ -1329,7 +1329,7 @@ trajectory_point {
     theta: -1.8117156443285154
     kappa: -0.0021919321977770584
     s: 33.388607175014684
-    dkappa: 0
+    dkappa: -0.00036584336083793573
     ddkappa: 0
   }
   v: 5.6017141398488137
@@ -1343,7 +1343,7 @@ trajectory_point {
     theta: -1.8129128006038597
     kappa: -0.002411861894950889
     s: 33.948338178245351
-    dkappa: 0
+    dkappa: -0.00037835653625871261
     ddkappa: 0
   }
   v: 5.5929183008138112
@@ -1357,7 +1357,7 @@ trajectory_point {
     theta: -1.8141080766613784
     kappa: -0.0026314465684152016
     s: 34.507191080566734
-    dkappa: 0
+    dkappa: -0.00039085008112364837
     ddkappa: 0
   }
   v: 5.5841293657454942
@@ -1371,7 +1371,7 @@ trajectory_point {
     theta: -1.8156803681927298
     kappa: -0.0028733364392793319
     s: 35.065159516744032
-    dkappa: 0
+    dkappa: -0.00040424672152591056
     ddkappa: 0
   }
   v: 5.5752039334522481
@@ -1385,7 +1385,7 @@ trajectory_point {
     theta: -1.8173253937761376
     kappa: -0.0031193340942503259
     s: 35.622222094854664
-    dkappa: 0
+    dkappa: -0.00041780496186584351
     ddkappa: 0
   }
   v: 5.5659848713710431
@@ -1399,7 +1399,7 @@ trajectory_point {
     theta: -1.8189676323756041
     kappa: -0.0033649150354615092
     s: 36.178341023151226
-    dkappa: 0
+    dkappa: -0.00043134023489208739
     ddkappa: 0
   }
   v: 5.5563013155674392
@@ -1413,7 +1413,7 @@ trajectory_point {
     theta: -1.8207278853388085
     kappa: -0.0036173162802715523
     s: 36.733460736924286
-    dkappa: 0
+    dkappa: -0.00044522866994543912
     ddkappa: 0
   }
   v: 5.545968670735582
@@ -1427,7 +1427,7 @@ trajectory_point {
     theta: -1.8228654364183885
     kappa: -0.0038920805664391395
     s: 37.287506525365238
-    dkappa: 0
+    dkappa: -0.00046027813110001024
     ddkappa: 0
   }
   v: 5.5347886101982011
@@ -1441,7 +1441,7 @@ trajectory_point {
     theta: -1.8249984877958709
     kappa: -0.0041662650410653661
     s: 37.840383158429191
-    dkappa: 0
+    dkappa: -0.0004752958346655934
     ddkappa: 0
   }
   v: 5.5225490759066131
@@ -1455,7 +1455,7 @@ trajectory_point {
     theta: -1.8271265681873325
     kappa: -0.0044398116203991523
     s: 38.391973513697828
-    dkappa: 0
+    dkappa: -0.00049027859926382327
     ddkappa: 0
   }
   v: 5.5090242784407222
@@ -1469,7 +1469,7 @@ trajectory_point {
     theta: -1.8295920218574875
     kappa: -0.004733862566059145
     s: 38.942137203242218
-    dkappa: 0
+    dkappa: -0.00050652941481959938
     ddkappa: 0
   }
   v: 5.4939746970090164
@@ -1483,7 +1483,7 @@ trajectory_point {
     theta: -1.8322579182163881
     kappa: -0.0050399016548471412
     s: 39.490709200485746
-    dkappa: 0
+    dkappa: -0.0005235241806287197
     ddkappa: 0
   }
   v: 5.4771470794485735
@@ -1497,7 +1497,7 @@ trajectory_point {
     theta: -1.8349151585743513
     kappa: -0.0053449461881889832
     s: 40.037498467066904
-    dkappa: 0
+    dkappa: -0.00054046371742458386
     ddkappa: 0
   }
   v: 5.4582744422250533
@@ -1511,7 +1511,7 @@ trajectory_point {
     theta: -1.8375626440641757
     kappa: -0.0056488743114092889
     s: 40.582286579702206
-    dkappa: 0
+    dkappa: -0.0005573412584517454
     ddkappa: 0
   }
   v: 5.4370760704327052
@@ -1525,7 +1525,7 @@ trajectory_point {
     theta: -1.8407798273643337
     kappa: -0.0059893366080030312
     s: 41.124826357048995
-    dkappa: 0
+    dkappa: -0.00057677754033275466
     ddkappa: 0
   }
   v: 5.4132575177943609
@@ -1539,7 +1539,7 @@ trajectory_point {
     theta: -1.8440013463364602
     kappa: -0.0063294670785557887
     s: 41.66484048656833
-    dkappa: 0
+    dkappa: -0.00059621050206628483
     ddkappa: 0
   }
   v: 5.3865106066614423
@@ -1553,7 +1553,7 @@ trajectory_point {
     theta: -1.8472059480923348
     kappa: -0.0066678122480221388
     s: 42.20202015138787
-    dkappa: 0
+    dkappa: -0.00061554146268348283
     ddkappa: 0
   }
   v: 5.3565134280139572
@@ -1567,7 +1567,7 @@ trajectory_point {
     theta: -1.8507749938050284
     kappa: -0.0070157644555388036
     s: 42.736023657164672
-    dkappa: 0
+    dkappa: -0.00063565859514463994
     ddkappa: 0
   }
   v: 5.3229303414604958
@@ -1581,7 +1581,7 @@ trajectory_point {
     theta: -1.8554323502309489
     kappa: -0.0073950601053213308
     s: 43.266475058948089
-    dkappa: 0
+    dkappa: -0.00065825296579927293
     ddkappa: 0
   }
   v: 5.2854119752382376
@@ -1595,7 +1595,7 @@ trajectory_point {
     theta: -1.8600550316911943
     kappa: -0.0077715215580350955
     s: 43.792962788042622
-    dkappa: 0
+    dkappa: -0.00068067850536101941
     ddkappa: 0
   }
   v: 5.2435952262129488
@@ -1609,7 +1609,7 @@ trajectory_point {
     theta: -1.864638901654682
     kappa: -0.008144828069929385
     s: 44.315038278870794
-    dkappa: 0
+    dkappa: -0.00070291610735398664
     ddkappa: 0
   }
   v: 5.197103259878979
@@ -1623,7 +1623,7 @@ trajectory_point {
     theta: -1.8691883471242954
     kappa: -0.0085382572705136016
     s: 44.832214595835971
-    dkappa: 0
+    dkappa: -0.00072685623988664647
     ddkappa: 0
   }
   v: 5.1455455103592671
@@ -1637,7 +1637,7 @@ trajectory_point {
     theta: -1.8737008676734452
     kappa: -0.00895619043280254
     s: 45.343965060185255
-    dkappa: 0
+    dkappa: -0.00075286135707729673
     ddkappa: 0
   }
   v: 5.0885176804053343
@@ -1651,7 +1651,7 @@ trajectory_point {
     theta: -1.87816062830064
     kappa: -0.0093692287404784844
     s: 45.849721876872309
-    dkappa: 0
+    dkappa: -0.00077856190100282258
     ddkappa: 0
   }
   v: 5.0256017413972938
@@ -1665,7 +1665,7 @@ trajectory_point {
     theta: -1.8825620702735679
     kappa: -0.00977687379023928
     s: 46.348874761420269
-    dkappa: 0
+    dkappa: -0.00080392685944057659
     ddkappa: 0
   }
   v: 4.9563659333438377
@@ -1679,7 +1679,7 @@ trajectory_point {
     theta: -1.8872651642747753
     kappa: -0.010208059171424607
     s: 46.840769566784559
-    dkappa: 0
+    dkappa: -0.00083140794917909214
     ddkappa: 0
   }
   v: 4.880364764882251
@@ -1693,7 +1693,7 @@ trajectory_point {
     theta: -1.8922669620446055
     kappa: -0.010662438045450896
     s: 47.324706910215724
-    dkappa: 0
+    dkappa: -0.00086098854755794682
     ddkappa: 0
   }
   v: 4.797139013278402
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_follow_02_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_follow_02_0.pb.txt
index 6ff83657248d64492157f4255ccb2db2de2f27d8..a9d1863fdb9f4db10198895484b42029b356c20e 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_follow_02_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_follow_02_0.pb.txt
@@ -27,7 +27,7 @@ trajectory_point {
     theta: -1.7838121780883047
     kappa: -0.01462662224286493
     s: 0.1215222021263741
-    dkappa: 0
+    dkappa: 0.00025262891824026043
     ddkappa: 0
   }
   v: 6.0911083996974069
@@ -41,7 +41,7 @@ trajectory_point {
     theta: -1.7853568190793254
     kappa: -0.014118929276916914
     s: 0.2436443045609667
-    dkappa: 0
+    dkappa: 0.00050650495152012188
     ddkappa: 0
   }
   v: 6.1211011109906295
@@ -55,7 +55,7 @@ trajectory_point {
     theta: -1.7869090926703906
     kappa: -0.013608742737018863
     s: 0.36636621931083546
-    dkappa: 0
+    dkappa: 0.00076162791691365445
     ddkappa: 0
   }
   v: 6.1510897330026921
@@ -69,7 +69,7 @@ trajectory_point {
     theta: -1.7884689910449727
     kappa: -0.01309606293820614
     s: 0.48968787059635793
-    dkappa: 0
+    dkappa: 0.0010179976568848383
     ddkappa: 0
   }
   v: 6.1810748613006972
@@ -83,7 +83,7 @@ trajectory_point {
     theta: -1.790036506314558
     kappa: -0.012580890147251
     s: 0.61360919424727589
-    dkappa: 0
+    dkappa: 0.0012756140380320179
     ddkappa: 0
   }
   v: 6.2110570612539791
@@ -97,7 +97,7 @@ trajectory_point {
     theta: -1.7916116305085392
     kappa: -0.01206322458517339
     s: 0.73813013709874009
-    dkappa: 0
+    dkappa: 0.0015344769498323581
     ddkappa: 0
   }
   v: 6.2410368680341026
@@ -111,7 +111,7 @@ trajectory_point {
     theta: -1.7931943555641041
     kappa: -0.011543066429751732
     s: 0.86325065638735488
-    dkappa: 0
+    dkappa: 0.001794586303386298
     ddkappa: 0
   }
   v: 6.271014786614864
@@ -125,7 +125,7 @@ trajectory_point {
     theta: -1.7947846733161157
     kappa: -0.011020415818033731
     s: 0.9889707191472229
-    dkappa: 0
+    dkappa: 0.0020559420301620073
     ddkappa: 0
   }
   v: 6.3009912917722914
@@ -139,7 +139,7 @@ trajectory_point {
     theta: -1.796382575486986
     kappa: -0.01049527284884718
     s: 1.1152903016059899
-    dkappa: 0
+    dkappa: 0.0023185440807398394
     ddkappa: 0
   }
   v: 6.3309668280846436
@@ -153,7 +153,7 @@ trajectory_point {
     theta: -1.7979880536765485
     kappa: -0.0099676375853107388
     s: 1.2422093885808887
-    dkappa: 0
+    dkappa: 0.0025823924235567897
     ddkappa: 0
   }
   v: 6.3609418099324122
@@ -167,7 +167,7 @@ trajectory_point {
     theta: -1.7996010993519236
     kappa: -0.00943751005734475
     s: 1.3697279728747844
-    dkappa: 0
+    dkappa: 0.0028474870436509449
     ddkappa: 0
   }
   v: 6.3909166214983193
@@ -181,7 +181,7 @@ trajectory_point {
     theta: -1.8012217038373843
     kappa: -0.0089048902641820239
     s: 1.4978460546722192
-    dkappa: 0
+    dkappa: 0.0031138279414059458
     ddkappa: 0
   }
   v: 6.4208916167673191
@@ -195,7 +195,7 @@ trajectory_point {
     theta: -1.8028498583042196
     kappa: -0.0083697781768786415
     s: 1.6265636409354562
-    dkappa: 0
+    dkappa: 0.0033814151312954351
     ddkappa: 0
   }
   v: 6.4508671195265954
@@ -209,7 +209,7 @@ trajectory_point {
     theta: -1.8044855537605975
     kappa: -0.0078321737408247553
     s: 1.7558807448005249
-    dkappa: 0
+    dkappa: 0.0036502486406275168
     ddkappa: 0
   }
   v: 6.480843423365565
@@ -223,7 +223,7 @@ trajectory_point {
     theta: -1.8061287810414317
     kappa: -0.0072920768782553788
     s: 1.8857973849732652
-    dkappa: 0
+    dkappa: 0.0039203285082892092
     ddkappa: 0
   }
   v: 6.5108207916758776
@@ -237,7 +237,7 @@ trajectory_point {
     theta: -1.8076238603615573
     kappa: -0.0067758044193300072
     s: 2.0163135851253724
-    dkappa: 0
+    dkappa: 0.0041440666836795358
     ddkappa: 0
   }
   v: 6.5407994576514108
@@ -251,7 +251,7 @@ trajectory_point {
     theta: -1.808048610936638
     kappa: -0.00643926136884248
     s: 2.1474293732904415
-    dkappa: 0
+    dkappa: 0.0040395447460310529
     ddkappa: 0
   }
   v: 6.5707796242882761
@@ -265,7 +265,7 @@ trajectory_point {
     theta: -1.8084753049846292
     kappa: -0.0061011792368990576
     s: 2.2791447812600123
-    dkappa: 0
+    dkappa: 0.0039345448077947225
     ddkappa: 0
   }
   v: 6.6007614643848163
@@ -279,7 +279,7 @@ trajectory_point {
     theta: -1.8089039424759925
     kappa: -0.0057615579338028827
     s: 2.4114598439796131
-    dkappa: 0
+    dkappa: 0.003829066841112921
     ddkappa: 0
   }
   v: 6.6307451205416044
@@ -293,7 +293,7 @@ trajectory_point {
     theta: -1.8093345233852394
     kappa: -0.0054203973633117561
     s: 2.5443745989448074
-    dkappa: 0
+    dkappa: 0.0037231108160952057
     ddkappa: 0
   }
   v: 6.6607307051614457
@@ -307,7 +307,7 @@ trajectory_point {
     theta: -1.8097670476889252
     kappa: -0.0050776974241883649
     s: 2.6778890855972359
-    dkappa: 0
+    dkappa: 0.0036166767012997744
     ddkappa: 0
   }
   v: 6.6907183004493778
@@ -321,7 +321,7 @@ trajectory_point {
     theta: -1.8102015153636437
     kappa: -0.0047334580117504872
     s: 2.8120033447206625
-    dkappa: 0
+    dkappa: 0.0035097644642149203
     ddkappa: 0
   }
   v: 6.7207079584126674
@@ -335,7 +335,7 @@ trajectory_point {
     theta: -1.8106379263840215
     kappa: -0.0043876790194211958
     s: 2.9467174178370192
-    dkappa: 0
+    dkappa: 0.0034023740717404891
     ddkappa: 0
   }
   v: 6.7506997008608147
@@ -349,7 +349,7 @@ trajectory_point {
     theta: -1.8110762807207121
     kappa: -0.0040403603402790755
     s: 3.0820313466024496
-    dkappa: 0
+    dkappa: 0.0032945054906693381
     ddkappa: 0
   }
   v: 6.7806935194055509
@@ -363,7 +363,7 @@ trajectory_point {
     theta: -1.81151657833839
     kappa: -0.0036915018686084263
     s: 3.2179451722033554
-    dkappa: 0
+    dkappa: 0.0031861586881687881
     ddkappa: 0
   }
   v: 6.8106893754608384
@@ -377,7 +377,7 @@ trajectory_point {
     theta: -1.8119588191937444
     kappa: -0.0033411035014494794
     s: 3.3544589347524392
-    dkappa: 0
+    dkappa: 0.0030773336322620836
     ddkappa: 0
   }
   v: 6.84068720024287
@@ -391,7 +391,7 @@ trajectory_point {
     theta: -1.8124030032334719
     kappa: -0.0029891651401486056
     s: 3.4915726726847489
-    dkappa: 0
+    dkappa: 0.0029680302923098516
     ddkappa: 0
   }
   v: 6.8706868947700732
@@ -405,7 +405,7 @@ trajectory_point {
     theta: -1.8128491303922714
     kappa: -0.0026356866919085226
     s: 3.6292864221537244
-    dkappa: 0
+    dkappa: 0.0028582486394915533
     ddkappa: 0
   }
   v: 6.9006883298631019
@@ -419,7 +419,7 @@ trajectory_point {
     theta: -1.8132972005908352
     kappa: -0.0022806680713385028
     s: 3.7676002164272417
-    dkappa: 0
+    dkappa: 0.0027479886472869439
     ddkappa: 0
   }
   v: 6.9306913461448465
@@ -433,7 +433,7 @@ trajectory_point {
     theta: -1.8137472137338444
     kappa: -0.0019241092020045917
     s: 3.9065140852836548
-    dkappa: 0
+    dkappa: 0.0026372502919575296
     ddkappa: 0
   }
   v: 6.9606957540404251
@@ -447,7 +447,7 @@ trajectory_point {
     theta: -1.8140365342903748
     kappa: -0.0016298710206515236
     s: 4.0460280544078442
-    dkappa: 0
+    dkappa: 0.0025347074356790529
     ddkappa: 0
   }
   v: 6.9907013337771895
@@ -461,7 +461,7 @@ trajectory_point {
     theta: -1.8140444191239604
     kappa: -0.0014453652369709729
     s: 4.1861421447872589
-    dkappa: 0
+    dkappa: 0.0024467997362672567
     ddkappa: 0
   }
   v: 7.0207078353847212
@@ -475,7 +475,7 @@ trajectory_point {
     theta: -1.8140523377298861
     kappa: -0.0012600691777630432
     s: 4.3268563721079607
-    dkappa: 0
+    dkappa: 0.0023585155104283344
     ddkappa: 0
   }
   v: 7.0507149786948364
@@ -489,7 +489,7 @@ trajectory_point {
     theta: -1.8140602901087013
     kappa: -0.0010739828301482297
     s: 4.4681707461506734
-    dkappa: 0
+    dkappa: 0.0022698547520258522
     ddkappa: 0
   }
   v: 7.0807224533415773
@@ -503,7 +503,7 @@ trajectory_point {
     theta: -1.8140682762605891
     kappa: -0.00088710618981863816
     s: 4.6100852701868211
-    dkappa: 0
+    dkappa: 0.0021808174590073146
     ddkappa: 0
   }
   v: 7.1107299187612227
@@ -517,7 +517,7 @@ trajectory_point {
     theta: -1.8140762961853325
     kappa: -0.00069943926183328416
     s: 4.7525999403745764
-    dkappa: 0
+    dkappa: 0.0020914036337830884
     ddkappa: 0
   }
   v: 7.1407370041922809
@@ -531,7 +531,7 @@ trajectory_point {
     theta: -1.8140843498822798
     kappa: -0.00051098206141339867
     s: 4.8957147451549039
-    dkappa: 0
+    dkappa: 0.0020016132836053221
     ddkappa: 0
   }
   v: 7.17074330867549
@@ -545,7 +545,7 @@ trajectory_point {
     theta: -1.8140924373503113
     kappa: -0.00032173461473772731
     s: 5.0394296646476073
-    dkappa: 0
+    dkappa: 0.0019114464209468672
     ddkappa: 0
   }
   v: 7.2007484010538239
@@ -559,7 +559,7 @@ trajectory_point {
     theta: -1.8141005585878052
     kappa: -0.00013169695973784057
     s: 5.18374467004737
-    dkappa: 0
+    dkappa: 0.0018209030638802037
     ddkappa: 0
   }
   v: 7.2307518199724816
@@ -573,7 +573,7 @@ trajectory_point {
     theta: -1.8141087135926026
     kappa: 5.9130853106565979e-05
     s: 5.3286597230198032
-    dkappa: 0
+    dkappa: 0.001729983236456361
     ddkappa: 0
   }
   v: 7.2607530738789
@@ -587,7 +587,7 @@ trajectory_point {
     theta: -1.8141169023619754
     kappa: 0.00025074875997236658
     s: 5.4741747750974863
-    dkappa: 0
+    dkappa: 0.0016386869690838387
     ddkappa: 0
   }
   v: 7.2907516410227426
@@ -601,7 +601,7 @@ trajectory_point {
     theta: -1.8141251248925907
     kappa: 0.00044315668289770561
     s: 5.6202897670760184
-    dkappa: 0
+    dkappa: 0.0015470142989075289
     ddkappa: 0
   }
   v: 7.3207469694559064
@@ -615,7 +615,7 @@ trajectory_point {
     theta: -1.8141333811804774
     kappa: 0.00063635452898668865
     s: 5.7670046284100565
-    dkappa: 0
+    dkappa: 0.0014549652701876393
     ddkappa: 0
   }
   v: 7.3507384770325208
@@ -629,7 +629,7 @@ trajectory_point {
     theta: -1.8141416712209921
     kappa: 0.00083034218961408059
     s: 5.9143192766093629
-    dkappa: 0
+    dkappa: 0.0013625399346786154
     ddkappa: 0
   }
   v: 7.3807255514089452
@@ -643,7 +643,7 @@ trajectory_point {
     theta: -1.8141678302235449
     kappa: 0.00095462081525655767
     s: 6.0622336166348481
-    dkappa: 0
+    dkappa: 0.0012814023621518988
     ddkappa: 0
   }
   v: 7.41070755004377
@@ -657,7 +657,7 @@ trajectory_point {
     theta: -1.8142114808506715
     kappa: 0.0010106817752980077
     s: 6.2107475402946193
-    dkappa: 0
+    dkappa: 0.0012113058499314522
     ddkappa: 0
   }
   v: 7.4406838001978182
@@ -671,7 +671,7 @@ trajectory_point {
     theta: -1.8142553076700836
     kappa: 0.0010669690198303414
     s: 6.3598609256400209
-    dkappa: 0
+    dkappa: 0.0011409264001093519
     ddkappa: 0
   }
   v: 7.4706535989341436
@@ -685,7 +685,7 @@ trajectory_point {
     theta: -1.8142993106415581
     kappa: 0.0011234824973995663
     s: 6.5095736363616794
-    dkappa: 0
+    dkappa: 0.0010702640770217317
     ddkappa: 0
   }
   v: 7.5006162131180325
@@ -699,7 +699,7 @@ trajectory_point {
     theta: -1.8143434897204711
     kappa: 0.0011802221509028342
     s: 6.6598855211855525
-    dkappa: 0
+    dkappa: 0.00099931895206784157
     ddkappa: 0
   }
   v: 7.5305708794170014
@@ -713,7 +713,7 @@ trajectory_point {
     theta: -1.8143878448576172
     kappa: 0.0012371879173604583
     s: 6.8107964132689682
-    dkappa: 0
+    dkappa: 0.00092809110399510856
     ddkappa: 0
   }
   v: 7.5605168043007982
@@ -727,7 +727,7 @@ trajectory_point {
     theta: -1.8146122593517127
     kappa: 0.0015254057995707027
     s: 7.5743289771834066
-    dkappa: 0
+    dkappa: 0.00056771432329845222
     ddkappa: 0
   }
   v: 7.7100854200895963
@@ -741,7 +741,7 @@ trajectory_point {
     theta: -1.8141570436064476
     kappa: 0.0015750774199442083
     s: 8.3528016873030957
-    dkappa: 0
+    dkappa: 0.00025496969998022495
     ddkappa: 0
   }
   v: 7.8592991072444258
@@ -755,7 +755,7 @@ trajectory_point {
     theta: -1.8129446954813988
     kappa: 0.001358524741178733
     s: 9.14617254985613
-    dkappa: 0
+    dkappa: -3.9281943699017048e-06
     ddkappa: 0
   }
   v: 8.0080247019941968
@@ -769,7 +769,7 @@ trajectory_point {
     theta: -1.8117096683046312
     kappa: 0.0011379209268694939
     s: 9.9543853110132261
-    dkappa: 0
+    dkappa: -0.00026766939275111473
     ddkappa: 0
   }
   v: 8.1561101669624687
@@ -783,7 +783,7 @@ trajectory_point {
     theta: -1.8113102639598531
     kappa: 0.0005977222650962083
     s: 10.777367569527188
-    dkappa: 0
+    dkappa: -0.00043111877938422814
     ddkappa: 0
   }
   v: 8.3033845911674486
@@ -797,7 +797,7 @@ trajectory_point {
     theta: -1.8109212643301318
     kappa: 4.1442951887563361e-05
     s: 11.615028889372375
-    dkappa: 0
+    dkappa: -0.00059533659839970492
     ddkappa: 0
   }
   v: 8.44965819002199
@@ -811,7 +811,7 @@ trajectory_point {
     theta: -1.8112996949805269
     kappa: -0.00060383773073308827
     s: 12.467258912384164
-    dkappa: 0
+    dkappa: -0.00070380890573683958
     ddkappa: 0
   }
   v: 8.594722305333601
@@ -825,7 +825,7 @@ trajectory_point {
     theta: -1.8122350339305977
     kappa: -0.0013164549367472894
     s: 13.333925470898411
-    dkappa: 0
+    dkappa: -0.00077244962645045372
     ddkappa: 0
   }
   v: 8.738349405304433
@@ -839,7 +839,7 @@ trajectory_point {
     theta: -1.8135920595290638
     kappa: -0.0020267740778915173
     s: 14.214872700390925
-    dkappa: 0
+    dkappa: -0.00081522484719234679
     ddkappa: 0
   }
   v: 8.8802930845312869
@@ -853,7 +853,7 @@ trajectory_point {
     theta: -1.8160001621557669
     kappa: -0.0027128886232134853
     s: 15.109919152116925
-    dkappa: 0
+    dkappa: -0.00079028559265761282
     ddkappa: 0
   }
   v: 9.0202880640056158
@@ -867,7 +867,7 @@ trajectory_point {
     theta: -1.818519755378843
     kappa: -0.0033947062421456837
     s: 16.018856363672
-    dkappa: 0
+    dkappa: -0.00075909315365388027
     ddkappa: 0
   }
   v: 9.1580687707224051
@@ -881,7 +881,7 @@ trajectory_point {
     theta: -1.8221859936680971
     kappa: -0.0038632142868502348
     s: 16.941452429166635
-    dkappa: 0
+    dkappa: -0.00063968396985192993
     ddkappa: 0
   }
   v: 9.2934355864724427
@@ -895,7 +895,7 @@ trajectory_point {
     theta: -1.8259055178747492
     kappa: -0.004338531524729251
     s: 17.877457303545043
-    dkappa: 0
+    dkappa: -0.000518539319159917
     ddkappa: 0
   }
   v: 9.4262175014554472
@@ -909,7 +909,7 @@ trajectory_point {
     theta: -1.8300519445147243
     kappa: -0.0044250267425528953
     s: 18.826604473585984
-    dkappa: 0
+    dkappa: -0.00031003716334740552
     ddkappa: 0
   }
   v: 9.5562561621625779
@@ -923,7 +923,7 @@ trajectory_point {
     theta: -1.8342908961471682
     kappa: -0.00447435216112244
     s: 19.788612223531906
-    dkappa: 0
+    dkappa: -9.0405817948495583e-05
     ddkappa: 0
   }
   v: 9.6834058713764311
@@ -937,7 +937,7 @@ trajectory_point {
     theta: -1.8383358502547984
     kappa: -0.0039807407232296255
     s: 20.763184900718088
-    dkappa: 0
+    dkappa: 0.00022055112708517098
     ddkappa: 0
   }
   v: 9.8075335881710473
@@ -951,7 +951,7 @@ trajectory_point {
     theta: -1.8423778174923868
     kappa: -0.0033634570636781602
     s: 21.750014181201777
-    dkappa: 0
+    dkappa: 0.00055453376639419166
     ddkappa: 0
   }
   v: 9.9285189279119077
@@ -965,7 +965,7 @@ trajectory_point {
     theta: -1.8445815402645298
     kappa: -0.0019760501189164518
     s: 22.748780335391363
-    dkappa: 0
+    dkappa: 0.00096531503240648376
     ddkappa: 0
   }
   v: 10.046254162255932
@@ -979,7 +979,7 @@ trajectory_point {
     theta: -1.8463033797716115
     kappa: -0.00036741849703229911
     s: 23.759153493675473
-    dkappa: 0
+    dkappa: 0.001400437121600615
     ddkappa: 0
   }
   v: 10.160644219151486
@@ -993,7 +993,7 @@ trajectory_point {
     theta: -1.8464676507222484
     kappa: 0.00071709252014629746
     s: 24.780794912052158
-    dkappa: 0
+    dkappa: 0.001321129298080085
     ddkappa: 0
   }
   v: 10.271606682838367
@@ -1007,7 +1007,7 @@ trajectory_point {
     theta: -1.8454034311992626
     kappa: 0.0013902637626382131
     s: 25.81335823775802
-    dkappa: 0
+    dkappa: 0.00083581224906072
     ddkappa: 0
   }
   v: 10.379071793847823
@@ -1021,7 +1021,7 @@ trajectory_point {
     theta: -1.8436733836751105
     kappa: 0.0018752918258406587
     s: 26.856490774897356
-    dkappa: 0
+    dkappa: 0.00055444778342448881
     ddkappa: 0
   }
   v: 10.482982449002535
@@ -1035,7 +1035,7 @@ trajectory_point {
     theta: -1.841529541999303
     kappa: 0.0022468881547370763
     s: 27.909834750071298
-    dkappa: 0
+    dkappa: 0.00039692324704695232
     ddkappa: 0
   }
   v: 10.58329420141663
@@ -1049,7 +1049,7 @@ trajectory_point {
     theta: -1.839015932326443
     kappa: 0.0024351830556510164
     s: 28.973028578006968
-    dkappa: 0
+    dkappa: 0.00025941979633096389
     ddkappa: 0
   }
   v: 10.679975260495672
@@ -1063,7 +1063,7 @@ trajectory_point {
     theta: -1.8363442694517582
     kappa: 0.0025527265428661964
     s: 30.045708127186618
-    dkappa: 0
+    dkappa: 0.00012902491197320875
     ddkappa: 0
   }
   v: 10.77300649193667
@@ -1077,7 +1077,7 @@ trajectory_point {
     theta: -1.8336322976506567
     kappa: 0.0024980631394223942
     s: 31.127507985476772
-    dkappa: 0
+    dkappa: 2.300804690738584e-05
     ddkappa: 0
   }
   v: 10.862381417728066
@@ -1091,7 +1091,7 @@ trajectory_point {
     theta: -1.8308988531472303
     kappa: 0.0024111081553186554
     s: 32.218062725757363
-    dkappa: 0
+    dkappa: -7.9088387754348865e-05
     ddkappa: 0
   }
   v: 10.948106216149753
@@ -1105,7 +1105,7 @@ trajectory_point {
     theta: -1.8284761528321638
     kappa: 0.0021789234256807035
     s: 33.317008171550896
-    dkappa: 0
+    dkappa: -0.00015212150203985773
     ddkappa: 0
   }
   v: 11.030199721773059
@@ -1119,7 +1119,7 @@ trajectory_point {
     theta: -1.8261480301606459
     kappa: 0.0019281029461022392
     s: 34.423982662651575
-    dkappa: 0
+    dkappa: -0.00021974594880378427
     ddkappa: 0
   }
   v: 11.10869342546075
@@ -1133,7 +1133,7 @@ trajectory_point {
     theta: -1.824267946229055
     kappa: 0.0016055136885373654
     s: 35.538628320754455
-    dkappa: 0
+    dkappa: -0.0002632725917473743
     ddkappa: 0
   }
   v: 11.18363147436704
@@ -1147,7 +1147,7 @@ trajectory_point {
     theta: -1.8226556743286721
     kappa: 0.0012703353347940482
     s: 36.66059327305868
-    dkappa: 0
+    dkappa: -0.00029464320615363879
     ddkappa: 0
   }
   v: 11.255108719616073
@@ -1161,7 +1161,7 @@ trajectory_point {
     theta: -1.8214502186756625
     kappa: 0.00091749040576603771
     s: 37.789543021554991
-    dkappa: 0
+    dkappa: -0.00030769643811511525
     ddkappa: 0
   }
   v: 11.323374007919563
@@ -1175,7 +1175,7 @@ trajectory_point {
     theta: -1.8206734916413156
     kappa: 0.00058570596371624456
     s: 38.925170021829331
-    dkappa: 0
+    dkappa: -0.00030097264079374591
     ddkappa: 0
   }
   v: 11.388697048560044
@@ -1189,7 +1189,7 @@ trajectory_point {
     theta: -1.8201487984630786
     kappa: 0.00026567022534139677
     s: 40.067192705497746
-    dkappa: 0
+    dkappa: -0.00028253103783665583
     ddkappa: 0
   }
   v: 11.451327652857898
@@ -1203,7 +1203,7 @@ trajectory_point {
     theta: -1.8201096995868282
     kappa: 3.2835979735757793e-05
     s: 41.215353490410266
-    dkappa: 0
+    dkappa: -0.00023578657627439806
     ddkappa: 0
   }
   v: 11.511495734171376
@@ -1217,7 +1217,7 @@ trajectory_point {
     theta: -1.8201431310168308
     kappa: -0.00016539268696542331
     s: 42.369416790854586
-    dkappa: 0
+    dkappa: -0.0001802625859729728
     ddkappa: 0
   }
   v: 11.569411307896571
@@ -1231,7 +1231,7 @@ trajectory_point {
     theta: -1.8201978581411844
     kappa: -0.00021945729098454635
     s: 43.529167027759939
-    dkappa: 0
+    dkappa: -9.60686887855332e-05
     ddkappa: 0
   }
   v: 11.625264491467441
@@ -1245,7 +1245,7 @@ trajectory_point {
     theta: -1.8203232011922434
     kappa: -0.00017554185825972179
     s: 44.69440663890083
-    dkappa: 0
+    dkappa: 2.4918551314666277e-06
     ddkappa: 0
   }
   v: 11.679225504355793
@@ -1259,7 +1259,7 @@ trajectory_point {
     theta: -1.8205485735655005
     kappa: 7.4425367544992264e-06
     s: 45.864954089100841
-    dkappa: 0
+    dkappa: 0.00012124708499808851
     ddkappa: 0
   }
   v: 11.731444668071301
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_follow_03_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_follow_03_0.pb.txt
index 8be20df2fe121f403e1bf95f4b72552edb154c3e..c2d457f2d0207364b9858dd019a2dd470db34077 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_follow_03_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_follow_03_0.pb.txt
@@ -27,7 +27,7 @@ trajectory_point {
     theta: -1.8273269156669139
     kappa: 0.0013990457637293729
     s: 0.091622363784443217
-    dkappa: 0
+    dkappa: 7.4404394854732774e-05
     ddkappa: 0
   }
   v: 4.5624035034816739
@@ -41,7 +41,7 @@ trajectory_point {
     theta: -1.8270114658484389
     kappa: 0.0015466691352516754
     s: 0.18250275723928269
-    dkappa: 0
+    dkappa: 0.00014820625282770366
     ddkappa: 0
   }
   v: 4.5257996737729238
@@ -55,7 +55,7 @@ trajectory_point {
     theta: -1.8266985226417316
     kappa: 0.0016931192079717744
     s: 0.27266084056338352
-    dkappa: 0
+    dkappa: 0.00022142153951005067
     ddkappa: 0
   }
   v: 4.4901693247074679
@@ -69,7 +69,7 @@ trajectory_point {
     theta: -1.8263880191976274
     kappa: 0.0018384273002518613
     s: 0.36211589407156952
-    dkappa: 0
+    dkappa: 0.00029406591199789987
     ddkappa: 0
   }
   v: 4.455493558680006
@@ -83,7 +83,7 @@ trajectory_point {
     theta: -1.8260798899687931
     kappa: 0.001982624119658296
     s: 0.45088682205849351
-    dkappa: 0
+    dkappa: 0.00036615472203012492
     ddkappa: 0
   }
   v: 4.4217536712787817
@@ -97,7 +97,7 @@ trajectory_point {
     theta: -1.8257740706965142
     kappa: 0.0021257397692379665
     s: 0.538992156662509
-    dkappa: 0
+    dkappa: 0.00043770301912610733
     ddkappa: 0
   }
   v: 4.3889311512855862
@@ -111,7 +111,7 @@ trajectory_point {
     theta: -1.825470498397469
     kappa: 0.0022678037537946396
     s: 0.62645006172954087
-    dkappa: 0
+    dkappa: 0.00050872555372349633
     ddkappa: 0
   }
   v: 4.3570076806757561
@@ -125,7 +125,7 @@ trajectory_point {
     theta: -1.8251691113504946
     kappa: 0.00240884498616532
     s: 0.71327833667695573
-    dkappa: 0
+    dkappa: 0.000579236780315968
     ddkappa: 0
   }
   v: 4.3259651346181727
@@ -139,7 +139,7 @@ trajectory_point {
     theta: -1.8248698490833442
     kappa: 0.0025488917934966032
     s: 0.79949442035743368
-    dkappa: 0
+    dkappa: 0.00064925086059098658
     ddkappa: 0
   }
   v: 4.2957855814752657
@@ -153,7 +153,7 @@ trajectory_point {
     theta: -1.8245726523594332
     kappa: 0.002687971923521033
     s: 0.88511539492283875
-    dkappa: 0
+    dkappa: 0.00071878166656756323
     ddkappa: 0
   }
   v: 4.26645128280301
@@ -167,7 +167,7 @@ trajectory_point {
     theta: -1.8242774631645793
     kappa: 0.0028261125508334552
     s: 0.97015798968808986
-    dkappa: 0
+    dkappa: 0.0007878427837340157
     ddkappa: 0
   }
   v: 4.2379446933509275
@@ -181,7 +181,7 @@ trajectory_point {
     theta: -1.823984224693733
     kappa: 0.002963340283167375
     s: 1.0546385849950322
-    dkappa: 0
+    dkappa: 0.00085644751418572978
     ddkappa: 0
   }
   v: 4.2102484610620836
@@ -195,7 +195,7 @@ trajectory_point {
     theta: -1.8236928813377016
     kappa: 0.00309968116767131
     s: 1.1385732160763069
-    dkappa: 0
+    dkappa: 0.0009246088797629173
     ddkappa: 0
   }
   v: 4.1833454270730916
@@ -209,7 +209,7 @@ trajectory_point {
     theta: -1.8234033786698647
     kappa: 0.0032351606971851472
     s: 1.2219775769192238
-    dkappa: 0
+    dkappa: 0.000992339625188377
     ddkappa: 0
   }
   v: 4.1572186257141119
@@ -223,7 +223,7 @@ trajectory_point {
     theta: -1.823115663432884
     kappa: 0.0033698038165165
     s: 1.304867024129631
-    dkappa: 0
+    dkappa: 0.0010596522212052552
     ddkappa: 0
   }
   v: 4.1318512845088486
@@ -237,7 +237,7 @@ trajectory_point {
     theta: -1.8228296835254065
     kappa: 0.0035036349287170592
     s: 1.3872565807957857
-    dkappa: 0
+    dkappa: 0.0011265588677148034
     ddkappa: 0
   }
   v: 4.1072268241745551
@@ -251,7 +251,7 @@ trajectory_point {
     theta: -1.8225453879887603
     kappa: 0.0036366779013589524
     s: 1.4691609403522266
-    dkappa: 0
+    dkappa: 0.0011930714969141402
     ddkappa: 0
   }
   v: 4.0833288586220267
@@ -265,7 +265,7 @@ trajectory_point {
     theta: -1.822262726993646
     kappa: 0.0037689560728110983
     s: 1.5505944704436427
-    dkappa: 0
+    dkappa: 0.0012592017764340108
     ddkappa: 0
   }
   v: 4.0601411949556088
@@ -279,7 +279,7 @@ trajectory_point {
     theta: -1.8219816518268215
     kappa: 0.0039004922585155616
     s: 1.6315712167887459
-    dkappa: 0
+    dkappa: 0.001324961112476546
     ddkappa: 0
   }
   v: 4.0376478334731907
@@ -293,7 +293,7 @@ trajectory_point {
     theta: -1.8217021148777821
     kappa: 0.0040313087572639086
     s: 1.7121049070441414
-    dkappa: 0
+    dkappa: 0.001390360652953023
     ddkappa: 0
   }
   v: 4.0158329676662081
@@ -307,7 +307,7 @@ trajectory_point {
     theta: -1.8214240696254351
     kappa: 0.0041614273574735634
     s: 1.7922089546681987
-    dkappa: 0
+    dkappa: 0.0014554112906216251
     ddkappa: 0
   }
   v: 3.9946809842196433
@@ -321,7 +321,7 @@ trajectory_point {
     theta: -1.82114747062477
     kappa: 0.0042908693434641626
     s: 1.8718964627849215
-    dkappa: 0
+    dkappa: 0.0015201236662252011
     ddkappa: 0
   }
   v: 3.9741764630120242
@@ -335,7 +335,7 @@ trajectory_point {
     theta: -1.820872273493523
     kappa: 0.0044196555017339129
     s: 1.9511802280478214
-    dkappa: 0
+    dkappa: 0.0015845081716290266
     ddkappa: 0
   }
   v: 3.9543041771154255
@@ -349,7 +349,7 @@ trajectory_point {
     theta: -1.8205503553550959
     kappa: 0.0044998568253865716
     s: 2.0300727445037845
-    dkappa: 0
+    dkappa: 0.0016003518521562293
     ddkappa: 0
   }
   v: 3.9350490927954671
@@ -363,7 +363,7 @@ trajectory_point {
     theta: -1.8201511846184704
     kappa: 0.0045010878541920244
     s: 2.1085862074569466
-    dkappa: 0
+    dkappa: 0.0015370857219729457
     ddkappa: 0
   }
   v: 3.9163963695113155
@@ -377,7 +377,7 @@ trajectory_point {
     theta: -1.8197538790949719
     kappa: 0.0045023131263285249
     s: 2.1867325173325622
-    dkappa: 0
+    dkappa: 0.0014741154436541391
     ddkappa: 0
   }
   v: 3.8983313599156828
@@ -391,7 +391,7 @@ trajectory_point {
     theta: -1.8193583799053132
     kappa: 0.004503532823822415
     s: 2.264523283540874
-    dkappa: 0
+    dkappa: 0.001411431662339737
     ddkappa: 0
   }
   v: 3.8808396098548288
@@ -405,7 +405,7 @@ trajectory_point {
     theta: -1.8189646296258697
     kappa: 0.0045047471241977353
     s: 2.3419698283409875
-    dkappa: 0
+    dkappa: 0.0013490252545559857
     ddkappa: 0
   }
   v: 3.8639068583685576
@@ -419,7 +419,7 @@ trajectory_point {
     theta: -1.8185725722692829
     kappa: 0.0045059562005368052
     s: 2.4190831907047379
-    dkappa: 0
+    dkappa: 0.0012868873251019482
     ddkappa: 0
   }
   v: 3.84751903769022
@@ -433,7 +433,7 @@ trajectory_point {
     theta: -1.8181821532650526
     kappa: 0.0045071602215408062
     s: 2.4958741301805634
-    dkappa: 0
+    dkappa: 0.0012250092039359963
     ddkappa: 0
   }
   v: 3.8316622732467129
@@ -447,7 +447,7 @@ trajectory_point {
     theta: -1.8177933194401152
     kappa: 0.0045083593515903679
     s: 2.5723531307573761
-    dkappa: 0
+    dkappa: 0.0011633824430623038
     ddkappa: 0
   }
   v: 3.8163228836584797
@@ -461,7 +461,7 @@ trajectory_point {
     theta: -1.8174060189994115
     kappa: 0.004509553750806145
     s: 2.6485304047284322
-    dkappa: 0
+    dkappa: 0.0011019988134173396
     ddkappa: 0
   }
   v: 3.8014873807395091
@@ -475,7 +475,7 @@ trajectory_point {
     theta: -1.8170202015064436
     kappa: 0.0045107435751094039
     s: 2.7244158965552034
-    dkappa: 0
+    dkappa: 0.0010408503017563641
     ddkappa: 0
   }
   v: 3.7871424694973372
@@ -489,7 +489,7 @@ trajectory_point {
     theta: -1.816635817863822
     kappa: 0.0045119289762826026
     s: 2.800019286731247
-    dkappa: 0
+    dkappa: 0.00097992910753992015
     ddkappa: 0
   }
   v: 3.7732750481330446
@@ -503,7 +503,7 @@ trajectory_point {
     theta: -1.8162528202938022
     kappa: 0.0045131101020299746
     s: 2.8753499956460766
-    dkappa: 0
+    dkappa: 0.00091922763982032942
     ddkappa: 0
   }
   v: 3.7598722080412594
@@ -517,7 +517,7 @@ trajectory_point {
     theta: -1.8158711623188108
     kappa: 0.00451428709603811
     s: 2.9504171874490366
-    dkappa: 0
+    dkappa: 0.00085873851412818182
     ddkappa: 0
   }
   v: 3.7469212338101547
@@ -531,7 +531,7 @@ trajectory_point {
     theta: -1.8154907987419659
     kappa: 0.0045154600980365405
     s: 3.0252297739131668
-    dkappa: 0
+    dkappa: 0.00079845454935883521
     ddkappa: 0
   }
   v: 3.7344096032214504
@@ -545,7 +545,7 @@ trajectory_point {
     theta: -1.8151116856275848
     kappa: 0.0045166292438583184
     s: 3.0997964182990807
-    dkappa: 0
+    dkappa: 0.00073836876465890248
     ddkappa: 0
   }
   v: 3.7223249872504125
@@ -559,7 +559,7 @@ trajectory_point {
     theta: -1.8147337802816861
     kappa: 0.0045177946655006011
     s: 3.1741255392188283
-    dkappa: 0
+    dkappa: 0.0006784743763127521
     ddkappa: 0
   }
   v: 3.710655250065853
@@ -573,7 +573,7 @@ trajectory_point {
     theta: -1.814357041232483
     kappa: 0.0045189564911852324
     s: 3.2482253144997748
-    dkappa: 0
+    dkappa: 0.00061876479462899536
     ddkappa: 0
   }
   v: 3.6993884490301294
@@ -587,7 +587,7 @@ trajectory_point {
     theta: -1.8139814282108695
     kappa: 0.0045201148454193287
     s: 3.3221036850484666
-    dkappa: 0
+    dkappa: 0.00055923362082698511
     ddkappa: 0
   }
   v: 3.6885128346991465
@@ -601,7 +601,7 @@ trajectory_point {
     theta: -1.8136069021308989
     kappa: 0.0045212698490558548
     s: 3.3957683587145038
-    dkappa: 0
+    dkappa: 0.00049987464392330593
     ddkappa: 0
   }
   v: 3.6780168508223534
@@ -615,7 +615,7 @@ trajectory_point {
     theta: -1.8132334250702555
     kappa: 0.0045224216193542107
     s: 3.4692268141544105
-    dkappa: 0
+    dkappa: 0.00044068183761827076
     ddkappa: 0
   }
   v: 3.6678891343427478
@@ -629,7 +629,7 @@ trajectory_point {
     theta: -1.8128609602507202
     kappa: 0.0045235702700408154
     s: 3.5424863046955064
-    dkappa: 0
+    dkappa: 0.00038164935718241268
     ddkappa: 0
   }
   v: 3.6581185153968714
@@ -643,7 +643,7 @@ trajectory_point {
     theta: -1.8124894720186289
     kappa: 0.0045247159113696849
     s: 3.6155538621997789
-    dkappa: 0
+    dkappa: 0.00032277153634297791
     ddkappa: 0
   }
   v: 3.6486940173148135
@@ -657,7 +657,7 @@ trajectory_point {
     theta: -1.8121189258253261
     kappa: 0.0045258586501830183
     s: 3.6884363009277505
-    dkappa: 0
+    dkappa: 0.00026404288417042294
     ddkappa: 0
   }
   v: 3.6396048566202075
@@ -671,7 +671,7 @@ trajectory_point {
     theta: -1.8117492882076114
     kappa: 0.0045269985899717771
     s: 3.7611402214023526
-    dkappa: 0
+    dkappa: 0.00020545808196490475
     ddkappa: 0
   }
   v: 3.6308404430302348
@@ -685,7 +685,7 @@ trajectory_point {
     theta: -1.8113805267681822
     kappa: 0.0045281358309362712
     s: 3.8336720142727976
-    dkappa: 0
+    dkappa: 0.00014701198014277489
     ddkappa: 0
   }
   v: 3.6223903794556227
@@ -699,7 +699,7 @@ trajectory_point {
     theta: -1.8110126101560688
     kappa: 0.0045292704700467387
     s: 3.9060378641784439
-    dkappa: 0
+    dkappa: 8.8699595123076876e-05
     ddkappa: 0
   }
   v: 3.6142444620006438
@@ -713,7 +713,7 @@ trajectory_point {
     theta: -1.8106455080470669
     kappa: 0.00453040260110393
     s: 3.978243753612674
-    dkappa: 0
+    dkappa: 3.0516106214034771e-05
     ddkappa: 0
   }
   v: 3.6063926799631174
@@ -727,7 +727,7 @@ trajectory_point {
     theta: -1.8092534320647309
     kappa: 0.0040957338636311708
     s: 4.3370695176101428
-    dkappa: 0
+    dkappa: -0.00020520088174346907
     ddkappa: 0
   }
   v: 3.5712089500091633
@@ -741,7 +741,7 @@ trajectory_point {
     theta: -1.8078969742768933
     kappa: 0.0036413529206461535
     s: 4.6926841716655954
-    dkappa: 0
+    dkappa: -0.00043594461710703183
     ddkappa: 0
   }
   v: 3.5419880864533893
@@ -755,7 +755,7 @@ trajectory_point {
     theta: -1.8065506873368444
     kappa: 0.0031903803705702373
     s: 5.045631296450801
-    dkappa: 0
+    dkappa: -0.00066495750234676248
     ddkappa: 0
   }
   v: 3.5176960942625493
@@ -769,7 +769,7 @@ trajectory_point {
     theta: -1.8052128741049771
     kappa: 0.0027422460648862217
     s: 5.396357110432521
-    dkappa: 0
+    dkappa: -0.00089252907028881
     ddkappa: 0
   }
   v: 3.4974197243697125
@@ -783,7 +783,7 @@ trajectory_point {
     theta: -1.8038821625817718
     kappa: 0.0022964888299559713
     s: 5.7452225444691392
-    dkappa: 0
+    dkappa: -0.0011188935141350624
     ddkappa: 0
   }
   v: 3.4803664736742652
@@ -797,7 +797,7 @@ trajectory_point {
     theta: -1.8029138568894658
     kappa: 0.0017555328917791652
     s: 6.0925153164072929
-    dkappa: 0
+    dkappa: -0.0013265330365220345
     ddkappa: 0
   }
   v: 3.465864585041909
@@ -811,7 +811,7 @@ trajectory_point {
     theta: -1.8027972470276774
     kappa: 0.000985396428126519
     s: 6.4384620056785069
-    dkappa: 0
+    dkappa: -0.0014912453214004366
     ddkappa: 0
   }
   v: 3.4533630473046593
@@ -825,7 +825,7 @@ trajectory_point {
     theta: -1.8026810310552308
     kappa: 0.00021786139448127227
     s: 6.7832401278958221
-    dkappa: 0
+    dkappa: -0.0016554012276359716
     ddkappa: 0
   }
   v: 3.4424315952608522
@@ -839,7 +839,7 @@ trajectory_point {
     theta: -1.8025651616074121
     kappa: -0.00054738504492381353
     s: 7.12699020945043
-    dkappa: 0
+    dkappa: -0.0018190676627137992
     ddkappa: 0
   }
   v: 3.4327607096751347
@@ -853,7 +853,7 @@ trajectory_point {
     theta: -1.8024495997175374
     kappa: -0.0013106002616415291
     s: 7.469827862108299
-    dkappa: 0
+    dkappa: -0.0019822996717571203
     ddkappa: 0
   }
   v: 3.4241616172784726
@@ -867,7 +867,7 @@ trajectory_point {
     theta: -1.8023343112444326
     kappa: -0.0020720097573812078
     s: 7.8118543816442063
-    dkappa: 0
+    dkappa: -0.0021451454837533171
     ddkappa: 0
   }
   v: 3.4165064564283916
@@ -881,7 +881,7 @@ trajectory_point {
     theta: -1.8034942231443962
     kappa: -0.0029316013688129236
     s: 8.1531513135131064
-    dkappa: 0
+    dkappa: -0.0022763114232866267
     ddkappa: 0
   }
   v: 3.4095169406763728
@@ -895,7 +895,7 @@ trajectory_point {
     theta: -1.8059173076737176
     kappa: -0.003888544891005972
     s: 8.49376976600832
-    dkappa: 0
+    dkappa: -0.0023761181708453162
     ddkappa: 0
   }
   v: 3.40288815331376
@@ -909,7 +909,7 @@ trajectory_point {
     theta: -1.8083357615867319
     kappa: -0.004843643740495689
     s: 8.83373161803862
-    dkappa: 0
+    dkappa: -0.0024757325237866014
     ddkappa: 0
   }
   v: 3.396340423358176
@@ -923,7 +923,7 @@ trajectory_point {
     theta: -1.8107495221109402
     kappa: -0.0057968843592474759
     s: 9.1730320437008483
-    dkappa: 0
+    dkappa: -0.0025751530680423527
     ddkappa: 0
   }
   v: 3.3896193255535283
@@ -937,7 +937,7 @@ trajectory_point {
     theta: -1.8131583547878565
     kappa: -0.0067481852565496048
     s: 9.5116420368525567
-    dkappa: 0
+    dkappa: -0.0026743713043409778
     ddkappa: 0
   }
   v: 3.3824956803700026
@@ -951,7 +951,7 @@ trajectory_point {
     theta: -1.8155618714570023
     kappa: -0.0076974041016219848
     s: 9.8495109356846271
-    dkappa: 0
+    dkappa: -0.0027733723879482691
     ddkappa: 0
   }
   v: 3.3747655540040666
@@ -965,7 +965,7 @@ trajectory_point {
     theta: -1.8187157316779581
     kappa: -0.0086892009611749255
     s: 10.186568947293908
-    dkappa: 0
+    dkappa: -0.0028319310416774035
     ddkappa: 0
   }
   v: 3.3662502583784697
@@ -979,7 +979,7 @@ trajectory_point {
     theta: -1.8222950053717357
     kappa: -0.0097040818228878237
     s: 10.522729672255828
-    dkappa: 0
+    dkappa: -0.0028672769839736692
     ddkappa: 0
   }
   v: 3.3567963511422416
@@ -993,7 +993,7 @@ trajectory_point {
     theta: -1.8258637767117838
     kappa: -0.010715950388417814
     s: 10.857892629197039
-    dkappa: 0
+    dkappa: -0.0029025180149930777
     ddkappa: 0
   }
   v: 3.3462756356706933
@@ -1007,7 +1007,7 @@ trajectory_point {
     theta: -1.8294207611412852
     kappa: -0.011724468408902967
     s: 11.191945779368035
-    dkappa: 0
+    dkappa: -0.0029376423543137032
     ddkappa: 0
   }
   v: 3.3345851610654167
@@ -1021,7 +1021,7 @@ trajectory_point {
     theta: -1.8329645796810001
     kappa: -0.012729270365134927
     s: 11.524768051215787
-    dkappa: 0
+    dkappa: -0.0029726372717508169
     ddkappa: 0
   }
   v: 3.3216472221542857
@@ -1035,7 +1035,7 @@ trajectory_point {
     theta: -1.8364937861332034
     kappa: -0.013729971089331029
     s: 11.856231864956364
-    dkappa: 0
+    dkappa: -0.0030074893528055026
     ddkappa: 0
   }
   v: 3.3074093594914542
@@ -1049,7 +1049,7 @@ trajectory_point {
     theta: -1.841260238203271
     kappa: -0.014694090913495373
     s: 12.186205657147564
-    dkappa: 0
+    dkappa: -0.0030029523463807961
     ddkappa: 0
   }
   v: 3.2918443593573579
@@ -1063,7 +1063,7 @@ trajectory_point {
     theta: -1.8466257657107432
     kappa: -0.015637547312509128
     s: 12.514556405261544
-    dkappa: 0
+    dkappa: -0.0029789682316656227
     ddkappa: 0
   }
   v: 3.2749502537587127
@@ -1077,7 +1077,7 @@ trajectory_point {
     theta: -1.8519630346424045
     kappa: -0.016575961033701566
     s: 12.841152152257447
-    dkappa: 0
+    dkappa: -0.0029551123095870819
     ddkappa: 0
   }
   v: 3.2567503204285169
@@ -1091,7 +1091,7 @@ trajectory_point {
     theta: -1.8572696388323602
     kappa: -0.0175089632378038
     s: 13.165864531154032
-    dkappa: 0
+    dkappa: -0.002931393956607453
     ddkappa: 0
   }
   v: 3.2372930828260484
@@ -1105,7 +1105,7 @@ trajectory_point {
     theta: -1.8625432822844685
     kappa: -0.0184362026548351
     s: 13.488571289602294
-    dkappa: 0
+    dkappa: -0.0029078221025506494
     ddkappa: 0
   }
   v: 3.216652310136868
@@ -1119,7 +1119,7 @@ trajectory_point {
     theta: -1.8677818214825532
     kappa: -0.019357352838005083
     s: 13.809158814458103
-    dkappa: 0
+    dkappa: -0.0028844050461968554
     ddkappa: 0
   }
   v: 3.1949270172728159
@@ -1133,7 +1133,7 @@ trajectory_point {
     theta: -1.873811378208138
     kappa: -0.020189871124311472
     s: 14.127524656354826
-    dkappa: 0
+    dkappa: -0.0028321150409172892
     ddkappa: 0
   }
   v: 3.1722414648720143
@@ -1147,7 +1147,7 @@ trajectory_point {
     theta: -1.8805470458011737
     kappa: -0.020941957748735945
     s: 14.443579416331989
-    dkappa: 0
+    dkappa: -0.00275394413816352
     ddkappa: 0
   }
   v: 3.1487192918626374
@@ -1161,7 +1161,7 @@ trajectory_point {
     theta: -1.8872327127807271
     kappa: -0.02168835120059126
     s: 14.757241693539989
-    dkappa: 0
+    dkappa: -0.0026763649763406638
     ddkappa: 0
   }
   v: 3.124391926045778
@@ -1175,7 +1175,7 @@ trajectory_point {
     theta: -1.8938660443782409
     kappa: -0.022428859295769609
     s: 15.068430725071877
-    dkappa: 0
+    dkappa: -0.00259939753081421
     ddkappa: 0
   }
   v: 3.0992517089643452
@@ -1189,7 +1189,7 @@ trajectory_point {
     theta: -1.9004446992022948
     kappa: -0.023163286038921004
     s: 15.377064146395053
-    dkappa: 0
+    dkappa: -0.0025230621730851881
     ddkappa: 0
   }
   v: 3.0732742665621844
@@ -1203,7 +1203,7 @@ trajectory_point {
     theta: -1.9069662948997672
     kappa: -0.02389142764579491
     s: 15.683056319791335
-    dkappa: 0
+    dkappa: -0.0024473800842227909
     ddkappa: 0
   }
   v: 3.0464185091840741
@@ -1217,7 +1217,7 @@ trajectory_point {
     theta: -1.9135318446616632
     kappa: -0.024588579396549196
     s: 15.986316662797078
-    dkappa: 0
+    dkappa: -0.0023673463211373413
     ddkappa: 0
   }
   v: 3.0186266315757262
@@ -1231,7 +1231,7 @@ trajectory_point {
     theta: -1.9209969489173691
     kappa: -0.0250519711312155
     s: 16.286747976643245
-    dkappa: 0
+    dkappa: -0.0022414060048506581
     ddkappa: 0
   }
   v: 2.98982411288379
@@ -1245,7 +1245,7 @@ trajectory_point {
     theta: -1.9283909656622402
     kappa: -0.025510836605463987
     s: 16.584244774695524
-    dkappa: 0
+    dkappa: -0.0021166958327939458
     ddkappa: 0
   }
   v: 2.9599197166558455
@@ -1259,7 +1259,7 @@ trajectory_point {
     theta: -1.9357101751386594
     kappa: -0.025964997752803844
     s: 16.878691610894386
-    dkappa: 0
+    dkappa: -0.0019932641997668838
     ddkappa: 0
   }
   v: 2.92880549084041
@@ -1273,7 +1273,7 @@ trajectory_point {
     theta: -1.942950615767387
     kappa: -0.026414258566862157
     s: 17.169961408195221
-    dkappa: 0
+    dkappa: -0.0018711643762591109
     ddkappa: 0
   }
   v: 2.8963567677869331
@@ -1287,7 +1287,7 @@ trajectory_point {
     theta: -1.9501080451180206
     kappa: -0.026858402523133817
     s: 17.457913787008405
-    dkappa: 0
+    dkappa: -0.001750455209165416
     ddkappa: 0
   }
   v: 2.8624321642457997
@@ -1301,7 +1301,7 @@ trajectory_point {
     theta: -1.9571779011044643
     kappa: -0.027297190000731511
     s: 17.742393393639389
-    dkappa: 0
+    dkappa: -0.0016312018225009264
     ddkappa: 0
   }
   v: 2.8268735813683286
@@ -1315,7 +1315,7 @@ trajectory_point {
     theta: -1.964546567926144
     kappa: -0.027649011398630682
     s: 18.023228228728819
-    dkappa: 0
+    dkappa: -0.0015016985347584926
     ddkappa: 0
   }
   v: 2.7895062047067745
@@ -1329,7 +1329,7 @@ trajectory_point {
     theta: -1.9729999083092733
     kappa: -0.027749937804231607
     s: 18.3002279756926
-    dkappa: 0
+    dkappa: -0.0013383324799122651
     ddkappa: 0
   }
   v: 2.7501385042143229
@@ -1343,7 +1343,7 @@ trajectory_point {
     theta: -1.9813325863543598
     kappa: -0.027849390248108369
     s: 18.573182329162009
-    dkappa: 0
+    dkappa: -0.0011773522755324016
     ddkappa: 0
   }
   v: 2.7085622342450977
@@ -1357,7 +1357,7 @@ trajectory_point {
     theta: -1.9895362651038038
     kappa: -0.027947284212257176
     s: 18.84185932342378
-    dkappa: 0
+    dkappa: -0.001018894727965663
     ddkappa: 0
   }
   v: 2.6645524335541544
@@ -1371,7 +1371,7 @@ trajectory_point {
     theta: -1.9976019965674856
     kappa: -0.02804352667756449
     s: 19.106003660860203
-    dkappa: 0
+    dkappa: -0.00086311040400885669
     ddkappa: 0
   }
   v: 2.6178674252974834
@@ -1385,7 +1385,7 @@ trajectory_point {
     theta: -2.0055201774393039
     kappa: -0.028138015514764816
     s: 19.365335040389205
-    dkappa: 0
+    dkappa: -0.00071016461674421288
     ddkappa: 0
   }
   v: 2.56824881703201
@@ -1399,7 +1399,7 @@ trajectory_point {
     theta: -2.0132805050813025
     kappa: -0.02823063887539852
     s: 19.619546485904458
-    dkappa: 0
+    dkappa: -0.00056023841137474136
     ddkappa: 0
   }
   v: 2.5154215007155938
@@ -1413,7 +1413,7 @@ trajectory_point {
     theta: -2.0208719336720011
     kappa: -0.028321274582769628
     s: 19.868302674715473
-    dkappa: 0
+    dkappa: -0.00041352955105960039
     ddkappa: 0
   }
   v: 2.4590936527070277
@@ -1427,7 +1427,7 @@ trajectory_point {
     theta: -2.0274305438067253
     kappa: -0.028166709461627821
     s: 20.111238265987669
-    dkappa: 0
+    dkappa: -0.00024270682394816342
     ddkappa: 0
   }
   v: 2.3989567337660409
@@ -1441,7 +1441,7 @@ trajectory_point {
     theta: -2.0333880672071607
     kappa: -0.027892327393446537
     s: 20.347956229182508
-    dkappa: 0
+    dkappa: -6.2229717985565685e-05
     ddkappa: 0
   }
   v: 2.3346854890532946
@@ -1455,7 +1455,7 @@ trajectory_point {
     theta: -2.0391793150080431
     kappa: -0.027625651109056695
     s: 20.578026172497545
-    dkappa: 0
+    dkappa: 0.00011317884407738396
     ddkappa: 0
   }
   v: 2.2659379481303858
@@ -1469,7 +1469,7 @@ trajectory_point {
     theta: -2.044792109902883
     kappa: -0.027367220086619588
     s: 20.800982671306549
-    dkappa: 0
+    dkappa: 0.00028316401595672675
     ddkappa: 0
   }
   v: 2.1923554249598456
@@ -1483,7 +1483,7 @@ trajectory_point {
     theta: -2.0502134167630519
     kappa: -0.027117616348700704
     s: 21.016323596599591
-    dkappa: 0
+    dkappa: 0.00044734296742991004
     ddkappa: 0
   }
   v: 2.1135625179051392
@@ -1497,7 +1497,7 @@ trajectory_point {
     theta: -2.0554293028517918
     kappa: -0.026877466399790932
     s: 21.223508443423135
-    dkappa: 0
+    dkappa: 0.00060530360991499531
     ddkappa: 0
   }
   v: 2.0291671097306661
@@ -1511,7 +1511,7 @@ trajectory_point {
     theta: -2.0604248974221915
     kappa: -0.026647443163827691
     s: 21.421956659320131
-    dkappa: 0
+    dkappa: 0.00075660332204986
     ddkappa: 0
   }
   v: 1.9387603676017602
@@ -1525,7 +1525,7 @@ trajectory_point {
     theta: -2.0651843505079763
     kappa: -0.026428267921716136
     s: 21.611045972770107
-    dkappa: 0
+    dkappa: 0.00090076767527140869
     ddkappa: 0
   }
   v: 1.8419167430846894
@@ -1539,7 +1539,7 @@ trajectory_point {
     theta: -2.0696907907012552
     kappa: -0.026220712248850295
     s: 21.790110721629258
-    dkappa: 0
+    dkappa: 0.0010372891593947717
     ddkappa: 0
   }
   v: 1.7381939721466571
@@ -1553,7 +1553,7 @@ trajectory_point {
     theta: -2.0739604704564107
     kappa: -0.02604648796808496
     s: 21.958440181570559
-    dkappa: 0
+    dkappa: 0.001148654192935007
     ddkappa: 0
   }
   v: 1.6271330751557997
@@ -1567,7 +1567,7 @@ trajectory_point {
     theta: -2.0785343756709573
     kappa: -0.026247994077585639
     s: 22.115276894523845
-    dkappa: 0
+    dkappa: 0.00095679613522529038
     ddkappa: 0
   }
   v: 1.5082583568811889
@@ -1581,7 +1581,7 @@ trajectory_point {
     theta: -2.0827505163851936
     kappa: -0.026433698752081146
     s: 22.259814997115896
-    dkappa: 0
+    dkappa: 0.00077998294457509883
     ddkappa: 0
   }
   v: 1.3810774064928297
@@ -1595,7 +1595,7 @@ trajectory_point {
     theta: -2.086583566381722
     kappa: -0.026602502266999915
     s: 22.391198549110534
-    dkappa: 0
+    dkappa: 0.00061926169107495189
     ddkappa: 0
   }
   v: 1.2450810975616622
@@ -1609,7 +1609,7 @@ trajectory_point {
     theta: -2.0900067651106844
     kappa: -0.026753238410637606
     s: 22.508519861848747
-    dkappa: 0
+    dkappa: 0.000475742748564265
     ddkappa: 0
   }
   v: 1.099743588059561
@@ -1623,7 +1623,7 @@ trajectory_point {
     theta: -2.092991861502171
     kappa: -0.026884672336512365
     s: 22.610817826688731
-    dkappa: 0
+    dkappa: 0.00035060183944751977
     ddkappa: 0
   }
   v: 0.94452232035933381
@@ -1637,7 +1637,7 @@ trajectory_point {
     theta: -2.0955090561038134
     kappa: -0.026995498415720163
     s: 22.697076243446027
-    dkappa: 0
+    dkappa: 0.00024508207951032078
     ddkappa: 0
   }
   v: 0.7788580212347247
@@ -1651,7 +1651,7 @@ trajectory_point {
     theta: -2.0975269418552838
     kappa: -0.027084338089290068
     s: 22.766222148833574
-    dkappa: 0
+    dkappa: 0.00016049602273555312
     ddkappa: 0
   }
   v: 0.60217470186040967
@@ -1665,7 +1665,7 @@ trajectory_point {
     theta: -2.0990124440454552
     kappa: -0.027149737720539632
     s: 22.817124144901854
-    dkappa: 0
+    dkappa: 9.8227706119425945e-05
     ddkappa: 0
   }
   v: 0.41387965781200131
@@ -1679,7 +1679,7 @@ trajectory_point {
     theta: -2.0999307603000648
     kappa: -0.027190166447430129
     s: 22.848590727478946
-    dkappa: 0
+    dkappa: 5.9734694487630904e-05
     ddkappa: 0
   }
   v: 0.21336346906604456
@@ -1693,7 +1693,7 @@ trajectory_point {
     theta: -2.1002453018318996
     kappa: -0.027204014034921887
     s: 22.859368614610617
-    dkappa: 0
+    dkappa: 4.6550125311453834e-05
     ddkappa: 0
   }
   v: 1.7807977314924246e-13
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_mission_complete_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_mission_complete_0.pb.txt
index 2cf8432bf0c4b30ce92468966381deb029ccb0cd..826799c7e7a72ecf4a87e0f27a7574084e677aa4 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_mission_complete_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_mission_complete_0.pb.txt
@@ -27,7 +27,7 @@ trajectory_point {
     theta: 1.3203319804636646
     kappa: 1.446888215812294e-09
     s: 5.3817390327768688e-06
-    dkappa: 0
+    dkappa: 7.2341683739918291e-10
     ddkappa: 0
   }
   v: 0.00060191969665356808
@@ -41,7 +41,7 @@ trajectory_point {
     theta: 1.3203319867314016
     kappa: 7.1499836960430452e-09
     s: 2.6594553691305245e-05
-    dkappa: 0
+    dkappa: 3.5748570873158629e-09
     ddkappa: 0
   }
   v: 0.0015815076986711138
@@ -55,7 +55,7 @@ trajectory_point {
     theta: 1.3203319998802634
     kappa: 1.9114304410440395e-08
     s: 7.1096161407582529e-05
-    dkappa: 0
+    dkappa: 9.55679194465831e-09
     ddkappa: 0
   }
   v: 0.0029292162964724248
@@ -69,7 +69,7 @@ trajectory_point {
     theta: 1.3203320220576604
     kappa: 3.9293806798277306e-08
     s: 0.00014615435489888197
-    dkappa: 0
+    dkappa: 1.9646162801489866e-08
     ddkappa: 0
   }
   v: 0.0046356007281652921
@@ -83,7 +83,7 @@ trajectory_point {
     theta: 1.3203320553554938
     kappa: 6.9591939321007986e-08
     s: 0.0002588490611215126
-    dkappa: 0
+    dkappa: 3.4794658012923418e-08
     ddkappa: 0
   }
   v: 0.0066913191795455185
@@ -97,7 +97,7 @@ trajectory_point {
     theta: 1.3203321018107654
     kappa: 1.1186219601492358e-07
     s: 0.00041607440022457982
-    dkappa: 0
+    dkappa: 5.5928989663016029e-08
     ddkappa: 0
   }
   v: 0.00908713278409691
@@ -111,7 +111,7 @@ trajectory_point {
     theta: 1.320332163406184
     kappa: 1.6790867004380732e-07
     s: 0.000624540744503745
-    dkappa: 0
+    dkappa: 8.3951170330663362e-08
     ddkappa: 0
   }
   v: 0.011813905622991278
@@ -125,7 +125,7 @@ trajectory_point {
     theta: 1.3203322420707762
     kappa: 2.3948660725158997e-07
     s: 0.00089077677735498564
-    dkappa: 0
+    dkappa: 1.1973878985549413e-07
     ddkappa: 0
   }
   v: 0.014862604725088444
@@ -139,7 +139,7 @@ trajectory_point {
     theta: 1.3203323396804925
     kappa: 3.2830295971500559e-07
     s: 0.001221131552228356
-    dkappa: 0
+    dkappa: 1.6414529210376492e-07
     ddkappa: 0
   }
   v: 0.018224300066936237
@@ -153,7 +153,7 @@ trajectory_point {
     theta: 1.3203324580588167
     kappa: 4.3601693929624662e-07
     s: 0.001621776551581747
-    dkappa: 0
+    dkappa: 2.1800025173425441e-07
     ddkappa: 0
   }
   v: 0.021890164572770487
@@ -167,7 +167,7 @@ trajectory_point {
     theta: 1.3203325989773746
     kappa: 5.6424057119561975e-07
     s: 0.0020987077458346466
-    dkappa: 0
+    dkappa: 2.821096509641581e-07
     ddkappa: 0
   }
   v: 0.025851474114515046
@@ -181,7 +181,7 @@ trajectory_point {
     theta: 1.3203327641565421
     kappa: 7.1453924750420013e-07
     s: 0.0026577476523218986
-    dkappa: 0
+    dkappa: 3.5725615633498257e-07
     ddkappa: 0
   }
   v: 0.030099607511781753
@@ -195,7 +195,7 @@ trajectory_point {
     theta: 1.3203329552660523
     kappa: 8.8843228075648918e-07
     s: 0.0033045473942474655
-    dkappa: 0
+    dkappa: 4.4419939547844052e-07
     ddkappa: 0
   }
   v: 0.034626046531870465
@@ -209,7 +209,7 @@ trajectory_point {
     theta: 1.3203331739256063
     kappa: 1.0873934574830672e-06
     s: 0.0040445887596381866
-    dkappa: 0
+    dkappa: 5.4367623388234475e-07
     ddkappa: 0
   }
   v: 0.039422375889769053
@@ -223,7 +223,7 @@ trajectory_point {
     theta: 1.320333421705479
     kappa: 1.3128515917632514e-06
     s: 0.0048831862602975373
-    dkappa: 0
+    dkappa: 6.5640105165650292e-07
     ddkappa: 0
   }
   v: 0.044480283248153375
@@ -237,7 +237,7 @@ trajectory_point {
     theta: 1.3203337001271296
     kappa: 1.5661910787777484e-06
     s: 0.00582548919075939
-    dkappa: 0
+    dkappa: 7.8306602029861178e-07
     ddkappa: 0
   }
   v: 0.049791559217387323
@@ -251,7 +251,7 @@ trajectory_point {
     theta: 1.3203340106638084
     kappa: 1.8487524483613132e-06
     s: 0.0068764836872417766
-    dkappa: 0
+    dkappa: 9.243413794601525e-07
     ddkappa: 0
   }
   v: 0.055348097355522749
@@ -265,7 +265,7 @@ trajectory_point {
     theta: 1.3203343547411661
     kappa: 2.1618329185554016e-06
     s: 0.0080409947866006467
-    dkappa: 0
+    dkappa: 1.0808757137122844e-06
     ddkappa: 0
   }
   v: 0.061141894168299578
@@ -279,7 +279,7 @@ trajectory_point {
     theta: 1.3203347337378617
     kappa: 2.5066869491608279e-06
     s: 0.0093236884852836276
-    dkappa: 0
+    dkappa: 1.25329622931174e-06
     ddkappa: 0
   }
   v: 0.067165049109145683
@@ -293,7 +293,7 @@ trajectory_point {
     theta: 1.320335148986171
     kappa: 2.8845267952904167e-06
     s: 0.010729073798283782
-    dkappa: 0
+    dkappa: 1.4422090309667184e-06
     ddkappa: 0
   }
   v: 0.073409764579176973
@@ -307,7 +307,7 @@ trajectory_point {
     theta: 1.3203356017725951
     kappa: 3.2965230609216649e-06
     s: 0.012261504818093374
-    dkappa: 0
+    dkappa: 1.6481993986027818e-06
     ddkappa: 0
   }
   v: 0.079868345927197368
@@ -321,7 +321,7 @@ trajectory_point {
     theta: 1.3203360933384685
     kappa: 3.7438052524493887e-06
     s: 0.013925182773657622
-    dkappa: 0
+    dkappa: 1.871832064128748e-06
     ddkappa: 0
   }
   v: 0.086533201449698768
@@ -335,7 +335,7 @@ trajectory_point {
     theta: 1.3203366248805675
     kappa: 4.2274623322383863e-06
     s: 0.015724158089328467
-    dkappa: 0
+    dkappa: 2.1136514882025865e-06
     ddkappa: 0
   }
   v: 0.093396842390861115
@@ -349,7 +349,7 @@ trajectory_point {
     theta: 1.3203371975517186
     kappa: 4.7485432721760912e-06
     s: 0.017662332443818329
-    dkappa: 0
+    dkappa: 2.3741821369973128e-06
     ddkappa: 0
   }
   v: 0.10045188294255233
@@ -363,7 +363,7 @@ trajectory_point {
     theta: 1.3203378124614069
     kappa: 5.3080576072252219e-06
     s: 0.019743460829153858
-    dkappa: 0
+    dkappa: 2.65392875896688e-06
     ddkappa: 0
   }
   v: 0.10769104024432834
@@ -377,7 +377,7 @@ trajectory_point {
     theta: 1.3203384706763848
     kappa: 5.906975988976452e-06
     s: 0.021971153609629722
-    dkappa: 0
+    dkappa: 2.9533766616120803e-06
     ddkappa: 0
   }
   v: 0.1151071343834331
@@ -391,7 +391,7 @@ trajectory_point {
     theta: 1.3203391732212795
     kappa: 6.5462307392010449e-06
     s: 0.024348878580762325
-    dkappa: 0
+    dkappa: 3.2729919882464303e-06
     ddkappa: 0
   }
   v: 0.12269308839479859
@@ -405,7 +405,7 @@ trajectory_point {
     theta: 1.320339921079202
     kappa: 7.2267164034035291e-06
     s: 0.026879963028243612
-    dkappa: 0
+    dkappa: 3.6132219947620746e-06
     ddkappa: 0
   }
   v: 0.13044192826104473
@@ -419,7 +419,7 @@ trajectory_point {
     theta: 1.3203407151923554
     kappa: 7.9492903043743384e-06
     s: 0.029567595786894781
-    dkappa: 0
+    dkappa: 3.9744953263956711e-06
     ddkappa: 0
   }
   v: 0.13834678291247951
@@ -433,7 +433,7 @@ trajectory_point {
     theta: 1.3203415564626433
     kappa: 8.7147730957424781e-06
     s: 0.0324148292996201
-    dkappa: 0
+    dkappa: 4.3572222944942964e-06
     ddkappa: 0
   }
   v: 0.14640088422709888
@@ -447,7 +447,7 @@ trajectory_point {
     theta: 1.3203424457522777
     kappa: 9.52394931552817e-06
     s: 0.035424581676360596
-    dkappa: 0
+    dkappa: 4.7617951532813263e-06
     ddkappa: 0
   }
   v: 0.15459756703058683
@@ -461,7 +461,7 @@ trajectory_point {
     theta: 1.3203433838843879
     kappa: 1.0377567939695526e-05
     s: 0.038599638753047921
-    dkappa: 0
+    dkappa: 5.1885883766223489e-06
     ddkappa: 0
   }
   v: 0.16293026909631539
@@ -475,7 +475,7 @@ trajectory_point {
     theta: 1.3203443716436285
     kappa: 1.1276342935705173e-05
     s: 0.041942656150557957
-    dkappa: 0
+    dkappa: 5.6379589347910368e-06
     ddkappa: 0
   }
   v: 0.17139253114534442
@@ -489,7 +489,7 @@ trajectory_point {
     theta: 1.3203454097767884
     kappa: 1.2220953816066953e-05
     s: 0.045456161333664757
-    dkappa: 0
+    dkappa: 6.1102465712350661e-06
     ddkappa: 0
   }
   v: 0.17997799684642207
@@ -503,7 +503,7 @@ trajectory_point {
     theta: 1.3203464989933977
     kappa: 1.3212046191892521e-05
     s: 0.049142555669994141
-    dkappa: 0
+    dkappa: 6.6057740793419845e-06
     ddkappa: 0
   }
   v: 0.1886804128159843
@@ -517,7 +517,7 @@ trajectory_point {
     theta: 1.3203476399663381
     kappa: 1.4250232326448062e-05
     s: 0.053004116488977591
-    dkappa: 0
+    dkappa: 7.1248475792051343e-06
     ddkappa: 0
   }
   v: 0.19749362861815511
@@ -531,7 +531,7 @@ trajectory_point {
     theta: 1.3203488333324491
     kappa: 1.5336091688706893e-05
     s: 0.057042999140805906
-    dkappa: 0
+    dkappa: 7.66775679438952e-06
     ddkappa: 0
   }
   v: 0.20641159676474649
@@ -545,7 +545,7 @@ trajectory_point {
     theta: 1.3203500796931387
     kappa: 1.6470171506902152e-05
     s: 0.061261239055383006
-    dkappa: 0
+    dkappa: 8.2347753286977178e-06
     ddkappa: 0
   }
   v: 0.2154283727152585
@@ -559,7 +559,7 @@ trajectory_point {
     theta: 1.3203513796149891
     kappa: 1.7652987322079443e-05
     s: 0.065660753801279712
-    dkappa: 0
+    dkappa: 8.8261609429357735e-06
     ddkappa: 0
   }
   v: 0.22453811487687922
@@ -573,7 +573,7 @@ trajectory_point {
     theta: 1.3203527336303675
     kappa: 1.8885023541649488e-05
     s: 0.070243345144687469
-    dkappa: 0
+    dkappa: 9.4421558316790819e-06
     ddkappa: 0
   }
   v: 0.23373508460448461
@@ -587,7 +587,7 @@ trajectory_point {
     theta: 1.3203541422380332
     kappa: 2.0166733992940798e-05
     s: 0.075010701108372158
-    dkappa: 0
+    dkappa: 1.0082986900038302e-05
     ddkappa: 0
   }
   v: 0.24301364620063878
@@ -601,7 +601,7 @@ trajectory_point {
     theta: 1.3203556059037453
     kappa: 2.1498542476752297e-05
     s: 0.079964398030627784
-    dkappa: 0
+    dkappa: 1.0748866040425228e-05
     ddkappa: 0
   }
   v: 0.25236826691559378
@@ -615,7 +615,7 @@ trajectory_point {
     theta: 1.3203571250608734
     kappa: 2.2880843320906008e-05
     s: 0.085105902624230262
-    dkappa: 0
+    dkappa: 1.14399904093187e-05
     ddkappa: 0
   }
   v: 0.26179351694728958
@@ -629,7 +629,7 @@ trajectory_point {
     theta: 1.3203587001110035
     kappa: 2.43140019337997e-05
     s: 0.090436574035391251
-    dkappa: 0
+    dkappa: 1.21565427040305e-05
     ddkappa: 0
   }
   v: 0.2712840694413543
@@ -643,7 +643,7 @@ trajectory_point {
     theta: 1.3203603314245473
     kappa: 2.5798355357959522e-05
     s: 0.09595766590271175
-    dkappa: 0
+    dkappa: 1.2898691439471225e-05
     ddkappa: 0
   }
   v: 0.28083470049110415
@@ -657,7 +657,7 @@ trajectory_point {
     theta: 1.320362019341351
     kappa: 2.7334212823592709e-05
     s: 0.10167032841613607
-    dkappa: 0
+    dkappa: 1.3666591224916215e-05
     ddkappa: 0
   }
   v: 0.29044028913754311
@@ -671,7 +671,7 @@ trajectory_point {
     theta: 1.3203637641713033
     kappa: 2.8921856302140177e-05
     s: 0.10757561037590542
-    dkappa: 0
+    dkappa: 1.4460383040771421e-05
     ddkappa: 0
   }
   v: 0.30009581736936314
@@ -685,7 +685,7 @@ trajectory_point {
     theta: 1.3203655661949429
     kappa: 3.0561541059829224e-05
     s: 0.11367446125151175
-    dkappa: 0
+    dkappa: 1.5280194515339307e-05
     ddkappa: 0
   }
   v: 0.30979637012294453
@@ -699,7 +699,7 @@ trajectory_point {
     theta: 1.3203674256640689
     kappa: 3.2253496211226166e-05
     s: 0.11996773324065148
-    dkappa: 0
+    dkappa: 1.6126140201584755e-05
     ddkappa: 0
   }
   v: 0.31953713528235528
@@ -713,7 +713,7 @@ trajectory_point {
     theta: 1.3203693428023471
     kappa: 3.3997925272788984e-05
     s: 0.12645618332817926
-    dkappa: 0
+    dkappa: 1.6998321853900935e-05
     ddkappa: 0
   }
   v: 0.32931340367935152
@@ -727,7 +727,7 @@ trajectory_point {
     theta: 1.3203797994667095
     kappa: 4.3512580189172706e-05
     s: 0.16184619424080343
-    dkappa: 0
+    dkappa: 2.1755469982788033e-05
     ddkappa: 0
   }
   v: 0.3785696796777398
@@ -741,7 +741,7 @@ trajectory_point {
     theta: 1.3203917162982275
     kappa: 5.435586013326226e-05
     s: 0.20217806112639922
-    dkappa: 0
+    dkappa: 2.7176905584010024e-05
     ddkappa: 0
   }
   v: 0.42806635629978607
@@ -755,7 +755,7 @@ trajectory_point {
     theta: 1.320405093256261
     kappa: 6.652772810637407e-05
     s: 0.24745164636738337
-    dkappa: 0
+    dkappa: 3.3262610158922473e-05
     ddkappa: 0
   }
   v: 0.47733060366401225
@@ -769,7 +769,7 @@ trajectory_point {
     theta: 1.3204199172800539
     kappa: 8.00162999370378e-05
     s: 0.29762274647327525
-    dkappa: 0
+    dkappa: 4.000664184577943e-05
     ddkappa: 0
   }
   v: 0.52595393419394387
@@ -783,7 +783,7 @@ trajectory_point {
     theta: 1.3204361641898517
     kappa: 9.4799574133044436e-05
     s: 0.3526095263111968
-    dkappa: 0
+    dkappa: 4.7398000313153782e-05
     ddkappa: 0
   }
   v: 0.57359220261811128
@@ -797,7 +797,7 @@ trajectory_point {
     theta: 1.3204538005880164
     kappa: 0.00011084716173349459
     s: 0.41229895333637329
-    dkappa: 0
+    dkappa: 5.5421491653357678e-05
     ddkappa: 0
   }
   v: 0.61996560597004913
@@ -811,7 +811,7 @@ trajectory_point {
     theta: 1.3204727857601455
     kappa: 0.00012812201616084674
     s: 0.4765532318226337
-    dkappa: 0
+    dkappa: 6.4058593275862914e-05
     ddkappa: 0
   }
   v: 0.664858683588296
@@ -825,7 +825,7 @@ trajectory_point {
     theta: 1.3204930735761877
     kappa: 0.00014658216307296545
     s: 0.545216237092911
-    dkappa: 0
+    dkappa: 7.3288318800721316e-05
     ddkappa: 0
   }
   v: 0.70812031711639545
@@ -839,7 +839,7 @@ trajectory_point {
     theta: 1.3205146143915605
     kappa: 0.00016618243021516952
     s: 0.61811994974974283
-    dkappa: 0
+    dkappa: 8.308808295198514e-05
     ddkappa: 0
   }
   v: 0.74966373050289448
@@ -853,7 +853,7 @@ trajectory_point {
     theta: 1.320537356948267
     kappa: 0.00018687617727228042
     s: 0.69509088990577184
-    dkappa: 0
+    dkappa: 9.3434566451127513e-05
     ddkappa: 0
   }
   v: 0.78946649000134594
@@ -867,7 +867,7 @@ trajectory_point {
     theta: 1.3205612499594146
     kappa: 0.00020861673764240528
     s: 0.77595547989951086
-    dkappa: 0
+    dkappa: 0.00010430443687675982
     ddkappa: 0
   }
   v: 0.82752727856312891
@@ -881,7 +881,7 @@ trajectory_point {
     theta: 1.3205862403551711
     kappa: 0.00023135582241119121
     s: 0.86053410783531825
-    dkappa: 0
+    dkappa: 0.0001156735506818403
     ddkappa: 0
   }
   v: 0.86372161830492356
@@ -895,7 +895,7 @@ trajectory_point {
     theta: 1.3206122708169019
     kappa: 0.00025504127663145273
     s: 0.94863278200605772
-    dkappa: 0
+    dkappa: 0.00012751583137577668
     ddkappa: 0
   }
   v: 0.8979059960859791
@@ -909,7 +909,7 @@ trajectory_point {
     theta: 1.32063928008904
     kappa: 0.00027961736309747067
     s: 1.0400441863991177
-    dkappa: 0
+    dkappa: 0.00013980341140622723
     ddkappa: 0
   }
   v: 0.92995743893753868
@@ -923,7 +923,7 @@ trajectory_point {
     theta: 1.320667203585985
     kappa: 0.00030502531457044608
     s: 1.1345497347136102
-    dkappa: 0
+    dkappa: 0.0001525069082614194
     ddkappa: 0
   }
   v: 0.95977351406284
@@ -937,7 +937,7 @@ trajectory_point {
     theta: 1.3206959739990016
     kappa: 0.00033120388600395479
     s: 1.2319216243775717
-    dkappa: 0
+    dkappa: 0.00016559570057246895
     ddkappa: 0
   }
   v: 0.98727232883711424
@@ -951,7 +951,7 @@ trajectory_point {
     theta: 1.3207255219031209
     kappa: 0.00035808990676940069
     s: 1.3319248905651602
-    dkappa: 0
+    dkappa: 0.00017903820421569854
     ddkappa: 0
   }
   v: 1.0123925308075876
@@ -965,7 +965,7 @@ trajectory_point {
     theta: 1.3207557763640385
     kappa: 0.00038561883288147023
     s: 1.4343194602138567
-    dkappa: 0
+    dkappa: 0.0001928021484149567
     ddkappa: 0
   }
   v: 1.0350933076934798
@@ -979,7 +979,7 @@ trajectory_point {
     theta: 1.3207866655450164
     kappa: 0.00041372529922358654
     s: 1.5388622060416639
-    dkappa: 0
+    dkappa: 0.00020685485184393667
     ddkappa: 0
   }
   v: 1.0553543873860054
@@ -993,7 +993,7 @@ trajectory_point {
     theta: 1.3208181173137816
     kappa: 0.00044234367177336343
     s: 1.6453090005643061
-    dkappa: 0
+    dkappa: 0.00022116349872849535
     ddkappa: 0
   }
   v: 1.0731760379483721
@@ -1007,7 +1007,7 @@ trajectory_point {
     theta: 1.3208500598494273
     kappa: 0.00047140859982805879
     s: 1.7534167701124268
-    dkappa: 0
+    dkappa: 0.00023569541494897173
     ddkappa: 0
   }
   v: 1.0885790676157829
@@ -1021,7 +1021,7 @@ trajectory_point {
     theta: 1.3208824222493132
     kappa: 0.00050085556823002968
     s: 1.8629455488487907
-    dkappa: 0
+    dkappa: 0.00025041834414250635
     ddkappa: 0
   }
   v: 1.1016048247954342
@@ -1035,7 +1035,7 @@ trajectory_point {
     theta: 1.3209151351359665
     kappa: 0.00053062144959218586
     s: 1.9736605327854815
-    dkappa: 0
+    dkappa: 0.00026530072380535961
     ddkappa: 0
   }
   v: 1.1123151980665171
@@ -1049,7 +1049,7 @@ trajectory_point {
     theta: 1.3209802205915684
     kappa: 0.00054732324070299167
     s: 2.0853341338011013
-    dkappa: 0
+    dkappa: 0.00026219072911543218
     ddkappa: 0
   }
   v: 1.1207926161802164
@@ -1063,7 +1063,7 @@ trajectory_point {
     theta: 1.3210557453341034
     kappa: 0.00055998102466692386
     s: 2.1977480336579713
-    dkappa: 0
+    dkappa: 0.00025340858006012607
     ddkappa: 0
   }
   v: 1.1271400480597111
@@ -1077,7 +1077,7 @@ trajectory_point {
     theta: 1.3211316283809187
     kappa: 0.00057269885861767616
     s: 2.3106952380193295
-    dkappa: 0
+    dkappa: 0.00024458476747694447
     ddkappa: 0
   }
   v: 1.1314810028001752
@@ -1091,7 +1091,7 @@ trajectory_point {
     theta: 1.3212077396509303
     kappa: 0.000585454941382954
     s: 2.4239821304665314
-    dkappa: 0
+    dkappa: 0.00023573441732673608
     ddkappa: 0
   }
   v: 1.1339595296687759
@@ -1105,7 +1105,7 @@ trajectory_point {
     theta: 1.3212839594308774
     kappa: 0.00059822920942347491
     s: 2.537430526516248
-    dkappa: 0
+    dkappa: 0.00022687144997605638
     ddkappa: 0
   }
   v: 1.1347402181046748
@@ -1119,7 +1119,7 @@ trajectory_point {
     theta: 1.32136017975532
     kappa: 0.00061100356811491811
     s: 2.6508797276376681
-    dkappa: 0
+    dkappa: 0.00021800841973048822
     ddkappa: 0
   }
   v: 1.1340081977190279
@@ -1133,7 +1133,7 @@ trajectory_point {
     theta: 1.321436305786637
     kappa: 0.00062376212302987441
     s: 2.7641885752696935
-    dkappa: 0
+    dkappa: 0.00020915635436796263
     ddkappa: 0
   }
   v: 1.1319691382949855
@@ -1147,7 +1147,7 @@ trajectory_point {
     theta: 1.3215122561138526
     kappa: 0.00063649123001808627
     s: 2.8772358955840245
-    dkappa: 0
+    dkappa: 0.00020032472039238254
     ddkappa: 0
   }
   v: 1.1287848194431134
@@ -1161,7 +1161,7 @@ trajectory_point {
     theta: 1.3215879521750677
     kappa: 0.00064917772242759574
     s: 2.989904755419305
-    dkappa: 0
+    dkappa: 0.00019152265301259532
     ddkappa: 0
   }
   v: 1.1243774323739633
@@ -1175,7 +1175,7 @@ trajectory_point {
     theta: 1.3216633074130428
     kappa: 0.00066180709360271644
     s: 3.1020663210283157
-    dkappa: 0
+    dkappa: 0.00018276021715093587
     ddkappa: 0
   }
   v: 1.1186186583428384
@@ -1189,7 +1189,7 @@ trajectory_point {
     theta: 1.3217382270969265
     kappa: 0.00067436346700465379
     s: 3.2135795927189124
-    dkappa: 0
+    dkappa: 0.00017404842817396251
     ddkappa: 0
   }
   v: 1.1113934971959472
@@ -1203,7 +1203,7 @@ trajectory_point {
     theta: 1.3218126092170692
     kappa: 0.00068682974617858715
     s: 3.3242927367131152
-    dkappa: 0
+    dkappa: 0.00016539914784317932
     ddkappa: 0
   }
   v: 1.1026002673704041
@@ -1217,7 +1217,7 @@ trajectory_point {
     theta: 1.3218863453798393
     kappa: 0.00069918776472075372
     s: 3.4340444170062034
-    dkappa: 0
+    dkappa: 0.0001568249802657585
     ddkappa: 0
   }
   v: 1.0921506058942287
@@ -1231,7 +1231,7 @@ trajectory_point {
     theta: 1.3219593217024366
     kappa: 0.00071141843624553081
     s: 3.5426651272258018
-    dkappa: 0
+    dkappa: 0.00014833916784526334
     ddkappa: 0
   }
   v: 1.0799694683863468
@@ -1245,7 +1245,7 @@ trajectory_point {
     theta: 1.3220314197077088
     kappa: 0.00072350190435251864
     s: 3.6499785224909749
-    dkappa: 0
+    dkappa: 0.0001399554872323704
     ddkappa: 0
   }
   v: 1.065995129056589
@@ -1259,7 +1259,7 @@ trajectory_point {
     theta: 1.3221025172189658
     kappa: 0.00073541769259362366
     s: 3.7558027512713137
-    dkappa: 0
+    dkappa: 0.00013168814527559231
     ddkappa: 0
   }
   v: 1.0501791807056926
@@ -1273,7 +1273,7 @@ trajectory_point {
     theta: 1.3221724892547932
     kappa: 0.00074714485444014129
     s: 3.8599517872460285
-    dkappa: 0
+    dkappa: 0.00012355167497200031
     ddkappa: 0
   }
   v: 1.0324865347252992
@@ -1287,7 +1287,7 @@ trajectory_point {
     theta: 1.3222412089238671
     kappa: 0.00075866212324983862
     s: 3.96223676116304
-    dkappa: 0
+    dkappa: 0.00011556083141794676
     ddkappa: 0
   }
   v: 1.0128954210979568
@@ -1301,7 +1301,7 @@ trajectory_point {
     theta: 1.3223146619869321
     kappa: 0.0007623996031178974
     s: 4.0624672926980683
-    dkappa: 0
+    dkappa: 0.00010882568122134472
     ddkappa: 0
   }
   v: 0.99139738839711911
@@ -1315,7 +1315,7 @@ trajectory_point {
     theta: 1.3223901042556796
     kappa: 0.00076156598962317222
     s: 4.160452822313724
-    dkappa: 0
+    dkappa: 0.00010289245403312241
     ddkappa: 0
   }
   v: 0.96799730378714488
@@ -1329,7 +1329,7 @@ trajectory_point {
     theta: 1.3224636722004395
     kappa: 0.00076075308690119379
     s: 4.2560039431186
-    dkappa: 0
+    dkappa: 9.7106635351033871e-05
     ddkappa: 0
   }
   v: 0.94271335302329884
@@ -1343,7 +1343,7 @@ trajectory_point {
     theta: 1.3225352219022108
     kappa: 0.000759962485197219
     s: 4.3489337327263593
-    dkappa: 0
+    dkappa: 9.147954371353109e-05
     ddkappa: 0
   }
   v: 0.91557704045175126
@@ -1357,7 +1357,7 @@ trajectory_point {
     theta: 1.3226046123804309
     kappa: 0.00075919574228872446
     s: 4.4390590851148275
-    dkappa: 0
+    dkappa: 8.60222665702953e-05
     ddkappa: 0
   }
   v: 0.88663318900957777
@@ -1371,7 +1371,7 @@ trajectory_point {
     theta: 1.322671706618427
     kappa: 0.00075845437215459294
     s: 4.5262020424850844
-    dkappa: 0
+    dkappa: 8.074557963540429e-05
     ddkappa: 0
   }
   v: 0.85593994022476
@@ -1385,7 +1385,7 @@ trajectory_point {
     theta: 1.3227363725888646
     kappa: 0.000757739833644301
     s: 4.6101911271205527
-    dkappa: 0
+    dkappa: 7.5659866240500691e-05
     ddkappa: 0
   }
   v: 0.82356875421618481
@@ -1399,7 +1399,7 @@ trajectory_point {
     theta: 1.3227984842791953
     kappa: 0.00075705351914710567
     s: 4.6908626732460883
-    dkappa: 0
+    dkappa: 7.07750366879595e-05
     ddkappa: 0
   }
   v: 0.78960440969364476
@@ -1413,7 +1413,7 @@ trajectory_point {
     theta: 1.3228579227171011
     kappa: 0.00075639674326123153
     s: 4.7680621588870711
-    dkappa: 0
+    dkappa: 6.61004476040559e-05
     ddkappa: 0
   }
   v: 0.75414500395783834
@@ -1427,7 +1427,7 @@ trajectory_point {
     theta: 1.3229145769959356
     kappa: 0.00075577073146305788
     s: 4.8416455377284988
-    dkappa: 0
+    dkappa: 6.164482129213297e-05
     ddkappa: 0
   }
   v: 0.717301952900369
@@ -1441,7 +1441,7 @@ trajectory_point {
     theta: 1.3229683453001631
     kappa: 0.00075517660877630561
     s: 4.9114805709740725
-    dkappa: 0
+    dkappa: 5.7416165085769593e-05
     ddkappa: 0
   }
   v: 0.67919999100374628
@@ -1455,7 +1455,7 @@ trajectory_point {
     theta: 1.3230191359307968
     kappa: 0.00075461538844122443
     s: 4.9774481592052888
-    dkappa: 0
+    dkappa: 5.3421690701948228e-05
     ddkappa: 0
   }
   v: 0.63997717134138521
@@ -1469,7 +1469,7 @@ trajectory_point {
     theta: 1.3230668683308326
     kappa: 0.00075408796058377915
     s: 5.039443674240534
-    dkappa: 0
+    dkappa: 4.96677335942225e-05
     ddkappa: 0
   }
   v: 0.59978486557760635
@@ -1483,7 +1483,7 @@ trajectory_point {
     theta: 1.3231114741106826
     kappa: 0.00075359508088483791
     s: 5.0973782909941683
-    dkappa: 0
+    dkappa: 4.6159672305885283e-05
     ddkappa: 0
   }
   v: 0.55878776396763574
@@ -1497,7 +1497,7 @@ trajectory_point {
     theta: 1.3231528980736069
     kappa: 0.0007531373592493582
     s: 5.1511803193356238
-    dkappa: 0
+    dkappa: 4.2901847823136086e-05
     ddkappa: 0
   }
   v: 0.51716387535760544
@@ -1511,7 +1511,7 @@ trajectory_point {
     theta: 1.3231910992411453
     kappa: 0.00075271524847557426
     s: 5.2007965359484869
-    dkappa: 0
+    dkappa: 3.98974829282493e-05
     ddkappa: 0
   }
   v: 0.47510452718455237
@@ -1525,7 +1525,7 @@ trajectory_point {
     theta: 1.3232260518785459
     kappa: 0.00075232903292418414
     s: 5.2461935161895967
-    dkappa: 0
+    dkappa: 3.7148601552741509e-05
     ddkappa: 0
   }
   v: 0.43281436547642
@@ -1539,7 +1539,7 @@ trajectory_point {
     theta: 1.3232577465201969
     kappa: 0.00075197881718753654
     s: 5.2873589659481288
-    dkappa: 0
+    dkappa: 3.4655948130539689e-05
     ddkappa: 0
   }
   v: 0.39051135485205657
@@ -1553,7 +1553,7 @@ trajectory_point {
     theta: 1.3232861909950566
     kappa: 0.00075166451475881823
     s: 5.3243030535046918
-    dkappa: 0
+    dkappa: 3.2418906951148623e-05
     ddkappa: 0
   }
   v: 0.34842677852121617
@@ -1567,7 +1567,7 @@ trajectory_point {
     theta: 1.3233114114520836
     kappa: 0.00075138583670124022
     s: 5.3570597413904109
-    dkappa: 0
+    dkappa: 3.0435421512819094e-05
     ddkappa: 0
   }
   v: 0.3068052382845588
@@ -1581,7 +1581,7 @@ trajectory_point {
     theta: 1.3233334533856702
     kappa: 0.000751142280317226
     s: 5.3856881182460272
-    dkappa: 0
+    dkappa: 2.8701913875715122e-05
     ddkappa: 0
   }
   v: 0.26590465453364981
@@ -1595,7 +1595,7 @@ trajectory_point {
     theta: 1.3233523826610758
     kappa: 0.00075093311781759711
     s: 5.41027373068098
-    dkappa: 0
+    dkappa: 2.7213204015082289e-05
     ddkappa: 0
   }
   v: 0.22599626625096003
@@ -1609,7 +1609,7 @@ trajectory_point {
     theta: 1.3233682865398608
     kappa: 0.00075075738499076172
     s: 5.4309299151325039
-    dkappa: 0
+    dkappa: 2.5962429174415055e-05
     ddkappa: 0
   }
   v: 0.18736463100986597
@@ -1623,7 +1623,7 @@ trajectory_point {
     theta: 1.3233812747053249
     kappa: 0.00075061386987190044
     s: 5.4477991297247144
-    dkappa: 0
+    dkappa: 2.4940963218624902e-05
     ddkappa: 0
   }
   v: 0.15030762497464994
@@ -1637,7 +1637,7 @@ trajectory_point {
     theta: 1.3233914802879441
     kappa: 0.00075050110141215373
     s: 5.4610542861277018
-    dkappa: 0
+    dkappa: 2.4138335987207886e-05
     ddkappa: 0
   }
   v: 0.11513644290049938
@@ -1651,7 +1651,7 @@ trajectory_point {
     theta: 1.3233990608908117
     kappa: 0.00075041733814780909
     s: 5.4709000814166187
-    dkappa: 0
+    dkappa: 2.3542152647412593e-05
     ddkappa: 0
   }
   v: 0.0821755981335079
@@ -1665,7 +1665,7 @@ trajectory_point {
     theta: 1.32340419961508
     kappa: 0.00075036055686948807
     s: 5.477574329930774
-    dkappa: 0
+    dkappa: 2.3138013047407695e-05
     ddkappa: 0
   }
   v: 0.051762922610674091
@@ -1679,7 +1679,7 @@ trajectory_point {
     theta: 1.3234071060854036
     kappa: 0.00075032844129133266
     s: 5.4813492951327216
-    dkappa: 0
+    dkappa: 2.2909431069449908e-05
     ddkappa: 0
   }
   v: 0.024249566859902778
@@ -1693,7 +1693,7 @@ trajectory_point {
     theta: 1.3234080174753833
     kappa: 0.00075031837072019351
     s: 5.4825330214673489
-    dkappa: 0
+    dkappa: 2.2837753983051797e-05
     ddkappa: 0
   }
   v: 1.9761969838257578e-14
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_nudge_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_nudge_0.pb.txt
index 28dc837a6f34906d528e6aa86e67196cd6ce88c8..1b5e70dba2f542b523d26b4f4ebb8c86e767b644 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_nudge_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_nudge_0.pb.txt
@@ -27,7 +27,7 @@ trajectory_point {
     theta: -1.8234862147701365
     kappa: -0.00049644876475473548
     s: 0.22025920112327554
-    dkappa: 0
+    dkappa: -6.2948312312929012e-05
     ddkappa: 0
   }
   v: 11.01487663647252
@@ -41,7 +41,7 @@ trajectory_point {
     theta: -1.823704563305744
     kappa: -0.000622384312505629
     s: 0.44059774063660767
-    dkappa: 0
+    dkappa: -0.00012591929890112198
     ddkappa: 0
   }
   v: 11.019043294044494
@@ -55,7 +55,7 @@ trajectory_point {
     theta: -1.8239229983623
     kappa: -0.00074836973191352006
     s: 0.66102353627628518
-    dkappa: 0
+    dkappa: -0.00018891522259007802
     ddkappa: 0
   }
   v: 11.023600464204835
@@ -69,7 +69,7 @@ trajectory_point {
     theta: -1.8241415275529862
     kappa: -0.00087440942597818256
     s: 0.88154429159032932
-    dkappa: 0
+    dkappa: -0.00025193828499200807
     ddkappa: 0
   }
   v: 11.0285374960983
@@ -83,7 +83,7 @@ trajectory_point {
     theta: -1.8243601582810025
     kappa: -0.0010005076766178276
     s: 1.1021674982808192
-    dkappa: 0
+    dkappa: -0.00031499062717525256
     ddkappa: 0
   }
   v: 11.033843855985937
@@ -97,7 +97,7 @@ trajectory_point {
     theta: -1.82457889774188
     kappa: -0.0011266686460078704
     s: 1.3229004385462177
-    dkappa: 0
+    dkappa: -0.00037807433033369968
     ddkappa: 0
   }
   v: 11.039509127245077
@@ -111,7 +111,7 @@ trajectory_point {
     theta: -1.8247977529257966
     kappa: -0.0012528963779196987
     s: 1.5437501874236974
-    dkappa: 0
+    dkappa: -0.00044119141645620281
     ddkappa: 0
   }
   v: 11.04552301036934
@@ -125,7 +125,7 @@ trajectory_point {
     theta: -1.8250167306198897
     kappa: -0.0013791947990594395
     s: 1.7647236151314656
-    dkappa: 0
+    dkappa: -0.00050434384899599892
     ddkappa: 0
   }
   v: 11.051875322968636
@@ -139,7 +139,7 @@ trajectory_point {
     theta: -1.8252358374105728
     kappa: -0.0015055677204067277
     s: 1.9858273894110916
-    dkappa: 0
+    dkappa: -0.0005675335335401262
     ddkappa: 0
   }
   v: 11.05855599976916
@@ -153,7 +153,7 @@ trajectory_point {
     theta: -1.8256469944337272
     kappa: -0.0015942053821203315
     s: 2.2070679778698308
-    dkappa: 0
+    dkappa: -0.00055264694239381
     ddkappa: 0
   }
   v: 11.065555092613401
@@ -167,7 +167,7 @@ trajectory_point {
     theta: -1.8260714607855029
     kappa: -0.0016803304936846991
     s: 2.4284516503229514
-    dkappa: 0
+    dkappa: -0.0005324418524003346
     ddkappa: 0
   }
   v: 11.07286277046013
@@ -181,7 +181,7 @@ trajectory_point {
     theta: -1.8264962135187488
     kappa: -0.0017665136324750699
     s: 2.6499844811360607
-    dkappa: 0
+    dkappa: -0.000512223149126108
     ddkappa: 0
   }
   v: 11.080469319384411
@@ -195,7 +195,7 @@ trajectory_point {
     theta: -1.8269212637571983
     kappa: -0.0018527570864832312
     s: 2.8716723515674278
-    dkappa: 0
+    dkappa: -0.00049199029580456876
     ddkappa: 0
   }
   v: 11.08836514257759
@@ -209,7 +209,7 @@ trajectory_point {
     theta: -1.8273466222582053
     kappa: -0.00193906306948747
     s: 3.0935209521103149
-    dkappa: 0
+    dkappa: -0.00047174277307976268
     ddkappa: 0
   }
   v: 11.09654076034731
@@ -223,7 +223,7 @@ trajectory_point {
     theta: -1.827772299417195
     kappa: -0.0020254337219638065
     s: 3.3155357848352978
-    dkappa: 0
+    dkappa: -0.00045148007879256523
     ddkappa: 0
   }
   v: 11.104986810117492
@@ -237,7 +237,7 @@ trajectory_point {
     theta: -1.8281983052721145
     kappa: -0.0021118711119972331
     s: 3.5377221657325935
-    dkappa: 0
+    dkappa: -0.00043120172776690369
     ddkappa: 0
   }
   v: 11.113694046428353
@@ -251,7 +251,7 @@ trajectory_point {
     theta: -1.8286246495078882
     kappa: -0.0021983772361929511
     s: 3.7600852270543874
-    dkappa: 0
+    dkappa: -0.00041090725159597958
     ddkappa: 0
   }
   v: 11.122653340936397
@@ -265,7 +265,7 @@ trajectory_point {
     theta: -1.8290513414608716
     kappa: -0.0022849540205876094
     s: 3.9826299196571573
-    dkappa: 0
+    dkappa: -0.00039059619842849076
     ddkappa: 0
   }
   v: 11.131855682414411
@@ -279,7 +279,7 @@ trajectory_point {
     theta: -1.8296104570497864
     kappa: -0.0023380835104583695
     s: 4.2053610153439989
-    dkappa: 0
+    dkappa: -0.00037227094614736636
     ddkappa: 0
   }
   v: 11.141292176751476
@@ -293,7 +293,7 @@ trajectory_point {
     theta: -1.8301810797986231
     kappa: -0.0023884599405567157
     s: 4.4282831092069541
-    dkappa: 0
+    dkappa: -0.00035409719737117392
     ddkappa: 0
   }
   v: 11.150954046952959
@@ -307,7 +307,7 @@ trajectory_point {
     theta: -1.8307522037229569
     kappa: -0.0024388805318521691
     s: 4.6514006219693327
-    dkappa: 0
+    dkappa: -0.00033590751704722549
     ddkappa: 0
   }
   v: 11.160832633140515
@@ -321,7 +321,7 @@ trajectory_point {
     theta: -1.8313238393278202
     kappa: -0.0024893462444746978
     s: 4.8747178023280417
-    dkappa: 0
+    dkappa: -0.00031770155880002783
     ddkappa: 0
   }
   v: 11.170919392552086
@@ -335,7 +335,7 @@ trajectory_point {
     theta: -1.8318959966819384
     kappa: -0.0025398580002086845
     s: 5.0982387292959119
-    dkappa: 0
+    dkappa: -0.00029947899008760115
     ddkappa: 0
   }
   v: 11.181205899541904
@@ -349,7 +349,7 @@ trajectory_point {
     theta: -1.832468685423652
     kappa: -0.00259041668302225
     s: 5.3219673145440183
-    dkappa: 0
+    dkappa: -0.00028123949201052105
     ddkappa: 0
   }
   v: 11.19168384558049
@@ -363,7 +363,7 @@ trajectory_point {
     theta: -1.8330419147668442
     kappa: -0.0026410231395965792
     s: 5.5459073047440093
-    dkappa: 0
+    dkappa: -0.00026298275912095992
     ddkappa: 0
   }
   v: 11.202345039254652
@@ -377,7 +377,7 @@ trajectory_point {
     theta: -1.8336156935068708
     kappa: -0.0026916781798552434
     s: 5.7700622839104367
-    dkappa: 0
+    dkappa: -0.00024470849923172839
     ddkappa: 0
   }
   v: 11.213181406267482
@@ -391,7 +391,7 @@ trajectory_point {
     theta: -1.8341900300264955
     kappa: -0.0027423825774935242
     s: 5.99443567574307
-    dkappa: 0
+    dkappa: -0.00022641643322531769
     ddkappa: 0
   }
   v: 11.224184989438367
@@ -405,7 +405,7 @@ trajectory_point {
     theta: -1.8348251694029811
     kappa: -0.0027616753607723711
     s: 6.2190307459692367
-    dkappa: 0
+    dkappa: -0.00021025211695331928
     ddkappa: 0
   }
   v: 11.23534794870298
@@ -419,7 +419,7 @@ trajectory_point {
     theta: -1.8354624108556836
     kappa: -0.0027802224748808956
     s: 6.4438506046861352
-    dkappa: 0
+    dkappa: -0.00019412379722843307
     ddkappa: 0
   }
   v: 11.246662561113279
@@ -433,7 +433,7 @@ trajectory_point {
     theta: -1.8361002991234676
     kappa: -0.0027987883774456242
     s: 6.668898208703169
-    dkappa: 0
+    dkappa: -0.00017797913931599804
     ddkappa: 0
   }
   v: 11.258121220837515
@@ -447,7 +447,7 @@ trajectory_point {
     theta: -1.8367388416434438
     kappa: -0.0028173732999437883
     s: 6.8941763638842675
-    dkappa: 0
+    dkappa: -0.0001618179419265426
     ddkappa: 0
   }
   v: 11.269716439160222
@@ -461,7 +461,7 @@ trajectory_point {
     theta: -1.8373780454283564
     kappa: -0.0028359774615932063
     s: 7.1196877274902137
-    dkappa: 0
+    dkappa: -0.00014564001443121506
     ddkappa: 0
   }
   v: 11.281440844482228
@@ -475,7 +475,7 @@ trajectory_point {
     theta: -1.838017917073151
     kappa: -0.0028546010695455195
     s: 7.345434810520973
-    dkappa: 0
+    dkappa: -0.00012944517669374769
     ddkappa: 0
   }
   v: 11.293287182320642
@@ -489,7 +489,7 @@ trajectory_point {
     theta: -1.8386584627615525
     kappa: -0.0028732443190794305
     s: 7.5714199800580131
-    dkappa: 0
+    dkappa: -0.00011323325890242119
     ddkappa: 0
   }
   v: 11.305248315308869
@@ -503,7 +503,7 @@ trajectory_point {
     theta: -1.8392996882726433
     kappa: -0.0028919073937939367
     s: 7.7976454616066322
-    dkappa: 0
+    dkappa: -9.7004101402029007e-05
     ddkappa: 0
   }
   v: 11.317317223196596
@@ -517,7 +517,7 @@ trajectory_point {
     theta: -1.83994325201406
     kappa: -0.0029075876242923549
     s: 8.024113341438289
-    dkappa: 0
+    dkappa: -8.0996512361176565e-05
     ddkappa: 0
   }
   v: 11.3294870028498
@@ -531,7 +531,7 @@ trajectory_point {
     theta: -1.8406013057124624
     kappa: -0.0028982246173463052
     s: 8.2508255689329175
-    dkappa: 0
+    dkappa: -6.6965869907255408e-05
     ddkappa: 0
   }
   v: 11.341750868250749
@@ -545,7 +545,7 @@ trajectory_point {
     theta: -1.8412600758070765
     kappa: -0.0028888514441151635
     s: 8.4777839589212682
-    dkappa: 0
+    dkappa: -5.2919993083432165e-05
     ddkappa: 0
   }
   v: 11.354102150497996
@@ -559,7 +559,7 @@ trajectory_point {
     theta: -1.8419195666153372
     kappa: -0.0028794680351081079
     s: 8.70499019402722
-    dkappa: 0
+    dkappa: -3.8858777756399634e-05
     ddkappa: 0
   }
   v: 11.366534297806382
@@ -573,7 +573,7 @@ trajectory_point {
     theta: -1.8425797820740197
     kappa: -0.0028700743261976022
     s: 8.9324458270101132
-    dkappa: 0
+    dkappa: -2.4782127829836205e-05
     ddkappa: 0
   }
   v: 11.379040875507037
@@ -587,7 +587,7 @@ trajectory_point {
     theta: -1.8432407257459886
     kappa: -0.0028606702585226608
     s: 9.1601522831070739
-    dkappa: 0
+    dkappa: -1.068995509944545e-05
     ddkappa: 0
   }
   v: 11.391615566047379
@@ -601,7 +601,7 @@ trajectory_point {
     theta: -1.8439024008269516
     kappa: -0.0028512557783921134
     s: 9.3881108623753384
-    dkappa: 0
+    dkappa: 3.4178208920045262e-06
     ddkappa: 0
   }
   v: 11.404252168991118
@@ -615,7 +615,7 @@ trajectory_point {
     theta: -1.8445648101522185
     kappa: -0.0028418308371878673
     s: 9.6163227420345816
-    dkappa: 0
+    dkappa: 1.7541272999641672e-05
     ddkappa: 0
   }
   v: 11.416944601018244
@@ -629,7 +629,7 @@ trajectory_point {
     theta: -1.8452279562034646
     kappa: -0.0028323953912681734
     s: 9.8447889788092411
-    dkappa: 0
+    dkappa: 3.1680466621450567e-05
     ddkappa: 0
   }
   v: 11.429686895925041
@@ -643,7 +643,7 @@ trajectory_point {
     theta: -1.8458757136800228
     kappa: -0.0028153266409944922
     s: 10.07351051127084
-    dkappa: 0
+    dkappa: 4.5109266658912336e-05
     ddkappa: 0
   }
   v: 11.442473204624081
@@ -657,7 +657,7 @@ trajectory_point {
     theta: -1.8464899630560874
     kappa: -0.0027820579128563822
     s: 10.302488162180319
-    dkappa: 0
+    dkappa: 5.7011610900345718e-05
     ddkappa: 0
   }
   v: 11.455297795144222
@@ -671,7 +671,7 @@ trajectory_point {
     theta: -1.8471049028066764
     kappa: -0.00274875186958186
     s: 10.531722640830358
-    dkappa: 0
+    dkappa: 6.89273051441341e-05
     ddkappa: 0
   }
   v: 11.468155052630612
@@ -685,7 +685,7 @@ trajectory_point {
     theta: -1.8477205340753242
     kappa: -0.0027154084242618027
     s: 10.761214545387704
-    dkappa: 0
+    dkappa: 8.0856380483209568e-05
     ddkappa: 0
   }
   v: 11.481039479344686
@@ -699,7 +699,7 @@ trajectory_point {
     theta: -1.8483368577106156
     kappa: -0.0026820275057925259
     s: 10.99096436523549
-    dkappa: 0
+    dkappa: 9.2798862355891084e-05
     ddkappa: 0
   }
   v: 11.493945694664168
@@ -713,7 +713,7 @@ trajectory_point {
     theta: -1.8489538742724514
     kappa: -0.0026486090585354635
     s: 11.220972483315572
-    dkappa: 0
+    dkappa: 0.00010475477066764004
     ddkappa: 0
   }
   v: 11.506868435083069
@@ -727,7 +727,7 @@ trajectory_point {
     theta: -1.8519190362039264
     kappa: -0.0024494729366002555
     s: 12.374893794408612
-    dkappa: 0
+    dkappa: 0.00016102474848725696
     ddkappa: 0
   }
   v: 11.571553989099439
@@ -741,7 +741,7 @@ trajectory_point {
     theta: -1.8546275726069925
     kappa: -0.0021832226275765843
     s: 13.535272897919803
-    dkappa: 0
+    dkappa: 0.00020982871045717181
     ddkappa: 0
   }
   v: 11.63593048656873
@@ -755,7 +755,7 @@ trajectory_point {
     theta: -1.8569613465932615
     kappa: -0.001870406939134435
     s: 14.702051969092555
-    dkappa: 0
+    dkappa: 0.00025198492475567765
     ddkappa: 0
   }
   v: 11.699471745066578
@@ -769,7 +769,7 @@ trajectory_point {
     theta: -1.8590466640683807
     kappa: -0.0015256947883198555
     s: 15.875124224811843
-    dkappa: 0
+    dkappa: 0.00028973504741367331
     ddkappa: 0
   }
   v: 11.761724779848718
@@ -783,7 +783,7 @@ trajectory_point {
     theta: -1.8604743375927417
     kappa: -0.0011320957550884309
     s: 17.054341243372214
-    dkappa: 0
+    dkappa: 0.00031737608460008938
     ddkappa: 0
   }
   v: 11.822309803850983
@@ -797,7 +797,7 @@ trajectory_point {
     theta: -1.8616683601510402
     kappa: -0.0007247953234261798
     s: 18.23952028424582
-    dkappa: 0
+    dkappa: 0.00034161366873338751
     ddkappa: 0
   }
   v: 11.880920227689304
@@ -811,7 +811,7 @@ trajectory_point {
     theta: -1.8622213055798456
     kappa: -0.00029132041640758904
     s: 19.43045160785039
-    dkappa: 0
+    dkappa: 0.00035668837890871362
     ddkappa: 0
   }
   v: 11.937322659659714
@@ -825,7 +825,7 @@ trajectory_point {
     theta: -1.8623011200223791
     kappa: 0.00014789333714651114
     s: 20.626905795317281
-    dkappa: 0
+    dkappa: 0.00036584237619481088
     ddkappa: 0
   }
   v: 11.991356905738337
@@ -839,7 +839,7 @@ trajectory_point {
     theta: -1.8619376745142793
     kappa: 0.00059252291739025572
     s: 21.828641068259454
-    dkappa: 0
+    dkappa: 0.00036945019140394755
     ddkappa: 0
   }
   v: 12.042935969581407
@@ -853,7 +853,7 @@ trajectory_point {
     theta: -1.8608516852763379
     kappa: 0.0010253712159461563
     s: 23.0354106085395
-    dkappa: 0
+    dkappa: 0.0003631737140631774
     ddkappa: 0
   }
   v: 12.092046052525244
@@ -867,7 +867,7 @@ trajectory_point {
     theta: -1.8594658761797807
     kappa: 0.0014498085781763691
     s: 24.246968540666259
-    dkappa: 0
+    dkappa: 0.00035284267324422845
     ddkappa: 0
   }
   v: 12.138692725502155
@@ -881,7 +881,7 @@ trajectory_point {
     theta: -1.857377392676141
     kappa: 0.0018438997283229805
     s: 25.463062007312168
-    dkappa: 0
+    dkappa: 0.00033301078660355195
     ddkappa: 0
   }
   v: 12.182726115655498
@@ -895,7 +895,7 @@ trajectory_point {
     theta: -1.8548975827709202
     kappa: 0.0022043649378725356
     s: 26.683421045523755
-    dkappa: 0
+    dkappa: 0.00030657089767777016
     ddkappa: 0
   }
   v: 12.223979064610097
@@ -909,7 +909,7 @@ trajectory_point {
     theta: -1.8520949871431074
     kappa: 0.0025373164301344081
     s: 27.907760637024158
-    dkappa: 0
+    dkappa: 0.00027468647606131218
     ddkappa: 0
   }
   v: 12.262317624273217
@@ -923,7 +923,7 @@ trajectory_point {
     theta: -1.84871863551986
     kappa: 0.0027906951960262241
     s: 29.135784029241361
-    dkappa: 0
+    dkappa: 0.00023167768776656818
     ddkappa: 0
   }
   v: 12.297641056834562
@@ -937,7 +937,7 @@ trajectory_point {
     theta: -1.8451792884829663
     kappa: 0.0030048747222533633
     s: 30.367186056336436
-    dkappa: 0
+    dkappa: 0.00018402313446575422
     ddkappa: 0
   }
   v: 12.329881834766285
@@ -951,7 +951,7 @@ trajectory_point {
     theta: -1.8413803753864708
     kappa: 0.0031389873136512215
     s: 31.601656460231805
-    dkappa: 0
+    dkappa: 0.00012754548145532164
     ddkappa: 0
   }
   v: 12.359005640822977
@@ -965,7 +965,7 @@ trajectory_point {
     theta: -1.8374604605320428
     kappa: 0.0031816533336327234
     s: 32.838883211639455
-    dkappa: 0
+    dkappa: 6.2778410019594314e-05
     ddkappa: 0
   }
   v: 12.38501136804167
@@ -979,7 +979,7 @@ trajectory_point {
     theta: -1.8334846807206524
     kappa: 0.0031700225658972485
     s: 34.078555831089211
-    dkappa: 0
+    dkappa: -6.8423876657653844e-06
     ddkappa: 0
   }
   v: 12.407931119741846
@@ -993,7 +993,7 @@ trajectory_point {
     theta: -1.8296594871725749
     kappa: 0.0030038760972861331
     s: 35.320368709956959
-    dkappa: 0
+    dkappa: -8.82213662068853e-05
     ddkappa: 0
   }
   v: 12.427830209525421
@@ -1007,7 +1007,7 @@ trajectory_point {
     theta: -1.8260707343906204
     kappa: 0.0027548437394539381
     s: 36.564024431492911
-    dkappa: 0
+    dkappa: -0.00017510969634949202
     ddkappa: 0
   }
   v: 12.444807161276762
@@ -1021,7 +1021,7 @@ trajectory_point {
     theta: -1.8227849651330563
     kappa: 0.0024005451882407641
     s: 37.809237091849816
-    dkappa: 0
+    dkappa: -0.00026895046803019322
     ddkappa: 0
   }
   v: 12.458993709162673
@@ -1035,7 +1035,7 @@ trajectory_point {
     theta: -1.8202338749267086
     kappa: 0.0018688875139165862
     s: 39.055735621111261
-    dkappa: 0
+    dkappa: -0.00037301454098328537
     ddkappa: 0
   }
   v: 12.470554797632401
@@ -1049,7 +1049,7 @@ trajectory_point {
     theta: -1.8181496509098034
     kappa: 0.0012470951178828395
     s: 40.303267104319836
-    dkappa: 0
+    dkappa: -0.00048198688544883866
     ddkappa: 0
   }
   v: 12.479688581417637
@@ -1063,7 +1063,7 @@ trajectory_point {
     theta: -1.81714250908845
     kappa: 0.00044252214168672159
     s: 41.551600102505461
-    dkappa: 0
+    dkappa: -0.00060042716616377518
     ddkappa: 0
   }
   v: 12.486626425532517
@@ -1077,7 +1077,7 @@ trajectory_point {
     theta: -1.8166945896361297
     kappa: 9.679007318500855e-05
     s: 42.800527973713571
-    dkappa: 0
+    dkappa: -0.00041493252455920019
     ddkappa: 0
   }
   v: 12.491632905273617
@@ -1091,7 +1091,7 @@ trajectory_point {
     theta: -1.8165825375715117
     kappa: 2.3682918878481552e-05
     s: 44.049872194033384
-    dkappa: 0
+    dkappa: -5.8674738482272879e-05
     ddkappa: 0
   }
   v: 12.495005806219954
@@ -1105,7 +1105,7 @@ trajectory_point {
     theta: -1.8166253044843887
     kappa: -3.0212855660321292e-05
     s: 45.299485678626141
-    dkappa: 0
+    dkappa: -4.8798224543826257e-05
     ddkappa: 0
   }
   v: 12.49707612423299
@@ -1119,7 +1119,7 @@ trajectory_point {
     theta: -1.8166928210511049
     kappa: -7.6493809130077692e-05
     s: 46.549256102753354
-    dkappa: 0
+    dkappa: -3.9319495622772546e-05
     ddkappa: 0
   }
   v: 12.49820806545663
@@ -1133,7 +1133,7 @@ trajectory_point {
     theta: -1.8167936636194471
     kappa: -0.000112514320153464
     s: 47.79910922280505
-    dkappa: 0
+    dkappa: -3.0377481361264671e-05
     ddkappa: 0
   }
   v: 12.498799046317224
@@ -1147,7 +1147,7 @@ trajectory_point {
     theta: -1.8169662188647018
     kappa: -0.00013531763192951932
     s: 49.049010367839024
-    dkappa: 0
+    dkappa: -2.2211088673033717e-05
     ddkappa: 0
   }
   v: 12.499206230015211
@@ -1161,7 +1161,7 @@ trajectory_point {
     theta: -1.8171527297202348
     kappa: -0.00015214618566646325
     s: 50.298947181911721
-    dkappa: 0
+    dkappa: -1.4419915506419678e-05
     ddkappa: 0
   }
   v: 12.499514778035307
@@ -1175,7 +1175,7 @@ trajectory_point {
     theta: -1.8173352080200162
     kappa: -0.00015803248341101148
     s: 51.5489104923449
-    dkappa: 0
+    dkappa: -7.3556762580267567e-06
     ddkappa: 0
   }
   v: 12.499738364233139
@@ -1189,7 +1189,7 @@ trajectory_point {
     theta: -1.817536176225141
     kappa: -0.00015625694688253111
     s: 52.798892450665285
-    dkappa: 0
+    dkappa: -8.785662195686686e-07
     ddkappa: 0
   }
   v: 12.499889798852404
@@ -1203,7 +1203,7 @@ trajectory_point {
     theta: -1.8177468731673931
     kappa: -0.0001496299553145576
     s: 54.048886446243387
-    dkappa: 0
+    dkappa: 5.22314440072503e-06
     ddkappa: 0
   }
   v: 12.499981028524864
@@ -1217,7 +1217,7 @@ trajectory_point {
     theta: -1.8179054517978004
     kappa: -0.00013289088866752564
     s: 55.298887019932259
-    dkappa: 0
+    dkappa: 1.04094467790985e-05
     ddkappa: 0
   }
   v: 12.500023136270345
@@ -1231,7 +1231,7 @@ trajectory_point {
     theta: -1.8180462084950786
     kappa: -0.00011254078435504619
     s: 56.548889777706378
-    dkappa: 0
+    dkappa: 1.5196577235561753e-05
     ddkappa: 0
   }
   v: 12.500026341496737
@@ -1245,7 +1245,7 @@ trajectory_point {
     theta: -1.8181628589288563
     kappa: -8.7306489719343955e-05
     s: 57.79889130430039
-    dkappa: 0
+    dkappa: 1.9443775158046743e-05
     ddkappa: 0
   }
   v: 12.5
@@ -1259,7 +1259,7 @@ trajectory_point {
     theta: -1.8182562301355567
     kappa: -5.66131500640465e-05
     s: 59.048889076847964
-    dkappa: 0
+    dkappa: 2.2916784689630316e-05
     ddkappa: 0
   }
   v: 12.499952603964154
@@ -1273,7 +1273,7 @@ trajectory_point {
     theta: -1.8183227124951087
     kappa: -2.3696228165825617e-05
     s: 60.298881378520548
-    dkappa: 0
+    dkappa: 2.6014137063072452e-05
     ddkappa: 0
   }
   v: 12.49989178196129
@@ -1287,7 +1287,7 @@ trajectory_point {
     theta: -1.8183135091790317
     kappa: 1.289319057333186e-05
     s: 61.548867212166215
-    dkappa: 0
+    dkappa: 2.8382733650398571e-05
     ddkappa: 0
   }
   v: 12.499824298951561
@@ -1301,7 +1301,7 @@ trajectory_point {
     theta: -1.8182743607343193
     kappa: 5.12699782143577e-05
     s: 62.798846213948465
-    dkappa: 0
+    dkappa: 3.0165586821550422e-05
     ddkappa: 0
   }
   v: 12.499756056283184
@@ -1315,7 +1315,7 @@ trajectory_point {
     theta: -1.8182145659160438
     kappa: 9.0744880034336633e-05
     s: 64.048818566985
-    dkappa: 0
+    dkappa: 3.1573413666342185e-05
     ddkappa: 0
   }
   v: 12.499692091692447
@@ -1329,7 +1329,7 @@ trajectory_point {
     theta: -1.81804911309516
     kappa: 0.00013117712115171221
     s: 65.298784914986584
-    dkappa: 0
+    dkappa: 3.2064134282668647e-05
     ddkappa: 0
   }
   v: 12.499636579303699
@@ -1343,7 +1343,7 @@ trajectory_point {
     theta: -1.8178613290646826
     kappa: 0.00017123004851534362
     s: 66.54874627589578
-    dkappa: 0
+    dkappa: 3.2157101458344937e-05
     ddkappa: 0
   }
   v: 12.499592829629355
@@ -1357,7 +1357,7 @@ trajectory_point {
     theta: -1.8176432395388111
     kappa: 0.00021076831923623918
     s: 67.798703955525838
-    dkappa: 0
+    dkappa: 3.171028936015203e-05
     ddkappa: 0
   }
   v: 12.499563289569899
@@ -1371,7 +1371,7 @@ trajectory_point {
     theta: -1.8173394771793576
     kappa: 0.00024802051530552172
     s: 69.048659461199449
-    dkappa: 0
+    dkappa: 3.0488805943874813e-05
     ddkappa: 0
   }
   v: 12.499549542413872
@@ -1385,7 +1385,7 @@ trajectory_point {
     theta: -1.8170079556774437
     kappa: 0.00028374387592068493
     s: 70.2986144153875
-    dkappa: 0
+    dkappa: 2.889203382531567e-05
     ddkappa: 0
   }
   v: 12.499552307837892
@@ -1399,7 +1399,7 @@ trajectory_point {
     theta: -1.8166435977316386
     kappa: 0.00031585855101560144
     s: 71.548570469348078
-    dkappa: 0
+    dkappa: 2.6567432723688782e-05
     ddkappa: 0
   }
   v: 12.499571441906632
@@ -1413,7 +1413,7 @@ trajectory_point {
     theta: -1.81622647691825
     kappa: 0.00034390092272891981
     s: 72.798529216764976
-    dkappa: 0
+    dkappa: 2.3656672189725809e-05
     ddkappa: 0
   }
   v: 12.499605937072838
@@ -1427,7 +1427,7 @@ trajectory_point {
     theta: -1.8157771214892646
     kappa: 0.00036928894810314087
     s: 74.0484921073868
-    dkappa: 0
+    dkappa: 2.037020878435677e-05
     ddkappa: 0
   }
   v: 12.499653922177318
@@ -1441,7 +1441,7 @@ trajectory_point {
     theta: -1.8153112181271902
     kappa: 0.0003864680121222277
     s: 75.298460360665572
-    dkappa: 0
+    dkappa: 1.6167350414081215e-05
     ddkappa: 0
   }
   v: 12.499712662448944
@@ -1455,7 +1455,7 @@ trajectory_point {
     theta: -1.8148240552294366
     kappa: 0.00039929306406519564
     s: 76.548434879395629
-    dkappa: 0
+    dkappa: 1.1566778287404028e-05
     ddkappa: 0
   }
   v: 12.499778559504659
@@ -1469,7 +1469,7 @@ trajectory_point {
     theta: -1.8143079302946636
     kappa: 0.00040618646179716383
     s: 77.798416163352371
-    dkappa: 0
+    dkappa: 6.4243921186903711e-06
     ddkappa: 0
   }
   v: 12.499847151349465
@@ -1483,7 +1483,7 @@ trajectory_point {
     theta: -1.8137904849423998
     kappa: 0.0004030675782625179
     s: 79.048404222931154
-    dkappa: 0
+    dkappa: 5.089903098073537e-07
     ddkappa: 0
   }
   v: 12.499913112376433
@@ -1497,7 +1497,7 @@ trajectory_point {
     theta: -1.8132837439745744
     kappa: 0.00039466572242980184
     s: 80.298398492786
-    dkappa: 0
+    dkappa: -5.7819762128435224e-06
     ddkappa: 0
   }
   v: 12.4999702533667
@@ -1511,7 +1511,7 @@ trajectory_point {
     theta: -1.8128152946936016
     kappa: 0.0003753528425904782
     s: 81.548397745468463
-    dkappa: 0
+    dkappa: -1.2802692048079716e-05
     ddkappa: 0
   }
   v: 12.500011521489466
@@ -1525,7 +1525,7 @@ trajectory_point {
     theta: -1.8123652449143266
     kappa: 0.00034612382951201343
     s: 82.798400005066441
-    dkappa: 0
+    dkappa: -2.0407845395553545e-05
     ddkappa: 0
   }
   v: 12.500029000301998
@@ -1539,7 +1539,7 @@ trajectory_point {
     theta: -1.8119288857011828
     kappa: 0.00031048681673774217
     s: 84.048402460842922
-    dkappa: 0
+    dkappa: -2.8388197110284207e-05
     ddkappa: 0
   }
   v: 12.500013909749628
@@ -1553,7 +1553,7 @@ trajectory_point {
     theta: -1.8116022106451835
     kappa: 0.00025745434845312725
     s: 85.298401380874878
-    dkappa: 0
+    dkappa: -3.7287602834280105e-05
     ddkappa: 0
   }
   v: 12.499956606165755
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 e43801fd10f9355588e0b99dbc77e6f3819da46c..950c0028d65f0c3a652d29d53ed24bff334b18b4 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
@@ -27,7 +27,7 @@ trajectory_point {
     theta: 2.6094684755889572
     kappa: -0.023591673568376541
     s: 0.097518691100866223
-    dkappa: 0
+    dkappa: -0.00057808056096762448
     ddkappa: 0
   }
   v: 4.8694799046978936
@@ -41,7 +41,7 @@ trajectory_point {
     theta: 2.6056199916454861
     kappa: -0.02475869672915176
     s: 0.19477515187637245
-    dkappa: 0
+    dkappa: -0.0011546066481018171
     ddkappa: 0
   }
   v: 4.8560684985770521
@@ -55,7 +55,7 @@ trajectory_point {
     theta: 2.6017813926751674
     kappa: -0.02592243261177991
     s: 0.29175766004415515
-    dkappa: 0
+    dkappa: -0.0017295087729436163
     ddkappa: 0
   }
   v: 4.8420914196658682
@@ -69,7 +69,7 @@ trajectory_point {
     theta: 2.5979532310179256
     kappa: -0.02708275031203719
     s: 0.38845530641068993
-    dkappa: 0
+    dkappa: -0.0023027222669393214
     ddkappa: 0
   }
   v: 4.8275889800857295
@@ -83,7 +83,7 @@ trajectory_point {
     theta: 2.5941360258785022
     kappa: -0.028239528518005097
     s: 0.48485798117846884
-    dkappa: 0
+    dkappa: -0.002874187200270877
     ddkappa: 0
   }
   v: 4.81260080731689
@@ -97,7 +97,7 @@ trajectory_point {
     theta: 2.5903302637083994
     kappa: -0.029392655345764247
     s: 0.58095636025317754
-    dkappa: 0
+    dkappa: -0.0034438483006862574
     ddkappa: 0
   }
   v: 4.7971658441984726
@@ -111,7 +111,7 @@ trajectory_point {
     theta: 2.586536398604883
     kappa: -0.030542028175088123
     s: 0.67674189155087294
-    dkappa: 0
+    dkappa: -0.0040116548723298532
     ddkappa: 0
   }
   v: 4.7813223489284624
@@ -125,7 +125,7 @@ trajectory_point {
     theta: 2.5827548527265933
     kappa: -0.031687553485136868
     s: 0.77220678130516018
-    dkappa: 0
+    dkappa: -0.0045775607145728554
     ddkappa: 0
   }
   v: 4.7651078950637116
@@ -139,7 +139,7 @@ trajectory_point {
     theta: 2.5789860167252829
     kappa: -0.0328291466901511
     s: 0.86734398037437033
-    dkappa: 0
+    dkappa: -0.0051415240408436381
     ddkappa: 0
   }
   v: 4.7485593715199395
@@ -153,7 +153,7 @@ trajectory_point {
     theta: 2.5752302501931652
     kappa: -0.033966731975145631
     s: 0.96214717054873722
-    dkappa: 0
+    dkappa: -0.0057035073974581489
     ddkappa: 0
   }
   v: 4.7317129825717306
@@ -167,7 +167,7 @@ trajectory_point {
     theta: 2.571487882125342
     kappa: -0.0351002421316033
     s: 1.0566107508575753
-    dkappa: 0
+    dkappa: -0.0062634775824502876
     ddkappa: 0
   }
   v: 4.7146042478525354
@@ -181,7 +181,7 @@ trajectory_point {
     theta: 2.567759211396754
     kappa: -0.036229618393168753
     s: 1.1507298238764565
-    dkappa: 0
+    dkappa: -0.0068214055644022975
     ddkappa: 0
   }
   v: 4.6972680023546713
@@ -195,7 +195,7 @@ trajectory_point {
     theta: 2.5640445072530857
     kappa: -0.0373548102713422
     s: 1.2445001820343882
-    dkappa: 0
+    dkappa: -0.0073772664012751454
     ddkappa: 0
   }
   v: 4.67973839642932
@@ -209,7 +209,7 @@ trajectory_point {
     theta: 2.560344009815033
     kappa: -0.038475775391173195
     s: 1.337918293920989
-    dkappa: 0
+    dkappa: -0.0079310391592389042
     ddkappa: 0
   }
   v: 4.6620488957865316
@@ -223,7 +223,7 @@ trajectory_point {
     theta: 2.556657930595347
     kappa: -0.039592479326954456
     s: 1.4309812905936685
-    dkappa: 0
+    dkappa: -0.0084827068315031472
     ddkappa: 0
   }
   v: 4.6442322814952188
@@ -237,7 +237,7 @@ trajectory_point {
     theta: 2.5529864530280464
     kappa: -0.040704895437915618
     s: 1.5236869518848022
-    dkappa: 0
+    dkappa: -0.0090322562571473267
     ddkappa: 0
   }
   v: 4.6263206499831639
@@ -251,7 +251,7 @@ trajectory_point {
     theta: 2.5493297330091949
     kappa: -0.041813004703917023
     s: 1.6160336927089105
-    dkappa: 0
+    dkappa: -0.0095796780399511582
     ddkappa: 0
   }
   v: 4.6083454130370125
@@ -265,7 +265,7 @@ trajectory_point {
     theta: 2.5456878994486365
     kappa: -0.042916795561143493
     s: 1.7080205493698348
-    dkappa: 0
+    dkappa: -0.010124966467225006
     ddkappa: 0
   }
   v: 4.5903372978022787
@@ -279,7 +279,7 @@ trajectory_point {
     theta: 2.5420610548320837
     kappa: -0.044016263737798135
     s: 1.7996471658679158
-    dkappa: 0
+    dkappa: -0.010668119428640275
     ddkappa: 0
   }
   v: 4.57232634678334
@@ -293,7 +293,7 @@ trajectory_point {
     theta: 2.5384492757929542
     kappa: -0.0451114120897961
     s: 1.8909137802071712
-    dkappa: 0
+    dkappa: -0.011209138335059782
     ddkappa: 0
   }
   v: 4.5543419178434421
@@ -307,7 +307,7 @@ trajectory_point {
     theta: 2.5348526136933591
     kappa: -0.04620225043645837
     s: 1.9818212107024706
-    dkappa: 0
+    dkappa: -0.011748028037368156
     ddkappa: 0
   }
   v: 4.536412684204695
@@ -321,7 +321,7 @@ trajectory_point {
     theta: 2.5306804858724465
     kappa: -0.047216970695829606
     s: 2.0723708422867162
-    dkappa: 0
+    dkappa: -0.011964034194039967
     ddkappa: 0
   }
   v: 4.518566634448077
@@ -335,7 +335,7 @@ trajectory_point {
     theta: 2.5260050251083142
     kappa: -0.048164670576236392
     s: 2.162564612818016
-    dkappa: 0
+    dkappa: -0.011897693980147993
     ddkappa: 0
   }
   v: 4.5008310725134288
@@ -349,7 +349,7 @@ trajectory_point {
     theta: 2.5213459038819472
     kappa: -0.049108657318396733
     s: 2.2524049993868669
-    dkappa: 0
+    dkappa: -0.011831613690722165
     ddkappa: 0
   }
   v: 4.4832326176994615
@@ -363,7 +363,7 @@ trajectory_point {
     theta: 2.5167031896516776
     kappa: -0.050048962472074313
     s: 2.3418950046233253
-    dkappa: 0
+    dkappa: -0.011765791117238333
     ddkappa: 0
   }
   v: 4.4657972046637475
@@ -377,7 +377,7 @@ trajectory_point {
     theta: 2.5120769187549552
     kappa: -0.0509856229651991
     s: 2.4310381430041903
-    dkappa: 0
+    dkappa: -0.011700223674693772
     ddkappa: 0
   }
   v: 4.44855008342273
@@ -391,7 +391,7 @@ trajectory_point {
     theta: 2.5074670971301489
     kappa: -0.051918680959991685
     s: 2.519838427160177
-    dkappa: 0
+    dkappa: -0.011634908411678666
     ddkappa: 0
   }
   v: 4.4315158193517146
@@ -405,7 +405,7 @@ trajectory_point {
     theta: 2.50287370105803
     kappa: -0.052848183709087657
     s: 2.608300354183096
-    dkappa: 0
+    dkappa: -0.011569842020447578
     ddkappa: 0
   }
   v: 4.4147182931848743
@@ -419,7 +419,7 @@ trajectory_point {
     theta: 2.4982966779218136
     kappa: -0.053774183411661937
     s: 2.6964288919330297
-    dkappa: 0
+    dkappa: -0.011505020846990942
     ddkappa: 0
   }
   v: 4.3981807010152485
@@ -433,7 +433,7 @@ trajectory_point {
     theta: 2.4937359469846636
     kappa: -0.054696737069553185
     s: 2.7842294653455095
-    dkappa: 0
+    dkappa: -0.01144044090110654
     ddkappa: 0
   }
   v: 4.3819255542947415
@@ -447,7 +447,7 @@ trajectory_point {
     theta: 2.4891914001835613
     kappa: -0.055615906343388116
     s: 2.8717079427386949
-    dkappa: 0
+    dkappa: -0.01137609786647097
     ddkappa: 0
   }
   v: 4.3659746798341246
@@ -461,7 +461,7 @@ trajectory_point {
     theta: 2.4846629029384815
     kappa: -0.056531757408705884
     s: 2.9588706221205472
-    dkappa: 0
+    dkappa: -0.011311987110711148
     ddkappa: 0
   }
   v: 4.3503492198030349
@@ -475,7 +475,7 @@ trajectory_point {
     theta: 2.4801502949758172
     kappa: -0.057444360812082429
     s: 3.0457242174960117
-    dkappa: 0
+    dkappa: -0.011248103695475775
     ddkappa: 0
   }
   v: 4.3350696317299748
@@ -489,7 +489,7 @@ trajectory_point {
     theta: 2.4756533911650394
     kappa: -0.05835379132725485
     s: 3.1322758451741919
-    dkappa: 0
+    dkappa: -0.011184442386506812
     ddkappa: 0
   }
   v: 4.3201556885023136
@@ -503,7 +503,7 @@ trajectory_point {
     theta: 2.4711719823675877
     kappa: -0.059260127811245768
     s: 3.2185330100755247
-    dkappa: 0
+    dkappa: -0.011120997663710976
     ddkappa: 0
   }
   v: 4.3056264783662872
@@ -517,7 +517,7 @@ trajectory_point {
     theta: 2.4667058362970238
     kappa: -0.060163453060487669
     s: 3.304503592038964
-    dkappa: 0
+    dkappa: -0.011057763731231209
     ddkappa: 0
   }
   v: 4.2915004049269951
@@ -531,7 +531,7 @@ trajectory_point {
     theta: 2.4622546983895095
     kappa: -0.061063853666947276
     s: 3.3901958321291521
-    dkappa: 0
+    dkappa: -0.010994734527518158
     ddkappa: 0
   }
   v: 4.2777951871484055
@@ -545,7 +545,7 @@ trajectory_point {
     theta: 2.4578182926836876
     kappa: -0.061961419874249891
     s: 3.4756183189435994
-    dkappa: 0
+    dkappa: -0.010931903735401668
     ddkappa: 0
   }
   v: 4.26452785935335
@@ -559,7 +559,7 @@ trajectory_point {
     theta: 2.4533963227090951
     kappa: -0.062856245433803831
     s: 3.5607799749198636
-    dkappa: 0
+    dkappa: -0.01086926479216224
     ddkappa: 0
   }
   v: 4.2517147712235293
@@ -573,7 +573,7 @@ trajectory_point {
     theta: 2.4489884723822497
     kappa: -0.063748427460924642
     s: 3.6456900426427219
-    dkappa: 0
+    dkappa: -0.010806810899602538
     ddkappa: 0
   }
   v: 4.239371587799508
@@ -587,7 +587,7 @@ trajectory_point {
     theta: 2.4445944069095922
     kappa: -0.064638066290959625
     s: 3.7303580711513549
-    dkappa: 0
+    dkappa: -0.010744535034118846
     ddkappa: 0
   }
   v: 4.2275132894807168
@@ -601,7 +601,7 @@ trajectory_point {
     theta: 2.4402137736964931
     kappa: -0.065525265335412086
     s: 3.8147939022465169
-    dkappa: 0
+    dkappa: -0.010682429956772559
     ddkappa: 0
   }
   v: 4.216154172025453
@@ -615,7 +615,7 @@ trajectory_point {
     theta: 2.4358462032615691
     kappa: -0.0664101309380657
     s: 3.8990076567977203
-    dkappa: 0
+    dkappa: -0.010620488223361669
     ddkappa: 0
   }
   v: 4.2053078465508786
@@ -629,7 +629,7 @@ trajectory_point {
     theta: 2.4314913101555762
     kappa: -0.06729277223110898
     s: 3.9830097210504061
-    dkappa: 0
+    dkappa: -0.010558702194492231
     ddkappa: 0
   }
   v: 4.1949872395330248
@@ -643,7 +643,7 @@ trajectory_point {
     theta: 2.4268029660907997
     kappa: -0.068145885302715259
     s: 4.0668107329331269
-    dkappa: 0
+    dkappa: -0.01049396769063553
     ddkappa: 0
   }
   v: 4.1852045928067856
@@ -657,7 +657,7 @@ trajectory_point {
     theta: 2.4204055946597385
     kappa: -0.0688609181797871
     s: 4.1504215683647221
-    dkappa: 0
+    dkappa: -0.010414003838286565
     ddkappa: 0
   }
   v: 4.1759714635659222
@@ -671,7 +671,7 @@ trajectory_point {
     theta: 2.4140161148806873
     kappa: -0.0695744196118645
     s: 4.2338533275614942
-    dkappa: 0
+    dkappa: -0.010334211251127415
     ddkappa: 0
   }
   v: 4.1672987243630626
@@ -685,7 +685,7 @@ trajectory_point {
     theta: 2.4076342096553369
     kappa: -0.070286486328142925
     s: 4.3171173213443845
-    dkappa: 0
+    dkappa: -0.010254579111698699
     ddkappa: 0
   }
   v: 4.1591965631096981
@@ -699,7 +699,7 @@ trajectory_point {
     theta: 2.4012595402088048
     kappa: -0.070997216741791214
     s: 4.4002250574461561
-    dkappa: 0
+    dkappa: -0.010175096414218229
     ddkappa: 0
   }
   v: 4.15167448307619
@@ -713,7 +713,7 @@ trajectory_point {
     theta: 2.3948917475426388
     kappa: -0.071706710832851767
     s: 4.4831882268185677
-    dkappa: 0
+    dkappa: -0.010095751977676566
     ddkappa: 0
   }
   v: 4.1447413028917612
@@ -727,7 +727,7 @@ trajectory_point {
     theta: 2.3631421431412738
     kappa: -0.075239230677385929
     s: 4.8962558560857987
-    dkappa: 0
+    dkappa: -0.0097007017578532636
     ddkappa: 0
   }
   v: 4.1191696268493345
@@ -741,7 +741,7 @@ trajectory_point {
     theta: 2.3315016185489719
     kappa: -0.078756535491090143
     s: 5.3075443483656253
-    dkappa: 0
+    dkappa: -0.0093073530716486011
     ddkappa: 0
   }
   v: 4.1092495727656635
@@ -755,7 +755,7 @@ trajectory_point {
     theta: 2.2999117210997739
     kappa: -0.082272182912745587
     s: 5.7186390370620366
-    dkappa: 0
+    dkappa: -0.00891418973559681
     ddkappa: 0
   }
   v: 4.1153132480181647
@@ -769,7 +769,7 @@ trajectory_point {
     theta: 2.2679399123292288
     kappa: -0.085675993252932015
     s: 6.13113707128132
-    dkappa: 0
+    dkappa: -0.0084916371952549856
     ddkappa: 0
   }
   v: 4.1372648592755148
@@ -783,7 +783,7 @@ trajectory_point {
     theta: 2.2314687608402783
     kappa: -0.08770240608790579
     s: 6.5466046257611978
-    dkappa: 0
+    dkappa: -0.0077485384329919341
     ddkappa: 0
   }
   v: 4.1745807124976482
@@ -797,7 +797,7 @@ trajectory_point {
     theta: 2.1944679823259841
     kappa: -0.089750581665254431
     s: 6.9665341107999481
-    dkappa: 0
+    dkappa: -0.0069974591315992924
     ddkappa: 0
   }
   v: 4.2263092129357585
@@ -811,7 +811,7 @@ trajectory_point {
     theta: 2.1569142938881982
     kappa: -0.091827230621699688
     s: 7.3923013821855292
-    dkappa: 0
+    dkappa: -0.006235938456958824
     ddkappa: 0
   }
   v: 4.2910708651322986
@@ -825,7 +825,7 @@ trajectory_point {
     theta: 2.1188071043112915
     kappa: -0.093938286402336646
     s: 7.8251229511247118
-    dkappa: 0
+    dkappa: -0.0054618005761750757
     ddkappa: 0
   }
   v: 4.3670582729209793
@@ -839,7 +839,7 @@ trajectory_point {
     theta: 2.0784318941597233
     kappa: -0.095419614235532552
     s: 8.2660131941721957
-    dkappa: 0
+    dkappa: -0.0045536427436498293
     ddkappa: 0
   }
   v: 4.45203613942677
@@ -853,7 +853,7 @@ trajectory_point {
     theta: 2.0320324482003618
     kappa: -0.094977458371841045
     s: 8.7157415631597424
-    dkappa: 0
+    dkappa: -0.0032781784761164914
     ddkappa: 0
   }
   v: 4.5433412670659
@@ -867,7 +867,7 @@ trajectory_point {
     theta: 1.9844546313496236
     kappa: -0.094526127665621615
     s: 9.1748019060158086
-    dkappa: 0
+    dkappa: -0.0019762480124620421
     ddkappa: 0
   }
   v: 4.6383707088724124
@@ -881,7 +881,7 @@ trajectory_point {
     theta: 1.9358736104339087
     kappa: -0.094065312795532416
     s: 9.6435088416203332
-    dkappa: 0
+    dkappa: -0.00064695906932851592
     ddkappa: 0
   }
   v: 4.7361948507304445
@@ -895,7 +895,7 @@ trajectory_point {
     theta: 1.8864981718173208
     kappa: -0.093594761788596756
     s: 10.122118658321227
-    dkappa: 0
+    dkappa: 0.00071041520676384871
     ddkappa: 0
   }
   v: 4.836353056836634
@@ -909,7 +909,7 @@ trajectory_point {
     theta: 1.8432703392251137
     kappa: -0.090859929681586737
     s: 10.610842480088515
-    dkappa: 0
+    dkappa: 0.0020815339585048562
     ddkappa: 0
   }
   v: 4.9384026754367145
@@ -923,7 +923,7 @@ trajectory_point {
     theta: 1.8007046210602429
     kappa: -0.087503659968951084
     s: 11.109848064919252
-    dkappa: 0
+    dkappa: 0.0034777612700620454
     ddkappa: 0
   }
   v: 5.0419190388255215
@@ -937,7 +937,7 @@ trajectory_point {
     theta: 1.7572091980973128
     kappa: -0.0840773872340265
     s: 11.619261603242432
-    dkappa: 0
+    dkappa: 0.0049031102374990894
     ddkappa: 0
   }
   v: 5.1464954633469846
@@ -951,7 +951,7 @@ trajectory_point {
     theta: 1.7129433271488148
     kappa: -0.080580530214626966
     s: 12.139169516323895
-    dkappa: 0
+    dkappa: 0.0063578226691951567
     ddkappa: 0
   }
   v: 5.2517432493941332
@@ -965,7 +965,7 @@ trajectory_point {
     theta: 1.6732455032907356
     kappa: -0.075891793786014891
     s: 12.669620254671242
-    dkappa: 0
+    dkappa: 0.0072749201498742248
     ddkappa: 0
   }
   v: 5.3572916814090945
@@ -979,7 +979,7 @@ trajectory_point {
     theta: 1.6344336566539674
     kappa: -0.070732709469726929
     s: 13.210626096438741
-    dkappa: 0
+    dkappa: 0.0080195116750080668
     ddkappa: 0
   }
   v: 5.4627880278830929
@@ -993,7 +993,7 @@ trajectory_point {
     theta: 1.594848915795219
     kappa: -0.065473181368453079
     s: 13.762164945832231
-    dkappa: 0
+    dkappa: 0.0087785998794880325
     ddkappa: 0
   }
   v: 5.5678975413564507
@@ -1007,7 +1007,7 @@ trajectory_point {
     theta: 1.5548187854045281
     kappa: -0.060116901982894266
     s: 14.324182131514046
-    dkappa: 0
+    dkappa: 0.0095345350221569918
     ddkappa: 0
   }
   v: 5.6723034584185887
@@ -1021,7 +1021,7 @@ trajectory_point {
     theta: 1.5236077982126732
     kappa: -0.054814366301025007
     s: 14.896592205007916
-    dkappa: 0
+    dkappa: 0.0094576728127387932
     ddkappa: 0
   }
   v: 5.7757069997080253
@@ -1035,7 +1035,7 @@ trajectory_point {
     theta: 1.4917896019003292
     kappa: -0.049416615830630839
     s: 15.479280739103872
-    dkappa: 0
+    dkappa: 0.0093794304298612588
     ddkappa: 0
   }
   v: 5.8778273699123771
@@ -1049,7 +1049,7 @@ trajectory_point {
     theta: 1.4594354669620366
     kappa: -0.043924962356849326
     s: 16.072106126263172
-    dkappa: 0
+    dkappa: 0.0092998268883335237
     ddkappa: 0
   }
   v: 5.9784017577683581
@@ -1063,7 +1063,7 @@ trajectory_point {
     theta: 1.4338527305834519
     kappa: -0.038990870779971654
     s: 16.674901377023183
-    dkappa: 0
+    dkappa: 0.008941357288907938
     ddkappa: 0
   }
   v: 6.07718533606178
@@ -1077,7 +1077,7 @@ trajectory_point {
     theta: 1.4138439140040722
     kappa: -0.034513793830134712
     s: 17.287475918402329
-    dkappa: 0
+    dkappa: 0.0083477366647027335
     ddkappa: 0
   }
   v: 6.173951261627554
@@ -1091,7 +1091,7 @@ trajectory_point {
     theta: 1.3935209899035159
     kappa: -0.02996679576835419
     s: 17.909617392304966
-    dkappa: 0
+    dkappa: 0.0077448451223109255
     ddkappa: 0
   }
   v: 6.2684906753496863
@@ -1105,7 +1105,7 @@ trajectory_point {
     theta: 1.3747860996164087
     kappa: -0.025873153848942204
     s: 18.541093453926305
-    dkappa: 0
+    dkappa: 0.0070492907483592172
     ddkappa: 0
   }
   v: 6.3606127021612835
@@ -1119,7 +1119,7 @@ trajectory_point {
     theta: 1.3605149546498234
     kappa: -0.023034087806649447
     s: 19.181653570157334
-    dkappa: 0
+    dkappa: 0.0061331621584079735
     ddkappa: 0
   }
   v: 6.4501444510445483
@@ -1133,7 +1133,7 @@ trajectory_point {
     theta: 1.3460456963269267
     kappa: -0.020155942810975915
     s: 19.8310308179897
-    dkappa: 0
+    dkappa: 0.0052044233135615688
     ddkappa: 0
   }
   v: 6.5369310150307829
@@ -1147,7 +1147,7 @@ trajectory_point {
     theta: 1.3325486946753315
     kappa: -0.017299297958666564
     s: 20.48894488133741
-    dkappa: 0
+    dkappa: 0.0044025785282205011
     ddkappa: 0
   }
   v: 6.6208832971444549
@@ -1161,7 +1161,7 @@ trajectory_point {
     theta: 1.3242530510404964
     kappa: -0.014681855490242547
     s: 21.15511685486398
-    dkappa: 0
+    dkappa: 0.0042354896103522846
     ddkappa: 0
   }
   v: 6.7021170591830117
@@ -1175,7 +1175,7 @@ trajectory_point {
     theta: 1.3158574772967333
     kappa: -0.012033002042562392
     s: 21.829283317015953
-    dkappa: 0
+    dkappa: 0.0040663955188856389
     ddkappa: 0
   }
   v: 6.7808016930430073
@@ -1189,7 +1189,7 @@ trajectory_point {
     theta: 1.3079197999929493
     kappa: -0.0095378169349348025
     s: 22.51119802027414
-    dkappa: 0
+    dkappa: 0.003837202405747548
     ddkappa: 0
   }
   v: 6.8571112875129305
@@ -1203,7 +1203,7 @@ trajectory_point {
     theta: 1.30217404785753
     kappa: -0.0077731064327302224
     s: 23.200632360842778
-    dkappa: 0
+    dkappa: 0.0033660548424572423
     ddkappa: 0
   }
   v: 6.9312246282732133
@@ -1217,7 +1217,7 @@ trajectory_point {
     theta: 1.2963672830327244
     kappa: -0.0059896870737341339
     s: 23.897375848338765
-    dkappa: 0
+    dkappa: 0.002889912333946032
     ddkappa: 0
   }
   v: 7.0033251978962276
@@ -1231,7 +1231,7 @@ trajectory_point {
     theta: 1.2916794686747683
     kappa: -0.00438309219921056
     s: 24.601236575480819
-    dkappa: 0
+    dkappa: 0.0024621962474817996
     ddkappa: 0
   }
   v: 7.0736011758462807
@@ -1245,7 +1245,7 @@ trajectory_point {
     theta: 1.28955183322663
     kappa: -0.00319217741502978
     s: 25.312041687778692
-    dkappa: 0
+    dkappa: 0.0021481649440961616
     ddkappa: 0
   }
   v: 7.1422454384796232
@@ -1259,7 +1259,7 @@ trajectory_point {
     theta: 1.2874038684433742
     kappa: -0.001989884595644204
     s: 26.029637853222351
-    dkappa: 0
+    dkappa: 0.0018311333763147914
     ddkappa: 0
   }
   v: 7.2094555590444438
@@ -1273,7 +1273,7 @@ trajectory_point {
     theta: 1.286170095584471
     kappa: -0.00099613715065893262
     s: 26.753891731971191
-    dkappa: 0
+    dkappa: 0.0015656083963184297
     ddkappa: 0
   }
   v: 7.27543380768087
@@ -1287,7 +1287,7 @@ trajectory_point {
     theta: 1.2858184066070568
     kappa: -0.00020349556981083924
     s: 27.484690446043206
-    dkappa: 0
+    dkappa: 0.0013497492803173604
     ddkappa: 0
   }
   v: 7.3403871514209706
@@ -1301,7 +1301,7 @@ trajectory_point {
     theta: 1.2854636122592815
     kappa: 0.00059614496704457252
     s: 28.2219420490042
-    dkappa: 0
+    dkappa: 0.0011319841471539647
     ddkappa: 0
   }
   v: 7.4045272541887526
@@ -1315,7 +1315,7 @@ trajectory_point {
     theta: 1.2861146979460978
     kappa: 0.0011625419180066544
     s: 28.965575995656973
-    dkappa: 0
+    dkappa: 0.00096450562177014237
     ddkappa: 0
   }
   v: 7.4680704768001611
@@ -1329,7 +1329,7 @@ trajectory_point {
     theta: 1.2870510410530063
     kappa: 0.001667181822210909
     s: 29.71554361173051
-    dkappa: 0
+    dkappa: 0.00081006421950109871
     ddkappa: 0
   }
   v: 7.5312378769630843
@@ -1343,7 +1343,7 @@ trajectory_point {
     theta: 1.2880955940301964
     kappa: 0.0021495292105566466
     s: 30.47181856356919
-    dkappa: 0
+    dkappa: 0.00065960650011695618
     ddkappa: 0
   }
   v: 7.5942552092773452
@@ -1357,7 +1357,7 @@ trajectory_point {
     theta: 1.2898967173517377
     kappa: 0.0024381050715600637
     s: 31.234397327821974
-    dkappa: 0
+    dkappa: 0.00054726849057952335
     ddkappa: 0
   }
   v: 7.65735292523471
@@ -1371,7 +1371,7 @@ trajectory_point {
     theta: 1.2917127784600073
     kappa: 0.0027290739042415951
     s: 32.003299661131585
-    dkappa: 0
+    dkappa: 0.000433998935089965
     ddkappa: 0
   }
   v: 7.7207661732188839
@@ -1385,7 +1385,7 @@ trajectory_point {
     theta: 1.2937935954415154
     kappa: 0.0029374017796451341
     s: 32.778569069823725
-    dkappa: 0
+    dkappa: 0.0003358662029008014
     ddkappa: 0
   }
   v: 7.7847347985055091
@@ -1399,7 +1399,7 @@ trajectory_point {
     theta: 1.2961303847512655
     kappa: 0.0030661662082765353
     s: 33.560273279596245
-    dkappa: 0
+    dkappa: 0.00025228350235998549
     ddkappa: 0
   }
   v: 7.84950334326217
@@ -1413,7 +1413,7 @@ trajectory_point {
     theta: 1.2984866801737389
     kappa: 0.0031960058175294981
     s: 34.34850470520837
-    dkappa: 0
+    dkappa: 0.00016800288779994526
     ddkappa: 0
   }
   v: 7.91532104654839
@@ -1427,7 +1427,7 @@ trajectory_point {
     theta: 1.3010529070789489
     kappa: 0.0032087322787137068
     s: 35.143380920169854
-    dkappa: 0
+    dkappa: 0.00010557362000783752
     ddkappa: 0
   }
   v: 7.9824418443156295
@@ -1441,7 +1441,7 @@ trajectory_point {
     theta: 1.3036487539464576
     kappa: 0.0032167815896255551
     s: 35.9450451264302
-    dkappa: 0
+    dkappa: 4.352467838512579e-05
     ddkappa: 0
   }
   v: 8.05112436940729
@@ -1455,7 +1455,7 @@ trajectory_point {
     theta: 1.3062527172669605
     kappa: 0.0031840685108780924
     s: 36.753666624067868
-    dkappa: 0
+    dkappa: -1.0390770028993064e-05
     ddkappa: 0
   }
   v: 8.1216319515587152
@@ -1469,7 +1469,7 @@ trajectory_point {
     theta: 1.3088630206016214
     kappa: 0.003103672501374651
     s: 37.569441280979404
-    dkappa: 0
+    dkappa: -5.471767709330403e-05
     ddkappa: 0
   }
   v: 8.1942326173971836
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 07b97dbb17905cd958d855a169299179aaad1999..6abbc754821e13f45e8e1390e2524901887609f7 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
@@ -27,7 +27,7 @@ trajectory_point {
     theta: -0.21402668469869818
     kappa: -0.0076197814742330044
     s: 0.10891186382375059
-    dkappa: 0
+    dkappa: -0.00018556867997863722
     ddkappa: 0
   }
   v: 5.44126238283559
@@ -41,7 +41,7 @@ trajectory_point {
     theta: -0.21528562053810321
     kappa: -0.00798826803617724
     s: 0.21765347080630573
-    dkappa: 0
+    dkappa: -0.0003708472690877517
     ddkappa: 0
   }
   v: 5.4329712879550556
@@ -55,7 +55,7 @@ trajectory_point {
     theta: -0.21654271509226736
     kappa: -0.0083562073332476
     s: 0.32623357810888276
-    dkappa: 0
+    dkappa: -0.00055585068815222334
     ddkappa: 0
   }
   v: 5.4251096157307064
@@ -69,7 +69,7 @@ trajectory_point {
     theta: -0.217798061771532
     kappa: -0.00872362790212213
     s: 0.43466060699985037
-    dkappa: 0
+    dkappa: -0.00074059328568836685
     ddkappa: 0
   }
   v: 5.4176606857705023
@@ -83,7 +83,7 @@ trajectory_point {
     theta: -0.21905175019098228
     kappa: -0.00909055715674384
     s: 0.54294264742474418
-    dkappa: 0
+    dkappa: -0.00092508884569051882
     ddkappa: 0
   }
   v: 5.4106080461831887
@@ -97,7 +97,7 @@ trajectory_point {
     theta: -0.22030386622001047
     kappa: -0.0094570214038068624
     s: 0.6510874625762828
-    dkappa: 0
+    dkappa: -0.0011093505954176263
     ddkappa: 0
   }
   v: 5.4039354735782936
@@ -111,7 +111,7 @@ trajectory_point {
     theta: -0.221554492032087
     kappa: -0.0098230458582426114
     s: 0.759102493464383
-    dkappa: 0
+    dkappa: -0.0012933912131798337
     ddkappa: 0
   }
   v: 5.397626973066135
@@ -125,7 +125,7 @@ trajectory_point {
     theta: -0.22280370615473252
     kappa: -0.010188654658705932
     s: 0.86699486348617572
-    dkappa: 0
+    dkappa: -0.0014772228361250711
     ddkappa: 0
   }
   v: 5.3916667782578136
@@ -139,7 +139,7 @@ trajectory_point {
     theta: -0.22405158351968421
     kappa: -0.010553870883061253
     s: 0.97477138299602228
-    dkappa: 0
+    dkappa: -0.0016608570680256426
     ddkappa: 0
   }
   v: 5.3860393512652154
@@ -153,7 +153,7 @@ trajectory_point {
     theta: -0.2252981955132512
     kappa: -0.010918716563868754
     s: 1.082438553875529
-    dkappa: 0
+    dkappa: -0.0018443049870648118
     ddkappa: 0
   }
   v: 5.3807293827010128
@@ -167,7 +167,7 @@ trajectory_point {
     theta: -0.22654361002685278
     kappa: -0.011283212703870504
     s: 1.1900025741035638
-    dkappa: 0
+    dkappa: -0.0020275771536233922
     ddkappa: 0
   }
   v: 5.3757217916786644
@@ -181,7 +181,7 @@ trajectory_point {
     theta: -0.22778789150773368
     kappa: -0.01164737929147663
     s: 1.2974693423262715
-    dkappa: 0
+    dkappa: -0.0022106836180663335
     ddkappa: 0
   }
   v: 5.3710017258124134
@@ -195,7 +195,7 @@ trajectory_point {
     theta: -0.22903110100985161
     kappa: -0.012011235316251465
     s: 1.40484446242709
-    dkappa: 0
+    dkappa: -0.002393633928529309
     ddkappa: 0
   }
   v: 5.3665545612172885
@@ -209,7 +209,7 @@ trajectory_point {
     theta: -0.230273296244932
     kappa: -0.012374798784399703
     s: 1.5121332480967646
-    dkappa: 0
+    dkappa: -0.0025764371387053046
     ddkappa: 0
   }
   v: 5.3623659025091035
@@ -223,7 +223,7 @@ trajectory_point {
     theta: -0.2315145316336838
     kappa: -0.012738086734252559
     s: 1.6193407274033664
-    dkappa: 0
+    dkappa: -0.0027591018156312057
     ddkappa: 0
   }
   v: 5.35842158280446
@@ -237,7 +237,7 @@ trajectory_point {
     theta: -0.23275485835717286
     kappa: -0.013101115251753916
     s: 1.7264716473623047
-    dkappa: 0
+    dkappa: -0.0029416360474743843
     ddkappa: 0
   }
   v: 5.3547076637207418
@@ -251,7 +251,7 @@ trajectory_point {
     theta: -0.23399432440834761
     kappa: -0.013463899485946489
     s: 1.8335304785063458
-    dkappa: 0
+    dkappa: -0.00312404745131929
     ddkappa: 0
   }
   v: 5.35121043537612
@@ -265,7 +265,7 @@ trajectory_point {
     theta: -0.23523297464371254
     kappa: -0.013826453664457974
     s: 1.9405214194556257
-    dkappa: 0
+    dkappa: -0.0033063431809540315
     ddkappa: 0
   }
   v: 5.3479164163895518
@@ -279,7 +279,7 @@ trajectory_point {
     theta: -0.2366985331589645
     kappa: -0.014179032289749665
     s: 2.0474484014876686
-    dkappa: 0
+    dkappa: -0.0033837379473384511
     ddkappa: 0
   }
   v: 5.34481235388078
@@ -293,7 +293,7 @@ trajectory_point {
     theta: -0.23835080886841417
     kappa: -0.014523375941250919
     s: 2.1543150931074009
-    dkappa: 0
+    dkappa: -0.0033747951724483593
     ddkappa: 0
   }
   v: 5.34188522347033
@@ -307,7 +307,7 @@ trajectory_point {
     theta: -0.24000227510517136
     kappa: -0.014867536314833993
     s: 2.261124904617168
-    dkappa: 0
+    dkappa: -0.0033658571573764247
     ddkappa: 0
   }
   v: 5.3391222292795169
@@ -321,7 +321,7 @@ trajectory_point {
     theta: -0.2416529715706327
     kappa: -0.015211523581855192
     s: 2.3678809926867488
-    dkappa: 0
+    dkappa: -0.0033569236379674952
     ddkappa: 0
   }
   v: 5.3365108039304383
@@ -335,7 +335,7 @@ trajectory_point {
     theta: -0.243302934147356
     kappa: -0.015555347111146613
     s: 2.4745862649233721
-    dkappa: 0
+    dkappa: -0.0033479943709083665
     ddkappa: 0
   }
   v: 5.3340386085459786
@@ -349,7 +349,7 @@ trajectory_point {
     theta: -0.24495219496688395
     kappa: -0.015899015483741546
     s: 2.5812433844417324
-    dkappa: 0
+    dkappa: -0.0033390691333453635
     ddkappa: 0
   }
   v: 5.3316935327498074
@@ -363,7 +363,7 @@ trajectory_point {
     theta: -0.24660078247789546
     kappa: -0.016242536507599881
     s: 2.6878547744340047
-    dkappa: 0
+    dkappa: -0.0033301477225019067
     ddkappa: 0
   }
   v: 5.32946369466638
@@ -377,7 +377,7 @@ trajectory_point {
     theta: -0.24824872151467428
     kappa: -0.016585917232333533
     s: 2.7944226227398627
-    dkappa: 0
+    dkappa: -0.003321229955296089
     ddkappa: 0
   }
   v: 5.327337440920938
@@ -391,7 +391,7 @@ trajectory_point {
     theta: -0.24989603336588725
     kappa: -0.016929163963931824
     s: 2.9009488864164914
-    dkappa: 0
+    dkappa: -0.0033123156679582487
     ddkappa: 0
   }
   v: 5.3253033466395054
@@ -405,7 +405,7 @@ trajectory_point {
     theta: -0.25154273584366182
     kappa: -0.01727228227948693
     s: 3.007435296308604
-    dkappa: 0
+    dkappa: -0.0033034047156485459
     ddkappa: 0
   }
   v: 5.3233502154488965
@@ -419,7 +419,7 @@ trajectory_point {
     theta: -0.25318884335295527
     kappa: -0.017615277041919262
     s: 3.1138833616184587
-    dkappa: 0
+    dkappa: -0.0032944969720745316
     ddkappa: 0
   }
   v: 5.3214670794767072
@@ -433,7 +433,7 @@ trajectory_point {
     theta: -0.25483436696120532
     kappa: -0.017958152414702895
     s: 3.220294374475873
-    dkappa: 0
+    dkappa: -0.0032855923291087268
     ddkappa: 0
   }
   v: 5.31964319935132
@@ -447,7 +447,7 @@ trajectory_point {
     theta: -0.256479314468255
     kappa: -0.018300911876590963
     s: 3.3266694145082405
-    dkappa: 0
+    dkappa: -0.0032766906964061919
     ddkappa: 0
   }
   v: 5.3178680642019041
@@ -461,7 +461,7 @@ trajectory_point {
     theta: -0.25812369047654127
     kappa: -0.01864355823634108
     s: 3.4330093534105441
-    dkappa: 0
+    dkappa: -0.0032677920010221019
     ddkappa: 0
   }
   v: 5.3161313916584128
@@ -475,7 +475,7 @@ trajectory_point {
     theta: -0.25976749646154085
     kappa: -0.018986093647440753
     s: 3.5393148595153767
-    dkappa: 0
+    dkappa: -0.0032588961870293233
     ddkappa: 0
   }
   v: 5.3144231278515868
@@ -489,7 +489,7 @@ trajectory_point {
     theta: -0.26141073084246247
     kappa: -0.019328519622832784
     s: 3.6455864023629512
-    dkappa: 0
+    dkappa: -0.0032500032151359828
     ddkappa: 0
   }
   v: 5.3127334474129491
@@ -503,7 +503,7 @@ trajectory_point {
     theta: -0.26305338905317821
     kappa: -0.019670837049640674
     s: 3.75182425727112
-    dkappa: 0
+    dkappa: -0.0032411130623030441
     ddkappa: 0
   }
   v: 5.3110527534748115
@@ -517,7 +517,7 @@ trajectory_point {
     theta: -0.26469546361338503
     kappa: -0.020013046203894064
     s: 3.85802850990539
-    dkappa: 0
+    dkappa: -0.0032322257213618808
     ddkappa: 0
   }
   v: 5.3093716776702689
@@ -531,7 +531,7 @@ trajectory_point {
     theta: -0.26633694419998788
     kappa: -0.020355146765254104
     s: 3.9641990608489364
-    dkappa: 0
+    dkappa: -0.0032233412006318521
     ddkappa: 0
   }
   v: 5.3076810801332046
@@ -545,7 +545,7 @@ trajectory_point {
     theta: -0.26880820391339039
     kappa: -0.020699433937307939
     s: 4.0703356301726208
-    dkappa: 0
+    dkappa: -0.0032233360002754635
     ddkappa: 0
   }
   v: 5.3059720494982834
@@ -559,7 +559,7 @@ trajectory_point {
     theta: -0.27140386142229322
     kappa: -0.021043954842727874
     s: 4.1764377620050075
-    dkappa: 0
+    dkappa: -0.0032246662407082421
     ddkappa: 0
   }
   v: 5.30423590290096
@@ -573,7 +573,7 @@ trajectory_point {
     theta: -0.27399892294936884
     kappa: -0.021388361890546184
     s: 4.2825048291023737
-    dkappa: 0
+    dkappa: -0.003225996041521878
     ddkappa: 0
   }
   v: 5.30246418597747
@@ -587,7 +587,7 @@ trajectory_point {
     theta: -0.27659333390524804
     kappa: -0.021732652502441045
     s: 4.3885360374187341
-    dkappa: 0
+    dkappa: -0.0032273253927611305
     ddkappa: 0
   }
   v: 5.30064867286484
@@ -601,7 +601,7 @@ trajectory_point {
     theta: -0.27918703577636977
     kappa: -0.022076823573311373
     s: 4.494530430675848
-    dkappa: 0
+    dkappa: -0.0032286542824367988
     ddkappa: 0
   }
   v: 5.2987813662008776
@@ -615,7 +615,7 @@ trajectory_point {
     theta: -0.28177996624118323
     kappa: -0.022420871486115976
     s: 4.60048689493324
-    dkappa: 0
+    dkappa: -0.0032299826965830106
     ddkappa: 0
   }
   v: 5.2968544971241771
@@ -629,7 +629,7 @@ trajectory_point {
     theta: -0.28437205928716386
     kappa: -0.022764792126712716
     s: 4.706404163158215
-    dkappa: 0
+    dkappa: -0.0032313106193145244
     ddkappa: 0
   }
   v: 5.2948605252741183
@@ -643,7 +643,7 @@ trajectory_point {
     theta: -0.28696324532860845
     kappa: -0.023108580898697662
     s: 4.8122808197958724
-    dkappa: 0
+    dkappa: -0.0032326380328840214
     ddkappa: 0
   }
   v: 5.292792138790869
@@ -657,7 +657,7 @@ trajectory_point {
     theta: -0.28955345132517468
     kappa: -0.02345223273824424
     s: 4.9181153053391231
-    dkappa: 0
+    dkappa: -0.0032339649177394012
     ddkappa: 0
   }
   v: 5.2906422543153777
@@ -671,7 +671,7 @@ trajectory_point {
     theta: -0.29214260090112959
     kappa: -0.023795742128942407
     s: 5.0239059208987067
-    dkappa: 0
+    dkappa: -0.0032352912525810819
     ddkappa: 0
   }
   v: 5.2884040169893831
@@ -685,7 +685,7 @@ trajectory_point {
     theta: -0.29473061446527127
     kappa: -0.024139103116637767
     s: 5.1296508327732013
-    dkappa: 0
+    dkappa: -0.0032366170144192911
     ddkappa: 0
   }
   v: 5.2860708004554073
@@ -699,7 +699,7 @@ trajectory_point {
     theta: -0.29731740933148865
     kappa: -0.024482309324270774
     s: 5.2353480770190473
-    dkappa: 0
+    dkappa: -0.0032379421786313655
     ddkappa: 0
   }
   v: 5.2836362068567579
@@ -713,7 +713,7 @@ trajectory_point {
     theta: -0.2999028998399213
     kappa: -0.024825353966715837
     s: 5.340995564020556
-    dkappa: 0
+    dkappa: -0.0032392667190190446
     ddkappa: 0
   }
   v: 5.2810940668375279
@@ -727,7 +727,7 @@ trajectory_point {
     theta: -0.31280759613801468
     kappa: -0.026537889526839026
     s: 5.8684052825871689
-    dkappa: 0
+    dkappa: -0.0032458790435799306
     ddkappa: 0
   }
   v: 5.2665702380309254
@@ -741,7 +741,7 @@ trajectory_point {
     theta: -0.32845403156532377
     kappa: -0.028343404893685503
     s: 6.3941933637473465
-    dkappa: 0
+    dkappa: -0.0032965958513131851
     ddkappa: 0
   }
   v: 5.2485637332754926
@@ -755,7 +755,7 @@ trajectory_point {
     theta: -0.34465982313940596
     kappa: -0.030163614775438573
     s: 6.9179846444927184
-    dkappa: 0
+    dkappa: -0.0033567993894500366
     ddkappa: 0
   }
   v: 5.2265604464666122
@@ -769,7 +769,7 @@ trajectory_point {
     theta: -0.36079147027490155
     kappa: -0.031975428083892282
     s: 7.4393596918539986
-    dkappa: 0
+    dkappa: -0.0034167252104807923
     ddkappa: 0
   }
   v: 5.2001890844906233
@@ -783,7 +783,7 @@ trajectory_point {
     theta: -0.37689743667130171
     kappa: -0.03378202360518108
     s: 7.957869084200083
-    dkappa: 0
+    dkappa: -0.0034774589228947117
     ddkappa: 0
   }
   v: 5.1692211672248156
@@ -797,7 +797,7 @@ trajectory_point {
     theta: -0.39617190935430746
     kappa: -0.035796064084838805
     s: 8.4730476925371434
-    dkappa: 0
+    dkappa: -0.0035903528308222762
     ddkappa: 0
   }
   v: 5.1335710275374344
@@ -811,7 +811,7 @@ trajectory_point {
     theta: -0.41531714474594045
     kappa: -0.037795259238096887
     s: 8.9844289618077244
-    dkappa: 0
+    dkappa: -0.0037024146070580291
     ddkappa: 0
   }
   v: 5.0932958112876774
@@ -825,7 +825,7 @@ trajectory_point {
     theta: -0.43430200796694907
     kappa: -0.0397778353697896
     s: 9.4915591921898379
-    dkappa: 0
+    dkappa: -0.003813544829876957
     ddkappa: 0
   }
   v: 5.0485954773256969
@@ -839,7 +839,7 @@ trajectory_point {
     theta: -0.45352565089375896
     kappa: -0.041778128683987677
     s: 9.9940118203960591
-    dkappa: 0
+    dkappa: -0.0039275859753523717
     ddkappa: 0
   }
   v: 4.9998127974925977
@@ -853,7 +853,7 @@ trajectory_point {
     theta: -0.47541869874088238
     kappa: -0.043998058363927506
     s: 10.49140170097262
-    dkappa: 0
+    dkappa: -0.0040666921335699227
     ddkappa: 0
   }
   v: 4.9474333566204365
@@ -867,7 +867,7 @@ trajectory_point {
     theta: -0.49709236512624028
     kappa: -0.046193932744724442
     s: 10.983401833181908
-    dkappa: 0
+    dkappa: -0.0042042909286134147
     ddkappa: 0
   }
   v: 4.892181445783093
@@ -881,7 +881,7 @@ trajectory_point {
     theta: -0.51851589279783283
     kappa: -0.048364721901580678
     s: 11.469781455582778
-    dkappa: 0
+    dkappa: -0.0043403178229061987
     ddkappa: 0
   }
   v: 4.8352305386444883
@@ -895,7 +895,7 @@ trajectory_point {
     theta: -0.53993804262480838
     kappa: -0.050527032944596442
     s: 11.950431402049137
-    dkappa: 0
+    dkappa: -0.0044718027113227025
     ddkappa: 0
   }
   v: 4.7777805057483818
@@ -909,7 +909,7 @@ trajectory_point {
     theta: -0.56397352064851114
     kappa: -0.052842708851023018
     s: 12.425358053648173
-    dkappa: 0
+    dkappa: -0.0045709214507189157
     ddkappa: 0
   }
   v: 4.7209424204164145
@@ -923,7 +923,7 @@ trajectory_point {
     theta: -0.58775237526049062
     kappa: -0.055131029981275975
     s: 12.894674458918123
-    dkappa: 0
+    dkappa: -0.00466886931342929
     ddkappa: 0
   }
   v: 4.6657385587481137
@@ -937,7 +937,7 @@ trajectory_point {
     theta: -0.61125793193410427
     kappa: -0.057393024343593134
     s: 13.35859145414608
-    dkappa: 0
+    dkappa: -0.0047656903016737983
     ddkappa: 0
   }
   v: 4.6131023996208853
@@ -951,7 +951,7 @@ trajectory_point {
     theta: -0.6344798432532327
     kappa: -0.059630153449311135
     s: 13.81740878364576
-    dkappa: 0
+    dkappa: -0.0048614469730672173
     ddkappa: 0
   }
   v: 4.5638786246900231
@@ -965,7 +965,7 @@ trajectory_point {
     theta: -0.66366778562521878
     kappa: -0.061757824533474837
     s: 14.271506220035317
-    dkappa: 0
+    dkappa: -0.004831984099274122
     ddkappa: 0
   }
   v: 4.5188231183886991
@@ -979,7 +979,7 @@ trajectory_point {
     theta: -0.69375303223329132
     kappa: -0.063850134431399724
     s: 14.721334684515101
-    dkappa: 0
+    dkappa: -0.0047807249614103924
     ddkappa: 0
   }
   v: 4.4786029679279729
@@ -993,7 +993,7 @@ trajectory_point {
     theta: -0.72360521618067053
     kappa: -0.0659249748711678
     s: 15.167407367145476
-    dkappa: 0
+    dkappa: -0.0047298938047768569
     ddkappa: 0
   }
   v: 4.4437964632967839
@@ -1007,7 +1007,7 @@ trajectory_point {
     theta: -0.7532092356559934
     kappa: -0.067984981211272028
     s: 15.610290847124585
-    dkappa: 0
+    dkappa: -0.0046794260661821635
     ddkappa: 0
   }
   v: 4.4148930972619569
@@ -1021,7 +1021,7 @@ trajectory_point {
     theta: -0.78389492943573524
     kappa: -0.069747640654456572
     s: 16.05059621306615
-    dkappa: 0
+    dkappa: -0.0045062369263792279
     ddkappa: 0
   }
   v: 4.3922935653681989
@@ -1035,7 +1035,7 @@ trajectory_point {
     theta: -0.81618772571961618
     kappa: -0.071141623271509719
     s: 16.48897018327726
-    dkappa: 0
+    dkappa: -0.0041782059339602129
     ddkappa: 0
   }
   v: 4.3763097659380987
@@ -1049,7 +1049,7 @@ trajectory_point {
     theta: -0.84844177413450106
     kappa: -0.072531605813008534
     s: 16.926086226036155
-    dkappa: 0
+    dkappa: -0.0038511162364596495
     ddkappa: 0
   }
   v: 4.367164800072131
@@ -1063,7 +1063,7 @@ trajectory_point {
     theta: -0.88064084238620055
     kappa: -0.073919786661965858
     s: 17.362635679870017
-    dkappa: 0
+    dkappa: -0.0035244505119593128
     ddkappa: 0
   }
   v: 4.3649929716486513
@@ -1077,7 +1077,7 @@ trajectory_point {
     theta: -0.91276995912431069
     kappa: -0.075308392790313664
     s: 17.799318873832757
-    dkappa: 0
+    dkappa: -0.0031976847110169824
     ddkappa: 0
   }
   v: 4.3698397873238992
@@ -1091,7 +1091,7 @@ trajectory_point {
     theta: -0.9462575502504188
     kappa: -0.075564557507663666
     s: 18.236836247782804
-    dkappa: 0
+    dkappa: -0.0026019708516257928
     ddkappa: 0
   }
   v: 4.3816619565319979
@@ -1105,7 +1105,7 @@ trajectory_point {
     theta: -0.98003577354398486
     kappa: -0.075756160213308776
     s: 18.6758794726609
-    dkappa: 0
+    dkappa: -0.0019887064868635749
     ddkappa: 0
   }
   v: 4.4003273914849519
@@ -1119,7 +1119,7 @@ trajectory_point {
     theta: -1.0140060060519229
     kappa: -0.0759487229648875
     s: 19.117122570767869
-    dkappa: 0
+    dkappa: -0.001372369295140781
     ddkappa: 0
   }
   v: 4.42561520717265
@@ -1133,7 +1133,7 @@ trajectory_point {
     theta: -1.048139413992456
     kappa: -0.0761425283350112
     s: 19.561213036042425
-    dkappa: 0
+    dkappa: -0.00075205484396304566
     ddkappa: 0
   }
   v: 4.4572157213628651
@@ -1147,7 +1147,7 @@ trajectory_point {
     theta: -1.0821432248873264
     kappa: -0.075746101689774234
     s: 20.008764247129651
-    dkappa: 0
+    dkappa: -0.00013515395011907667
     ddkappa: 0
   }
   v: 4.4947829052524471
@@ -1161,7 +1161,7 @@ trajectory_point {
     theta: -1.1162324186273598
     kappa: -0.0747371493243309
     s: 20.460361990793981
-    dkappa: 0
+    dkappa: 0.00047883733637729047
     ddkappa: 0
   }
   v: 4.5381212963606039
@@ -1175,7 +1175,7 @@ trajectory_point {
     theta: -1.150724987489667
     kappa: -0.073717883679508073
     s: 20.91657586288725
-    dkappa: 0
+    dkappa: 0.0010991047008046921
     ddkappa: 0
   }
   v: 4.5870803782197855
@@ -1189,7 +1189,7 @@ trajectory_point {
     theta: -1.1855812587043215
     kappa: -0.072687065814403221
     s: 21.377960401894395
-    dkappa: 0
+    dkappa: 0.0017264020924521923
     ddkappa: 0
   }
   v: 4.6415095199594134
@@ -1203,7 +1203,7 @@ trajectory_point {
     theta: -1.2200440145058666
     kappa: -0.071479749557316868
     s: 21.845055077493139
-    dkappa: 0
+    dkappa: 0.0023266232426734732
     ddkappa: 0
   }
   v: 4.7012579763058744
@@ -1217,7 +1217,7 @@ trajectory_point {
     theta: -1.2520802589457265
     kappa: -0.069594353317923019
     s: 22.318384279113687
-    dkappa: 0
+    dkappa: 0.002794007101139286
     ddkappa: 0
   }
   v: 4.76617488758253
@@ -1231,7 +1231,7 @@ trajectory_point {
     theta: -1.2846179699053653
     kappa: -0.06768209463285578
     s: 22.798457304498438
-    dkappa: 0
+    dkappa: 0.0032680500762641506
     ddkappa: 0
   }
   v: 4.8361092797097083
@@ -1245,7 +1245,7 @@ trajectory_point {
     theta: -1.3176222363470322
     kappa: -0.06574100499338148
     s: 23.285768348261669
-    dkappa: 0
+    dkappa: 0.0037492401552351295
     ddkappa: 0
   }
   v: 4.9109100642047085
@@ -1259,7 +1259,7 @@ trajectory_point {
     theta: -1.3479351239390069
     kappa: -0.063493611015014245
     s: 23.780796490449227
-    dkappa: 0
+    dkappa: 0.0041228702697135395
     ddkappa: 0
   }
   v: 4.9904260381818
@@ -1273,7 +1273,7 @@ trajectory_point {
     theta: -1.3758490212783616
     kappa: -0.06095177858210174
     s: 24.284005685098247
-    dkappa: 0
+    dkappa: 0.0043951306695015728
     ddkappa: 0
   }
   v: 5.07450588435222
@@ -1287,7 +1287,7 @@ trajectory_point {
     theta: -1.4042614184521927
     kappa: -0.05836635457479987
     s: 24.795844748796831
-    dkappa: 0
+    dkappa: 0.0046720602439043329
     ddkappa: 0
   }
   v: 5.1629981710241788
@@ -1301,7 +1301,7 @@ trajectory_point {
     theta: -1.4331503600755688
     kappa: -0.055735148431740524
     s: 25.316747349243755
-    dkappa: 0
+    dkappa: 0.0049538936280244771
     ddkappa: 0
   }
   v: 5.2557513521028536
@@ -1315,7 +1315,7 @@ trajectory_point {
     theta: -1.4626812357502024
     kappa: -0.053000313184359091
     s: 25.847131993808144
-    dkappa: 0
+    dkappa: 0.0050794761421448118
     ddkappa: 0
   }
   v: 5.3526137670903928
@@ -1329,7 +1329,7 @@ trajectory_point {
     theta: -1.4928943899871336
     kappa: -0.05018535821604686
     s: 26.387402018089208
-    dkappa: 0
+    dkappa: 0.0051229994325981965
     ddkappa: 0
   }
   v: 5.4534336410859137
@@ -1343,7 +1343,7 @@ trajectory_point {
     theta: -1.5236918258215606
     kappa: -0.047316875328466186
     s: 26.937945574475908
-    dkappa: 0
+    dkappa: 0.0051673503423932442
     ddkappa: 0
   }
   v: 5.558059084785504
@@ -1357,7 +1357,7 @@ trajectory_point {
     theta: -1.5545781756453743
     kappa: -0.044407434034670415
     s: 27.499135620706657
-    dkappa: 0
+    dkappa: 0.005202949813306961
     ddkappa: 0
   }
   v: 5.6663380944822226
@@ -1371,7 +1371,7 @@ trajectory_point {
     theta: -1.5772941587019991
     kappa: -0.041718944820520049
     s: 28.071329908429032
-    dkappa: 0
+    dkappa: 0.0050551774484411689
     ddkappa: 0
   }
   v: 5.778118552066096
@@ -1385,7 +1385,7 @@ trajectory_point {
     theta: -1.600476931659903
     kappa: -0.038977142099939792
     s: 28.654870971759461
-    dkappa: 0
+    dkappa: 0.00490447471551477
     ddkappa: 0
   }
   v: 5.8932482250241209
@@ -1399,7 +1399,7 @@ trajectory_point {
     theta: -1.624114815201225
     kappa: -0.036180488010055335
     s: 29.250086115842908
-    dkappa: 0
+    dkappa: 0.0047507570861493051
     ddkappa: 0
   }
   v: 6.0115747664402663
@@ -1413,7 +1413,7 @@ trajectory_point {
     theta: -1.6452097590234309
     kappa: -0.033611555114815207
     s: 29.857287405412588
-    dkappa: 0
+    dkappa: 0.0045554368620084867
     ddkappa: 0
   }
   v: 6.1329457149954667
@@ -1427,7 +1427,7 @@ trajectory_point {
     theta: -1.6652158537292756
     kappa: -0.031135240630941342
     s: 30.476771653349676
-    dkappa: 0
+    dkappa: 0.0043365641915291651
     ddkappa: 0
   }
   v: 6.25720849496763
@@ -1441,7 +1441,7 @@ trajectory_point {
     theta: -1.6856267445422684
     kappa: -0.02860870102328384
     s: 31.108820409242952
-    dkappa: 0
+    dkappa: 0.0041132523001676533
     ddkappa: 0
   }
   v: 6.3842104162316327
@@ -1455,7 +1455,7 @@ trajectory_point {
     theta: -1.703056076015077
     kappa: -0.026251287123689605
     s: 31.753699947948569
-    dkappa: 0
+    dkappa: 0.003886581194017572
     ddkappa: 0
   }
   v: 6.51379867425932
@@ -1469,7 +1469,7 @@ trajectory_point {
     theta: -1.7172696060787316
     kappa: -0.024078766157987472
     s: 32.411661258149678
-    dkappa: 0
+    dkappa: 0.0036565515266463546
     ddkappa: 0
   }
   v: 6.64582035011951
@@ -1483,7 +1483,7 @@ trajectory_point {
     theta: -1.7317709629445488
     kappa: -0.021862272292686655
     s: 33.082940030916191
-    dkappa: 0
+    dkappa: 0.0034218659449582124
     ddkappa: 0
   }
   v: 6.7801224104779871
@@ -1497,7 +1497,7 @@ trajectory_point {
     theta: -1.74607793368878
     kappa: -0.019808815660934481
     s: 33.767756648264417
-    dkappa: 0
+    dkappa: 0.0031974845735205579
     ddkappa: 0
   }
   v: 6.9165517075975078
@@ -1511,7 +1511,7 @@ trajectory_point {
     theta: -1.7601803373719827
     kappa: -0.017926975627871614
     s: 34.466316171716826
-    dkappa: 0
+    dkappa: 0.0029840057526475867
     ddkappa: 0
   }
   v: 7.0549549793377961
@@ -1525,7 +1525,7 @@ trajectory_point {
     theta: -1.774563596532972
     kappa: -0.016007602656337692
     s: 35.178808330861671
-    dkappa: 0
+    dkappa: 0.0027662691376433811
     ddkappa: 0
   }
   v: 7.1951788491555506
@@ -1539,7 +1539,7 @@ trajectory_point {
     theta: -1.7854846744561985
     kappa: -0.014314655607388989
     s: 35.905407511912749
-    dkappa: 0
+    dkappa: 0.0025612164070492171
     ddkappa: 0
   }
   v: 7.3370698261044351
@@ -1553,7 +1553,7 @@ trajectory_point {
     theta: -1.7947748859579591
     kappa: -0.012718866115185367
     s: 36.646272746269062
-    dkappa: 0
+    dkappa: 0.0023605183480152718
     ddkappa: 0
   }
   v: 7.4804743048350826
@@ -1567,7 +1567,7 @@ trajectory_point {
     theta: -1.8042452585521978
     kappa: -0.011092038755066759
     s: 37.40154769907452
-    dkappa: 0
+    dkappa: 0.0021559167416290454
     ddkappa: 0
   }
   v: 7.6252385655950992
@@ -1581,7 +1581,7 @@ trajectory_point {
     theta: -1.8114085014534342
     kappa: -0.0098028435910211126
     s: 38.171360657777655
-    dkappa: 0
+    dkappa: 0.0019689882338942235
     ddkappa: 0
   }
   v: 7.7712087742290619
@@ -1595,7 +1595,7 @@ trajectory_point {
     theta: -1.8186841969234324
     kappa: -0.008492685280982553
     s: 38.955824520691287
-    dkappa: 0
+    dkappa: 0.0017787114095179251
     ddkappa: 0
   }
   v: 7.9182309821785131
@@ -1609,7 +1609,7 @@ trajectory_point {
     theta: -1.8251796047449498
     kappa: -0.0073198251497695937
     s: 39.755036785552271
-    dkappa: 0
+    dkappa: 0.0015890069968936286
     ddkappa: 0
   }
   v: 8.0661511264819676
@@ -1623,7 +1623,7 @@ trajectory_point {
     theta: -1.8306067610412042
     kappa: -0.0063352100719995079
     s: 40.569079538081112
-    dkappa: 0
+    dkappa: 0.001401164199865167
     ddkappa: 0
   }
   v: 8.21481502977491
@@ -1637,7 +1637,7 @@ trajectory_point {
     theta: -1.8361331657781148
     kappa: -0.005332576335635531
     s: 41.398019440541752
-    dkappa: 0
+    dkappa: 0.0012098838409859939
     ddkappa: 0
   }
   v: 8.364068400289792
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 c3c11bb4e6bd5616912428632e78397f1ec38ff1..e64ce464601b0675a466195ccba49ad43b23b744 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
@@ -27,7 +27,7 @@ trajectory_point {
     theta: -0.21366716184272408
     kappa: -0.002818136342486165
     s: 0.108912711377402
-    dkappa: 0
+    dkappa: -0.00027230089781878888
     ddkappa: 0
   }
   v: 5.4413549654516231
@@ -41,7 +41,7 @@ trajectory_point {
     theta: -0.2145671354457615
     kappa: -0.0033589633219799683
     s: 0.21765748893243866
-    dkappa: 0
+    dkappa: -0.00054418193160126924
     ddkappa: 0
   }
   v: 5.4332036750666814
@@ -55,7 +55,7 @@ trajectory_point {
     theta: -0.2154658099520666
     kappa: -0.0038990033803497437
     s: 0.3262440392432171
-    dkappa: 0
+    dkappa: -0.00081566736949667616
     ddkappa: 0
   }
   v: 5.4255295204264309
@@ -69,7 +69,7 @@ trajectory_point {
     theta: -0.21636326149992241
     kappa: -0.0044383031682449121
     s: 0.43468174241592006
-    dkappa: 0
+    dkappa: -0.0010867806634171228
     ddkappa: 0
   }
   v: 5.4183162720223113
@@ -83,7 +83,7 @@ trajectory_point {
     theta: -0.21725956357175458
     kappa: -0.0049769077313696012
     s: 0.5429796558483122
-    dkappa: 0
+    dkappa: -0.0013575444584470258
     ddkappa: 0
   }
   v: 5.4115478885210866
@@ -97,7 +97,7 @@ trajectory_point {
     theta: -0.21815478702391708
     kappa: -0.005514860529199919
     s: 0.65114651799324663
-    dkappa: 0
+    dkappa: -0.001627980602252533
     ddkappa: 0
   }
   v: 5.4052085167648416
@@ -111,7 +111,7 @@ trajectory_point {
     theta: -0.2190490001165534
     kappa: -0.00605220345370123
     s: 0.75919075212217169
-    dkappa: 0
+    dkappa: -0.0018981101544909519
     ddkappa: 0
   }
   v: 5.3992824917709852
@@ -125,7 +125,7 @@ trajectory_point {
     theta: -0.21994226854353074
     kappa: -0.0065889768480454237
     s: 0.8671204700886368
-    dkappa: 0
+    dkappa: -0.0021679533962201719
     ddkappa: 0
   }
   v: 5.3937543367322469
@@ -139,7 +139,7 @@ trajectory_point {
     theta: -0.22083465546244394
     kappa: -0.007125219525328198
     s: 0.9749434760917991
-    dkappa: 0
+    dkappa: -0.0024375298393080969
     ddkappa: 0
   }
   v: 5.3886087630166788
@@ -153,7 +153,7 @@ trajectory_point {
     theta: -0.22172622152468877
     kappa: -0.0076609687872863244
     s: 1.0826672704399307
-    dkappa: 0
+    dkappa: -0.0027068582358420672
     ddkappa: 0
   }
   v: 5.3838306701676562
@@ -167,7 +167,7 @@ trajectory_point {
     theta: -0.222617024905601
     kappa: -0.0081962604430149289
     s: 1.1902990533139235
-    dkappa: 0
+    dkappa: -0.0029759565875382885
     ddkappa: 0
   }
   v: 5.379405145903875
@@ -181,7 +181,7 @@ trajectory_point {
     theta: -0.22350712133466036
     kappa: -0.0087311288276847657
     s: 1.2978457285307972
-    dkappa: 0
+    dkappa: -0.0032448421551512594
     ddkappa: 0
   }
   v: 5.3753174661193546
@@ -195,7 +195,7 @@ trajectory_point {
     theta: -0.22439656412575723
     kappa: -0.009265606821259478
     s: 1.4053139073072047
-    dkappa: 0
+    dkappa: -0.0035135314678831956
     ddkappa: 0
   }
   v: 5.3715530948834367
@@ -209,7 +209,7 @@ trajectory_point {
     theta: -0.22528540420751997
     kappa: -0.0097997258672129009
     s: 1.51270991202294
-    dkappa: 0
+    dkappa: -0.0037820403327934612
     ddkappa: 0
   }
   v: 5.3680976844407846
@@ -223,7 +223,7 @@ trajectory_point {
     theta: -0.22617369015370239
     kappa: -0.010333515991246304
     s: 1.6200397799844415
-    dkappa: 0
+    dkappa: -0.0040503838442079885
     ddkappa: 0
   }
   v: 5.3649370752113841
@@ -237,7 +237,7 @@ trajectory_point {
     theta: -0.22706146821362766
     kappa: -0.01086700582000568
     s: 1.7273092671883024
-    dkappa: 0
+    dkappa: -0.00431857639312871
     ddkappa: 0
   }
   v: 5.3620572957905441
@@ -251,7 +251,7 @@ trajectory_point {
     theta: -0.22794878234268959
     kappa: -0.011400222599799031
     s: 1.8345238520847744
-    dkappa: 0
+    dkappa: -0.0045866316766429872
     ddkappa: 0
   }
   v: 5.3594445629488954
@@ -265,7 +265,7 @@ trajectory_point {
     theta: -0.22883567423290807
     kappa: -0.011933192215313623
     s: 1.9416887393412758
-    dkappa: 0
+    dkappa: -0.0048545627073330322
     ddkappa: 0
   }
   v: 5.3570852816323891
@@ -279,7 +279,7 @@ trajectory_point {
     theta: -0.23010860550605383
     kappa: -0.012423353616927176
     s: 2.0488088636058963
-    dkappa: 0
+    dkappa: -0.0049519633528665556
     ddkappa: 0
   }
   v: 5.3549660449623016
@@ -293,7 +293,7 @@ trajectory_point {
     theta: -0.23168904540060292
     kappa: -0.012879395608204169
     s: 2.1558888932709048
-    dkappa: 0
+    dkappa: -0.004913523115074832
     ddkappa: 0
   }
   v: 5.353073634235229
@@ -307,7 +307,7 @@ trajectory_point {
     theta: -0.23326901961582783
     kappa: -0.013335285605260493
     s: 2.2629332342362551
-    dkappa: 0
+    dkappa: -0.0048750956890287328
     ddkappa: 0
   }
   v: 5.3513950189230917
@@ -321,7 +321,7 @@ trajectory_point {
     theta: -0.23484858142471104
     kappa: -0.013791041270438916
     s: 2.3699460336730933
-    dkappa: 0
+    dkappa: -0.0048366795859516309
     ddkappa: 0
   }
   v: 5.3499173566731315
@@ -335,7 +335,7 @@ trajectory_point {
     theta: -0.236427780357265
     kappa: -0.014246679180213207
     s: 2.4769311837872618
-    dkappa: 0
+    dkappa: -0.0047982734085958954
     ddkappa: 0
   }
   v: 5.3486279933079128
@@ -349,7 +349,7 @@ trajectory_point {
     theta: -0.23800666225285988
     kappa: -0.014702214841216511
     s: 2.5838923255828106
-    dkappa: 0
+    dkappa: -0.0047598758498918316
     ddkappa: 0
   }
   v: 5.3475144628253206
@@ -363,7 +363,7 @@ trajectory_point {
     theta: -0.23958526931284285
     kappa: -0.015157662706269674
     s: 2.6908328526254968
-    dkappa: 0
+    dkappa: -0.0047214856915966438
     ddkappa: 0
   }
   v: 5.3465644873985649
@@ -377,7 +377,7 @@ trajectory_point {
     theta: -0.24116364015344025
     kappa: -0.01561303619040965
     s: 2.7977559148062987
-    dkappa: 0
+    dkappa: -0.0046831018029433874
     ddkappa: 0
   }
   v: 5.3457659773761756
@@ -391,7 +391,7 @@ trajectory_point {
     theta: -0.24274180985893787
     kappa: -0.0160683476869178
     s: 2.9046644221049154
-    dkappa: 0
+    dkappa: -0.0046447231392899172
     ddkappa: 0
   }
   v: 5.3451070312820068
@@ -405,7 +405,7 @@ trajectory_point {
     theta: -0.24431981003513115
     kappa: -0.016523608583348295
     s: 3.0115610483532782
-    dkappa: 0
+    dkappa: -0.0046063487407678478
     ddkappa: 0
   }
   v: 5.3445759358152332
@@ -419,7 +419,7 @@ trajectory_point {
     theta: -0.24589766886304121
     kappa: -0.016978829277556458
     s: 3.1184482349990543
-    dkappa: 0
+    dkappa: -0.0045679777309315054
     ddkappa: 0
   }
   v: 5.3441611658503527
@@ -433,7 +433,7 @@ trajectory_point {
     theta: -0.2474754111528879
     kappa: -0.017434019193727102
     s: 3.2253281948691539
-    dkappa: 0
+    dkappa: -0.0045296093154068781
     ddkappa: 0
   }
   v: 5.343851384437186
@@ -447,7 +447,7 @@ trajectory_point {
     theta: -0.24905305839831629
     kappa: -0.017889186798402925
     s: 3.3322029159332378
-    dkappa: 0
+    dkappa: -0.0044912427805405743
     ddkappa: 0
   }
   v: 5.3436354428008741
@@ -461,7 +461,7 @@ trajectory_point {
     theta: -0.25063062883086862
     kappa: -0.018344339616512835
     s: 3.4390741650672214
-    dkappa: 0
+    dkappa: -0.0044528774920487724
     ddkappa: 0
   }
   v: 5.3435023803418815
@@ -475,7 +475,7 @@ trajectory_point {
     theta: -0.25220813747469689
     kappa: -0.018799484247400328
     s: 3.5459434918167836
-    dkappa: 0
+    dkappa: -0.0044145128936661764
     ddkappa: 0
   }
   v: 5.343441424635996
@@ -489,7 +489,7 @@ trajectory_point {
     theta: -0.25378559620151
     kappa: -0.019254626380851833
     s: 3.6528122321608727
-    dkappa: 0
+    dkappa: -0.00437614850579497
     ddkappa: 0
   }
   v: 5.343441991434327
@@ -503,7 +503,7 @@ trajectory_point {
     theta: -0.25536301378574844
     kappa: -0.019709770813125076
     s: 3.7596815122752116
-    dkappa: 0
+    dkappa: -0.00433778392415377
     ddkappa: 0
   }
   v: 5.3434936846633043
@@ -517,7 +517,7 @@ trajectory_point {
     theta: -0.25694039595998275
     kappa: -0.020164921462977416
     s: 3.8665522522958042
-    dkappa: 0
+    dkappa: -0.0042994188184265775
     ddkappa: 0
   }
   v: 5.3435862964246814
@@ -531,7 +531,7 @@ trajectory_point {
     theta: -0.25851774547052797
     kappa: -0.020620081387694254
     s: 3.9734251700824448
-    dkappa: 0
+    dkappa: -0.0042610529309117344
     ddkappa: 0
   }
   v: 5.3437098069955358
@@ -545,7 +545,7 @@ trajectory_point {
     theta: -0.26116331441794355
     kappa: -0.021031365123291096
     s: 4.0803007849822217
-    dkappa: 0
+    dkappa: -0.0042368012345332957
     ddkappa: 0
   }
   v: 5.3438543848282647
@@ -559,7 +559,7 @@ trajectory_point {
     theta: -0.26387312307892408
     kappa: -0.021440037516849589
     s: 4.1871794215930231
-    dkappa: 0
+    dkappa: -0.0042133924525023089
     ddkappa: 0
   }
   v: 5.3440103865505888
@@ -573,7 +573,7 @@ trajectory_point {
     theta: -0.26658330449053264
     kappa: -0.021848721975433753
     s: 4.294061213527046
-    dkappa: 0
+    dkappa: -0.0041899829793858451
     ddkappa: 0
   }
   v: 5.34416835696555
@@ -587,7 +587,7 @@ trajectory_point {
     theta: -0.26929381754593268
     kappa: -0.022257418294054782
     s: 4.4009461071743017
-    dkappa: 0
+    dkappa: -0.0041665728269256767
     ddkappa: 0
   }
   v: 5.3443190290515137
@@ -601,7 +601,7 @@ trajectory_point {
     theta: -0.2720046164556717
     kappa: -0.022666125566231994
     s: 4.5078338654661216
-    dkappa: 0
+    dkappa: -0.0041431620470450767
     ddkappa: 0
   }
   v: 5.3444533239621679
@@ -615,7 +615,7 @@ trajectory_point {
     theta: -0.27471565084551519
     kappa: -0.023074842198383332
     s: 4.6147240716386628
-    dkappa: 0
+    dkappa: -0.0041197507310245327
     ddkappa: 0
   }
   v: 5.3445623510265206
@@ -629,7 +629,7 @@ trajectory_point {
     theta: -0.27742686585533638
     kappa: -0.023483565924215938
     s: 4.7216161329964192
-    dkappa: 0
+    dkappa: -0.004096339008677452
     ddkappa: 0
   }
   v: 5.3446374077489045
@@ -643,7 +643,7 @@ trajectory_point {
     theta: -0.2801382022390318
     kappa: -0.023892293819116662
     s: 4.8285092846757216
-    dkappa: 0
+    dkappa: -0.0040729270475258709
     ddkappa: 0
   }
   v: 5.3446699798089732
@@ -657,7 +657,7 @@ trajectory_point {
     theta: -0.28284959646543256
     kappa: -0.024301022314542595
     s: 4.9354025934082459
-    dkappa: 0
+    dkappa: -0.0040495150519761648
     ddkappa: 0
   }
   v: 5.3446517410617034
@@ -671,7 +671,7 @@ trajectory_point {
     theta: -0.28556098082018244
     kappa: -0.024709747212411662
     s: 5.0422949612845258
-    dkappa: 0
+    dkappa: -0.0040261032624947582
     ddkappa: 0
   }
   v: 5.3445745535373934
@@ -685,7 +685,7 @@ trajectory_point {
     theta: -0.28827228350854789
     kappa: -0.025118463699493089
     s: 5.1491851295174493
-    dkappa: 0
+    dkappa: -0.0040026919547838293
     ddkappa: 0
   }
   v: 5.3444304674416632
@@ -699,7 +699,7 @@ trajectory_point {
     theta: -0.29098342875913391
     kappa: -0.025527166361797998
     s: 5.2560716822057723
-    dkappa: 0
+    dkappa: -0.0039792814389570238
     ddkappa: 0
   }
   v: 5.3442117211554576
@@ -713,7 +713,7 @@ trajectory_point {
     theta: -0.2936943369284673
     kappa: -0.025935849198969921
     s: 5.362953050097623
-    dkappa: 0
+    dkappa: -0.00395587205871516
     ddkappa: 0
   }
   v: 5.3439107412350406
@@ -727,7 +727,7 @@ trajectory_point {
     theta: -0.30724227633892887
     kappa: -0.027978716762835053
     s: 5.897216933452107
-    dkappa: 0
+    dkappa: -0.0038388564680502447
     ddkappa: 0
   }
   v: 5.3409204388616374
@@ -741,7 +741,7 @@ trajectory_point {
     theta: -0.32413390395696046
     kappa: -0.029951529310879545
     s: 6.4310342186391383
-    dkappa: 0
+    dkappa: -0.0037892930966208654
     ddkappa: 0
   }
   v: 5.334846479883069
@@ -755,7 +755,7 @@ trajectory_point {
     theta: -0.34150912124769817
     kappa: -0.031911251665230872
     s: 6.9640588107569332
-    dkappa: 0
+    dkappa: -0.0037498163717945257
     ddkappa: 0
   }
   v: 5.3249538810700567
@@ -769,7 +769,7 @@ trajectory_point {
     theta: -0.35884493065748918
     kappa: -0.033866538538594476
     s: 7.4958769970596
-    dkappa: 0
+    dkappa: -0.0037104289954731084
     ddkappa: 0
   }
   v: 5.3106252687697415
@@ -783,7 +783,7 @@ trajectory_point {
     theta: -0.3766003039020806
     kappa: -0.035826584899517989
     s: 8.0260192079147821
-    dkappa: 0
+    dkappa: -0.0036821091138140058
     ddkappa: 0
   }
   v: 5.2913608789056825
@@ -797,7 +797,7 @@ trajectory_point {
     theta: -0.39728930464347417
     kappa: -0.03784612995225374
     s: 8.5539717777613049
-    dkappa: 0
+    dkappa: -0.0037216364728230381
     ddkappa: 0
   }
   v: 5.266778556977858
@@ -811,7 +811,7 @@ trajectory_point {
     theta: -0.41788417005652939
     kappa: -0.039855210520761609
     s: 9.0791887060668035
-    dkappa: 0
+    dkappa: -0.0037609590166810744
     ddkappa: 0
   }
   v: 5.2366137580626644
@@ -825,7 +825,7 @@ trajectory_point {
     theta: -0.438345169517326
     kappa: -0.041851659321678406
     s: 9.60110341828538
-    dkappa: 0
+    dkappa: -0.00380003432643467
     ddkappa: 0
   }
   v: 5.2007195468129144
@@ -839,7 +839,7 @@ trajectory_point {
     theta: -0.4597676612297289
     kappa: -0.043898758736162738
     s: 10.119140526815237
-    dkappa: 0
+    dkappa: -0.0038582507065557966
     ddkappa: 0
   }
   v: 5.1590665974578425
@@ -853,7 +853,7 @@ trajectory_point {
     theta: -0.48310786578165055
     kappa: -0.046048771890362106
     s: 10.63272759195632
-    dkappa: 0
+    dkappa: -0.0039517235082496
     ddkappa: 0
   }
   v: 5.1117431938030977
@@ -867,7 +867,7 @@ trajectory_point {
     theta: -0.50623532075368727
     kappa: -0.048177844006668566
     s: 11.141312340636494
-    dkappa: 0
+    dkappa: -0.0040442858887776521
     ddkappa: 0
   }
   v: 5.0591715811684965
@@ -881,7 +881,7 @@ trajectory_point {
     theta: -0.529104184124681
     kappa: -0.050284034983747249
     s: 11.644431328737424
-    dkappa: 0
+    dkappa: -0.004135853501350267
     ddkappa: 0
   }
   v: 5.0026786700612638
@@ -895,7 +895,7 @@ trajectory_point {
     theta: -0.55299702459012168
     kappa: -0.052435461712151139
     s: 12.141765675317366
-    dkappa: 0
+    dkappa: -0.0042213237977457877
     ddkappa: 0
   }
   v: 4.9437098641178387
@@ -909,7 +909,7 @@ trajectory_point {
     theta: -0.5782630626643015
     kappa: -0.054647353677867577
     s: 12.633135167456043
-    dkappa: 0
+    dkappa: -0.0042995030159603811
     ddkappa: 0
   }
   v: 4.8835931201311276
@@ -923,7 +923,7 @@ trajectory_point {
     theta: -0.603237776962305
     kappa: -0.056832155063828604
     s: 13.118486515570289
-    dkappa: 0
+    dkappa: -0.004376724718898873
     ddkappa: 0
   }
   v: 4.8235389480505075
@@ -937,7 +937,7 @@ trajectory_point {
     theta: -0.62789312422422816
     kappa: -0.058990144462698166
     s: 13.597881608729702
-    dkappa: 0
+    dkappa: -0.00445299875340596
     ddkappa: 0
   }
   v: 4.7646404109818263
@@ -951,7 +951,7 @@ trajectory_point {
     theta: -0.65480816937809228
     kappa: -0.061083508474311689
     s: 14.071485769972291
-    dkappa: 0
+    dkappa: -0.0044819508755169915
     ddkappa: 0
   }
   v: 4.707873125187402
@@ -965,7 +965,7 @@ trajectory_point {
     theta: -0.68609058814903057
     kappa: -0.063083643264361236
     s: 14.53955601162011
-    dkappa: 0
+    dkappa: -0.0044278076082050286
     ddkappa: 0
   }
   v: 4.65409526008602
@@ -979,7 +979,7 @@ trajectory_point {
     theta: -0.71707105351169043
     kappa: -0.065061570648513214
     s: 15.002429290594932
-    dkappa: 0
+    dkappa: -0.0043742654911354392
     ddkappa: 0
   }
   v: 4.6040475382529396
@@ -993,7 +993,7 @@ trajectory_point {
     theta: -0.74771673789193671
     kappa: -0.067019021923084288
     s: 15.460510763733861
-    dkappa: 0
+    dkappa: -0.0043212776584471129
     ddkappa: 0
   }
   v: 4.5583532354198875
@@ -1007,7 +1007,7 @@ trajectory_point {
     theta: -0.778234329953887
     kappa: -0.068905915272922127
     s: 15.914262043105008
-    dkappa: 0
+    dkappa: -0.0042467136799301721
     ddkappa: 0
   }
   v: 4.5175181804750606
@@ -1021,7 +1021,7 @@ trajectory_point {
     theta: -0.81105495127933724
     kappa: -0.070207189783403984
     s: 16.364189451323128
-    dkappa: 0
+    dkappa: -0.0039311522958587163
     ddkappa: 0
   }
   v: 4.4819307554631269
@@ -1035,7 +1035,7 @@ trajectory_point {
     theta: -0.8437137561548308
     kappa: -0.071498964664310577
     s: 16.810832276865263
-    dkappa: 0
+    dkappa: -0.0036178945888613811
     ddkappa: 0
   }
   v: 4.4518618955852238
@@ -1049,7 +1049,7 @@ trajectory_point {
     theta: -0.87618128087683456
     kappa: -0.072782861014764985
     s: 17.254751029386391
-    dkappa: 0
+    dkappa: -0.0033065474395165017
     ddkappa: 0
   }
   v: 4.4274650891989582
@@ -1063,7 +1063,7 @@ trajectory_point {
     theta: -0.90843122172197777
     kappa: -0.07406052734168829
     s: 17.696515695035071
-    dkappa: 0
+    dkappa: -0.0029967110819626181
     ddkappa: 0
   }
   v: 4.408776377818409
@@ -1077,7 +1077,7 @@ trajectory_point {
     theta: -0.94130810190515268
     kappa: -0.074585706819645992
     s: 18.136693991769107
-    dkappa: 0
+    dkappa: -0.0025116459763391335
     ddkappa: 0
   }
   v: 4.395714356114123
@@ -1091,7 +1091,7 @@ trajectory_point {
     theta: -0.97461500573643611
     kappa: -0.074727727721125181
     s: 18.575839624671165
-    dkappa: 0
+    dkappa: -0.0019376672944220969
     ddkappa: 0
   }
   v: 4.3880801719131162
@@ -1105,7 +1105,7 @@ trajectory_point {
     theta: -1.0079261515500753
     kappa: -0.074869585395966581
     s: 19.014480541264447
-    dkappa: 0
+    dkappa: -0.0013643482943507801
     ddkappa: 0
   }
   v: 4.3855575261988786
@@ -1119,7 +1119,7 @@ trajectory_point {
     theta: -1.0412048628532804
     kappa: -0.075011438455517943
     s: 19.453107186828326
-    dkappa: 0
+    dkappa: -0.00079104794701339285
     ddkappa: 0
   }
   v: 4.3877126731113654
@@ -1133,7 +1133,7 @@ trajectory_point {
     theta: -1.0743074714533303
     kappa: -0.074957300362389429
     s: 19.892160759713985
-    dkappa: 0
+    dkappa: -0.00022357215967631992
     ddkappa: 0
   }
   v: 4.3939944199470062
@@ -1147,7 +1147,7 @@ trajectory_point {
     theta: -1.1070273772598427
     kappa: -0.074037930724273643
     s: 20.332025500877265
-    dkappa: 0
+    dkappa: 0.00031679836961303535
     ddkappa: 0
   }
   v: 4.4038963387538042
@@ -1161,7 +1161,7 @@ trajectory_point {
     theta: -1.1399085191406015
     kappa: -0.073116110563389416
     s: 20.773062674236
-    dkappa: 0
+    dkappa: 0.00085860922304462409
     ddkappa: 0
   }
   v: 4.4174771174355216
@@ -1175,7 +1175,7 @@ trajectory_point {
     theta: -1.1729084211034766
     kappa: -0.072191050169189308
     s: 21.215650110220032
-    dkappa: 0
+    dkappa: 0.00140232456246241
     ddkappa: 0
   }
   v: 4.4349329285243684
@@ -1189,7 +1189,7 @@ trajectory_point {
     theta: -1.2059844723789424
     kappa: -0.071261920531959
     s: 21.660184441618053
-    dkappa: 0
+    dkappa: 0.0019484316482669295
     ddkappa: 0
   }
   v: 4.4564436466656172
@@ -1203,7 +1203,7 @@ trajectory_point {
     theta: -1.2368115607967134
     kappa: -0.069855881472782572
     s: 22.107079473788939
-    dkappa: 0
+    dkappa: 0.0023565659584398372
     ddkappa: 0
   }
   v: 4.4821728486175969
@@ -1217,7 +1217,7 @@ trajectory_point {
     theta: -1.2670647519414631
     kappa: -0.0682758613812298
     s: 22.55676455487307
-    dkappa: 0
+    dkappa: 0.0027179393805403214
     ddkappa: 0
   }
   v: 4.5122678132516958
@@ -1231,7 +1231,7 @@ trajectory_point {
     theta: -1.2975483437184825
     kappa: -0.06668448068411012
     s: 23.009682946003593
-    dkappa: 0
+    dkappa: 0.0030819111372650284
     ddkappa: 0
   }
   v: 4.5468595215523617
@@ -1245,7 +1245,7 @@ trajectory_point {
     theta: -1.328236004638774
     kappa: -0.065080138774163632
     s: 23.466290191517768
-    dkappa: 0
+    dkappa: 0.0034488473106188936
     ddkappa: 0
   }
   v: 4.586062656617103
@@ -1259,7 +1259,7 @@ trajectory_point {
     theta: -1.3547064256812271
     kappa: -0.062876641290795718
     s: 23.927052489168261
-    dkappa: 0
+    dkappa: 0.0038097566969555363
     ddkappa: 0
   }
   v: 4.6299756036564839
@@ -1273,7 +1273,7 @@ trajectory_point {
     theta: -1.3805297986012723
     kappa: -0.060525829850929636
     s: 24.392445060334435
-    dkappa: 0
+    dkappa: 0.0041722874164124447
     ddkappa: 0
   }
   v: 4.67868044999413
@@ -1287,7 +1287,7 @@ trajectory_point {
     theta: -1.4066473872229011
     kappa: -0.058149191962449776
     s: 24.862950520233678
-    dkappa: 0
+    dkappa: 0.004538800965303561
     ddkappa: 0
   }
   v: 4.7322429850667254
@@ -1301,7 +1301,7 @@ trajectory_point {
     theta: -1.4330504143391967
     kappa: -0.055744260702507972
     s: 25.339057248132697
-    dkappa: 0
+    dkappa: 0.0049096777804856313
     ddkappa: 0
   }
   v: 4.7907127004240131
@@ -1315,7 +1315,7 @@ trajectory_point {
     theta: -1.4598882261422252
     kappa: -0.053260764896258049
     s: 25.821257757558826
-    dkappa: 0
+    dkappa: 0.005075449180565083
     ddkappa: 0
   }
   v: 4.8541227897287937
@@ -1329,7 +1329,7 @@ trajectory_point {
     theta: -1.4872176009700493
     kappa: -0.050714038570797579
     s: 26.310047066511327
-    dkappa: 0
+    dkappa: 0.0051148252667959594
     ddkappa: 0
   }
   v: 4.922490148756931
@@ -1343,7 +1343,7 @@ trajectory_point {
     theta: -1.5149597941880384
     kappa: -0.048130399056309281
     s: 26.805921067672713
-    dkappa: 0
+    dkappa: 0.0051547720845183534
     ddkappa: 0
   }
   v: 4.9958153753973438
@@ -1357,7 +1357,7 @@ trajectory_point {
     theta: -1.5430995953291107
     kappa: -0.045507266550250218
     s: 27.309374898620032
-    dkappa: 0
+    dkappa: 0.005195329521224837
     ddkappa: 0
   }
   v: 5.07408276965201
@@ -1371,7 +1371,7 @@ trajectory_point {
     theta: -1.5663914448738316
     kappa: -0.043008899056202615
     s: 27.820901312036185
-    dkappa: 0
+    dkappa: 0.0051260795695923962
     ddkappa: 0
   }
   v: 5.15726033363597
@@ -1385,7 +1385,7 @@ trajectory_point {
     theta: -1.587048527519564
     kappa: -0.040565235613619854
     s: 28.34098904592123
-    dkappa: 0
+    dkappa: 0.0049917640127562658
     ddkappa: 0
   }
   v: 5.2452997715773222
@@ -1399,7 +1399,7 @@ trajectory_point {
     theta: -1.6080697678849731
     kappa: -0.0380790764488635
     s: 28.870121193803694
-    dkappa: 0
+    dkappa: 0.0048551126855814562
     ddkappa: 0
   }
   v: 5.3381364898172183
@@ -1413,7 +1413,7 @@ trajectory_point {
     theta: -1.6294552545141168
     kappa: -0.035548185896534377
     s: 29.408773574951859
-    dkappa: 0
+    dkappa: 0.0047160027050891758
     ddkappa: 0
   }
   v: 5.4356895968098788
@@ -1427,7 +1427,7 @@ trajectory_point {
     theta: -1.6476640484832306
     kappa: -0.033307706860982424
     s: 29.957413104585093
-    dkappa: 0
+    dkappa: 0.0045285807905482813
     ddkappa: 0
   }
   v: 5.5378619031225753
@@ -1441,7 +1441,7 @@ trajectory_point {
     theta: -1.6657200522345721
     kappa: -0.031072838932477656
     s: 30.516496164085147
-    dkappa: 0
+    dkappa: 0.0043310487263190159
     ddkappa: 0
   }
   v: 5.6445399214356415
@@ -1455,7 +1455,7 @@ trajectory_point {
     theta: -1.6841265671220718
     kappa: -0.028794448529876886
     s: 31.086466971207443
-    dkappa: 0
+    dkappa: 0.0041296698646844567
     ddkappa: 0
   }
   v: 5.7555938665424708
@@ -1469,7 +1469,7 @@ trajectory_point {
     theta: -1.700678986129418
     kappa: -0.026614686930614414
     s: 31.66775595029241
-    dkappa: 0
+    dkappa: 0.003925058492064112
     ddkappa: 0
   }
   v: 5.8708776553495161
@@ -1483,7 +1483,7 @@ trajectory_point {
     theta: -1.7134890005659931
     kappa: -0.024656588424256169
     s: 32.260778102476763
-    dkappa: 0
+    dkappa: 0.0037177321853409488
     ddkappa: 0
   }
   v: 5.9902289068762844
@@ -1497,7 +1497,7 @@ trajectory_point {
     theta: -1.7265624564147741
     kappa: -0.022658434196032758
     s: 32.865931375904836
-    dkappa: 0
+    dkappa: 0.0035061647207544457
     ddkappa: 0
   }
   v: 6.1134689422553485
@@ -1511,7 +1511,7 @@ trajectory_point {
     theta: -1.7398558549618586
     kappa: -0.020639274790220718
     s: 33.483595035939857
-    dkappa: 0
+    dkappa: 0.0032916931314151787
     ddkappa: 0
   }
   v: 6.2404027847323364
@@ -1525,7 +1525,7 @@ trajectory_point {
     theta: -1.7525833029077118
     kappa: -0.018940690488283809
     s: 34.11412803537528
-    dkappa: 0
+    dkappa: 0.0030990031218713571
     ddkappa: 0
   }
   v: 6.3708191596659356
@@ -1539,7 +1539,7 @@ trajectory_point {
     theta: -1.7655795270365173
     kappa: -0.017206529779503269
     s: 34.757867384646076
-    dkappa: 0
+    dkappa: 0.0029022772701462461
     ddkappa: 0
   }
   v: 6.5044904945278939
@@ -1553,7 +1553,7 @@ trajectory_point {
     theta: -1.7788464646624347
     kappa: -0.015435948296723411
     s: 35.415126522040055
-    dkappa: 0
+    dkappa: 0.0027014197899340943
     ddkappa: 0
   }
   v: 6.641172918903016
@@ -1567,7 +1567,7 @@ trajectory_point {
     theta: -1.7874492572743705
     kappa: -0.013977190694281398
     s: 36.086193683909144
-    dkappa: 0
+    dkappa: 0.002518774372317414
     ddkappa: 0
   }
   v: 6.7806062644891671
@@ -1581,7 +1581,7 @@ trajectory_point {
     theta: -1.7960406835296368
     kappa: -0.012501438125669447
     s: 36.771330274880704
-    dkappa: 0
+    dkappa: 0.0023331730272892659
     ddkappa: 0
   }
   v: 6.922514065097273
@@ -1595,7 +1595,7 @@ trajectory_point {
     theta: -1.8046872385102819
     kappa: -0.011013197178219756
     s: 37.470769238068883
-    dkappa: 0
+    dkappa: 0.00214477022541389
     ddkappa: 0
   }
   v: 7.0666035566513141
@@ -1609,7 +1609,7 @@ trajectory_point {
     theta: -1.811308694958065
     kappa: -0.0098208160450572318
     s: 38.184713425285828
-    dkappa: 0
+    dkappa: 0.0019715984081411365
     ddkappa: 0
   }
   v: 7.2125656771883344
@@ -1623,7 +1623,7 @@ trajectory_point {
     theta: -1.8180664686566046
     kappa: -0.0086039234601297046
     s: 38.913333967253088
-    dkappa: 0
+    dkappa: 0.0017948667453390587
     ddkappa: 0
   }
   v: 7.3600750668584354
@@ -1637,7 +1637,7 @@ trajectory_point {
     theta: -1.824363708065738
     kappa: -0.007467850751950271
     s: 39.656768643812825
-    dkappa: 0
+    dkappa: 0.0016172470104514282
     ddkappa: 0
   }
   v: 7.5087900679247754
@@ -1651,7 +1651,7 @@ trajectory_point {
     theta: -1.8294195564843232
     kappa: -0.00655059618828149
     s: 40.415120254139204
-    dkappa: 0
+    dkappa: 0.0014422551108586633
     ddkappa: 0
   }
   v: 7.6583527247635752
@@ -1665,7 +1665,7 @@ trajectory_point {
     theta: -1.8345752976936467
     kappa: -0.0056152189790095279
     s: 41.188454986949679
-    dkappa: 0
+    dkappa: 0.0012638058109919084
     ddkappa: 0
   }
   v: 7.8083887838641131
diff --git a/modules/planning/testdata/sunnyvale_loop_test/result_slowdown_01_0.pb.txt b/modules/planning/testdata/sunnyvale_loop_test/result_slowdown_01_0.pb.txt
index 4bdc53eb1be00b867fb14fad7c370465a3a1ad91..524d4ac06e3a6c32dd38d0dc2c00e95b0499bdd8 100644
--- a/modules/planning/testdata/sunnyvale_loop_test/result_slowdown_01_0.pb.txt
+++ b/modules/planning/testdata/sunnyvale_loop_test/result_slowdown_01_0.pb.txt
@@ -12,7 +12,7 @@ trajectory_point {
     theta: -1.7938388115138242
     kappa: 0.0010060738435035618
     s: 1.8110513089197866e-15
-    dkappa: 0
+    dkappa: -5.2829713626945012e-18
     ddkappa: 0
   }
   v: 1.7111110687255817
@@ -26,7 +26,7 @@ trajectory_point {
     theta: -1.79400082323139
     kappa: 0.00080808592146547982
     s: 0.034112174979151554
-    dkappa: 0
+    dkappa: -9.9507751462641416e-05
     ddkappa: 0
   }
   v: 1.7001835974338126
@@ -40,7 +40,7 @@ trajectory_point {
     theta: -1.7941618190347728
     kappa: 0.00061133979677582861
     s: 0.068010395450358335
-    dkappa: 0
+    dkappa: -0.00019839138171302086
     ddkappa: 0
   }
   v: 1.689713862845402
@@ -54,7 +54,7 @@ trajectory_point {
     theta: -1.7943218418923492
     kappa: 0.00041578294414334452
     s: 0.10170371121781202
-    dkappa: 0
+    dkappa: -0.00029667728970303285
     ddkappa: 0
   }
   v: 1.6796913907364079
@@ -68,7 +68,7 @@ trajectory_point {
     theta: -1.7944809337834848
     kappa: 0.00022136404759562247
     s: 0.13520096372705082
-    dkappa: 0
+    dkappa: -0.00039439126658688328
     ddkappa: 0
   }
   v: 1.6701058194655996
@@ -82,7 +82,7 @@ trajectory_point {
     theta: -1.7946391357091931
     kappa: 2.8032987410456993e-05
     s: 0.1685107883166154
-    dkappa: 0
+    dkappa: -0.00049155850228933731
     ddkappa: 0
   }
   v: 1.6609468999744574
@@ -96,7 +96,7 @@ trajectory_point {
     theta: -1.7947964877027935
     kappa: -0.00016425917295281422
     s: 0.20164161646970316
-    dkappa: 0
+    dkappa: -0.00058820359207396227
     ddkappa: 0
   }
   v: 1.6522044957871731
@@ -110,7 +110,7 @@ trajectory_point {
     theta: -1.7949530288405751
     kappa: -0.00035556019992196808
     s: 0.23460167806582236
-    dkappa: 0
+    dkappa: -0.00068435054311137035
     ddkappa: 0
   }
   v: 1.643868583010649
@@ -124,7 +124,7 @@ trajectory_point {
     theta: -1.7951087972524575
     kappa: -0.00054591670288055554
     s: 0.26739900363244656
-    dkappa: 0
+    dkappa: -0.00078002278104746185
     ddkappa: 0
   }
   v: 1.6359292503344984
@@ -138,7 +138,7 @@ trajectory_point {
     theta: -1.7952638301326573
     kappa: -0.00073537414723655792
     s: 0.30004142659666855
-    dkappa: 0
+    dkappa: -0.00087524315657166746
     ddkappa: 0
   }
   v: 1.6283766990310464
@@ -152,7 +152,7 @@ trajectory_point {
     theta: -1.7954181637503539
     kappa: -0.00092397686749104532
     s: 0.33253658553685495
-    dkappa: 0
+    dkappa: -0.00097003395198519177
     ddkappa: 0
   }
   v: 1.6212012429553289
@@ -166,7 +166,7 @@ trajectory_point {
     theta: -1.7955718334603563
     kappa: -0.0011117680803068328
     s: 0.36489192643429985
-    dkappa: 0
+    dkappa: -0.0010644168877692549
     ddkappa: 0
   }
   v: 1.6143933085450923
@@ -180,7 +180,7 @@ trajectory_point {
     theta: -1.795724873713773
     kappa: -0.0012987898975771405
     s: 0.39711470492487977
-    dkappa: 0
+    dkappa: -0.001158413129153337
     ddkappa: 0
   }
   v: 1.6079434348207948
@@ -194,7 +194,7 @@ trajectory_point {
     theta: -1.795877318068682
     kappa: -0.0014850833394942512
     s: 0.4292119885507073
-    dkappa: 0
+    dkappa: -0.0012520432926834196
     ddkappa: 0
   }
   v: 1.6018422733856055
@@ -208,7 +208,7 @@ trajectory_point {
     theta: -1.7960291992008028
     kappa: -0.0016706883476181654
     s: 0.46119065901178558
-    dkappa: 0
+    dkappa: -0.0013453274527902295
     ddkappa: 0
   }
   v: 1.5960805884254043
@@ -222,7 +222,7 @@ trajectory_point {
     theta: -1.7961805489141678
     kappa: -0.0018556437979452619
     s: 0.49305741441766232
-    dkappa: 0
+    dkappa: -0.0014382851483574805
     ddkappa: 0
   }
   v: 1.5906492567087824
@@ -236,7 +236,7 @@ trajectory_point {
     theta: -1.7963313981517972
     kappa: -0.0020399875139769548
     s: 0.52481877153908441
-    dkappa: 0
+    dkappa: -0.0015309353892901177
     ddkappa: 0
   }
   v: 1.5855392675870423
@@ -250,7 +250,7 @@ trajectory_point {
     theta: -1.7964817770063735
     kappa: -0.0022237562797883486
     s: 0.55648106805965158
-    dkappa: 0
+    dkappa: -0.001623296663082558
     ddkappa: 0
   }
   v: 1.5807417229941971
@@ -264,7 +264,7 @@ trajectory_point {
     theta: -1.7966317147309163
     kappa: -0.0024069858530969043
     s: 0.58805046482747136
-    dkappa: 0
+    dkappa: -0.0017153869413869369
     ddkappa: 0
   }
   v: 1.5762478374469715
@@ -278,7 +278,7 @@ trajectory_point {
     theta: -1.7967812397494607
     kappa: -0.0025897109783310848
     s: 0.61953294810681259
-    dkappa: 0
+    dkappa: -0.0018072236865813462
     ddkappa: 0
   }
   v: 1.5720489380448008
@@ -292,7 +292,7 @@ trajectory_point {
     theta: -1.7969303796677336
     kappa: -0.0027719653996990221
     s: 0.65093433182975979
-    dkappa: 0
+    dkappa: -0.0018988238583380807
     ddkappa: 0
   }
   v: 1.5681364644698317
@@ -306,7 +306,7 @@ trajectory_point {
     theta: -1.7970791612838324
     kappa: -0.0029537818742571722
     s: 0.68226025984786787
-    dkappa: 0
+    dkappa: -0.0019902039201918796
     ddkappa: 0
   }
   v: 1.5645019689869217
@@ -320,7 +320,7 @@ trajectory_point {
     theta: -1.7972276105989042
     kappa: -0.0031351921849789724
     s: 0.71351620818381589
-    dkappa: 0
+    dkappa: -0.0020813798461081709
     ddkappa: 0
   }
   v: 1.5611371164436398
@@ -334,7 +334,7 @@ trajectory_point {
     theta: -1.7973757528278265
     kappa: -0.0033162271538234997
     s: 0.74470748728306113
-    dkappa: 0
+    dkappa: -0.00217236712705131
     ddkappa: 0
   }
   v: 1.558033684270266
@@ -348,7 +348,7 @@ trajectory_point {
     theta: -1.7975236124098866
     kappa: -0.0034969166548041281
     s: 0.77583924426549411
-    dkappa: 0
+    dkappa: -0.0022631807775528277
     ddkappa: 0
   }
   v: 1.5551835624797907
@@ -362,7 +362,7 @@ trajectory_point {
     theta: -1.7976712130194643
     kappa: -0.0036772896270571859
     s: 0.80691646517709181
-    dkappa: 0
+    dkappa: -0.0023538353422796698
     ddkappa: 0
   }
   v: 1.5525787536679165
@@ -376,7 +376,7 @@ trajectory_point {
     theta: -1.7978185775767133
     kappa: -0.0038573740879106165
     s: 0.83794397724157244
-    dkappa: 0
+    dkappa: -0.0024443449026024413
     ddkappa: 0
   }
   v: 1.550211373013056
@@ -390,7 +390,7 @@ trajectory_point {
     theta: -1.797965728258244
     kappa: -0.0040371971459526312
     s: 0.86892645111204991
-    dkappa: 0
+    dkappa: -0.0025347230831636483
     ddkappa: 0
   }
   v: 1.5480736482763335
@@ -404,7 +404,7 @@ trajectory_point {
     theta: -1.7981126865078072
     kappa: -0.00421678501410037
     s: 0.89986840312268734
-    dkappa: 0
+    dkappa: -0.0026249830584459421
     ddkappa: 0
   }
   v: 1.5461579198015847
@@ -418,7 +418,7 @@ trajectory_point {
     theta: -1.7982594730469779
     kappa: -0.0043961630226685595
     s: 0.93077419754035173
-    dkappa: 0
+    dkappa: -0.0027151375593403595
     ddkappa: 0
   }
   v: 1.5444566405153555
@@ -432,7 +432,7 @@ trajectory_point {
     theta: -1.7984061078858395
     kappa: -0.0045753556324381736
     s: 0.96164804881626842
-    dkappa: 0
+    dkappa: -0.0028051988797145691
     ddkappa: 0
   }
   v: 1.5429623759269033
@@ -446,7 +446,7 @@ trajectory_point {
     theta: -1.7985526103336691
     kappa: -0.0047543864477250794
     s: 0.9924940238376746
-    dkappa: 0
+    dkappa: -0.0028951788829811118
     ddkappa: 0
   }
   v: 1.5416678041281968
@@ -460,7 +460,7 @@ trajectory_point {
     theta: -1.7986989990096229
     kappa: -0.0049332782294487093
     s: 1.023316044179474
-    dkappa: 0
+    dkappa: -0.0029850890086656431
     ddkappa: 0
   }
   v: 1.5405657157939154
@@ -474,7 +474,7 @@ trajectory_point {
     theta: -1.798845291853423
     kappa: -0.0051120529082007166
     s: 1.0541178883558917
-    dkappa: 0
+    dkappa: -0.003074940278975181
     ddkappa: 0
   }
   v: 1.53964901418145
@@ -488,7 +488,7 @@ trajectory_point {
     theta: -1.7989915061360422
     kappa: -0.0052907315973136186
     s: 1.0849031940721265
-    dkappa: 0
+    dkappa: -0.0031647433053663384
     ddkappa: 0
   }
   v: 1.5389107151309025
@@ -502,7 +502,7 @@ trajectory_point {
     theta: -1.7991376584703929
     kappa: -0.0054693346059294752
     s: 1.1156754604760075
-    dkappa: 0
+    dkappa: -0.0032545082951135778
     ddkappa: 0
   }
   v: 1.5383439470650853
@@ -516,7 +516,7 @@ trajectory_point {
     theta: -1.799283764822013
     kappa: -0.0056478814520685315
     s: 1.1464380504096467
-    dkappa: 0
+    dkappa: -0.0033442450578774468
     ddkappa: 0
   }
   v: 1.5379419509895225
@@ -530,7 +530,7 @@ trajectory_point {
     theta: -1.7994298405197553
     kappa: -0.0058263908756978821
     s: 1.1771941926610934
-    dkappa: 0
+    dkappa: -0.0034339630122728225
     ddkappa: 0
   }
   v: 1.5376980804924492
@@ -544,7 +544,7 @@ trajectory_point {
     theta: -1.7995759002664737
     kappa: -0.00600488085180013
     s: 1.2079469842159893
-    dkappa: 0
+    dkappa: -0.0035236711924371568
     ddkappa: 0
   }
   v: 1.5376058017448113
@@ -558,7 +558,7 @@ trajectory_point {
     theta: -1.7997219581497137
     kappa: -0.0061833686034420349
     s: 1.2386993925092213
-    dkappa: 0
+    dkappa: -0.0036133782545987123
     ddkappa: 0
   }
   v: 1.5376586935002663
@@ -572,7 +572,7 @@ trajectory_point {
     theta: -1.7998680276524006
     kappa: -0.0063618706148431872
     s: 1.2694542576765779
-    dkappa: 0
+    dkappa: -0.0037030924836448154
     ddkappa: 0
   }
   v: 1.5378504470951821
@@ -586,7 +586,7 @@ trajectory_point {
     theta: -1.8000141216635286
     kappa: -0.006540402644444652
     s: 1.3002142948064008
-    dkappa: 0
+    dkappa: -0.0037928217996900915
     ddkappa: 0
   }
   v: 1.5381748664486383
@@ -600,7 +600,7 @@ trajectory_point {
     theta: -1.8001602524888511
     kappa: -0.0067189797379776268
     s: 1.3309820961912406
-    dkappa: 0
+    dkappa: -0.0038825737646447076
     ddkappa: 0
   }
   v: 1.5386258680624252
@@ -614,7 +614,7 @@ trajectory_point {
     theta: -1.8003064318615709
     kappa: -0.0068976162415321151
     s: 1.3617601335795118
-    dkappa: 0
+    dkappa: -0.0039723555887826228
     ddkappa: 0
   }
   v: 1.539197481021044
@@ -628,7 +628,7 @@ trajectory_point {
     theta: -1.8004526709530291
     kappa: -0.00707632581462556
     s: 1.3925507604271445
-    dkappa: 0
+    dkappa: -0.00406217413730982
     ddkappa: 0
   }
   v: 1.5398838469917078
@@ -642,7 +642,7 @@ trajectory_point {
     theta: -1.8005989803833982
     kappa: -0.0072551214432715269
     s: 1.4233562141492415
-    dkappa: 0
+    dkappa: -0.0041520359369325594
     ddkappa: 0
   }
   v: 1.54067922022434
@@ -656,7 +656,7 @@ trajectory_point {
     theta: -1.8007453702323712
     kappa: -0.0074340154530483406
     s: 1.4541786183717305
-    dkappa: 0
+    dkappa: -0.0042419471824256131
     ddkappa: 0
   }
   v: 1.5415779675515755
@@ -670,7 +670,7 @@ trajectory_point {
     theta: -1.8008918500498536
     kappa: -0.0076130195221677556
     s: 1.485019985183019
-    dkappa: 0
+    dkappa: -0.004331913743200513
     ddkappa: 0
   }
   v: 1.54257456838876
@@ -684,7 +684,7 @@ trajectory_point {
     theta: -1.8010384288666552
     kappa: -0.00779214469454361
     s: 1.5158822173856492
-    dkappa: 0
+    dkappa: -0.0044219411698737925
     ddkappa: 0
   }
   v: 1.5436636147339504
@@ -698,7 +698,7 @@ trajectory_point {
     theta: -1.8011851152051817
     kappa: -0.0079714013928604815
     s: 1.5467671107479506
-    dkappa: 0
+    dkappa: -0.0045120347008352274
     ddkappa: 0
   }
   v: 1.5448398111679149
@@ -712,7 +712,7 @@ trajectory_point {
     theta: -1.8013319170901265
     kappa: -0.00815079943164235
     s: 1.5776763562556957
-    dkappa: 0
+    dkappa: -0.0046021992688160809
     ddkappa: 0
   }
   v: 1.5460979748541321
@@ -726,7 +726,7 @@ trajectory_point {
     theta: -1.8020679070828434
     kappa: -0.0090502123162746854
     s: 1.7326399994075175
-    dkappa: 0
+    dkappa: -0.0050542397411084882
     ddkappa: 0
   }
   v: 1.5534447835805576
@@ -740,7 +740,7 @@ trajectory_point {
     theta: -1.8028077219681498
     kappa: -0.0099543040689342426
     s: 1.8884097844856043
-    dkappa: 0
+    dkappa: -0.0055086317893555644
     ddkappa: 0
   }
   v: 1.5621280831940427
@@ -754,7 +754,7 @@ trajectory_point {
     theta: -1.8040815948262834
     kappa: -0.010766274688010058
     s: 2.0450927142596185
-    dkappa: 0
+    dkappa: -0.0057551009980415591
     ddkappa: 0
   }
   v: 1.5716266746794398
@@ -768,7 +768,7 @@ trajectory_point {
     theta: -1.8063373835392109
     kappa: -0.011404166242015576
     s: 2.2027471898074276
-    dkappa: 0
+    dkappa: -0.0056159237397898441
     ddkappa: 0
   }
   v: 1.5814897232160283
@@ -782,7 +782,7 @@ trajectory_point {
     theta: -1.8086074294494225
     kappa: -0.012046056922968729
     s: 2.3613900469345492
-    dkappa: 0
+    dkappa: -0.0054758739389868492
     ddkappa: 0
   }
   v: 1.5913367581775142
@@ -796,7 +796,7 @@ trajectory_point {
     theta: -1.8108914579185678
     kappa: -0.012691875142381744
     s: 2.5210035925935932
-    dkappa: 0
+    dkappa: -0.0053349672150421572
     ddkappa: 0
   }
   v: 1.6008576731320296
@@ -810,7 +810,7 @@ trajectory_point {
     theta: -1.8131887999816272
     kappa: -0.013341438073623774
     s: 2.6815426413037033
-    dkappa: 0
+    dkappa: -0.0051932434576659452
     ddkappa: 0
   }
   v: 1.6098127258421338
@@ -824,7 +824,7 @@ trajectory_point {
     theta: -1.8154984930270426
     kappa: -0.013994480122235612
     s: 2.8429415515700014
-    dkappa: 0
+    dkappa: -0.0050507606151229435
     ddkappa: 0
   }
   v: 1.618032538264812
@@ -838,7 +838,7 @@ trajectory_point {
     theta: -1.817819381543369
     kappa: -0.014650681396244376
     s: 3.0051212623030277
-    dkappa: 0
+    dkappa: -0.0049075884824863816
     ddkappa: 0
   }
   v: 1.6254180965514764
@@ -852,7 +852,7 @@ trajectory_point {
     theta: -1.8201502179117779
     kappa: -0.015309696176478229
     s: 3.1679963292381879
-    dkappa: 0
+    dkappa: -0.0047638024898919258
     ddkappa: 0
   }
   v: 1.6319407510479667
@@ -866,7 +866,7 @@ trajectory_point {
     theta: -1.8224897445804897
     kappa: -0.015971176115967673
     s: 3.3314806586524361
-    dkappa: 0
+    dkappa: -0.0046194786408167042
     ddkappa: 0
   }
   v: 1.6375897344125083
@@ -880,7 +880,7 @@ trajectory_point {
     theta: -1.8248365812708041
     kappa: -0.016634738310481262
     s: 3.4954796160182955
-    dkappa: 0
+    dkappa: -0.0044747004785538865
     ddkappa: 0
   }
   v: 1.6421997650904587
@@ -894,7 +894,7 @@ trajectory_point {
     theta: -1.8271890769151795
     kappa: -0.017299923387666657
     s: 3.6598796677628043
-    dkappa: 0
+    dkappa: -0.0043295682304604786
     ddkappa: 0
   }
   v: 1.6455823289219222
@@ -908,7 +908,7 @@ trajectory_point {
     theta: -1.8295453288082801
     kappa: -0.0179662008609284
     s: 3.82454970447486
-    dkappa: 0
+    dkappa: -0.004184197639830827
     ddkappa: 0
   }
   v: 1.6475744233040024
@@ -922,7 +922,7 @@ trajectory_point {
     theta: -1.8319563451133403
     kappa: -0.018621013105480682
     s: 3.989343592060921
-    dkappa: 0
+    dkappa: -0.00404013031614548
     ddkappa: 0
   }
   v: 1.6480385571908029
@@ -936,7 +936,7 @@ trajectory_point {
     theta: -1.8353554366039648
     kappa: -0.019053049730686503
     s: 4.1541027229007073
-    dkappa: 0
+    dkappa: -0.0039223752752688837
     ddkappa: 0
   }
   v: 1.6468627510934284
@@ -950,7 +950,7 @@ trajectory_point {
     theta: -1.8387507259753122
     kappa: -0.019484553290953142
     s: 4.3186585670028972
-    dkappa: 0
+    dkappa: -0.003804765525508921
     ddkappa: 0
   }
   v: 1.6439605370799819
@@ -964,7 +964,7 @@ trajectory_point {
     theta: -1.842138503842462
     kappa: -0.019915062532550338
     s: 4.48283522316083
-    dkappa: 0
+    dkappa: -0.0036874267852623514
     ddkappa: 0
   }
   v: 1.6392709587755678
@@ -978,7 +978,7 @@ trajectory_point {
     theta: -1.84551496105119
     kappa: -0.020344103562434365
     s: 4.6464519701082079
-    dkappa: 0
+    dkappa: -0.0035704882178719992
     ddkappa: 0
   }
   v: 1.6327585713622896
@@ -992,7 +992,7 @@ trajectory_point {
     theta: -1.8488762421527907
     kappa: -0.020771196537969536
     s: 4.8093258176747886
-    dkappa: 0
+    dkappa: -0.0034540806082896127
     ddkappa: 0
   }
   v: 1.624413441579251
@@ -1006,7 +1006,7 @@ trajectory_point {
     theta: -1.8522184989024291
     kappa: -0.021195862356649688
     s: 4.9712740579420949
-    dkappa: 0
+    dkappa: -0.0033383345397387078
     ddkappa: 0
   }
   v: 1.6142511477225561
@@ -1020,7 +1020,7 @@ trajectory_point {
     theta: -1.8555379437485973
     kappa: -0.021617629345819685
     s: 5.1321168163991073
-    dkappa: 0
+    dkappa: -0.0032233785703774248
     ddkappa: 0
   }
   v: 1.6023127796453085
@@ -1034,7 +1034,7 @@ trajectory_point {
     theta: -1.8588309032821622
     kappa: -0.022036039952396912
     s: 5.2916796030979683
-    dkappa: 0
+    dkappa: -0.0031093374099613815
     ddkappa: 0
   }
   v: 1.5886649387576117
@@ -1048,7 +1048,7 @@ trajectory_point {
     theta: -1.8620938716151321
     kappa: -0.022450657432592763
     s: 5.449795863809678
-    dkappa: 0
+    dkappa: -0.0029963300965065221
     ddkappa: 0
   }
   v: 1.5733997380265696
@@ -1062,7 +1062,7 @@ trajectory_point {
     theta: -1.8653235636615513
     kappa: -0.02286107254163414
     s: 5.6063095311798
-    dkappa: 0
+    dkappa: -0.0028844681729519694
     ddkappa: 0
   }
   v: 1.5566348019762859
@@ -1076,7 +1076,7 @@ trajectory_point {
     theta: -1.8685169682957972
     kappa: -0.023266910223484948
     s: 5.7610775758841557
-    dkappa: 0
+    dkappa: -0.0027738538638228793
     ddkappa: 0
   }
   v: 1.5385132666878643
@@ -1090,7 +1090,7 @@ trajectory_point {
     theta: -1.8716714013668889
     kappa: -0.0236678363005676
     s: 5.9139725577845308
-    dkappa: 0
+    dkappa: -0.0026645782518932891
     ddkappa: 0
   }
   v: 1.519203779799408
@@ -1104,7 +1104,7 @@ trajectory_point {
     theta: -1.8751256034225556
     kappa: -0.023962682155876072
     s: 6.0648851770843653
-    dkappa: 0
+    dkappa: -0.0025716121423810831
     ddkappa: 0
   }
   v: 1.498900500506021
@@ -1118,7 +1118,7 @@ trajectory_point {
     theta: -1.878750206347239
     kappa: -0.024189167594028622
     s: 6.213726825484466
-    dkappa: 0
+    dkappa: -0.0024894161845883168
     ddkappa: 0
   }
   v: 1.4778230995598067
@@ -1132,7 +1132,7 @@ trajectory_point {
     theta: -1.8823232590968237
     kappa: -0.024412402267870018
     s: 6.3604321373386972
-    dkappa: 0
+    dkappa: -0.0024083999922069311
     ddkappa: 0
   }
   v: 1.4562167592698672
@@ -1146,7 +1146,7 @@ trajectory_point {
     theta: -1.8858436299473331
     kappa: -0.024632322335709361
     s: 6.5049591571792478
-    dkappa: 0
+    dkappa: -0.002328586734624652
     ddkappa: 0
   }
   v: 1.4342568267533591
@@ -1160,7 +1160,7 @@ trajectory_point {
     theta: -1.8893101885801362
     kappa: -0.024848863558625964
     s: 6.6472656684869289
-    dkappa: 0
+    dkappa: -0.0022499997253915957
     ddkappa: 0
   }
   v: 1.4117627124222538
@@ -1174,7 +1174,7 @@ trajectory_point {
     theta: -1.8927212291505495
     kappa: -0.025061925265673853
     s: 6.787285512352506
-    dkappa: 0
+    dkappa: -0.002172675499946394
     ddkappa: 0
   }
   v: 1.3884838230140264
@@ -1188,7 +1188,7 @@ trajectory_point {
     theta: -1.8960744738398083
     kappa: -0.025271370581295454
     s: 6.9249287369283872
-    dkappa: 0
+    dkappa: -0.0020966637330833509
     ddkappa: 0
   }
   v: 1.364194893024008
@@ -1202,7 +1202,7 @@ trajectory_point {
     theta: -1.8993671346428633
     kappa: -0.025477030279329124
     s: 7.0600841302044
-    dkappa: 0
+    dkappa: -0.0020220258402583718
     ddkappa: 0
   }
   v: 1.338695984705387
@@ -1216,7 +1216,7 @@ trajectory_point {
     theta: -1.902595975295448
     kappa: -0.025678706637016692
     s: 7.1926217527835918
-    dkappa: 0
+    dkappa: -0.0019488335788948813
     ddkappa: 0
   }
   v: 1.3118124880692086
@@ -1230,7 +1230,7 @@ trajectory_point {
     theta: -1.9057573732894513
     kappa: -0.025876177289010968
     s: 7.322395470658
-    dkappa: 0
+    dkappa: -0.0018771676496897594
     ddkappa: 0
   }
   v: 1.2833951208843741
@@ -1244,7 +1244,7 @@ trajectory_point {
     theta: -1.9088473819283283
     kappa: -0.026069199081383297
     s: 7.4492454879844487
-    dkappa: 0
+    dkappa: -0.0018071162979192595
     ddkappa: 0
   }
   v: 1.2533199286776422
@@ -1258,7 +1258,7 @@ trajectory_point {
     theta: -1.9118617923773362
     kappa: -0.026257511925631073
     s: 7.5730008798603308
-    dkappa: 0
+    dkappa: -0.0017387739147449379
     ddkappa: 0
   }
   v: 1.2214882847336281
@@ -1272,7 +1272,7 @@ trajectory_point {
     theta: -1.9147961956669606
     kappa: -0.026440842652685256
     s: 7.6934821250993934
-    dkappa: 0
+    dkappa: -0.0016722396385195795
     ddkappa: 0
   }
   v: 1.1878268900948035
@@ -1286,7 +1286,7 @@ trajectory_point {
     theta: -1.9176460446121073
     kappa: -0.026618908866917937
     s: 7.8105036390075213
-    dkappa: 0
+    dkappa: -0.0016076159560931245
     ddkappa: 0
   }
   v: 1.1522877735614967
@@ -1300,7 +1300,7 @@ trajectory_point {
     theta: -1.9204067156145592
     kappa: -0.026791422800149824
     s: 7.9238763061585287
-    dkappa: 0
+    dkappa: -0.0015450073041185922
     ddkappa: 0
   }
   v: 1.1148482916918934
@@ -1314,7 +1314,7 @@ trajectory_point {
     theta: -1.9234595294465
     kappa: -0.026905698564511284
     s: 8.033410013169938
-    dkappa: 0
+    dkappa: -0.0014953535213824462
     ddkappa: 0
   }
   v: 1.0755111288020351
@@ -1328,7 +1328,7 @@ trajectory_point {
     theta: -1.9266342813395185
     kappa: -0.026984032586791516
     s: 8.13891618147877
-    dkappa: 0
+    dkappa: -0.0014540888356736048
     ddkappa: 0
   }
   v: 1.0343042969658209
@@ -1342,7 +1342,7 @@ trajectory_point {
     theta: -1.9296827609387539
     kappa: -0.027059239334164459
     s: 8.2402103001173277
-    dkappa: 0
+    dkappa: -0.0014144715314270977
     ddkappa: 0
   }
   v: 0.99128113601500567
@@ -1356,7 +1356,7 @@ trajectory_point {
     theta: -1.9325995022989351
     kappa: -0.027131186715275986
     s: 8.3371144584889834
-    dkappa: 0
+    dkappa: -0.0013765711915400371
     ddkappa: 0
   }
   v: 0.946520313539202
@@ -1370,7 +1370,7 @@ trajectory_point {
     theta: -1.9353793279391074
     kappa: -0.027199749419760624
     s: 8.4294598791439643
-    dkappa: 0
+    dkappa: -0.0013404538268299063
     ddkappa: 0
   }
   v: 0.90012582488587833
@@ -1384,7 +1384,7 @@ trajectory_point {
     theta: -1.9380174248393289
     kappa: -0.027264810798724139
     s: 8.5170894505551331
-    dkappa: 0
+    dkappa: -0.0013061808854365863
     ddkappa: 0
   }
   v: 0.85222699316036055
@@ -1398,7 +1398,7 @@ trajectory_point {
     theta: -1.9405094202271036
     kappa: -0.027326264745226182
     s: 8.5998602598937826
-    dkappa: 0
+    dkappa: -0.0012738082622243767
     ddkappa: 0
   }
   v: 0.80297846922583038
@@ -1412,7 +1412,7 @@ trajectory_point {
     theta: -1.9428514571614852
     kappa: -0.027384017574762869
     s: 8.6776461258054134
-    dkappa: 0
+    dkappa: -0.001243385308184022
     ddkappa: 0
   }
   v: 0.75256023170332709
@@ -1426,7 +1426,7 @@ trajectory_point {
     theta: -1.9450402699331255
     kappa: -0.027437989905749402
     s: 8.750340131185526
-    dkappa: 0
+    dkappa: -0.0012149538398347345
     ddkappa: 0
   }
   v: 0.70117758697174615
@@ -1440,7 +1440,7 @@ trajectory_point {
     theta: -1.9470732593076239
     kappa: -0.02748811854000268
     s: 8.817857155955398
-    dkappa: 0
+    dkappa: -0.0011885471486262206
     ddkappa: 0
   }
   v: 0.64906116916784
@@ -1454,7 +1454,7 @@ trajectory_point {
     theta: -1.9489485676469731
     kappa: -0.027534358343223916
     s: 8.8801364098378812
-    dkappa: 0
+    dkappa: -0.0011641890103407013
     ddkappa: 0
   }
   v: 0.5964669401862176
@@ -1468,7 +1468,7 @@ trajectory_point {
     theta: -1.950665153949394
     kappa: -0.027576684125481234
     s: 8.93714396513318
-    dkappa: 0
+    dkappa: -0.001141892694494939
     ddkappa: 0
   }
   v: 0.54367618967934483
@@ -1482,7 +1482,7 @@ trajectory_point {
     theta: -1.9522228688511467
     kappa: -0.027615092521692287
     s: 8.9888752894946329
-    dkappa: 0
+    dkappa: -0.0011216599737422623
     ddkappa: 0
   }
   v: 0.49099553505754423
@@ -1496,7 +1496,7 @@ trajectory_point {
     theta: -1.9536225296348506
     kappa: -0.027649603872106882
     s: 9.03535777870451
-    dkappa: 0
+    dkappa: -0.0011034801332745875
     ddkappa: 0
   }
   v: 0.43875692148899487
@@ -1510,7 +1510,7 @@ trajectory_point {
     theta: -1.9548659952873859
     kappa: -0.027680264102789558
     s: 9.0766532894497871
-    dkappa: 0
+    dkappa: -0.0010873289802244461
     ddkappa: 0
   }
   v: 0.38731762189973257
@@ -1524,7 +1524,7 @@ trajectory_point {
     theta: -1.9559562416466574
     kappa: -0.027707146606102233
     s: 9.1128606720979413
-    dkappa: 0
+    dkappa: -0.0010731678530670043
     ddkappa: 0
   }
   v: 0.33706023697365006
@@ -1538,7 +1538,7 @@ trajectory_point {
     theta: -1.956897436670556
     kappa: -0.027730354121186775
     s: 9.1441183034727285
-    dkappa: 0
+    dkappa: -0.0010609426310220916
     ddkappa: 0
   }
   v: 0.28839269515249638
@@ -1552,7 +1552,7 @@ trajectory_point {
     theta: -1.9576950158536828
     kappa: -0.02775002061444766
     s: 9.1706066196299716
-    dkappa: 0
+    dkappa: -0.0010505827434562234
     ddkappa: 0
   }
   v: 0.24174825263587796
@@ -1566,7 +1566,7 @@ trajectory_point {
     theta: -1.9583557578082471
     kappa: -0.027766313160034539
     s: 9.19255064863335
-    dkappa: 0
+    dkappa: -0.001042000179284624
     ddkappa: 0
   }
   v: 0.19758549338125725
@@ -1580,7 +1580,7 @@ trajectory_point {
     theta: -1.9588878600155408
     kappa: -0.027779433820324877
     s: 9.21022254333018
-    dkappa: 0
+    dkappa: -0.0010350884963732524
     ddkappa: 0
   }
   v: 0.15638832910395398
@@ -1594,7 +1594,7 @@ trajectory_point {
     theta: -1.9593010147441994
     kappa: -0.027789621526406551
     s: 9.2239441141272049
-    dkappa: 0
+    dkappa: -0.0010297218309408256
     ddkappa: 0
   }
   v: 0.11866599927714407
@@ -1608,7 +1608,7 @@ trajectory_point {
     theta: -1.9596064851217352
     kappa: -0.027797153958560458
     s: 9.2340893617663742
-    dkappa: 0
+    dkappa: -0.0010257539069608431
     ddkappa: 0
   }
   v: 0.084953071131860569
@@ -1622,7 +1622,7 @@ trajectory_point {
     theta: -1.9598171813372858
     kappa: -0.027802349426743145
     s: 9.241087010100637
-    dkappa: 0
+    dkappa: -0.0010230170455636106
     ddkappa: 0
   }
   v: 0.055809439656992761
@@ -1636,7 +1636,7 @@ trajectory_point {
     theta: -1.9599477369467753
     kappa: -0.02780556875106939
     s: 9.2454230388697241
-    dkappa: 0
+    dkappa: -0.0010213211744382641
     ddkappa: 0
   }
   v: 0.0318203275992869
@@ -1650,7 +1650,7 @@ trajectory_point {
     theta: -1.9600145852472333
     kappa: -0.027807217142294834
     s: 9.2476432164759323
-    dkappa: 0
+    dkappa: -0.0010204528372347956
     ddkappa: 0
   }
   v: 0.013596285463346928
@@ -1664,7 +1664,7 @@ trajectory_point {
     theta: -1.9600360356850728
     kappa: -0.0278077460822986
     s: 9.2483556327599157
-    dkappa: 0
+    dkappa: -0.0010201742029660732
     ddkappa: 0
   }
   v: 0.0017731915116309361
@@ -1678,7 +1678,7 @@ trajectory_point {
     theta: -1.9600323502635786
     kappa: -0.027807655204565865
     s: 9.2482332317764584
-    dkappa: 0
+    dkappa: -0.0010202220754098722
     ddkappa: 0
   }
   v: 0
@@ -1692,7 +1692,7 @@ trajectory_point {
     theta: -1.9600258199170144
     kappa: -0.02780749417467052
     s: 9.2480163445702779
-    dkappa: 0
+    dkappa: -0.0010203069025108917
     ddkappa: 0
   }
   v: 5.64735903074707e-15