orks

15 files changed, 5014 insertions, 79 deletions

diff --git a/.Glass/glass.json b/.Glass/glass.json
index e7b9ec9..d58cf04 100644
--- a/.Glass/glass.json
+++ b/.Glass/glass.json
@@ -13,6 +13,9 @@
     }
   },
   "NetworkTables Info": {
+    "Clients": {
+      "open": true
+    },
     "visible": true
   },
   "NetworkTables Settings": {
diff --git a/.SysId/sysid.json b/.SysId/sysid.json
index d8d2589..0967ef4 100644
--- a/.SysId/sysid.json
+++ b/.SysId/sysid.json
@@ -1,9 +1 @@
-{
-  "SysId": {
-    "Data Selector": {
-      "window": {
-        "visible": false
-      }
-    }
-  }
-}
+{}
diff --git a/PathWeaver/Paths/Unnamed.path b/PathWeaver/Paths/Unnamed.path
new file mode 100644
index 0000000..95acbca
--- /dev/null
+++ b/PathWeaver/Paths/Unnamed.path
@@ -0,0 +1,14 @@
+X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name

+1.3928313275795738,-4.303791875932503,0.24240454789842625,0.022036777081675396,true,false,

+2.560780512908356,-2.8052910343785933,0.4848090957968534,1.2671146821963208,true,false,

+3.6185458128287626,-1.1855879188754703,0.920655093454001,0.6421533221411488,false,false,

+5.194175374168535,-0.7117972116194546,0.7492504207769555,0.08814710832670059,true,false,

+7.254614031305161,-0.6567052689152667,1.0577652999204066,-0.055091942704187935,true,false,

+8.279324165603056,-1.2296614730388205,0.7823055863994686,-0.8043423634811429,true,false,

+8.213213834358031,-2.2102980531733643,-0.9806365801345454,-0.5839745926643909,true,false,

+7.144430145896786,-3.609633397859736,-0.03305516562251398,-1.6086847269622853,true,false,

+7.6292392416936385,-4.876748080056056,0.6327044008273585,-1.0684319556620538,false,false,

+9.480328516554351,-6.485432807018342,1.0577652999204066,-0.5949929812052286,true,false,

+12.334091148631282,-6.970241902815195,3.008020071648655,0.1432390510308892,true,false,

+14.405548194308746,-5.725163997700549,0.8484159176444912,1.5205376186355846,true,false,

+14.51573207971712,-2.474739378153466,0.0,0.848415917644493,true,false,

diff --git a/PathWeaver/output/Unnamed.wpilib.json b/PathWeaver/output/Unnamed.wpilib.json
new file mode 100644
index 0000000..c1aa487
--- /dev/null
+++ b/PathWeaver/output/Unnamed.wpilib.json
@@ -0,0 +1,4682 @@
+[
+{
+"acceleration": 1.5,
+"curvature": 0.0,
+"pose": {
+"rotation": {
+"radians": 0.09065988720074628
+},
+"translation": {
+"x": 1.3928313275795738,
+"y": 3.9067581240674967
+}
+},
+"time": 0.0,
+"velocity": 0.0
+},
+{
+"acceleration": 1.5,
+"curvature": 12.584358928909815,
+"pose": {
+"rotation": {
+"radians": 0.11627112707766694
+},
+"translation": {
+"x": 1.3966497846988808,
+"y": 3.9071389771594935
+}
+},
+"time": 0.07152997757673454,
+"velocity": 0.10729496636510182
+},
+{
+"acceleration": 1.5000000000000004,
+"curvature": 19.174871153408663,
+"pose": {
+"rotation": {
+"radians": 0.18301163899272396
+},
+"translation": {
+"x": 1.4006478551406178,
+"y": 3.907732710874286
+}
+},
+"time": 0.1024976066076931,
+"velocity": 0.15374640991153965
+},
+{
+"acceleration": 1.4999999999999951,
+"curvature": 19.93220727219254,
+"pose": {
+"rotation": {
+"radians": 0.2255950592561312
+},
+"translation": {
+"x": 1.4027661944758616,
+"y": 3.908171320660859
+}
+},
+"time": 0.1157156872775916,
+"velocity": 0.17357353091638741
+},
+{
+"acceleration": 1.5000000000000004,
+"curvature": 19.367866923101765,
+"pose": {
+"rotation": {
+"radians": 0.2708802660269444
+},
+"translation": {
+"x": 1.4049897116095122,
+"y": 3.9087351657914127
+}
+},
+"time": 0.12825227343830964,
+"velocity": 0.19237841015746443
+},
+{
+"acceleration": 1.4999999999999967,
+"curvature": 17.90661640926242,
+"pose": {
+"rotation": {
+"radians": 0.316699877196034
+},
+"translation": {
+"x": 1.4073363770326133,
+"y": 3.909445907594436
+}
+},
+"time": 0.14042041976366923,
+"velocity": 0.21063062964550383
+},
+{
+"acceleration": 1.4999999999999993,
+"curvature": 15.956531261474854,
+"pose": {
+"rotation": {
+"radians": 0.3613926244443248
+},
+"translation": {
+"x": 1.409823172821509,
+"y": 3.9103241138990468
+}
+},
+"time": 0.15242803712203215,
+"velocity": 0.22864205568304818
+},
+{
+"acceleration": 1.4999999999999996,
+"curvature": 11.782373823594101,
+"pose": {
+"rotation": {
+"radians": 0.4433048494863578
+},
+"translation": {
+"x": 1.415280242857356,
+"y": 3.9126598311954064
+}
+},
+"time": 0.17649042825290354,
+"velocity": 0.26473564237935526
+},
+{
+"acceleration": 1.4999999999999996,
+"curvature": 8.259167775073706,
+"pose": {
+"rotation": {
+"radians": 0.5124477728190026
+},
+"translation": {
+"x": 1.4214776505161897,
+"y": 3.915885670290163
+}
+},
+"time": 0.20115780841328187,
+"velocity": 0.30173671261992274
+},
+{
+"acceleration": 1.4999999999999993,
+"curvature": 5.692201613184614,
+"pose": {
+"rotation": {
+"radians": 0.568880992811599
+},
+"translation": {
+"x": 1.4285173886691178,
+"y": 3.920128633005848
+}
+},
+"time": 0.22676824068695361,
+"velocity": 0.34015236103043034
+},
+{
+"acceleration": 1.4999999999999993,
+"curvature": 3.9354504470194467,
+"pose": {
+"rotation": {
+"radians": 0.6143814490671687
+},
+"translation": {
+"x": 1.4364872536128057,
+"y": 3.9254999429792603
+}
+},
+"time": 0.2534529353981691,
+"velocity": 0.38017940309725357
+},
+{
+"acceleration": 1.5000000000000004,
+"curvature": 1.971565028614077,
+"pose": {
+"rotation": {
+"radians": 0.6808435547702958
+},
+"translation": {
+"x": 1.4555008023084117,
+"y": 3.939997043926266
+}
+},
+"time": 0.3100293399931443,
+"velocity": 0.46504400998971634
+},
+{
+"acceleration": 1.4999999999999993,
+"curvature": 1.0739937269409685,
+"pose": {
+"rotation": {
+"radians": 0.7255111860637624
+},
+"translation": {
+"x": 1.4789572309236787,
+"y": 3.9599729573644797
+}
+},
+"time": 0.37040230615762854,
+"velocity": 0.5556034592364427
+},
+{
+"acceleration": 1.5,
+"curvature": 0.4101380038202177,
+"pose": {
+"rotation": {
+"radians": 0.7803195355651401
+},
+"translation": {
+"x": 1.5400085917022068,
+"y": 4.017684459484972
+}
+},
+"time": 0.4992122160882051,
+"velocity": 0.7488183241323074
+},
+{
+"acceleration": 1.5000000000000004,
+"curvature": 0.2101040025305299,
+"pose": {
+"rotation": {
+"radians": 0.813213834238043
+},
+"translation": {
+"x": 1.6196109469449602,
+"y": 4.099416051987036
+}
+},
+"time": 0.6335086327106696,
+"velocity": 0.9502629490660042
+},
+{
+"acceleration": 1.5,
+"curvature": 0.1653849061607894,
+"pose": {
+"rotation": {
+"radians": 0.8257548254297804
+},
+"translation": {
+"x": 1.6657279363246777,
+"y": 4.148813948103339
+}
+},
+"time": 0.7010268155474854,
+"velocity": 1.0515402233212279
+},
+{
+"acceleration": 1.5000000000000007,
+"curvature": 0.13778861510520488,
+"pose": {
+"rotation": {
+"radians": 0.8368529696281827
+},
+"translation": {
+"x": 1.715480329767629,
+"y": 4.203375981575104
+}
+},
+"time": 0.76804405053245,
+"velocity": 1.1520660757986747
+},
+{
+"acceleration": 1.5,
+"curvature": 0.12095417272807119,
+"pose": {
+"rotation": {
+"radians": 0.8470543049875805
+},
+"translation": {
+"x": 1.7683211212716001,
+"y": 4.262572293411064
+}
+},
+"time": 0.8340809769631005,
+"velocity": 1.2511214654446505
+},
+{
+"acceleration": 1.5000000000000007,
+"curvature": 0.11138334689917267,
+"pose": {
+"rotation": {
+"radians": 0.8567659948048121
+},
+"translation": {
+"x": 1.8236314934756503,
+"y": 4.325785482853849
+}
+},
+"time": 0.8987125719497603,
+"velocity": 1.3480688579246403
+},
+{
+"acceleration": 1.5000000000000007,
+"curvature": 0.10714460672372975,
+"pose": {
+"rotation": {
+"radians": 0.8663117248069004
+},
+"translation": {
+"x": 1.8807380769748197,
+"y": 4.392328930625051
+}
+},
+"time": 0.9615621006217527,
+"velocity": 1.442343150932629
+},
+{
+"acceleration": 1.5,
+"curvature": 0.10722923181467173,
+"pose": {
+"rotation": {
+"radians": 0.8759675692376021
+},
+"translation": {
+"x": 1.9389302096348355,
+"y": 4.461465122170289
+}
+},
+"time": 1.0222967197423896,
+"velocity": 1.5334450796135841
+},
+{
+"acceleration": 1.4999999999999964,
+"curvature": 0.11122768486929695,
+"pose": {
+"rotation": {
+"radians": 0.885986765306922
+},
+"translation": {
+"x": 1.9974771959068207,
+"y": 4.5324239709042775
+}
+},
+"time": 1.080624511671222,
+"velocity": 1.620936767506833
+},
+{
+"acceleration": 1.499999999999999,
+"curvature": 0.11917459801649088,
+"pose": {
+"rotation": {
+"radians": 0.896618181102282
+},
+"translation": {
+"x": 2.055645566141999,
+"y": 4.604421141455889
+}
+},
+"time": 1.1362927508056273,
+"velocity": 1.7044391262084404
+},
+{
+"acceleration": 1.4999999999999951,
+"curvature": 0.13149063117393758,
+"pose": {
+"rotation": {
+"radians": 0.9081212121400041
+},
+"translation": {
+"x": 2.112716335906404,
+"y": 4.676676372913224
+}
+},
+"time": 1.189087255861223,
+"velocity": 1.783630883791834
+},
+{
+"acceleration": 1.4999999999999976,
+"curvature": 0.1489855484784283,
+"pose": {
+"rotation": {
+"radians": 0.920778528745865
+},
+"translation": {
+"x": 2.168002265295584,
+"y": 4.74843180206867
+}
+},
+"time": 1.238832732778238,
+"velocity": 1.8582490991673561
+},
+{
+"acceleration": 1.4999999999999973,
+"curvature": 0.172900579171571,
+"pose": {
+"rotation": {
+"radians": 0.9349069756789327
+},
+"translation": {
+"x": 2.2208651182493138,
+"y": 4.818970286663976
+}
+},
+"time": 1.285394056521778,
+"velocity": 1.928091084782666
+},
+{
+"acceleration": 1.5000000000000082,
+"curvature": 0.20496457583705746,
+"pose": {
+"rotation": {
+"radians": 0.9508655146023578
+},
+"translation": {
+"x": 2.2707329218662937,
+"y": 4.88763372863531
+}
+},
+"time": 1.3286784726644112,
+"velocity": 1.9930177089966157
+},
+{
+"acceleration": 1.4999999999999956,
+"curvature": 0.2474109272567346,
+"pose": {
+"rotation": {
+"radians": 0.9690567821648134
+},
+"translation": {
+"x": 2.3171172257188655,
+"y": 4.953841397358332
+}
+},
+"time": 1.3686387184909456,
+"velocity": 2.0529580777364176
+},
+{
+"acceleration": 1.500000000000003,
+"curvature": 0.3028295728623051,
+"pose": {
+"rotation": {
+"radians": 0.9899146763027574
+},
+"translation": {
+"x": 2.359630361167713,
+"y": 5.017108252893251
+}
+},
+"time": 1.4052770626048803,
+"velocity": 2.1079155939073195
+},
+{
+"acceleration": 0.9016425025228133,
+"curvature": 0.3735709135788844,
+"pose": {
+"rotation": {
+"radians": 1.013863030267036
+},
+"translation": {
+"x": 2.3980027006765736,
+"y": 5.077063269229901
+}
+},
+"time": 1.4386502303933462,
+"velocity": 2.1579753455900184
+},
+{
+"acceleration": -1.500000000000002,
+"curvature": 0.4601282715767947,
+"pose": {
+"rotation": {
+"radians": 1.041218523211808
+},
+"translation": {
+"x": 2.4320999171269424,
+"y": 5.1334677575328005
+}
+},
+"time": 1.4690001712977319,
+"velocity": 2.185340142258468
+},
+{
+"acceleration": 0.11538280211878277,
+"curvature": 0.6488075195771044,
+"pose": {
+"rotation": {
+"radians": 1.1055594034080038
+},
+"translation": {
+"x": 2.4877117303552225,
+"y": 5.235442020039241
+}
+},
+"time": 1.5231575793716177,
+"velocity": 2.1041040301476395
+},
+{
+"acceleration": 1.4999999999999987,
+"curvature": 0.6383802829353782,
+"pose": {
+"rotation": {
+"radians": 1.1721061912038242
+},
+"translation": {
+"x": 2.528753046218558,
+"y": 5.324464556534931
+}
+},
+"time": 1.5696869586501039,
+"velocity": 2.109472720309639
+},
+{
+"acceleration": 1.4999999999999922,
+"curvature": 5.8314319337489114e-15,
+"pose": {
+"rotation": {
+"radians": 1.2053717623294868
+},
+"translation": {
+"x": 2.5607805129083543,
+"y": 5.405258965621411
+}
+},
+"time": 1.6103007735438228,
+"velocity": 2.170393442650217
+},
+{
+"acceleration": 1.5,
+"curvature": -0.23765558046386948,
+"pose": {
+"rotation": {
+"radians": 1.1936472713482496
+},
+"translation": {
+"x": 2.591971715019674,
+"y": 5.485781110084698
+}
+},
+"time": 1.6495547483577475,
+"velocity": 2.229274404871104
+},
+{
+"acceleration": 0.8320926812026572,
+"curvature": -0.2888537099977768,
+"pose": {
+"rotation": {
+"radians": 1.167550954733963
+},
+"translation": {
+"x": 2.6278787881228487,
+"y": 5.573179815544707
+}
+},
+"time": 1.6913517854754143,
+"velocity": 2.291969960547604
+},
+{
+"acceleration": 0.5944764390938576,
+"curvature": -0.2504447117674668,
+"pose": {
+"rotation": {
+"radians": 1.1382392093566074
+},
+"translation": {
+"x": 2.671600495521087,
+"y": 5.671544318133349
+}
+},
+"time": 1.7379236466673191,
+"velocity": 2.330722065395374
+},
+{
+"acceleration": 0.4469681822219057,
+"curvature": -0.2016967143256976,
+"pose": {
+"rotation": {
+"radians": 1.1106153421017066
+},
+"translation": {
+"x": 2.724693102998651,
+"y": 5.782338287510987
+}
+},
+"time": 1.7902864656575945,
+"velocity": 2.3618505275696293
+},
+{
+"acceleration": 0.3371716320377961,
+"curvature": -0.1822342043045817,
+"pose": {
+"rotation": {
+"radians": 1.0977665318481775
+},
+"translation": {
+"x": 2.754878043875048,
+"y": 5.842248173507871
+}
+},
+"time": 1.818613884540857,
+"velocity": 2.3745119824949197
+},
+{
+"acceleration": 0.23752576703719405,
+"curvature": -0.16701202729556114,
+"pose": {
+"rotation": {
+"radians": 1.0854746115475387
+},
+"translation": {
+"x": 2.787414160776401,
+"y": 5.9048564935854895
+}
+},
+"time": 1.848266104894896,
+"velocity": 2.3845098700252354
+},
+{
+"acceleration": 0.14980674295997043,
+"curvature": -0.15593709693665408,
+"pose": {
+"rotation": {
+"radians": 1.0736062770475898
+},
+"translation": {
+"x": 2.822170718650617,
+"y": 5.969811125279593
+}
+},
+"time": 1.8791135326179407,
+"velocity": 2.391836928956276
+},
+{
+"acceleration": 0.07178156782273779,
+"curvature": -0.14875505757516289,
+"pose": {
+"rotation": {
+"radians": 1.062003715060932
+},
+"translation": {
+"x": 2.8589662854673312,
+"y": 6.036681473231283
+}
+},
+"time": 1.910992468414088,
+"velocity": 2.396612608496927
+},
+{
+"acceleration": -0.00038889580779420186,
+"curvature": -0.14523364114044093,
+"pose": {
+"rotation": {
+"radians": 1.0504958274564906
+},
+"translation": {
+"x": 2.8975763504040204,
+"y": 6.104972740021984
+}
+},
+"time": 1.9437102228971632,
+"velocity": 2.3989611402093614
+},
+{
+"acceleration": -0.0711963967044641,
+"curvature": -0.14525306379485814,
+"pose": {
+"rotation": {
+"radians": 1.0389022367134124
+},
+"translation": {
+"x": 2.937740942032112,
+"y": 6.174140197008411
+}
+},
+"time": 1.9770511283051537,
+"velocity": 2.39894817407102
+},
+{
+"acceleration": -0.14557818074451442,
+"curvature": -0.14885416295406625,
+"pose": {
+"rotation": {
+"radians": 1.0270328658622199
+},
+"translation": {
+"x": 2.979172246503095,
+"y": 6.243603455157542
+}
+},
+"time": 2.010783105276969,
+"velocity": 2.396546578856909
+},
+{
+"acceleration": -0.22908702452889004,
+"curvature": -0.15627279170879435,
+"pose": {
+"rotation": {
+"radians": 1.0146847430079062
+},
+"translation": {
+"x": 3.0215622257346304,
+"y": 6.3127607358815885
+}
+},
+"time": 2.0446645821769285,
+"velocity": 2.3916141750888755
+},
+{
+"acceleration": -0.3282517325299175,
+"curvature": -0.16797636867380203,
+"pose": {
+"rotation": {
+"radians": 1.0016369697347198
+},
+"translation": {
+"x": 3.0645902355966625,
+"y": 6.381003141872965
+}
+},
+"time": 2.0784516519190626,
+"velocity": 2.3838739958141
+},
+{
+"acceleration": -0.9988585480006459,
+"curvature": -0.18471162538506675,
+"pose": {
+"rotation": {
+"radians": 0.9876444205047119
+},
+"translation": {
+"x": 3.1079306440975287,
+"y": 6.447728927939258
+}
+},
+"time": 2.1119053971937674,
+"velocity": 2.3728927459680635
+},
+{
+"acceleration": -1.499999999999994,
+"curvature": -0.2075681008664555,
+"pose": {
+"rotation": {
+"radians": 0.9724306568797655
+},
+"translation": {
+"x": 3.1512604495700716,
+"y": 6.512357771838203
+}
+},
+"time": 2.144925988799337,
+"velocity": 2.339909845782802
+},
+{
+"acceleration": -1.499999999999997,
+"curvature": -0.23805639760451866,
+"pose": {
+"rotation": {
+"radians": 0.9556807755718008
+},
+"translation": {
+"x": 3.194266898857747,
+"y": 6.574345045112645
+}
+},
+"time": 2.17750905280457,
+"velocity": 2.291035249774952
+},
+{
+"acceleration": -1.500000000000003,
+"curvature": -0.27818873959210694,
+"pose": {
+"rotation": {
+"radians": 0.9370356042732699
+},
+"translation": {
+"x": 3.236655105500737,
+"y": 6.633196083925521
+}
+},
+"time": 2.2095010911053845,
+"velocity": 2.2430471923237305
+},
+{
+"acceleration": -1.4999999999999982,
+"curvature": -0.33052304667748805,
+"pose": {
+"rotation": {
+"radians": 0.9160901184970405
+},
+"translation": {
+"x": 3.27815566792206,
+"y": 6.6884804598948175
+}
+},
+"time": 2.2406441096763956,
+"velocity": 2.196332664467214
+},
+{
+"acceleration": -1.5,
+"curvature": -0.4838746246224618,
+"pose": {
+"rotation": {
+"radians": 0.8655188434945268
+},
+"translation": {
+"x": 3.3575893873226184,
+"y": 6.787034312059733
+}
+},
+"time": 2.2994578849179566,
+"velocity": 2.1081120016048724
+},
+{
+"acceleration": -1.5,
+"curvature": -0.7136568979955945,
+"pose": {
+"rotation": {
+"radians": 0.800790925613983
+},
+"translation": {
+"x": 3.431212033172492,
+"y": 6.8683901753813075
+}
+},
+"time": 2.3525069400460183,
+"velocity": 2.0285384189127798
+},
+{
+"acceleration": 0.8838499819108474,
+"curvature": -0.9536799634883513,
+"pose": {
+"rotation": {
+"radians": 0.7226771338523006
+},
+"translation": {
+"x": 3.4985629036212034,
+"y": 6.932872724352457
+}
+},
+"time": 2.399281126728279,
+"velocity": 1.9583771388893885
+},
+{
+"acceleration": 1.499999999999997,
+"curvature": -0.8750186855672413,
+"pose": {
+"rotation": {
+"radians": 0.6457996807566623
+},
+"translation": {
+"x": 3.5603204008102742,
+"y": 6.983204400904163
+}
+},
+"time": 2.439595874392357,
+"velocity": 1.994009327883024
+},
+{
+"acceleration": -0.17673488022513,
+"curvature": 4.0657125409604414e-14,
+"pose": {
+"rotation": {
+"radians": 0.6090435061130041
+},
+"translation": {
+"x": 3.6185458128287626,
+"y": 7.0249620811245315
+}
+},
+"time": 2.475056180353696,
+"velocity": 2.0471997868250327
+},
+{
+"acceleration": -1.0497478572590713,
+"curvature": -0.775451643522088,
+"pose": {
+"rotation": {
+"radians": 0.576629397352888
+},
+"translation": {
+"x": 3.6776892207385403,
+"y": 7.065206626453871
+}
+},
+"time": 2.510052958247798,
+"velocity": 2.041014635475653
+},
+{
+"acceleration": 1.5000000000000098,
+"curvature": -0.8619760935015458,
+"pose": {
+"rotation": {
+"radians": 0.508573204778664
+},
+"translation": {
+"x": 3.7452033745184887,
+"y": 7.105960754631148
+}
+},
+"time": 2.549082846852517,
+"velocity": 2.0000430935437894
+},
+{
+"acceleration": 1.5,
+"curvature": -0.661029691908854,
+"pose": {
+"rotation": {
+"radians": 0.43879282929817287
+},
+"translation": {
+"x": 3.8262667678750133,
+"y": 7.147354585737661
+}
+},
+"time": 2.593840878789894,
+"velocity": 2.0671801414498554
+},
+{
+"acceleration": 1.5000000000000038,
+"curvature": -0.45692997173249555,
+"pose": {
+"rotation": {
+"radians": 0.3805832368574192
+},
+"translation": {
+"x": 3.923037785245287,
+"y": 7.1891720963872965
+}
+},
+"time": 2.643927606409668,
+"velocity": 2.1423102328795163
+},
+{
+"acceleration": 1.5,
+"curvature": -0.31490641549261783,
+"pose": {
+"rotation": {
+"radians": 0.33518315157269535
+},
+"translation": {
+"x": 4.035163284197612,
+"y": 7.230925717232347
+}
+},
+"time": 2.698726026727654,
+"velocity": 2.2245078633564956
+},
+{
+"acceleration": 1.5000000000000067,
+"curvature": -0.26495259424653567,
+"pose": {
+"rotation": {
+"radians": 0.316547509033942
+},
+"translation": {
+"x": 4.0963120205790196,
+"y": 7.2515703106880665
+}
+},
+"time": 2.7274606507388572,
+"velocity": 2.2676097993733006
+},
+{
+"acceleration": 1.5,
+"curvature": -0.22585471052654335,
+"pose": {
+"rotation": {
+"radians": 0.3001336225766449
+},
+"translation": {
+"x": 4.16028717783178,
+"y": 7.271930930469322
+}
+},
+"time": 2.756783213930512,
+"velocity": 2.3115936441607827
+},
+{
+"acceleration": 0.8426317956907995,
+"curvature": -0.19544743654965796,
+"pose": {
+"rotation": {
+"radians": 0.28560109832842523
+},
+"translation": {
+"x": 4.226564724320221,
+"y": 7.291903477307758
+}
+},
+"time": 2.7864431476718896,
+"velocity": 2.356083544772849
+},
+{
+"acceleration": 0.39462468384920596,
+"curvature": -0.1719692027377567,
+"pose": {
+"rotation": {
+"radians": 0.27264627670673025
+},
+"translation": {
+"x": 4.294559017179436,
+"y": 7.31138086734476
+}
+},
+"time": 2.816303441602822,
+"velocity": 2.3812447778677255
+},
+{
+"acceleration": 0.29695755586774497,
+"curvature": -0.1540603917815662,
+"pose": {
+"rotation": {
+"radians": 0.2610031471062409
+},
+"translation": {
+"x": 4.363638695515291,
+"y": 7.33025536330351
+}
+},
+"time": 2.846302132071442,
+"velocity": 2.393083001609795
+},
+{
+"acceleration": 0.2143878280783764,
+"curvature": -0.1407158956523086,
+"pose": {
+"rotation": {
+"radians": 0.2504397474465357
+},
+"translation": {
+"x": 4.433142573604437,
+"y": 7.348420905661045
+}
+},
+"time": 2.876265670676984,
+"velocity": 2.4019809007992454
+},
+{
+"acceleration": 0.14021614296276558,
+"curvature": -0.13123229486604726,
+"pose": {
+"rotation": {
+"radians": 0.240752700237494
+},
+"translation": {
+"x": 4.502395534094326,
+"y": 7.365775443820309
+}
+},
+"time": 2.9059494594714677,
+"velocity": 2.408344743808032
+},
+{
+"acceleration": 0.0679853328699442,
+"curvature": -0.12516612375841452,
+"pose": {
+"rotation": {
+"radians": 0.23176118152428202
+},
+"translation": {
+"x": 4.570724421203216,
+"y": 7.382223267282216
+}
+},
+"time": 2.9351068435939887,
+"velocity": 2.412433079748576
+},
+{
+"acceleration": -0.0092775249150282,
+"curvature": -0.12230947390508816,
+"pose": {
+"rotation": {
+"radians": 0.22330089468137157
+},
+"translation": {
+"x": 4.637473933920184,
+"y": 7.3976773368177025
+}
+},
+"time": 2.9634963368603824,
+"velocity": 2.4143631488983006
+},
+{
+"acceleration": -0.15605810121257305,
+"curvature": -0.12268535102460874,
+"pose": {
+"rotation": {
+"radians": 0.21521826693242077
+},
+"translation": {
+"x": 4.702022519205141,
+"y": 7.412061615639786
+}
+},
+"time": 2.9908888085464627,
+"velocity": 2.414109014559749
+},
+{
+"acceleration": -0.47291942797227987,
+"curvature": -0.1344965347507388,
+"pose": {
+"rotation": {
+"radians": 0.19959296570643423
+},
+"translation": {
+"x": 4.822294794372881,
+"y": 7.4373856532385485
+}
+},
+"time": 3.041885824543538,
+"velocity": 2.406150517075738
+},
+{
+"acceleration": -1.2825960699704075,
+"curvature": -0.1663860024185031,
+"pose": {
+"rotation": {
+"radians": 0.18368147341760305
+},
+"translation": {
+"x": 4.92784772340276,
+"y": 7.4578957191624005
+}
+},
+"time": 3.086772271330193,
+"velocity": 2.384922844337685
+},
+{
+"acceleration": -1.5000000000000062,
+"curvature": -0.2284723311614058,
+"pose": {
+"rotation": {
+"radians": 0.16625280345787866
+},
+"translation": {
+"x": 5.016544915325127,
+"y": 7.473617386580145
+}
+},
+"time": 3.1249343905960787,
+"velocity": 2.335976260145518
+},
+{
+"acceleration": 1.4999999999999958,
+"curvature": -0.3181237292613115,
+"pose": {
+"rotation": {
+"radians": 0.14657099319654823
+},
+"translation": {
+"x": 5.088315693904711,
+"y": 7.484976060251313
+}
+},
+"time": 3.156357924237339,
+"velocity": 2.2888409596836277
+},
+{
+"acceleration": -0.8632539991479081,
+"curvature": -4.549285404322813e-14,
+"pose": {
+"rotation": {
+"radians": 0.11710874456685227
+},
+"translation": {
+"x": 5.194175374168538,
+"y": 7.498752788380543
+}
+},
+"time": 3.202306475289804,
+"velocity": 2.3577637862623253
+},
+{
+"acceleration": 1.5,
+"curvature": -0.27414053336268907,
+"pose": {
+"rotation": {
+"radians": 0.08119277125978723
+},
+"translation": {
+"x": 5.306967922244752,
+"y": 7.510127942981009
+}
+},
+"time": 3.250818756283532,
+"velocity": 2.3158853656867024
+},
+{
+"acceleration": 1.5,
+"curvature": -0.14020638673754293,
+"pose": {
+"rotation": {
+"radians": 0.06419675846444771
+},
+"translation": {
+"x": 5.392925193940504,
+"y": 7.516305432628022
+}
+},
+"time": 3.287592904249051,
+"velocity": 2.3710465876349804
+},
+{
+"acceleration": 0.6612069900477351,
+"curvature": -0.0748146522674918,
+"pose": {
+"rotation": {
+"radians": 0.05270510089802956
+},
+"translation": {
+"x": 5.505466370737315,
+"y": 7.522823232878449
+}
+},
+"time": 3.334442917707495,
+"velocity": 2.4413216078226467
+},
+{
+"acceleration": 0.2848112114864506,
+"curvature": -0.05713937501194204,
+"pose": {
+"rotation": {
+"radians": 0.04836999957288724
+},
+"translation": {
+"x": 5.571817567277828,
+"y": 7.526172841932824
+}
+},
+"time": 3.361556368118334,
+"velocity": 2.459249210758606
+},
+{
+"acceleration": 0.18080258487991505,
+"curvature": -0.04518707082924415,
+"pose": {
+"rotation": {
+"radians": 0.0446806750124015
+},
+"translation": {
+"x": 5.6445146605061876,
+"y": 7.5295522850392995
+}
+},
+"time": 3.3910984404575313,
+"velocity": 2.467663124171353
+},
+{
+"acceleration": 0.11568936815825505,
+"curvature": -0.03706188859598559,
+"pose": {
+"rotation": {
+"radians": 0.041472186507363916
+},
+"translation": {
+"x": 5.72304893015116,
+"y": 7.53293318481092
+}
+},
+"time": 3.4229161912498216,
+"velocity": 2.473415855759664
+},
+{
+"acceleration": 0.07328665427805611,
+"curvature": -0.03155330786136919,
+"pose": {
+"rotation": {
+"radians": 0.03861533333756962
+},
+"translation": {
+"x": 5.806759175018703,
+"y": 7.53628383809324
+}
+},
+"time": 3.456760487345439,
+"velocity": 2.477331280990727
+},
+{
+"acceleration": 0.04414756465343292,
+"curvature": -0.027895723481140427,
+"pose": {
+"rotation": {
+"radians": 0.03600759792580103
+},
+"translation": {
+"x": 5.8948565380336655,
+"y": 7.5395700434657105
+}
+},
+"time": 3.4923279054118765,
+"velocity": 2.479937898062125
+},
+{
+"acceleration": 0.02259107017978311,
+"curvature": -0.025611061813077714,
+"pose": {
+"rotation": {
+"radians": 0.03356544180829089
+},
+"translation": {
+"x": 5.98644933128149,
+"y": 7.542755928743072
+}
+},
+"time": 3.529271595423312,
+"velocity": 2.4815688720054414
+},
+{
+"acceleration": 0.004987030624435722,
+"curvature": -0.02441162402852412,
+"pose": {
+"rotation": {
+"radians": 0.03121812475280187
+},
+"translation": {
+"x": 6.080567861049917,
+"y": 7.545804778476744
+}
+},
+"time": 3.5672119645502223,
+"velocity": 2.4824259855470343
+},
+{
+"acceleration": -0.011250208414384632,
+"curvature": -0.024142807496834874,
+"pose": {
+"rotation": {
+"radians": 0.0289027514859915
+},
+"translation": {
+"x": 6.176189252870683,
+"y": 7.548679861456216
+}
+},
+"time": 3.6057472130971058,
+"velocity": 2.482618162011658
+},
+{
+"acceleration": -0.02830334039719144,
+"curvature": -0.02475214180208891,
+"pose": {
+"rotation": {
+"radians": 0.026560162954176333
+},
+"translation": {
+"x": 6.27226227656123,
+"y": 7.551345258210432
+}
+},
+"time": 3.6444637678885523,
+"velocity": 2.482182592701167
+},
+{
+"acceleration": -0.04843956915663519,
+"curvature": -0.026276793091310206,
+"pose": {
+"rotation": {
+"radians": 0.024131316874241637
+},
+"translation": {
+"x": 6.367732171266395,
+"y": 7.55376668850919
+}
+},
+"time": 3.6829466560674047,
+"velocity": 2.481093398417574
+},
+{
+"acceleration": -0.07448716450338778,
+"curvature": -0.02884579777333522,
+"pose": {
+"rotation": {
+"radians": 0.021553856282402844
+},
+"translation": {
+"x": 6.461565470500126,
+"y": 7.555912338864523
+}
+},
+"time": 3.720789855889941,
+"velocity": 2.479260290122662
+},
+{
+"acceleration": -0.5431294518241827,
+"curvature": -0.03269619792375755,
+"pose": {
+"rotation": {
+"radians": 0.01875862583172248
+},
+"translation": {
+"x": 6.552774827187173,
+"y": 7.557753690032097
+}
+},
+"time": 3.75760665351625,
+"velocity": 2.476517911261383
+},
+{
+"acceleration": -1.5,
+"curvature": -0.038204215599846145,
+"pose": {
+"rotation": {
+"radians": 0.015665981844734733
+},
+"translation": {
+"x": 6.640443838704797,
+"y": 7.559266344512594
+}
+},
+"time": 3.793150570509139,
+"velocity": 2.457212963109351
+},
+{
+"acceleration": -1.4999999999999942,
+"curvature": -0.0459330952918765,
+"pose": {
+"rotation": {
+"radians": 0.012181911167278795
+},
+"translation": {
+"x": 6.723751871924467,
+"y": 7.560430854053105
+}
+},
+"time": 3.8274157107010116,
+"velocity": 2.405815252821542
+},
+{
+"acceleration": -1.4999999999999938,
+"curvature": -0.05669578882530817,
+"pose": {
+"rotation": {
+"radians": 0.008194354781752154
+},
+"translation": {
+"x": 6.801998888253569,
+"y": 7.561233547148522
+}
+},
+"time": 3.8602782054382008,
+"velocity": 2.3565215107157584
+},
+{
+"acceleration": -1.4999999999999933,
+"curvature": -0.07161583930936123,
+"pose": {
+"rotation": {
+"radians": 0.0035709997611009245
+},
+"translation": {
+"x": 6.874630268677097,
+"y": 7.561667356542926
+}
+},
+"time": 3.8914086249402704,
+"velocity": 2.309825881462654
+},
+{
+"acceleration": -1.5000000000000073,
+"curvature": -0.09212425500968142,
+"pose": {
+"rotation": {
+"radians": -0.0018382838496620072
+},
+"translation": {
+"x": 6.941261638799366,
+"y": 7.561732646730976
+}
+},
+"time": 3.920530943081456,
+"velocity": 2.2661424042508758
+},
+{
+"acceleration": -1.5,
+"curvature": -0.11970933984691887,
+"pose": {
+"rotation": {
+"radians": -0.008187194954826128
+},
+"translation": {
+"x": 7.00170369388571,
+"y": 7.561438041459301
+}
+},
+"time": 3.947442736630175,
+"velocity": 2.225774713927797
+},
+{
+"acceleration": -1.5,
+"curvature": -0.1949962784325499,
+"pose": {
+"rotation": {
+"radians": -0.024039021827935905
+},
+"translation": {
+"x": 7.1043869385672505,
+"y": 7.559849900791484
+}
+},
+"time": 3.9943225081734166,
+"velocity": 2.1554550566129347
+},
+{
+"acceleration": -1.5,
+"curvature": -0.2278367067131923,
+"pose": {
+"rotation": {
+"radians": -0.042167534114385015
+},
+"translation": {
+"x": 7.1860103096494266,
+"y": 7.557168554604721
+}
+},
+"time": 4.0327243416888345,
+"velocity": 2.097852306339808
+},
+{
+"acceleration": -1.4999999999999933,
+"curvature": -1.712966741004169e-15,
+"pose": {
+"rotation": {
+"radians": -0.05203631480921561
+},
+"translation": {
+"x": 7.254614031305161,
+"y": 7.553844731084733
+}
+},
+"time": 4.065857048612951,
+"velocity": 2.048153245953633
+},
+{
+"acceleration": -1.5,
+"curvature": -0.6007555731604474,
+"pose": {
+"rotation": {
+"radians": -0.07332023285912433
+},
+"translation": {
+"x": 7.320891407497461,
+"y": 7.549906539008896
+}
+},
+"time": 4.098667919362387,
+"velocity": 1.9989369398294803
+},
+{
+"acceleration": -1.499999999999997,
+"curvature": -0.9592315314498099,
+"pose": {
+"rotation": {
+"radians": -0.1273127054692113
+},
+"translation": {
+"x": 7.388010575897486,
+"y": 7.543287011254251
+}
+},
+"time": 4.132846552965905,
+"velocity": 1.9476689894242032
+},
+{
+"acceleration": 0.04770535430879272,
+"curvature": -1.1044145640580711,
+"pose": {
+"rotation": {
+"radians": -0.19993727564824346
+},
+"translation": {
+"x": 7.456404359137068,
+"y": 7.532053482859951
+}
+},
+"time": 4.168934270070408,
+"velocity": 1.893537413767449
+},
+{
+"acceleration": 0.9423844319477864,
+"curvature": -1.1000730350034786,
+"pose": {
+"rotation": {
+"radians": -0.2797541520124656
+},
+"translation": {
+"x": 7.526124771589297,
+"y": 7.515000607420257
+}
+},
+"time": 4.206821748777978,
+"velocity": 1.8953448493630605
+},
+{
+"acceleration": 1.317109698643756,
+"curvature": -1.0136232575288757,
+"pose": {
+"rotation": {
+"radians": -0.3588919579127874
+},
+"translation": {
+"x": 7.59691489377497,
+"y": 7.491552002633608
+}
+},
+"time": 4.245789403560134,
+"velocity": 1.93206736057928
+},
+{
+"acceleration": 1.385155394578869,
+"curvature": -0.8965845406655487,
+"pose": {
+"rotation": {
+"radians": -0.43285657127434
+},
+"translation": {
+"x": 7.668280746769049,
+"y": 7.4616618958516865
+}
+},
+"time": 4.285303680603943,
+"velocity": 1.984111998108577
+},
+{
+"acceleration": 1.2938490541372072,
+"curvature": -0.7794518372577566,
+"pose": {
+"rotation": {
+"radians": -0.49968734179154517
+},
+"translation": {
+"x": 7.739563166607099,
+"y": 7.4257167696284805
+}
+},
+"time": 4.324989819230848,
+"velocity": 2.0390834671176394
+},
+{
+"acceleration": 1.144231805521859,
+"curvature": -0.6759617610142584,
+"pose": {
+"rotation": {
+"radians": -0.5589873849548617
+},
+"translation": {
+"x": 7.810009678691755,
+"y": 7.384437007269355
+}
+},
+"time": 4.364536170338996,
+"velocity": 2.0902504760934955
+},
+{
+"acceleration": 1.0032678565007147,
+"curvature": -0.589537796346221,
+"pose": {
+"rotation": {
+"radians": -0.6111415258116542
+},
+"translation": {
+"x": 7.878846372199163,
+"y": 7.338778538380115
+}
+},
+"time": 4.403635762588991,
+"velocity": 2.1349894731288748
+},
+{
+"acceleration": 0.920447609390403,
+"curvature": -0.5180946608401927,
+"pose": {
+"rotation": {
+"radians": -0.656790152386187
+},
+"translation": {
+"x": 7.945349774485435,
+"y": 7.289834484416074
+}
+},
+"time": 4.441966402057183,
+"velocity": 2.17344537162643
+},
+{
+"acceleration": -1.4339322222210362,
+"curvature": -0.45652515569027846,
+"pose": {
+"rotation": {
+"radians": -0.696500330973338
+},
+"translation": {
+"x": 8.008918725493103,
+"y": 7.238736804231118
+}
+},
+"time": 4.479198405836864,
+"velocity": 2.207715480498252
+},
+{
+"acceleration": -1.5,
+"curvature": -0.3974641890817925,
+"pose": {
+"rotation": {
+"radians": -0.7305513602114357
+},
+"translation": {
+"x": 8.069146252157566,
+"y": 7.186557939626774
+}
+},
+"time": 4.515726420009646,
+"velocity": 2.155336783962154
+},
+{
+"acceleration": -1.5,
+"curvature": -0.3312840891920024,
+"pose": {
+"rotation": {
+"radians": -0.7587783789615292
+},
+"translation": {
+"x": 8.125891442813542,
+"y": 7.134212460901276
+}
+},
+"time": 4.552003098956486,
+"velocity": 2.1009217655418944
+},
+{
+"acceleration": -1.500000000000003,
+"curvature": -0.24680563067643704,
+"pose": {
+"rotation": {
+"radians": -0.780472399075354
+},
+"translation": {
+"x": 8.179351321601525,
+"y": 7.08235871239863
+}
+},
+"time": 4.5879129452041925,
+"velocity": 2.0470569961703347
+},
+{
+"acceleration": -1.5,
+"curvature": -0.1352691237332068,
+"pose": {
+"rotation": {
+"radians": -0.7944222242855352
+},
+"translation": {
+"x": 8.23013272287423,
+"y": 7.0313004580576814
+}
+},
+"time": 4.623556618483837,
+"velocity": 1.9935914862508675
+},
+{
+"acceleration": -1.499999999999997,
+"curvature": -1.0147854878849304e-13,
+"pose": {
+"rotation": {
+"radians": -0.7992861593284419
+},
+"translation": {
+"x": 8.279324165603047,
+"y": 6.980888526961181
+}
+},
+"time": 4.659370105457375,
+"velocity": 1.939871255790561
+},
+{
+"acceleration": -1.500000000000003,
+"curvature": -0.724412937265256,
+"pose": {
+"rotation": {
+"radians": -0.8263587556313332
+},
+"translation": {
+"x": 8.327874633636096,
+"y": 6.930033769660485
+}
+},
+"time": 4.696136895484282,
+"velocity": 1.884721070750201
+},
+{
+"acceleration": -1.5,
+"curvature": -1.185296894311173,
+"pose": {
+"rotation": {
+"radians": -0.8958734205013904
+},
+"translation": {
+"x": 8.374414930038409,
+"y": 6.876150619970998
+}
+},
+"time": 4.734499843710719,
+"velocity": 1.8271766484105458
+},
+{
+"acceleration": -1.499999999999994,
+"curvature": -1.3308075240787625,
+"pose": {
+"rotation": {
+"radians": -0.9414626195462539
+},
+"translation": {
+"x": 8.396343609825403,
+"y": 6.847457953672803
+}
+},
+"time": 4.754427116047595,
+"velocity": 1.7972857399052304
+},
+{
+"acceleration": -1.119669222798429,
+"curvature": -1.4316641820537412,
+"pose": {
+"rotation": {
+"radians": -0.9920141334468949
+},
+"translation": {
+"x": 8.417087038355495,
+"y": 6.817419520550447
+}
+},
+"time": 4.774913296392159,
+"velocity": 1.766556469388386
+},
+{
+"acceleration": -0.658756616637048,
+"curvature": -1.497611703546305,
+"pose": {
+"rotation": {
+"radians": -1.0461840810687102
+},
+"translation": {
+"x": 8.436442300466393,
+"y": 6.785984448970856
+}
+},
+"time": 4.795950694362622,
+"velocity": 1.7430015423530956
+},
+{
+"acceleration": -0.3758045489295412,
+"curvature": -1.5381371113871662,
+"pose": {
+"rotation": {
+"radians": -1.1029181094006104
+},
+"translation": {
+"x": 8.454219563398734,
+"y": 6.75316081692379
+}
+},
+"time": 4.817454331921207,
+"velocity": 1.7288358788296128
+},
+{
+"acceleration": -0.22766804410954308,
+"curvature": -1.5619579527215153,
+"pose": {
+"rotation": {
+"radians": -1.1614401341915868
+},
+"translation": {
+"x": 8.470242786082986,
+"y": 6.71900950486866
+}
+},
+"time": 4.839326449750711,
+"velocity": 1.720616237454562
+},
+{
+"acceleration": -0.18291444679617636,
+"curvature": -1.5766951092544812,
+"pose": {
+"rotation": {
+"radians": -1.2212290002809874
+},
+"translation": {
+"x": 8.48435042842636,
+"y": 6.683638048581349
+}
+},
+"time": 4.8614911610922915,
+"velocity": 1.7155700409751717
+},
+{
+"acceleration": -0.21667780534208844,
+"curvature": -1.5887248890525703,
+"pose": {
+"rotation": {
+"radians": -1.2819870573855083
+},
+"translation": {
+"x": 8.496396160599712,
+"y": 6.64719449200102
+}
+},
+"time": 4.88389106254986,
+"velocity": 1.7114727753917718
+},
+{
+"acceleration": -0.30786884661445757,
+"curvature": -1.6031736514122077,
+"pose": {
+"rotation": {
+"radians": -1.3436044959303923
+},
+"translation": {
+"x": 8.506249572324453,
+"y": 6.609861240076943
+}
+},
+"time": 4.906483869868072,
+"velocity": 1.7065774154855449
+},
+{
+"acceleration": -0.437520272939913,
+"curvature": -1.6239959645508766,
+"pose": {
+"rotation": {
+"radians": -1.4061224126472716
+},
+"translation": {
+"x": 8.513796882159445,
+"y": 6.571848911615302
+}
+},
+"time": 4.929239386658464,
+"velocity": 1.6995717007771711
+},
+{
+"acceleration": -0.587643942553939,
+"curvature": -1.654071786587593,
+"pose": {
+"rotation": {
+"radians": -1.469696239263753
+},
+"translation": {
+"x": 8.518941646787919,
+"y": 6.53339019212602
+}
+},
+"time": 4.9521369237001,
+"velocity": 1.689553564121063
+},
+{
+"acceleration": -0.740352976351179,
+"curvature": -1.69526506886456,
+"pose": {
+"rotation": {
+"radians": -1.5345597341465933
+},
+"translation": {
+"x": 8.521605470304374,
+"y": 6.494733686669568
+}
+},
+"time": 4.975163103881359,
+"velocity": 1.6760223688173905
+},
+{
+"acceleration": -0.8772439560320553,
+"curvature": -1.7483992838832845,
+"pose": {
+"rotation": {
+"radians": -1.6009884916718644
+},
+"translation": {
+"x": 8.521728713501481,
+"y": 6.45613777270379
+}
+},
+"time": 4.998309835904527,
+"velocity": 1.6588856168712351
+},
+{
+"acceleration": -0.9791692707913844,
+"curvature": -1.8131221430488826,
+"pose": {
+"rotation": {
+"radians": -1.6692610592489874
+},
+"translation": {
+"x": 8.519271203156997,
+"y": 6.417864452930711
+}
+},
+"time": 5.021572132856613,
+"velocity": 1.6384789074665949
+},
+{
+"acceleration": -1.0265522151192739,
+"curvature": -1.8876547968615747,
+"pose": {
+"rotation": {
+"radians": -1.7396154358140525
+},
+"translation": {
+"x": 8.514212941320658,
+"y": 6.380173208143359
+}
+},
+"time": 5.044945403462452,
+"velocity": 1.6155925191314657
+},
+{
+"acceleration": -1.0002891213917853,
+"curvature": -1.968456295022359,
+"pose": {
+"rotation": {
+"radians": -1.8121991778429465
+},
+"translation": {
+"x": 8.506554814601097,
+"y": 6.343314850072581
+}
+},
+"time": 5.068421875913611,
+"velocity": 1.5914926943335426
+},
+{
+"acceleration": -0.8830444977198459,
+"curvature": -2.049887156026798,
+"pose": {
+"rotation": {
+"radians": -1.8870128389921985
+},
+"translation": {
+"x": 8.496319303452745,
+"y": 6.307525374233858
+}
+},
+"time": 5.091985957522358,
+"velocity": 1.5679217998447248
+},
+{
+"acceleration": -0.6604368714069757,
+"curvature": -2.124021809224906,
+"pose": {
+"rotation": {
+"radians": -1.9638492298006822
+},
+"translation": {
+"x": 8.483551191462738,
+"y": 6.273019812774124
+}
+},
+"time": 5.1156086154772105,
+"velocity": 1.5470619417161744
+},
+{
+"acceleration": -0.3213588423411472,
+"curvature": -2.180812121634148,
+"pose": {
+"rotation": {
+"radians": -2.0422348375306005
+},
+"translation": {
+"x": 8.468318274637817,
+"y": 6.23998608731858
+}
+},
+"time": 5.1392412757567625,
+"velocity": 1.5314540614981231
+},
+{
+"acceleration": 0.14341613081305415,
+"curvature": -2.2087901452622987,
+"pose": {
+"rotation": {
+"radians": -2.1213837124973205
+},
+"translation": {
+"x": 8.45071207069124,
+"y": 6.208578861817512
+}
+},
+"time": 5.162810186199229,
+"velocity": 1.52387998372309
+},
+{
+"acceleration": 0.745172018742894,
+"curvature": -2.1963581378320924,
+"pose": {
+"rotation": {
+"radians": -2.2001761734175407
+},
+"translation": {
+"x": 8.430848528329689,
+"y": 6.178913395393108
+}
+},
+"time": 5.186212469725178,
+"velocity": 1.5272362486785718
+},
+{
+"acceleration": 1.4999999999999873,
+"curvature": -2.1334399317019757,
+"pose": {
+"rotation": {
+"radians": -2.2771722495844378
+},
+"translation": {
+"x": 8.408868736540168,
+"y": 6.151059395186275
+}
+},
+"time": 5.209314933448958,
+"velocity": 1.5444515582095553
+},
+{
+"acceleration": 1.5000000000000189,
+"curvature": -2.0129672930412794,
+"pose": {
+"rotation": {
+"radians": -2.350661404065905
+},
+"translation": {
+"x": 8.384939633876916,
+"y": 6.125034869203453
+}
+},
+"time": 5.2319567085751775,
+"velocity": 1.578414220898884
+},
+{
+"acceleration": 1.4999999999999873,
+"curvature": -1.83155411269839,
+"pose": {
+"rotation": {
+"radians": -2.4187374241166113
+},
+"translation": {
+"x": 8.359254717748312,
+"y": 6.100799979163433
+}
+},
+"time": 5.254096582060451,
+"velocity": 1.6116240311267946
+},
+{
+"acceleration": 1.4999999999999811,
+"curvature": -1.5889259271769012,
+"pose": {
+"rotation": {
+"radians": -2.4793756281638286
+},
+"translation": {
+"x": 8.332034753703777,
+"y": 6.078250893344181
+}
+},
+"time": 5.275809518564643,
+"velocity": 1.6441934358830834
+},
+{
+"acceleration": 1.5,
+"curvature": -1.2861579295017167,
+"pose": {
+"rotation": {
+"radians": -2.530484937638303
+},
+"translation": {
+"x": 8.303528484720683,
+"y": 6.057213639429635
+}
+},
+"time": 5.297149394564854,
+"velocity": 1.6762032498833987
+},
+{
+"acceleration": 1.4999999999999876,
+"curvature": -0.923269689654022,
+"pose": {
+"rotation": {
+"radians": -2.569913145746541
+},
+"translation": {
+"x": 8.274013340491255,
+"y": 6.037437957356548
+}
+},
+"time": 5.318147483376322,
+"velocity": 1.7077003831006008
+},
+{
+"acceleration": 1.500000000000003,
+"curvature": -1.4059365344546992e-14,
+"pose": {
+"rotation": {
+"radians": -2.6044843994954294
+},
+"translation": {
+"x": 8.213213834358022,
+"y": 6.000251946826639
+}
+},
+"time": 5.35914369563977,
+"velocity": 1.769194701495773
+},
+{
+"acceleration": 1.4999999999999944,
+"curvature": 0.2601196647268544,
+"pose": {
+"rotation": {
+"radians": -2.5940732535618807
+},
+"translation": {
+"x": 8.150901645175804,
+"y": 5.962839038042556
+}
+},
+"time": 5.399533564160784,
+"velocity": 1.829779504277293
+},
+{
+"acceleration": 1.5000000000000049,
+"curvature": 0.35510048882303075,
+"pose": {
+"rotation": {
+"radians": -2.568478872098707
+},
+"translation": {
+"x": 8.083348971126926,
+"y": 5.920514900627465
+}
+},
+"time": 5.4423482985142035,
+"velocity": 1.8940016058074223
+},
+{
+"acceleration": 1.5000000000000107,
+"curvature": 0.3517110258787969,
+"pose": {
+"rotation": {
+"radians": -2.5359002554326917
+},
+"translation": {
+"x": 8.00760000876994,
+"y": 5.86985799759564
+}
+},
+"time": 5.489578163288547,
+"velocity": 1.964846402968938
+},
+{
+"acceleration": 1.5000000000000113,
+"curvature": 0.3278462476402164,
+"pose": {
+"rotation": {
+"radians": -2.500463330566685
+},
+"translation": {
+"x": 7.922840559784747,
+"y": 5.808882766944999
+}
+},
+"time": 5.541682579508508,
+"velocity": 2.0430030272988797
+},
+{
+"acceleration": 1.500000000000014,
+"curvature": 0.31460545835499076,
+"pose": {
+"rotation": {
+"radians": -2.46289221328339
+},
+"translation": {
+"x": 7.830012178895862,
+"y": 5.736831564164724
+}
+},
+"time": 5.598034832848613,
+"velocity": 2.1275314073090383
+},
+{
+"acceleration": 1.4999999999999798,
+"curvature": 0.3148821497727589,
+"pose": {
+"rotation": {
+"radians": -2.4430414971640637
+},
+"translation": {
+"x": 7.781251507777571,
+"y": 5.696700266356856
+}
+},
+"time": 5.627413605746683,
+"velocity": 2.171599566656143
+},
+{
+"acceleration": 1.5000000000000067,
+"curvature": 0.3202949690122064,
+"pose": {
+"rotation": {
+"radians": -2.4222222217626386
+},
+"translation": {
+"x": 7.7314263217956745,
+"y": 5.653966604742917
+}
+},
+"time": 5.657331412614989,
+"velocity": 2.216476276958603
+},
+{
+"acceleration": 0.2964652380955796,
+"curvature": 0.33115377756856085,
+"pose": {
+"rotation": {
+"radians": -2.40019313727038
+},
+"translation": {
+"x": 7.680976296081608,
+"y": 5.608788574951411
+}
+},
+"time": 5.687575763820927,
+"velocity": 2.2618428037675096
+},
+{
+"acceleration": -0.4409588389772437,
+"curvature": 0.3478554754561956,
+"pose": {
+"rotation": {
+"radians": -2.3766811760018074
+},
+"translation": {
+"x": 7.630378493067713,
+"y": 5.561361906674225
+}
+},
+"time": 5.718175253097962,
+"velocity": 2.270914488641629
+},
+{
+"acceleration": -0.5597381543273501,
+"curvature": 0.3709465093281269,
+"pose": {
+"rotation": {
+"radians": -2.3513823440062565
+},
+"translation": {
+"x": 7.580135304609831,
+"y": 5.511913561870003
+}
+},
+"time": 5.749311851234043,
+"velocity": 2.2571845304778413
+},
+{
+"acceleration": -0.68822613945507,
+"curvature": 0.4011510538347123,
+"pose": {
+"rotation": {
+"radians": -2.3239613701087984
+},
+"translation": {
+"x": 7.530762394109905,
+"y": 5.460695232967495
+}
+},
+"time": 5.780953446124559,
+"velocity": 2.2394735225538502
+},
+{
+"acceleration": -0.8275746321191164,
+"curvature": 0.4393698851508497,
+"pose": {
+"rotation": {
+"radians": -2.294051977011219
+},
+"translation": {
+"x": 7.482776638638585,
+"y": 5.407976841068937
+}
+},
+"time": 5.8129428014103715,
+"velocity": 2.2174576120618386
+},
+{
+"acceleration": -0.9764908758410039,
+"curvature": 0.4866406203342659,
+"pose": {
+"rotation": {
+"radians": -2.2612599270017486
+},
+"translation": {
+"x": 7.436684071057829,
+"y": 5.354040034153403
+}
+},
+"time": 5.845131616860452,
+"velocity": 2.190818964957388
+},
+{
+"acceleration": -1.1297392764044851,
+"curvature": 0.5440347859067597,
+"pose": {
+"rotation": {
+"radians": -2.2251717165884983
+},
+"translation": {
+"x": 7.392967822143502,
+"y": 5.299171685280173
+}
+},
+"time": 5.877385478698907,
+"velocity": 2.159323363161501
+},
+{
+"acceleration": -1.2762126580388409,
+"curvature": 0.6124493843587427,
+"pose": {
+"rotation": {
+"radians": -2.1853729495660073
+},
+"translation": {
+"x": 7.352076062707984,
+"y": 5.243657390792098
+}
+},
+"time": 5.909587611206428,
+"velocity": 2.122943349283773
+},
+{
+"acceleration": -1.3968565664779362,
+"curvature": 0.6922319222981539,
+"pose": {
+"rotation": {
+"radians": -2.141481905593326
+},
+"translation": {
+"x": 7.314409945722764,
+"y": 5.187774968518963
+}
+},
+"time": 5.941640678792482,
+"velocity": 2.082036818701476
+},
+{
+"acceleration": -1.4630911965241482,
+"curvature": 0.7825674201740078,
+"pose": {
+"rotation": {
+"radians": -2.0932052610200005
+},
+"translation": {
+"x": 7.2803115484410466,
+"y": 5.131787955980847
+}
+},
+"time": 5.973465655571976,
+"velocity": 2.037581890909032
+},
+{
+"acceleration": -1.436604671765453,
+"curvature": 0.8805765518317656,
+"pose": {
+"rotation": {
+"radians": -2.040423369913376
+},
+"translation": {
+"x": 7.250051814520355,
+"y": 5.075939108591491
+}
+},
+"time": 6.0049966332472655,
+"velocity": 1.9914491950545172
+},
+{
+"acceleration": -1.2709939387237286,
+"curvature": 0.9801653897701343,
+"pose": {
+"rotation": {
+"radians": -1.9833101979104337
+},
+"translation": {
+"x": 7.22381849614513,
+"y": 5.020443897861665
+}
+},
+"time": 6.036170576040125,
+"velocity": 1.9466645632009465
+},
+{
+"acceleration": -0.914162248549567,
+"curvature": 1.0708699329341247,
+"pose": {
+"rotation": {
+"radians": -1.9224854393410684
+},
+"translation": {
+"x": 7.2017040961493315,
+"y": 4.9654840096025215
+}
+},
+"time": 6.066911741708284,
+"velocity": 1.9075927279674143
+},
+{
+"acceleration": -0.307625514226768,
+"curvature": 1.1372344916790957,
+"pose": {
+"rotation": {
+"radians": -1.859181734337182
+},
+"translation": {
+"x": 7.183693810139044,
+"y": 4.911200842128967
+}
+},
+"time": 6.097112017452818,
+"velocity": 1.8799847759859742
+},
+{
+"acceleration": 0.6243387672143992,
+"curvature": 1.1594757243516123,
+"pose": {
+"rotation": {
+"radians": -1.7953904155864346
+},
+"translation": {
+"x": 7.169653468615073,
+"y": 4.857689004463025
+}
+},
+"time": 6.126610642645572,
+"velocity": 1.8709102462420706
+},
+{
+"acceleration": 1.500000000000021,
+"curvature": 1.1159628333465057,
+"pose": {
+"rotation": {
+"radians": -1.7339347493624455
+},
+"translation": {
+"x": 7.159317479095556,
+"y": 4.804989814537199
+}
+},
+"time": 6.155178800783818,
+"velocity": 1.8887464548756894
+},
+{
+"acceleration": 1.4999999999999785,
+"curvature": 0.9871644456223426,
+"pose": {
+"rotation": {
+"radians": -1.6784260387831644
+},
+"translation": {
+"x": 7.1522767682385515,
+"y": 4.753084797397834
+}
+},
+"time": 6.182612813402345,
+"velocity": 1.9298974738034809
+},
+{
+"acceleration": 1.4999999999999956,
+"curvature": 0.7596483126758463,
+"pose": {
+"rotation": {
+"radians": -1.6330932053903273
+},
+"translation": {
+"x": 7.147966723964652,
+"y": 4.701889183408484
+}
+},
+"time": 6.208964428006902,
+"velocity": 1.9694248957103153
+},
+{
+"acceleration": 1.5000000000000089,
+"curvature": 5.5498842491478397e-14,
+"pose": {
+"rotation": {
+"radians": -1.5913413808282677
+},
+"translation": {
+"x": 7.1444301458967985,
+"y": 4.600916602140259
+}
+},
+"time": 6.259301034688459,
+"velocity": 2.0449298057326506
+},
+{
+"acceleration": 0.011261922893453352,
+"curvature": 0.3149119692449802,
+"pose": {
+"rotation": {
+"radians": -1.5748868996847962
+},
+"translation": {
+"x": 7.1429319988235385,
+"y": 4.500660062196745
+}
+},
+"time": 6.307481993731279,
+"velocity": 2.117201244296881
+},
+{
+"acceleration": -1.5000000000000047,
+"curvature": 0.5639663370674379,
+"pose": {
+"rotation": {
+"radians": -1.5311076331963522
+},
+"translation": {
+"x": 7.144486708736369,
+"y": 4.401967074837268
+}
+},
+"time": 6.3540968333800265,
+"velocity": 2.117726217026696
+},
+{
+"acceleration": -1.3741039845010348,
+"curvature": 0.7601180017580457,
+"pose": {
+"rotation": {
+"radians": -1.467013611506824
+},
+"translation": {
+"x": 7.151230934935861,
+"y": 4.305999822050589
+}
+},
+"time": 6.400280147214505,
+"velocity": 2.0484512462749787
+},
+{
+"acceleration": -0.5780289301529772,
+"curvature": 0.8950692274808717,
+"pose": {
+"rotation": {
+"radians": -1.3891858783121052
+},
+"translation": {
+"x": 7.16441731993931,
+"y": 4.213527223782977
+}
+},
+"time": 6.446599078471235,
+"velocity": 1.984804218277277
+},
+{
+"acceleration": 0.23758992378895957,
+"curvature": 0.9541254378624974,
+"pose": {
+"rotation": {
+"radians": -1.3045888617176409
+},
+"translation": {
+"x": 7.18454150824168,
+"y": 4.124973994405794
+}
+},
+"time": 6.49266121157466,
+"velocity": 1.9581789727589398
+},
+{
+"acceleration": 1.0453289172073046,
+"curvature": 0.9301299345899846,
+"pose": {
+"rotation": {
+"radians": -1.2203958632136618
+},
+"translation": {
+"x": 7.211469165076581,
+"y": 4.040469699183101
+}
+},
+"time": 6.537829976699569,
+"velocity": 1.9689106162226082
+},
+{
+"acceleration": 1.500000000000005,
+"curvature": 0.830778976101694,
+"pose": {
+"rotation": {
+"radians": -1.1431884914207087
+},
+"translation": {
+"x": 7.24456299517723,
+"y": 3.959897810739241
+}
+},
+"time": 6.5815617658188765,
+"velocity": 2.0146247199902323
+},
+{
+"acceleration": 1.5000000000000053,
+"curvature": 0.675763660526861,
+"pose": {
+"rotation": {
+"radians": -1.078117863713012
+},
+"translation": {
+"x": 7.282809761537419,
+"y": 3.8829447655264415
+}
+},
+"time": 6.62356002221341,
+"velocity": 2.0776221045820322
+},
+{
+"acceleration": 1.5000000000000107,
+"curvature": 0.4883497260186772,
+"pose": {
+"rotation": {
+"radians": -1.0284847575953808
+},
+"translation": {
+"x": 7.324947304172484,
+"y": 3.809149020292402
+}
+},
+"time": 6.663875218747651,
+"velocity": 2.1380948993833937
+},
+{
+"acceleration": 1.5,
+"curvature": 0.2891173638020842,
+"pose": {
+"rotation": {
+"radians": -0.9957787961317092
+},
+"translation": {
+"x": 7.3695915588802725,
+"y": 3.7379501085478886
+}
+},
+"time": 6.702652859036672,
+"velocity": 2.1962613598169245
+},
+{
+"acceleration": 1.2507594513748674,
+"curvature": 0.09532517572204599,
+"pose": {
+"rotation": {
+"radians": -0.9799047272928517
+},
+"translation": {
+"x": 7.415363576002107,
+"y": 3.66873769703433
+}
+},
+"time": 6.739959300630276,
+"velocity": 2.2522210222073307
+},
+{
+"acceleration": -1.5,
+"curvature": -0.07549988876316552,
+"pose": {
+"rotation": {
+"radians": -0.9793003067693973
+},
+"translation": {
+"x": 7.461016539183757,
+"y": 3.6009006421914105
+}
+},
+"time": 6.775906139125658,
+"velocity": 2.2971818702024756
+},
+{
+"acceleration": -1.5000000000000056,
+"curvature": -0.20099306282441212,
+"pose": {
+"rotation": {
+"radians": -0.990806759106492
+},
+"translation": {
+"x": 7.505562784136404,
+"y": 3.533876046624661
+}
+},
+"time": 6.81134952391025,
+"velocity": 2.2440167930255863
+},
+{
+"acceleration": -1.5,
+"curvature": -0.25179068177905317,
+"pose": {
+"rotation": {
+"radians": -1.0093470216866678
+},
+"translation": {
+"x": 7.548400817397607,
+"y": 3.4671983155730546
+}
+},
+"time": 6.847093960269795,
+"velocity": 2.190400138486268
+},
+{
+"acceleration": -1.4999999999999942,
+"curvature": -0.19447877094669144,
+"pose": {
+"rotation": {
+"radians": -1.0276280326574119
+},
+"translation": {
+"x": 7.589442335092275,
+"y": 3.4005482133766005
+}
+},
+"time": 6.883276740090296,
+"velocity": 2.136125968755517
+},
+{
+"acceleration": -1.4999999999999944,
+"curvature": -1.0654079672290709e-13,
+"pose": {
+"rotation": {
+"radians": -1.0361464155210947
+},
+"translation": {
+"x": 7.629239241693627,
+"y": 3.3338019199439373
+}
+},
+"time": 6.920132656738415,
+"velocity": 2.0808420937833385
+},
+{
+"acceleration": 0.9117122637173966,
+"curvature": 0.8147293708896286,
+"pose": {
+"rotation": {
+"radians": -0.9956295939059031
+},
+"translation": {
+"x": 7.671115031437971,
+"y": 3.265413380387497
+}
+},
+"time": 6.959221056576481,
+"velocity": 2.0222094940262405
+},
+{
+"acceleration": 1.4999999999999925,
+"curvature": 0.7281570826420326,
+"pose": {
+"rotation": {
+"radians": -0.9192623616369794
+},
+"translation": {
+"x": 7.7252518243051655,
+"y": 3.1886426682376925
+}
+},
+"time": 7.0051981732490605,
+"velocity": 2.064127395146997
+},
+{
+"acceleration": 1.5,
+"curvature": 0.4552251040915523,
+"pose": {
+"rotation": {
+"radians": -0.8511471980354113
+},
+"translation": {
+"x": 7.79946506401762,
+"y": 3.0984070076443624
+}
+},
+"time": 7.060681546493302,
+"velocity": 2.147352455013359
+},
+{
+"acceleration": 1.4999999999999825,
+"curvature": 0.3503416291421221,
+"pose": {
+"rotation": {
+"radians": -0.8237577835878177
+},
+"translation": {
+"x": 7.845369387356048,
+"y": 3.0475176189845605
+}
+},
+"time": 7.092249170707777,
+"velocity": 2.1947038913350707
+},
+{
+"acceleration": 1.5,
+"curvature": 0.27104109219709904,
+"pose": {
+"rotation": {
+"radians": -0.8004891454796156
+},
+"translation": {
+"x": 7.897348370790036,
+"y": 2.9927379605580775
+}
+},
+"time": 7.126261994757979,
+"velocity": 2.2457231274103733
+},
+{
+"acceleration": 1.500000000000005,
+"curvature": 0.21262157548627122,
+"pose": {
+"rotation": {
+"radians": -0.7807292519436259
+},
+"translation": {
+"x": 7.955304869219153,
+"y": 2.9342210648062634
+}
+},
+"time": 7.162497677860122,
+"velocity": 2.3000766520635887
+},
+{
+"acceleration": 1.1517561322723722,
+"curvature": 0.16999218043929668,
+"pose": {
+"rotation": {
+"radians": -0.7638546641680701
+},
+"translation": {
+"x": 8.018964206751498,
+"y": 2.8722479222191346
+}
+},
+"time": 7.200649412285975,
+"velocity": 2.3573042537023685
+},
+{
+"acceleration": 0.3671934626892712,
+"curvature": 0.13895198985170198,
+"pose": {
+"rotation": {
+"radians": -0.7493073217853874
+},
+"translation": {
+"x": 8.087895759998146,
+"y": 2.807210809319411
+}
+},
+"time": 7.240464911352243,
+"velocity": 2.4031619989114263
+},
+{
+"acceleration": 0.25921611800360767,
+"curvature": 0.11636552693677757,
+"pose": {
+"rotation": {
+"radians": -0.736613898728134
+},
+"translation": {
+"x": 8.161534541367573,
+"y": 2.7395966166465424
+}
+},
+"time": 7.281933617637644,
+"velocity": 2.418389036765607
+},
+{
+"acceleration": 0.1815745223334493,
+"curvature": 0.09999943688385911,
+"pose": {
+"rotation": {
+"radians": -0.7253812044910692
+},
+"translation": {
+"x": 8.239202782360111,
+"y": 2.6699701767407396
+}
+},
+"time": 7.3249656434310735,
+"velocity": 2.429543631441611
+},
+{
+"acceleration": 0.12367605575566869,
+"curvature": 0.08830563900128274,
+"pose": {
+"rotation": {
+"radians": -0.715283312358745
+},
+"translation": {
+"x": 8.320131516862384,
+"y": 2.5989575921270074
+}
+},
+"time": 7.369208351777047,
+"velocity": 2.437576980076269
+},
+{
+"acceleration": 0.07803404733756221,
+"curvature": 0.08023852755369064,
+"pose": {
+"rotation": {
+"radians": -0.7060471048562406
+},
+"translation": {
+"x": 8.403482164441742,
+"y": 2.5272295632991737
+}
+},
+"time": 7.41426914965572,
+"velocity": 2.4431499218271067
+},
+{
+"acceleration": 0.03910737938684107,
+"curvature": 0.07512270124018586,
+"pose": {
+"rotation": {
+"radians": -0.6974387143432585
+},
+"translation": {
+"x": 8.488368113640709,
+"y": 2.455484716703922
+}
+},
+"time": 7.459728162812225,
+"velocity": 2.4466972726116802
+},
+{
+"acceleration": 0.0023730316204066807,
+"curvature": 0.07256649112463516,
+"pose": {
+"rotation": {
+"radians": -0.6892514782433852
+},
+"translation": {
+"x": 8.573876305271417,
+"y": 2.3844329327248195
+}
+},
+"time": 7.505150710819629,
+"velocity": 2.448473629429323
+},
+{
+"acceleration": -0.03645399325570889,
+"curvature": 0.07241311607706202,
+"pose": {
+"rotation": {
+"radians": -0.6812952664896001
+},
+"translation": {
+"x": 8.659088815710058,
+"y": 2.31477867366635
+}
+},
+"time": 7.550099599173881,
+"velocity": 2.4485802945626896
+},
+{
+"acceleration": -0.08217502011999692,
+"curvature": 0.07472340535351765,
+"pose": {
+"rotation": {
+"radians": -0.6733867724634701
+},
+"translation": {
+"x": 8.743104440191305,
+"y": 2.247204311737944
+}
+},
+"time": 7.594147280353715,
+"velocity": 2.4469745806900303
+},
+{
+"acceleration": -0.14095936325343267,
+"curvature": 0.07978733818732373,
+"pose": {
+"rotation": {
+"radians": -0.6653403277080758
+},
+"translation": {
+"x": 8.82506027610277,
+"y": 2.18235345703801
+}
+},
+"time": 7.6368879398606975,
+"velocity": 2.443462366135102
+},
+{
+"acceleration": -0.22138319229212491,
+"curvature": 0.08816437710102733,
+"pose": {
+"rotation": {
+"radians": -0.6569589308411548
+},
+"translation": {
+"x": 8.904153306279435,
+"y": 2.1208142855379677
+}
+},
+"time": 7.677949546152157,
+"velocity": 2.4376743482580947
+},
+{
+"acceleration": -0.33559431011793966,
+"curvature": 0.10075275228468823,
+"pose": {
+"rotation": {
+"radians": -0.648025535761514
+},
+"translation": {
+"x": 8.979661982298094,
+"y": 2.06310286706627
+}
+},
+"time": 7.7170058570264395,
+"velocity": 2.4290279374775925
+},
+{
+"acceleration": -0.5000106897800937,
+"curvature": 0.11888052453856093,
+"pose": {
+"rotation": {
+"radians": -0.6382954580296721
+},
+"translation": {
+"x": 9.050967807771794,
+"y": 2.009646493292446
+}
+},
+"time": 7.753788269294499,
+"velocity": 2.4166839692080195
+},
+{
+"acceleration": -0.7333856633186433,
+"curvature": 0.14438298954070117,
+"pose": {
+"rotation": {
+"radians": -0.6274925626528427
+},
+"translation": {
+"x": 9.117576921644275,
+"y": 1.9607670057111228
+}
+},
+"time": 7.788097166001642,
+"velocity": 2.399529154099887
+},
+{
+"acceleration": -1.090729524491623,
+"curvature": 0.17954826383422182,
+"pose": {
+"rotation": {
+"radians": -0.615315760200758
+},
+"translation": {
+"x": 9.179141681484412,
+"y": 1.9166641236260613
+}
+},
+"time": 7.819811923849626,
+"velocity": 2.3762700053785535
+},
+{
+"acceleration": -1.5000000000000038,
+"curvature": 0.22660250787788266,
+"pose": {
+"rotation": {
+"radians": -0.6014701567765406
+},
+"translation": {
+"x": 9.235482246780649,
+"y": 1.8773987721341818
+}
+},
+"time": 7.848905824353554,
+"velocity": 2.3445364291162973
+},
+{
+"acceleration": -0.6411051131829418,
+"curvature": 0.35169655491130275,
+"pose": {
+"rotation": {
+"radians": -0.5682326172204568
+},
+"translation": {
+"x": 9.332739941059721,
+"y": 1.812830358187659
+}
+},
+"time": 7.8995174384073525,
+"velocity": 2.268619008035599
+},
+{
+"acceleration": -1.5,
+"curvature": 0.39463129427405697,
+"pose": {
+"rotation": {
+"radians": -0.5317161471101414
+},
+"translation": {
+"x": 9.412087483969259,
+"y": 1.7641397439033604
+}
+},
+"time": 7.940794466500363,
+"velocity": 2.242156094268174
+},
+{
+"acceleration": -1.4999999999999942,
+"curvature": 1.1299914089951265e-13,
+"pose": {
+"rotation": {
+"radians": -0.5123894603106853
+},
+"translation": {
+"x": 9.480328516554373,
+"y": 1.7251171929816653
+}
+},
+"time": 7.976275765665215,
+"velocity": 2.1889341455208964
+},
+{
+"acceleration": -0.10881697790876248,
+"curvature": 0.5899225316747702,
+"pose": {
+"rotation": {
+"radians": -0.4848995475102036
+},
+"translation": {
+"x": 9.548720996807617,
+"y": 1.6875303565403295
+}
+},
+"time": 8.012374486589815,
+"velocity": 2.134786064133996
+},
+{
+"acceleration": 1.5,
+"curvature": 0.5986069922379186,
+"pose": {
+"rotation": {
+"radians": -0.4285862075467598
+},
+"translation": {
+"x": 9.629297008574843,
+"y": 1.647938371270362
+}
+},
+"time": 8.05447427499014,
+"velocity": 2.130204892389674
+},
+{
+"acceleration": 1.5000000000000349,
+"curvature": 0.425803549382478,
+"pose": {
+"rotation": {
+"radians": -0.3725892023020384
+},
+"translation": {
+"x": 9.730342977394917,
+"y": 1.6053382109154057
+}
+},
+"time": 8.10505170279558,
+"velocity": 2.2060710340978327
+},
+{
+"acceleration": 1.4999999999999685,
+"curvature": 0.34873977271535517,
+"pose": {
+"rotation": {
+"radians": -0.3479832148734613
+},
+"translation": {
+"x": 9.790093913499858,
+"y": 1.5828562134578563
+}
+},
+"time": 8.133711034818436,
+"velocity": 2.2490600321321175
+},
+{
+"acceleration": 1.2787633879449205,
+"curvature": 0.28687521672882665,
+"pose": {
+"rotation": {
+"radians": -0.3257782637772579
+},
+"translation": {
+"x": 9.85640634652585,
+"y": 1.5596584424030837
+}
+},
+"time": 8.16462884239192,
+"velocity": 2.2954367434923433
+},
+{
+"acceleration": 0.6481775065808993,
+"curvature": 0.23925414417052973,
+"pose": {
+"rotation": {
+"radians": -0.3057006918209042
+},
+"translation": {
+"x": 9.929356964179096,
+"y": 1.535852360628036
+}
+},
+"time": 8.19775332607413,
+"velocity": 2.3377951204697327
+},
+{
+"acceleration": 0.45881097382693853,
+"curvature": 0.2034295805653484,
+"pose": {
+"rotation": {
+"radians": -0.2873928928437178
+},
+"translation": {
+"x": 10.008859223605889,
+"y": 1.5115814312606117
+}
+},
+"time": 8.233136571268739,
+"velocity": 2.360729744114715
+},
+{
+"acceleration": 0.32062515067214997,
+"curvature": 0.17688511415375013,
+"pose": {
+"rotation": {
+"radians": -0.2704915708281581
+},
+"translation": {
+"x": 10.094680965350758,
+"y": 1.4870195615989008
+}
+},
+"time": 8.270812084624605,
+"velocity": 2.3780156830869497
+},
+{
+"acceleration": 0.21995071578114705,
+"curvature": 0.15750820600589424,
+"pose": {
+"rotation": {
+"radians": -0.2546605047206269
+},
+"translation": {
+"x": 10.186462027314644,
+"y": 1.4623655470304167
+}
+},
+"time": 8.31066883558515,
+"velocity": 2.3907947598689763
+},
+{
+"acceleration": 0.1457168209807414,
+"curvature": 0.14366937568024818,
+"pose": {
+"rotation": {
+"radians": -0.23960053538381157
+},
+"translation": {
+"x": 10.283731858713052,
+"y": 1.4378375149513403
+}
+},
+"time": 8.35254689930571,
+"velocity": 2.400005869959842
+},
+{
+"acceleration": 0.0897160462684492,
+"curvature": 0.1341646826526822,
+"pose": {
+"rotation": {
+"radians": -0.22504922436543356
+},
+"translation": {
+"x": 10.38592713403421,
+"y": 1.413667368685755
+}
+},
+"time": 8.396244915329916,
+"velocity": 2.406373405938055
+},
+{
+"acceleration": 0.04610676518266477,
+"curvature": 0.12812672528321647,
+"pose": {
+"rotation": {
+"radians": -0.21077655330365813
+},
+"translation": {
+"x": 10.492409366997233,
+"y": 1.3900952314048869
+}
+},
+"time": 8.441528055458962,
+"velocity": 2.410436030233053
+},
+{
+"acceleration": 0.010818850878262079,
+"curvature": 0.1249411393427993,
+"pose": {
+"rotation": {
+"radians": -0.19657977836368642
+},
+"translation": {
+"x": 10.602482524510277,
+"y": 1.367363890046338
+}
+},
+"time": 8.488136100942642,
+"velocity": 2.412584976441792
+},
+{
+"acceleration": -0.018925055745749687,
+"curvature": 0.12417769634489732,
+"pose": {
+"rotation": {
+"radians": -0.18227893696262426
+},
+"translation": {
+"x": 10.715410640628711,
+"y": 1.3457132392333317
+}
+},
+"time": 8.535791433969734,
+"velocity": 2.413100552383366
+},
+{
+"acceleration": -0.04492860222712176,
+"curvature": 0.12553468700756312,
+"pose": {
+"rotation": {
+"radians": -0.16771377466715912
+},
+"translation": {
+"x": 10.83043543051326,
+"y": 1.325374725193945
+}
+},
+"time": 8.58420684068612,
+"velocity": 2.4121842881123055
+},
+{
+"acceleration": -0.06818560929552317,
+"curvature": 0.1287917486169903,
+"pose": {
+"rotation": {
+"radians": -0.152742602828697
+},
+"translation": {
+"x": 10.946793904388182,
+"y": 1.3065657896803495
+}
+},
+"time": 8.633093054349095,
+"velocity": 2.4099878988642516
+},
+{
+"acceleration": -0.0888971078791867,
+"curvature": 0.13376511243234865,
+"pose": {
+"rotation": {
+"radians": -0.13724359655540147
+},
+"translation": {
+"x": 11.063735981499413,
+"y": 1.2894843138880479
+}
+},
+"time": 8.682165964903374,
+"velocity": 2.4066418325582033
+},
+{
+"acceleration": -0.10634016843887163,
+"curvature": 0.1402585995781988,
+"pose": {
+"rotation": {
+"radians": -0.12111918795125635
+},
+"translation": {
+"x": 11.180542104072739,
+"y": 1.2743030623751146
+}
+},
+"time": 8.73115339910763,
+"velocity": 2.402286991335023
+},
+{
+"acceleration": -0.11857851211457891,
+"curvature": 0.14800304450914864,
+"pose": {
+"rotation": {
+"radians": -0.10430443626215033
+},
+"translation": {
+"x": 11.29654085127195,
+"y": 1.2611641269814307
+}
+},
+"time": 8.77980134147057,
+"velocity": 2.3971137609499436
+},
+{
+"acceleration": -0.12200868250447694,
+"curvature": 0.15657642317416817,
+"pose": {
+"rotation": {
+"radians": -0.08678050111078121
+},
+"translation": {
+"x": 11.411126553156999,
+"y": 1.2501733707479241
+}
+},
+"time": 8.827879428665259,
+"velocity": 2.3914127329050823
+},
+{
+"acceleration": -0.1107675312384779,
+"curvature": 0.16529790359382923,
+"pose": {
+"rotation": {
+"radians": -0.06859450345427177
+},
+"translation": {
+"x": 11.523776904642165,
+"y": 1.2413948718358068
+}
+},
+"time": 8.87518552210774,
+"velocity": 2.3856409787697315
+},
+{
+"acceleration": -0.07611100228493164,
+"curvature": 0.17309365230464654,
+"pose": {
+"rotation": {
+"radians": -0.04988693820765568
+},
+"translation": {
+"x": 11.634070579454216,
+"y": 1.2348453674458133
+}
+},
+"time": 8.921549170124607,
+"velocity": 2.3805053919396935
+},
+{
+"acceleration": -0.006042799237358702,
+"curvature": 0.17834452799938041,
+"pose": {
+"rotation": {
+"radians": -0.030927066570615225
+},
+"translation": {
+"x": 11.741704844090558,
+"y": 1.2304886977374405
+}
+},
+"time": 8.966833856747254,
+"velocity": 2.377058729052685
+},
+{
+"acceleration": 0.114243026957442,
+"curvature": 0.1787511867106332,
+"pose": {
+"rotation": {
+"radians": -0.012154852950734176
+},
+"translation": {
+"x": 11.846513171777406,
+"y": 1.2282302497481803
+}
+},
+"time": 9.010938166711492,
+"velocity": 2.3767922155620687
+},
+{
+"acceleration": 0.300127344264292,
+"curvature": 0.17129476565776477,
+"pose": {
+"rotation": {
+"radians": 0.005775606424505827
+},
+"translation": {
+"x": 11.948482856427944,
+"y": 1.2279114013127668
+}
+},
+"time": 9.05379646108885,
+"velocity": 2.3816884768419713
+},
+{
+"acceleration": 0.5620849115247822,
+"curvature": 0.15242642617210914,
+"pose": {
+"rotation": {
+"radians": 0.02196204084413161
+},
+"translation": {
+"x": 12.04777262660047,
+"y": 1.2293039649824031
+}
+},
+"time": 9.09538042230077,
+"velocity": 2.3941689606844943
+},
+{
+"acceleration": 0.8953042965404892,
+"curvature": 0.11865885995496812,
+"pose": {
+"rotation": {
+"radians": 0.03524784784215264
+},
+"translation": {
+"x": 12.14473025945658,
+"y": 1.2321046319440114
+}
+},
+"time": 9.135703852337135,
+"velocity": 2.4168341522888603
+},
+{
+"acceleration": 0.7825723581978271,
+"curvature": 0.06769275942290126,
+"pose": {
+"rotation": {
+"radians": 0.04426935811568594
+},
+"translation": {
+"x": 12.239910194719302,
+"y": 1.2359294159394612
+}
+},
+"time": 9.174834100003519,
+"velocity": 2.451867631149266
+},
+{
+"acceleration": -1.5000000000000047,
+"curvature": 1.2149307650299772e-14,
+"pose": {
+"rotation": {
+"radians": 0.047583103276993555
+},
+"translation": {
+"x": 12.334091148631282,
+"y": 1.240308097184811
+}
+},
+"time": 9.21305439290487,
+"velocity": 2.481777775896087
+},
+{
+"acceleration": -1.4999999999999953,
+"curvature": 0.10599368916552536,
+"pose": {
+"rotation": {
+"radians": 0.05269997543188024
+},
+"translation": {
+"x": 12.428068589412703,
+"y": 1.2449459512087762
+}
+},
+"time": 9.251412096767089,
+"velocity": 2.424241220102757
+},
+{
+"acceleration": -1.2104979690057325,
+"curvature": 0.19627084349446744,
+"pose": {
+"rotation": {
+"radians": 0.06702057986971198
+},
+"translation": {
+"x": 12.521899371991411,
+"y": 1.250502840754752
+}
+},
+"time": 9.290661732707935,
+"velocity": 2.365366766191487
+},
+{
+"acceleration": -0.9580832749843498,
+"curvature": 0.2725636704184413,
+"pose": {
+"rotation": {
+"radians": 0.0891110688002503
+},
+"translation": {
+"x": 12.615417487767115,
+"y": 1.257762183061364
+}
+},
+"time": 9.330727843915803,
+"velocity": 2.3168668199484066
+},
+{
+"acceleration": -0.7527907023664805,
+"curvature": 0.3362681568598906,
+"pose": {
+"rotation": {
+"radians": 0.11767148623862528
+},
+"translation": {
+"x": 12.708438612775414,
+"y": 1.267367508227494
+}
+},
+"time": 9.371433454087796,
+"velocity": 2.277867455644587
+},
+{
+"acceleration": -0.5811969395776064,
+"curvature": 0.3884820572051368,
+"pose": {
+"rotation": {
+"radians": 0.15151655014547227
+},
+"translation": {
+"x": 12.800763181264406,
+"y": 1.2798313252985976
+}
+},
+"time": 9.412612467845873,
+"velocity": 2.2468682769548853
+},
+{
+"acceleration": -0.4351595020695317,
+"curvature": 0.4301213893546678,
+"pose": {
+"rotation": {
+"radians": 0.18956130720561593
+},
+"translation": {
+"x": 12.892179459271269,
+"y": 1.2955439883530246
+}
+},
+"time": 9.454117978236553,
+"velocity": 2.222745401340216
+},
+{
+"acceleration": -0.3100223272120345,
+"curvature": 0.4620460331712726,
+"pose": {
+"rotation": {
+"radians": 0.23081421334615107
+},
+"translation": {
+"x": 12.982466618198856,
+"y": 1.3147825625883431
+}
+},
+"time": 9.495819778139252,
+"velocity": 2.2045984668591547
+},
+{
+"acceleration": -0.20327818057609892,
+"curvature": 0.4851572407645364,
+"pose": {
+"rotation": {
+"radians": 0.2743768212702214
+},
+"translation": {
+"x": 13.071397808392291,
+"y": 1.337719690407659
+}
+},
+"time": 9.5376016025144,
+"velocity": 2.1916451684312066
+},
+{
+"acceleration": -0.11359332829467501,
+"curvature": 0.5004506726719216,
+"pose": {
+"rotation": {
+"radians": 0.3194476094244123
+},
+"translation": {
+"x": 13.158743232715548,
+"y": 1.3644324575059397
+}
+},
+"time": 9.57935841824797,
+"velocity": 2.183156918902235
+},
+{
+"acceleration": -0.04012900004615342,
+"curvature": 0.5090236239739091,
+"pose": {
+"rotation": {
+"radians": 0.3653270166343734
+},
+"translation": {
+"x": 13.244273220128056,
+"y": 1.394911258956336
+}
+},
+"time": 9.620993897318279,
+"velocity": 2.1784274062594955
+},
+{
+"acceleration": 0.017889527016041535,
+"curvature": 0.5120456636627979,
+"pose": {
+"rotation": {
+"radians": 0.41142109023890383
+},
+"translation": {
+"x": 13.327761299261285,
+"y": 1.4290686652965041
+}
+},
+"time": 9.662418129907413,
+"velocity": 2.1767650932280143
+},
+{
+"acceleration": 0.061403979193286364,
+"curvature": 0.5107075187825005,
+"pose": {
+"rotation": {
+"radians": 0.45724198256217663
+},
+"translation": {
+"x": 13.40898727199534,
+"y": 1.4667482886149292
+}
+},
+"time": 9.703545616009427,
+"velocity": 2.177500844501738
+},
+{
+"acceleration": 0.09160706275242977,
+"curvature": 0.506163615611916,
+"pose": {
+"rotation": {
+"radians": 0.5024045170476972
+},
+"translation": {
+"x": 13.487740287035548,
+"y": 1.5077336486372452
+}
+},
+"time": 9.744293583524312,
+"velocity": 2.180002931851191
+},
+{
+"acceleration": 0.10993557844588701,
+"curvature": 0.4994803563388272,
+"pose": {
+"rotation": {
+"radians": 0.5466189193905108
+},
+"translation": {
+"x": 13.563821913489052,
+"y": 1.5517570388125579
+}
+},
+"time": 9.78458069244269,
+"velocity": 2.1836935155659907
+},
+{
+"acceleration": 0.11802692876394964,
+"curvature": 0.49159690034383247,
+"pose": {
+"rotation": {
+"radians": 0.5896804015834484
+},
+"translation": {
+"x": 13.637049214441408,
+"y": 1.5985083923997685
+}
+},
+"time": 9.824326183746969,
+"velocity": 2.1880629591431426
+},
+{
+"acceleration": 0.11768659180944462,
+"curvature": 0.4832998495339,
+"pose": {
+"rotation": {
+"radians": 0.6314565472827764
+},
+"translation": {
+"x": 13.707257820533169,
+"y": 1.6476441485538942
+}
+},
+"time": 9.863449513861958,
+"velocity": 2.192680565639633
+},
+{
+"acceleration": 0.11090600123249449,
+"curvature": 0.47520904080469883,
+"pose": {
+"rotation": {
+"radians": 0.6718734180857844
+},
+"translation": {
+"x": 13.774305003536476,
+"y": 1.698796118412389
+}
+},
+"time": 9.901870484288079,
+"velocity": 2.1972021987030947
+},
+{
+"acceleration": 0.09996434319048116,
+"curvature": 0.46776900814336775,
+"pose": {
+"rotation": {
+"radians": 0.710901073198297
+},
+"translation": {
+"x": 13.83807274993166,
+"y": 1.7515803511814703
+}
+},
+"time": 9.939509838265343,
+"velocity": 2.2013766289416874
+},
+{
+"acceleration": 0.08764641660775951,
+"curvature": 0.4612393512497733,
+"pose": {
+"rotation": {
+"radians": 0.7485388668471398
+},
+"translation": {
+"x": 13.898470834483822,
+"y": 1.8056060002224346
+}
+},
+"time": 9.976290258643424,
+"velocity": 2.205053359507052
+},
+{
+"acceleration": 0.07761191741201812,
+"curvature": 0.4556767570370438,
+"pose": {
+"rotation": {
+"radians": 0.7848005376797789
+},
+"translation": {
+"x": 13.955439893819435,
+"y": 1.8604841891379875
+}
+},
+"time": 10.012137668387874,
+"velocity": 2.208195256515823
+},
+{
+"acceleration": 0.07495600422207634,
+"curvature": 0.45090141028046343,
+"pose": {
+"rotation": {
+"radians": 0.8196987882654774
+},
+"translation": {
+"x": 14.008954500002925,
+"y": 1.9158368778585597
+}
+},
+"time": 10.046982710379487,
+"velocity": 2.2108996470370945
+},
+{
+"acceleration": 0.08699831016011897,
+"curvature": 0.4464410774286511,
+"pose": {
+"rotation": {
+"radians": 0.8532288144825836
+},
+"translation": {
+"x": 14.059026234113269,
+"y": 1.9713057287286286
+}
+},
+"time": 10.080762269187426,
+"velocity": 2.2134316277897224
+},
+{
+"acceleration": 0.12431014491069357,
+"curvature": 0.44144807874110703,
+"pose": {
+"rotation": {
+"radians": 0.8853501371154769
+},
+"translation": {
+"x": 14.10570675982059,
+"y": 2.0265609725930487
+}
+},
+"time": 10.113420899466222,
+"velocity": 2.2162728734361217
+},
+{
+"acceleration": 0.20190364554911386,
+"curvature": 0.4345896555723384,
+"pose": {
+"rotation": {
+"radians": 0.9159662020831767
+},
+"translation": {
+"x": 14.149090896962738,
+"y": 2.0813102748833634
+}
+},
+"time": 10.144912054272671,
+"velocity": 2.2201875434535165
+},
+{
+"acceleration": 0.34029978811157974,
+"curvature": 0.4239244564710277,
+"pose": {
+"rotation": {
+"radians": 0.9449017110349948
+},
+"translation": {
+"x": 14.189319695121894,
+"y": 2.135307601704132
+}
+},
+"time": 10.175199081791327,
+"velocity": 2.2263026047223793
+},
+{
+"acceleration": -1.1599390923694373,
+"curvature": 0.4068021650733051,
+"pose": {
+"rotation": {
+"radians": 0.9718787721008235
+},
+"translation": {
+"x": 14.22658350720115,
+"y": 2.1883620859192536
+}
+},
+"time": 10.204256112138724,
+"velocity": 2.23619070599275
+},
+{
+"acceleration": -1.5000000000000038,
+"curvature": 0.3392836817515641,
+"pose": {
+"rotation": {
+"radians": 1.0182103823257231
+},
+"translation": {
+"x": 14.293242542796484,
+"y": 2.2912080883027723
+}
+},
+"time": 10.259865214671064,
+"velocity": 2.171687534073908
+},
+{
+"acceleration": -1.5,
+"curvature": 0.20406980022437712,
+"pose": {
+"rotation": {
+"radians": 1.0501589705834076
+},
+"translation": {
+"x": 14.35169368930232,
+"y": 2.3897615914121006
+}
+},
+"time": 10.313625677445964,
+"velocity": 2.091046839911558
+},
+{
+"acceleration": -1.500000000000002,
+"curvature": -5.2157928028545256e-14,
+"pose": {
+"rotation": {
+"radians": 1.0618539400422868
+},
+"translation": {
+"x": 14.40554819430877,
+"y": 2.485386002299446
+}
+},
+"time": 10.367136802535915,
+"velocity": 2.0107801522766304
+},
+{
+"acceleration": -0.24224338836075404,
+"curvature": 1.029557432992968,
+"pose": {
+"rotation": {
+"radians": 1.1336066747952362
+},
+"translation": {
+"x": 14.457676946479703,
+"y": 2.584843420641496
+}
+},
+"time": 10.424195448907621,
+"velocity": 1.925192182719071
+},
+{
+"acceleration": 1.4999999999999871,
+"curvature": 1.0479043777054697,
+"pose": {
+"rotation": {
+"radians": 1.1996378269499706
+},
+"translation": {
+"x": 14.482183623112192,
+"y": 2.6421273642868153
+}
+},
+"time": 10.456625094063073,
+"velocity": 1.9173363155932774
+},
+{
+"acceleration": 1.4999999999999942,
+"curvature": 0.8972494576006016,
+"pose": {
+"rotation": {
+"radians": 1.2673330023047817
+},
+"translation": {
+"x": 14.505005896365555,
+"y": 2.707385616365368
+}
+},
+"time": 10.492187647367018,
+"velocity": 1.9706801455491956
+},
+{
+"acceleration": 1.5,
+"curvature": 0.7040363023257407,
+"pose": {
+"rotation": {
+"radians": 1.3292875913931097
+},
+"translation": {
+"x": 14.525782630841476,
+"y": 2.7820994536214094
+}
+},
+"time": 10.53096665989964,
+"velocity": 2.0288486643481294
+},
+{
+"acceleration": 1.5000000000000138,
+"curvature": 0.5308425540610521,
+"pose": {
+"rotation": {
+"radians": 1.3826818156330491
+},
+"translation": {
+"x": 14.544243259198543,
+"y": 2.8672258794423096
+}
+},
+"time": 10.573239350494921,
+"velocity": 2.092257700241051
+},
+{
+"acceleration": 1.4999999999999918,
+"curvature": 0.39537491143027376,
+"pose": {
+"rotation": {
+"radians": 1.427331087264109
+},
+"translation": {
+"x": 14.560201043926641,
+"y": 2.963241954290167
+}
+},
+"time": 10.619009036705197,
+"velocity": 2.1609122295564656
+},
+{
+"acceleration": 1.5000000000000113,
+"curvature": 0.29549137942553677,
+"pose": {
+"rotation": {
+"radians": 1.4641887630587787
+},
+"translation": {
+"x": 14.573546339121341,
+"y": 3.0701891261334167
+}
+},
+"time": 10.668049828052892,
+"velocity": 2.2344734165780062
+},
+{
+"acceleration": 1.499999999999993,
+"curvature": 0.22349693712070853,
+"pose": {
+"rotation": {
+"radians": 1.4945193700588906
+},
+"translation": {
+"x": 14.584239852258305,
+"y": 3.1877175608784425
+}
+},
+"time": 10.719960439614722,
+"velocity": 2.3123393339207525
+},
+{
+"acceleration": 1.0454421192850534,
+"curvature": 0.19552746605489585,
+"pose": {
+"rotation": {
+"radians": 1.507621677583229
+},
+"translation": {
+"x": 14.588597490658442,
+"y": 3.250244346938178
+}
+},
+"time": 10.746832307115975,
+"velocity": 2.352647135172632
+},
+{
+"acceleration": 0.47405057111068766,
+"curvature": 0.17180557003892236,
+"pose": {
+"rotation": {
+"radians": 1.5195331128893124
+},
+"translation": {
+"x": 14.59230590596768,
+"y": 3.3151304728011866
+}
+},
+"time": 10.774289856157074,
+"velocity": 2.3813524134325315
+},
+{
+"acceleration": 0.39815646910133756,
+"curvature": 0.1516448241391937,
+"pose": {
+"rotation": {
+"radians": 1.5303763601254794
+},
+"translation": {
+"x": 14.595376972892534,
+"y": 3.382241902021141
+}
+},
+"time": 10.802422636818575,
+"velocity": 2.3946887741720477
+},
+{
+"acceleration": 0.33575064667543536,
+"curvature": 0.1344647710085441,
+"pose": {
+"rotation": {
+"radians": 1.5402615426796327
+},
+"translation": {
+"x": 14.597825699808482,
+"y": 3.4514284549787613
+}
+},
+"time": 10.831263246387135,
+"velocity": 2.406171849444596
+},
+{
+"acceleration": 0.2844440045580343,
+"curvature": 0.11977895743055027,
+"pose": {
+"rotation": {
+"radians": 1.5492868523520194
+},
+"translation": {
+"x": 14.599670018190396,
+"y": 3.522525194207807
+}
+},
+"time": 10.860760139635142,
+"velocity": 2.4160754504275306
+},
+{
+"acceleration": 0.24222404124363497,
+"curvature": 0.10718188166874665,
+"pose": {
+"rotation": {
+"radians": 1.5575394401312181
+},
+"translation": {
+"x": 14.600930572043008,
+"y": 3.5953538097210593
+}
+},
+"time": 10.890854694789267,
+"velocity": 2.4246356662109627
+},
+{
+"acceleration": 0.20742520101503603,
+"curvature": 0.09633643953868115,
+"pose": {
+"rotation": {
+"radians": 1.5650964130136606
+},
+"translation": {
+"x": 14.601630507331354,
+"y": 3.669724004336314
+}
+},
+"time": 10.921481927945775,
+"velocity": 2.4320543183982433
+},
+{
+"acceleration": -1.295699292352763,
+"curvature": 0.08696259838622296,
+"pose": {
+"rotation": {
+"radians": 1.5720258421854452
+},
+"translation": {
+"x": 14.601795261411219,
+"y": 3.7454348790023646
+}
+},
+"time": 10.952571205289999,
+"velocity": 2.438503018000781
+},
+{
+"acceleration": -1.5,
+"curvature": 0.07882754841821012,
+"pose": {
+"rotation": {
+"radians": 1.578387727657952
+},
+"translation": {
+"x": 14.6014523524596,
+"y": 3.8222763181249944
+}
+},
+"time": 10.984351574869327,
+"velocity": 2.397325215626135
+},
+{
+"acceleration": -1.5000000000000056,
+"curvature": 0.0717373275423103,
+"pose": {
+"rotation": {
+"radians": 1.5842348898948395
+},
+"translation": {
+"x": 14.600631168905139,
+"y": 3.900030374892962
+}
+},
+"time": 11.017123044263919,
+"velocity": 2.3481680115342485
+},
+{
+"acceleration": -1.5,
+"curvature": 0.06552979366108969,
+"pose": {
+"rotation": {
+"radians": 1.5896137748500303
+},
+"translation": {
+"x": 14.599362758858588,
+"y": 3.978472656603989
+}
+},
+"time": 11.050897489097492,
+"velocity": 2.297506344283887
+},
+{
+"acceleration": -1.5000000000000056,
+"curvature": 0.06006876925748621,
+"pose": {
+"rotation": {
+"radians": 1.5945651684226014
+},
+"translation": {
+"x": 14.597679619543248,
+"y": 4.05737370999075
+}
+},
+"time": 11.085641401215264,
+"velocity": 2.2453904761072287
+},
+{
+"acceleration": -1.4999999999999944,
+"curvature": 0.05523917269971213,
+"pose": {
+"rotation": {
+"radians": 1.5991248218286502
+},
+"translation": {
+"x": 14.595615486725427,
+"y": 4.1365004065468565
+}
+},
+"time": 11.121318152630586,
+"velocity": 2.1918753489842473
+},
+{
+"acceleration": -1.5,
+"curvature": 0.050942959019971416,
+"pose": {
+"rotation": {
+"radians": 1.603323992256461
+},
+"translation": {
+"x": 14.593205124144882,
+"y": 4.21561732785285
+}
+},
+"time": 11.157888045846219,
+"velocity": 2.1370205091607977
+},
+{
+"acceleration": -1.5,
+"curvature": 0.047095708656321005,
+"pose": {
+"rotation": {
+"radians": 1.607189904369106
+},
+"translation": {
+"x": 14.590484112945276,
+"y": 4.2944881509021835
+}
+},
+"time": 11.195308348998358,
+"velocity": 2.0808900544325892
+},
+{
+"acceleration": -1.5000000000000029,
+"curvature": 0.04362371949233402,
+"pose": {
+"rotation": {
+"radians": 1.610746138366785
+},
+"translation": {
+"x": 14.587488641104624,
+"y": 4.372877033427217
+}
+},
+"time": 11.23353331698508,
+"velocity": 2.0235526024525066
+},
+{
+"acceleration": -1.5,
+"curvature": 0.04046147200560862,
+"pose": {
+"rotation": {
+"radians": 1.6140129498101696
+},
+"translation": {
+"x": 14.584255292865743,
+"y": 4.450549999225197
+}
+},
+"time": 11.27251419988848,
+"velocity": 1.9650812780974054
+},
+{
+"acceleration": -1.5,
+"curvature": 0.03754934743280742,
+"pose": {
+"rotation": {
+"radians": 1.6170075254734664
+},
+"translation": {
+"x": 14.580820838166701,
+"y": 4.527276323484252
+}
+},
+"time": 11.312199240251902,
+"velocity": 1.9055537175522743
+},
+{
+"acceleration": -1.5,
+"curvature": 0.03483148319375945,
+"pose": {
+"rotation": {
+"radians": 1.6197441782771393
+},
+"translation": {
+"x": 14.577222022071272,
+"y": 4.602829918109375
+}
+},
+"time": 11.352533661170606,
+"velocity": 1.8450520861742172
+},
+{
+"acceleration": -1.500000000000003,
+"curvature": 0.03225364708379775,
+"pose": {
+"rotation": {
+"radians": 1.622234482911262
+},
+"translation": {
+"x": 14.573495354199379,
+"y": 4.676990717048412
+}
+},
+"time": 11.393459647751616,
+"velocity": 1.7836631063027024
+},
+{
+"acceleration": -1.499999999999997,
+"curvature": 0.02976100030065775,
+"pose": {
+"rotation": {
+"radians": 1.6244873521200869
+},
+"translation": {
+"x": 14.569676898157548,
+"y": 4.7495460616180525
+}
+},
+"time": 11.434916325373766,
+"velocity": 1.7214780898694773
+},
+{
+"acceleration": -1.5,
+"curvature": 0.027295596880158327,
+"pose": {
+"rotation": {
+"radians": 1.6265090517632605
+},
+"translation": {
+"x": 14.565802060969355,
+"y": 4.820292085829816
+}
+},
+"time": 11.476839739441962,
+"velocity": 1.6585929687671843
+},
+{
+"acceleration": -1.4999999999999967,
+"curvature": 0.024793430155251456,
+"pose": {
+"rotation": {
+"radians": 1.628303150665911
+},
+"translation": {
+"x": 14.561905382505882,
+"y": 4.889035101716041
+}
+},
+"time": 11.519162843135039,
+"velocity": 1.5951083132275676
+},
+{
+"acceleration": -1.4999999999999982,
+"curvature": 0.022180780497442442,
+"pose": {
+"rotation": {
+"radians": 1.6298703988785799
+},
+"translation": {
+"x": 14.558020324916159,
+"y": 4.955592984655867
+}
+},
+"time": 11.561815502215325,
+"velocity": 1.5311293246071387
+},
+{
+"acceleration": -1.5,
+"curvature": 0.016251038987281526,
+"pose": {
+"rotation": {
+"radians": 1.6323119423099364
+},
+"translation": {
+"x": 14.550412268926543,
+"y": 5.081490981902849
+}
+},
+"time": 11.64781375092693,
+"velocity": 1.4021319515397286
+},
+{
+"acceleration": -1.499999999999999,
+"curvature": 0.008502571577023417,
+"pose": {
+"rotation": {
+"radians": 1.6337731713085175
+},
+"translation": {
+"x": 14.543216034108877,
+"y": 5.196812989927551
+}
+},
+"time": 11.734214408046883,
+"velocity": 1.272530965859801
+},
+{
+"acceleration": -1.499999999999999,
+"curvature": -0.0027403867241128457,
+"pose": {
+"rotation": {
+"radians": 1.6341244241908057
+},
+"translation": {
+"x": 14.536639041567547,
+"y": 5.300659595498862
+}
+},
+"time": 11.820357842925905,
+"velocity": 1.1433158135412687
+},
+{
+"acceleration": -1.4999999999999993,
+"curvature": -0.020427742480318468,
+"pose": {
+"rotation": {
+"radians": 1.6331424030278288
+},
+"translation": {
+"x": 14.53085125417735,
+"y": 5.3924493902294035
+}
+},
+"time": 11.905563613709885,
+"velocity": 1.0155071573652967
+},
+{
+"acceleration": -1.5000000000000004,
+"curvature": -0.04986098203760299,
+"pose": {
+"rotation": {
+"radians": 1.6304755323263018
+},
+"translation": {
+"x": 14.525978438357887,
+"y": 5.471963301007115
+}
+},
+"time": 11.9891730528822,
+"velocity": 0.8900929986068264
+},
+{
+"acceleration": -1.5,
+"curvature": -0.1860390076340229,
+"pose": {
+"rotation": {
+"radians": 1.6178622631759
+},
+"translation": {
+"x": 14.519219375479953,
+"y": 5.595364837222159
+}
+},
+"time": 12.149745078791785,
+"velocity": 0.6492349597424487
+},
+{
+"acceleration": -1.5,
+"curvature": -0.4610705169443318,
+"pose": {
+"rotation": {
+"radians": 1.592140798956125
+},
+"translation": {
+"x": 14.516228002613662,
+"y": 5.678042881155375
+}
+},
+"time": 12.305031914783648,
+"velocity": 0.4163047057546556
+},
+{
+"acceleration": -1.5,
+"curvature": 1.5794015885367656e-13,
+"pose": {
+"rotation": {
+"radians": 1.5707963267949763
+},
+"translation": {
+"x": 14.515732079717097,
+"y": 5.735810621846528
+}
+},
+"time": 12.582568385286752,
+"velocity": 0.0
+}
+]
\ No newline at end of file
diff --git a/PathWeaver/pathweaver.json b/PathWeaver/pathweaver.json
new file mode 100644
index 0000000..0c82c1e
--- /dev/null
+++ b/PathWeaver/pathweaver.json
@@ -0,0 +1,9 @@
+{
+  "lengthUnit": "Meter",
+  "exportUnit": "Always Meters",
+  "maxVelocity": 2.5,
+  "maxAcceleration": 1.5,
+  "trackWidth": 0.58,
+  "gameName": "Crescendo",
+  "outputDir": ""
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index c4c2329..efff2b8 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -3,7 +3,6 @@ package frc.robot;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
-import edu.wpi.first.math.controller.ArmFeedforward;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
@@ -16,6 +15,8 @@ import frc.robot.util.IMUGyro;
 public class Constants
 {
     public static final GenericHID controller = new GenericHID(0);
+    public static final GenericHID controller2 = new GenericHID(1);
+
     public static final double InputSmoothing = 0.1;
 
     public static final double DriveMaxSpeed = 5; // m/s
@@ -45,36 +46,50 @@ public class Constants
     public static final Encoder encoderR = new Encoder(4, 5);
 
     // for Intake
-    public static final CANSparkMax intakeT = null;//new CANSparkMax(41, MotorType.kBrushless);
-    public static final CANSparkMax intakeD = null;//new CANSparkMax(42, MotorType.kBrushless);
-    // for Shooter
-    public static final CANSparkMax shooterT = null;//new CANSparkMax(61, MotorType.kBrushless);
-    public static final CANSparkMax shooterD = null;//new CANSparkMax(62, MotorType.kBrushless);
+    public static final int intakeT_ID = 32;
+    public static final int intakeD_ID = 31;
+
+    // for Shooter    CommandScheduler.getInstance().run();
 
-    // for Arm]
+    public static final int shooterT_ID = 21;
+    public static final int shooterD_ID = 22;
+
+    // for Arm
     public static final double ArmGearReduction = 60;
 
-    public static final ArmFeedforward arm_feedforward = new ArmFeedforward(0, 0, 0);
+    public static final double ArmEpsilon = 3 * Math.PI/180;
+
+    public static final double ArmkS = 0.4;
+    public static final double ArmkG = 0.495;
+    public static final double ArmkV = 0;
+
+    public static final double arm_initial_position = -43.0 * Math.PI / 180.0;
     public static final TrapezoidProfile.Constraints arm_constraints = 
-        new TrapezoidProfile.Constraints(0, 0);
+        new TrapezoidProfile.Constraints(7, 5);
 
-    public static final double ArmkP = 0.0;
+    public static final double ArmkP = 11.5;
     public static final double ArmkI = 0.0;
-    public static final double ArmkD = 0.0;
+    public static final double ArmkD = 0.4;
 
-    public static final CANSparkMax armL = null;//new CANSparkMax(51, MotorType.kBrushless);
-    public static final CANSparkMax armR = null;//new CANSparkMax(52, MotorType.kBrushless);
+    public static final int armL_ID = 41;
+    public static final int armR_ID = 42;
 
     // for Chain 
-    public static final double ChainEncoderScale = 2600;
+    public static final double ChainEncoderScale = 3000;
+
+    public static final double ChainEpsilon = 0.05;
 
     public static final TrapezoidProfile.Constraints chain_constraints = 
-        new TrapezoidProfile.Constraints(0.5, 0.5);
+        new TrapezoidProfile.Constraints(5, 5);
 
     public static final double ChainkP = 45.0;
     public static final double ChainkI = 0.0;
     public static final double ChainkD = 0.0;
-    
-    public static final CANSparkMax chain = new CANSparkMax(50, MotorType.kBrushed);
-    public static final Encoder chain_encoder = new Encoder(8, 9);
+
+    public static final int chain_ID = 50;
+    public static final int[] chain_encoder_PINS = {8, 9};
+
+    // Hooks
+    public static final int hookL_ID = 62;
+    public static final int hookR_ID = 61;
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 0a5123a..2fc88fa 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -8,6 +8,7 @@ import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 
+import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -21,6 +22,8 @@ public class Robot extends LoggedRobot {
     Logger.addDataReceiver(new NT4Publisher());
     Logger.start();
 
+    CameraServer.startAutomaticCapture();
+
     m_robotContainer = new RobotContainer();
   }
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 87c4e60..88e493c 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,26 +4,28 @@
 
 package frc.robot;
 
-import java.util.List;
+import com.fasterxml.jackson.databind.deser.std.ContainerDeserializerBase;
 
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.trajectory.Trajectory;
-import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
-
+import frc.robot.subsystems.Arm;
+import frc.robot.subsystems.Chain;
 import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.Hooks;
+import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.Shooter;
 
 public class RobotContainer 
 {
     private final Drivetrain m_drive = new Drivetrain();
-    // private final Chain m_chain = new Chain();
-
+    private final Chain m_chain = new Chain();
+    private final Arm m_arm = new Arm();
+    private final Intake m_intake = new Intake();
+    private final Shooter m_shooter = new Shooter();
+    private final Hooks m_hooks = new Hooks();
+    
     private final GenericHID m_controller = Constants.controller;
 
     public RobotContainer() 
@@ -31,7 +33,7 @@ public class RobotContainer
         configureBindings();
     }
 
-    private double[] correctInputs(double in1, double in2)  
+    private double[] correctInputs(double in1, double in2) 
     {
         double p1 = processInput(in1);
         double p2 = processInput(in2);
@@ -59,6 +61,19 @@ public class RobotContainer
 
     private void configureBindings() 
     {   
+        m_shooter.setDefaultCommand(m_shooter.run(() -> m_shooter.atPercentage(0.0)));
+        m_intake.setDefaultCommand(m_intake.run(() -> m_intake.atPercentage(0.0)));
+        m_arm.setDefaultCommand(m_arm.run(() -> m_arm.incrementAngleDegrees(
+            deadzone(-0.05, 0.05, Constants.controller2.getRawAxis(2)))));
+        m_hooks.setDefaultCommand(m_hooks.run(() -> m_hooks.atPercentage(
+            deadzone(-0.1, 0.1, Constants.controller2.getRawAxis(4)), 
+            deadzone(-0.1, 0.1, Constants.controller2.getRawAxis(5)))));
+        m_chain.setDefaultCommand(m_chain.run(() -> 
+            m_chain.atPercentage(
+                Constants.controller2.getRawAxis(0)
+                * (-0.5) + 0.5
+            )));
+
         m_drive.setDefaultCommand(m_drive.run(() -> {
             double[] inputs = correctInputs(
                     m_controller.getRawAxis(2),
@@ -68,25 +83,39 @@ public class RobotContainer
                 inputs[1] * Constants.DriveMaxSpeed);
         }));
 
-        Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
-            new Pose2d(0, 0, new Rotation2d(0)),
-            // List.of(),
-            List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
-            new Pose2d(3, 0, new Rotation2d(0)), 
-            m_drive.trajectoyConfig);
 
-        new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.followTrajectory(trajectory));
+        // Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
+        //     new Pose2d(0, 0, new Rotation2d(0)),
+        //      List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
+        //     new Pose2d(3, 0, new Rotation2d(0)),     CameraServer camere = CameraServer.startAutomaticCapture();
 
-        // m_chain.setDefaultCommand(m_chain.run(() ->
-        // m_chain.atPercentage(
-        //     processInput(m_controller.getRawAxis(2))
-        // )));
+        //     m_drive.trajectoyConfig);
 
+        // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.followTrajectory(trajectory));
 
-        // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kForward));
+        new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(0.3)));
+        new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(-0.1)));
+        new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_shooter.run(() -> m_shooter.atPercentage(1)));
+        new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(1)));
+
+        new Trigger(() -> Constants.controller2.getRawAxis(6) > 0).whileTrue(
+            Commands.parallel(m_arm.run(() -> m_arm.atAngleDegrees(0)), 
+                Commands.waitUntil(() -> m_arm.hasClearedAngle(-20)).andThen(m_chain.run(() -> m_chain.atPercentage(0.9)))
+                .until(() -> m_chain.isAtPosition(0.9)).andThen(Commands.run(() -> m_arm.atAngleDegrees((-40))))));//.whileTrue(
+            // Commands.parallel(m_arm.run(() -> m_arm.atAngleDegrees(-30)), Commands.waitUntil(() -> m_arm.hasClearedAngleDegree(-20))
+            //     .andThen(() ->
+            //         m_chain.run(() -> m_chain.atPercentage(0.9)).until(() ->
+            //             m_chain.atPosition(0)).andThen(m_arm.run(() -> m_arm.atAngleDegrees(-40))))
+            // ));
+            // Commands.parallel(arm (chain at 0.waitUntil(arm > -30)).andThen());
         // new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kReverse));
         // new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kForward));
         // new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kReverse));
