diff --git a/src/main/deploy/choreo/B_PickUpFarPiece1.1.traj b/src/main/deploy/choreo/B_PickUpFarPiece1.1.traj
index 26157f4..14191bb 100644
--- a/src/main/deploy/choreo/B_PickUpFarPiece1.1.traj
+++ b/src/main/deploy/choreo/B_PickUpFarPiece1.1.traj
@@ -1,490 +1,499 @@
 {
     "samples": [
         {
-            "x": 1.9,
+            "x": 1.8999999999999968,
             "y": 7.9,
             "heading": -1.571,
-            "angularVelocity": -3.961354221571043e-29,
-            "velocityX": 2.631024912553489e-19,
-            "velocityY": 1.4754690322578834e-18,
+            "angularVelocity": 4.470498597716865e-29,
+            "velocityX": 2.7707804022206673e-16,
+            "velocityY": 1.8739023107890902e-17,
             "timestamp": 0
         },
         {
-            "x": 1.9151360676032831,
-            "y": 7.8963970015368545,
+            "x": 1.914365871743733,
+            "y": 7.899792228900449,
             "heading": -1.571,
-            "angularVelocity": -4.116427886741637e-21,
-            "velocityX": 0.3416005719831094,
-            "velocityY": -0.08131480171228687,
-            "timestamp": 0.044309257198877544
+            "angularVelocity": 4.2271636316208514e-19,
+            "velocityX": 0.33739566619749,
+            "velocityY": -0.004879694723755523,
+            "timestamp": 0.042578708569808256
         },
         {
-            "x": 1.9454081997191606,
-            "y": 7.889190993474901,
+            "x": 1.9430976157693656,
+            "y": 7.899376752477764,
             "heading": -1.571,
-            "angularVelocity": -4.1164277763040896e-21,
-            "velocityX": 0.683201074213543,
-            "velocityY": -0.16262985474143174,
-            "timestamp": 0.08861851439775509
+            "angularVelocity": 4.227163636037315e-19,
+            "velocityX": 0.6747913450340882,
+            "velocityY": -0.009757844628066633,
+            "timestamp": 0.08515741713961651
         },
         {
-            "x": 1.9908163924842541,
-            "y": 7.878381961894546,
+            "x": 1.986195232749876,
+            "y": 7.898753653015407,
             "heading": -1.571,
-            "angularVelocity": -4.1164278445298885e-21,
-            "velocityX": 1.024801489252768,
-            "velocityY": -0.24394522191693332,
-            "timestamp": 0.13292777159663263
+            "angularVelocity": 4.227163633163533e-19,
+            "velocityX": 1.0121870396762844,
+            "velocityY": -0.014634062029792581,
+            "timestamp": 0.12773612570942477
         },
         {
-            "x": 2.051360640931338,
-            "y": 7.8639698888991525,
+            "x": 2.0436587235509935,
+            "y": 7.897923036409323,
             "heading": -1.571,
-            "angularVelocity": -4.116427739843989e-21,
-            "velocityX": 1.3664017921884157,
-            "velocityY": -0.3252609929953516,
-            "timestamp": 0.17723702879551018
+            "angularVelocity": 4.227163634858386e-19,
+            "velocityX": 1.349582754650927,
+            "velocityY": -0.019507792368223267,
+            "timestamp": 0.17031483427923302
         },
         {
-            "x": 2.1270409384373976,
-            "y": 7.845954750626501,
+            "x": 2.115488089327842,
+            "y": 7.896885044034559,
             "heading": -1.571,
-            "angularVelocity": -4.116449324284025e-21,
-            "velocityX": 1.7080019456515922,
-            "velocityY": -0.40657730261178276,
-            "timestamp": 0.22154628599438772
+            "angularVelocity": 4.2271636331977046e-19,
+            "velocityX": 1.6869784967546948,
+            "velocityY": -0.024378202384055483,
+            "timestamp": 0.21289354284904127
         },
         {
-            "x": 2.217857275730126,
-            "y": 7.824336513669419,
+            "x": 2.2016833316991637,
+            "y": 7.895639874163583,
             "heading": -1.571,
-            "angularVelocity": -4.1164215643720176e-21,
-            "velocityX": 2.0496018898513264,
-            "velocityY": -0.4878943661829266,
-            "timestamp": 0.26585554319326526
+            "angularVelocity": 4.2271636334033824e-19,
+            "velocityX": 2.024374276876126,
+            "velocityY": -0.029243955789217388,
+            "timestamp": 0.25547225141884955
         },
         {
-            "x": 2.3238096389008693,
-            "y": 7.799115127917003,
+            "x": 2.302244453096514,
+            "y": 7.894187824967971,
             "heading": -1.571,
-            "angularVelocity": -4.1164277949404545e-21,
-            "velocityX": 2.3912015201515753,
-            "velocityY": -0.5692125606893493,
-            "timestamp": 0.3101648003921428
+            "angularVelocity": 4.227163633528535e-19,
+            "velocityX": 2.361770114104849,
+            "velocityY": -0.03410270636161241,
+            "timestamp": 0.2980509599886578
         },
         {
-            "x": 2.444898004768031,
-            "y": 7.770290509850712,
+            "x": 2.4171714575816776,
+            "y": 7.8925293954471245,
             "heading": -1.571,
-            "angularVelocity": -4.1164277219688166e-21,
-            "velocityX": 2.7328006272745453,
-            "velocityY": -0.6505326400962694,
-            "timestamp": 0.35447405759102035
+            "angularVelocity": 4.227163633894554e-19,
+            "velocityX": 2.699166046728326,
+            "velocityY": -0.03894973747556378,
+            "timestamp": 0.3406296685584661
         },
         {
-            "x": 2.5811223269665233,
-            "y": 7.737862492432118,
+            "x": 2.5464643533128632,
+            "y": 7.890665591362613,
             "heading": -1.571,
-            "angularVelocity": -4.116427836078919e-21,
-            "velocityX": 3.0743986880002026,
-            "velocityY": -0.7318564893345968,
-            "timestamp": 0.3987833147898979
+            "angularVelocity": 4.2271636346276787e-19,
+            "velocityX": 3.0365621709548005,
+            "velocityY": -0.04377314735710882,
+            "timestamp": 0.3832083771282744
         },
         {
-            "x": 2.732482466386069,
-            "y": 7.701830574535674,
+            "x": 2.6901231650458177,
+            "y": 7.888599493997031,
             "heading": -1.571,
-            "angularVelocity": -4.116426228269473e-21,
-            "velocityX": 3.4159936091950516,
-            "velocityY": -0.8131916482986443,
-            "timestamp": 0.4430925719887755
-        },
-        {
-            "x": 2.8914051055908203,
-            "y": 7.672097682952881,
-            "heading": -1.571,
-            "angularVelocity": -2.0582133368498575e-21,
-            "velocityX": 3.586668999921246,
-            "velocityY": -0.6710311447862857,
-            "timestamp": 0.48740182918765307
-        },
-        {
-            "x": 2.9915482053824713,
-            "y": 7.6595269719549215,
-            "heading": -1.571,
-            "angularVelocity": -1.8777229046989095e-21,
-            "velocityX": 3.6204875876208633,
-            "velocityY": -0.4544706847538144,
-            "timestamp": 0.5150619432932944
-        },
-        {
-            "x": 3.092263880199214,
-            "y": 7.652968306689749,
-            "heading": -1.571,
-            "angularVelocity": -1.8777227665838625e-21,
-            "velocityX": 3.641187973125586,
-            "velocityY": -0.23711634883800817,
-            "timestamp": 0.5427220573989356
-        },
-        {
-            "x": 3.1930238774745283,
-            "y": 7.647129968341588,
-            "heading": -1.571,
-            "angularVelocity": -1.8778541277359e-21,
-            "velocityX": 3.6427903692112493,
-            "velocityY": -0.2110742683802162,
-            "timestamp": 0.5703821715045769
-        },
-        {
-            "x": 3.2937837193193285,
-            "y": 7.64128894814038,
-            "heading": -1.571,
-            "angularVelocity": -1.877566449716214e-21,
-            "velocityX": 3.642784749909989,
-            "velocityY": -0.21117122579101652,
-            "timestamp": 0.5980422856102182
-        },
-        {
-            "x": 3.394543560926969,
-            "y": 7.635447923848087,
-            "heading": -1.571,
-            "angularVelocity": -1.877739673587389e-21,
-            "velocityX": 3.642784741335938,
-            "velocityY": -0.21117137369661676,
-            "timestamp": 0.6257023997158595
-        },
-        {
-            "x": 3.495303402534779,
-            "y": 7.6296068995587145,
-            "heading": -1.571,
-            "angularVelocity": -1.877724322718657e-21,
-            "velocityX": 3.6427847413420595,
-            "velocityY": -0.21117137359101854,
-            "timestamp": 0.6533625138215008
-        },
-        {
-            "x": 3.5960632441425897,
-            "y": 7.623765875269346,
-            "heading": -1.571,
-            "angularVelocity": -1.8777552872754136e-21,
-            "velocityX": 3.642784741342068,
-            "velocityY": -0.211171373590874,
-            "timestamp": 0.6810226279271421
-        },
-        {
-            "x": 3.696823085750408,
-            "y": 7.6179248509801,
-            "heading": -1.571,
-            "angularVelocity": -1.877516595814209e-21,
-            "velocityX": 3.642784741342324,
-            "velocityY": -0.2111713735864506,
-            "timestamp": 0.7086827420327834
-        },
-        {
-            "x": 3.7975829273583934,
-            "y": 7.61208382669375,
-            "heading": -1.571,
-            "angularVelocity": -1.877721219756007e-21,
-            "velocityX": 3.6427847413483936,
-            "velocityY": -0.21117137348175347,
-            "timestamp": 0.7363428561384247
-        },
-        {
-            "x": 3.8983427689639933,
-            "y": 7.606242802366244,
-            "heading": -1.571,
-            "angularVelocity": -1.8782896337041454e-21,
-            "velocityX": 3.6427847412621372,
-            "velocityY": -0.21117137496968985,
-            "timestamp": 0.7640029702440659
-        },
-        {
-            "x": 3.9991026114578196,
-            "y": 7.60040179336099,
-            "heading": -1.571,
-            "angularVelocity": -1.8769887827392664e-21,
-            "velocityX": 3.6427847733743164,
-            "velocityY": -0.2111708210221328,
-            "timestamp": 0.7916630843497072
+            "angularVelocity": 4.227163652979875e-19,
+            "velocityX": 3.3739588765926873,
+            "velocityY": -0.04852419049232661,
+            "timestamp": 0.42578708569808266
+        },
+        {
+            "x": 2.8454680992564305,
+            "y": 7.886072511387067,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 4.2271698321650397e-19,
+            "velocityX": 3.6484181749177784,
+            "velocityY": -0.05934850292215207,
+            "timestamp": 0.46836579426789093
+        },
+        {
+            "x": 3.000000000000003,
+            "y": 7.87,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.1135793766278028e-19,
+            "velocityX": 3.6293233386873958,
+            "velocityY": -0.3774776625911161,
+            "timestamp": 0.5109445028376992
+        },
+        {
+            "x": 3.0933267855298086,
+            "y": 7.854947925249578,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.652450956207458e-19,
+            "velocityX": 3.6023482406759033,
+            "velocityY": -0.580999492137869,
+            "timestamp": 0.5368517109404605
+        },
+        {
+            "x": 3.1856594234462645,
+            "y": 7.8346713899397304,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.6524509613572167e-19,
+            "velocityX": 3.563974842453837,
+            "velocityY": -0.7826599929029426,
+            "timestamp": 0.5627589190432217
+        },
+        {
+            "x": 3.2770421386792723,
+            "y": 7.8104711155350515,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.652450981385339e-19,
+            "velocityX": 3.527308495401703,
+            "velocityY": -0.9341135605458323,
+            "timestamp": 0.588666127145983
+        },
+        {
+            "x": 3.368397018870737,
+            "y": 7.7861659763653845,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.652451746814082e-19,
+            "velocityX": 3.5262340823876155,
+            "velocityY": -0.9381612666736713,
+            "timestamp": 0.6145733352487442
+        },
+        {
+            "x": 3.4597414869906253,
+            "y": 7.761821735388127,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.652451049308022e-19,
+            "velocityX": 3.5258321837525024,
+            "velocityY": -0.9396705689278512,
+            "timestamp": 0.6404805433515055
+        },
+        {
+            "x": 3.5510841613375907,
+            "y": 7.7374707648012215,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.6524500373464137e-19,
+            "velocityX": 3.525762945380196,
+            "velocityY": -0.9399303271237421,
+            "timestamp": 0.6663877514542668
+        },
+        {
+            "x": 3.6424273103238014,
+            "y": 7.7131215747012565,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.6530371780360815e-19,
+            "velocityX": 3.5257812661210104,
+            "velocityY": -0.9398616015809562,
+            "timestamp": 0.692294959557028
+        },
+        {
+            "x": 3.7337711953747554,
+            "y": 7.688775146038041,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.651560076415564e-19,
+            "velocityX": 3.5258096777019814,
+            "velocityY": -0.9397550120662235,
+            "timestamp": 0.7182021676597893
+        },
+        {
+            "x": 3.8251143320372014,
+            "y": 7.664425909712902,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.6527560956628585e-19,
+            "velocityX": 3.525780790432169,
+            "velocityY": -0.9398633858408949,
+            "timestamp": 0.7441093757625505
+        },
+        {
+            "x": 3.9164615673273144,
+            "y": 7.640092054070024,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.651878932000989e-19,
+            "velocityX": 3.525938994575509,
+            "velocityY": -0.9392697023305191,
+            "timestamp": 0.7700165838653118
+        },
+        {
+            "x": 4.007822648813717,
+            "y": 7.615810235461623,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.653022563334735e-19,
+            "velocityX": 3.526473448007752,
+            "velocityY": -0.9372611094217806,
+            "timestamp": 0.7959237919680731
         },
         {
             "x": 4.099836826324463,
             "y": 7.594134330749512,
-            "heading": -1.571,
-            "angularVelocity": -2.3085742218256872e-21,
-            "velocityX": 3.6418582541602307,
-            "velocityY": -0.2265884582956271,
-            "timestamp": 0.8193231984553485
-        },
-        {
-            "x": 4.307393706052396,
-            "y": 7.574701993779823,
-            "heading": -1.4944377172372005,
-            "angularVelocity": 1.1899935016259806,
-            "velocityX": 3.226018467333577,
-            "velocityY": -0.30203324510292745,
-            "timestamp": 0.8836616025375443
-        },
-        {
-            "x": 4.50626618663146,
-            "y": 7.557280233264484,
-            "heading": -1.3980779902010738,
-            "angularVelocity": 1.4977015425036349,
-            "velocityX": 3.091038446104337,
-            "velocityY": -0.27078322448100967,
-            "timestamp": 0.9480000066197402
-        },
-        {
-            "x": 4.698238976536795,
-            "y": 7.5388711827735575,
-            "heading": -1.289295609076384,
-            "angularVelocity": 1.6907845737938185,
-            "velocityX": 2.9837978209729736,
-            "velocityY": -0.2861284912726089,
-            "timestamp": 1.012338410701936
-        },
-        {
-            "x": 4.863540401734348,
-            "y": 7.521644869212672,
-            "heading": -1.1210651488169852,
-            "angularVelocity": 2.6147751511597206,
-            "velocityX": 2.5692496970607546,
-            "velocityY": -0.2677454283584362,
-            "timestamp": 1.076676814784132
-        },
-        {
-            "x": 5.001538745942632,
-            "y": 7.49333125872089,
-            "heading": -0.8960981596700744,
-            "angularVelocity": 3.4966206009633534,
-            "velocityX": 2.144882922989247,
-            "velocityY": -0.4400732485625517,
-            "timestamp": 1.1410152188663278
-        },
-        {
-            "x": 5.122682323650213,
-            "y": 7.438843446196177,
-            "heading": -0.6307802228270052,
-            "angularVelocity": 4.123787971242043,
-            "velocityX": 1.8829123823589617,
-            "velocityY": -0.8468940643150127,
-            "timestamp": 1.2053536229485238
-        },
-        {
-            "x": 5.27238865572227,
-            "y": 7.377472619855293,
-            "heading": -0.42129435245692515,
-            "angularVelocity": 3.2560004146582555,
-            "velocityX": 2.326858028383782,
-            "velocityY": -0.9538754841117861,
-            "timestamp": 1.2696920270307197
-        },
-        {
-            "x": 5.448010618855816,
-            "y": 7.330334666184069,
-            "heading": -0.26222553003546084,
-            "angularVelocity": 2.4723774966230923,
-            "velocityX": 2.7296599230092786,
-            "velocityY": -0.7326565578313575,
-            "timestamp": 1.3340304311129156
-        },
-        {
-            "x": 5.647829111306376,
-            "y": 7.297673043786227,
-            "heading": -0.16202343165080943,
-            "angularVelocity": 1.557422814787848,
-            "velocityX": 3.1057421349040775,
-            "velocityY": -0.507653599180288,
-            "timestamp": 1.3983688351951116
+            "heading": -1.5709999999999997,
+            "angularVelocity": 8.821604445268149e-19,
+            "velocityX": 3.551682494916745,
+            "velocityY": -0.8366746669941396,
+            "timestamp": 0.8218310000708343
+        },
+        {
+            "x": 4.314086123865472,
+            "y": 7.565866389645391,
+            "heading": -1.5166611039162066,
+            "angularVelocity": 0.8444409025240843,
+            "velocityX": 3.3294910868420495,
+            "velocityY": -0.4392913257079289,
+            "timestamp": 0.8861799627058939
+        },
+        {
+            "x": 4.512621682190808,
+            "y": 7.550271754810037,
+            "heading": -1.4174555506263589,
+            "angularVelocity": 1.5416806927054159,
+            "velocityX": 3.085295398641018,
+            "velocityY": -0.24234477443848973,
+            "timestamp": 0.9505289253409536
+        },
+        {
+            "x": 4.703674537259271,
+            "y": 7.533864741231446,
+            "heading": -1.3046929681948032,
+            "angularVelocity": 1.7523605325406573,
+            "velocityX": 2.9690121991853746,
+            "velocityY": -0.2549693562540318,
+            "timestamp": 1.0148778879760132
+        },
+        {
+            "x": 4.868121057502234,
+            "y": 7.5183747890499655,
+            "heading": -1.1331539755708713,
+            "angularVelocity": 2.6657615849509235,
+            "velocityX": 2.555542677129605,
+            "velocityY": -0.2407179781503281,
+            "timestamp": 1.0792268506110727
+        },
+        {
+            "x": 5.005308136079898,
+            "y": 7.491356941262662,
+            "heading": -0.9056990534597417,
+            "angularVelocity": 3.534710006145519,
+            "velocityX": 2.131923701018079,
+            "velocityY": -0.4198645429702136,
+            "timestamp": 1.1435758132461322
+        },
+        {
+            "x": 5.1255280916701365,
+            "y": 7.4382119450007,
+            "heading": -0.6380105243345551,
+            "angularVelocity": 4.159950963671538,
+            "velocityX": 1.8682500955315844,
+            "velocityY": -0.8258873816434845,
+            "timestamp": 1.2079247758811917
+        },
+        {
+            "x": 5.274137127844687,
+            "y": 7.377257884535905,
+            "heading": -0.4258350917264682,
+            "angularVelocity": 3.2972626740101054,
+            "velocityX": 2.309423960995877,
+            "velocityY": -0.9472423170300921,
+            "timestamp": 1.2722737385162513
+        },
+        {
+            "x": 5.448926084967563,
+            "y": 7.329899834255518,
+            "heading": -0.26459184239693584,
+            "angularVelocity": 2.505762994874233,
+            "velocityX": 2.7162668979480253,
+            "velocityY": -0.7359567014145671,
+            "timestamp": 1.3366227011513108
+        },
+        {
+            "x": 5.647781118238161,
+            "y": 7.297534713393957,
+            "heading": -0.1613424754389799,
+            "angularVelocity": 1.6045226329976747,
+            "velocityX": 3.090260124291229,
+            "velocityY": -0.5029625892352149,
+            "timestamp": 1.4009716637863703
         },
         {
             "x": 5.854011535644531,
             "y": 7.282280921936035,
-            "heading": -0.07451425190609767,
-            "angularVelocity": 1.3601391112050898,
-            "velocityX": 3.204655559605509,
-            "velocityY": -0.23923692341711228,
-            "timestamp": 1.4627072392773075
-        },
-        {
-            "x": 5.983548232727189,
-            "y": 7.284896204008577,
-            "heading": -0.02811649988985118,
-            "angularVelocity": 1.1686052664039241,
-            "velocityX": 3.2625991524407896,
-            "velocityY": 0.06587026893100557,
-            "timestamp": 1.5024107673634195
-        },
-        {
-            "x": 6.119161391656251,
-            "y": 7.29693122547571,
-            "heading": -0.002088831618303945,
-            "angularVelocity": 0.6555505146821342,
-            "velocityX": 3.415645043809042,
-            "velocityY": 0.30312221727575994,
-            "timestamp": 1.5421142954495315
-        },
-        {
-            "x": 6.2619885188025055,
-            "y": 7.316524083557185,
-            "heading": -0.000009475598596952791,
-            "angularVelocity": 0.05237207170096152,
-            "velocityX": 3.5973409425097227,
-            "velocityY": 0.4934790187657932,
-            "timestamp": 1.5818178235356435
-        },
-        {
-            "x": 6.4051075433325195,
-            "y": 7.339003917341873,
-            "heading": -0.00000794174457486947,
-            "angularVelocity": 0.00003863268822752122,
-            "velocityX": 3.6046928680898653,
-            "velocityY": 0.566192347842029,
-            "timestamp": 1.6215213516217555
-        },
-        {
-            "x": 6.548226438768295,
-            "y": 7.361484573385393,
-            "heading": -0.000006408069907557222,
-            "angularVelocity": 0.00003862817087655221,
-            "velocityX": 3.6046896166348037,
-            "velocityY": 0.5662130578109599,
-            "timestamp": 1.6612248797078675
-        },
-        {
-            "x": 6.691345334218433,
-            "y": 7.383965229329843,
-            "heading": -0.0000048743902181696355,
-            "angularVelocity": 0.00003862829736594954,
-            "velocityX": 3.6046896169965312,
-            "velocityY": 0.566213055315681,
-            "timestamp": 1.7009284077939795
-        },
-        {
-            "x": 6.8344642296723395,
-            "y": 7.4064458852426815,
-            "heading": -0.000003340705516645168,
-            "angularVelocity": 0.000038628423605030105,
-            "velocityX": 3.604689617091453,
-            "velocityY": 0.5662130545195021,
-            "timestamp": 1.7406319358800915
-        },
-        {
-            "x": 6.977583128990317,
-            "y": 7.428926516540241,
-            "heading": -0.0000018070121354737395,
-            "angularVelocity": 0.000038628642216508214,
-            "velocityX": 3.604689714414578,
-            "velocityY": 0.5662124345424108,
-            "timestamp": 1.7803354639662035
+            "heading": -0.07382990470356181,
+            "angularVelocity": 1.359968632778447,
+            "velocityX": 3.2048755560510305,
+            "velocityY": -0.23704797767091085,
+            "timestamp": 1.4653206264214298
+        },
+        {
+            "x": 5.983589932246309,
+            "y": 7.284960854996629,
+            "heading": -0.027638204496835066,
+            "angularVelocity": 1.1636335547284058,
+            "velocityX": 3.264261102719684,
+            "velocityY": 0.06751126327279944,
+            "timestamp": 1.5050167146092823
+        },
+        {
+            "x": 6.11925745255698,
+            "y": 7.297037313900181,
+            "heading": -0.001860333184984073,
+            "angularVelocity": 0.6493806440077139,
+            "velocityX": 3.4176546482011396,
+            "velocityY": 0.3042228958778286,
+            "timestamp": 1.5447128027971349
+        },
+        {
+            "x": 6.262128938331292,
+            "y": 7.316680359638158,
+            "heading": -0.000009480075392796495,
+            "angularVelocity": 0.046625579353406145,
+            "velocityX": 3.599132617252429,
+            "velocityY": 0.49483580460253374,
+            "timestamp": 1.5844088909849874
+        },
+        {
+            "x": 6.405225952749555,
+            "y": 7.339125340175403,
+            "heading": -0.000007943471062431687,
+            "angularVelocity": 0.00003870921293486644,
+            "velocityX": 3.6048139993312827,
+            "velocityY": 0.5654204623651486,
+            "timestamp": 1.62410497917284
+        },
+        {
+            "x": 6.548321378993718,
+            "y": 7.3615804485544425,
+            "heading": -0.000006409115010503546,
+            "angularVelocity": 0.00003865257565632945,
+            "velocityX": 3.6047739910041234,
+            "velocityY": 0.5656755968668977,
+            "timestamp": 1.6638010673606924
+        },
+        {
+            "x": 6.691417142733098,
+            "y": 7.384033405453259,
+            "heading": -0.000004874435347527005,
+            "angularVelocity": 0.00003866072787080933,
+            "velocityX": 3.604782492980222,
+            "velocityY": 0.5656213980739361,
+            "timestamp": 1.703497155548545
+        },
+        {
+            "x": 6.83451270884544,
+            "y": 7.40648762220655,
+            "heading": -0.0000033399372412399076,
+            "angularVelocity": 0.000038656154204017926,
+            "velocityX": 3.6047775144789,
+            "velocityY": 0.5656531355692576,
+            "timestamp": 1.7431932437363975
+        },
+        {
+            "x": 6.977608912847965,
+            "y": 7.428937772164743,
+            "heading": -0.0000018048282304933975,
+            "angularVelocity": 0.00003867154374202805,
+            "velocityX": 3.6047935838250287,
+            "velocityY": 0.5655506873111692,
+            "timestamp": 1.78288933192425
         },
         {
             "x": 7.12091588973999,
             "y": 7.45,
-            "heading": 7.358367534195036e-20,
-            "angularVelocity": 0.000045512633828276086,
-            "velocityX": 3.610076173553221,
-            "velocityY": 0.5307710542512357,
-            "timestamp": 1.8200389920523155
-        },
-        {
-            "x": 7.357842732165802,
-            "y": 7.451152673287208,
-            "heading": 1.409466595931257e-14,
-            "angularVelocity": 2.170677043875596e-13,
-            "velocityX": 3.6488579749253423,
-            "velocityY": 0.017752066728473136,
-            "timestamp": 1.8849707718184656
-        },
-        {
-            "x": 7.5947722614592195,
-            "y": 7.450917144779397,
-            "heading": 1.4103485973699193e-14,
-            "angularVelocity": 1.358350936661623e-16,
-            "velocityX": 3.6488993547799042,
-            "velocityY": -0.0036273225323404344,
-            "timestamp": 1.9499025515846158
-        },
-        {
-            "x": 7.831701790736089,
-            "y": 7.450681599643842,
-            "heading": 1.4142344148461468e-14,
-            "angularVelocity": 5.984461676862343e-16,
-            "velocityX": 3.648899354525078,
-            "velocityY": -0.0036275786125580032,
-            "timestamp": 2.014834331350766
-        },
-        {
-            "x": 8.060236467962737,
-            "y": 7.450484956720549,
-            "heading": 1.0035367627367513e-14,
-            "angularVelocity": -6.325063837592617e-14,
-            "velocityX": 3.5196120921020335,
-            "velocityY": -0.0030284542330493774,
-            "timestamp": 2.079766111116916
-        },
-        {
-            "x": 8.255358836990055,
-            "y": 7.450321531005239,
-            "heading": 6.643671314235426e-15,
-            "angularVelocity": -5.2234765862684076e-14,
-            "velocityX": 3.005036512629802,
-            "velocityY": -0.002516883348940323,
-            "timestamp": 2.144697890883066
-        },
-        {
-            "x": 8.417068879960697,
-            "y": 7.450191322480526,
-            "heading": 3.947270146358174e-15,
-            "angularVelocity": -4.152667888649298e-14,
-            "velocityX": 2.4904606581405235,
-            "velocityY": -0.0020053127325709405,
-            "timestamp": 2.209629670649216
-        },
-        {
-            "x": 8.54536659092222,
-            "y": 7.45009433114008,
-            "heading": 1.9461799832436813e-15,
-            "angularVelocity": -3.081834766152247e-14,
-            "velocityX": 1.975884711978984,
-            "velocityY": -0.001493742213689111,
-            "timestamp": 2.2745614504153657
-        },
-        {
-            "x": 8.640251966898402,
-            "y": 7.450030556980587,
-            "heading": 6.304260222246035e-16,
-            "angularVelocity": -2.0263636169497805e-14,
-            "velocityX": 1.4613087199813373,
-            "velocityY": -0.0009821717458379814,
-            "timestamp": 2.3394932301815157
+            "heading": 5.756100309873575e-19,
+            "angularVelocity": 0.000045466148249922065,
+            "velocityX": 3.6101032478025736,
+            "velocityY": 0.5305869872012366,
+            "timestamp": 1.8225854201121026
+        },
+        {
+            "x": 7.357842782347066,
+            "y": 7.451140658234923,
+            "heading": 1.3946076252127016e-14,
+            "angularVelocity": 2.1477158274944538e-13,
+            "velocityX": 3.6488588704771803,
+            "velocityY": 0.017567026152596314,
+            "timestamp": 1.8875171976944158
+        },
+        {
+            "x": 7.594772307901899,
+            "y": 7.450909424040305,
+            "heading": 1.3956817600616414e-14,
+            "angularVelocity": 1.6542514133334603e-16,
+            "velocityX": 3.648899419925491,
+            "velocityY": -0.003561186883034476,
+            "timestamp": 1.9524489752767291
+        },
+        {
+            "x": 7.831701831188383,
+            "y": 7.450675877221787,
+            "heading": 1.3958919096191331e-14,
+            "angularVelocity": 3.236467023043882e-17,
+            "velocityX": 3.6488993849911218,
+            "velocityY": -0.00359680309416508,
+            "timestamp": 2.0173807528590424
+        },
+        {
+            "x": 8.06023649674366,
+            "y": 7.450480884382308,
+            "heading": 1.3831887827184638e-14,
+            "angularVelocity": -1.9563805854348244e-15,
+            "velocityX": 3.519612030728179,
+            "velocityY": -0.0030030417576737706,
+            "timestamp": 2.0823125304413557
+        },
+        {
+            "x": 8.255358856071796,
+            "y": 7.450318830701623,
+            "heading": 1.0574442032523106e-14,
+            "angularVelocity": -5.016720496430017e-14,
+            "velocityX": 3.0050364643226115,
+            "velocityY": -0.002495753030620786,
+            "timestamp": 2.147244308023669
+        },
+        {
+            "x": 8.417068891314898,
+            "y": 7.450189715578673,
+            "heading": 6.865176887322045e-15,
+            "angularVelocity": -5.712557523159656e-14,
+            "velocityX": 2.4904606228915314,
+            "velocityY": -0.0019884735603726703,
+            "timestamp": 2.2121760856059822
+        },
+        {
+            "x": 8.54536659652034,
+            "y": 7.450093538813877,
+            "heading": 3.601695099473894e-15,
+            "angularVelocity": -5.026016396528296e-14,
+            "velocityX": 1.975884689785358,
+            "velocityY": -0.001481197163799284,
+            "timestamp": 2.2771078631882955
+        },
+        {
+            "x": 8.640251968711807,
+            "y": 7.450030300307574,
+            "heading": 1.2253067804043182e-15,
+            "angularVelocity": -3.659823290767036e-14,
+            "velocityX": 1.4613087108416603,
+            "velocityY": -0.0009739223021094422,
+            "timestamp": 2.3420396407706088
         },
         {
             "x": 8.701725006103516,
             "y": 7.45,
-            "heading": 2.2598418426144496e-27,
-            "angularVelocity": -9.709051938711168e-15,
-            "velocityX": 0.9467327004820354,
-            "velocityY": -0.0004706013095010225,
-            "timestamp": 2.4044250099476656
+            "heading": -1.9088649305256032e-26,
+            "angularVelocity": -1.8870679751086276e-14,
+            "velocityX": 0.9467327043954562,
+            "velocityY": -0.00046664836083104116,
+            "timestamp": 2.406971418352922
         },
         {
             "x": 8.73,
             "y": 7.45,
-            "heading": -4.894516543566799e-27,
-            "angularVelocity": -1.605657569882155e-25,
-            "velocityX": 0.47336647332214415,
-            "velocityY": -1.2371738084027526e-15,
-            "timestamp": 2.4641567304855543
+            "heading": 2.623532798918036e-26,
+            "angularVelocity": 9.781191839132487e-25,
+            "velocityX": 0.4733664733221421,
+            "velocityY": 5.037124173048106e-15,
+            "timestamp": 2.4667031388908103
         },
         {
             "x": 8.73,
             "y": 7.45,
-            "heading": -2.4365105603708565e-27,
-            "angularVelocity": 3.5986630710075474e-28,
-            "velocityX": 4.937529040802876e-22,
-            "velocityY": 5.715521127367984e-21,
-            "timestamp": 2.523888451023443
+            "heading": 1.3100764528202313e-26,
+            "angularVelocity": -5.658464945099905e-28,
+            "velocityX": 7.815178981981403e-20,
+            "velocityY": 1.9973998116523263e-19,
+            "timestamp": 2.5264348594286985
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/deploy/choreo/B_PickUpFarPiece1.traj b/src/main/deploy/choreo/B_PickUpFarPiece1.traj
index 26157f4..14191bb 100644
--- a/src/main/deploy/choreo/B_PickUpFarPiece1.traj
+++ b/src/main/deploy/choreo/B_PickUpFarPiece1.traj
@@ -1,490 +1,499 @@
 {
     "samples": [
         {
-            "x": 1.9,
+            "x": 1.8999999999999968,
             "y": 7.9,
             "heading": -1.571,
-            "angularVelocity": -3.961354221571043e-29,
-            "velocityX": 2.631024912553489e-19,
-            "velocityY": 1.4754690322578834e-18,
+            "angularVelocity": 4.470498597716865e-29,
+            "velocityX": 2.7707804022206673e-16,
+            "velocityY": 1.8739023107890902e-17,
             "timestamp": 0
         },
         {
-            "x": 1.9151360676032831,
-            "y": 7.8963970015368545,
+            "x": 1.914365871743733,
+            "y": 7.899792228900449,
             "heading": -1.571,
-            "angularVelocity": -4.116427886741637e-21,
-            "velocityX": 0.3416005719831094,
-            "velocityY": -0.08131480171228687,
-            "timestamp": 0.044309257198877544
+            "angularVelocity": 4.2271636316208514e-19,
+            "velocityX": 0.33739566619749,
+            "velocityY": -0.004879694723755523,
+            "timestamp": 0.042578708569808256
         },
         {
-            "x": 1.9454081997191606,
-            "y": 7.889190993474901,
+            "x": 1.9430976157693656,
+            "y": 7.899376752477764,
             "heading": -1.571,
-            "angularVelocity": -4.1164277763040896e-21,
-            "velocityX": 0.683201074213543,
-            "velocityY": -0.16262985474143174,
-            "timestamp": 0.08861851439775509
+            "angularVelocity": 4.227163636037315e-19,
+            "velocityX": 0.6747913450340882,
+            "velocityY": -0.009757844628066633,
+            "timestamp": 0.08515741713961651
         },
         {
-            "x": 1.9908163924842541,
-            "y": 7.878381961894546,
+            "x": 1.986195232749876,
+            "y": 7.898753653015407,
             "heading": -1.571,
-            "angularVelocity": -4.1164278445298885e-21,
-            "velocityX": 1.024801489252768,
-            "velocityY": -0.24394522191693332,
-            "timestamp": 0.13292777159663263
+            "angularVelocity": 4.227163633163533e-19,
+            "velocityX": 1.0121870396762844,
+            "velocityY": -0.014634062029792581,
+            "timestamp": 0.12773612570942477
         },
         {
-            "x": 2.051360640931338,
-            "y": 7.8639698888991525,
+            "x": 2.0436587235509935,
+            "y": 7.897923036409323,
             "heading": -1.571,
-            "angularVelocity": -4.116427739843989e-21,
-            "velocityX": 1.3664017921884157,
-            "velocityY": -0.3252609929953516,
-            "timestamp": 0.17723702879551018
+            "angularVelocity": 4.227163634858386e-19,
+            "velocityX": 1.349582754650927,
+            "velocityY": -0.019507792368223267,
+            "timestamp": 0.17031483427923302
         },
         {
-            "x": 2.1270409384373976,
-            "y": 7.845954750626501,
+            "x": 2.115488089327842,
+            "y": 7.896885044034559,
             "heading": -1.571,
-            "angularVelocity": -4.116449324284025e-21,
-            "velocityX": 1.7080019456515922,
-            "velocityY": -0.40657730261178276,
-            "timestamp": 0.22154628599438772
+            "angularVelocity": 4.2271636331977046e-19,
+            "velocityX": 1.6869784967546948,
+            "velocityY": -0.024378202384055483,
+            "timestamp": 0.21289354284904127
         },
         {
-            "x": 2.217857275730126,
-            "y": 7.824336513669419,
+            "x": 2.2016833316991637,
+            "y": 7.895639874163583,
             "heading": -1.571,
-            "angularVelocity": -4.1164215643720176e-21,
-            "velocityX": 2.0496018898513264,
-            "velocityY": -0.4878943661829266,
-            "timestamp": 0.26585554319326526
+            "angularVelocity": 4.2271636334033824e-19,
+            "velocityX": 2.024374276876126,
+            "velocityY": -0.029243955789217388,
+            "timestamp": 0.25547225141884955
         },
         {
-            "x": 2.3238096389008693,
-            "y": 7.799115127917003,
+            "x": 2.302244453096514,
+            "y": 7.894187824967971,
             "heading": -1.571,
-            "angularVelocity": -4.1164277949404545e-21,
-            "velocityX": 2.3912015201515753,
-            "velocityY": -0.5692125606893493,
-            "timestamp": 0.3101648003921428
+            "angularVelocity": 4.227163633528535e-19,
+            "velocityX": 2.361770114104849,
+            "velocityY": -0.03410270636161241,
+            "timestamp": 0.2980509599886578
         },
         {
-            "x": 2.444898004768031,
-            "y": 7.770290509850712,
+            "x": 2.4171714575816776,
+            "y": 7.8925293954471245,
             "heading": -1.571,
-            "angularVelocity": -4.1164277219688166e-21,
-            "velocityX": 2.7328006272745453,
-            "velocityY": -0.6505326400962694,
-            "timestamp": 0.35447405759102035
+            "angularVelocity": 4.227163633894554e-19,
+            "velocityX": 2.699166046728326,
+            "velocityY": -0.03894973747556378,
+            "timestamp": 0.3406296685584661
         },
         {
-            "x": 2.5811223269665233,
-            "y": 7.737862492432118,
+            "x": 2.5464643533128632,
+            "y": 7.890665591362613,
             "heading": -1.571,
-            "angularVelocity": -4.116427836078919e-21,
-            "velocityX": 3.0743986880002026,
-            "velocityY": -0.7318564893345968,
-            "timestamp": 0.3987833147898979
+            "angularVelocity": 4.2271636346276787e-19,
+            "velocityX": 3.0365621709548005,
+            "velocityY": -0.04377314735710882,
+            "timestamp": 0.3832083771282744
         },
         {
-            "x": 2.732482466386069,
-            "y": 7.701830574535674,
+            "x": 2.6901231650458177,
+            "y": 7.888599493997031,
             "heading": -1.571,
-            "angularVelocity": -4.116426228269473e-21,
-            "velocityX": 3.4159936091950516,
-            "velocityY": -0.8131916482986443,
-            "timestamp": 0.4430925719887755
-        },
-        {
-            "x": 2.8914051055908203,
-            "y": 7.672097682952881,
-            "heading": -1.571,
-            "angularVelocity": -2.0582133368498575e-21,
-            "velocityX": 3.586668999921246,
-            "velocityY": -0.6710311447862857,
-            "timestamp": 0.48740182918765307
-        },
-        {
-            "x": 2.9915482053824713,
-            "y": 7.6595269719549215,
-            "heading": -1.571,
-            "angularVelocity": -1.8777229046989095e-21,
-            "velocityX": 3.6204875876208633,
-            "velocityY": -0.4544706847538144,
-            "timestamp": 0.5150619432932944
-        },
-        {
-            "x": 3.092263880199214,
-            "y": 7.652968306689749,
-            "heading": -1.571,
-            "angularVelocity": -1.8777227665838625e-21,
-            "velocityX": 3.641187973125586,
-            "velocityY": -0.23711634883800817,
-            "timestamp": 0.5427220573989356
-        },
-        {
-            "x": 3.1930238774745283,
-            "y": 7.647129968341588,
-            "heading": -1.571,
-            "angularVelocity": -1.8778541277359e-21,
-            "velocityX": 3.6427903692112493,
-            "velocityY": -0.2110742683802162,
-            "timestamp": 0.5703821715045769
-        },
-        {
-            "x": 3.2937837193193285,
-            "y": 7.64128894814038,
-            "heading": -1.571,
-            "angularVelocity": -1.877566449716214e-21,
-            "velocityX": 3.642784749909989,
-            "velocityY": -0.21117122579101652,
-            "timestamp": 0.5980422856102182
-        },
-        {
-            "x": 3.394543560926969,
-            "y": 7.635447923848087,
-            "heading": -1.571,
-            "angularVelocity": -1.877739673587389e-21,
-            "velocityX": 3.642784741335938,
-            "velocityY": -0.21117137369661676,
-            "timestamp": 0.6257023997158595
-        },
-        {
-            "x": 3.495303402534779,
-            "y": 7.6296068995587145,
-            "heading": -1.571,
-            "angularVelocity": -1.877724322718657e-21,
-            "velocityX": 3.6427847413420595,
-            "velocityY": -0.21117137359101854,
-            "timestamp": 0.6533625138215008
-        },
-        {
-            "x": 3.5960632441425897,
-            "y": 7.623765875269346,
-            "heading": -1.571,
-            "angularVelocity": -1.8777552872754136e-21,
-            "velocityX": 3.642784741342068,
-            "velocityY": -0.211171373590874,
-            "timestamp": 0.6810226279271421
-        },
-        {
-            "x": 3.696823085750408,
-            "y": 7.6179248509801,
-            "heading": -1.571,
-            "angularVelocity": -1.877516595814209e-21,
-            "velocityX": 3.642784741342324,
-            "velocityY": -0.2111713735864506,
-            "timestamp": 0.7086827420327834
-        },
-        {
-            "x": 3.7975829273583934,
-            "y": 7.61208382669375,
-            "heading": -1.571,
-            "angularVelocity": -1.877721219756007e-21,
-            "velocityX": 3.6427847413483936,
-            "velocityY": -0.21117137348175347,
-            "timestamp": 0.7363428561384247
-        },
-        {
-            "x": 3.8983427689639933,
-            "y": 7.606242802366244,
-            "heading": -1.571,
-            "angularVelocity": -1.8782896337041454e-21,
-            "velocityX": 3.6427847412621372,
-            "velocityY": -0.21117137496968985,
-            "timestamp": 0.7640029702440659
-        },
-        {
-            "x": 3.9991026114578196,
-            "y": 7.60040179336099,
-            "heading": -1.571,
-            "angularVelocity": -1.8769887827392664e-21,
-            "velocityX": 3.6427847733743164,
-            "velocityY": -0.2111708210221328,
-            "timestamp": 0.7916630843497072
+            "angularVelocity": 4.227163652979875e-19,
+            "velocityX": 3.3739588765926873,
+            "velocityY": -0.04852419049232661,
+            "timestamp": 0.42578708569808266
+        },
+        {
+            "x": 2.8454680992564305,
+            "y": 7.886072511387067,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 4.2271698321650397e-19,
+            "velocityX": 3.6484181749177784,
+            "velocityY": -0.05934850292215207,
+            "timestamp": 0.46836579426789093
+        },
+        {
+            "x": 3.000000000000003,
+            "y": 7.87,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.1135793766278028e-19,
+            "velocityX": 3.6293233386873958,
+            "velocityY": -0.3774776625911161,
+            "timestamp": 0.5109445028376992
+        },
+        {
+            "x": 3.0933267855298086,
+            "y": 7.854947925249578,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.652450956207458e-19,
+            "velocityX": 3.6023482406759033,
+            "velocityY": -0.580999492137869,
+            "timestamp": 0.5368517109404605
+        },
+        {
+            "x": 3.1856594234462645,
+            "y": 7.8346713899397304,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.6524509613572167e-19,
+            "velocityX": 3.563974842453837,
+            "velocityY": -0.7826599929029426,
+            "timestamp": 0.5627589190432217
+        },
+        {
+            "x": 3.2770421386792723,
+            "y": 7.8104711155350515,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.652450981385339e-19,
+            "velocityX": 3.527308495401703,
+            "velocityY": -0.9341135605458323,
+            "timestamp": 0.588666127145983
+        },
+        {
+            "x": 3.368397018870737,
+            "y": 7.7861659763653845,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.652451746814082e-19,
+            "velocityX": 3.5262340823876155,
+            "velocityY": -0.9381612666736713,
+            "timestamp": 0.6145733352487442
+        },
+        {
+            "x": 3.4597414869906253,
+            "y": 7.761821735388127,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.652451049308022e-19,
+            "velocityX": 3.5258321837525024,
+            "velocityY": -0.9396705689278512,
+            "timestamp": 0.6404805433515055
+        },
+        {
+            "x": 3.5510841613375907,
+            "y": 7.7374707648012215,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.6524500373464137e-19,
+            "velocityX": 3.525762945380196,
+            "velocityY": -0.9399303271237421,
+            "timestamp": 0.6663877514542668
+        },
+        {
+            "x": 3.6424273103238014,
+            "y": 7.7131215747012565,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.6530371780360815e-19,
+            "velocityX": 3.5257812661210104,
+            "velocityY": -0.9398616015809562,
+            "timestamp": 0.692294959557028
+        },
+        {
+            "x": 3.7337711953747554,
+            "y": 7.688775146038041,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.651560076415564e-19,
+            "velocityX": 3.5258096777019814,
+            "velocityY": -0.9397550120662235,
+            "timestamp": 0.7182021676597893
+        },
+        {
+            "x": 3.8251143320372014,
+            "y": 7.664425909712902,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.6527560956628585e-19,
+            "velocityX": 3.525780790432169,
+            "velocityY": -0.9398633858408949,
+            "timestamp": 0.7441093757625505
+        },
+        {
+            "x": 3.9164615673273144,
+            "y": 7.640092054070024,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.651878932000989e-19,
+            "velocityX": 3.525938994575509,
+            "velocityY": -0.9392697023305191,
+            "timestamp": 0.7700165838653118
+        },
+        {
+            "x": 4.007822648813717,
+            "y": 7.615810235461623,
+            "heading": -1.5709999999999997,
+            "angularVelocity": 2.653022563334735e-19,
+            "velocityX": 3.526473448007752,
+            "velocityY": -0.9372611094217806,
+            "timestamp": 0.7959237919680731
         },
         {
             "x": 4.099836826324463,
             "y": 7.594134330749512,
-            "heading": -1.571,
-            "angularVelocity": -2.3085742218256872e-21,
-            "velocityX": 3.6418582541602307,
-            "velocityY": -0.2265884582956271,
-            "timestamp": 0.8193231984553485
-        },
-        {
-            "x": 4.307393706052396,
-            "y": 7.574701993779823,
-            "heading": -1.4944377172372005,
-            "angularVelocity": 1.1899935016259806,
-            "velocityX": 3.226018467333577,
-            "velocityY": -0.30203324510292745,
-            "timestamp": 0.8836616025375443
-        },
-        {
-            "x": 4.50626618663146,
-            "y": 7.557280233264484,
-            "heading": -1.3980779902010738,
-            "angularVelocity": 1.4977015425036349,
-            "velocityX": 3.091038446104337,
-            "velocityY": -0.27078322448100967,
-            "timestamp": 0.9480000066197402
-        },
-        {
-            "x": 4.698238976536795,
-            "y": 7.5388711827735575,
-            "heading": -1.289295609076384,
-            "angularVelocity": 1.6907845737938185,
-            "velocityX": 2.9837978209729736,
-            "velocityY": -0.2861284912726089,
-            "timestamp": 1.012338410701936
-        },
-        {
-            "x": 4.863540401734348,
-            "y": 7.521644869212672,
-            "heading": -1.1210651488169852,
-            "angularVelocity": 2.6147751511597206,
-            "velocityX": 2.5692496970607546,
-            "velocityY": -0.2677454283584362,
-            "timestamp": 1.076676814784132
-        },
-        {
-            "x": 5.001538745942632,
-            "y": 7.49333125872089,
-            "heading": -0.8960981596700744,
-            "angularVelocity": 3.4966206009633534,
-            "velocityX": 2.144882922989247,
-            "velocityY": -0.4400732485625517,
-            "timestamp": 1.1410152188663278
-        },
-        {
-            "x": 5.122682323650213,
-            "y": 7.438843446196177,
-            "heading": -0.6307802228270052,
-            "angularVelocity": 4.123787971242043,
-            "velocityX": 1.8829123823589617,
-            "velocityY": -0.8468940643150127,
-            "timestamp": 1.2053536229485238
-        },
-        {
-            "x": 5.27238865572227,
-            "y": 7.377472619855293,
-            "heading": -0.42129435245692515,
-            "angularVelocity": 3.2560004146582555,
-            "velocityX": 2.326858028383782,
-            "velocityY": -0.9538754841117861,
-            "timestamp": 1.2696920270307197
-        },
-        {
-            "x": 5.448010618855816,
-            "y": 7.330334666184069,
-            "heading": -0.26222553003546084,
-            "angularVelocity": 2.4723774966230923,
-            "velocityX": 2.7296599230092786,
-            "velocityY": -0.7326565578313575,
-            "timestamp": 1.3340304311129156
-        },
-        {
-            "x": 5.647829111306376,
-            "y": 7.297673043786227,
-            "heading": -0.16202343165080943,
-            "angularVelocity": 1.557422814787848,
-            "velocityX": 3.1057421349040775,
-            "velocityY": -0.507653599180288,
-            "timestamp": 1.3983688351951116
+            "heading": -1.5709999999999997,
+            "angularVelocity": 8.821604445268149e-19,
+            "velocityX": 3.551682494916745,
+            "velocityY": -0.8366746669941396,
+            "timestamp": 0.8218310000708343
+        },
+        {
+            "x": 4.314086123865472,
+            "y": 7.565866389645391,
+            "heading": -1.5166611039162066,
+            "angularVelocity": 0.8444409025240843,
+            "velocityX": 3.3294910868420495,
+            "velocityY": -0.4392913257079289,
+            "timestamp": 0.8861799627058939
+        },
+        {
+            "x": 4.512621682190808,
+            "y": 7.550271754810037,
+            "heading": -1.4174555506263589,
+            "angularVelocity": 1.5416806927054159,
+            "velocityX": 3.085295398641018,
+            "velocityY": -0.24234477443848973,
+            "timestamp": 0.9505289253409536
+        },
+        {
+            "x": 4.703674537259271,
+            "y": 7.533864741231446,
+            "heading": -1.3046929681948032,
+            "angularVelocity": 1.7523605325406573,
+            "velocityX": 2.9690121991853746,
+            "velocityY": -0.2549693562540318,
+            "timestamp": 1.0148778879760132
+        },
+        {
+            "x": 4.868121057502234,
+            "y": 7.5183747890499655,
+            "heading": -1.1331539755708713,
+            "angularVelocity": 2.6657615849509235,
+            "velocityX": 2.555542677129605,
+            "velocityY": -0.2407179781503281,
+            "timestamp": 1.0792268506110727
+        },
+        {
+            "x": 5.005308136079898,
+            "y": 7.491356941262662,
+            "heading": -0.9056990534597417,
+            "angularVelocity": 3.534710006145519,
+            "velocityX": 2.131923701018079,
+            "velocityY": -0.4198645429702136,
+            "timestamp": 1.1435758132461322
+        },
+        {
+            "x": 5.1255280916701365,
+            "y": 7.4382119450007,
+            "heading": -0.6380105243345551,
+            "angularVelocity": 4.159950963671538,
+            "velocityX": 1.8682500955315844,
+            "velocityY": -0.8258873816434845,
+            "timestamp": 1.2079247758811917
+        },
+        {
+            "x": 5.274137127844687,
+            "y": 7.377257884535905,
+            "heading": -0.4258350917264682,
+            "angularVelocity": 3.2972626740101054,
+            "velocityX": 2.309423960995877,
+            "velocityY": -0.9472423170300921,
+            "timestamp": 1.2722737385162513
+        },
+        {
+            "x": 5.448926084967563,
+            "y": 7.329899834255518,
+            "heading": -0.26459184239693584,
+            "angularVelocity": 2.505762994874233,
+            "velocityX": 2.7162668979480253,
+            "velocityY": -0.7359567014145671,
+            "timestamp": 1.3366227011513108
+        },
+        {
+            "x": 5.647781118238161,
+            "y": 7.297534713393957,
+            "heading": -0.1613424754389799,
+            "angularVelocity": 1.6045226329976747,
+            "velocityX": 3.090260124291229,
+            "velocityY": -0.5029625892352149,
+            "timestamp": 1.4009716637863703
         },
         {
             "x": 5.854011535644531,
             "y": 7.282280921936035,
-            "heading": -0.07451425190609767,
-            "angularVelocity": 1.3601391112050898,
-            "velocityX": 3.204655559605509,
-            "velocityY": -0.23923692341711228,
-            "timestamp": 1.4627072392773075
-        },
-        {
-            "x": 5.983548232727189,
-            "y": 7.284896204008577,
-            "heading": -0.02811649988985118,
-            "angularVelocity": 1.1686052664039241,
-            "velocityX": 3.2625991524407896,
-            "velocityY": 0.06587026893100557,
-            "timestamp": 1.5024107673634195
-        },
-        {
-            "x": 6.119161391656251,
-            "y": 7.29693122547571,
-            "heading": -0.002088831618303945,
-            "angularVelocity": 0.6555505146821342,
-            "velocityX": 3.415645043809042,
-            "velocityY": 0.30312221727575994,
-            "timestamp": 1.5421142954495315
-        },
-        {
-            "x": 6.2619885188025055,
-            "y": 7.316524083557185,
-            "heading": -0.000009475598596952791,
-            "angularVelocity": 0.05237207170096152,
-            "velocityX": 3.5973409425097227,
-            "velocityY": 0.4934790187657932,
-            "timestamp": 1.5818178235356435
-        },
-        {
-            "x": 6.4051075433325195,
-            "y": 7.339003917341873,
-            "heading": -0.00000794174457486947,
-            "angularVelocity": 0.00003863268822752122,
-            "velocityX": 3.6046928680898653,
-            "velocityY": 0.566192347842029,
-            "timestamp": 1.6215213516217555
-        },
-        {
-            "x": 6.548226438768295,
-            "y": 7.361484573385393,
-            "heading": -0.000006408069907557222,
-            "angularVelocity": 0.00003862817087655221,
-            "velocityX": 3.6046896166348037,
-            "velocityY": 0.5662130578109599,
-            "timestamp": 1.6612248797078675
-        },
-        {
-            "x": 6.691345334218433,
-            "y": 7.383965229329843,
-            "heading": -0.0000048743902181696355,
-            "angularVelocity": 0.00003862829736594954,
-            "velocityX": 3.6046896169965312,
-            "velocityY": 0.566213055315681,
-            "timestamp": 1.7009284077939795
-        },
-        {
-            "x": 6.8344642296723395,
-            "y": 7.4064458852426815,
-            "heading": -0.000003340705516645168,
-            "angularVelocity": 0.000038628423605030105,
-            "velocityX": 3.604689617091453,
-            "velocityY": 0.5662130545195021,
-            "timestamp": 1.7406319358800915
-        },
-        {
-            "x": 6.977583128990317,
-            "y": 7.428926516540241,
-            "heading": -0.0000018070121354737395,
-            "angularVelocity": 0.000038628642216508214,
-            "velocityX": 3.604689714414578,
-            "velocityY": 0.5662124345424108,
-            "timestamp": 1.7803354639662035
+            "heading": -0.07382990470356181,
+            "angularVelocity": 1.359968632778447,
+            "velocityX": 3.2048755560510305,
+            "velocityY": -0.23704797767091085,
+            "timestamp": 1.4653206264214298
+        },
+        {
+            "x": 5.983589932246309,
+            "y": 7.284960854996629,
+            "heading": -0.027638204496835066,
+            "angularVelocity": 1.1636335547284058,
+            "velocityX": 3.264261102719684,
+            "velocityY": 0.06751126327279944,
+            "timestamp": 1.5050167146092823
+        },
+        {
+            "x": 6.11925745255698,
+            "y": 7.297037313900181,
+            "heading": -0.001860333184984073,
+            "angularVelocity": 0.6493806440077139,
+            "velocityX": 3.4176546482011396,
+            "velocityY": 0.3042228958778286,
+            "timestamp": 1.5447128027971349
+        },
+        {
+            "x": 6.262128938331292,
+            "y": 7.316680359638158,
+            "heading": -0.000009480075392796495,
+            "angularVelocity": 0.046625579353406145,
+            "velocityX": 3.599132617252429,
+            "velocityY": 0.49483580460253374,
+            "timestamp": 1.5844088909849874
+        },
+        {
+            "x": 6.405225952749555,
+            "y": 7.339125340175403,
+            "heading": -0.000007943471062431687,
+            "angularVelocity": 0.00003870921293486644,
+            "velocityX": 3.6048139993312827,
+            "velocityY": 0.5654204623651486,
+            "timestamp": 1.62410497917284
+        },
+        {
+            "x": 6.548321378993718,
+            "y": 7.3615804485544425,
+            "heading": -0.000006409115010503546,
+            "angularVelocity": 0.00003865257565632945,
+            "velocityX": 3.6047739910041234,
+            "velocityY": 0.5656755968668977,
+            "timestamp": 1.6638010673606924
+        },
+        {
+            "x": 6.691417142733098,
+            "y": 7.384033405453259,
+            "heading": -0.000004874435347527005,
+            "angularVelocity": 0.00003866072787080933,
+            "velocityX": 3.604782492980222,
+            "velocityY": 0.5656213980739361,
+            "timestamp": 1.703497155548545
+        },
+        {
+            "x": 6.83451270884544,
+            "y": 7.40648762220655,
+            "heading": -0.0000033399372412399076,
+            "angularVelocity": 0.000038656154204017926,
+            "velocityX": 3.6047775144789,
+            "velocityY": 0.5656531355692576,
+            "timestamp": 1.7431932437363975
+        },
+        {
+            "x": 6.977608912847965,
+            "y": 7.428937772164743,
+            "heading": -0.0000018048282304933975,
+            "angularVelocity": 0.00003867154374202805,
+            "velocityX": 3.6047935838250287,
+            "velocityY": 0.5655506873111692,
+            "timestamp": 1.78288933192425
         },
         {
             "x": 7.12091588973999,
             "y": 7.45,
-            "heading": 7.358367534195036e-20,
-            "angularVelocity": 0.000045512633828276086,
-            "velocityX": 3.610076173553221,
-            "velocityY": 0.5307710542512357,
-            "timestamp": 1.8200389920523155
-        },
-        {
-            "x": 7.357842732165802,
-            "y": 7.451152673287208,
-            "heading": 1.409466595931257e-14,
-            "angularVelocity": 2.170677043875596e-13,
-            "velocityX": 3.6488579749253423,
-            "velocityY": 0.017752066728473136,
-            "timestamp": 1.8849707718184656
-        },
-        {
-            "x": 7.5947722614592195,
-            "y": 7.450917144779397,
-            "heading": 1.4103485973699193e-14,
-            "angularVelocity": 1.358350936661623e-16,
-            "velocityX": 3.6488993547799042,
-            "velocityY": -0.0036273225323404344,
-            "timestamp": 1.9499025515846158
-        },
-        {
-            "x": 7.831701790736089,
-            "y": 7.450681599643842,
-            "heading": 1.4142344148461468e-14,
-            "angularVelocity": 5.984461676862343e-16,
-            "velocityX": 3.648899354525078,
-            "velocityY": -0.0036275786125580032,
-            "timestamp": 2.014834331350766
-        },
-        {
-            "x": 8.060236467962737,
-            "y": 7.450484956720549,
-            "heading": 1.0035367627367513e-14,
-            "angularVelocity": -6.325063837592617e-14,
-            "velocityX": 3.5196120921020335,
-            "velocityY": -0.0030284542330493774,
-            "timestamp": 2.079766111116916
-        },
-        {
-            "x": 8.255358836990055,
-            "y": 7.450321531005239,
-            "heading": 6.643671314235426e-15,
-            "angularVelocity": -5.2234765862684076e-14,
-            "velocityX": 3.005036512629802,
-            "velocityY": -0.002516883348940323,
-            "timestamp": 2.144697890883066
-        },
-        {
-            "x": 8.417068879960697,
-            "y": 7.450191322480526,
-            "heading": 3.947270146358174e-15,
-            "angularVelocity": -4.152667888649298e-14,
-            "velocityX": 2.4904606581405235,
-            "velocityY": -0.0020053127325709405,
-            "timestamp": 2.209629670649216
-        },
-        {
-            "x": 8.54536659092222,
-            "y": 7.45009433114008,
-            "heading": 1.9461799832436813e-15,
-            "angularVelocity": -3.081834766152247e-14,
-            "velocityX": 1.975884711978984,
-            "velocityY": -0.001493742213689111,
-            "timestamp": 2.2745614504153657
-        },
-        {
-            "x": 8.640251966898402,
-            "y": 7.450030556980587,
-            "heading": 6.304260222246035e-16,
-            "angularVelocity": -2.0263636169497805e-14,
-            "velocityX": 1.4613087199813373,
-            "velocityY": -0.0009821717458379814,
-            "timestamp": 2.3394932301815157
+            "heading": 5.756100309873575e-19,
+            "angularVelocity": 0.000045466148249922065,
+            "velocityX": 3.6101032478025736,
+            "velocityY": 0.5305869872012366,
+            "timestamp": 1.8225854201121026
+        },
+        {
+            "x": 7.357842782347066,
+            "y": 7.451140658234923,
+            "heading": 1.3946076252127016e-14,
+            "angularVelocity": 2.1477158274944538e-13,
+            "velocityX": 3.6488588704771803,
+            "velocityY": 0.017567026152596314,
+            "timestamp": 1.8875171976944158
+        },
+        {
+            "x": 7.594772307901899,
+            "y": 7.450909424040305,
+            "heading": 1.3956817600616414e-14,
+            "angularVelocity": 1.6542514133334603e-16,
+            "velocityX": 3.648899419925491,
+            "velocityY": -0.003561186883034476,
+            "timestamp": 1.9524489752767291
+        },
+        {
+            "x": 7.831701831188383,
+            "y": 7.450675877221787,
+            "heading": 1.3958919096191331e-14,
+            "angularVelocity": 3.236467023043882e-17,
+            "velocityX": 3.6488993849911218,
+            "velocityY": -0.00359680309416508,
+            "timestamp": 2.0173807528590424
+        },
+        {
+            "x": 8.06023649674366,
+            "y": 7.450480884382308,
+            "heading": 1.3831887827184638e-14,
+            "angularVelocity": -1.9563805854348244e-15,
+            "velocityX": 3.519612030728179,
+            "velocityY": -0.0030030417576737706,
+            "timestamp": 2.0823125304413557
+        },
+        {
+            "x": 8.255358856071796,
+            "y": 7.450318830701623,
+            "heading": 1.0574442032523106e-14,
+            "angularVelocity": -5.016720496430017e-14,
+            "velocityX": 3.0050364643226115,
+            "velocityY": -0.002495753030620786,
+            "timestamp": 2.147244308023669
+        },
+        {
+            "x": 8.417068891314898,
+            "y": 7.450189715578673,
+            "heading": 6.865176887322045e-15,
+            "angularVelocity": -5.712557523159656e-14,
+            "velocityX": 2.4904606228915314,
+            "velocityY": -0.0019884735603726703,
+            "timestamp": 2.2121760856059822
+        },
+        {
+            "x": 8.54536659652034,
+            "y": 7.450093538813877,
+            "heading": 3.601695099473894e-15,
+            "angularVelocity": -5.026016396528296e-14,
+            "velocityX": 1.975884689785358,
+            "velocityY": -0.001481197163799284,
+            "timestamp": 2.2771078631882955
+        },
+        {
+            "x": 8.640251968711807,
+            "y": 7.450030300307574,
+            "heading": 1.2253067804043182e-15,
+            "angularVelocity": -3.659823290767036e-14,
+            "velocityX": 1.4613087108416603,
+            "velocityY": -0.0009739223021094422,
+            "timestamp": 2.3420396407706088
         },
         {
             "x": 8.701725006103516,
             "y": 7.45,
-            "heading": 2.2598418426144496e-27,
-            "angularVelocity": -9.709051938711168e-15,
-            "velocityX": 0.9467327004820354,
-            "velocityY": -0.0004706013095010225,
-            "timestamp": 2.4044250099476656
+            "heading": -1.9088649305256032e-26,
+            "angularVelocity": -1.8870679751086276e-14,
+            "velocityX": 0.9467327043954562,
+            "velocityY": -0.00046664836083104116,
+            "timestamp": 2.406971418352922
         },
         {
             "x": 8.73,
             "y": 7.45,
-            "heading": -4.894516543566799e-27,
-            "angularVelocity": -1.605657569882155e-25,
-            "velocityX": 0.47336647332214415,
-            "velocityY": -1.2371738084027526e-15,
-            "timestamp": 2.4641567304855543
+            "heading": 2.623532798918036e-26,
+            "angularVelocity": 9.781191839132487e-25,
+            "velocityX": 0.4733664733221421,
+            "velocityY": 5.037124173048106e-15,
+            "timestamp": 2.4667031388908103
         },
         {
             "x": 8.73,
             "y": 7.45,
-            "heading": -2.4365105603708565e-27,
-            "angularVelocity": 3.5986630710075474e-28,
-            "velocityX": 4.937529040802876e-22,
-            "velocityY": 5.715521127367984e-21,
-            "timestamp": 2.523888451023443
+            "heading": 1.3100764528202313e-26,
+            "angularVelocity": -5.658464945099905e-28,
+            "velocityX": 7.815178981981403e-20,
+            "velocityY": 1.9973998116523263e-19,
+            "timestamp": 2.5264348594286985
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/omicron.chor b/src/main/omicron.chor
index bbf8b40..aaac978 100644
--- a/src/main/omicron.chor
+++ b/src/main/omicron.chor
@@ -5246,11 +5246,11 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 11
+                    "controlIntervalCount": 12
                 },
                 {
-                    "x": 2.8914051055908203,
-                    "y": 7.672097682952881,
+                    "x": 3,
+                    "y": 7.87,
                     "heading": -0.8280080399812538,
                     "isInitialGuess": false,
                     "translationConstrained": true,
@@ -5305,490 +5305,499 @@
             ],
             "trajectory": [
                 {
-                    "x": 1.9,
+                    "x": 1.8999999999999968,
                     "y": 7.9,
                     "heading": -1.571,
-                    "angularVelocity": -3.961354221571043e-29,
-                    "velocityX": 2.631024912553489e-19,
-                    "velocityY": 1.4754690322578834e-18,
+                    "angularVelocity": 4.470498597716865e-29,
+                    "velocityX": 2.7707804022206673e-16,
+                    "velocityY": 1.8739023107890902e-17,
                     "timestamp": 0
                 },
                 {
-                    "x": 1.9151360676032831,
-                    "y": 7.8963970015368545,
+                    "x": 1.914365871743733,
+                    "y": 7.899792228900449,
                     "heading": -1.571,
-                    "angularVelocity": -4.116427886741637e-21,
-                    "velocityX": 0.3416005719831094,
-                    "velocityY": -0.08131480171228687,
-                    "timestamp": 0.044309257198877544
+                    "angularVelocity": 4.2271636316208514e-19,
+                    "velocityX": 0.33739566619749,
+                    "velocityY": -0.004879694723755523,
+                    "timestamp": 0.042578708569808256
                 },
                 {
-                    "x": 1.9454081997191606,
-                    "y": 7.889190993474901,
+                    "x": 1.9430976157693656,
+                    "y": 7.899376752477764,
                     "heading": -1.571,
-                    "angularVelocity": -4.1164277763040896e-21,
-                    "velocityX": 0.683201074213543,
-                    "velocityY": -0.16262985474143174,
-                    "timestamp": 0.08861851439775509
+                    "angularVelocity": 4.227163636037315e-19,
+                    "velocityX": 0.6747913450340882,
+                    "velocityY": -0.009757844628066633,
+                    "timestamp": 0.08515741713961651
                 },
                 {
-                    "x": 1.9908163924842541,
-                    "y": 7.878381961894546,
+                    "x": 1.986195232749876,
+                    "y": 7.898753653015407,
                     "heading": -1.571,
-                    "angularVelocity": -4.1164278445298885e-21,
-                    "velocityX": 1.024801489252768,
-                    "velocityY": -0.24394522191693332,
-                    "timestamp": 0.13292777159663263
+                    "angularVelocity": 4.227163633163533e-19,
+                    "velocityX": 1.0121870396762844,
+                    "velocityY": -0.014634062029792581,
+                    "timestamp": 0.12773612570942477
                 },
                 {
-                    "x": 2.051360640931338,
-                    "y": 7.8639698888991525,
+                    "x": 2.0436587235509935,
+                    "y": 7.897923036409323,
                     "heading": -1.571,
-                    "angularVelocity": -4.116427739843989e-21,
-                    "velocityX": 1.3664017921884157,
-                    "velocityY": -0.3252609929953516,
-                    "timestamp": 0.17723702879551018
+                    "angularVelocity": 4.227163634858386e-19,
+                    "velocityX": 1.349582754650927,
+                    "velocityY": -0.019507792368223267,
+                    "timestamp": 0.17031483427923302
                 },
                 {
-                    "x": 2.1270409384373976,
-                    "y": 7.845954750626501,
+                    "x": 2.115488089327842,
+                    "y": 7.896885044034559,
                     "heading": -1.571,
-                    "angularVelocity": -4.116449324284025e-21,
-                    "velocityX": 1.7080019456515922,
-                    "velocityY": -0.40657730261178276,
-                    "timestamp": 0.22154628599438772
+                    "angularVelocity": 4.2271636331977046e-19,
+                    "velocityX": 1.6869784967546948,
+                    "velocityY": -0.024378202384055483,
+                    "timestamp": 0.21289354284904127
                 },
                 {
-                    "x": 2.217857275730126,
-                    "y": 7.824336513669419,
+                    "x": 2.2016833316991637,
+                    "y": 7.895639874163583,
                     "heading": -1.571,
-                    "angularVelocity": -4.1164215643720176e-21,
-                    "velocityX": 2.0496018898513264,
-                    "velocityY": -0.4878943661829266,
-                    "timestamp": 0.26585554319326526
+                    "angularVelocity": 4.2271636334033824e-19,
+                    "velocityX": 2.024374276876126,
+                    "velocityY": -0.029243955789217388,
+                    "timestamp": 0.25547225141884955
                 },
                 {
-                    "x": 2.3238096389008693,
-                    "y": 7.799115127917003,
+                    "x": 2.302244453096514,
+                    "y": 7.894187824967971,
                     "heading": -1.571,
-                    "angularVelocity": -4.1164277949404545e-21,
-                    "velocityX": 2.3912015201515753,
-                    "velocityY": -0.5692125606893493,
-                    "timestamp": 0.3101648003921428
+                    "angularVelocity": 4.227163633528535e-19,
+                    "velocityX": 2.361770114104849,
+                    "velocityY": -0.03410270636161241,
+                    "timestamp": 0.2980509599886578
                 },
                 {
-                    "x": 2.444898004768031,
-                    "y": 7.770290509850712,
+                    "x": 2.4171714575816776,
+                    "y": 7.8925293954471245,
                     "heading": -1.571,
-                    "angularVelocity": -4.1164277219688166e-21,
-                    "velocityX": 2.7328006272745453,
-                    "velocityY": -0.6505326400962694,
-                    "timestamp": 0.35447405759102035
+                    "angularVelocity": 4.227163633894554e-19,
+                    "velocityX": 2.699166046728326,
+                    "velocityY": -0.03894973747556378,
+                    "timestamp": 0.3406296685584661
                 },
                 {
-                    "x": 2.5811223269665233,
-                    "y": 7.737862492432118,
+                    "x": 2.5464643533128632,
+                    "y": 7.890665591362613,
                     "heading": -1.571,
-                    "angularVelocity": -4.116427836078919e-21,
-                    "velocityX": 3.0743986880002026,
-                    "velocityY": -0.7318564893345968,
-                    "timestamp": 0.3987833147898979
+                    "angularVelocity": 4.2271636346276787e-19,
+                    "velocityX": 3.0365621709548005,
+                    "velocityY": -0.04377314735710882,
+                    "timestamp": 0.3832083771282744
                 },
                 {
-                    "x": 2.732482466386069,
-                    "y": 7.701830574535674,
+                    "x": 2.6901231650458177,
+                    "y": 7.888599493997031,
                     "heading": -1.571,
-                    "angularVelocity": -4.116426228269473e-21,
-                    "velocityX": 3.4159936091950516,
-                    "velocityY": -0.8131916482986443,
-                    "timestamp": 0.4430925719887755
+                    "angularVelocity": 4.227163652979875e-19,
+                    "velocityX": 3.3739588765926873,
+                    "velocityY": -0.04852419049232661,
+                    "timestamp": 0.42578708569808266
                 },
                 {
-                    "x": 2.8914051055908203,
-                    "y": 7.672097682952881,
-                    "heading": -1.571,
-                    "angularVelocity": -2.0582133368498575e-21,
-                    "velocityX": 3.586668999921246,
-                    "velocityY": -0.6710311447862857,
-                    "timestamp": 0.48740182918765307
+                    "x": 2.8454680992564305,
+                    "y": 7.886072511387067,
+                    "heading": -1.5709999999999997,
+                    "angularVelocity": 4.2271698321650397e-19,
+                    "velocityX": 3.6484181749177784,
+                    "velocityY": -0.05934850292215207,
+                    "timestamp": 0.46836579426789093
                 },
                 {
-                    "x": 2.9915482053824713,
-                    "y": 7.6595269719549215,
-                    "heading": -1.571,
-                    "angularVelocity": -1.8777229046989095e-21,
-                    "velocityX": 3.6204875876208633,
-                    "velocityY": -0.4544706847538144,
-                    "timestamp": 0.5150619432932944
+                    "x": 3.000000000000003,
+                    "y": 7.87,
+                    "heading": -1.5709999999999997,
+                    "angularVelocity": 2.1135793766278028e-19,
+                    "velocityX": 3.6293233386873958,
+                    "velocityY": -0.3774776625911161,
+                    "timestamp": 0.5109445028376992
                 },
                 {
-                    "x": 3.092263880199214,
-                    "y": 7.652968306689749,
-                    "heading": -1.571,
-                    "angularVelocity": -1.8777227665838625e-21,
-                    "velocityX": 3.641187973125586,
-                    "velocityY": -0.23711634883800817,
-                    "timestamp": 0.5427220573989356
+                    "x": 3.0933267855298086,
+                    "y": 7.854947925249578,
+                    "heading": -1.5709999999999997,
+                    "angularVelocity": 2.652450956207458e-19,
+                    "velocityX": 3.6023482406759033,
+                    "velocityY": -0.580999492137869,
+                    "timestamp": 0.5368517109404605
                 },
                 {
-                    "x": 3.1930238774745283,
-                    "y": 7.647129968341588,
-                    "heading": -1.571,
-                    "angularVelocity": -1.8778541277359e-21,
-                    "velocityX": 3.6427903692112493,
-                    "velocityY": -0.2110742683802162,
-                    "timestamp": 0.5703821715045769
+                    "x": 3.1856594234462645,
+                    "y": 7.8346713899397304,
+                    "heading": -1.5709999999999997,
+                    "angularVelocity": 2.6524509613572167e-19,
+                    "velocityX": 3.563974842453837,
+                    "velocityY": -0.7826599929029426,
+                    "timestamp": 0.5627589190432217
                 },
                 {
-                    "x": 3.2937837193193285,
-                    "y": 7.64128894814038,
-                    "heading": -1.571,
-                    "angularVelocity": -1.877566449716214e-21,
-                    "velocityX": 3.642784749909989,
-                    "velocityY": -0.21117122579101652,
-                    "timestamp": 0.5980422856102182
+                    "x": 3.2770421386792723,
+                    "y": 7.8104711155350515,
+                    "heading": -1.5709999999999997,
+                    "angularVelocity": 2.652450981385339e-19,
+                    "velocityX": 3.527308495401703,
+                    "velocityY": -0.9341135605458323,
+                    "timestamp": 0.588666127145983
                 },
                 {
-                    "x": 3.394543560926969,
-                    "y": 7.635447923848087,
-                    "heading": -1.571,
-                    "angularVelocity": -1.877739673587389e-21,
-                    "velocityX": 3.642784741335938,
-                    "velocityY": -0.21117137369661676,
-                    "timestamp": 0.6257023997158595
+                    "x": 3.368397018870737,
+                    "y": 7.7861659763653845,
+                    "heading": -1.5709999999999997,
+                    "angularVelocity": 2.652451746814082e-19,
+                    "velocityX": 3.5262340823876155,
+                    "velocityY": -0.9381612666736713,
+                    "timestamp": 0.6145733352487442
                 },
                 {
-                    "x": 3.495303402534779,
-                    "y": 7.6296068995587145,
-                    "heading": -1.571,
-                    "angularVelocity": -1.877724322718657e-21,
-                    "velocityX": 3.6427847413420595,
-                    "velocityY": -0.21117137359101854,
-                    "timestamp": 0.6533625138215008
+                    "x": 3.4597414869906253,
+                    "y": 7.761821735388127,
+                    "heading": -1.5709999999999997,
+                    "angularVelocity": 2.652451049308022e-19,
+                    "velocityX": 3.5258321837525024,
+                    "velocityY": -0.9396705689278512,
+                    "timestamp": 0.6404805433515055
                 },
                 {
-                    "x": 3.5960632441425897,
-                    "y": 7.623765875269346,
-                    "heading": -1.571,
-                    "angularVelocity": -1.8777552872754136e-21,
-                    "velocityX": 3.642784741342068,
-                    "velocityY": -0.211171373590874,
-                    "timestamp": 0.6810226279271421
+                    "x": 3.5510841613375907,
+                    "y": 7.7374707648012215,
+                    "heading": -1.5709999999999997,
+                    "angularVelocity": 2.6524500373464137e-19,
+                    "velocityX": 3.525762945380196,
+                    "velocityY": -0.9399303271237421,
+                    "timestamp": 0.6663877514542668
                 },
                 {
-                    "x": 3.696823085750408,
-                    "y": 7.6179248509801,
-                    "heading": -1.571,
-                    "angularVelocity": -1.877516595814209e-21,
-                    "velocityX": 3.642784741342324,
-                    "velocityY": -0.2111713735864506,
-                    "timestamp": 0.7086827420327834
+                    "x": 3.6424273103238014,
+                    "y": 7.7131215747012565,
+                    "heading": -1.5709999999999997,
+                    "angularVelocity": 2.6530371780360815e-19,
+                    "velocityX": 3.5257812661210104,
+                    "velocityY": -0.9398616015809562,
+                    "timestamp": 0.692294959557028
                 },
                 {
-                    "x": 3.7975829273583934,
-                    "y": 7.61208382669375,
-                    "heading": -1.571,
-                    "angularVelocity": -1.877721219756007e-21,
-                    "velocityX": 3.6427847413483936,
-                    "velocityY": -0.21117137348175347,
-                    "timestamp": 0.7363428561384247
+                    "x": 3.7337711953747554,
+                    "y": 7.688775146038041,
+                    "heading": -1.5709999999999997,
+                    "angularVelocity": 2.651560076415564e-19,
+                    "velocityX": 3.5258096777019814,
+                    "velocityY": -0.9397550120662235,
+                    "timestamp": 0.7182021676597893
                 },
                 {
-                    "x": 3.8983427689639933,
-                    "y": 7.606242802366244,
-                    "heading": -1.571,
-                    "angularVelocity": -1.8782896337041454e-21,
-                    "velocityX": 3.6427847412621372,
-                    "velocityY": -0.21117137496968985,
-                    "timestamp": 0.7640029702440659
+                    "x": 3.8251143320372014,
+                    "y": 7.664425909712902,
+                    "heading": -1.5709999999999997,
+                    "angularVelocity": 2.6527560956628585e-19,
+                    "velocityX": 3.525780790432169,
+                    "velocityY": -0.9398633858408949,
+                    "timestamp": 0.7441093757625505
                 },
                 {
-                    "x": 3.9991026114578196,
-                    "y": 7.60040179336099,
-                    "heading": -1.571,
-                    "angularVelocity": -1.8769887827392664e-21,
-                    "velocityX": 3.6427847733743164,
-                    "velocityY": -0.2111708210221328,
-                    "timestamp": 0.7916630843497072
+                    "x": 3.9164615673273144,
+                    "y": 7.640092054070024,
+                    "heading": -1.5709999999999997,
+                    "angularVelocity": 2.651878932000989e-19,
+                    "velocityX": 3.525938994575509,
+                    "velocityY": -0.9392697023305191,
+                    "timestamp": 0.7700165838653118
+                },
+                {
+                    "x": 4.007822648813717,
+                    "y": 7.615810235461623,
+                    "heading": -1.5709999999999997,
+                    "angularVelocity": 2.653022563334735e-19,
+                    "velocityX": 3.526473448007752,
+                    "velocityY": -0.9372611094217806,
+                    "timestamp": 0.7959237919680731
                 },
                 {
                     "x": 4.099836826324463,
                     "y": 7.594134330749512,
-                    "heading": -1.571,
-                    "angularVelocity": -2.3085742218256872e-21,
-                    "velocityX": 3.6418582541602307,
-                    "velocityY": -0.2265884582956271,
-                    "timestamp": 0.8193231984553485
-                },
-                {
-                    "x": 4.307393706052396,
-                    "y": 7.574701993779823,
-                    "heading": -1.4944377172372005,
-                    "angularVelocity": 1.1899935016259806,
-                    "velocityX": 3.226018467333577,
-                    "velocityY": -0.30203324510292745,
-                    "timestamp": 0.8836616025375443
-                },
-                {
-                    "x": 4.50626618663146,
-                    "y": 7.557280233264484,
-                    "heading": -1.3980779902010738,
-                    "angularVelocity": 1.4977015425036349,
-                    "velocityX": 3.091038446104337,
-                    "velocityY": -0.27078322448100967,
-                    "timestamp": 0.9480000066197402
-                },
-                {
-                    "x": 4.698238976536795,
-                    "y": 7.5388711827735575,
-                    "heading": -1.289295609076384,
-                    "angularVelocity": 1.6907845737938185,
-                    "velocityX": 2.9837978209729736,
-                    "velocityY": -0.2861284912726089,
-                    "timestamp": 1.012338410701936
-                },
-                {
-                    "x": 4.863540401734348,
-                    "y": 7.521644869212672,
-                    "heading": -1.1210651488169852,
-                    "angularVelocity": 2.6147751511597206,
-                    "velocityX": 2.5692496970607546,
-                    "velocityY": -0.2677454283584362,
-                    "timestamp": 1.076676814784132
-                },
-                {
-                    "x": 5.001538745942632,
-                    "y": 7.49333125872089,
-                    "heading": -0.8960981596700744,
-                    "angularVelocity": 3.4966206009633534,
-                    "velocityX": 2.144882922989247,
-                    "velocityY": -0.4400732485625517,
-                    "timestamp": 1.1410152188663278
-                },
-                {
-                    "x": 5.122682323650213,
-                    "y": 7.438843446196177,
-                    "heading": -0.6307802228270052,
-                    "angularVelocity": 4.123787971242043,
-                    "velocityX": 1.8829123823589617,
-                    "velocityY": -0.8468940643150127,
-                    "timestamp": 1.2053536229485238
-                },
-                {
-                    "x": 5.27238865572227,
-                    "y": 7.377472619855293,
-                    "heading": -0.42129435245692515,
-                    "angularVelocity": 3.2560004146582555,
-                    "velocityX": 2.326858028383782,
-                    "velocityY": -0.9538754841117861,
-                    "timestamp": 1.2696920270307197
-                },
-                {
-                    "x": 5.448010618855816,
-                    "y": 7.330334666184069,
-                    "heading": -0.26222553003546084,
-                    "angularVelocity": 2.4723774966230923,
-                    "velocityX": 2.7296599230092786,
-                    "velocityY": -0.7326565578313575,
-                    "timestamp": 1.3340304311129156
-                },
-                {
-                    "x": 5.647829111306376,
-                    "y": 7.297673043786227,
-                    "heading": -0.16202343165080943,
-                    "angularVelocity": 1.557422814787848,
-                    "velocityX": 3.1057421349040775,
-                    "velocityY": -0.507653599180288,
-                    "timestamp": 1.3983688351951116
+                    "heading": -1.5709999999999997,
+                    "angularVelocity": 8.821604445268149e-19,
+                    "velocityX": 3.551682494916745,
+                    "velocityY": -0.8366746669941396,
+                    "timestamp": 0.8218310000708343
+                },
+                {
+                    "x": 4.314086123865472,
+                    "y": 7.565866389645391,
+                    "heading": -1.5166611039162066,
+                    "angularVelocity": 0.8444409025240843,
+                    "velocityX": 3.3294910868420495,
+                    "velocityY": -0.4392913257079289,
+                    "timestamp": 0.8861799627058939
+                },
+                {
+                    "x": 4.512621682190808,
+                    "y": 7.550271754810037,
+                    "heading": -1.4174555506263589,
+                    "angularVelocity": 1.5416806927054159,
+                    "velocityX": 3.085295398641018,
+                    "velocityY": -0.24234477443848973,
+                    "timestamp": 0.9505289253409536
+                },
+                {
+                    "x": 4.703674537259271,
+                    "y": 7.533864741231446,
+                    "heading": -1.3046929681948032,
+                    "angularVelocity": 1.7523605325406573,
+                    "velocityX": 2.9690121991853746,
+                    "velocityY": -0.2549693562540318,
+                    "timestamp": 1.0148778879760132
+                },
+                {
+                    "x": 4.868121057502234,
+                    "y": 7.5183747890499655,
+                    "heading": -1.1331539755708713,
+                    "angularVelocity": 2.6657615849509235,
+                    "velocityX": 2.555542677129605,
+                    "velocityY": -0.2407179781503281,
+                    "timestamp": 1.0792268506110727
+                },
+                {
+                    "x": 5.005308136079898,
+                    "y": 7.491356941262662,
+                    "heading": -0.9056990534597417,
+                    "angularVelocity": 3.534710006145519,
+                    "velocityX": 2.131923701018079,
+                    "velocityY": -0.4198645429702136,
+                    "timestamp": 1.1435758132461322
+                },
+                {
+                    "x": 5.1255280916701365,
+                    "y": 7.4382119450007,
+                    "heading": -0.6380105243345551,
+                    "angularVelocity": 4.159950963671538,
+                    "velocityX": 1.8682500955315844,
+                    "velocityY": -0.8258873816434845,
+                    "timestamp": 1.2079247758811917
+                },
+                {
+                    "x": 5.274137127844687,
+                    "y": 7.377257884535905,
+                    "heading": -0.4258350917264682,
+                    "angularVelocity": 3.2972626740101054,
+                    "velocityX": 2.309423960995877,
+                    "velocityY": -0.9472423170300921,
+                    "timestamp": 1.2722737385162513
+                },
+                {
+                    "x": 5.448926084967563,
+                    "y": 7.329899834255518,
+                    "heading": -0.26459184239693584,
+                    "angularVelocity": 2.505762994874233,
+                    "velocityX": 2.7162668979480253,
+                    "velocityY": -0.7359567014145671,
+                    "timestamp": 1.3366227011513108
+                },
+                {
+                    "x": 5.647781118238161,
+                    "y": 7.297534713393957,
+                    "heading": -0.1613424754389799,
+                    "angularVelocity": 1.6045226329976747,
+                    "velocityX": 3.090260124291229,
+                    "velocityY": -0.5029625892352149,
+                    "timestamp": 1.4009716637863703
                 },
                 {
                     "x": 5.854011535644531,
                     "y": 7.282280921936035,
-                    "heading": -0.07451425190609767,
-                    "angularVelocity": 1.3601391112050898,
-                    "velocityX": 3.204655559605509,
-                    "velocityY": -0.23923692341711228,
-                    "timestamp": 1.4627072392773075
-                },
-                {
-                    "x": 5.983548232727189,
-                    "y": 7.284896204008577,
-                    "heading": -0.02811649988985118,
-                    "angularVelocity": 1.1686052664039241,
-                    "velocityX": 3.2625991524407896,
-                    "velocityY": 0.06587026893100557,
-                    "timestamp": 1.5024107673634195
-                },
-                {
-                    "x": 6.119161391656251,
-                    "y": 7.29693122547571,
-                    "heading": -0.002088831618303945,
-                    "angularVelocity": 0.6555505146821342,
-                    "velocityX": 3.415645043809042,
-                    "velocityY": 0.30312221727575994,
-                    "timestamp": 1.5421142954495315
-                },
-                {
-                    "x": 6.2619885188025055,
-                    "y": 7.316524083557185,
-                    "heading": -0.000009475598596952791,
-                    "angularVelocity": 0.05237207170096152,
-                    "velocityX": 3.5973409425097227,
-                    "velocityY": 0.4934790187657932,
-                    "timestamp": 1.5818178235356435
-                },
-                {
-                    "x": 6.4051075433325195,
-                    "y": 7.339003917341873,
-                    "heading": -0.00000794174457486947,
-                    "angularVelocity": 0.00003863268822752122,
-                    "velocityX": 3.6046928680898653,
-                    "velocityY": 0.566192347842029,
-                    "timestamp": 1.6215213516217555
-                },
-                {
-                    "x": 6.548226438768295,
-                    "y": 7.361484573385393,
-                    "heading": -0.000006408069907557222,
-                    "angularVelocity": 0.00003862817087655221,
-                    "velocityX": 3.6046896166348037,
-                    "velocityY": 0.5662130578109599,
-                    "timestamp": 1.6612248797078675
-                },
-                {
-                    "x": 6.691345334218433,
-                    "y": 7.383965229329843,
-                    "heading": -0.0000048743902181696355,
-                    "angularVelocity": 0.00003862829736594954,
-                    "velocityX": 3.6046896169965312,
-                    "velocityY": 0.566213055315681,
-                    "timestamp": 1.7009284077939795
-                },
-                {
-                    "x": 6.8344642296723395,
-                    "y": 7.4064458852426815,
-                    "heading": -0.000003340705516645168,
-                    "angularVelocity": 0.000038628423605030105,
-                    "velocityX": 3.604689617091453,
-                    "velocityY": 0.5662130545195021,
-                    "timestamp": 1.7406319358800915
-                },
-                {
-                    "x": 6.977583128990317,
-                    "y": 7.428926516540241,
-                    "heading": -0.0000018070121354737395,
-                    "angularVelocity": 0.000038628642216508214,
-                    "velocityX": 3.604689714414578,
-                    "velocityY": 0.5662124345424108,
-                    "timestamp": 1.7803354639662035
+                    "heading": -0.07382990470356181,
+                    "angularVelocity": 1.359968632778447,
+                    "velocityX": 3.2048755560510305,
+                    "velocityY": -0.23704797767091085,
+                    "timestamp": 1.4653206264214298
+                },
+                {
+                    "x": 5.983589932246309,
+                    "y": 7.284960854996629,
+                    "heading": -0.027638204496835066,
+                    "angularVelocity": 1.1636335547284058,
+                    "velocityX": 3.264261102719684,
+                    "velocityY": 0.06751126327279944,
+                    "timestamp": 1.5050167146092823
+                },
+                {
+                    "x": 6.11925745255698,
+                    "y": 7.297037313900181,
+                    "heading": -0.001860333184984073,
+                    "angularVelocity": 0.6493806440077139,
+                    "velocityX": 3.4176546482011396,
+                    "velocityY": 0.3042228958778286,
+                    "timestamp": 1.5447128027971349
+                },
+                {
+                    "x": 6.262128938331292,
+                    "y": 7.316680359638158,
+                    "heading": -0.000009480075392796495,
+                    "angularVelocity": 0.046625579353406145,
+                    "velocityX": 3.599132617252429,
+                    "velocityY": 0.49483580460253374,
+                    "timestamp": 1.5844088909849874
+                },
+                {
+                    "x": 6.405225952749555,
+                    "y": 7.339125340175403,
+                    "heading": -0.000007943471062431687,
+                    "angularVelocity": 0.00003870921293486644,
+                    "velocityX": 3.6048139993312827,
+                    "velocityY": 0.5654204623651486,
+                    "timestamp": 1.62410497917284
+                },
+                {
+                    "x": 6.548321378993718,
+                    "y": 7.3615804485544425,
+                    "heading": -0.000006409115010503546,
+                    "angularVelocity": 0.00003865257565632945,
+                    "velocityX": 3.6047739910041234,
+                    "velocityY": 0.5656755968668977,
+                    "timestamp": 1.6638010673606924
+                },
+                {
+                    "x": 6.691417142733098,
+                    "y": 7.384033405453259,
+                    "heading": -0.000004874435347527005,
+                    "angularVelocity": 0.00003866072787080933,
+                    "velocityX": 3.604782492980222,
+                    "velocityY": 0.5656213980739361,
+                    "timestamp": 1.703497155548545
+                },
+                {
+                    "x": 6.83451270884544,
+                    "y": 7.40648762220655,
+                    "heading": -0.0000033399372412399076,
+                    "angularVelocity": 0.000038656154204017926,
+                    "velocityX": 3.6047775144789,
+                    "velocityY": 0.5656531355692576,
+                    "timestamp": 1.7431932437363975
+                },
+                {
+                    "x": 6.977608912847965,
+                    "y": 7.428937772164743,
+                    "heading": -0.0000018048282304933975,
+                    "angularVelocity": 0.00003867154374202805,
+                    "velocityX": 3.6047935838250287,
+                    "velocityY": 0.5655506873111692,
+                    "timestamp": 1.78288933192425
                 },
                 {
                     "x": 7.12091588973999,
                     "y": 7.45,
-                    "heading": 7.358367534195036e-20,
-                    "angularVelocity": 0.000045512633828276086,
-                    "velocityX": 3.610076173553221,
-                    "velocityY": 0.5307710542512357,
-                    "timestamp": 1.8200389920523155
-                },
-                {
-                    "x": 7.357842732165802,
-                    "y": 7.451152673287208,
-                    "heading": 1.409466595931257e-14,
-                    "angularVelocity": 2.170677043875596e-13,
-                    "velocityX": 3.6488579749253423,
-                    "velocityY": 0.017752066728473136,
-                    "timestamp": 1.8849707718184656
-                },
-                {
-                    "x": 7.5947722614592195,
-                    "y": 7.450917144779397,
-                    "heading": 1.4103485973699193e-14,
-                    "angularVelocity": 1.358350936661623e-16,
-                    "velocityX": 3.6488993547799042,
-                    "velocityY": -0.0036273225323404344,
-                    "timestamp": 1.9499025515846158
-                },
-                {
-                    "x": 7.831701790736089,
-                    "y": 7.450681599643842,
-                    "heading": 1.4142344148461468e-14,
-                    "angularVelocity": 5.984461676862343e-16,
-                    "velocityX": 3.648899354525078,
-                    "velocityY": -0.0036275786125580032,
-                    "timestamp": 2.014834331350766
-                },
-                {
-                    "x": 8.060236467962737,
-                    "y": 7.450484956720549,
-                    "heading": 1.0035367627367513e-14,
-                    "angularVelocity": -6.325063837592617e-14,
-                    "velocityX": 3.5196120921020335,
-                    "velocityY": -0.0030284542330493774,
-                    "timestamp": 2.079766111116916
-                },
-                {
-                    "x": 8.255358836990055,
-                    "y": 7.450321531005239,
-                    "heading": 6.643671314235426e-15,
-                    "angularVelocity": -5.2234765862684076e-14,
-                    "velocityX": 3.005036512629802,
-                    "velocityY": -0.002516883348940323,
-                    "timestamp": 2.144697890883066
-                },
-                {
-                    "x": 8.417068879960697,
-                    "y": 7.450191322480526,
-                    "heading": 3.947270146358174e-15,
-                    "angularVelocity": -4.152667888649298e-14,
-                    "velocityX": 2.4904606581405235,
-                    "velocityY": -0.0020053127325709405,
-                    "timestamp": 2.209629670649216
-                },
-                {
-                    "x": 8.54536659092222,
-                    "y": 7.45009433114008,
-                    "heading": 1.9461799832436813e-15,
-                    "angularVelocity": -3.081834766152247e-14,
-                    "velocityX": 1.975884711978984,
-                    "velocityY": -0.001493742213689111,
-                    "timestamp": 2.2745614504153657
-                },
-                {
-                    "x": 8.640251966898402,
-                    "y": 7.450030556980587,
-                    "heading": 6.304260222246035e-16,
-                    "angularVelocity": -2.0263636169497805e-14,
-                    "velocityX": 1.4613087199813373,
-                    "velocityY": -0.0009821717458379814,
-                    "timestamp": 2.3394932301815157
+                    "heading": 5.756100309873575e-19,
+                    "angularVelocity": 0.000045466148249922065,
+                    "velocityX": 3.6101032478025736,
+                    "velocityY": 0.5305869872012366,
+                    "timestamp": 1.8225854201121026
+                },
+                {
+                    "x": 7.357842782347066,
+                    "y": 7.451140658234923,
+                    "heading": 1.3946076252127016e-14,
+                    "angularVelocity": 2.1477158274944538e-13,
+                    "velocityX": 3.6488588704771803,
+                    "velocityY": 0.017567026152596314,
+                    "timestamp": 1.8875171976944158
+                },
+                {
+                    "x": 7.594772307901899,
+                    "y": 7.450909424040305,
+                    "heading": 1.3956817600616414e-14,
+                    "angularVelocity": 1.6542514133334603e-16,
+                    "velocityX": 3.648899419925491,
+                    "velocityY": -0.003561186883034476,
+                    "timestamp": 1.9524489752767291
+                },
+                {
+                    "x": 7.831701831188383,
+                    "y": 7.450675877221787,
+                    "heading": 1.3958919096191331e-14,
+                    "angularVelocity": 3.236467023043882e-17,
+                    "velocityX": 3.6488993849911218,
+                    "velocityY": -0.00359680309416508,
+                    "timestamp": 2.0173807528590424
+                },
+                {
+                    "x": 8.06023649674366,
+                    "y": 7.450480884382308,
+                    "heading": 1.3831887827184638e-14,
+                    "angularVelocity": -1.9563805854348244e-15,
+                    "velocityX": 3.519612030728179,
+                    "velocityY": -0.0030030417576737706,
+                    "timestamp": 2.0823125304413557
+                },
+                {
+                    "x": 8.255358856071796,
+                    "y": 7.450318830701623,
+                    "heading": 1.0574442032523106e-14,
+                    "angularVelocity": -5.016720496430017e-14,
+                    "velocityX": 3.0050364643226115,
+                    "velocityY": -0.002495753030620786,
+                    "timestamp": 2.147244308023669
+                },
+                {
+                    "x": 8.417068891314898,
+                    "y": 7.450189715578673,
+                    "heading": 6.865176887322045e-15,
+                    "angularVelocity": -5.712557523159656e-14,
+                    "velocityX": 2.4904606228915314,
+                    "velocityY": -0.0019884735603726703,
+                    "timestamp": 2.2121760856059822
+                },
+                {
+                    "x": 8.54536659652034,
+                    "y": 7.450093538813877,
+                    "heading": 3.601695099473894e-15,
+                    "angularVelocity": -5.026016396528296e-14,
+                    "velocityX": 1.975884689785358,
+                    "velocityY": -0.001481197163799284,
+                    "timestamp": 2.2771078631882955
+                },
+                {
+                    "x": 8.640251968711807,
+                    "y": 7.450030300307574,
+                    "heading": 1.2253067804043182e-15,
+                    "angularVelocity": -3.659823290767036e-14,
+                    "velocityX": 1.4613087108416603,
+                    "velocityY": -0.0009739223021094422,
+                    "timestamp": 2.3420396407706088
                 },
                 {
                     "x": 8.701725006103516,
                     "y": 7.45,
-                    "heading": 2.2598418426144496e-27,
-                    "angularVelocity": -9.709051938711168e-15,
-                    "velocityX": 0.9467327004820354,
-                    "velocityY": -0.0004706013095010225,
-                    "timestamp": 2.4044250099476656
+                    "heading": -1.9088649305256032e-26,
+                    "angularVelocity": -1.8870679751086276e-14,
+                    "velocityX": 0.9467327043954562,
+                    "velocityY": -0.00046664836083104116,
+                    "timestamp": 2.406971418352922
                 },
                 {
                     "x": 8.73,
                     "y": 7.45,
-                    "heading": -4.894516543566799e-27,
-                    "angularVelocity": -1.605657569882155e-25,
-                    "velocityX": 0.47336647332214415,
-                    "velocityY": -1.2371738084027526e-15,
-                    "timestamp": 2.4641567304855543
+                    "heading": 2.623532798918036e-26,
+                    "angularVelocity": 9.781191839132487e-25,
+                    "velocityX": 0.4733664733221421,
+                    "velocityY": 5.037124173048106e-15,
+                    "timestamp": 2.4667031388908103
                 },
                 {
                     "x": 8.73,
                     "y": 7.45,
-                    "heading": -2.4365105603708565e-27,
-                    "angularVelocity": 3.5986630710075474e-28,
-                    "velocityX": 4.937529040802876e-22,
-                    "velocityY": 5.715521127367984e-21,
-                    "timestamp": 2.523888451023443
+                    "heading": 1.3100764528202313e-26,
+                    "angularVelocity": -5.658464945099905e-28,
+                    "velocityX": 7.815178981981403e-20,
+                    "velocityY": 1.9973998116523263e-19,
+                    "timestamp": 2.5264348594286985
                 }
             ],
             "constraints": [