+        // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_arm.sysIdQausistatic(Direction.kForward));
+        // new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_arm.sysIdQausistatic(Direction.kReverse));
+        // new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_arm.sysIdDynamic(Direction.kForward));
+        // new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_arm.sysIdDynamic(Direction.kReverse));
+
 
     }
 
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index dca7ebd..75e8ad2 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -4,56 +4,192 @@ import org.littletonrobotics.junction.Logger;
 
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.RelativeEncoder;
+import com.revrobotics.CANSparkLowLevel.MotorType;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
 
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Seconds;
+
 public class Arm extends SubsystemBase 
 {
-    private final CANSparkMax m_armL = Constants.armL;
-    private final CANSparkMax m_armR = Constants.armR;
+    // rad
+    private double m_setpoint = Constants.arm_initial_position;
+    private final CANSparkMax m_armL = new CANSparkMax(Constants.armL_ID, MotorType.kBrushless);
+    private final CANSparkMax m_armR = new CANSparkMax(Constants.armR_ID, MotorType.kBrushless);
 
     private final RelativeEncoder m_encoderL = m_armL.getEncoder();
     private final RelativeEncoder m_encoderR = m_armR.getEncoder();
 
-    private final ArmFeedforward m_feedforward = Constants.arm_feedforward;
+    private ArmFeedforward m_feedforward = new ArmFeedforward(Constants.ArmkS, Constants.ArmkG, Constants.ArmkV);
 
-    private final ProfiledPIDController m_pidL = new ProfiledPIDController(Constants.ArmkP, Constants.ArmkI, Constants.ArmkD, Constants.arm_constraints);
-    private final ProfiledPIDController m_pidR = new ProfiledPIDController(Constants.ArmkP, Constants.ArmkI, Constants.ArmkD, Constants.arm_constraints);
+    private ProfiledPIDController m_pidL = new ProfiledPIDController(Constants.ArmkP, Constants.ArmkI, Constants.ArmkD, Constants.arm_constraints);
+    private ProfiledPIDController m_pidR = new ProfiledPIDController(Constants.ArmkP, Constants.ArmkI, Constants.ArmkD, Constants.arm_constraints);
     
+    // SYSID
+    private final MutableMeasure<Voltage> m_voltage = mutable(Volts.of(0));
+    private final MutableMeasure<Angle> m_angle = mutable(Radians.of(0));
+    private final MutableMeasure<Velocity<Angle>> m_velocity = mutable(RadiansPerSecond.of(0));
+
+    private final SysIdRoutine m_sysIdRoutine = new SysIdRoutine(
+    new SysIdRoutine.Config(
+        Volts.per(Seconds).of(0.5),
+        Volts.of(3),
+        Seconds.of(10)),
+    new SysIdRoutine.Mechanism(
+        (Measure<Voltage> volts) -> {
+            m_armL.setVoltage(volts.in(Volts));
+            m_armR.setVoltage(volts.in(Volts));
+        },
+        log -> {
+            log.motor("arm-left")
+                .voltage(
+                    m_voltage.mut_replace(
+                        m_armL.getAppliedOutput() * m_armL.getBusVoltage(), Volts)
+                )
+                .angularPosition(m_angle.mut_replace(m_encoderL.getPosition(), Radians))
+                .angularVelocity(m_velocity.mut_replace(m_encoderL.getVelocity(), RadiansPerSecond));
+            log.motor("arm-right")
+
+                .voltage(
+                    m_voltage.mut_replace(
+                        m_armR.getAppliedOutput() * m_armR.getBusVoltage(), Volts)
+                )
+                .angularPosition(m_angle.mut_replace(m_encoderR.getPosition(), Radians))
+                .angularVelocity(m_velocity.mut_replace(m_encoderR.getVelocity(), RadiansPerSecond));
+            
+        }, this));
+
     public Arm()
     {   
+        m_armL.setInverted(false);
         m_armR.setInverted(true);
 
         m_encoderL.setPositionConversionFactor(2*Math.PI / Constants.ArmGearReduction);
         m_encoderR.setPositionConversionFactor(2*Math.PI / Constants.ArmGearReduction);
+        reset();
+    }
+
+    // public Arm(double kP, double kI, double kD, double kS, double kG, double kV)
+    // {   
+    //     this();
+
+    //     m_feedforward = new ArmFeedforward(kS, kG, kV);
+    //     m_pidL = new ProfiledPIDController(kP, kI, kD, Constants.arm_constraints);
+    //     m_pidR = new ProfiledPIDController(kP, kI, kD, Constants.arm_constraints);
+    // }
+
+    public void reset()
+    {
+        m_setpoint = Constants.arm_initial_position;
+        m_encoderL.setPosition(Constants.arm_initial_position);
+        m_encoderR.setPosition(Constants.arm_initial_position);
+
+        m_pidL.reset(Constants.arm_initial_position);
+        m_pidR.reset(Constants.arm_initial_position);
     }
 
+
+    public void incrementAngleDegrees(double amount) {
+        incrementAngle(amount * Math.PI / 180.0);
+    }
+
+    public void incrementAngle(double amount) {
+        atAngle(m_setpoint + amount);
+    }
+    
+
     // in rad
     public void atAngle(double angle)
     {
+        m_setpoint = angle;
+        m_pidL.setGoal(m_setpoint);
+        m_pidR.setGoal(m_setpoint);
+
         voltage(
-            m_pidL.calculate(m_encoderL.getPosition())
-            + m_feedforward.calculate(m_pidL.getSetpoint().position, m_pidR.getSetpoint().velocity),
+            m_pidL.calculate(m_encoderL.getPosition()),
+            m_feedforward.calculate(m_pidL.getSetpoint().position, m_pidL.getSetpoint().velocity),
 
-            m_pidR.calculate(m_encoderR.getPosition())
-            + m_feedforward.calculate(m_pidR.getSetpoint().position, m_pidR.getSetpoint().velocity)
+            m_pidR.calculate(m_encoderR.getPosition()),
+            m_feedforward.calculate(m_pidR.getSetpoint().position, m_pidR.getSetpoint().velocity)
         );
     }
+    // in deg
+    public void atAngleDegrees(double angle) 
+    {
+        atAngle(angle * Math.PI / 180.0);
+    }
+
+    // in rad
+    public boolean hasClearedAngle(double angle) 
+    {
+        if(m_encoderL.getPosition() > angle
+        && m_encoderR.getPosition() > angle) 
+            return true;
+
+        return false;
+    }
+
+    public boolean hasClearedAngleDegree(double angle)
+    {
+        return hasClearedAngle(angle * Math.PI/180);
+    }
+    // in rad
+    public boolean isAtAngle(double angle) 
+    {
+        if(Math.abs(m_encoderL.getPosition() - angle) < Constants.ArmEpsilon
+        && Math.abs(m_encoderR.getPosition() - angle) < Constants.ArmEpsilon) 
+            return true;
 
+        return false;
+    }
 
-    public void voltage(double leftVolts, double rightVolts)
+    public boolean isAtAngleDegree(double angle)
     {
-        m_armL.setVoltage(leftVolts);
-        m_armR.setVoltage(rightVolts);
+        return isAtAngle(angle * Math.PI/180);
+    }
+
+    public void voltage(double leftVoltsPID, double leftVoltsFFW, double rightVoltsPID, double rightVoltsFFW)
+    {   
+        Logger.recordOutput(getName() + "/leftPID", leftVoltsPID);
+        Logger.recordOutput(getName() + "/rightPID", rightVoltsPID);
+        Logger.recordOutput(getName() + "/leftFFW", leftVoltsFFW);
+        Logger.recordOutput(getName() + "/rightFFW", rightVoltsFFW);
+
+        m_armL.setVoltage(MathUtil.clamp(leftVoltsPID + leftVoltsFFW, -4, 4));
+        m_armR.setVoltage(MathUtil.clamp(rightVoltsPID + rightVoltsFFW, -4, 4));
     }
 
     @Override
     public void periodic() 
     {
-        Logger.recordOutput(getName() + "/positionL", m_encoderL.getPosition());
-        Logger.recordOutput(getName() + "/positionR", m_encoderR.getPosition());
+        Logger.recordOutput(getName() + "/positionL", m_encoderL.getPosition() * 180/Math.PI);
+        Logger.recordOutput(getName() + "/positionR", m_encoderR.getPosition() * 180/Math.PI);
+    }
+
+    public Command sysIdQausistatic(SysIdRoutine.Direction direction)
+    {
+        return m_sysIdRoutine.quasistatic(direction);
+    }
+
+    public Command sysIdDynamic(SysIdRoutine.Direction direction)
+    {
+        return m_sysIdRoutine.dynamic(direction);
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/Chain.java b/src/main/java/frc/robot/subsystems/Chain.java
index 2485739..40235fe 100644
--- a/src/main/java/frc/robot/subsystems/Chain.java
+++ b/src/main/java/frc/robot/subsystems/Chain.java
@@ -3,7 +3,9 @@ package frc.robot.subsystems;
 import org.littletonrobotics.junction.Logger;
 
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -11,15 +13,16 @@ import frc.robot.Constants;
 
 public class Chain extends SubsystemBase 
 {
-    private final CANSparkMax m_motor = Constants.chain;
-    private final Encoder m_encoder = Constants.chain_encoder;
+    private final CANSparkMax m_motor = new CANSparkMax(Constants.chain_ID, MotorType.kBrushed);
+    private final Encoder m_encoder = new Encoder(Constants.chain_encoder_PINS[0], Constants.chain_encoder_PINS[1]);
 
     private final ProfiledPIDController m_pid = new ProfiledPIDController(Constants.ChainkP, Constants.ChainkI, Constants.ChainkD, Constants.chain_constraints);
 
     public Chain()
     {
+        // TODO: figure out invertedness
+
         // m_motor.setInverted(true);
-        // m_encoder.setReverseDirection(true);
 
         m_encoder.setDistancePerPulse(1.0/Constants.ChainEncoderScale);
         m_encoder.reset();
@@ -33,7 +36,18 @@ public class Chain extends SubsystemBase
     // perc between 0 and 1;
     public void atPercentage(double perc) 
     {
-        m_motor.setVoltage(m_pid.calculate(m_encoder.getDistance(), perc));
+        // System.out.println(perc);
+        Logger.recordOutput(getName() + "/perc", perc);
+        double pid_out = m_pid.calculate(m_encoder.getDistance(), perc);
+        Logger.recordOutput(getName() + "/pidOUt", pid_out);
+        m_motor.setVoltage(MathUtil.clamp(pid_out, -6, 6));
+    }
+
+    public boolean isAtPosition(double pos)
+    {
+        if(Math.abs(pos - m_encoder.getDistance()) < Constants.ChainEpsilon)
+            return true;
+        return false;
     }
 
     @Override
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 5f34119..aee9597 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -13,7 +13,6 @@ import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
 import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.math.trajectory.Trajectory;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
-import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
 import edu.wpi.first.units.Distance;
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Velocity;
diff --git a/src/main/java/frc/robot/subsystems/Hooks.java b/src/main/java/frc/robot/subsystems/Hooks.java
index eb9c4f7..b6c26c5 100644
--- a/src/main/java/frc/robot/subsystems/Hooks.java
+++ b/src/main/java/frc/robot/subsystems/Hooks.java
@@ -1,8 +1,31 @@
 package frc.robot.subsystems;
 
+import java.lang.constant.Constable;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
+import frc.robot.Constants;
+
+
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Hooks extends SubsystemBase 
 {
-    
+    private final CANSparkMax m_hookL = new CANSparkMax(Constants.hookL_ID, MotorType.kBrushed);
+    private final CANSparkMax m_hookR = new CANSparkMax(Constants.hookR_ID, MotorType.kBrushed);
+
+    public Hooks() 
+    { 
+        // TODO: possibly invert to move forward upon positive votage
+        // TODO: invert one hook
+        m_hookL.setInverted(true);
+    }
+
+    public void atPercentage(double percL, double percR)
+    {
+        m_hookL.set(percL);
+        m_hookR.set(percR);
+    }
+
 }
diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
index 85cbbf8..41620bb 100644
--- a/src/main/java/frc/robot/subsystems/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake.java
@@ -1,17 +1,25 @@
 package frc.robot.subsystems;
 
 import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
 public class Intake extends SubsystemBase 
 {
-    private final CANSparkMax m_intakeT = Constants.intakeT;
-    private final CANSparkMax m_intakeD = Constants.intakeD;
+    public static final CANSparkMax m_intakeT = new CANSparkMax(Constants.intakeT_ID, MotorType.kBrushless);
+    public static final CANSparkMax m_intakeD = new CANSparkMax(Constants.intakeD_ID, MotorType.kBrushless);
+    private final RelativeEncoder m_intakeD_encoder = m_intakeD.getEncoder();
 
-    public Intake() { }
+    public Intake() 
+    {
+        m_intakeT.setInverted(true);
+        m_intakeD.setInverted(true);
+    }
 
     public void atPercentage(double perc) 
     {
@@ -23,12 +31,18 @@ public class Intake extends SubsystemBase
         m_intakeT.set(upPerc);
         m_intakeD.set(downPerc);
     }
+    public void shoot() 
+    {
+        if (m_intakeD_encoder.getVelocity() > 2000) {
+            m_intakeT.set(1);
+        }
+        m_intakeD.set(1);
+    }
 
     @Override
     public void periodic()
     {
         Logger.recordOutput(getName() + "/speedT", m_intakeT.getEncoder().getVelocity());
         Logger.recordOutput(getName() + "/speedD", m_intakeD.getEncoder().getVelocity());
-
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java
index f171049..677073c 100644
--- a/src/main/java/frc/robot/subsystems/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -1,17 +1,19 @@
 package frc.robot.subsystems;
 
 import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
 public class Shooter extends SubsystemBase 
-{
-    private final CANSparkMax m_shooterT = Constants.shooterT;
-    private final CANSparkMax m_shooterD = Constants.shooterD;
+{   
+    public static final CANSparkMax m_shooterT = new CANSparkMax(Constants.shooterT_ID, MotorType.kBrushless);
+    public static final CANSparkMax m_shooterD = new CANSparkMax(Constants.shooterD_ID, MotorType.kBrushless);
 
-    public Shooter() { }
+    public Shooter() {}
 
     public void atPercentage(double perc)
     {
diff --git a/src/main/java/frc/robot/util/IMUGyro.java b/src/main/java/frc/robot/util/IMUGyro.java
index 580af8b..585fdbd 100644
--- a/src/main/java/frc/robot/util/IMUGyro.java
+++ b/src/main/java/frc/robot/util/IMUGyro.java
@@ -23,7 +23,7 @@ public class IMUGyro
  
     private final int PWR_MGMT_1 = 0x6B;
     private final int CONFIG_ADDR = 0x1A;
-    private final int SMPLRT_DIV = 0x19;
+    // private final int SMPLRT_DIV = 0x19;
  
     private final int GYRO_ADDR = 0x43;
     private final int GYRO_CONFIG = 0x1B;