diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml new file mode 100644 index 0000000..5e68b31 --- /dev/null +++ b/.github/workflows/main.yml @@ -0,0 +1,36 @@ +# This is a basic workflow to build robot code. + +name: CI + +# Controls when the action will run. Triggers the workflow on push or pull request +# events but only for the main branch. +on: [push, pull_request, workflow_dispatch] + +# A workflow run is made up of one or more jobs that can run sequentially or in parallel +jobs: + # This workflow contains a single job called "build" + build: + # The type of runner that the job will run on + runs-on: ubuntu-latest + + # This grabs the WPILib docker container + container: wpilib/roborio-cross-ubuntu:2024-22.04 + + # Steps represent a sequence of tasks that will be executed as part of the job + steps: + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v4 + + - name: Update Git settings to set directory as safe + run: git config --global --add safe.directory /__w/2024-Robot/2024-Robot + + - name: Check out the submodule + run: git submodule update --init + + # Grant execute permission for gradlew + - name: Grant execute permission for gradlew + run: chmod +x gradlew + + # Runs a single command using the runners shell + - name: Compile and run tests on robot code + run: ./gradlew build diff --git a/.gitignore b/.gitignore index cdbfbfa..26d4566 100644 --- a/.gitignore +++ b/.gitignore @@ -176,3 +176,5 @@ logs/ # Folder that has CTRE Phoenix Sim device config storage ctre_sim/ + +networktables.json \ No newline at end of file diff --git a/houndutil b/houndutil index 9ce13b0..b58c82d 160000 --- a/houndutil +++ b/houndutil @@ -1 +1 @@ -Subproject commit 9ce13b05d5a8762c364cb454083d5ebc88c39c3f +Subproject commit b58c82deb4089a2559bf21449bc977997eccd133 diff --git a/resources/2024Robot/README.md b/resources/2024Robot/README.md new file mode 100644 index 0000000..0c3d9d7 --- /dev/null +++ b/resources/2024Robot/README.md @@ -0,0 +1,7 @@ +`model.glb`: Drivetrain +`model_0.glb`: Intake +`model_1.glb`: Shooter +`model_2.glb`: Lead screw outer +`model_3.glb`: Lead screw inner +`model_4.glb`: Climber +`model_5.glb`: Note lift \ No newline at end of file diff --git a/resources/2024Robot/config.json b/resources/2024Robot/config.json new file mode 100644 index 0000000..e69de29 diff --git a/resources/2024Robot/model.glb b/resources/2024Robot/model.glb new file mode 100644 index 0000000..2ed64bb Binary files /dev/null and b/resources/2024Robot/model.glb differ diff --git a/resources/2024Robot/model_0.glb b/resources/2024Robot/model_0.glb new file mode 100644 index 0000000..850250c Binary files /dev/null and b/resources/2024Robot/model_0.glb differ diff --git a/resources/2024Robot/model_1.glb b/resources/2024Robot/model_1.glb new file mode 100644 index 0000000..dbed1eb Binary files /dev/null and b/resources/2024Robot/model_1.glb differ diff --git a/resources/2024Robot/model_2.glb b/resources/2024Robot/model_2.glb new file mode 100644 index 0000000..8a36142 Binary files /dev/null and b/resources/2024Robot/model_2.glb differ diff --git a/resources/2024Robot/model_3.glb b/resources/2024Robot/model_3.glb new file mode 100644 index 0000000..d59232e Binary files /dev/null and b/resources/2024Robot/model_3.glb differ diff --git a/resources/2024Robot/model_4.glb b/resources/2024Robot/model_4.glb new file mode 100644 index 0000000..6870718 Binary files /dev/null and b/resources/2024Robot/model_4.glb differ diff --git a/resources/2024Robot/model_5.glb b/resources/2024Robot/model_5.glb new file mode 100644 index 0000000..66e8b05 Binary files /dev/null and b/resources/2024Robot/model_5.glb differ diff --git a/src/main/deploy/choreo/1234.1.traj b/src/main/deploy/choreo/1234.1.traj new file mode 100644 index 0000000..8dfdba4 --- /dev/null +++ b/src/main/deploy/choreo/1234.1.traj @@ -0,0 +1,778 @@ +{ + "samples": [ + { + "x": 1.17792546749115, + "y": 6.456913471221924, + "heading": -2.3204952998605908, + "angularVelocity": 1.0915534190095111e-19, + "velocityX": -5.147338398718608e-19, + "velocityY": 5.503263052262533e-18, + "timestamp": 0 + }, + { + "x": 1.1827150715238435, + "y": 6.457088134209186, + "heading": -2.3224346077509246, + "angularVelocity": -0.0789736445243584, + "velocityX": 0.19504509220825755, + "velocityY": 0.0071127296167531025, + "timestamp": 0.024556393490687705 + }, + { + "x": 1.192295354492295, + "y": 6.457437607941965, + "heading": -2.3262914929987693, + "angularVelocity": -0.15706236542053373, + "velocityX": 0.39013395725576233, + "velocityY": 0.01423147633270856, + "timestamp": 0.04911278698137541 + }, + { + "x": 1.206667489265113, + "y": 6.457962064674032, + "heading": -2.332041946372013, + "angularVelocity": -0.2341733681464424, + "velocityX": 0.5852705845534056, + "velocityY": 0.021357237668724143, + "timestamp": 0.07366918047206311 + }, + { + "x": 1.2258327601765544, + "y": 6.458661699320965, + "heading": -2.3396593139577733, + "angularVelocity": -0.31019895444536805, + "velocityX": 0.7804595132713229, + "velocityY": 0.028490936472328844, + "timestamp": 0.09822557396275082 + }, + { + "x": 1.2497925792060098, + "y": 6.459536727575483, + "heading": -2.3491138418876267, + "angularVelocity": -0.3850128860917086, + "velocityX": 0.9757059414503028, + "velocityY": 0.03563341884264574, + "timestamp": 0.12278196745343853 + }, + { + "x": 1.2785485056119763, + "y": 6.460587384059239, + "heading": -2.3603720996871065, + "angularVelocity": -0.45846544215660834, + "velocityX": 1.171015866677306, + "velocityY": 0.042785455614790954, + "timestamp": 0.14733836094412622 + }, + { + "x": 1.3121022699714886, + "y": 6.461813920680284, + "heading": -2.37339623803971, + "angularVelocity": -0.5303766759374715, + "velocityX": 1.3663962654873019, + "velocityY": 0.04994775073590437, + "timestamp": 0.17189475443481392 + }, + { + "x": 1.3504558037765833, + "y": 6.463216605493996, + "heading": -2.3881430188647683, + "angularVelocity": -0.6005271429882628, + "velocityX": 1.5618553196599922, + "velocityY": 0.0571209617668197, + "timestamp": 0.1964511479255016 + }, + { + "x": 1.3936112759432409, + "y": 6.464795722561273, + "heading": -2.4045625285492975, + "angularVelocity": -0.6686449983282632, + "velocityX": 1.7574026977138553, + "velocityY": 0.06430574049382957, + "timestamp": 0.2210075414161893 + }, + { + "x": 1.4415711377320954, + "y": 6.4665515735936046, + "heading": -2.422596443035158, + "angularVelocity": -0.7343877468285036, + "velocityX": 1.9530498974550927, + "velocityY": 0.07150280569490924, + "timestamp": 0.245563934906877 + }, + { + "x": 1.4943381774964477, + "y": 6.468484482620495, + "heading": -2.4421756455788666, + "angularVelocity": -0.7973158823641606, + "velocityX": 2.1488106461709386, + "velocityY": 0.07871306621733853, + "timestamp": 0.2701203283975647 + }, + { + "x": 1.5519155859304148, + "y": 6.470594805589591, + "heading": -2.4632168842753686, + "angularVelocity": -0.8568537845136457, + "velocityX": 2.344701328222389, + "velocityY": 0.08593782185059054, + "timestamp": 0.29467672188825245 + }, + { + "x": 1.6143070299659465, + "y": 6.4728829478329, + "heading": -2.485617957059904, + "angularVelocity": -0.9122297536497331, + "velocityX": 2.540741337248552, + "velocityY": 0.09317908365400698, + "timestamp": 0.31923311537894017 + }, + { + "x": 1.6815167261349753, + "y": 6.4753493938588065, + "heading": -2.50925054323321, + "angularVelocity": -0.9623801712685371, + "velocityX": 2.7369530543854603, + "velocityY": 0.10044007589479852, + "timestamp": 0.3437895088696279 + }, + { + "x": 1.7535494834234393, + "y": 6.4779947560501325, + "heading": -2.5339490739505, + "angularVelocity": -1.0057881963267985, + "velocityX": 2.9333606058959827, + "velocityY": 0.10772600595153933, + "timestamp": 0.3683459023603156 + }, + { + "x": 1.8304106245820337, + "y": 6.480819851058323, + "heading": -2.5594924852596286, + "angularVelocity": -1.0401939241939082, + "velocityX": 3.1299849136128906, + "velocityY": 0.11504519217211771, + "timestamp": 0.39290229585100334 + }, + { + "x": 1.9121055005220093, + "y": 6.483825811723467, + "heading": -2.5855720493348286, + "angularVelocity": -1.0620274546866881, + "velocityX": 3.3268271243069956, + "velocityY": 0.12241051057780712, + "timestamp": 0.41745868934169106 + }, + { + "x": 1.9986376079459907, + "y": 6.487014227014825, + "heading": -2.6117286672590567, + "angularVelocity": -1.0651652871643127, + "velocityX": 3.523811729796402, + "velocityY": 0.1298405359308217, + "timestamp": 0.4420150828323788 + }, + { + "x": 2.0900011902761935, + "y": 6.490387314111071, + "heading": -2.6372111268909095, + "angularVelocity": -1.037711813891415, + "velocityX": 3.720561912515762, + "velocityY": 0.1373608505469304, + "timestamp": 0.4665714763230665 + }, + { + "x": 2.186144606759753, + "y": 6.493950905854893, + "heading": -2.6605668053733122, + "angularVelocity": -0.9511037722725775, + "velocityX": 3.9152091499111883, + "velocityY": 0.1451186936377184, + "timestamp": 0.4911278698137542 + }, + { + "x": 2.2866032796675606, + "y": 6.497891465085372, + "heading": -2.6774789394698564, + "angularVelocity": -0.6887059414061727, + "velocityX": 4.090937577861477, + "velocityY": 0.16046978689984315, + "timestamp": 0.515684263304442 + }, + { + "x": 2.390651909884553, + "y": 6.502515861566495, + "heading": -2.6841281768726843, + "angularVelocity": -0.2707741837313909, + "velocityX": 4.237129945667706, + "velocityY": 0.188317412444054, + "timestamp": 0.5402406567951297 + }, + { + "x": 2.4970494575830844, + "y": 6.506375954880804, + "heading": -2.6841282155484834, + "angularVelocity": -0.000001574978804933473, + "velocityX": 4.3327839545688835, + "velocityY": 0.1571930062031463, + "timestamp": 0.5647970502858174 + }, + { + "x": 2.6034474806695886, + "y": 6.510222923143476, + "heading": -2.6841282541506195, + "angularVelocity": -0.0000015719790591645795, + "velocityX": 4.332803313599462, + "velocityY": 0.15665852007669273, + "timestamp": 0.5893534437765051 + }, + { + "x": 2.709845511835245, + "y": 6.51406966794852, + "heading": -2.684128292752748, + "angularVelocity": -0.0000015719787382653465, + "velocityX": 4.332803642603493, + "velocityY": 0.15664942030278362, + "timestamp": 0.6139098372671928 + }, + { + "x": 2.8162435409813273, + "y": 6.517916468613033, + "heading": -2.6841283313548785, + "angularVelocity": -0.0000015719788238959483, + "velocityX": 4.3328035603611985, + "velocityY": 0.15665169504528215, + "timestamp": 0.6384662307578806 + }, + { + "x": 2.922641569756633, + "y": 6.521763279532346, + "heading": -2.6841283699570098, + "angularVelocity": -0.0000015719788438017344, + "velocityX": 4.332803545262214, + "velocityY": 0.15665211264722553, + "timestamp": 0.6630226242485683 + }, + { + "x": 3.029039598618472, + "y": 6.525610088058124, + "heading": -2.6841284085591406, + "angularVelocity": -0.0000015719788449275249, + "velocityX": 4.332803548786091, + "velocityY": 0.15665201517631638, + "timestamp": 0.687579017739256 + }, + { + "x": 3.1354376275492, + "y": 6.52945689467851, + "heading": -2.684128447161272, + "angularVelocity": -0.0000015719788459691416, + "velocityX": 4.33280355159142, + "velocityY": 0.1566519375837843, + "timestamp": 0.7121354112299437 + }, + { + "x": 3.2418356565025146, + "y": 6.533303700674159, + "heading": -2.684128485763403, + "angularVelocity": -0.000001571978849857335, + "velocityX": 4.332803552511197, + "velocityY": 0.15665191214291052, + "timestamp": 0.7366918047206314 + }, + { + "x": 3.3482336854320547, + "y": 6.53715050732732, + "heading": -2.684128524365535, + "angularVelocity": -0.0000015719788550374975, + "velocityX": 4.332803551543035, + "velocityY": 0.15665193891849358, + "timestamp": 0.7612481982113192 + }, + { + "x": 3.4546317141846608, + "y": 6.54099731887419, + "heading": -2.684128562967666, + "angularVelocity": -0.0000015719788669289655, + "velocityX": 4.33280354433783, + "velocityY": 0.15665213820297527, + "timestamp": 0.7858045917020069 + }, + { + "x": 3.5610297424845294, + "y": 6.544844142943354, + "heading": -2.6841286015697983, + "angularVelocity": -0.0000015719788893437079, + "velocityX": 4.332803525901206, + "velocityY": 0.15665264814328536, + "timestamp": 0.8103609851926946 + }, + { + "x": 3.667427770171028, + "y": 6.548690983979332, + "heading": -2.6841286401719313, + "angularVelocity": -0.0000015719789187769318, + "velocityX": 4.332803500923143, + "velocityY": 0.15665333907592197, + "timestamp": 0.8349173786833823 + }, + { + "x": 3.7738258011124195, + "y": 6.5525377349997225, + "heading": -2.6841286787740612, + "angularVelocity": -0.0000015719787926702145, + "velocityX": 4.332803633470852, + "velocityY": 0.15664967340792557, + "timestamp": 0.8594737721740701 + }, + { + "x": 3.8802238716303594, + "y": 6.556383391212679, + "heading": -2.684128717376152, + "angularVelocity": -0.0000015719771994988576, + "velocityX": 4.332805245130483, + "velocityY": 0.15660509001120598, + "timestamp": 0.8840301656647578 + }, + { + "x": 3.986621456193882, + "y": 6.560242469781829, + "heading": -2.6841287559787252, + "angularVelocity": -0.0000015719968565290703, + "velocityX": 4.332785455806911, + "velocityY": 0.15715168314906297, + "timestamp": 0.9085865591554455 + }, + { + "x": 4.093016902723116, + "y": 6.56416005056415, + "heading": -2.6841287945844416, + "angularVelocity": -0.0000015721248440196224, + "velocityX": 4.33269838950829, + "velocityY": 0.1595340449242531, + "timestamp": 0.9331429526461332 + }, + { + "x": 4.199217262171535, + "y": 6.571698095232862, + "heading": -2.6841288334335762, + "angularVelocity": -0.0000015820374694756593, + "velocityX": 4.324753937857032, + "velocityY": 0.30696871963591016, + "timestamp": 0.957699346136821 + }, + { + "x": 4.304968357086182, + "y": 6.584028720855713, + "heading": -2.6841288728935164, + "angularVelocity": -0.000001606911063517827, + "velocityX": 4.306458721422164, + "velocityY": 0.5021350398025036, + "timestamp": 0.9822557396275087 + }, + { + "x": 4.414700482208155, + "y": 6.602139612854462, + "heading": -2.6841289106192554, + "angularVelocity": -0.0000014706882277086669, + "velocityX": 4.277762279149216, + "velocityY": 0.7060292557703594, + "timestamp": 1.0079074981781961 + }, + { + "x": 4.523455429440765, + "y": 6.625409877143289, + "heading": -2.6841289481054, + "angularVelocity": -0.0000014613479251873946, + "velocityX": 4.23966828697975, + "velocityY": 0.907160584832358, + "timestamp": 1.0335592567288836 + }, + { + "x": 4.631457869220697, + "y": 6.651954495449144, + "heading": -2.684128985593184, + "angularVelocity": -0.0000014614118405678238, + "velocityX": 4.210332775685651, + "velocityY": 1.0348069608325312, + "timestamp": 1.059211015279571 + }, + { + "x": 4.739457727470495, + "y": 6.6785096150501175, + "heading": -2.684129023081148, + "angularVelocity": -0.00000146141888775809, + "velocityX": 4.210232138135551, + "velocityY": 1.0352163399831225, + "timestamp": 1.0848627738302585 + }, + { + "x": 4.847457616086101, + "y": 6.705064611153251, + "heading": -2.684129060569109, + "angularVelocity": -0.0000014614187615548696, + "velocityX": 4.2102333219064825, + "velocityY": 1.035211525582661, + "timestamp": 1.110514532380946 + }, + { + "x": 4.955457519849879, + "y": 6.7316195456479555, + "heading": -2.6841290980570682, + "angularVelocity": -0.000001461418696152274, + "velocityX": 4.210233912438069, + "velocityY": 1.035209123859201, + "timestamp": 1.1361662909316335 + }, + { + "x": 5.063457420443, + "y": 6.758174493037793, + "heading": -2.684129135545028, + "angularVelocity": -0.0000014614187142453692, + "velocityX": 4.210233788834204, + "velocityY": 1.0352096265589994, + "timestamp": 1.161818049482321 + }, + { + "x": 5.171457321226551, + "y": 6.784729439653125, + "heading": -2.6841291730329884, + "angularVelocity": -0.0000014614187176590148, + "velocityX": 4.210233796257874, + "velocityY": 1.0352095963658907, + "timestamp": 1.1874698080330084 + }, + { + "x": 5.279457221587324, + "y": 6.811284387987912, + "heading": -2.6841292105209487, + "angularVelocity": -0.0000014614187200214774, + "velocityX": 4.210233779776412, + "velocityY": 1.035209663396565, + "timestamp": 1.2131215665836959 + }, + { + "x": 5.387457121765594, + "y": 6.837839337064946, + "heading": -2.6841292480089085, + "angularVelocity": -0.0000014614187249678457, + "velocityX": 4.210233772661787, + "velocityY": 1.0352096923321552, + "timestamp": 1.2387733251343833 + }, + { + "x": 5.495457021941072, + "y": 6.864394286153341, + "heading": -2.6841292854968692, + "angularVelocity": -0.0000014614187291337482, + "velocityX": 4.210233772552937, + "velocityY": 1.0352096927749588, + "timestamp": 1.2644250836850708 + }, + { + "x": 5.603456922143669, + "y": 6.890949235131444, + "heading": -2.6841293229848295, + "angularVelocity": -0.000001461418731009078, + "velocityX": 4.210233773610118, + "velocityY": 1.035209688475399, + "timestamp": 1.2900768422357582 + }, + { + "x": 5.711456822363459, + "y": 6.917504184039622, + "heading": -2.6841293604727903, + "angularVelocity": -0.0000014614187365607823, + "velocityX": 4.210233774280365, + "velocityY": 1.035209685749491, + "timestamp": 1.3157286007864457 + }, + { + "x": 5.819456722592084, + "y": 6.944059132911867, + "heading": -2.6841293979607506, + "angularVelocity": -0.0000014614187388310343, + "velocityX": 4.2102337746248, + "velocityY": 1.0352096843486585, + "timestamp": 1.3413803593371332 + }, + { + "x": 5.927456622827952, + "y": 6.970614081754651, + "heading": -2.6841294354487113, + "angularVelocity": -0.0000014614187405368475, + "velocityX": 4.210233774907183, + "velocityY": 1.035209683200198, + "timestamp": 1.3670321178878206 + }, + { + "x": 6.035456523069077, + "y": 6.997169030576061, + "heading": -2.684129472936672, + "angularVelocity": -0.0000014614187445336427, + "velocityX": 4.210233775112074, + "velocityY": 1.0352096823669164, + "timestamp": 1.392683876438508 + }, + { + "x": 6.143456423285194, + "y": 7.023723979499181, + "heading": -2.6841295104246328, + "angularVelocity": -0.0000014614187497197174, + "velocityX": 4.210233774137161, + "velocityY": 1.0352096863319555, + "timestamp": 1.4183356349891956 + }, + { + "x": 6.251456323349948, + "y": 7.050278929037894, + "heading": -2.684129547912594, + "angularVelocity": -0.0000014614187535451282, + "velocityX": 4.210233768236523, + "velocityY": 1.0352097103300821, + "timestamp": 1.443987393539883 + }, + { + "x": 6.359456223124247, + "y": 7.076833879757894, + "heading": -2.6841295854005547, + "angularVelocity": -0.000001461418756010585, + "velocityX": 4.210233756913517, + "velocityY": 1.0352097563809437, + "timestamp": 1.4696391520905705 + }, + { + "x": 6.467456123835202, + "y": 7.103388826668465, + "heading": -2.6841296228885163, + "angularVelocity": -0.0000014614187566390387, + "velocityX": 4.210233793427791, + "velocityY": 1.035209607875408, + "timestamp": 1.495290910641258 + }, + { + "x": 6.57545602902077, + "y": 7.129943755380618, + "heading": -2.6841296603764766, + "angularVelocity": -0.000001461418738378337, + "velocityX": 4.21023396786468, + "velocityY": 1.0352088984340948, + "timestamp": 1.5209426691919454 + }, + { + "x": 6.6834559198048344, + "y": 7.156498742664419, + "heading": -2.6841296978644387, + "angularVelocity": -0.0000014614188054188506, + "velocityX": 4.210233406441041, + "velocityY": 1.0352111817725804, + "timestamp": 1.546594427742633 + }, + { + "x": 6.791453813817713, + "y": 7.183061849470797, + "heading": -2.6841297354229963, + "angularVelocity": -0.0000014641708570784773, + "velocityX": 4.210155564947969, + "velocityY": 1.0355277106593013, + "timestamp": 1.5722461862933204 + }, + { + "x": 6.896786766452706, + "y": 7.209560411118278, + "heading": -2.6924335459211086, + "angularVelocity": -0.32371310846796236, + "velocityX": 4.106266337524453, + "velocityY": 1.0330115027053868, + "timestamp": 1.5978979448440078 + }, + { + "x": 6.998488316252308, + "y": 7.234843401534966, + "heading": -2.7129985246425123, + "angularVelocity": -0.8016985923506064, + "velocityX": 3.9647008839039692, + "velocityY": 0.9856240603048587, + "timestamp": 1.6235497033946953 + }, + { + "x": 7.095354057099806, + "y": 7.258847333068417, + "heading": -2.738625094492488, + "angularVelocity": -0.9990180516995718, + "velocityX": 3.776183245140594, + "velocityY": 0.9357616354457536, + "timestamp": 1.6492014619453828 + }, + { + "x": 7.187121562044359, + "y": 7.2815620826343475, + "heading": -2.765681380196159, + "angularVelocity": -1.0547536399973723, + "velocityX": 3.577435237557771, + "velocityY": 0.8855045754873788, + "timestamp": 1.6748532204960702 + }, + { + "x": 7.27376588306665, + "y": 7.302992641940746, + "heading": -2.7929553754669367, + "angularVelocity": -1.063240760546102, + "velocityX": 3.3777146643214673, + "velocityY": 0.8354421106862051, + "timestamp": 1.7005049790467577 + }, + { + "x": 7.355287023426883, + "y": 7.32314481942894, + "heading": -2.819817295961529, + "angularVelocity": -1.047176568480221, + "velocityX": 3.1779942181799417, + "velocityY": 0.785606080315067, + "timestamp": 1.7261567375974451 + }, + { + "x": 7.431689467162592, + "y": 7.342023303561365, + "heading": -2.8458760006675994, + "angularVelocity": -1.0158642595430196, + "velocityX": 2.978448576332041, + "velocityY": 0.7359528234729867, + "timestamp": 1.7518084961481326 + }, + { + "x": 7.502978255871904, + "y": 7.359631767530785, + "heading": -2.870863472244511, + "angularVelocity": -0.9741036478078832, + "velocityX": 2.7790994745427215, + "velocityY": 0.686442761209752, + "timestamp": 1.77746025469882 + }, + { + "x": 7.5691580661290105, + "y": 7.375973138048295, + "heading": -2.8945841408031066, + "angularVelocity": -0.9247190016904283, + "velocityX": 2.5799326828348144, + "velocityY": 0.6370467929214264, + "timestamp": 1.8031120132495075 + }, + { + "x": 7.630233019820151, + "y": 7.391049803070599, + "heading": -2.9168889167473195, + "angularVelocity": -0.869522294159258, + "velocityX": 2.3809265774296597, + "velocityY": 0.5877439159780639, + "timestamp": 1.828763771800195 + }, + { + "x": 7.686206695576408, + "y": 7.404863754066865, + "heading": -2.9376604219188827, + "angularVelocity": -0.809749753823667, + "velocityX": 2.182059980240895, + "velocityY": 0.5385186738355405, + "timestamp": 1.8544155303508825 + }, + { + "x": 7.737082192643966, + "y": 7.417416682243981, + "heading": -2.95680392542072, + "angularVelocity": -0.7462842543137888, + "velocityX": 1.9833142030800017, + "velocityY": 0.489359361164762, + "timestamp": 1.88006728890157 + }, + { + "x": 7.782862202147126, + "y": 7.428710044654634, + "heading": -2.974241445014127, + "angularVelocity": -0.6797787200027948, + "velocityX": 1.784673335853397, + "velocityY": 0.4402568497726161, + "timestamp": 1.9057190474522574 + }, + { + "x": 7.823549072317337, + "y": 7.43874511075604, + "heading": -2.989907725307504, + "angularVelocity": -0.6107292902519902, + "velocityX": 1.5861240113348993, + "velocityY": 0.3912038264969958, + "timestamp": 1.9313708060029449 + }, + { + "x": 7.85914486452642, + "y": 7.447522996121348, + "heading": -3.0037473896814832, + "angularVelocity": -0.5395210759773936, + "velocityX": 1.3876550466801778, + "velocityY": 0.3421942923703846, + "timestamp": 1.9570225645536323 + }, + { + "x": 7.889651400291962, + "y": 7.45504468753028, + "heading": -3.015712859637522, + "angularVelocity": -0.46645807664200445, + "velocityX": 1.18925709148796, + "velocityY": 0.2932232265506935, + "timestamp": 1.9826743231043198 + }, + { + "x": 7.915070300383151, + "y": 7.4613110621213785, + "heading": -3.025762795293749, + "angularVelocity": -0.391783496494747, + "velocityX": 0.9909223198464887, + "velocityY": 0.24428635482108035, + "timestamp": 2.0083260816550075 + }, + { + "x": 7.935403017300441, + "y": 7.466322902324817, + "heading": -3.0338609016195863, + "angularVelocity": -0.31569400241450996, + "velocityX": 0.7926441720208706, + "velocityY": 0.1953799851007025, + "timestamp": 2.033977840205695 + }, + { + "x": 7.950650862283985, + "y": 7.470080907688823, + "heading": -3.0399749988944915, + "angularVelocity": -0.23835002433943484, + "velocityX": 0.5944171411646091, + "velocityY": 0.14650088634584787, + "timestamp": 2.0596295987563824 + }, + { + "x": 7.960815027821627, + "y": 7.472585704328216, + "heading": -3.0440762890820414, + "angularVelocity": -0.159883392768021, + "velocityX": 0.3962365978752573, + "velocityY": 0.09764619585213934, + "timestamp": 2.08528135730707 + }, + { + "x": 7.965896606445312, + "y": 7.473837852478027, + "heading": -3.046138770984786, + "angularVelocity": -0.0804031387816541, + "velocityX": 0.1980986455039259, + "velocityY": 0.04881334538283516, + "timestamp": 2.1109331158577573 + }, + { + "x": 7.965896606445312, + "y": 7.473837852478027, + "heading": -3.046138770984786, + "angularVelocity": 2.257846198921834e-19, + "velocityX": 2.7831811723292994e-20, + "velocityY": -2.187862053904655e-18, + "timestamp": 2.136584874408445 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/1234.2.traj b/src/main/deploy/choreo/1234.2.traj new file mode 100644 index 0000000..5cb8511 --- /dev/null +++ b/src/main/deploy/choreo/1234.2.traj @@ -0,0 +1,121 @@ +{ + "samples": [ + { + "x": 7.965896606445312, + "y": 7.473837852478027, + "heading": -3.046138770984786, + "angularVelocity": 2.257846198921834e-19, + "velocityX": 2.7831811723292994e-20, + "velocityY": -2.187862053904655e-18, + "timestamp": 0 + }, + { + "x": 7.895295442516664, + "y": 7.453869842194936, + "heading": -3.045310713379553, + "angularVelocity": 0.00866143460185216, + "velocityX": -0.7384840865157103, + "velocityY": -0.20886423130853593, + "timestamp": 0.09560282370031192 + }, + { + "x": 7.754093116683618, + "y": 7.413933822201269, + "heading": -3.0436545985981667, + "angularVelocity": 0.0173228647155626, + "velocityX": -1.4769681518578985, + "velocityY": -0.41772845662859964, + "timestamp": 0.19120564740062385 + }, + { + "x": 7.542289632319933, + "y": 7.35402979345122, + "heading": -3.041170427348722, + "angularVelocity": 0.02598428742263653, + "velocityX": -2.2154521819107464, + "velocityY": -0.6265926719678376, + "timestamp": 0.28680847110093577 + }, + { + "x": 7.259884996173116, + "y": 7.274157757853176, + "heading": -3.0378582010749082, + "angularVelocity": 0.034645695028804865, + "velocityX": -2.953936141385066, + "velocityY": -0.8354568673454674, + "timestamp": 0.3824112948012477 + }, + { + "x": 6.906879228485775, + "y": 7.17431772113232, + "heading": -3.033717924090085, + "angularVelocity": 0.04330705751748156, + "velocityX": -3.692419889122889, + "velocityY": -1.0443210028380143, + "timestamp": 0.4780141185015596 + }, + { + "x": 6.508025842761737, + "y": 7.061510678522763, + "heading": -3.0337179232569076, + "angularVelocity": 8.714985822174448e-9, + "velocityX": -4.171983318969032, + "velocityY": -1.1799551335761433, + "timestamp": 0.5736169422018715 + }, + { + "x": 6.155020075077593, + "y": 6.961670641802811, + "heading": -3.0295776429996932, + "angularVelocity": 0.0433070917465069, + "velocityX": -3.692419889089439, + "velocityY": -1.044321002828554, + "timestamp": 0.6692197659021835 + }, + { + "x": 5.872615438933694, + "y": 6.881798606205592, + "heading": -3.0262654137382636, + "angularVelocity": 0.034645726279091356, + "velocityX": -2.953936141354553, + "velocityY": -0.8354568673368374, + "timestamp": 0.7648225896024954 + }, + { + "x": 5.660811954572396, + "y": 6.821894577456218, + "heading": -3.023781240040345, + "angularVelocity": 0.025984313033535218, + "velocityX": -2.215452181885791, + "velocityY": -0.6265926719607796, + "timestamp": 0.8604254133028073 + }, + { + "x": 5.519609628741025, + "y": 6.781958557463025, + "heading": -3.022125123534869, + "angularVelocity": 0.017322882749443342, + "velocityX": -1.4769681518403805, + "velocityY": -0.41772845662364505, + "timestamp": 0.9560282370031192 + }, + { + "x": 5.449008464813232, + "y": 6.761990547180176, + "heading": -3.0212970650452653, + "angularVelocity": 0.008661443852320889, + "velocityX": -0.7384840865067626, + "velocityY": -0.20886423130600526, + "timestamp": 1.0516310607034312 + }, + { + "x": 5.449008464813232, + "y": 6.761990547180176, + "heading": -3.0212970650452653, + "angularVelocity": 6.236868827474949e-28, + "velocityX": 9.645303873884742e-19, + "velocityY": -1.3645963517294365e-18, + "timestamp": 1.147233884403743 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/1234.3.traj b/src/main/deploy/choreo/1234.3.traj new file mode 100644 index 0000000..f2539af --- /dev/null +++ b/src/main/deploy/choreo/1234.3.traj @@ -0,0 +1,130 @@ +{ + "samples": [ + { + "x": 5.449008464813232, + "y": 6.761990547180176, + "heading": -3.0212970650452653, + "angularVelocity": 6.236868827474949e-28, + "velocityX": 9.645303873884742e-19, + "velocityY": -1.3645963517294365e-18, + "timestamp": 0 + }, + { + "x": 5.506534539725527, + "y": 6.740622006309749, + "heading": -3.025242040367869, + "angularVelocity": -0.045114414232386484, + "velocityX": 0.6578634745536448, + "velocityY": -0.2443688808004527, + "timestamp": 0.0874437890799844 + }, + { + "x": 5.621586688038202, + "y": 6.697884925130509, + "heading": -3.0331320221125657, + "angularVelocity": -0.09022918411597865, + "velocityX": 1.3157269318171694, + "velocityY": -0.4887377551783459, + "timestamp": 0.1748875781599688 + }, + { + "x": 5.7941649073731085, + "y": 6.633779304525839, + "heading": -3.044967072221135, + "angularVelocity": -0.13534466236068432, + "velocityX": 1.9735903618843658, + "velocityY": -0.7331066194539356, + "timestamp": 0.2623313672399532 + }, + { + "x": 6.024269193849061, + "y": 6.548305145937437, + "heading": -3.060747273757664, + "angularVelocity": -0.1804610905194783, + "velocityX": 2.6314537475666477, + "velocityY": -0.9774754672423787, + "timestamp": 0.3497751563199376 + }, + { + "x": 6.311899540184541, + "y": 6.441462452070085, + "heading": -3.0804726993222316, + "angularVelocity": -0.2255783489268115, + "velocityX": 3.2893170499780955, + "velocityY": -1.2218442840991746, + "timestamp": 0.437218945399922 + }, + { + "x": 6.657055926215122, + "y": 6.313251230414028, + "heading": -3.1041432514952634, + "angularVelocity": -0.2706944932518914, + "velocityX": 3.9471801217907987, + "velocityY": -1.4662130152981419, + "timestamp": 0.5246627344799064 + }, + { + "x": 7.002212276855087, + "y": 6.185040021904109, + "heading": -3.1278203147498056, + "angularVelocity": -0.27076895344602725, + "velocityX": 3.9471797170666147, + "velocityY": -1.4662128649599642, + "timestamp": 0.6121065235598908 + }, + { + "x": 7.289842590220327, + "y": 6.078197340283827, + "heading": -3.1475518152563606, + "angularVelocity": -0.2256478214651337, + "velocityX": 3.289316672933121, + "velocityY": -1.2218441440426708, + "timestamp": 0.6995503126398752 + }, + { + "x": 7.5199468481440634, + "y": 5.992723192301384, + "heading": -3.1633372870726673, + "angularVelocity": -0.18052136100675872, + "velocityX": 2.631453421045854, + "velocityY": -0.9774753459535034, + "timestamp": 0.7869941017198596 + }, + { + "x": 7.692525044903304, + "y": 5.9286175800826335, + "heading": -3.1751765140759507, + "angularVelocity": -0.135392428986058, + "velocityX": 1.9735901037109, + "velocityY": -0.7331065235532462, + "timestamp": 0.8744378907998436 + }, + { + "x": 7.807577177702952, + "y": 5.8858805046658365, + "heading": -3.1830693746554464, + "angularVelocity": -0.09026210623462518, + "velocityX": 1.3157267544114646, + "velocityY": -0.48873768927951544, + "timestamp": 0.9618816798798275 + }, + { + "x": 7.865103244781494, + "y": 5.864511966705322, + "heading": -3.1870158092601475, + "angularVelocity": -0.04513110246276267, + "velocityX": 0.6578633849674967, + "velocityY": -0.24436884752292995, + "timestamp": 1.0493254689598115 + }, + { + "x": 7.865103244781494, + "y": 5.864511966705322, + "heading": -3.1870158092601475, + "angularVelocity": -3.432876184233931e-28, + "velocityX": 4.443954479240038e-19, + "velocityY": 1.2242859016477994e-18, + "timestamp": 1.1367692580397954 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/1234.4.traj b/src/main/deploy/choreo/1234.4.traj new file mode 100644 index 0000000..31a2075 --- /dev/null +++ b/src/main/deploy/choreo/1234.4.traj @@ -0,0 +1,130 @@ +{ + "samples": [ + { + "x": 7.865103244781494, + "y": 5.864511966705322, + "heading": -3.1870158092601475, + "angularVelocity": -3.432876184233931e-28, + "velocityX": 4.443954479240038e-19, + "velocityY": 1.2242859016477994e-18, + "timestamp": 0 + }, + { + "x": 7.80727449997893, + "y": 5.886485815257223, + "heading": -3.1831983186843216, + "angularVelocity": 0.043481495025401884, + "velocityX": -0.6586736049542378, + "velocityY": 0.2502837315562369, + "timestamp": 0.08779575250564697 + }, + { + "x": 7.6916170112977955, + "y": 5.930433512009924, + "heading": -3.1755634246058455, + "angularVelocity": 0.08696199828100887, + "velocityX": -1.3173471993841017, + "velocityY": 0.5005674591134074, + "timestamp": 0.17559150501129395 + }, + { + "x": 7.518130780033237, + "y": 5.996355056471295, + "heading": -3.164111270762368, + "angularVelocity": 0.1304408643543523, + "velocityX": -1.9760207790621667, + "velocityY": 0.7508511810651682, + "timestamp": 0.2633872575169409 + }, + { + "x": 7.286815808414609, + "y": 6.084250447794221, + "heading": -3.1488420788016804, + "angularVelocity": 0.17391720584325185, + "velocityX": -2.6346943333477326, + "velocityY": 1.0011348933682513, + "timestamp": 0.3511830100225879 + }, + { + "x": 6.997672101306374, + "y": 6.194119684130298, + "heading": -3.1297562098801364, + "angularVelocity": 0.2173894337350346, + "velocityX": -3.2933678322267044, + "velocityY": 1.2514185846178583, + "timestamp": 0.43897876252823487 + }, + { + "x": 6.650699674750629, + "y": 6.325962759383826, + "heading": -3.1068544665735547, + "angularVelocity": 0.2608525202299327, + "velocityX": -3.9520411483850135, + "velocityY": 1.501702206437008, + "timestamp": 0.5267745150338818 + }, + { + "x": 6.303727218542563, + "y": 6.457805845904685, + "heading": -3.083958400156642, + "angularVelocity": 0.26078786004414234, + "velocityX": -3.952041486127124, + "velocityY": 1.5017023347727347, + "timestamp": 0.6145702675395288 + }, + { + "x": 6.0145834840631185, + "y": 6.567675092641313, + "heading": -3.0648777920053, + "angularVelocity": 0.21732951318020463, + "velocityX": -3.293368143986769, + "velocityY": 1.2514187030808899, + "timestamp": 0.7023660200451758 + }, + { + "x": 5.783268488823356, + "y": 6.655570492939832, + "heading": -3.0496131525042647, + "angularVelocity": 0.17386535299704567, + "velocityX": -2.6346946023941684, + "velocityY": 1.0011349956008981, + "timestamp": 0.7901617725508228 + }, + { + "x": 5.609782238915005, + "y": 6.721492044485497, + "heading": -3.0381646021766326, + "angularVelocity": 0.13039982004705136, + "velocityX": -1.9760209914163132, + "velocityY": 0.7508512617557984, + "timestamp": 0.8779575250564697 + }, + { + "x": 5.494124737436081, + "y": 6.76543974610112, + "heading": -3.0305321899890547, + "angularVelocity": 0.08693372936335396, + "velocityX": -1.3173473451518534, + "velocityY": 0.5005675145024446, + "timestamp": 0.9657532775621167 + }, + { + "x": 5.436295986175537, + "y": 6.787413597106934, + "heading": -3.0267159569026636, + "angularVelocity": 0.04346717212937656, + "velocityX": -0.6586736785110937, + "velocityY": 0.2502837595064755, + "timestamp": 1.0535490300677637 + }, + { + "x": 5.436295986175537, + "y": 6.787413597106934, + "heading": -3.0267159569026636, + "angularVelocity": -3.974435126391844e-21, + "velocityX": 8.842380334460247e-19, + "velocityY": 2.3059926136011616e-18, + "timestamp": 1.1413447825734107 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/1234.5.traj b/src/main/deploy/choreo/1234.5.traj new file mode 100644 index 0000000..af11c1e --- /dev/null +++ b/src/main/deploy/choreo/1234.5.traj @@ -0,0 +1,229 @@ +{ + "samples": [ + { + "x": 5.436295986175537, + "y": 6.787413597106934, + "heading": -3.0267159569026636, + "angularVelocity": -3.974435126391844e-21, + "velocityX": 8.842380334460247e-19, + "velocityY": 2.3059926136011616e-18, + "timestamp": 0 + }, + { + "x": 5.4614010785364835, + "y": 6.770412417602454, + "heading": -3.0386899363107447, + "angularVelocity": -0.19391287497961154, + "velocityX": 0.40656497480315146, + "velocityY": -0.2753259783905342, + "timestamp": 0.06174927481911574 + }, + { + "x": 5.5116279180197765, + "y": 6.736396670320351, + "heading": -3.062194055668532, + "angularVelocity": -0.3806379820109448, + "velocityX": 0.8133996655090318, + "velocityY": -0.5508687734673186, + "timestamp": 0.12349854963823148 + }, + { + "x": 5.586997122869661, + "y": 6.685349278958885, + "heading": -3.0966469418180393, + "angularVelocity": -0.5579480285465932, + "velocityX": 1.2205682588284168, + "velocityY": -0.8266881110912201, + "timestamp": 0.18524782445734722 + }, + { + "x": 5.687534779910401, + "y": 6.6172475013120335, + "heading": -3.1412471867595344, + "angularVelocity": -0.7222796554638842, + "velocityX": 1.6281593158016558, + "velocityY": -1.102875747874692, + "timestamp": 0.24699709927646296 + }, + { + "x": 5.813274852979219, + "y": 6.532060138458457, + "heading": -3.1948134699284605, + "angularVelocity": -0.8674803603093312, + "velocityX": 2.036300400889785, + "velocityY": -1.3795686362814745, + "timestamp": 0.3087463740955787 + }, + { + "x": 5.964262125973882, + "y": 6.4297440605952545, + "heading": -3.2554414713381568, + "angularVelocity": -0.9818415129132555, + "velocityX": 2.4451667398031227, + "velocityY": -1.6569599912374746, + "timestamp": 0.37049564891469444 + }, + { + "x": 6.140548631500476, + "y": 6.3102434255545505, + "heading": -3.3196239769413203, + "angularVelocity": -1.0394050098754253, + "velocityX": 2.854875721909237, + "velocityY": -1.9352556834191568, + "timestamp": 0.4322449237338102 + }, + { + "x": 6.34209645810039, + "y": 6.173539563313386, + "heading": -3.379002265616315, + "angularVelocity": -0.9616030123257975, + "velocityX": 3.263970746058413, + "velocityY": -2.213853727701531, + "timestamp": 0.4939941985529259 + }, + { + "x": 6.565003089251376, + "y": 6.025257004399179, + "heading": -3.3790032986592844, + "angularVelocity": -0.00001672963727023266, + "velocityX": 3.609866379871756, + "velocityY": -2.4013651876653985, + "timestamp": 0.5557434733720417 + }, + { + "x": 6.779539733581947, + "y": 5.865103058909337, + "heading": -3.379003341176066, + "angularVelocity": -6.885389656783489e-7, + "velocityX": 3.4743184427512874, + "velocityY": -2.5936166207455313, + "timestamp": 0.6174927481911574 + }, + { + "x": 6.97439432144165, + "y": 5.681508541107178, + "heading": -3.379003385149404, + "angularVelocity": -7.121271958295491e-7, + "velocityX": 3.1555769428952645, + "velocityY": -2.973225488719833, + "timestamp": 0.6792420230102731 + }, + { + "x": 7.122909407779235, + "y": 5.511762371505047, + "heading": -3.3790034194155005, + "angularVelocity": -6.586950870648493e-7, + "velocityX": 2.8548964763469873, + "velocityY": -3.2630202992911737, + "timestamp": 0.7312632040121212 + }, + { + "x": 7.254405341042785, + "y": 5.328515780926305, + "heading": -3.379003452128528, + "angularVelocity": -6.288405402556099e-7, + "velocityX": 2.527738331409253, + "velocityY": -3.5225380710259566, + "timestamp": 0.7832843850139692 + }, + { + "x": 7.369249923871109, + "y": 5.146276495162188, + "heading": -3.406177877810794, + "angularVelocity": -0.5223723329407031, + "velocityX": 2.207650434238391, + "velocityY": -3.503174711809081, + "timestamp": 0.8353055660158173 + }, + { + "x": 7.472952308705079, + "y": 4.981547414238825, + "heading": -3.4515343756544477, + "angularVelocity": -0.8718852008000858, + "velocityX": 1.9934646395337656, + "velocityY": -3.1665771086110306, + "timestamp": 0.8873267470176653 + }, + { + "x": 7.565086623545703, + "y": 4.835192956514777, + "heading": -3.4989015182220933, + "angularVelocity": -0.9105357021010838, + "velocityX": 1.771092333281533, + "velocityY": -2.8133628438548675, + "timestamp": 0.9393479280195134 + }, + { + "x": 7.645656925903314, + "y": 4.707210342234374, + "heading": -3.5438918663970616, + "angularVelocity": -0.864846727977402, + "velocityX": 1.5487980243037662, + "velocityY": -2.460202014172973, + "timestamp": 0.9913691090213614 + }, + { + "x": 7.714683601406547, + "y": 4.597567978647708, + "heading": -3.584524535930262, + "angularVelocity": -0.7810793363525659, + "velocityX": 1.3268955870260062, + "velocityY": -2.107648490002, + "timestamp": 1.0433902900232095 + }, + { + "x": 7.772183020683445, + "y": 4.506239107696108, + "heading": -3.6196694404073297, + "angularVelocity": -0.6755883622830359, + "velocityX": 1.105307841335925, + "velocityY": -1.7556093343662444, + "timestamp": 1.0954114710250575 + }, + { + "x": 7.818167453247437, + "y": 4.433202846918064, + "heading": -3.648592309890163, + "angularVelocity": -0.5559825618300757, + "velocityX": 0.8839559517566051, + "velocityY": -1.4039716010186059, + "timestamp": 1.1474326520269056 + }, + { + "x": 7.852646237712458, + "y": 4.378442821799572, + "heading": -3.670776152617206, + "angularVelocity": -0.42643866017295573, + "velocityX": 0.6627835777853055, + "velocityY": -1.0526486339582835, + "timestamp": 1.1994538330287536 + }, + { + "x": 7.875626660250975, + "y": 4.341945996815336, + "heading": -3.685837677441819, + "angularVelocity": -0.2895267761044795, + "velocityX": 0.4417512654643735, + "velocityY": -0.7015762480082658, + "timestamp": 1.2514750140306017 + }, + { + "x": 7.887114524841309, + "y": 4.323701858520508, + "heading": -3.693483193557926, + "angularVelocity": -0.1469692915244614, + "velocityX": 0.22083052266585948, + "velocityY": -0.3507059613694702, + "timestamp": 1.3034961950324497 + }, + { + "x": 7.887114524841309, + "y": 4.323701858520508, + "heading": -3.693483193557926, + "angularVelocity": -3.58149095021185e-18, + "velocityX": -4.3323929495550666e-18, + "velocityY": -6.867429667891556e-19, + "timestamp": 1.3555173760342978 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/1234.6.traj b/src/main/deploy/choreo/1234.6.traj new file mode 100644 index 0000000..1eac923 --- /dev/null +++ b/src/main/deploy/choreo/1234.6.traj @@ -0,0 +1,220 @@ +{ + "samples": [ + { + "x": 7.887114524841309, + "y": 4.323701858520508, + "heading": -3.693483193557926, + "angularVelocity": -3.58149095021185e-18, + "velocityX": -4.3323929495550666e-18, + "velocityY": -6.867429667891556e-19, + "timestamp": 0 + }, + { + "x": 7.877220674416456, + "y": 4.339296435616169, + "heading": -3.6860278197150365, + "angularVelocity": 0.1546730005334719, + "velocityX": -0.2052628834300042, + "velocityY": 0.3235330759081108, + "timestamp": 0.04820087421321695 + }, + { + "x": 7.8574280453898675, + "y": 4.370495236689797, + "heading": -3.6713431382027326, + "angularVelocity": 0.30465591655776203, + "velocityX": -0.4106280093393107, + "velocityY": 0.6472662909726413, + "timestamp": 0.0964017484264339 + }, + { + "x": 7.827730700014785, + "y": 4.417309792394871, + "heading": -3.649709320568005, + "angularVelocity": 0.4488262503091546, + "velocityX": -0.6161163227810843, + "velocityY": 0.9712387268743375, + "timestamp": 0.14460262263965085 + }, + { + "x": 7.78812139040693, + "y": 4.479753936181193, + "heading": -3.6214786357466644, + "angularVelocity": 0.5856882324681533, + "velocityX": -0.8217550045387327, + "velocityY": 1.2954981586039502, + "timestamp": 0.1928034968528678 + }, + { + "x": 7.738591082271519, + "y": 4.557844393857851, + "heading": -3.5871056587121233, + "angularVelocity": 0.7131193696303854, + "velocityX": -1.0275811163986608, + "velocityY": 1.6201045925272002, + "timestamp": 0.24100437106608474 + }, + { + "x": 7.679128229221916, + "y": 4.651601611491865, + "heading": -3.5472016043309003, + "angularVelocity": 0.8278699304234943, + "velocityX": -1.2336467755038363, + "velocityY": 1.9451352110187774, + "timestamp": 0.2892052452793017 + }, + { + "x": 7.609717807763235, + "y": 4.761050792490485, + "heading": -3.502640608758667, + "angularVelocity": 0.9244852152503467, + "velocityX": -1.44002411972116, + "velocityY": 2.27068871229333, + "timestamp": 0.33740611949251864 + }, + { + "x": 7.530340734883155, + "y": 4.886222255013547, + "heading": -3.4547926290801594, + "angularVelocity": 0.992678669413335, + "velocityX": -1.6467973698765865, + "velocityY": 2.5968712096250886, + "timestamp": 0.3856069937057356 + }, + { + "x": 7.440978533351249, + "y": 5.027143704553463, + "heading": -3.4061300682182822, + "angularVelocity": 1.0095783874509316, + "velocityX": -1.8539539581089521, + "velocityY": 2.9236284992788755, + "timestamp": 0.43380786791895254 + }, + { + "x": 7.341670133172821, + "y": 5.183749888471115, + "heading": -3.362339357555316, + "angularVelocity": 0.9085044903808221, + "velocityX": -2.0603028845314446, + "velocityY": 3.2490320242928163, + "timestamp": 0.4820087421321695 + }, + { + "x": 7.233912335944607, + "y": 5.3544068604560495, + "heading": -3.343182068843528, + "angularVelocity": 0.39744691407575744, + "velocityX": -2.2355983991400867, + "velocityY": 3.540536863087266, + "timestamp": 0.5302096163453864 + }, + { + "x": 7.111458392005018, + "y": 5.523753439275115, + "heading": -3.3431820280984104, + "angularVelocity": 8.453190557195011e-7, + "velocityX": -2.5404921785841763, + "velocityY": 3.513350775962189, + "timestamp": 0.5784104905586034 + }, + { + "x": 6.97439432144165, + "y": 5.681508541107178, + "heading": -3.34318198516747, + "angularVelocity": 8.906672529661562e-7, + "velocityX": -2.8436013412757295, + "velocityY": 3.272868063227098, + "timestamp": 0.6266113647718203 + }, + { + "x": 6.7313273316705375, + "y": 5.893656042818141, + "heading": -3.34318194956056, + "angularVelocity": 4.785050059350481e-7, + "velocityX": -3.266466313196734, + "velocityY": 2.8509534281896696, + "timestamp": 0.7010241908858106 + }, + { + "x": 6.463571916500496, + "y": 6.0736426038302405, + "heading": -3.343181914758351, + "angularVelocity": 4.6769100676461487e-7, + "velocityX": -3.598242791637579, + "velocityY": 2.4187572279055924, + "timestamp": 0.7754370169998008 + }, + { + "x": 6.20686377949356, + "y": 6.2452739854960395, + "heading": -3.3037503885523654, + "angularVelocity": 0.529902279824675, + "velocityX": -3.4497834635886053, + "velocityY": 2.306475786887526, + "timestamp": 0.8498498431137911 + }, + { + "x": 5.9863622153072455, + "y": 6.393046429956987, + "heading": -3.2362350016761288, + "angularVelocity": 0.9073084628297773, + "velocityX": -2.9632198600888495, + "velocityY": 1.9858464216179963, + "timestamp": 0.9242626692277813 + }, + { + "x": 5.802814692409499, + "y": 6.516118854857836, + "heading": -3.1699341420458658, + "angularVelocity": 0.8909869856137588, + "velocityX": -2.4666113690746707, + "velocityY": 1.6539141345380624, + "timestamp": 0.9986754953417716 + }, + { + "x": 5.656112580001017, + "y": 6.614514316298437, + "heading": -3.112369789753736, + "angularVelocity": 0.7735810517930446, + "velocityX": -1.9714627177813826, + "velocityY": 1.3222916878587008, + "timestamp": 1.0730883214557618 + }, + { + "x": 5.546164743378319, + "y": 6.688272502359933, + "heading": -3.066929995025816, + "angularVelocity": 0.6106446576605511, + "velocityX": -1.4775387841643228, + "velocityY": 0.9912025911838206, + "timestamp": 1.147501147569752 + }, + { + "x": 5.472907694722162, + "y": 6.737423740742976, + "heading": -3.0355291255006427, + "angularVelocity": 0.4219819507604332, + "velocityX": -0.9844680343673129, + "velocityY": 0.6605210546331426, + "timestamp": 1.2219139736837423 + }, + { + "x": 5.436295986175537, + "y": 6.761990547180176, + "heading": -3.019397960613997, + "angularVelocity": 0.21677936088512653, + "velocityX": -0.4920080375732523, + "velocityY": 0.33014209673434414, + "timestamp": 1.2963267997977326 + }, + { + "x": 5.436295986175537, + "y": 6.761990547180176, + "heading": -3.019397960613997, + "angularVelocity": -7.216632961030075e-19, + "velocityX": 1.385331577266768e-17, + "velocityY": 1.0244819922206164e-18, + "timestamp": 1.3707396259117228 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/1234.7.traj b/src/main/deploy/choreo/1234.7.traj new file mode 100644 index 0000000..5a41d3a --- /dev/null +++ b/src/main/deploy/choreo/1234.7.traj @@ -0,0 +1,265 @@ +{ + "samples": [ + { + "x": 5.436295986175537, + "y": 6.761990547180176, + "heading": -3.019397960613997, + "angularVelocity": -7.216632961030075e-19, + "velocityX": 1.385331577266768e-17, + "velocityY": 1.0244819922206164e-18, + "timestamp": 0 + }, + { + "x": 5.467021331993691, + "y": 6.743681754630262, + "heading": -3.0405281639164583, + "angularVelocity": -0.3132514333002431, + "velocityX": 0.4554976816082698, + "velocityY": -0.2714245303824937, + "timestamp": 0.06745445050273169 + }, + { + "x": 5.528456798036073, + "y": 6.706927903016198, + "heading": -3.0820180627519798, + "angularVelocity": -0.6150802286031658, + "velocityX": 0.9107696465467936, + "velocityY": -0.544869187135031, + "timestamp": 0.13490890100546338 + }, + { + "x": 5.620575385666307, + "y": 6.6515418667178325, + "heading": -3.142819906623659, + "angularVelocity": -0.9013763127344223, + "velocityX": 1.3656413615955947, + "velocityY": -0.8210879472826135, + "timestamp": 0.20236335150819507 + }, + { + "x": 5.743329468675338, + "y": 6.577249796463415, + "heading": -3.2213441024636853, + "angularVelocity": -1.1641069678100113, + "velocityX": 1.8198070267292152, + "velocityY": -1.1013664732382369, + "timestamp": 0.26981780201092675 + }, + { + "x": 5.896629362899617, + "y": 6.483625547580609, + "heading": -3.314894408186393, + "angularVelocity": -1.3868663227627636, + "velocityX": 2.272643140396938, + "velocityY": -1.387962516706195, + "timestamp": 0.33727225251365844 + }, + { + "x": 6.08026059744355, + "y": 6.369945182455195, + "heading": -3.4183484305078555, + "angularVelocity": -1.5336871259113138, + "velocityX": 2.7222997619185247, + "velocityY": -1.6852908040635355, + "timestamp": 0.40472670301639013 + }, + { + "x": 6.293404304224734, + "y": 6.2347548367007635, + "heading": -3.5197782469381993, + "angularVelocity": -1.503678640540327, + "velocityX": 3.1598168125697015, + "velocityY": -2.0041723673808027, + "timestamp": 0.4721811535191218 + }, + { + "x": 6.528550724023803, + "y": 6.079659939579352, + "heading": -3.5547887453718556, + "angularVelocity": -0.5190242922850319, + "velocityX": 3.4860030442252885, + "velocityY": -2.2992537329338925, + "timestamp": 0.5396356040218535 + }, + { + "x": 6.752670914137969, + "y": 5.89177230101219, + "heading": -3.5547888588953853, + "angularVelocity": -0.0000016829657464890388, + "velocityX": 3.322541188073102, + "velocityY": -2.7854001799266244, + "timestamp": 0.6070900545245852 + }, + { + "x": 6.95162296295166, + "y": 5.677413463592529, + "heading": -3.5547889362466214, + "angularVelocity": -0.0000011467180563286706, + "velocityX": 2.949428055983232, + "velocityY": -3.177830903996148, + "timestamp": 0.6745445050273169 + }, + { + "x": 7.098585597806102, + "y": 5.481458375845758, + "heading": -3.5547889980207357, + "angularVelocity": -0.0000010934442451876683, + "velocityX": 2.6013395651065, + "velocityY": -3.4685396273989286, + "timestamp": 0.7310394881605262 + }, + { + "x": 7.2242750181804, + "y": 5.271223782919906, + "heading": -3.5547890511728277, + "angularVelocity": -9.408285364307061e-7, + "velocityX": 2.224789059197234, + "velocityY": -3.7212966756736487, + "timestamp": 0.7875344712937355 + }, + { + "x": 7.3273161189708, + "y": 5.049010040930901, + "heading": -3.554789099761621, + "angularVelocity": -8.600550112485466e-7, + "velocityX": 1.823898248583237, + "velocityY": -3.9333358409019663, + "timestamp": 0.8440294544269449 + }, + { + "x": 7.406582487237177, + "y": 4.817248829975611, + "heading": -3.554789146622121, + "angularVelocity": -8.294630283569178e-7, + "velocityX": 1.4030691553527146, + "velocityY": -4.102332598433042, + "timestamp": 0.9005244375601542 + }, + { + "x": 7.478638737084507, + "y": 4.58314561993378, + "heading": -3.5547891933302385, + "angularVelocity": -8.267657533220866e-7, + "velocityX": 1.2754451077085862, + "velocityY": -4.1437875906580794, + "timestamp": 0.9570194206933635 + }, + { + "x": 7.550696000844859, + "y": 4.349042721974096, + "heading": -3.554789240038353, + "angularVelocity": -8.267657147462614e-7, + "velocityX": 1.2754630546653534, + "velocityY": -4.143782066589741, + "timestamp": 1.0135144038265729 + }, + { + "x": 7.62275325466431, + "y": 4.114939820954579, + "heading": -3.5547892867464688, + "angularVelocity": -8.267657143276858e-7, + "velocityX": 1.2754628787046107, + "velocityY": -4.1437821207509, + "timestamp": 1.0700093869597822 + }, + { + "x": 7.694810030922688, + "y": 3.880836772951051, + "heading": -3.5547893334838476, + "angularVelocity": -8.2728370553242e-7, + "velocityX": 1.2754544255456055, + "velocityY": -4.143784722468827, + "timestamp": 1.1265043700929915 + }, + { + "x": 7.761719458964053, + "y": 3.664123207681456, + "heading": -3.603373681714835, + "angularVelocity": -0.8599763295163799, + "velocityX": 1.1843428271073144, + "velocityY": -3.8359789356624097, + "timestamp": 1.1829993532262009 + }, + { + "x": 7.82148239293419, + "y": 3.4701821668493893, + "heading": -3.6781611768684126, + "angularVelocity": -1.3237900253414745, + "velocityX": 1.0578449741142784, + "velocityY": -3.432889613840102, + "timestamp": 1.2394943363594102 + }, + { + "x": 7.8737258923647415, + "y": 3.300733490685395, + "heading": -3.7535206924966134, + "angularVelocity": -1.333915180583573, + "velocityX": 0.9247458187104354, + "velocityY": -2.9993579388182336, + "timestamp": 1.2959893194926195 + }, + { + "x": 7.918466935683176, + "y": 3.155694031456225, + "heading": -3.822908621298775, + "angularVelocity": -1.228213992710683, + "velocityX": 0.7919471931328769, + "velocityY": -2.5672980357774713, + "timestamp": 1.3524843026258289 + }, + { + "x": 7.955727725126361, + "y": 3.0349643646800413, + "heading": -3.88346036229386, + "angularVelocity": -1.071807400177649, + "velocityX": 0.6595415623955151, + "velocityY": -2.1369980143460783, + "timestamp": 1.4089792857590382 + }, + { + "x": 7.985522348817118, + "y": 2.938469798831404, + "heading": -3.9335348241903825, + "angularVelocity": -0.8863523647480636, + "velocityX": 0.527385301107251, + "velocityY": -1.7080200842101736, + "timestamp": 1.4654742688922475 + }, + { + "x": 8.00786007413313, + "y": 2.8661541802383343, + "heading": -3.9720495265001765, + "angularVelocity": -0.6817366812727442, + "velocityX": 0.3953930787684817, + "velocityY": -1.2800361126324549, + "timestamp": 1.5219692520254569 + }, + { + "x": 8.022747512167825, + "y": 2.817974467927218, + "heading": -3.998236759250909, + "angularVelocity": -0.4635320040539836, + "velocityX": 0.26351787732356136, + "velocityY": -0.8528139958465588, + "timestamp": 1.5784642351586662 + }, + { + "x": 8.030189514160156, + "y": 2.7938973903656006, + "heading": -4.0115353139615895, + "angularVelocity": -0.2353935513056714, + "velocityX": 0.13172854613983054, + "velocityY": -0.4261808080346905, + "timestamp": 1.6349592182918755 + }, + { + "x": 8.030189514160156, + "y": 2.7938973903656006, + "heading": -4.0115353139615895, + "angularVelocity": -9.739346179638407e-20, + "velocityX": -1.4888070530854099e-18, + "velocityY": 1.5026054090472747e-18, + "timestamp": 1.6914542014250848 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/1234.8.traj b/src/main/deploy/choreo/1234.8.traj new file mode 100644 index 0000000..f5d0c63 --- /dev/null +++ b/src/main/deploy/choreo/1234.8.traj @@ -0,0 +1,274 @@ +{ + "samples": [ + { + "x": 8.030189514160156, + "y": 2.7938973903656006, + "heading": -4.0115353139615895, + "angularVelocity": -9.739346179638407e-20, + "velocityX": -1.4888070530854099e-18, + "velocityY": 1.5026054090472747e-18, + "timestamp": 0 + }, + { + "x": 8.023575299667664, + "y": 2.8153265027144045, + "heading": -3.9982317028281145, + "angularVelocity": 0.2490951997345419, + "velocityX": -0.12384374915683626, + "velocityY": 0.40123609801159893, + "timestamp": 0.05340773787552955 + }, + { + "x": 8.010342464279917, + "y": 2.858214474225702, + "heading": -3.9720678856506844, + "angularVelocity": 0.4898881364046355, + "velocityX": -0.24777000326411402, + "velocityY": 0.8030291717513138, + "timestamp": 0.1068154757510591 + }, + { + "x": 7.990485842450547, + "y": 2.9225981919232042, + "heading": -3.933611374260751, + "angularVelocity": 0.7200550504415586, + "velocityX": -0.37179297643436726, + "velocityY": 1.2055129136447036, + "timestamp": 0.16022321362658865 + }, + { + "x": 7.963999104860506, + "y": 3.008522703065025, + "heading": -3.88358340210928, + "angularVelocity": 0.9367176769041401, + "velocityX": -0.4959344590061064, + "velocityY": 1.6088401149300549, + "timestamp": 0.2136309515021182 + }, + { + "x": 7.930873132180931, + "y": 3.116042932711411, + "heading": -3.8229207451243807, + "angularVelocity": 1.1358402246183505, + "velocityX": -0.6202466907843547, + "velocityY": 2.013195726375272, + "timestamp": 0.26703868937764774 + }, + { + "x": 7.891092490084177, + "y": 3.2452269626181964, + "heading": -3.7529191339342174, + "angularVelocity": 1.3107016693593585, + "velocityX": -0.7448479130396041, + "velocityY": 2.4188260923512273, + "timestamp": 0.3204464272531773 + }, + { + "x": 7.8446301181344165, + "y": 3.3961613863205478, + "heading": -3.6755920589128284, + "angularVelocity": 1.4478627647852322, + "velocityX": -0.8699558116100112, + "velocityY": 2.8260778251667436, + "timestamp": 0.37385416512870684 + }, + { + "x": 7.79144486607711, + "y": 3.568948460788326, + "heading": -3.594682943142528, + "angularVelocity": 1.5149324608891876, + "velocityX": -0.9958342025505497, + "velocityY": 3.2352442050713903, + "timestamp": 0.4272619030042364 + }, + { + "x": 7.731528693856673, + "y": 3.7635325092715983, + "heading": -3.5194115433511453, + "angularVelocity": 1.409372551348413, + "velocityX": -1.1218631345157435, + "velocityY": 3.643368100269756, + "timestamp": 0.48066964087976594 + }, + { + "x": 7.670369210430734, + "y": 3.975507979931414, + "heading": -3.489780134920616, + "angularVelocity": 0.554814894043769, + "velocityX": -1.1451427425830363, + "velocityY": 3.969002977692863, + "timestamp": 0.5340773787552955 + }, + { + "x": 7.6026179901960935, + "y": 4.19693105080222, + "heading": -3.489780079264961, + "angularVelocity": 0.0000010420897352541278, + "velocityX": -1.2685656223175075, + "velocityY": 4.145898697054893, + "timestamp": 0.587485116630825 + }, + { + "x": 7.534866230416538, + "y": 4.418353956582559, + "heading": -3.4897800236112295, + "angularVelocity": 0.0000010420537160061253, + "velocityX": -1.2685757246909648, + "velocityY": 4.145895605921018, + "timestamp": 0.6408928545063546 + }, + { + "x": 7.467114460621537, + "y": 4.639776859298328, + "heading": -3.4897799679574972, + "angularVelocity": 0.0000010420537157088877, + "velocityX": -1.2685759122189693, + "velocityY": 4.145895548540395, + "timestamp": 0.6943005923818841 + }, + { + "x": 7.399363331617181, + "y": 4.861199958084797, + "heading": -3.4897799123037565, + "angularVelocity": 0.0000010420538852908062, + "velocityX": -1.2685639141327167, + "velocityY": 4.145899219744388, + "timestamp": 0.7477083302574137 + }, + { + "x": 7.323806767003022, + "y": 5.080082597053995, + "heading": -3.4897798565337, + "angularVelocity": 0.0000010442317738037892, + "velocityX": -1.4147119428695523, + "velocityY": 4.098331958550998, + "timestamp": 0.8011160681329432 + }, + { + "x": 7.227002735387266, + "y": 5.290433216494062, + "heading": -3.489779799010186, + "angularVelocity": 0.000001077063294583615, + "velocityX": -1.8125469354527803, + "velocityY": 3.9385794607198172, + "timestamp": 0.8545238060084728 + }, + { + "x": 7.109896904403833, + "y": 5.490194641427451, + "heading": -3.4897797367177428, + "angularVelocity": 0.0000011663561406035081, + "velocityX": -2.1926753620675226, + "velocityY": 3.740308668360906, + "timestamp": 0.9079315438840023 + }, + { + "x": 6.973634243011475, + "y": 5.677413463592529, + "heading": -3.4897796638429868, + "angularVelocity": 0.0000013644980810784123, + "velocityX": -2.551365528903858, + "velocityY": 3.50546249686525, + "timestamp": 0.9613392817595319 + }, + { + "x": 6.772589320106531, + "y": 5.8912480994990855, + "heading": -3.4897796014962164, + "angularVelocity": 9.209868538745789e-7, + "velocityX": -2.9698367649130955, + "velocityY": 3.1587664794069745, + "timestamp": 1.0290348956330533 + }, + { + "x": 6.54637425079476, + "y": 6.0782535940174505, + "heading": -3.4897795323629706, + "angularVelocity": 0.0000010212367041132869, + "velocityX": -3.341650313333725, + "velocityY": 2.7624462475184366, + "timestamp": 1.0967305095065747 + }, + { + "x": 6.306507010011589, + "y": 6.232281236398436, + "heading": -3.463607168318969, + "angularVelocity": 0.38661831315837925, + "velocityX": -3.5433202693357058, + "velocityY": 2.2752972248507035, + "timestamp": 1.164426123380096 + }, + { + "x": 6.091413412046711, + "y": 6.369010367225917, + "heading": -3.373216136469404, + "angularVelocity": 1.3352568457159812, + "velocityX": -3.1773638742200982, + "velocityY": 2.019763512048771, + "timestamp": 1.2321217372536175 + }, + { + "x": 5.906610596748241, + "y": 6.484891166356038, + "heading": -3.279739146420341, + "angularVelocity": 1.3808426381022285, + "velocityX": -2.7299082573318043, + "velocityY": 1.711791835533487, + "timestamp": 1.299817351127139 + }, + { + "x": 5.752541410695039, + "y": 6.58077269135908, + "heading": -3.1947588504378728, + "angularVelocity": 1.2553294241668493, + "velocityX": -2.275910908217137, + "velocityY": 1.4163624423612229, + "timestamp": 1.3675129650006603 + }, + { + "x": 5.629279463345689, + "y": 6.6570975887825545, + "heading": -3.123126667114855, + "angularVelocity": 1.0581510266950425, + "velocityX": -1.8208261997541688, + "velocityY": 1.1274718265510586, + "timestamp": 1.4352085788741817 + }, + { + "x": 5.536838294747283, + "y": 6.714128828465892, + "heading": -3.067440339641402, + "angularVelocity": 0.8225987517816818, + "velocityX": -1.3655414776372279, + "velocityY": 0.8424658027312018, + "timestamp": 1.5029041927477031 + }, + { + "x": 5.47521645832478, + "y": 6.752037909221401, + "heading": -3.029307242162313, + "angularVelocity": 0.5633023366969564, + "velocityX": -0.910278124335105, + "velocityY": 0.5599931603594854, + "timestamp": 1.5705998066212246 + }, + { + "x": 5.444408416748047, + "y": 6.770946025848389, + "heading": -3.0098303253058174, + "angularVelocity": 0.28771312854751757, + "velocityX": -0.4550965685058097, + "velocityY": 0.2793108082646994, + "timestamp": 1.638295420494746 + }, + { + "x": 5.444408416748047, + "y": 6.770946025848389, + "heading": -3.0098303253058174, + "angularVelocity": 4.439381970105003e-20, + "velocityX": -1.0620325200300552e-18, + "velocityY": -2.934155998551033e-18, + "timestamp": 1.7059910343682674 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/1234.traj b/src/main/deploy/choreo/1234.traj new file mode 100644 index 0000000..28145d6 --- /dev/null +++ b/src/main/deploy/choreo/1234.traj @@ -0,0 +1,2056 @@ +{ + "samples": [ + { + "x": 1.17792546749115, + "y": 6.456913471221924, + "heading": -2.3204952998605908, + "angularVelocity": 1.0915534190095111e-19, + "velocityX": -5.147338398718608e-19, + "velocityY": 5.503263052262533e-18, + "timestamp": 0 + }, + { + "x": 1.1827150715238435, + "y": 6.457088134209186, + "heading": -2.3224346077509246, + "angularVelocity": -0.0789736445243584, + "velocityX": 0.19504509220825755, + "velocityY": 0.0071127296167531025, + "timestamp": 0.024556393490687705 + }, + { + "x": 1.192295354492295, + "y": 6.457437607941965, + "heading": -2.3262914929987693, + "angularVelocity": -0.15706236542053373, + "velocityX": 0.39013395725576233, + "velocityY": 0.01423147633270856, + "timestamp": 0.04911278698137541 + }, + { + "x": 1.206667489265113, + "y": 6.457962064674032, + "heading": -2.332041946372013, + "angularVelocity": -0.2341733681464424, + "velocityX": 0.5852705845534056, + "velocityY": 0.021357237668724143, + "timestamp": 0.07366918047206311 + }, + { + "x": 1.2258327601765544, + "y": 6.458661699320965, + "heading": -2.3396593139577733, + "angularVelocity": -0.31019895444536805, + "velocityX": 0.7804595132713229, + "velocityY": 0.028490936472328844, + "timestamp": 0.09822557396275082 + }, + { + "x": 1.2497925792060098, + "y": 6.459536727575483, + "heading": -2.3491138418876267, + "angularVelocity": -0.3850128860917086, + "velocityX": 0.9757059414503028, + "velocityY": 0.03563341884264574, + "timestamp": 0.12278196745343853 + }, + { + "x": 1.2785485056119763, + "y": 6.460587384059239, + "heading": -2.3603720996871065, + "angularVelocity": -0.45846544215660834, + "velocityX": 1.171015866677306, + "velocityY": 0.042785455614790954, + "timestamp": 0.14733836094412622 + }, + { + "x": 1.3121022699714886, + "y": 6.461813920680284, + "heading": -2.37339623803971, + "angularVelocity": -0.5303766759374715, + "velocityX": 1.3663962654873019, + "velocityY": 0.04994775073590437, + "timestamp": 0.17189475443481392 + }, + { + "x": 1.3504558037765833, + "y": 6.463216605493996, + "heading": -2.3881430188647683, + "angularVelocity": -0.6005271429882628, + "velocityX": 1.5618553196599922, + "velocityY": 0.0571209617668197, + "timestamp": 0.1964511479255016 + }, + { + "x": 1.3936112759432409, + "y": 6.464795722561273, + "heading": -2.4045625285492975, + "angularVelocity": -0.6686449983282632, + "velocityX": 1.7574026977138553, + "velocityY": 0.06430574049382957, + "timestamp": 0.2210075414161893 + }, + { + "x": 1.4415711377320954, + "y": 6.4665515735936046, + "heading": -2.422596443035158, + "angularVelocity": -0.7343877468285036, + "velocityX": 1.9530498974550927, + "velocityY": 0.07150280569490924, + "timestamp": 0.245563934906877 + }, + { + "x": 1.4943381774964477, + "y": 6.468484482620495, + "heading": -2.4421756455788666, + "angularVelocity": -0.7973158823641606, + "velocityX": 2.1488106461709386, + "velocityY": 0.07871306621733853, + "timestamp": 0.2701203283975647 + }, + { + "x": 1.5519155859304148, + "y": 6.470594805589591, + "heading": -2.4632168842753686, + "angularVelocity": -0.8568537845136457, + "velocityX": 2.344701328222389, + "velocityY": 0.08593782185059054, + "timestamp": 0.29467672188825245 + }, + { + "x": 1.6143070299659465, + "y": 6.4728829478329, + "heading": -2.485617957059904, + "angularVelocity": -0.9122297536497331, + "velocityX": 2.540741337248552, + "velocityY": 0.09317908365400698, + "timestamp": 0.31923311537894017 + }, + { + "x": 1.6815167261349753, + "y": 6.4753493938588065, + "heading": -2.50925054323321, + "angularVelocity": -0.9623801712685371, + "velocityX": 2.7369530543854603, + "velocityY": 0.10044007589479852, + "timestamp": 0.3437895088696279 + }, + { + "x": 1.7535494834234393, + "y": 6.4779947560501325, + "heading": -2.5339490739505, + "angularVelocity": -1.0057881963267985, + "velocityX": 2.9333606058959827, + "velocityY": 0.10772600595153933, + "timestamp": 0.3683459023603156 + }, + { + "x": 1.8304106245820337, + "y": 6.480819851058323, + "heading": -2.5594924852596286, + "angularVelocity": -1.0401939241939082, + "velocityX": 3.1299849136128906, + "velocityY": 0.11504519217211771, + "timestamp": 0.39290229585100334 + }, + { + "x": 1.9121055005220093, + "y": 6.483825811723467, + "heading": -2.5855720493348286, + "angularVelocity": -1.0620274546866881, + "velocityX": 3.3268271243069956, + "velocityY": 0.12241051057780712, + "timestamp": 0.41745868934169106 + }, + { + "x": 1.9986376079459907, + "y": 6.487014227014825, + "heading": -2.6117286672590567, + "angularVelocity": -1.0651652871643127, + "velocityX": 3.523811729796402, + "velocityY": 0.1298405359308217, + "timestamp": 0.4420150828323788 + }, + { + "x": 2.0900011902761935, + "y": 6.490387314111071, + "heading": -2.6372111268909095, + "angularVelocity": -1.037711813891415, + "velocityX": 3.720561912515762, + "velocityY": 0.1373608505469304, + "timestamp": 0.4665714763230665 + }, + { + "x": 2.186144606759753, + "y": 6.493950905854893, + "heading": -2.6605668053733122, + "angularVelocity": -0.9511037722725775, + "velocityX": 3.9152091499111883, + "velocityY": 0.1451186936377184, + "timestamp": 0.4911278698137542 + }, + { + "x": 2.2866032796675606, + "y": 6.497891465085372, + "heading": -2.6774789394698564, + "angularVelocity": -0.6887059414061727, + "velocityX": 4.090937577861477, + "velocityY": 0.16046978689984315, + "timestamp": 0.515684263304442 + }, + { + "x": 2.390651909884553, + "y": 6.502515861566495, + "heading": -2.6841281768726843, + "angularVelocity": -0.2707741837313909, + "velocityX": 4.237129945667706, + "velocityY": 0.188317412444054, + "timestamp": 0.5402406567951297 + }, + { + "x": 2.4970494575830844, + "y": 6.506375954880804, + "heading": -2.6841282155484834, + "angularVelocity": -0.000001574978804933473, + "velocityX": 4.3327839545688835, + "velocityY": 0.1571930062031463, + "timestamp": 0.5647970502858174 + }, + { + "x": 2.6034474806695886, + "y": 6.510222923143476, + "heading": -2.6841282541506195, + "angularVelocity": -0.0000015719790591645795, + "velocityX": 4.332803313599462, + "velocityY": 0.15665852007669273, + "timestamp": 0.5893534437765051 + }, + { + "x": 2.709845511835245, + "y": 6.51406966794852, + "heading": -2.684128292752748, + "angularVelocity": -0.0000015719787382653465, + "velocityX": 4.332803642603493, + "velocityY": 0.15664942030278362, + "timestamp": 0.6139098372671928 + }, + { + "x": 2.8162435409813273, + "y": 6.517916468613033, + "heading": -2.6841283313548785, + "angularVelocity": -0.0000015719788238959483, + "velocityX": 4.3328035603611985, + "velocityY": 0.15665169504528215, + "timestamp": 0.6384662307578806 + }, + { + "x": 2.922641569756633, + "y": 6.521763279532346, + "heading": -2.6841283699570098, + "angularVelocity": -0.0000015719788438017344, + "velocityX": 4.332803545262214, + "velocityY": 0.15665211264722553, + "timestamp": 0.6630226242485683 + }, + { + "x": 3.029039598618472, + "y": 6.525610088058124, + "heading": -2.6841284085591406, + "angularVelocity": -0.0000015719788449275249, + "velocityX": 4.332803548786091, + "velocityY": 0.15665201517631638, + "timestamp": 0.687579017739256 + }, + { + "x": 3.1354376275492, + "y": 6.52945689467851, + "heading": -2.684128447161272, + "angularVelocity": -0.0000015719788459691416, + "velocityX": 4.33280355159142, + "velocityY": 0.1566519375837843, + "timestamp": 0.7121354112299437 + }, + { + "x": 3.2418356565025146, + "y": 6.533303700674159, + "heading": -2.684128485763403, + "angularVelocity": -0.000001571978849857335, + "velocityX": 4.332803552511197, + "velocityY": 0.15665191214291052, + "timestamp": 0.7366918047206314 + }, + { + "x": 3.3482336854320547, + "y": 6.53715050732732, + "heading": -2.684128524365535, + "angularVelocity": -0.0000015719788550374975, + "velocityX": 4.332803551543035, + "velocityY": 0.15665193891849358, + "timestamp": 0.7612481982113192 + }, + { + "x": 3.4546317141846608, + "y": 6.54099731887419, + "heading": -2.684128562967666, + "angularVelocity": -0.0000015719788669289655, + "velocityX": 4.33280354433783, + "velocityY": 0.15665213820297527, + "timestamp": 0.7858045917020069 + }, + { + "x": 3.5610297424845294, + "y": 6.544844142943354, + "heading": -2.6841286015697983, + "angularVelocity": -0.0000015719788893437079, + "velocityX": 4.332803525901206, + "velocityY": 0.15665264814328536, + "timestamp": 0.8103609851926946 + }, + { + "x": 3.667427770171028, + "y": 6.548690983979332, + "heading": -2.6841286401719313, + "angularVelocity": -0.0000015719789187769318, + "velocityX": 4.332803500923143, + "velocityY": 0.15665333907592197, + "timestamp": 0.8349173786833823 + }, + { + "x": 3.7738258011124195, + "y": 6.5525377349997225, + "heading": -2.6841286787740612, + "angularVelocity": -0.0000015719787926702145, + "velocityX": 4.332803633470852, + "velocityY": 0.15664967340792557, + "timestamp": 0.8594737721740701 + }, + { + "x": 3.8802238716303594, + "y": 6.556383391212679, + "heading": -2.684128717376152, + "angularVelocity": -0.0000015719771994988576, + "velocityX": 4.332805245130483, + "velocityY": 0.15660509001120598, + "timestamp": 0.8840301656647578 + }, + { + "x": 3.986621456193882, + "y": 6.560242469781829, + "heading": -2.6841287559787252, + "angularVelocity": -0.0000015719968565290703, + "velocityX": 4.332785455806911, + "velocityY": 0.15715168314906297, + "timestamp": 0.9085865591554455 + }, + { + "x": 4.093016902723116, + "y": 6.56416005056415, + "heading": -2.6841287945844416, + "angularVelocity": -0.0000015721248440196224, + "velocityX": 4.33269838950829, + "velocityY": 0.1595340449242531, + "timestamp": 0.9331429526461332 + }, + { + "x": 4.199217262171535, + "y": 6.571698095232862, + "heading": -2.6841288334335762, + "angularVelocity": -0.0000015820374694756593, + "velocityX": 4.324753937857032, + "velocityY": 0.30696871963591016, + "timestamp": 0.957699346136821 + }, + { + "x": 4.304968357086182, + "y": 6.584028720855713, + "heading": -2.6841288728935164, + "angularVelocity": -0.000001606911063517827, + "velocityX": 4.306458721422164, + "velocityY": 0.5021350398025036, + "timestamp": 0.9822557396275087 + }, + { + "x": 4.414700482208155, + "y": 6.602139612854462, + "heading": -2.6841289106192554, + "angularVelocity": -0.0000014706882277086669, + "velocityX": 4.277762279149216, + "velocityY": 0.7060292557703594, + "timestamp": 1.0079074981781961 + }, + { + "x": 4.523455429440765, + "y": 6.625409877143289, + "heading": -2.6841289481054, + "angularVelocity": -0.0000014613479251873946, + "velocityX": 4.23966828697975, + "velocityY": 0.907160584832358, + "timestamp": 1.0335592567288836 + }, + { + "x": 4.631457869220697, + "y": 6.651954495449144, + "heading": -2.684128985593184, + "angularVelocity": -0.0000014614118405678238, + "velocityX": 4.210332775685651, + "velocityY": 1.0348069608325312, + "timestamp": 1.059211015279571 + }, + { + "x": 4.739457727470495, + "y": 6.6785096150501175, + "heading": -2.684129023081148, + "angularVelocity": -0.00000146141888775809, + "velocityX": 4.210232138135551, + "velocityY": 1.0352163399831225, + "timestamp": 1.0848627738302585 + }, + { + "x": 4.847457616086101, + "y": 6.705064611153251, + "heading": -2.684129060569109, + "angularVelocity": -0.0000014614187615548696, + "velocityX": 4.2102333219064825, + "velocityY": 1.035211525582661, + "timestamp": 1.110514532380946 + }, + { + "x": 4.955457519849879, + "y": 6.7316195456479555, + "heading": -2.6841290980570682, + "angularVelocity": -0.000001461418696152274, + "velocityX": 4.210233912438069, + "velocityY": 1.035209123859201, + "timestamp": 1.1361662909316335 + }, + { + "x": 5.063457420443, + "y": 6.758174493037793, + "heading": -2.684129135545028, + "angularVelocity": -0.0000014614187142453692, + "velocityX": 4.210233788834204, + "velocityY": 1.0352096265589994, + "timestamp": 1.161818049482321 + }, + { + "x": 5.171457321226551, + "y": 6.784729439653125, + "heading": -2.6841291730329884, + "angularVelocity": -0.0000014614187176590148, + "velocityX": 4.210233796257874, + "velocityY": 1.0352095963658907, + "timestamp": 1.1874698080330084 + }, + { + "x": 5.279457221587324, + "y": 6.811284387987912, + "heading": -2.6841292105209487, + "angularVelocity": -0.0000014614187200214774, + "velocityX": 4.210233779776412, + "velocityY": 1.035209663396565, + "timestamp": 1.2131215665836959 + }, + { + "x": 5.387457121765594, + "y": 6.837839337064946, + "heading": -2.6841292480089085, + "angularVelocity": -0.0000014614187249678457, + "velocityX": 4.210233772661787, + "velocityY": 1.0352096923321552, + "timestamp": 1.2387733251343833 + }, + { + "x": 5.495457021941072, + "y": 6.864394286153341, + "heading": -2.6841292854968692, + "angularVelocity": -0.0000014614187291337482, + "velocityX": 4.210233772552937, + "velocityY": 1.0352096927749588, + "timestamp": 1.2644250836850708 + }, + { + "x": 5.603456922143669, + "y": 6.890949235131444, + "heading": -2.6841293229848295, + "angularVelocity": -0.000001461418731009078, + "velocityX": 4.210233773610118, + "velocityY": 1.035209688475399, + "timestamp": 1.2900768422357582 + }, + { + "x": 5.711456822363459, + "y": 6.917504184039622, + "heading": -2.6841293604727903, + "angularVelocity": -0.0000014614187365607823, + "velocityX": 4.210233774280365, + "velocityY": 1.035209685749491, + "timestamp": 1.3157286007864457 + }, + { + "x": 5.819456722592084, + "y": 6.944059132911867, + "heading": -2.6841293979607506, + "angularVelocity": -0.0000014614187388310343, + "velocityX": 4.2102337746248, + "velocityY": 1.0352096843486585, + "timestamp": 1.3413803593371332 + }, + { + "x": 5.927456622827952, + "y": 6.970614081754651, + "heading": -2.6841294354487113, + "angularVelocity": -0.0000014614187405368475, + "velocityX": 4.210233774907183, + "velocityY": 1.035209683200198, + "timestamp": 1.3670321178878206 + }, + { + "x": 6.035456523069077, + "y": 6.997169030576061, + "heading": -2.684129472936672, + "angularVelocity": -0.0000014614187445336427, + "velocityX": 4.210233775112074, + "velocityY": 1.0352096823669164, + "timestamp": 1.392683876438508 + }, + { + "x": 6.143456423285194, + "y": 7.023723979499181, + "heading": -2.6841295104246328, + "angularVelocity": -0.0000014614187497197174, + "velocityX": 4.210233774137161, + "velocityY": 1.0352096863319555, + "timestamp": 1.4183356349891956 + }, + { + "x": 6.251456323349948, + "y": 7.050278929037894, + "heading": -2.684129547912594, + "angularVelocity": -0.0000014614187535451282, + "velocityX": 4.210233768236523, + "velocityY": 1.0352097103300821, + "timestamp": 1.443987393539883 + }, + { + "x": 6.359456223124247, + "y": 7.076833879757894, + "heading": -2.6841295854005547, + "angularVelocity": -0.000001461418756010585, + "velocityX": 4.210233756913517, + "velocityY": 1.0352097563809437, + "timestamp": 1.4696391520905705 + }, + { + "x": 6.467456123835202, + "y": 7.103388826668465, + "heading": -2.6841296228885163, + "angularVelocity": -0.0000014614187566390387, + "velocityX": 4.210233793427791, + "velocityY": 1.035209607875408, + "timestamp": 1.495290910641258 + }, + { + "x": 6.57545602902077, + "y": 7.129943755380618, + "heading": -2.6841296603764766, + "angularVelocity": -0.000001461418738378337, + "velocityX": 4.21023396786468, + "velocityY": 1.0352088984340948, + "timestamp": 1.5209426691919454 + }, + { + "x": 6.6834559198048344, + "y": 7.156498742664419, + "heading": -2.6841296978644387, + "angularVelocity": -0.0000014614188054188506, + "velocityX": 4.210233406441041, + "velocityY": 1.0352111817725804, + "timestamp": 1.546594427742633 + }, + { + "x": 6.791453813817713, + "y": 7.183061849470797, + "heading": -2.6841297354229963, + "angularVelocity": -0.0000014641708570784773, + "velocityX": 4.210155564947969, + "velocityY": 1.0355277106593013, + "timestamp": 1.5722461862933204 + }, + { + "x": 6.896786766452706, + "y": 7.209560411118278, + "heading": -2.6924335459211086, + "angularVelocity": -0.32371310846796236, + "velocityX": 4.106266337524453, + "velocityY": 1.0330115027053868, + "timestamp": 1.5978979448440078 + }, + { + "x": 6.998488316252308, + "y": 7.234843401534966, + "heading": -2.7129985246425123, + "angularVelocity": -0.8016985923506064, + "velocityX": 3.9647008839039692, + "velocityY": 0.9856240603048587, + "timestamp": 1.6235497033946953 + }, + { + "x": 7.095354057099806, + "y": 7.258847333068417, + "heading": -2.738625094492488, + "angularVelocity": -0.9990180516995718, + "velocityX": 3.776183245140594, + "velocityY": 0.9357616354457536, + "timestamp": 1.6492014619453828 + }, + { + "x": 7.187121562044359, + "y": 7.2815620826343475, + "heading": -2.765681380196159, + "angularVelocity": -1.0547536399973723, + "velocityX": 3.577435237557771, + "velocityY": 0.8855045754873788, + "timestamp": 1.6748532204960702 + }, + { + "x": 7.27376588306665, + "y": 7.302992641940746, + "heading": -2.7929553754669367, + "angularVelocity": -1.063240760546102, + "velocityX": 3.3777146643214673, + "velocityY": 0.8354421106862051, + "timestamp": 1.7005049790467577 + }, + { + "x": 7.355287023426883, + "y": 7.32314481942894, + "heading": -2.819817295961529, + "angularVelocity": -1.047176568480221, + "velocityX": 3.1779942181799417, + "velocityY": 0.785606080315067, + "timestamp": 1.7261567375974451 + }, + { + "x": 7.431689467162592, + "y": 7.342023303561365, + "heading": -2.8458760006675994, + "angularVelocity": -1.0158642595430196, + "velocityX": 2.978448576332041, + "velocityY": 0.7359528234729867, + "timestamp": 1.7518084961481326 + }, + { + "x": 7.502978255871904, + "y": 7.359631767530785, + "heading": -2.870863472244511, + "angularVelocity": -0.9741036478078832, + "velocityX": 2.7790994745427215, + "velocityY": 0.686442761209752, + "timestamp": 1.77746025469882 + }, + { + "x": 7.5691580661290105, + "y": 7.375973138048295, + "heading": -2.8945841408031066, + "angularVelocity": -0.9247190016904283, + "velocityX": 2.5799326828348144, + "velocityY": 0.6370467929214264, + "timestamp": 1.8031120132495075 + }, + { + "x": 7.630233019820151, + "y": 7.391049803070599, + "heading": -2.9168889167473195, + "angularVelocity": -0.869522294159258, + "velocityX": 2.3809265774296597, + "velocityY": 0.5877439159780639, + "timestamp": 1.828763771800195 + }, + { + "x": 7.686206695576408, + "y": 7.404863754066865, + "heading": -2.9376604219188827, + "angularVelocity": -0.809749753823667, + "velocityX": 2.182059980240895, + "velocityY": 0.5385186738355405, + "timestamp": 1.8544155303508825 + }, + { + "x": 7.737082192643966, + "y": 7.417416682243981, + "heading": -2.95680392542072, + "angularVelocity": -0.7462842543137888, + "velocityX": 1.9833142030800017, + "velocityY": 0.489359361164762, + "timestamp": 1.88006728890157 + }, + { + "x": 7.782862202147126, + "y": 7.428710044654634, + "heading": -2.974241445014127, + "angularVelocity": -0.6797787200027948, + "velocityX": 1.784673335853397, + "velocityY": 0.4402568497726161, + "timestamp": 1.9057190474522574 + }, + { + "x": 7.823549072317337, + "y": 7.43874511075604, + "heading": -2.989907725307504, + "angularVelocity": -0.6107292902519902, + "velocityX": 1.5861240113348993, + "velocityY": 0.3912038264969958, + "timestamp": 1.9313708060029449 + }, + { + "x": 7.85914486452642, + "y": 7.447522996121348, + "heading": -3.0037473896814832, + "angularVelocity": -0.5395210759773936, + "velocityX": 1.3876550466801778, + "velocityY": 0.3421942923703846, + "timestamp": 1.9570225645536323 + }, + { + "x": 7.889651400291962, + "y": 7.45504468753028, + "heading": -3.015712859637522, + "angularVelocity": -0.46645807664200445, + "velocityX": 1.18925709148796, + "velocityY": 0.2932232265506935, + "timestamp": 1.9826743231043198 + }, + { + "x": 7.915070300383151, + "y": 7.4613110621213785, + "heading": -3.025762795293749, + "angularVelocity": -0.391783496494747, + "velocityX": 0.9909223198464887, + "velocityY": 0.24428635482108035, + "timestamp": 2.0083260816550075 + }, + { + "x": 7.935403017300441, + "y": 7.466322902324817, + "heading": -3.0338609016195863, + "angularVelocity": -0.31569400241450996, + "velocityX": 0.7926441720208706, + "velocityY": 0.1953799851007025, + "timestamp": 2.033977840205695 + }, + { + "x": 7.950650862283985, + "y": 7.470080907688823, + "heading": -3.0399749988944915, + "angularVelocity": -0.23835002433943484, + "velocityX": 0.5944171411646091, + "velocityY": 0.14650088634584787, + "timestamp": 2.0596295987563824 + }, + { + "x": 7.960815027821627, + "y": 7.472585704328216, + "heading": -3.0440762890820414, + "angularVelocity": -0.159883392768021, + "velocityX": 0.3962365978752573, + "velocityY": 0.09764619585213934, + "timestamp": 2.08528135730707 + }, + { + "x": 7.965896606445312, + "y": 7.473837852478027, + "heading": -3.046138770984786, + "angularVelocity": -0.0804031387816541, + "velocityX": 0.1980986455039259, + "velocityY": 0.04881334538283516, + "timestamp": 2.1109331158577573 + }, + { + "x": 7.965896606445312, + "y": 7.473837852478027, + "heading": -3.046138770984786, + "angularVelocity": 2.257846198921834e-19, + "velocityX": 2.7831811723292994e-20, + "velocityY": -2.187862053904655e-18, + "timestamp": 2.136584874408445 + }, + { + "x": 7.895295442516664, + "y": 7.453869842194936, + "heading": -3.045310713379553, + "angularVelocity": 0.00866143460185216, + "velocityX": -0.7384840865157103, + "velocityY": -0.20886423130853593, + "timestamp": 2.2321876981087567 + }, + { + "x": 7.754093116683618, + "y": 7.413933822201269, + "heading": -3.0436545985981667, + "angularVelocity": 0.0173228647155626, + "velocityX": -1.4769681518578985, + "velocityY": -0.41772845662859964, + "timestamp": 2.3277905218090686 + }, + { + "x": 7.542289632319933, + "y": 7.35402979345122, + "heading": -3.041170427348722, + "angularVelocity": 0.02598428742263653, + "velocityX": -2.2154521819107464, + "velocityY": -0.6265926719678376, + "timestamp": 2.4233933455093806 + }, + { + "x": 7.259884996173116, + "y": 7.274157757853176, + "heading": -3.0378582010749082, + "angularVelocity": 0.034645695028804865, + "velocityX": -2.953936141385066, + "velocityY": -0.8354568673454674, + "timestamp": 2.5189961692096925 + }, + { + "x": 6.906879228485775, + "y": 7.17431772113232, + "heading": -3.033717924090085, + "angularVelocity": 0.04330705751748156, + "velocityX": -3.692419889122889, + "velocityY": -1.0443210028380143, + "timestamp": 2.6145989929100044 + }, + { + "x": 6.508025842761737, + "y": 7.061510678522763, + "heading": -3.0337179232569076, + "angularVelocity": 8.714985822174448e-9, + "velocityX": -4.171983318969032, + "velocityY": -1.1799551335761433, + "timestamp": 2.7102018166103163 + }, + { + "x": 6.155020075077593, + "y": 6.961670641802811, + "heading": -3.0295776429996932, + "angularVelocity": 0.0433070917465069, + "velocityX": -3.692419889089439, + "velocityY": -1.044321002828554, + "timestamp": 2.8058046403106283 + }, + { + "x": 5.872615438933694, + "y": 6.881798606205592, + "heading": -3.0262654137382636, + "angularVelocity": 0.034645726279091356, + "velocityX": -2.953936141354553, + "velocityY": -0.8354568673368374, + "timestamp": 2.90140746401094 + }, + { + "x": 5.660811954572396, + "y": 6.821894577456218, + "heading": -3.023781240040345, + "angularVelocity": 0.025984313033535218, + "velocityX": -2.215452181885791, + "velocityY": -0.6265926719607796, + "timestamp": 2.997010287711252 + }, + { + "x": 5.519609628741025, + "y": 6.781958557463025, + "heading": -3.022125123534869, + "angularVelocity": 0.017322882749443342, + "velocityX": -1.4769681518403805, + "velocityY": -0.41772845662364505, + "timestamp": 3.092613111411564 + }, + { + "x": 5.449008464813232, + "y": 6.761990547180176, + "heading": -3.0212970650452653, + "angularVelocity": 0.008661443852320889, + "velocityX": -0.7384840865067626, + "velocityY": -0.20886423130600526, + "timestamp": 3.188215935111876 + }, + { + "x": 5.449008464813232, + "y": 6.761990547180176, + "heading": -3.0212970650452653, + "angularVelocity": 6.236868827474949e-28, + "velocityX": 9.645303873884742e-19, + "velocityY": -1.3645963517294365e-18, + "timestamp": 3.283818758812188 + }, + { + "x": 5.506534539725527, + "y": 6.740622006309749, + "heading": -3.025242040367869, + "angularVelocity": -0.045114414232386484, + "velocityX": 0.6578634745536448, + "velocityY": -0.2443688808004527, + "timestamp": 3.3712625478921723 + }, + { + "x": 5.621586688038202, + "y": 6.697884925130509, + "heading": -3.0331320221125657, + "angularVelocity": -0.09022918411597865, + "velocityX": 1.3157269318171694, + "velocityY": -0.4887377551783459, + "timestamp": 3.4587063369721567 + }, + { + "x": 5.7941649073731085, + "y": 6.633779304525839, + "heading": -3.044967072221135, + "angularVelocity": -0.13534466236068432, + "velocityX": 1.9735903618843658, + "velocityY": -0.7331066194539356, + "timestamp": 3.546150126052141 + }, + { + "x": 6.024269193849061, + "y": 6.548305145937437, + "heading": -3.060747273757664, + "angularVelocity": -0.1804610905194783, + "velocityX": 2.6314537475666477, + "velocityY": -0.9774754672423787, + "timestamp": 3.6335939151321255 + }, + { + "x": 6.311899540184541, + "y": 6.441462452070085, + "heading": -3.0804726993222316, + "angularVelocity": -0.2255783489268115, + "velocityX": 3.2893170499780955, + "velocityY": -1.2218442840991746, + "timestamp": 3.72103770421211 + }, + { + "x": 6.657055926215122, + "y": 6.313251230414028, + "heading": -3.1041432514952634, + "angularVelocity": -0.2706944932518914, + "velocityX": 3.9471801217907987, + "velocityY": -1.4662130152981419, + "timestamp": 3.8084814932920943 + }, + { + "x": 7.002212276855087, + "y": 6.185040021904109, + "heading": -3.1278203147498056, + "angularVelocity": -0.27076895344602725, + "velocityX": 3.9471797170666147, + "velocityY": -1.4662128649599642, + "timestamp": 3.8959252823720787 + }, + { + "x": 7.289842590220327, + "y": 6.078197340283827, + "heading": -3.1475518152563606, + "angularVelocity": -0.2256478214651337, + "velocityX": 3.289316672933121, + "velocityY": -1.2218441440426708, + "timestamp": 3.983369071452063 + }, + { + "x": 7.5199468481440634, + "y": 5.992723192301384, + "heading": -3.1633372870726673, + "angularVelocity": -0.18052136100675872, + "velocityX": 2.631453421045854, + "velocityY": -0.9774753459535034, + "timestamp": 4.0708128605320475 + }, + { + "x": 7.692525044903304, + "y": 5.9286175800826335, + "heading": -3.1751765140759507, + "angularVelocity": -0.135392428986058, + "velocityX": 1.9735901037109, + "velocityY": -0.7331065235532462, + "timestamp": 4.1582566496120315 + }, + { + "x": 7.807577177702952, + "y": 5.8858805046658365, + "heading": -3.1830693746554464, + "angularVelocity": -0.09026210623462518, + "velocityX": 1.3157267544114646, + "velocityY": -0.48873768927951544, + "timestamp": 4.245700438692015 + }, + { + "x": 7.865103244781494, + "y": 5.864511966705322, + "heading": -3.1870158092601475, + "angularVelocity": -0.04513110246276267, + "velocityX": 0.6578633849674967, + "velocityY": -0.24436884752292995, + "timestamp": 4.333144227771999 + }, + { + "x": 7.865103244781494, + "y": 5.864511966705322, + "heading": -3.1870158092601475, + "angularVelocity": -3.432876184233931e-28, + "velocityX": 4.443954479240038e-19, + "velocityY": 1.2242859016477994e-18, + "timestamp": 4.420588016851983 + }, + { + "x": 7.80727449997893, + "y": 5.886485815257223, + "heading": -3.1831983186843216, + "angularVelocity": 0.043481495025401884, + "velocityX": -0.6586736049542378, + "velocityY": 0.2502837315562369, + "timestamp": 4.50838376935763 + }, + { + "x": 7.6916170112977955, + "y": 5.930433512009924, + "heading": -3.1755634246058455, + "angularVelocity": 0.08696199828100887, + "velocityX": -1.3173471993841017, + "velocityY": 0.5005674591134074, + "timestamp": 4.596179521863277 + }, + { + "x": 7.518130780033237, + "y": 5.996355056471295, + "heading": -3.164111270762368, + "angularVelocity": 0.1304408643543523, + "velocityX": -1.9760207790621667, + "velocityY": 0.7508511810651682, + "timestamp": 4.683975274368924 + }, + { + "x": 7.286815808414609, + "y": 6.084250447794221, + "heading": -3.1488420788016804, + "angularVelocity": 0.17391720584325185, + "velocityX": -2.6346943333477326, + "velocityY": 1.0011348933682513, + "timestamp": 4.771771026874571 + }, + { + "x": 6.997672101306374, + "y": 6.194119684130298, + "heading": -3.1297562098801364, + "angularVelocity": 0.2173894337350346, + "velocityX": -3.2933678322267044, + "velocityY": 1.2514185846178583, + "timestamp": 4.859566779380218 + }, + { + "x": 6.650699674750629, + "y": 6.325962759383826, + "heading": -3.1068544665735547, + "angularVelocity": 0.2608525202299327, + "velocityX": -3.9520411483850135, + "velocityY": 1.501702206437008, + "timestamp": 4.947362531885865 + }, + { + "x": 6.303727218542563, + "y": 6.457805845904685, + "heading": -3.083958400156642, + "angularVelocity": 0.26078786004414234, + "velocityX": -3.952041486127124, + "velocityY": 1.5017023347727347, + "timestamp": 5.035158284391512 + }, + { + "x": 6.0145834840631185, + "y": 6.567675092641313, + "heading": -3.0648777920053, + "angularVelocity": 0.21732951318020463, + "velocityX": -3.293368143986769, + "velocityY": 1.2514187030808899, + "timestamp": 5.122954036897159 + }, + { + "x": 5.783268488823356, + "y": 6.655570492939832, + "heading": -3.0496131525042647, + "angularVelocity": 0.17386535299704567, + "velocityX": -2.6346946023941684, + "velocityY": 1.0011349956008981, + "timestamp": 5.210749789402806 + }, + { + "x": 5.609782238915005, + "y": 6.721492044485497, + "heading": -3.0381646021766326, + "angularVelocity": 0.13039982004705136, + "velocityX": -1.9760209914163132, + "velocityY": 0.7508512617557984, + "timestamp": 5.298545541908453 + }, + { + "x": 5.494124737436081, + "y": 6.76543974610112, + "heading": -3.0305321899890547, + "angularVelocity": 0.08693372936335396, + "velocityX": -1.3173473451518534, + "velocityY": 0.5005675145024446, + "timestamp": 5.3863412944141 + }, + { + "x": 5.436295986175537, + "y": 6.787413597106934, + "heading": -3.0267159569026636, + "angularVelocity": 0.04346717212937656, + "velocityX": -0.6586736785110937, + "velocityY": 0.2502837595064755, + "timestamp": 5.474137046919747 + }, + { + "x": 5.436295986175537, + "y": 6.787413597106934, + "heading": -3.0267159569026636, + "angularVelocity": -3.974435126391844e-21, + "velocityX": 8.842380334460247e-19, + "velocityY": 2.3059926136011616e-18, + "timestamp": 5.561932799425394 + }, + { + "x": 5.4614010785364835, + "y": 6.770412417602454, + "heading": -3.0386899363107447, + "angularVelocity": -0.19391287497961154, + "velocityX": 0.40656497480315146, + "velocityY": -0.2753259783905342, + "timestamp": 5.62368207424451 + }, + { + "x": 5.5116279180197765, + "y": 6.736396670320351, + "heading": -3.062194055668532, + "angularVelocity": -0.3806379820109448, + "velocityX": 0.8133996655090318, + "velocityY": -0.5508687734673186, + "timestamp": 5.6854313490636255 + }, + { + "x": 5.586997122869661, + "y": 6.685349278958885, + "heading": -3.0966469418180393, + "angularVelocity": -0.5579480285465932, + "velocityX": 1.2205682588284168, + "velocityY": -0.8266881110912201, + "timestamp": 5.747180623882741 + }, + { + "x": 5.687534779910401, + "y": 6.6172475013120335, + "heading": -3.1412471867595344, + "angularVelocity": -0.7222796554638842, + "velocityX": 1.6281593158016558, + "velocityY": -1.102875747874692, + "timestamp": 5.808929898701857 + }, + { + "x": 5.813274852979219, + "y": 6.532060138458457, + "heading": -3.1948134699284605, + "angularVelocity": -0.8674803603093312, + "velocityX": 2.036300400889785, + "velocityY": -1.3795686362814745, + "timestamp": 5.870679173520973 + }, + { + "x": 5.964262125973882, + "y": 6.4297440605952545, + "heading": -3.2554414713381568, + "angularVelocity": -0.9818415129132555, + "velocityX": 2.4451667398031227, + "velocityY": -1.6569599912374746, + "timestamp": 5.932428448340088 + }, + { + "x": 6.140548631500476, + "y": 6.3102434255545505, + "heading": -3.3196239769413203, + "angularVelocity": -1.0394050098754253, + "velocityX": 2.854875721909237, + "velocityY": -1.9352556834191568, + "timestamp": 5.994177723159204 + }, + { + "x": 6.34209645810039, + "y": 6.173539563313386, + "heading": -3.379002265616315, + "angularVelocity": -0.9616030123257975, + "velocityX": 3.263970746058413, + "velocityY": -2.213853727701531, + "timestamp": 6.05592699797832 + }, + { + "x": 6.565003089251376, + "y": 6.025257004399179, + "heading": -3.3790032986592844, + "angularVelocity": -0.00001672963727023266, + "velocityX": 3.609866379871756, + "velocityY": -2.4013651876653985, + "timestamp": 6.117676272797436 + }, + { + "x": 6.779539733581947, + "y": 5.865103058909337, + "heading": -3.379003341176066, + "angularVelocity": -6.885389656783489e-7, + "velocityX": 3.4743184427512874, + "velocityY": -2.5936166207455313, + "timestamp": 6.179425547616551 + }, + { + "x": 6.97439432144165, + "y": 5.681508541107178, + "heading": -3.379003385149404, + "angularVelocity": -7.121271958295491e-7, + "velocityX": 3.1555769428952645, + "velocityY": -2.973225488719833, + "timestamp": 6.241174822435667 + }, + { + "x": 7.122909407779235, + "y": 5.511762371505047, + "heading": -3.3790034194155005, + "angularVelocity": -6.586950870648493e-7, + "velocityX": 2.8548964763469873, + "velocityY": -3.2630202992911737, + "timestamp": 6.293196003437515 + }, + { + "x": 7.254405341042785, + "y": 5.328515780926305, + "heading": -3.379003452128528, + "angularVelocity": -6.288405402556099e-7, + "velocityX": 2.527738331409253, + "velocityY": -3.5225380710259566, + "timestamp": 6.345217184439363 + }, + { + "x": 7.369249923871109, + "y": 5.146276495162188, + "heading": -3.406177877810794, + "angularVelocity": -0.5223723329407031, + "velocityX": 2.207650434238391, + "velocityY": -3.503174711809081, + "timestamp": 6.397238365441211 + }, + { + "x": 7.472952308705079, + "y": 4.981547414238825, + "heading": -3.4515343756544477, + "angularVelocity": -0.8718852008000858, + "velocityX": 1.9934646395337656, + "velocityY": -3.1665771086110306, + "timestamp": 6.449259546443059 + }, + { + "x": 7.565086623545703, + "y": 4.835192956514777, + "heading": -3.4989015182220933, + "angularVelocity": -0.9105357021010838, + "velocityX": 1.771092333281533, + "velocityY": -2.8133628438548675, + "timestamp": 6.501280727444907 + }, + { + "x": 7.645656925903314, + "y": 4.707210342234374, + "heading": -3.5438918663970616, + "angularVelocity": -0.864846727977402, + "velocityX": 1.5487980243037662, + "velocityY": -2.460202014172973, + "timestamp": 6.553301908446755 + }, + { + "x": 7.714683601406547, + "y": 4.597567978647708, + "heading": -3.584524535930262, + "angularVelocity": -0.7810793363525659, + "velocityX": 1.3268955870260062, + "velocityY": -2.107648490002, + "timestamp": 6.6053230894486035 + }, + { + "x": 7.772183020683445, + "y": 4.506239107696108, + "heading": -3.6196694404073297, + "angularVelocity": -0.6755883622830359, + "velocityX": 1.105307841335925, + "velocityY": -1.7556093343662444, + "timestamp": 6.6573442704504515 + }, + { + "x": 7.818167453247437, + "y": 4.433202846918064, + "heading": -3.648592309890163, + "angularVelocity": -0.5559825618300757, + "velocityX": 0.8839559517566051, + "velocityY": -1.4039716010186059, + "timestamp": 6.7093654514523 + }, + { + "x": 7.852646237712458, + "y": 4.378442821799572, + "heading": -3.670776152617206, + "angularVelocity": -0.42643866017295573, + "velocityX": 0.6627835777853055, + "velocityY": -1.0526486339582835, + "timestamp": 6.761386632454148 + }, + { + "x": 7.875626660250975, + "y": 4.341945996815336, + "heading": -3.685837677441819, + "angularVelocity": -0.2895267761044795, + "velocityX": 0.4417512654643735, + "velocityY": -0.7015762480082658, + "timestamp": 6.813407813455996 + }, + { + "x": 7.887114524841309, + "y": 4.323701858520508, + "heading": -3.693483193557926, + "angularVelocity": -0.1469692915244614, + "velocityX": 0.22083052266585948, + "velocityY": -0.3507059613694702, + "timestamp": 6.865428994457844 + }, + { + "x": 7.887114524841309, + "y": 4.323701858520508, + "heading": -3.693483193557926, + "angularVelocity": -3.58149095021185e-18, + "velocityX": -4.3323929495550666e-18, + "velocityY": -6.867429667891556e-19, + "timestamp": 6.917450175459692 + }, + { + "x": 7.877220674416456, + "y": 4.339296435616169, + "heading": -3.6860278197150365, + "angularVelocity": 0.1546730005334719, + "velocityX": -0.2052628834300042, + "velocityY": 0.3235330759081108, + "timestamp": 6.965651049672909 + }, + { + "x": 7.8574280453898675, + "y": 4.370495236689797, + "heading": -3.6713431382027326, + "angularVelocity": 0.30465591655776203, + "velocityX": -0.4106280093393107, + "velocityY": 0.6472662909726413, + "timestamp": 7.013851923886126 + }, + { + "x": 7.827730700014785, + "y": 4.417309792394871, + "heading": -3.649709320568005, + "angularVelocity": 0.4488262503091546, + "velocityX": -0.6161163227810843, + "velocityY": 0.9712387268743375, + "timestamp": 7.062052798099343 + }, + { + "x": 7.78812139040693, + "y": 4.479753936181193, + "heading": -3.6214786357466644, + "angularVelocity": 0.5856882324681533, + "velocityX": -0.8217550045387327, + "velocityY": 1.2954981586039502, + "timestamp": 7.11025367231256 + }, + { + "x": 7.738591082271519, + "y": 4.557844393857851, + "heading": -3.5871056587121233, + "angularVelocity": 0.7131193696303854, + "velocityX": -1.0275811163986608, + "velocityY": 1.6201045925272002, + "timestamp": 7.1584545465257765 + }, + { + "x": 7.679128229221916, + "y": 4.651601611491865, + "heading": -3.5472016043309003, + "angularVelocity": 0.8278699304234943, + "velocityX": -1.2336467755038363, + "velocityY": 1.9451352110187774, + "timestamp": 7.2066554207389935 + }, + { + "x": 7.609717807763235, + "y": 4.761050792490485, + "heading": -3.502640608758667, + "angularVelocity": 0.9244852152503467, + "velocityX": -1.44002411972116, + "velocityY": 2.27068871229333, + "timestamp": 7.25485629495221 + }, + { + "x": 7.530340734883155, + "y": 4.886222255013547, + "heading": -3.4547926290801594, + "angularVelocity": 0.992678669413335, + "velocityX": -1.6467973698765865, + "velocityY": 2.5968712096250886, + "timestamp": 7.303057169165427 + }, + { + "x": 7.440978533351249, + "y": 5.027143704553463, + "heading": -3.4061300682182822, + "angularVelocity": 1.0095783874509316, + "velocityX": -1.8539539581089521, + "velocityY": 2.9236284992788755, + "timestamp": 7.351258043378644 + }, + { + "x": 7.341670133172821, + "y": 5.183749888471115, + "heading": -3.362339357555316, + "angularVelocity": 0.9085044903808221, + "velocityX": -2.0603028845314446, + "velocityY": 3.2490320242928163, + "timestamp": 7.399458917591861 + }, + { + "x": 7.233912335944607, + "y": 5.3544068604560495, + "heading": -3.343182068843528, + "angularVelocity": 0.39744691407575744, + "velocityX": -2.2355983991400867, + "velocityY": 3.540536863087266, + "timestamp": 7.447659791805078 + }, + { + "x": 7.111458392005018, + "y": 5.523753439275115, + "heading": -3.3431820280984104, + "angularVelocity": 8.453190557195011e-7, + "velocityX": -2.5404921785841763, + "velocityY": 3.513350775962189, + "timestamp": 7.495860666018295 + }, + { + "x": 6.97439432144165, + "y": 5.681508541107178, + "heading": -3.34318198516747, + "angularVelocity": 8.906672529661562e-7, + "velocityX": -2.8436013412757295, + "velocityY": 3.272868063227098, + "timestamp": 7.544061540231512 + }, + { + "x": 6.7313273316705375, + "y": 5.893656042818141, + "heading": -3.34318194956056, + "angularVelocity": 4.785050059350481e-7, + "velocityX": -3.266466313196734, + "velocityY": 2.8509534281896696, + "timestamp": 7.618474366345502 + }, + { + "x": 6.463571916500496, + "y": 6.0736426038302405, + "heading": -3.343181914758351, + "angularVelocity": 4.6769100676461487e-7, + "velocityX": -3.598242791637579, + "velocityY": 2.4187572279055924, + "timestamp": 7.692887192459493 + }, + { + "x": 6.20686377949356, + "y": 6.2452739854960395, + "heading": -3.3037503885523654, + "angularVelocity": 0.529902279824675, + "velocityX": -3.4497834635886053, + "velocityY": 2.306475786887526, + "timestamp": 7.767300018573483 + }, + { + "x": 5.9863622153072455, + "y": 6.393046429956987, + "heading": -3.2362350016761288, + "angularVelocity": 0.9073084628297773, + "velocityX": -2.9632198600888495, + "velocityY": 1.9858464216179963, + "timestamp": 7.841712844687473 + }, + { + "x": 5.802814692409499, + "y": 6.516118854857836, + "heading": -3.1699341420458658, + "angularVelocity": 0.8909869856137588, + "velocityX": -2.4666113690746707, + "velocityY": 1.6539141345380624, + "timestamp": 7.916125670801463 + }, + { + "x": 5.656112580001017, + "y": 6.614514316298437, + "heading": -3.112369789753736, + "angularVelocity": 0.7735810517930446, + "velocityX": -1.9714627177813826, + "velocityY": 1.3222916878587008, + "timestamp": 7.990538496915454 + }, + { + "x": 5.546164743378319, + "y": 6.688272502359933, + "heading": -3.066929995025816, + "angularVelocity": 0.6106446576605511, + "velocityX": -1.4775387841643228, + "velocityY": 0.9912025911838206, + "timestamp": 8.064951323029444 + }, + { + "x": 5.472907694722162, + "y": 6.737423740742976, + "heading": -3.0355291255006427, + "angularVelocity": 0.4219819507604332, + "velocityX": -0.9844680343673129, + "velocityY": 0.6605210546331426, + "timestamp": 8.139364149143434 + }, + { + "x": 5.436295986175537, + "y": 6.761990547180176, + "heading": -3.019397960613997, + "angularVelocity": 0.21677936088512653, + "velocityX": -0.4920080375732523, + "velocityY": 0.33014209673434414, + "timestamp": 8.213776975257424 + }, + { + "x": 5.436295986175537, + "y": 6.761990547180176, + "heading": -3.019397960613997, + "angularVelocity": -7.216632961030075e-19, + "velocityX": 1.385331577266768e-17, + "velocityY": 1.0244819922206164e-18, + "timestamp": 8.288189801371415 + }, + { + "x": 5.467021331993691, + "y": 6.743681754630262, + "heading": -3.0405281639164583, + "angularVelocity": -0.3132514333002431, + "velocityX": 0.4554976816082698, + "velocityY": -0.2714245303824937, + "timestamp": 8.355644251874146 + }, + { + "x": 5.528456798036073, + "y": 6.706927903016198, + "heading": -3.0820180627519798, + "angularVelocity": -0.6150802286031658, + "velocityX": 0.9107696465467936, + "velocityY": -0.544869187135031, + "timestamp": 8.423098702376878 + }, + { + "x": 5.620575385666307, + "y": 6.6515418667178325, + "heading": -3.142819906623659, + "angularVelocity": -0.9013763127344223, + "velocityX": 1.3656413615955947, + "velocityY": -0.8210879472826135, + "timestamp": 8.49055315287961 + }, + { + "x": 5.743329468675338, + "y": 6.577249796463415, + "heading": -3.2213441024636853, + "angularVelocity": -1.1641069678100113, + "velocityX": 1.8198070267292152, + "velocityY": -1.1013664732382369, + "timestamp": 8.558007603382341 + }, + { + "x": 5.896629362899617, + "y": 6.483625547580609, + "heading": -3.314894408186393, + "angularVelocity": -1.3868663227627636, + "velocityX": 2.272643140396938, + "velocityY": -1.387962516706195, + "timestamp": 8.625462053885073 + }, + { + "x": 6.08026059744355, + "y": 6.369945182455195, + "heading": -3.4183484305078555, + "angularVelocity": -1.5336871259113138, + "velocityX": 2.7222997619185247, + "velocityY": -1.6852908040635355, + "timestamp": 8.692916504387805 + }, + { + "x": 6.293404304224734, + "y": 6.2347548367007635, + "heading": -3.5197782469381993, + "angularVelocity": -1.503678640540327, + "velocityX": 3.1598168125697015, + "velocityY": -2.0041723673808027, + "timestamp": 8.760370954890536 + }, + { + "x": 6.528550724023803, + "y": 6.079659939579352, + "heading": -3.5547887453718556, + "angularVelocity": -0.5190242922850319, + "velocityX": 3.4860030442252885, + "velocityY": -2.2992537329338925, + "timestamp": 8.827825405393268 + }, + { + "x": 6.752670914137969, + "y": 5.89177230101219, + "heading": -3.5547888588953853, + "angularVelocity": -0.0000016829657464890388, + "velocityX": 3.322541188073102, + "velocityY": -2.7854001799266244, + "timestamp": 8.895279855896 + }, + { + "x": 6.95162296295166, + "y": 5.677413463592529, + "heading": -3.5547889362466214, + "angularVelocity": -0.0000011467180563286706, + "velocityX": 2.949428055983232, + "velocityY": -3.177830903996148, + "timestamp": 8.962734306398731 + }, + { + "x": 7.098585597806102, + "y": 5.481458375845758, + "heading": -3.5547889980207357, + "angularVelocity": -0.0000010934442451876683, + "velocityX": 2.6013395651065, + "velocityY": -3.4685396273989286, + "timestamp": 9.01922928953194 + }, + { + "x": 7.2242750181804, + "y": 5.271223782919906, + "heading": -3.5547890511728277, + "angularVelocity": -9.408285364307061e-7, + "velocityX": 2.224789059197234, + "velocityY": -3.7212966756736487, + "timestamp": 9.07572427266515 + }, + { + "x": 7.3273161189708, + "y": 5.049010040930901, + "heading": -3.554789099761621, + "angularVelocity": -8.600550112485466e-7, + "velocityX": 1.823898248583237, + "velocityY": -3.9333358409019663, + "timestamp": 9.13221925579836 + }, + { + "x": 7.406582487237177, + "y": 4.817248829975611, + "heading": -3.554789146622121, + "angularVelocity": -8.294630283569178e-7, + "velocityX": 1.4030691553527146, + "velocityY": -4.102332598433042, + "timestamp": 9.188714238931569 + }, + { + "x": 7.478638737084507, + "y": 4.58314561993378, + "heading": -3.5547891933302385, + "angularVelocity": -8.267657533220866e-7, + "velocityX": 1.2754451077085862, + "velocityY": -4.1437875906580794, + "timestamp": 9.245209222064778 + }, + { + "x": 7.550696000844859, + "y": 4.349042721974096, + "heading": -3.554789240038353, + "angularVelocity": -8.267657147462614e-7, + "velocityX": 1.2754630546653534, + "velocityY": -4.143782066589741, + "timestamp": 9.301704205197987 + }, + { + "x": 7.62275325466431, + "y": 4.114939820954579, + "heading": -3.5547892867464688, + "angularVelocity": -8.267657143276858e-7, + "velocityX": 1.2754628787046107, + "velocityY": -4.1437821207509, + "timestamp": 9.358199188331197 + }, + { + "x": 7.694810030922688, + "y": 3.880836772951051, + "heading": -3.5547893334838476, + "angularVelocity": -8.2728370553242e-7, + "velocityX": 1.2754544255456055, + "velocityY": -4.143784722468827, + "timestamp": 9.414694171464406 + }, + { + "x": 7.761719458964053, + "y": 3.664123207681456, + "heading": -3.603373681714835, + "angularVelocity": -0.8599763295163799, + "velocityX": 1.1843428271073144, + "velocityY": -3.8359789356624097, + "timestamp": 9.471189154597615 + }, + { + "x": 7.82148239293419, + "y": 3.4701821668493893, + "heading": -3.6781611768684126, + "angularVelocity": -1.3237900253414745, + "velocityX": 1.0578449741142784, + "velocityY": -3.432889613840102, + "timestamp": 9.527684137730825 + }, + { + "x": 7.8737258923647415, + "y": 3.300733490685395, + "heading": -3.7535206924966134, + "angularVelocity": -1.333915180583573, + "velocityX": 0.9247458187104354, + "velocityY": -2.9993579388182336, + "timestamp": 9.584179120864034 + }, + { + "x": 7.918466935683176, + "y": 3.155694031456225, + "heading": -3.822908621298775, + "angularVelocity": -1.228213992710683, + "velocityX": 0.7919471931328769, + "velocityY": -2.5672980357774713, + "timestamp": 9.640674103997243 + }, + { + "x": 7.955727725126361, + "y": 3.0349643646800413, + "heading": -3.88346036229386, + "angularVelocity": -1.071807400177649, + "velocityX": 0.6595415623955151, + "velocityY": -2.1369980143460783, + "timestamp": 9.697169087130453 + }, + { + "x": 7.985522348817118, + "y": 2.938469798831404, + "heading": -3.9335348241903825, + "angularVelocity": -0.8863523647480636, + "velocityX": 0.527385301107251, + "velocityY": -1.7080200842101736, + "timestamp": 9.753664070263662 + }, + { + "x": 8.00786007413313, + "y": 2.8661541802383343, + "heading": -3.9720495265001765, + "angularVelocity": -0.6817366812727442, + "velocityX": 0.3953930787684817, + "velocityY": -1.2800361126324549, + "timestamp": 9.810159053396871 + }, + { + "x": 8.022747512167825, + "y": 2.817974467927218, + "heading": -3.998236759250909, + "angularVelocity": -0.4635320040539836, + "velocityX": 0.26351787732356136, + "velocityY": -0.8528139958465588, + "timestamp": 9.86665403653008 + }, + { + "x": 8.030189514160156, + "y": 2.7938973903656006, + "heading": -4.0115353139615895, + "angularVelocity": -0.2353935513056714, + "velocityX": 0.13172854613983054, + "velocityY": -0.4261808080346905, + "timestamp": 9.92314901966329 + }, + { + "x": 8.030189514160156, + "y": 2.7938973903656006, + "heading": -4.0115353139615895, + "angularVelocity": -9.739346179638407e-20, + "velocityX": -1.4888070530854099e-18, + "velocityY": 1.5026054090472747e-18, + "timestamp": 9.9796440027965 + }, + { + "x": 8.023575299667664, + "y": 2.8153265027144045, + "heading": -3.9982317028281145, + "angularVelocity": 0.2490951997345419, + "velocityX": -0.12384374915683626, + "velocityY": 0.40123609801159893, + "timestamp": 10.033051740672029 + }, + { + "x": 8.010342464279917, + "y": 2.858214474225702, + "heading": -3.9720678856506844, + "angularVelocity": 0.4898881364046355, + "velocityX": -0.24777000326411402, + "velocityY": 0.8030291717513138, + "timestamp": 10.086459478547559 + }, + { + "x": 7.990485842450547, + "y": 2.9225981919232042, + "heading": -3.933611374260751, + "angularVelocity": 0.7200550504415586, + "velocityX": -0.37179297643436726, + "velocityY": 1.2055129136447036, + "timestamp": 10.139867216423088 + }, + { + "x": 7.963999104860506, + "y": 3.008522703065025, + "heading": -3.88358340210928, + "angularVelocity": 0.9367176769041401, + "velocityX": -0.4959344590061064, + "velocityY": 1.6088401149300549, + "timestamp": 10.193274954298618 + }, + { + "x": 7.930873132180931, + "y": 3.116042932711411, + "heading": -3.8229207451243807, + "angularVelocity": 1.1358402246183505, + "velocityX": -0.6202466907843547, + "velocityY": 2.013195726375272, + "timestamp": 10.246682692174147 + }, + { + "x": 7.891092490084177, + "y": 3.2452269626181964, + "heading": -3.7529191339342174, + "angularVelocity": 1.3107016693593585, + "velocityX": -0.7448479130396041, + "velocityY": 2.4188260923512273, + "timestamp": 10.300090430049677 + }, + { + "x": 7.8446301181344165, + "y": 3.3961613863205478, + "heading": -3.6755920589128284, + "angularVelocity": 1.4478627647852322, + "velocityX": -0.8699558116100112, + "velocityY": 2.8260778251667436, + "timestamp": 10.353498167925206 + }, + { + "x": 7.79144486607711, + "y": 3.568948460788326, + "heading": -3.594682943142528, + "angularVelocity": 1.5149324608891876, + "velocityX": -0.9958342025505497, + "velocityY": 3.2352442050713903, + "timestamp": 10.406905905800736 + }, + { + "x": 7.731528693856673, + "y": 3.7635325092715983, + "heading": -3.5194115433511453, + "angularVelocity": 1.409372551348413, + "velocityX": -1.1218631345157435, + "velocityY": 3.643368100269756, + "timestamp": 10.460313643676265 + }, + { + "x": 7.670369210430734, + "y": 3.975507979931414, + "heading": -3.489780134920616, + "angularVelocity": 0.554814894043769, + "velocityX": -1.1451427425830363, + "velocityY": 3.969002977692863, + "timestamp": 10.513721381551795 + }, + { + "x": 7.6026179901960935, + "y": 4.19693105080222, + "heading": -3.489780079264961, + "angularVelocity": 0.0000010420897352541278, + "velocityX": -1.2685656223175075, + "velocityY": 4.145898697054893, + "timestamp": 10.567129119427324 + }, + { + "x": 7.534866230416538, + "y": 4.418353956582559, + "heading": -3.4897800236112295, + "angularVelocity": 0.0000010420537160061253, + "velocityX": -1.2685757246909648, + "velocityY": 4.145895605921018, + "timestamp": 10.620536857302854 + }, + { + "x": 7.467114460621537, + "y": 4.639776859298328, + "heading": -3.4897799679574972, + "angularVelocity": 0.0000010420537157088877, + "velocityX": -1.2685759122189693, + "velocityY": 4.145895548540395, + "timestamp": 10.673944595178384 + }, + { + "x": 7.399363331617181, + "y": 4.861199958084797, + "heading": -3.4897799123037565, + "angularVelocity": 0.0000010420538852908062, + "velocityX": -1.2685639141327167, + "velocityY": 4.145899219744388, + "timestamp": 10.727352333053913 + }, + { + "x": 7.323806767003022, + "y": 5.080082597053995, + "heading": -3.4897798565337, + "angularVelocity": 0.0000010442317738037892, + "velocityX": -1.4147119428695523, + "velocityY": 4.098331958550998, + "timestamp": 10.780760070929443 + }, + { + "x": 7.227002735387266, + "y": 5.290433216494062, + "heading": -3.489779799010186, + "angularVelocity": 0.000001077063294583615, + "velocityX": -1.8125469354527803, + "velocityY": 3.9385794607198172, + "timestamp": 10.834167808804972 + }, + { + "x": 7.109896904403833, + "y": 5.490194641427451, + "heading": -3.4897797367177428, + "angularVelocity": 0.0000011663561406035081, + "velocityX": -2.1926753620675226, + "velocityY": 3.740308668360906, + "timestamp": 10.887575546680502 + }, + { + "x": 6.973634243011475, + "y": 5.677413463592529, + "heading": -3.4897796638429868, + "angularVelocity": 0.0000013644980810784123, + "velocityX": -2.551365528903858, + "velocityY": 3.50546249686525, + "timestamp": 10.940983284556031 + }, + { + "x": 6.772589320106531, + "y": 5.8912480994990855, + "heading": -3.4897796014962164, + "angularVelocity": 9.209868538745789e-7, + "velocityX": -2.9698367649130955, + "velocityY": 3.1587664794069745, + "timestamp": 11.008678898429553 + }, + { + "x": 6.54637425079476, + "y": 6.0782535940174505, + "heading": -3.4897795323629706, + "angularVelocity": 0.0000010212367041132869, + "velocityX": -3.341650313333725, + "velocityY": 2.7624462475184366, + "timestamp": 11.076374512303074 + }, + { + "x": 6.306507010011589, + "y": 6.232281236398436, + "heading": -3.463607168318969, + "angularVelocity": 0.38661831315837925, + "velocityX": -3.5433202693357058, + "velocityY": 2.2752972248507035, + "timestamp": 11.144070126176596 + }, + { + "x": 6.091413412046711, + "y": 6.369010367225917, + "heading": -3.373216136469404, + "angularVelocity": 1.3352568457159812, + "velocityX": -3.1773638742200982, + "velocityY": 2.019763512048771, + "timestamp": 11.211765740050117 + }, + { + "x": 5.906610596748241, + "y": 6.484891166356038, + "heading": -3.279739146420341, + "angularVelocity": 1.3808426381022285, + "velocityX": -2.7299082573318043, + "velocityY": 1.711791835533487, + "timestamp": 11.279461353923638 + }, + { + "x": 5.752541410695039, + "y": 6.58077269135908, + "heading": -3.1947588504378728, + "angularVelocity": 1.2553294241668493, + "velocityX": -2.275910908217137, + "velocityY": 1.4163624423612229, + "timestamp": 11.34715696779716 + }, + { + "x": 5.629279463345689, + "y": 6.6570975887825545, + "heading": -3.123126667114855, + "angularVelocity": 1.0581510266950425, + "velocityX": -1.8208261997541688, + "velocityY": 1.1274718265510586, + "timestamp": 11.414852581670681 + }, + { + "x": 5.536838294747283, + "y": 6.714128828465892, + "heading": -3.067440339641402, + "angularVelocity": 0.8225987517816818, + "velocityX": -1.3655414776372279, + "velocityY": 0.8424658027312018, + "timestamp": 11.482548195544203 + }, + { + "x": 5.47521645832478, + "y": 6.752037909221401, + "heading": -3.029307242162313, + "angularVelocity": 0.5633023366969564, + "velocityX": -0.910278124335105, + "velocityY": 0.5599931603594854, + "timestamp": 11.550243809417724 + }, + { + "x": 5.444408416748047, + "y": 6.770946025848389, + "heading": -3.0098303253058174, + "angularVelocity": 0.28771312854751757, + "velocityX": -0.4550965685058097, + "velocityY": 0.2793108082646994, + "timestamp": 11.617939423291245 + }, + { + "x": 5.444408416748047, + "y": 6.770946025848389, + "heading": -3.0098303253058174, + "angularVelocity": 4.439381970105003e-20, + "velocityX": -1.0620325200300552e-18, + "velocityY": -2.934155998551033e-18, + "timestamp": 11.685635037164767 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/453.1.traj b/src/main/deploy/choreo/453.1.traj new file mode 100644 index 0000000..090f41a --- /dev/null +++ b/src/main/deploy/choreo/453.1.traj @@ -0,0 +1,283 @@ +{ + "samples": [ + { + "x": 1.402008891105652, + "y": 3.348797559738159, + "heading": 1.986288521819101, + "angularVelocity": -4.772790939425341e-21, + "velocityX": -1.8126451827293707e-20, + "velocityY": 3.628710439908743e-20, + "timestamp": 0 + }, + { + "x": 1.4486883841972304, + "y": 3.3293922637608246, + "heading": 2.030724464534454, + "angularVelocity": 0.6364534985554904, + "velocityX": 0.6685877439181028, + "velocityY": -0.2779409586152936, + "timestamp": 0.06981805083357366 + }, + { + "x": 1.5420268342222572, + "y": 3.290138269622852, + "heading": 2.1189457404378427, + "angularVelocity": 1.2635883535861399, + "velocityX": 1.3368813496028258, + "velocityY": -0.5622327416665235, + "timestamp": 0.13963610166714732 + }, + { + "x": 1.6821658927549905, + "y": 3.230725535127495, + "heading": 2.24987954869418, + "angularVelocity": 1.8753575428286537, + "velocityX": 2.0072038227876763, + "velocityY": -0.8509652415960443, + "timestamp": 0.20945415250072097 + }, + { + "x": 1.8698641645684424, + "y": 3.1508349518156304, + "heading": 2.4175769819232236, + "angularVelocity": 2.401920867553102, + "velocityX": 2.688391749303789, + "velocityY": -1.1442683139679877, + "timestamp": 0.27927220333429464 + }, + { + "x": 2.1065173503150554, + "y": 3.048807421420711, + "heading": 2.5949528195462372, + "angularVelocity": 2.540544107222745, + "velocityX": 3.3895702174603337, + "velocityY": -1.461334557135146, + "timestamp": 0.3490902541678683 + }, + { + "x": 2.376491888321871, + "y": 2.930418013607003, + "heading": 2.658710182192554, + "angularVelocity": 0.9131931052944469, + "velocityX": 3.866830064482291, + "velocityY": -1.695684803574854, + "timestamp": 0.418908305001442 + }, + { + "x": 2.6674986540228987, + "y": 2.8147717400807317, + "heading": 2.658710349301536, + "angularVelocity": 0.0000023934925125177552, + "velocityX": 4.168073474218082, + "velocityY": -1.6563950460596366, + "timestamp": 0.48872635583501567 + }, + { + "x": 2.9585059641387246, + "y": 2.6991268364960277, + "heading": 2.658710516400385, + "angularVelocity": 0.000002393347381270105, + "velocityX": 4.168081271840486, + "velocityY": -1.6563754244639193, + "timestamp": 0.5585444066685893 + }, + { + "x": 3.249513274191479, + "y": 2.5834819327526177, + "heading": 2.658710683499246, + "angularVelocity": 0.0000023933475558782913, + "velocityX": 4.1680812709371295, + "velocityY": -1.6563754267370547, + "timestamp": 0.6283624575021629 + }, + { + "x": 3.540520584244246, + "y": 2.4678370290092424, + "heading": 2.6587108505981196, + "angularVelocity": 0.0000023933477293082605, + "velocityX": 4.168081270937306, + "velocityY": -1.6563754267365562, + "timestamp": 0.6981805083357365 + }, + { + "x": 3.8315278942970123, + "y": 2.3521921252658693, + "heading": 2.6587110176970046, + "angularVelocity": 0.0000023933479006784206, + "velocityX": 4.168081270937295, + "velocityY": -1.6563754267365276, + "timestamp": 0.7679985591693101 + }, + { + "x": 4.122535204349912, + "y": 2.236547221522837, + "heading": 2.658711184795902, + "angularVelocity": 0.000002393348074985324, + "velocityX": 4.168081270939212, + "velocityY": -1.6563754267316464, + "timestamp": 0.8378166100028838 + }, + { + "x": 4.413542514402566, + "y": 2.1209023177791893, + "heading": 2.6587113518948113, + "angularVelocity": 0.0000023933482457405575, + "velocityX": 4.168081270935688, + "velocityY": -1.6563754267404591, + "timestamp": 0.9076346608364574 + }, + { + "x": 4.704549824293959, + "y": 2.00525741362975, + "heading": 2.6587115189937336, + "angularVelocity": 0.0000023933484214965284, + "velocityX": 4.16808126862595, + "velocityY": -1.6563754325525946, + "timestamp": 0.977452711670031 + }, + { + "x": 4.995557455494251, + "y": 1.889613318020985, + "heading": 2.6587116860924342, + "angularVelocity": 0.000002393345257841174, + "velocityX": 4.168085870715172, + "velocityY": -1.656363851870161, + "timestamp": 1.0472707625036046 + }, + { + "x": 5.29724334339808, + "y": 1.8056810636761744, + "heading": 2.658711846333939, + "angularVelocity": 0.0000022951300252156057, + "velocityX": 4.3210299385607565, + "velocityY": -1.20215693997074, + "timestamp": 1.1170888133371784 + }, + { + "x": 5.608850955963135, + "y": 1.7747021913528442, + "heading": 2.658712011161958, + "angularVelocity": 0.0000023608224121896315, + "velocityX": 4.463138240679893, + "velocityY": -0.44370863914799147, + "timestamp": 1.186906864170752 + }, + { + "x": 5.875229984253185, + "y": 1.787380524511446, + "heading": 2.658712143051779, + "angularVelocity": 0.0000022181754586112354, + "velocityX": 4.480068446986742, + "velocityY": 0.2132292497230326, + "timestamp": 1.2463655602692938 + }, + { + "x": 6.136897172641307, + "y": 1.838847151157444, + "heading": 2.6587122514613757, + "angularVelocity": 0.0000018232757260252147, + "velocityX": 4.400822849435795, + "velocityY": 0.865586197193116, + "timestamp": 1.3058242563678355 + }, + { + "x": 6.388924893224618, + "y": 1.9260283435917724, + "heading": 2.658712354170833, + "angularVelocity": 0.0000017274085020042764, + "velocityX": 4.238702445906684, + "velocityY": 1.466247969680363, + "timestamp": 1.3652829524663772 + }, + { + "x": 6.64088166554894, + "y": 2.013414367297805, + "heading": 2.6587124568709988, + "angularVelocity": 0.0000017272522313604299, + "velocityX": 4.2375092098681515, + "velocityY": 1.4696929034771757, + "timestamp": 1.424741648564919 + }, + { + "x": 6.892839053321575, + "y": 2.1007986164800005, + "heading": 2.6587125595759944, + "angularVelocity": 0.0000017273334665849146, + "velocityX": 4.237519560722661, + "velocityY": 1.4696630588294823, + "timestamp": 1.4842003446634606 + }, + { + "x": 7.136636450310397, + "y": 2.1779815587686944, + "heading": 2.688070177118974, + "angularVelocity": 0.49374808849364216, + "velocityX": 4.100281590177727, + "velocityY": 1.2980934220417046, + "timestamp": 1.5436590407620023 + }, + { + "x": 7.352651079830789, + "y": 2.253382751730419, + "heading": 2.7899600332166923, + "angularVelocity": 1.7136241253736015, + "velocityX": 3.633019956616387, + "velocityY": 1.2681272531903705, + "timestamp": 1.603117736860544 + }, + { + "x": 7.53203786297685, + "y": 2.315567813162813, + "heading": 2.899685711550104, + "angularVelocity": 1.8454101003419598, + "velocityX": 3.016998267986917, + "velocityY": 1.0458530965652777, + "timestamp": 1.6625764329590857 + }, + { + "x": 7.674899201290455, + "y": 2.3648328695682928, + "heading": 2.997122096012855, + "angularVelocity": 1.6387238680994411, + "velocityX": 2.4026988092177444, + "velocityY": 0.8285593132387635, + "timestamp": 1.7220351290576275 + }, + { + "x": 7.781659482434556, + "y": 2.401488912698896, + "heading": 3.0749787829720594, + "angularVelocity": 1.3094247278845657, + "velocityX": 1.7955368709593682, + "velocityY": 0.6164959128914116, + "timestamp": 1.7814938251561692 + }, + { + "x": 7.852631290421507, + "y": 2.4257708157921387, + "heading": 3.1291699090572695, + "angularVelocity": 0.9114079124002065, + "velocityX": 1.1936320949475463, + "velocityY": 0.4083827040707386, + "timestamp": 1.8409525212547109 + }, + { + "x": 7.888041019439697, + "y": 2.4378485679626465, + "heading": 3.1570845336658366, + "angularVelocity": 0.46947925938879653, + "velocityX": 0.5955349064416933, + "velocityY": 0.20312843979105816, + "timestamp": 1.9004112173532526 + }, + { + "x": 7.888041019439697, + "y": 2.4378485679626465, + "heading": 3.1570845336658366, + "angularVelocity": -6.5675658801427375e-21, + "velocityX": -1.9943274045954098e-20, + "velocityY": -1.41680586488362e-19, + "timestamp": 1.9598699134517943 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/453.2.traj b/src/main/deploy/choreo/453.2.traj new file mode 100644 index 0000000..e535275 --- /dev/null +++ b/src/main/deploy/choreo/453.2.traj @@ -0,0 +1,193 @@ +{ + "samples": [ + { + "x": 7.888041019439697, + "y": 2.4378485679626465, + "heading": 3.1570845336658366, + "angularVelocity": -6.5675658801427375e-21, + "velocityX": -1.9943274045954098e-20, + "velocityY": -1.41680586488362e-19, + "timestamp": 0 + }, + { + "x": 7.858249340154391, + "y": 2.4252980054611535, + "heading": 3.1411221298856744, + "angularVelocity": -0.29219537518715366, + "velocityX": -0.5453433596914764, + "velocityY": -0.22974085666791233, + "timestamp": 0.05462921433967982 + }, + { + "x": 7.798616091241273, + "y": 2.4001661825898526, + "heading": 3.109940998101945, + "angularVelocity": -0.5707775987743037, + "velocityX": -1.0915999732729573, + "velocityY": -0.46004364468860043, + "timestamp": 0.10925842867935986 + }, + { + "x": 7.7090741082999905, + "y": 2.3624118351876593, + "heading": 3.064591493757076, + "angularVelocity": -0.8301328308126376, + "velocityX": -1.6390860462410948, + "velocityY": -0.6911017824170038, + "timestamp": 0.1638876430190399 + }, + { + "x": 7.589529913625826, + "y": 2.31198041430475, + "heading": 3.0066355317093274, + "angularVelocity": -1.0608968616569114, + "velocityX": -2.1882832495237374, + "velocityY": -0.9231584508854718, + "timestamp": 0.21851685735871995 + }, + { + "x": 7.439843733345756, + "y": 2.248801164332235, + "heading": 2.938737698881861, + "angularVelocity": -1.242885032270157, + "velocityX": -2.7400390448475846, + "velocityY": -1.1565103166168895, + "timestamp": 0.2731460716984 + }, + { + "x": 7.259812429071698, + "y": 2.1727924683021866, + "heading": 2.866995100648553, + "angularVelocity": -1.3132643238692565, + "velocityX": -3.2955133338480462, + "velocityY": -1.3913561992203078, + "timestamp": 0.32777528603808004 + }, + { + "x": 7.050685712931125, + "y": 2.084647731659991, + "heading": 2.8184896362477745, + "angularVelocity": -0.887903386257304, + "velocityX": -3.8281113625438707, + "velocityY": -1.6135091398920511, + "timestamp": 0.3824045003777601 + }, + { + "x": 6.825086195412971, + "y": 1.9890469912053048, + "heading": 2.8184895683294764, + "angularVelocity": -0.000001243259649552996, + "velocityX": -4.129649680030092, + "velocityY": -1.7499929590831957, + "timestamp": 0.4370337147174401 + }, + { + "x": 6.599487919525426, + "y": 1.8934433207944097, + "heading": 2.8184895004083503, + "angularVelocity": -0.0000012433114214237418, + "velocityX": -4.129626951703787, + "velocityY": -1.750046592587612, + "timestamp": 0.49166292905712017 + }, + { + "x": 6.3690722425418835, + "y": 1.8101170974402798, + "heading": 2.818489431491537, + "angularVelocity": -0.0000012615376929961426, + "velocityX": -4.217810557384865, + "velocityY": -1.5253051752861435, + "timestamp": 0.5462921433968002 + }, + { + "x": 6.129552771918778, + "y": 1.75849282334362, + "heading": 2.8184893560491466, + "angularVelocity": -0.000001380989851962125, + "velocityX": -4.384457538301637, + "velocityY": -0.944993896043691, + "timestamp": 0.6009213577364803 + }, + { + "x": 5.885269812104598, + "y": 1.7395066427805248, + "heading": 2.818489264333038, + "angularVelocity": -0.0000016788839079463646, + "velocityX": -4.471654274492206, + "velocityY": -0.34754628622408146, + "timestamp": 0.6555505720761603 + }, + { + "x": 5.640650272369385, + "y": 1.7535027265548706, + "heading": 2.818488970175976, + "angularVelocity": -0.000005384610881826656, + "velocityX": -4.477815445307147, + "velocityY": 0.25620144722051685, + "timestamp": 0.7101797864158403 + }, + { + "x": 5.317111218010938, + "y": 1.8335830693812192, + "heading": 2.805254564148882, + "angularVelocity": -0.17607305299809856, + "velocityX": -4.304425068143131, + "velocityY": 1.0654041003202679, + "timestamp": 0.7853440764746791 + }, + { + "x": 5.032821589363983, + "y": 1.9501424980664932, + "heading": 2.7103212385268245, + "angularVelocity": -1.2630110062603335, + "velocityX": -3.7822432490802944, + "velocityY": 1.550728791478392, + "timestamp": 0.8605083665335178 + }, + { + "x": 4.809796020651664, + "y": 2.054633591251526, + "heading": 2.6147072284980224, + "angularVelocity": -1.272066960972499, + "velocityX": -2.967174552406934, + "velocityY": 1.3901693623825526, + "timestamp": 0.9356726565923565 + }, + { + "x": 4.644755427475796, + "y": 2.1369592776048787, + "heading": 2.535476805564598, + "angularVelocity": -1.0540966045365723, + "velocityX": -2.1957314177606606, + "velocityY": 1.0952765773335758, + "timestamp": 1.0108369466511953 + }, + { + "x": 4.535762508218388, + "y": 2.1934508330248605, + "heading": 2.4794362763754623, + "angularVelocity": -0.7455738508973778, + "velocityX": -1.4500625120264967, + "velocityY": 0.7515743895905873, + "timestamp": 1.086001236710034 + }, + { + "x": 4.4816508293151855, + "y": 2.2222511768341064, + "heading": 2.4502412669675735, + "angularVelocity": -0.38841595370667265, + "velocityX": -0.7199120601131723, + "velocityY": 0.38316524757569187, + "timestamp": 1.1611655267688727 + }, + { + "x": 4.4816508293151855, + "y": 2.2222511768341064, + "heading": 2.4502412669675735, + "angularVelocity": -1.695796641331656e-20, + "velocityX": 6.611674129926287e-21, + "velocityY": -2.0802489677537025e-21, + "timestamp": 1.2363298168277115 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/453.3.traj b/src/main/deploy/choreo/453.3.traj new file mode 100644 index 0000000..3d55efe --- /dev/null +++ b/src/main/deploy/choreo/453.3.traj @@ -0,0 +1,148 @@ +{ + "samples": [ + { + "x": 4.4816508293151855, + "y": 2.2222511768341064, + "heading": 2.4502412669675735, + "angularVelocity": -1.695796641331656e-20, + "velocityX": 6.611674129926287e-21, + "velocityY": -2.0802489677537025e-21, + "timestamp": 0 + }, + { + "x": 4.549872957098698, + "y": 2.1931238398910824, + "heading": 2.4796703273668, + "angularVelocity": 0.3568867765585547, + "velocityX": 0.8273310443599396, + "velocityY": -0.353227770452506, + "timestamp": 0.0824604954070014 + }, + { + "x": 4.686495078566755, + "y": 2.1347932264206446, + "heading": 2.5354421814878645, + "angularVelocity": 0.6763463382773813, + "velocityX": 1.6568190718929057, + "velocityY": -0.7073764616927737, + "timestamp": 0.1649209908140028 + }, + { + "x": 4.891792155516278, + "y": 2.047141941888709, + "heading": 2.6118695897899324, + "angularVelocity": 0.9268366376510857, + "velocityX": 2.489641566379575, + "velocityY": -1.0629487986861335, + "timestamp": 0.2473814862210042 + }, + { + "x": 5.166150185128691, + "y": 1.9300051853673699, + "heading": 2.695625209420133, + "angularVelocity": 1.0157059961476873, + "velocityX": 3.327145056044848, + "velocityY": -1.420519679674317, + "timestamp": 0.3298419816280056 + }, + { + "x": 5.504185836826239, + "y": 1.785681361915343, + "heading": 2.703998978176573, + "angularVelocity": 0.10154885336438363, + "velocityX": 4.099364793154602, + "velocityY": -1.7502177586938532, + "timestamp": 0.412302477035007 + }, + { + "x": 5.844328329121435, + "y": 1.6404580259742356, + "heading": 2.703999042124718, + "angularVelocity": 7.755003720294035e-7, + "velocityX": 4.124914489251482, + "velocityY": -1.7611261637991182, + "timestamp": 0.4947629724420084 + }, + { + "x": 6.1844708214166335, + "y": 1.4952346900331273, + "heading": 2.703999106072851, + "angularVelocity": 7.755002271176481e-7, + "velocityX": 4.124914489251517, + "velocityY": -1.7611261637991327, + "timestamp": 0.5772234678490098 + }, + { + "x": 6.524613313711832, + "y": 1.350011354092019, + "heading": 2.7039991700209836, + "angularVelocity": 7.755002268276392e-7, + "velocityX": 4.124914489251517, + "velocityY": -1.761126163799133, + "timestamp": 0.6596839632560112 + }, + { + "x": 6.864755806007028, + "y": 1.2047880181509114, + "heading": 2.7039992339691272, + "angularVelocity": 7.755003544824911e-7, + "velocityX": 4.124914489251488, + "velocityY": -1.7611261637991202, + "timestamp": 0.7421444586630126 + }, + { + "x": 7.203138205815544, + "y": 1.0603161511037837, + "heading": 2.711044021755172, + "angularVelocity": 0.08543227579792817, + "velocityX": 4.103569814107435, + "velocityY": -1.7520130861942547, + "timestamp": 0.8246049540700144 + }, + { + "x": 7.477630170379687, + "y": 0.9431222112521354, + "heading": 2.7918538144189164, + "angularVelocity": 0.979981896360077, + "velocityX": 3.328769287758077, + "velocityY": -1.4212131430112402, + "timestamp": 0.9070654494770163 + }, + { + "x": 7.683053573536903, + "y": 0.8554169919319458, + "heading": 2.8658518961937767, + "angularVelocity": 0.8973761485378771, + "velocityX": 2.491173526708827, + "velocityY": -1.0636028668916144, + "timestamp": 0.9895259448840181 + }, + { + "x": 7.819769980022167, + "y": 0.7970461236123249, + "heading": 2.919972038595159, + "angularVelocity": 0.6563159987611107, + "velocityX": 1.65796246809422, + "velocityY": -0.7078646330163175, + "timestamp": 1.07198644029102 + }, + { + "x": 7.888041019439697, + "y": 0.76789790391922, + "heading": 2.948608540855105, + "angularVelocity": 0.34727540889251207, + "velocityX": 0.8279241966782274, + "velocityY": -0.353481015960886, + "timestamp": 1.1544469356980218 + }, + { + "x": 7.888041019439697, + "y": 0.76789790391922, + "heading": 2.948608540855105, + "angularVelocity": -2.222034632801661e-28, + "velocityX": -9.857931768481573e-20, + "velocityY": -2.2471643962624357e-19, + "timestamp": 1.2369074311050237 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/453.4.traj b/src/main/deploy/choreo/453.4.traj new file mode 100644 index 0000000..caeeab2 --- /dev/null +++ b/src/main/deploy/choreo/453.4.traj @@ -0,0 +1,148 @@ +{ + "samples": [ + { + "x": 7.888041019439697, + "y": 0.76789790391922, + "heading": 2.948608540855105, + "angularVelocity": -2.222034632801661e-28, + "velocityX": -9.857931768481573e-20, + "velocityY": -2.2471643962624357e-19, + "timestamp": 0 + }, + { + "x": 7.819855304083459, + "y": 0.7962161310448899, + "heading": 2.9201779748674546, + "angularVelocity": -0.34572299107579807, + "velocityX": -0.8291558272825678, + "velocityY": 0.3443569216350881, + "timestamp": 0.08223510359892927 + }, + { + "x": 7.683310667089189, + "y": 0.8529245196420355, + "heading": 2.8664329957677785, + "angularVelocity": -0.653552762112354, + "velocityX": -1.6604178874780198, + "velocityY": 0.6895885834073882, + "timestamp": 0.16447020719785854 + }, + { + "x": 7.478144407945387, + "y": 0.9381321719282073, + "heading": 2.793015918109701, + "angularVelocity": -0.8927705377029863, + "velocityX": -2.494874453425899, + "velocityY": 1.0361469561920882, + "timestamp": 0.2467053107967878 + }, + { + "x": 7.203998785219726, + "y": 1.051987661590431, + "heading": 2.71291669528747, + "angularVelocity": -0.974027140682948, + "velocityX": -3.3336812471557438, + "velocityY": 1.3845120232049635, + "timestamp": 0.3289404143957171 + }, + { + "x": 6.865719109224617, + "y": 1.192478681546433, + "heading": 2.7037114201062042, + "angularVelocity": -0.11193851261087887, + "velocityX": -4.113567821899265, + "velocityY": 1.7084069187921709, + "timestamp": 0.41117551799464636 + }, + { + "x": 6.525091311846971, + "y": 1.3339449004027024, + "heading": 2.703711355942481, + "angularVelocity": -7.802473717689034e-7, + "velocityX": -4.142121581544164, + "velocityY": 1.7202655881144993, + "timestamp": 0.4934106215935756 + }, + { + "x": 6.1844635144693205, + "y": 1.475411119258973, + "heading": 2.703711291778771, + "angularVelocity": -7.80247206882837e-7, + "velocityX": -4.142121581544203, + "velocityY": 1.7202655881145157, + "timestamp": 0.5756457251925049 + }, + { + "x": 5.843835717091672, + "y": 1.6168773381152435, + "heading": 2.7037112276150617, + "angularVelocity": -7.802472036749515e-7, + "velocityX": -4.142121581544203, + "velocityY": 1.720265588114516, + "timestamp": 0.6578808287914342 + }, + { + "x": 5.5032079197140265, + "y": 1.7583435569715125, + "heading": 2.703711163451336, + "angularVelocity": -7.802473935152152e-7, + "velocityX": -4.142121581544159, + "velocityY": 1.7202655881144975, + "timestamp": 0.7401159323903634 + }, + { + "x": 5.1652853947837905, + "y": 1.8986862484261504, + "heading": 2.693209302936876, + "angularVelocity": -0.12770532357664416, + "velocityX": -4.109224773137335, + "velocityY": 1.7066032060846685, + "timestamp": 0.8223510359892927 + }, + { + "x": 4.891273492905135, + "y": 2.0124862024487293, + "heading": 2.610396383097362, + "angularVelocity": -1.0070263940251403, + "velocityX": -3.332055167280438, + "velocityY": 1.3838366955502954, + "timestamp": 0.904586139588222 + }, + { + "x": 4.6862341991539855, + "y": 2.0976411247047557, + "heading": 2.5347285457847115, + "angularVelocity": -0.9201403537069965, + "velocityX": -2.4933305216121706, + "velocityY": 1.0355057454701755, + "timestamp": 0.9868212431871513 + }, + { + "x": 4.549784608292612, + "y": 2.1543100396678017, + "heading": 2.4794610955282415, + "angularVelocity": -0.6720664027617208, + "velocityX": -1.6592621020684148, + "velocityY": 0.6891085738692211, + "timestamp": 1.0690563467860805 + }, + { + "x": 4.4816508293151855, + "y": 2.1826066970825195, + "heading": 2.4502412669675735, + "angularVelocity": -0.35532062685998894, + "velocityX": -0.8285242675648898, + "velocityY": 0.344094628404971, + "timestamp": 1.1512914503850098 + }, + { + "x": 4.4816508293151855, + "y": 2.1826066970825195, + "heading": 2.4502412669675735, + "angularVelocity": 7.602856908637436e-22, + "velocityX": 1.0463730087756701e-19, + "velocityY": 2.4540129007169126e-19, + "timestamp": 1.233526553983939 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/453.5.traj b/src/main/deploy/choreo/453.5.traj new file mode 100644 index 0000000..f643938 --- /dev/null +++ b/src/main/deploy/choreo/453.5.traj @@ -0,0 +1,247 @@ +{ + "samples": [ + { + "x": 4.4816508293151855, + "y": 2.1826066970825195, + "heading": 2.4502412669675735, + "angularVelocity": 7.602856908637436e-22, + "velocityX": 1.0463730087756701e-19, + "velocityY": 2.4540129007169126e-19, + "timestamp": 0 + }, + { + "x": 4.522764779586549, + "y": 2.173702089868958, + "heading": 2.4895097249631246, + "angularVelocity": 0.6155040409703461, + "velocityX": 0.6444307676951455, + "velocityY": -0.13957313332297588, + "timestamp": 0.06379886301582083 + }, + { + "x": 4.605330769247347, + "y": 2.156395974789388, + "heading": 2.566793871917863, + "angularVelocity": 1.2113718536892006, + "velocityX": 1.2941608323070741, + "velocityY": -0.27126055640330266, + "timestamp": 0.12759772603164166 + }, + { + "x": 4.729880594888555, + "y": 2.131468994237289, + "heading": 2.6796372881354387, + "angularVelocity": 1.7687371041329187, + "velocityX": 1.952226415231286, + "velocityY": -0.39071198723272255, + "timestamp": 0.19139658904746248 + }, + { + "x": 4.897346143499178, + "y": 2.1001730300771664, + "heading": 2.8220786737189165, + "angularVelocity": 2.23266338693459, + "velocityX": 2.624898637599474, + "velocityY": -0.4905410955734734, + "timestamp": 0.2551954520632833 + }, + { + "x": 5.109209706471012, + "y": 2.0650706464559785, + "heading": 2.9795893325205087, + "angularVelocity": 2.4688631012520226, + "velocityX": 3.3208046814141152, + "velocityY": -0.5502039058671528, + "timestamp": 0.31899431507910414 + }, + { + "x": 5.361614939609982, + "y": 2.0362260781830925, + "heading": 3.089669721704174, + "angularVelocity": 1.7254286985705027, + "velocityX": 3.9562653816632913, + "velocityY": -0.45211727779118777, + "timestamp": 0.38279317809492497 + }, + { + "x": 5.636552062814736, + "y": 2.033389433501791, + "heading": 3.131276583604512, + "angularVelocity": 0.652156792982666, + "velocityX": 4.309436096636636, + "velocityY": -0.04446230774674009, + "timestamp": 0.4465920411107458 + }, + { + "x": 5.918162824771871, + "y": 2.072735852050771, + "heading": 3.137289248200623, + "angularVelocity": 0.09424407131864133, + "velocityX": 4.414040449079824, + "velocityY": 0.6167260149953366, + "timestamp": 0.5103909041265666 + }, + { + "x": 6.191837310791016, + "y": 2.1562933921813965, + "heading": 3.1372916368921975, + "angularVelocity": 0.0000374409740449244, + "velocityX": 4.289645192443007, + "velocityY": 1.3097026526931217, + "timestamp": 0.5741897671423875 + }, + { + "x": 6.390489575313254, + "y": 2.243984052370421, + "heading": 3.137292152069018, + "angularVelocity": 0.00001064094601409734, + "velocityX": 4.103150486343585, + "velocityY": 1.811245272575824, + "timestamp": 0.6226043366746064 + }, + { + "x": 6.577284982010824, + "y": 2.3547084308199744, + "heading": 3.1372924085615264, + "angularVelocity": 0.000005297837216713321, + "velocityX": 3.85824780644312, + "velocityY": 2.2870053275154882, + "timestamp": 0.6710189062068252 + }, + { + "x": 6.749564767911577, + "y": 2.4868904986243314, + "heading": 3.137292587844308, + "angularVelocity": 0.0000037030749878800212, + "velocityX": 3.5584285384610155, + "velocityY": 2.7302126009071026, + "timestamp": 0.7194334757390441 + }, + { + "x": 6.904876862359124, + "y": 2.6386487999270924, + "heading": 3.1372927358529417, + "angularVelocity": 0.0000030571093674214147, + "velocityX": 3.207961899654838, + "velocityY": 3.134558517591893, + "timestamp": 0.767848045271263 + }, + { + "x": 7.041010956390259, + "y": 2.8078230861439333, + "heading": 3.1372928730865968, + "angularVelocity": 0.0000028345528292870936, + "velocityX": 2.811841463147611, + "velocityY": 3.4942846306680346, + "timestamp": 0.816262614803482 + }, + { + "x": 7.162462195458692, + "y": 2.9878287177907508, + "heading": 3.1372930108093384, + "angularVelocity": 0.000002844654873697454, + "velocityX": 2.508567983602721, + "velocityY": 3.718005414197231, + "timestamp": 0.8646771843357008 + }, + { + "x": 7.283896132649579, + "y": 3.1678460219163584, + "heading": 3.1372931485973305, + "angularVelocity": 0.0000028460026277310735, + "velocityX": 2.508210614370468, + "velocityY": 3.7182465085393464, + "timestamp": 0.9130917538679197 + }, + { + "x": 7.397867414319233, + "y": 3.343303097157428, + "heading": 3.1588948465103956, + "angularVelocity": 0.44618176143629246, + "velocityX": 2.354069916780078, + "velocityY": 3.624055257256956, + "timestamp": 0.9615063234001386 + }, + { + "x": 7.506043046157439, + "y": 3.501095473470491, + "heading": 3.228380651047953, + "angularVelocity": 1.4352250822206813, + "velocityX": 2.2343611206997815, + "velocityY": 3.2591919712940056, + "timestamp": 1.0099208929323575 + }, + { + "x": 7.6042586153437055, + "y": 3.6443897724231573, + "heading": 3.344703303282488, + "angularVelocity": 2.402637333316058, + "velocityX": 2.0286366301554346, + "velocityY": 2.959735061928151, + "timestamp": 1.0583354624645764 + }, + { + "x": 7.688078454298536, + "y": 3.766145806545298, + "heading": 3.4608719199161126, + "angularVelocity": 2.3994557373130703, + "velocityX": 1.7312936945365136, + "velocityY": 2.5148635069679726, + "timestamp": 1.1067500319967953 + }, + { + "x": 7.757675538495618, + "y": 3.866750816123695, + "heading": 3.5658223462991305, + "angularVelocity": 2.167744697454667, + "velocityX": 1.4375235568451696, + "velocityY": 2.0779903766664107, + "timestamp": 1.1551646015290142 + }, + { + "x": 7.8131946706107165, + "y": 3.94664371955485, + "heading": 3.6546108231176855, + "angularVelocity": 1.8339206085364161, + "velocityX": 1.1467443096473655, + "velocityY": 1.6501830792482484, + "timestamp": 1.2035791710612331 + }, + { + "x": 7.8547317895295015, + "y": 4.006195440456753, + "heading": 3.7240095680654406, + "angularVelocity": 1.4334268716687006, + "velocityX": 0.857946674319663, + "velocityY": 1.2300371866834907, + "timestamp": 1.251993740593452 + }, + { + "x": 7.882369894183202, + "y": 4.045699068997907, + "heading": 3.7716896343345847, + "angularVelocity": 0.9848288796085234, + "velocityX": 0.5708633768871593, + "velocityY": 0.81594505378934, + "timestamp": 1.300408310125671 + }, + { + "x": 7.896173477172852, + "y": 4.065373420715332, + "heading": 3.79602872084114, + "angularVelocity": 0.5027223569623597, + "velocityX": 0.28511217022106833, + "velocityY": 0.4063725425531795, + "timestamp": 1.3488228796578898 + }, + { + "x": 7.896173477172852, + "y": 4.065373420715332, + "heading": 3.79602872084114, + "angularVelocity": 4.6010842651263e-18, + "velocityX": 6.606474514740261e-18, + "velocityY": 4.020513640198354e-18, + "timestamp": 1.3972374491901087 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/453.6.traj b/src/main/deploy/choreo/453.6.traj new file mode 100644 index 0000000..468aada --- /dev/null +++ b/src/main/deploy/choreo/453.6.traj @@ -0,0 +1,247 @@ +{ + "samples": [ + { + "x": 7.896173477172852, + "y": 4.065373420715332, + "heading": 3.79602872084114, + "angularVelocity": 4.6010842651263e-18, + "velocityX": 6.606474514740261e-18, + "velocityY": 4.020513640198354e-18, + "timestamp": 0 + }, + { + "x": 7.883686491968607, + "y": 4.04817527202583, + "heading": 3.7732837957184815, + "angularVelocity": -0.4976688757750139, + "velocityX": -0.273220679114313, + "velocityY": -0.3763029896806603, + "timestamp": 0.04570292865357928 + }, + { + "x": 7.858644371282851, + "y": 4.01363266629876, + "heading": 3.7289682484036177, + "angularVelocity": -0.9696434915748829, + "velocityX": -0.5479325160006953, + "velocityY": -0.7558072697900738, + "timestamp": 0.09140585730715856 + }, + { + "x": 7.8209799739706884, + "y": 3.961528626225198, + "heading": 3.664728508619979, + "angularVelocity": -1.4055935073779289, + "velocityX": -0.8241134304028905, + "velocityY": -1.1400591079951226, + "timestamp": 0.13710878596073783 + }, + { + "x": 7.770648499127757, + "y": 3.8915760964517476, + "heading": 3.5826191900873825, + "angularVelocity": -1.796587679423395, + "velocityX": -1.1012746081204872, + "velocityY": -1.530591842454609, + "timestamp": 0.1828117146143171 + }, + { + "x": 7.707615042416261, + "y": 3.8034422194517306, + "heading": 3.4850702036014467, + "angularVelocity": -2.134414344107738, + "velocityX": -1.3791995079631632, + "velocityY": -1.9284076446842287, + "timestamp": 0.2285146432678964 + }, + { + "x": 7.631773516002201, + "y": 3.696764579350947, + "heading": 3.3754828392132095, + "angularVelocity": -2.39781929991619, + "velocityX": -1.6594456558554744, + "velocityY": -2.3341532642116922, + "timestamp": 0.27421757192147567 + }, + { + "x": 7.542882516453489, + "y": 3.5711235953994676, + "heading": 3.261672437263455, + "angularVelocity": -2.490221202506508, + "velocityX": -1.9449738160650807, + "velocityY": -2.7490794934351226, + "timestamp": 0.31992050057505494 + }, + { + "x": 7.44454344391469, + "y": 3.428056176286614, + "heading": 3.1768425535327034, + "angularVelocity": -1.8561148318034846, + "velocityX": -2.151701771327938, + "velocityY": -3.13037749062619, + "timestamp": 0.3656234292286342 + }, + { + "x": 7.340219532855234, + "y": 3.2695180348342774, + "heading": 3.135286948560975, + "angularVelocity": -0.9092547500998464, + "velocityX": -2.2826526468400488, + "velocityY": -3.468883638815311, + "timestamp": 0.4113263578822135 + }, + { + "x": 7.224153526183016, + "y": 3.1005590910373835, + "heading": 3.135286791197327, + "angularVelocity": -0.00000344318520706556, + "velocityX": -2.5395748170106347, + "velocityY": -3.696895336347947, + "timestamp": 0.4570292865357928 + }, + { + "x": 7.107477607000754, + "y": 2.932020744346244, + "heading": 3.135286646763266, + "angularVelocity": -0.000003160280207883062, + "velocityX": -2.5529199685789394, + "velocityY": -3.687692488344275, + "timestamp": 0.5027322151893721 + }, + { + "x": 6.990772092919987, + "y": 2.763502889489356, + "heading": 3.135286502334091, + "angularVelocity": -0.0000031601733036030435, + "velocityX": -2.553567517858124, + "velocityY": -3.687244118078817, + "timestamp": 0.5484351438429513 + }, + { + "x": 6.858421912732069, + "y": 2.6069721451251513, + "heading": 3.135286357995183, + "angularVelocity": -0.000003158198218572508, + "velocityX": -2.8958796315026465, + "velocityY": -3.424960915539557, + "timestamp": 0.5941380724965306 + }, + { + "x": 6.709310605779643, + "y": 2.466315612343406, + "heading": 3.1352862013811205, + "angularVelocity": -0.000003426783961068071, + "velocityX": -3.2626203909746376, + "velocityY": -3.077626246840689, + "timestamp": 0.6398410011501099 + }, + { + "x": 6.545329184626209, + "y": 2.343317519944224, + "heading": 3.13528601237849, + "angularVelocity": -0.000004135459935590175, + "velocityX": -3.5879849712998917, + "velocityY": -2.691251874283261, + "timestamp": 0.6855439298036892 + }, + { + "x": 6.368557481944258, + "y": 2.23953798710901, + "heading": 3.1352857469986737, + "angularVelocity": -0.0000058066260578378354, + "velocityX": -3.8678419061905776, + "velocityY": -2.270741414009659, + "timestamp": 0.7312468584572684 + }, + { + "x": 6.181237697601318, + "y": 2.1562933921813965, + "heading": 3.135285039902942, + "angularVelocity": -0.000015471562811805473, + "velocityX": -4.098638530646304, + "velocityY": -1.8214280217924592, + "timestamp": 0.7769497871108477 + }, + { + "x": 5.874969956696071, + "y": 2.079869439194049, + "heading": 3.1352844358731087, + "angularVelocity": -0.000008582546125130102, + "velocityX": -4.351700640081255, + "velocityY": -1.0858935523199316, + "timestamp": 0.8473286495914998 + }, + { + "x": 5.561830297244809, + "y": 2.0576986839525397, + "heading": 3.1292065379013336, + "angularVelocity": -0.08635970741139995, + "velocityX": -4.4493424362655105, + "velocityY": -0.31502008500930834, + "timestamp": 0.9177075120721518 + }, + { + "x": 5.265650774435423, + "y": 2.0816151214602616, + "heading": 3.0610179423428407, + "angularVelocity": -0.9688789098751426, + "velocityX": -4.2083590494348115, + "velocityY": 0.3398241555024946, + "timestamp": 0.9880863745528039 + }, + { + "x": 5.005528861829494, + "y": 2.114389972569813, + "heading": 2.8883900412149814, + "angularVelocity": -2.45283732989175, + "velocityX": -3.6960232580831667, + "velocityY": 0.46569168574679387, + "timestamp": 1.058465237033456 + }, + { + "x": 4.8000582736739394, + "y": 2.145902528001046, + "heading": 2.722850674189578, + "angularVelocity": -2.3521176840691327, + "velocityX": -2.9194928834213356, + "velocityY": 0.4477559642271928, + "timestamp": 1.128844099514108 + }, + { + "x": 4.647342956856588, + "y": 2.17150300925771, + "heading": 2.5899379229501136, + "angularVelocity": -1.8885322461130774, + "velocityX": -2.1699031702783396, + "velocityY": 0.363752415914727, + "timestamp": 1.19922296199476 + }, + { + "x": 4.5461805350001505, + "y": 2.189414674483752, + "heading": 2.4978059035992817, + "angularVelocity": -1.3090865084124141, + "velocityX": -1.4373977965933769, + "velocityY": 0.25450347724741956, + "timestamp": 1.2696018244754121 + }, + { + "x": 4.495876312255859, + "y": 2.198692560195923, + "heading": 2.4502412669675735, + "angularVelocity": -0.6758369623377337, + "velocityX": -0.7147632253664121, + "velocityY": 0.13182773044566, + "timestamp": 1.3399806869560642 + }, + { + "x": 4.495876312255859, + "y": 2.198692560195923, + "heading": 2.4502412669675735, + "angularVelocity": -7.70463937191343e-20, + "velocityX": -4.406552052721558e-19, + "velocityY": -7.177153869205785e-19, + "timestamp": 1.4103595494367163 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/453.traj b/src/main/deploy/choreo/453.traj new file mode 100644 index 0000000..a144dce --- /dev/null +++ b/src/main/deploy/choreo/453.traj @@ -0,0 +1,1201 @@ +{ + "samples": [ + { + "x": 1.402008891105652, + "y": 3.348797559738159, + "heading": 1.986288521819101, + "angularVelocity": -4.772790939425341e-21, + "velocityX": -1.8126451827293707e-20, + "velocityY": 3.628710439908743e-20, + "timestamp": 0 + }, + { + "x": 1.4486883841972304, + "y": 3.3293922637608246, + "heading": 2.030724464534454, + "angularVelocity": 0.6364534985554904, + "velocityX": 0.6685877439181028, + "velocityY": -0.2779409586152936, + "timestamp": 0.06981805083357366 + }, + { + "x": 1.5420268342222572, + "y": 3.290138269622852, + "heading": 2.1189457404378427, + "angularVelocity": 1.2635883535861399, + "velocityX": 1.3368813496028258, + "velocityY": -0.5622327416665235, + "timestamp": 0.13963610166714732 + }, + { + "x": 1.6821658927549905, + "y": 3.230725535127495, + "heading": 2.24987954869418, + "angularVelocity": 1.8753575428286537, + "velocityX": 2.0072038227876763, + "velocityY": -0.8509652415960443, + "timestamp": 0.20945415250072097 + }, + { + "x": 1.8698641645684424, + "y": 3.1508349518156304, + "heading": 2.4175769819232236, + "angularVelocity": 2.401920867553102, + "velocityX": 2.688391749303789, + "velocityY": -1.1442683139679877, + "timestamp": 0.27927220333429464 + }, + { + "x": 2.1065173503150554, + "y": 3.048807421420711, + "heading": 2.5949528195462372, + "angularVelocity": 2.540544107222745, + "velocityX": 3.3895702174603337, + "velocityY": -1.461334557135146, + "timestamp": 0.3490902541678683 + }, + { + "x": 2.376491888321871, + "y": 2.930418013607003, + "heading": 2.658710182192554, + "angularVelocity": 0.9131931052944469, + "velocityX": 3.866830064482291, + "velocityY": -1.695684803574854, + "timestamp": 0.418908305001442 + }, + { + "x": 2.6674986540228987, + "y": 2.8147717400807317, + "heading": 2.658710349301536, + "angularVelocity": 0.0000023934925125177552, + "velocityX": 4.168073474218082, + "velocityY": -1.6563950460596366, + "timestamp": 0.48872635583501567 + }, + { + "x": 2.9585059641387246, + "y": 2.6991268364960277, + "heading": 2.658710516400385, + "angularVelocity": 0.000002393347381270105, + "velocityX": 4.168081271840486, + "velocityY": -1.6563754244639193, + "timestamp": 0.5585444066685893 + }, + { + "x": 3.249513274191479, + "y": 2.5834819327526177, + "heading": 2.658710683499246, + "angularVelocity": 0.0000023933475558782913, + "velocityX": 4.1680812709371295, + "velocityY": -1.6563754267370547, + "timestamp": 0.6283624575021629 + }, + { + "x": 3.540520584244246, + "y": 2.4678370290092424, + "heading": 2.6587108505981196, + "angularVelocity": 0.0000023933477293082605, + "velocityX": 4.168081270937306, + "velocityY": -1.6563754267365562, + "timestamp": 0.6981805083357365 + }, + { + "x": 3.8315278942970123, + "y": 2.3521921252658693, + "heading": 2.6587110176970046, + "angularVelocity": 0.0000023933479006784206, + "velocityX": 4.168081270937295, + "velocityY": -1.6563754267365276, + "timestamp": 0.7679985591693101 + }, + { + "x": 4.122535204349912, + "y": 2.236547221522837, + "heading": 2.658711184795902, + "angularVelocity": 0.000002393348074985324, + "velocityX": 4.168081270939212, + "velocityY": -1.6563754267316464, + "timestamp": 0.8378166100028838 + }, + { + "x": 4.413542514402566, + "y": 2.1209023177791893, + "heading": 2.6587113518948113, + "angularVelocity": 0.0000023933482457405575, + "velocityX": 4.168081270935688, + "velocityY": -1.6563754267404591, + "timestamp": 0.9076346608364574 + }, + { + "x": 4.704549824293959, + "y": 2.00525741362975, + "heading": 2.6587115189937336, + "angularVelocity": 0.0000023933484214965284, + "velocityX": 4.16808126862595, + "velocityY": -1.6563754325525946, + "timestamp": 0.977452711670031 + }, + { + "x": 4.995557455494251, + "y": 1.889613318020985, + "heading": 2.6587116860924342, + "angularVelocity": 0.000002393345257841174, + "velocityX": 4.168085870715172, + "velocityY": -1.656363851870161, + "timestamp": 1.0472707625036046 + }, + { + "x": 5.29724334339808, + "y": 1.8056810636761744, + "heading": 2.658711846333939, + "angularVelocity": 0.0000022951300252156057, + "velocityX": 4.3210299385607565, + "velocityY": -1.20215693997074, + "timestamp": 1.1170888133371784 + }, + { + "x": 5.608850955963135, + "y": 1.7747021913528442, + "heading": 2.658712011161958, + "angularVelocity": 0.0000023608224121896315, + "velocityX": 4.463138240679893, + "velocityY": -0.44370863914799147, + "timestamp": 1.186906864170752 + }, + { + "x": 5.875229984253185, + "y": 1.787380524511446, + "heading": 2.658712143051779, + "angularVelocity": 0.0000022181754586112354, + "velocityX": 4.480068446986742, + "velocityY": 0.2132292497230326, + "timestamp": 1.2463655602692938 + }, + { + "x": 6.136897172641307, + "y": 1.838847151157444, + "heading": 2.6587122514613757, + "angularVelocity": 0.0000018232757260252147, + "velocityX": 4.400822849435795, + "velocityY": 0.865586197193116, + "timestamp": 1.3058242563678355 + }, + { + "x": 6.388924893224618, + "y": 1.9260283435917724, + "heading": 2.658712354170833, + "angularVelocity": 0.0000017274085020042764, + "velocityX": 4.238702445906684, + "velocityY": 1.466247969680363, + "timestamp": 1.3652829524663772 + }, + { + "x": 6.64088166554894, + "y": 2.013414367297805, + "heading": 2.6587124568709988, + "angularVelocity": 0.0000017272522313604299, + "velocityX": 4.2375092098681515, + "velocityY": 1.4696929034771757, + "timestamp": 1.424741648564919 + }, + { + "x": 6.892839053321575, + "y": 2.1007986164800005, + "heading": 2.6587125595759944, + "angularVelocity": 0.0000017273334665849146, + "velocityX": 4.237519560722661, + "velocityY": 1.4696630588294823, + "timestamp": 1.4842003446634606 + }, + { + "x": 7.136636450310397, + "y": 2.1779815587686944, + "heading": 2.688070177118974, + "angularVelocity": 0.49374808849364216, + "velocityX": 4.100281590177727, + "velocityY": 1.2980934220417046, + "timestamp": 1.5436590407620023 + }, + { + "x": 7.352651079830789, + "y": 2.253382751730419, + "heading": 2.7899600332166923, + "angularVelocity": 1.7136241253736015, + "velocityX": 3.633019956616387, + "velocityY": 1.2681272531903705, + "timestamp": 1.603117736860544 + }, + { + "x": 7.53203786297685, + "y": 2.315567813162813, + "heading": 2.899685711550104, + "angularVelocity": 1.8454101003419598, + "velocityX": 3.016998267986917, + "velocityY": 1.0458530965652777, + "timestamp": 1.6625764329590857 + }, + { + "x": 7.674899201290455, + "y": 2.3648328695682928, + "heading": 2.997122096012855, + "angularVelocity": 1.6387238680994411, + "velocityX": 2.4026988092177444, + "velocityY": 0.8285593132387635, + "timestamp": 1.7220351290576275 + }, + { + "x": 7.781659482434556, + "y": 2.401488912698896, + "heading": 3.0749787829720594, + "angularVelocity": 1.3094247278845657, + "velocityX": 1.7955368709593682, + "velocityY": 0.6164959128914116, + "timestamp": 1.7814938251561692 + }, + { + "x": 7.852631290421507, + "y": 2.4257708157921387, + "heading": 3.1291699090572695, + "angularVelocity": 0.9114079124002065, + "velocityX": 1.1936320949475463, + "velocityY": 0.4083827040707386, + "timestamp": 1.8409525212547109 + }, + { + "x": 7.888041019439697, + "y": 2.4378485679626465, + "heading": 3.1570845336658366, + "angularVelocity": 0.46947925938879653, + "velocityX": 0.5955349064416933, + "velocityY": 0.20312843979105816, + "timestamp": 1.9004112173532526 + }, + { + "x": 7.888041019439697, + "y": 2.4378485679626465, + "heading": 3.1570845336658366, + "angularVelocity": -6.5675658801427375e-21, + "velocityX": -1.9943274045954098e-20, + "velocityY": -1.41680586488362e-19, + "timestamp": 1.9598699134517943 + }, + { + "x": 7.858249340154391, + "y": 2.4252980054611535, + "heading": 3.1411221298856744, + "angularVelocity": -0.29219537518715366, + "velocityX": -0.5453433596914764, + "velocityY": -0.22974085666791233, + "timestamp": 2.014499127791474 + }, + { + "x": 7.798616091241273, + "y": 2.4001661825898526, + "heading": 3.109940998101945, + "angularVelocity": -0.5707775987743037, + "velocityX": -1.0915999732729573, + "velocityY": -0.46004364468860043, + "timestamp": 2.069128342131154 + }, + { + "x": 7.7090741082999905, + "y": 2.3624118351876593, + "heading": 3.064591493757076, + "angularVelocity": -0.8301328308126376, + "velocityX": -1.6390860462410948, + "velocityY": -0.6911017824170038, + "timestamp": 2.123757556470834 + }, + { + "x": 7.589529913625826, + "y": 2.31198041430475, + "heading": 3.0066355317093274, + "angularVelocity": -1.0608968616569114, + "velocityX": -2.1882832495237374, + "velocityY": -0.9231584508854718, + "timestamp": 2.1783867708105142 + }, + { + "x": 7.439843733345756, + "y": 2.248801164332235, + "heading": 2.938737698881861, + "angularVelocity": -1.242885032270157, + "velocityX": -2.7400390448475846, + "velocityY": -1.1565103166168895, + "timestamp": 2.2330159851501943 + }, + { + "x": 7.259812429071698, + "y": 2.1727924683021866, + "heading": 2.866995100648553, + "angularVelocity": -1.3132643238692565, + "velocityX": -3.2955133338480462, + "velocityY": -1.3913561992203078, + "timestamp": 2.2876451994898743 + }, + { + "x": 7.050685712931125, + "y": 2.084647731659991, + "heading": 2.8184896362477745, + "angularVelocity": -0.887903386257304, + "velocityX": -3.8281113625438707, + "velocityY": -1.6135091398920511, + "timestamp": 2.3422744138295544 + }, + { + "x": 6.825086195412971, + "y": 1.9890469912053048, + "heading": 2.8184895683294764, + "angularVelocity": -0.000001243259649552996, + "velocityX": -4.129649680030092, + "velocityY": -1.7499929590831957, + "timestamp": 2.3969036281692344 + }, + { + "x": 6.599487919525426, + "y": 1.8934433207944097, + "heading": 2.8184895004083503, + "angularVelocity": -0.0000012433114214237418, + "velocityX": -4.129626951703787, + "velocityY": -1.750046592587612, + "timestamp": 2.4515328425089145 + }, + { + "x": 6.3690722425418835, + "y": 1.8101170974402798, + "heading": 2.818489431491537, + "angularVelocity": -0.0000012615376929961426, + "velocityX": -4.217810557384865, + "velocityY": -1.5253051752861435, + "timestamp": 2.5061620568485945 + }, + { + "x": 6.129552771918778, + "y": 1.75849282334362, + "heading": 2.8184893560491466, + "angularVelocity": -0.000001380989851962125, + "velocityX": -4.384457538301637, + "velocityY": -0.944993896043691, + "timestamp": 2.5607912711882745 + }, + { + "x": 5.885269812104598, + "y": 1.7395066427805248, + "heading": 2.818489264333038, + "angularVelocity": -0.0000016788839079463646, + "velocityX": -4.471654274492206, + "velocityY": -0.34754628622408146, + "timestamp": 2.6154204855279546 + }, + { + "x": 5.640650272369385, + "y": 1.7535027265548706, + "heading": 2.818488970175976, + "angularVelocity": -0.000005384610881826656, + "velocityX": -4.477815445307147, + "velocityY": 0.25620144722051685, + "timestamp": 2.6700496998676346 + }, + { + "x": 5.317111218010938, + "y": 1.8335830693812192, + "heading": 2.805254564148882, + "angularVelocity": -0.17607305299809856, + "velocityX": -4.304425068143131, + "velocityY": 1.0654041003202679, + "timestamp": 2.7452139899264734 + }, + { + "x": 5.032821589363983, + "y": 1.9501424980664932, + "heading": 2.7103212385268245, + "angularVelocity": -1.2630110062603335, + "velocityX": -3.7822432490802944, + "velocityY": 1.550728791478392, + "timestamp": 2.820378279985312 + }, + { + "x": 4.809796020651664, + "y": 2.054633591251526, + "heading": 2.6147072284980224, + "angularVelocity": -1.272066960972499, + "velocityX": -2.967174552406934, + "velocityY": 1.3901693623825526, + "timestamp": 2.895542570044151 + }, + { + "x": 4.644755427475796, + "y": 2.1369592776048787, + "heading": 2.535476805564598, + "angularVelocity": -1.0540966045365723, + "velocityX": -2.1957314177606606, + "velocityY": 1.0952765773335758, + "timestamp": 2.9707068601029896 + }, + { + "x": 4.535762508218388, + "y": 2.1934508330248605, + "heading": 2.4794362763754623, + "angularVelocity": -0.7455738508973778, + "velocityX": -1.4500625120264967, + "velocityY": 0.7515743895905873, + "timestamp": 3.0458711501618283 + }, + { + "x": 4.4816508293151855, + "y": 2.2222511768341064, + "heading": 2.4502412669675735, + "angularVelocity": -0.38841595370667265, + "velocityX": -0.7199120601131723, + "velocityY": 0.38316524757569187, + "timestamp": 3.121035440220667 + }, + { + "x": 4.4816508293151855, + "y": 2.2222511768341064, + "heading": 2.4502412669675735, + "angularVelocity": -1.695796641331656e-20, + "velocityX": 6.611674129926287e-21, + "velocityY": -2.0802489677537025e-21, + "timestamp": 3.1961997302795058 + }, + { + "x": 4.549872957098698, + "y": 2.1931238398910824, + "heading": 2.4796703273668, + "angularVelocity": 0.3568867765585547, + "velocityX": 0.8273310443599396, + "velocityY": -0.353227770452506, + "timestamp": 3.278660225686507 + }, + { + "x": 4.686495078566755, + "y": 2.1347932264206446, + "heading": 2.5354421814878645, + "angularVelocity": 0.6763463382773813, + "velocityX": 1.6568190718929057, + "velocityY": -0.7073764616927737, + "timestamp": 3.3611207210935086 + }, + { + "x": 4.891792155516278, + "y": 2.047141941888709, + "heading": 2.6118695897899324, + "angularVelocity": 0.9268366376510857, + "velocityX": 2.489641566379575, + "velocityY": -1.0629487986861335, + "timestamp": 3.44358121650051 + }, + { + "x": 5.166150185128691, + "y": 1.9300051853673699, + "heading": 2.695625209420133, + "angularVelocity": 1.0157059961476873, + "velocityX": 3.327145056044848, + "velocityY": -1.420519679674317, + "timestamp": 3.5260417119075114 + }, + { + "x": 5.504185836826239, + "y": 1.785681361915343, + "heading": 2.703998978176573, + "angularVelocity": 0.10154885336438363, + "velocityX": 4.099364793154602, + "velocityY": -1.7502177586938532, + "timestamp": 3.6085022073145128 + }, + { + "x": 5.844328329121435, + "y": 1.6404580259742356, + "heading": 2.703999042124718, + "angularVelocity": 7.755003720294035e-7, + "velocityX": 4.124914489251482, + "velocityY": -1.7611261637991182, + "timestamp": 3.690962702721514 + }, + { + "x": 6.1844708214166335, + "y": 1.4952346900331273, + "heading": 2.703999106072851, + "angularVelocity": 7.755002271176481e-7, + "velocityX": 4.124914489251517, + "velocityY": -1.7611261637991327, + "timestamp": 3.7734231981285156 + }, + { + "x": 6.524613313711832, + "y": 1.350011354092019, + "heading": 2.7039991700209836, + "angularVelocity": 7.755002268276392e-7, + "velocityX": 4.124914489251517, + "velocityY": -1.761126163799133, + "timestamp": 3.855883693535517 + }, + { + "x": 6.864755806007028, + "y": 1.2047880181509114, + "heading": 2.7039992339691272, + "angularVelocity": 7.755003544824911e-7, + "velocityX": 4.124914489251488, + "velocityY": -1.7611261637991202, + "timestamp": 3.9383441889425184 + }, + { + "x": 7.203138205815544, + "y": 1.0603161511037837, + "heading": 2.711044021755172, + "angularVelocity": 0.08543227579792817, + "velocityX": 4.103569814107435, + "velocityY": -1.7520130861942547, + "timestamp": 4.02080468434952 + }, + { + "x": 7.477630170379687, + "y": 0.9431222112521354, + "heading": 2.7918538144189164, + "angularVelocity": 0.979981896360077, + "velocityX": 3.328769287758077, + "velocityY": -1.4212131430112402, + "timestamp": 4.103265179756522 + }, + { + "x": 7.683053573536903, + "y": 0.8554169919319458, + "heading": 2.8658518961937767, + "angularVelocity": 0.8973761485378771, + "velocityX": 2.491173526708827, + "velocityY": -1.0636028668916144, + "timestamp": 4.185725675163524 + }, + { + "x": 7.819769980022167, + "y": 0.7970461236123249, + "heading": 2.919972038595159, + "angularVelocity": 0.6563159987611107, + "velocityX": 1.65796246809422, + "velocityY": -0.7078646330163175, + "timestamp": 4.268186170570526 + }, + { + "x": 7.888041019439697, + "y": 0.76789790391922, + "heading": 2.948608540855105, + "angularVelocity": 0.34727540889251207, + "velocityX": 0.8279241966782274, + "velocityY": -0.353481015960886, + "timestamp": 4.350646665977528 + }, + { + "x": 7.888041019439697, + "y": 0.76789790391922, + "heading": 2.948608540855105, + "angularVelocity": -2.222034632801661e-28, + "velocityX": -9.857931768481573e-20, + "velocityY": -2.2471643962624357e-19, + "timestamp": 4.433107161384529 + }, + { + "x": 7.819855304083459, + "y": 0.7962161310448899, + "heading": 2.9201779748674546, + "angularVelocity": -0.34572299107579807, + "velocityX": -0.8291558272825678, + "velocityY": 0.3443569216350881, + "timestamp": 4.515342264983459 + }, + { + "x": 7.683310667089189, + "y": 0.8529245196420355, + "heading": 2.8664329957677785, + "angularVelocity": -0.653552762112354, + "velocityX": -1.6604178874780198, + "velocityY": 0.6895885834073882, + "timestamp": 4.597577368582388 + }, + { + "x": 7.478144407945387, + "y": 0.9381321719282073, + "heading": 2.793015918109701, + "angularVelocity": -0.8927705377029863, + "velocityX": -2.494874453425899, + "velocityY": 1.0361469561920882, + "timestamp": 4.679812472181317 + }, + { + "x": 7.203998785219726, + "y": 1.051987661590431, + "heading": 2.71291669528747, + "angularVelocity": -0.974027140682948, + "velocityX": -3.3336812471557438, + "velocityY": 1.3845120232049635, + "timestamp": 4.7620475757802465 + }, + { + "x": 6.865719109224617, + "y": 1.192478681546433, + "heading": 2.7037114201062042, + "angularVelocity": -0.11193851261087887, + "velocityX": -4.113567821899265, + "velocityY": 1.7084069187921709, + "timestamp": 4.844282679379176 + }, + { + "x": 6.525091311846971, + "y": 1.3339449004027024, + "heading": 2.703711355942481, + "angularVelocity": -7.802473717689034e-7, + "velocityX": -4.142121581544164, + "velocityY": 1.7202655881144993, + "timestamp": 4.926517782978105 + }, + { + "x": 6.1844635144693205, + "y": 1.475411119258973, + "heading": 2.703711291778771, + "angularVelocity": -7.80247206882837e-7, + "velocityX": -4.142121581544203, + "velocityY": 1.7202655881145157, + "timestamp": 5.008752886577034 + }, + { + "x": 5.843835717091672, + "y": 1.6168773381152435, + "heading": 2.7037112276150617, + "angularVelocity": -7.802472036749515e-7, + "velocityX": -4.142121581544203, + "velocityY": 1.720265588114516, + "timestamp": 5.090987990175964 + }, + { + "x": 5.5032079197140265, + "y": 1.7583435569715125, + "heading": 2.703711163451336, + "angularVelocity": -7.802473935152152e-7, + "velocityX": -4.142121581544159, + "velocityY": 1.7202655881144975, + "timestamp": 5.173223093774893 + }, + { + "x": 5.1652853947837905, + "y": 1.8986862484261504, + "heading": 2.693209302936876, + "angularVelocity": -0.12770532357664416, + "velocityX": -4.109224773137335, + "velocityY": 1.7066032060846685, + "timestamp": 5.255458197373822 + }, + { + "x": 4.891273492905135, + "y": 2.0124862024487293, + "heading": 2.610396383097362, + "angularVelocity": -1.0070263940251403, + "velocityX": -3.332055167280438, + "velocityY": 1.3838366955502954, + "timestamp": 5.337693300972751 + }, + { + "x": 4.6862341991539855, + "y": 2.0976411247047557, + "heading": 2.5347285457847115, + "angularVelocity": -0.9201403537069965, + "velocityX": -2.4933305216121706, + "velocityY": 1.0355057454701755, + "timestamp": 5.419928404571681 + }, + { + "x": 4.549784608292612, + "y": 2.1543100396678017, + "heading": 2.4794610955282415, + "angularVelocity": -0.6720664027617208, + "velocityX": -1.6592621020684148, + "velocityY": 0.6891085738692211, + "timestamp": 5.50216350817061 + }, + { + "x": 4.4816508293151855, + "y": 2.1826066970825195, + "heading": 2.4502412669675735, + "angularVelocity": -0.35532062685998894, + "velocityX": -0.8285242675648898, + "velocityY": 0.344094628404971, + "timestamp": 5.584398611769539 + }, + { + "x": 4.4816508293151855, + "y": 2.1826066970825195, + "heading": 2.4502412669675735, + "angularVelocity": 7.602856908637436e-22, + "velocityX": 1.0463730087756701e-19, + "velocityY": 2.4540129007169126e-19, + "timestamp": 5.6666337153684685 + }, + { + "x": 4.522764779586549, + "y": 2.173702089868958, + "heading": 2.4895097249631246, + "angularVelocity": 0.6155040409703461, + "velocityX": 0.6444307676951455, + "velocityY": -0.13957313332297588, + "timestamp": 5.730432578384289 + }, + { + "x": 4.605330769247347, + "y": 2.156395974789388, + "heading": 2.566793871917863, + "angularVelocity": 1.2113718536892006, + "velocityX": 1.2941608323070741, + "velocityY": -0.27126055640330266, + "timestamp": 5.79423144140011 + }, + { + "x": 4.729880594888555, + "y": 2.131468994237289, + "heading": 2.6796372881354387, + "angularVelocity": 1.7687371041329187, + "velocityX": 1.952226415231286, + "velocityY": -0.39071198723272255, + "timestamp": 5.858030304415931 + }, + { + "x": 4.897346143499178, + "y": 2.1001730300771664, + "heading": 2.8220786737189165, + "angularVelocity": 2.23266338693459, + "velocityX": 2.624898637599474, + "velocityY": -0.4905410955734734, + "timestamp": 5.921829167431752 + }, + { + "x": 5.109209706471012, + "y": 2.0650706464559785, + "heading": 2.9795893325205087, + "angularVelocity": 2.4688631012520226, + "velocityX": 3.3208046814141152, + "velocityY": -0.5502039058671528, + "timestamp": 5.985628030447573 + }, + { + "x": 5.361614939609982, + "y": 2.0362260781830925, + "heading": 3.089669721704174, + "angularVelocity": 1.7254286985705027, + "velocityX": 3.9562653816632913, + "velocityY": -0.45211727779118777, + "timestamp": 6.0494268934633935 + }, + { + "x": 5.636552062814736, + "y": 2.033389433501791, + "heading": 3.131276583604512, + "angularVelocity": 0.652156792982666, + "velocityX": 4.309436096636636, + "velocityY": -0.04446230774674009, + "timestamp": 6.113225756479214 + }, + { + "x": 5.918162824771871, + "y": 2.072735852050771, + "heading": 3.137289248200623, + "angularVelocity": 0.09424407131864133, + "velocityX": 4.414040449079824, + "velocityY": 0.6167260149953366, + "timestamp": 6.177024619495035 + }, + { + "x": 6.191837310791016, + "y": 2.1562933921813965, + "heading": 3.1372916368921975, + "angularVelocity": 0.0000374409740449244, + "velocityX": 4.289645192443007, + "velocityY": 1.3097026526931217, + "timestamp": 6.240823482510856 + }, + { + "x": 6.390489575313254, + "y": 2.243984052370421, + "heading": 3.137292152069018, + "angularVelocity": 0.00001064094601409734, + "velocityX": 4.103150486343585, + "velocityY": 1.811245272575824, + "timestamp": 6.289238052043075 + }, + { + "x": 6.577284982010824, + "y": 2.3547084308199744, + "heading": 3.1372924085615264, + "angularVelocity": 0.000005297837216713321, + "velocityX": 3.85824780644312, + "velocityY": 2.2870053275154882, + "timestamp": 6.337652621575294 + }, + { + "x": 6.749564767911577, + "y": 2.4868904986243314, + "heading": 3.137292587844308, + "angularVelocity": 0.0000037030749878800212, + "velocityX": 3.5584285384610155, + "velocityY": 2.7302126009071026, + "timestamp": 6.386067191107513 + }, + { + "x": 6.904876862359124, + "y": 2.6386487999270924, + "heading": 3.1372927358529417, + "angularVelocity": 0.0000030571093674214147, + "velocityX": 3.207961899654838, + "velocityY": 3.134558517591893, + "timestamp": 6.4344817606397315 + }, + { + "x": 7.041010956390259, + "y": 2.8078230861439333, + "heading": 3.1372928730865968, + "angularVelocity": 0.0000028345528292870936, + "velocityX": 2.811841463147611, + "velocityY": 3.4942846306680346, + "timestamp": 6.48289633017195 + }, + { + "x": 7.162462195458692, + "y": 2.9878287177907508, + "heading": 3.1372930108093384, + "angularVelocity": 0.000002844654873697454, + "velocityX": 2.508567983602721, + "velocityY": 3.718005414197231, + "timestamp": 6.531310899704169 + }, + { + "x": 7.283896132649579, + "y": 3.1678460219163584, + "heading": 3.1372931485973305, + "angularVelocity": 0.0000028460026277310735, + "velocityX": 2.508210614370468, + "velocityY": 3.7182465085393464, + "timestamp": 6.579725469236388 + }, + { + "x": 7.397867414319233, + "y": 3.343303097157428, + "heading": 3.1588948465103956, + "angularVelocity": 0.44618176143629246, + "velocityX": 2.354069916780078, + "velocityY": 3.624055257256956, + "timestamp": 6.628140038768607 + }, + { + "x": 7.506043046157439, + "y": 3.501095473470491, + "heading": 3.228380651047953, + "angularVelocity": 1.4352250822206813, + "velocityX": 2.2343611206997815, + "velocityY": 3.2591919712940056, + "timestamp": 6.676554608300826 + }, + { + "x": 7.6042586153437055, + "y": 3.6443897724231573, + "heading": 3.344703303282488, + "angularVelocity": 2.402637333316058, + "velocityX": 2.0286366301554346, + "velocityY": 2.959735061928151, + "timestamp": 6.724969177833045 + }, + { + "x": 7.688078454298536, + "y": 3.766145806545298, + "heading": 3.4608719199161126, + "angularVelocity": 2.3994557373130703, + "velocityX": 1.7312936945365136, + "velocityY": 2.5148635069679726, + "timestamp": 6.773383747365264 + }, + { + "x": 7.757675538495618, + "y": 3.866750816123695, + "heading": 3.5658223462991305, + "angularVelocity": 2.167744697454667, + "velocityX": 1.4375235568451696, + "velocityY": 2.0779903766664107, + "timestamp": 6.821798316897483 + }, + { + "x": 7.8131946706107165, + "y": 3.94664371955485, + "heading": 3.6546108231176855, + "angularVelocity": 1.8339206085364161, + "velocityX": 1.1467443096473655, + "velocityY": 1.6501830792482484, + "timestamp": 6.870212886429702 + }, + { + "x": 7.8547317895295015, + "y": 4.006195440456753, + "heading": 3.7240095680654406, + "angularVelocity": 1.4334268716687006, + "velocityX": 0.857946674319663, + "velocityY": 1.2300371866834907, + "timestamp": 6.9186274559619205 + }, + { + "x": 7.882369894183202, + "y": 4.045699068997907, + "heading": 3.7716896343345847, + "angularVelocity": 0.9848288796085234, + "velocityX": 0.5708633768871593, + "velocityY": 0.81594505378934, + "timestamp": 6.967042025494139 + }, + { + "x": 7.896173477172852, + "y": 4.065373420715332, + "heading": 3.79602872084114, + "angularVelocity": 0.5027223569623597, + "velocityX": 0.28511217022106833, + "velocityY": 0.4063725425531795, + "timestamp": 7.015456595026358 + }, + { + "x": 7.896173477172852, + "y": 4.065373420715332, + "heading": 3.79602872084114, + "angularVelocity": 4.6010842651263e-18, + "velocityX": 6.606474514740261e-18, + "velocityY": 4.020513640198354e-18, + "timestamp": 7.063871164558577 + }, + { + "x": 7.883686491968607, + "y": 4.04817527202583, + "heading": 3.7732837957184815, + "angularVelocity": -0.4976688757750139, + "velocityX": -0.273220679114313, + "velocityY": -0.3763029896806603, + "timestamp": 7.1095740932121565 + }, + { + "x": 7.858644371282851, + "y": 4.01363266629876, + "heading": 3.7289682484036177, + "angularVelocity": -0.9696434915748829, + "velocityX": -0.5479325160006953, + "velocityY": -0.7558072697900738, + "timestamp": 7.155277021865736 + }, + { + "x": 7.8209799739706884, + "y": 3.961528626225198, + "heading": 3.664728508619979, + "angularVelocity": -1.4055935073779289, + "velocityX": -0.8241134304028905, + "velocityY": -1.1400591079951226, + "timestamp": 7.200979950519315 + }, + { + "x": 7.770648499127757, + "y": 3.8915760964517476, + "heading": 3.5826191900873825, + "angularVelocity": -1.796587679423395, + "velocityX": -1.1012746081204872, + "velocityY": -1.530591842454609, + "timestamp": 7.246682879172894 + }, + { + "x": 7.707615042416261, + "y": 3.8034422194517306, + "heading": 3.4850702036014467, + "angularVelocity": -2.134414344107738, + "velocityX": -1.3791995079631632, + "velocityY": -1.9284076446842287, + "timestamp": 7.292385807826474 + }, + { + "x": 7.631773516002201, + "y": 3.696764579350947, + "heading": 3.3754828392132095, + "angularVelocity": -2.39781929991619, + "velocityX": -1.6594456558554744, + "velocityY": -2.3341532642116922, + "timestamp": 7.338088736480053 + }, + { + "x": 7.542882516453489, + "y": 3.5711235953994676, + "heading": 3.261672437263455, + "angularVelocity": -2.490221202506508, + "velocityX": -1.9449738160650807, + "velocityY": -2.7490794934351226, + "timestamp": 7.383791665133632 + }, + { + "x": 7.44454344391469, + "y": 3.428056176286614, + "heading": 3.1768425535327034, + "angularVelocity": -1.8561148318034846, + "velocityX": -2.151701771327938, + "velocityY": -3.13037749062619, + "timestamp": 7.429494593787211 + }, + { + "x": 7.340219532855234, + "y": 3.2695180348342774, + "heading": 3.135286948560975, + "angularVelocity": -0.9092547500998464, + "velocityX": -2.2826526468400488, + "velocityY": -3.468883638815311, + "timestamp": 7.475197522440791 + }, + { + "x": 7.224153526183016, + "y": 3.1005590910373835, + "heading": 3.135286791197327, + "angularVelocity": -0.00000344318520706556, + "velocityX": -2.5395748170106347, + "velocityY": -3.696895336347947, + "timestamp": 7.52090045109437 + }, + { + "x": 7.107477607000754, + "y": 2.932020744346244, + "heading": 3.135286646763266, + "angularVelocity": -0.000003160280207883062, + "velocityX": -2.5529199685789394, + "velocityY": -3.687692488344275, + "timestamp": 7.566603379747949 + }, + { + "x": 6.990772092919987, + "y": 2.763502889489356, + "heading": 3.135286502334091, + "angularVelocity": -0.0000031601733036030435, + "velocityX": -2.553567517858124, + "velocityY": -3.687244118078817, + "timestamp": 7.6123063084015286 + }, + { + "x": 6.858421912732069, + "y": 2.6069721451251513, + "heading": 3.135286357995183, + "angularVelocity": -0.000003158198218572508, + "velocityX": -2.8958796315026465, + "velocityY": -3.424960915539557, + "timestamp": 7.658009237055108 + }, + { + "x": 6.709310605779643, + "y": 2.466315612343406, + "heading": 3.1352862013811205, + "angularVelocity": -0.000003426783961068071, + "velocityX": -3.2626203909746376, + "velocityY": -3.077626246840689, + "timestamp": 7.703712165708687 + }, + { + "x": 6.545329184626209, + "y": 2.343317519944224, + "heading": 3.13528601237849, + "angularVelocity": -0.000004135459935590175, + "velocityX": -3.5879849712998917, + "velocityY": -2.691251874283261, + "timestamp": 7.749415094362266 + }, + { + "x": 6.368557481944258, + "y": 2.23953798710901, + "heading": 3.1352857469986737, + "angularVelocity": -0.0000058066260578378354, + "velocityX": -3.8678419061905776, + "velocityY": -2.270741414009659, + "timestamp": 7.795118023015846 + }, + { + "x": 6.181237697601318, + "y": 2.1562933921813965, + "heading": 3.135285039902942, + "angularVelocity": -0.000015471562811805473, + "velocityX": -4.098638530646304, + "velocityY": -1.8214280217924592, + "timestamp": 7.840820951669425 + }, + { + "x": 5.874969956696071, + "y": 2.079869439194049, + "heading": 3.1352844358731087, + "angularVelocity": -0.000008582546125130102, + "velocityX": -4.351700640081255, + "velocityY": -1.0858935523199316, + "timestamp": 7.911199814150077 + }, + { + "x": 5.561830297244809, + "y": 2.0576986839525397, + "heading": 3.1292065379013336, + "angularVelocity": -0.08635970741139995, + "velocityX": -4.4493424362655105, + "velocityY": -0.31502008500930834, + "timestamp": 7.981578676630729 + }, + { + "x": 5.265650774435423, + "y": 2.0816151214602616, + "heading": 3.0610179423428407, + "angularVelocity": -0.9688789098751426, + "velocityX": -4.2083590494348115, + "velocityY": 0.3398241555024946, + "timestamp": 8.051957539111381 + }, + { + "x": 5.005528861829494, + "y": 2.114389972569813, + "heading": 2.8883900412149814, + "angularVelocity": -2.45283732989175, + "velocityX": -3.6960232580831667, + "velocityY": 0.46569168574679387, + "timestamp": 8.122336401592033 + }, + { + "x": 4.8000582736739394, + "y": 2.145902528001046, + "heading": 2.722850674189578, + "angularVelocity": -2.3521176840691327, + "velocityX": -2.9194928834213356, + "velocityY": 0.4477559642271928, + "timestamp": 8.192715264072685 + }, + { + "x": 4.647342956856588, + "y": 2.17150300925771, + "heading": 2.5899379229501136, + "angularVelocity": -1.8885322461130774, + "velocityX": -2.1699031702783396, + "velocityY": 0.363752415914727, + "timestamp": 8.263094126553337 + }, + { + "x": 4.5461805350001505, + "y": 2.189414674483752, + "heading": 2.4978059035992817, + "angularVelocity": -1.3090865084124141, + "velocityX": -1.4373977965933769, + "velocityY": 0.25450347724741956, + "timestamp": 8.33347298903399 + }, + { + "x": 4.495876312255859, + "y": 2.198692560195923, + "heading": 2.4502412669675735, + "angularVelocity": -0.6758369623377337, + "velocityX": -0.7147632253664121, + "velocityY": 0.13182773044566, + "timestamp": 8.403851851514641 + }, + { + "x": 4.495876312255859, + "y": 2.198692560195923, + "heading": 2.4502412669675735, + "angularVelocity": -7.70463937191343e-20, + "velocityX": -4.406552052721558e-19, + "velocityY": -7.177153869205785e-19, + "timestamp": 8.474230713995293 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/A123.1.traj b/src/main/deploy/choreo/A123.1.traj new file mode 100644 index 0000000..9a5ef0e --- /dev/null +++ b/src/main/deploy/choreo/A123.1.traj @@ -0,0 +1,103 @@ +{ + "samples": [ + { + "x": 1.775368571281433, + "y": 6.0120086669921875, + "heading": -2.6482728621413125, + "angularVelocity": -8.923254857024236e-25, + "velocityX": -9.616936178650736e-23, + "velocityY": -9.058247523111096e-23, + "timestamp": 0 + }, + { + "x": 1.8190963321489977, + "y": 6.053194106989434, + "heading": -2.644950811196117, + "angularVelocity": 0.035592499333761476, + "velocityX": 0.468499828937473, + "velocityY": 0.4412613683070305, + "timestamp": 0.09333570295369455 + }, + { + "x": 1.906551852113573, + "y": 6.1355649856042165, + "heading": -2.6383066719214034, + "angularVelocity": 0.07118539920366639, + "velocityX": 0.9369996389052054, + "velocityY": 0.8825227218318404, + "timestamp": 0.1866714059073891 + }, + { + "x": 2.0377351280171383, + "y": 6.259121300336279, + "heading": -2.6283403806935786, + "angularVelocity": 0.10677898073762115, + "velocityX": 1.4054994150378615, + "velocityY": 1.323784048568862, + "timestamp": 0.28000710886108365 + }, + { + "x": 2.2126461527991568, + "y": 6.42386304535477, + "heading": -2.615051832718829, + "angularVelocity": 0.1423736850339289, + "velocityX": 1.8739991155238322, + "velocityY": 1.7650453128340622, + "timestamp": 0.3733428118147782 + }, + { + "x": 2.4312848931630335, + "y": 6.629790191733643, + "heading": -2.5984407382107015, + "angularVelocity": 0.1779714941062607, + "velocityX": 2.342498459269624, + "velocityY": 2.206306267185214, + "timestamp": 0.4666785147684727 + }, + { + "x": 2.606195913698415, + "y": 6.794531937816056, + "heading": -2.585151593446195, + "angularVelocity": 0.14238007904755712, + "velocityX": 1.8739990700253055, + "velocityY": 1.7650453242329325, + "timestamp": 0.5600142177221673 + }, + { + "x": 2.7373791849088662, + "y": 6.918088253515589, + "heading": -2.57518466420097, + "angularVelocity": 0.1067858164647894, + "velocityX": 1.405499364755771, + "velocityY": 1.3237840589343497, + "timestamp": 0.6533499206758618 + }, + { + "x": 2.8248347011284207, + "y": 7.000459132839087, + "heading": -2.5685400217250343, + "angularVelocity": 0.07119079050845539, + "velocityX": 0.9369995987810015, + "velocityY": 0.8825227294250255, + "timestamp": 0.7466856236295564 + }, + { + "x": 2.8685624599456787, + "y": 7.04164457321167, + "heading": -2.5652176967040865, + "angularVelocity": 0.03559543578512434, + "velocityX": 0.468499806970458, + "velocityY": 0.4412613723284018, + "timestamp": 0.8400213265832509 + }, + { + "x": 2.8685624599456787, + "y": 7.04164457321167, + "heading": -2.5652176967040865, + "angularVelocity": 8.96777028872441e-25, + "velocityX": 9.964414836663175e-23, + "velocityY": 4.965750215054413e-23, + "timestamp": 0.9333570295369454 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/A123.2.traj b/src/main/deploy/choreo/A123.2.traj new file mode 100644 index 0000000..84305ed --- /dev/null +++ b/src/main/deploy/choreo/A123.2.traj @@ -0,0 +1,202 @@ +{ + "samples": [ + { + "x": 2.8685624599456787, + "y": 7.04164457321167, + "heading": -2.5652176967040865, + "angularVelocity": 8.96777028872441e-25, + "velocityX": 9.964414836663175e-23, + "velocityY": 4.965750215054413e-23, + "timestamp": 0 + }, + { + "x": 2.9198374533140026, + "y": 7.045992082496191, + "heading": -2.5770293802157602, + "angularVelocity": -0.13652275169185538, + "velocityX": 0.5926507580995501, + "velocityY": 0.050249731965998315, + "timestamp": 0.08651805919011568 + }, + { + "x": 3.022404200010661, + "y": 7.054688522110406, + "heading": -2.6000376852804123, + "angularVelocity": -0.26593644471489375, + "velocityX": 1.1854952325187602, + "velocityY": 0.10051588877075252, + "timestamp": 0.17303611838023147 + }, + { + "x": 3.176284936967033, + "y": 7.067735777481485, + "heading": -2.633369012545948, + "angularVelocity": -0.3852528313457996, + "velocityX": 1.7785967276292372, + "velocityY": 0.15080383787167673, + "timestamp": 0.25955417757034727 + }, + { + "x": 3.381510228293591, + "y": 7.085136440082294, + "heading": -2.675682452707487, + "angularVelocity": -0.4890706120505913, + "velocityX": 2.3720514913030337, + "velocityY": 0.20112173994300755, + "timestamp": 0.34607223676046306 + }, + { + "x": 3.6381227114829775, + "y": 7.106894125064837, + "heading": -2.7246679320884843, + "angularVelocity": -0.5661879131310169, + "velocityX": 2.965999071078366, + "velocityY": 0.2514814269554226, + "timestamp": 0.43259029595057885 + }, + { + "x": 3.9461710841169118, + "y": 7.133012961404564, + "heading": -2.775445026128338, + "angularVelocity": -0.5868958979798145, + "velocityX": 3.5605095111649008, + "velocityY": 0.3018888378244019, + "timestamp": 0.5191083551406946 + }, + { + "x": 4.305378922938188, + "y": 7.163469511698595, + "heading": -2.810773174167225, + "angularVelocity": -0.40833264603470154, + "velocityX": 4.151824973696521, + "velocityY": 0.3520253526157542, + "timestamp": 0.6056264143308104 + }, + { + "x": 4.675926456784465, + "y": 7.194887533192645, + "heading": -2.810773196454242, + "angularVelocity": -2.5759959514533893e-7, + "velocityX": 4.282892350047188, + "velocityY": 0.3631383064778689, + "timestamp": 0.6921444735209262 + }, + { + "x": 5.046473990630761, + "y": 7.226305554686697, + "heading": -2.810773218741196, + "angularVelocity": -2.5759886662698844e-7, + "velocityX": 4.28289235004742, + "velocityY": 0.3631383064778886, + "timestamp": 0.778662532711042 + }, + { + "x": 5.4170215244770565, + "y": 7.25772357618075, + "heading": -2.8107732410281496, + "angularVelocity": -2.575988646921058e-7, + "velocityX": 4.282892350047419, + "velocityY": 0.3631383064778885, + "timestamp": 0.8651805919011578 + }, + { + "x": 5.787569058323353, + "y": 7.289141597674803, + "heading": -2.8107732633151037, + "angularVelocity": -2.575988664941198e-7, + "velocityX": 4.28289235004742, + "velocityY": 0.3631383064778886, + "timestamp": 0.9516986510912736 + }, + { + "x": 6.158116592169649, + "y": 7.320559619168855, + "heading": -2.810773285602058, + "angularVelocity": -2.575988666257598e-7, + "velocityX": 4.28289235004742, + "velocityY": 0.36313830647788864, + "timestamp": 1.0382167102813895 + }, + { + "x": 6.528664126015926, + "y": 7.351977640662906, + "heading": -2.810773307889075, + "angularVelocity": -2.575995940422885e-7, + "velocityX": 4.28289235004719, + "velocityY": 0.36313830647786904, + "timestamp": 1.1247347694715053 + }, + { + "x": 6.888011462286063, + "y": 7.382446018680633, + "heading": -2.8464199451568986, + "angularVelocity": -0.41201383389210466, + "velocityX": 4.153437324345239, + "velocityY": 0.3521620607643999, + "timestamp": 1.211252828661621 + }, + { + "x": 7.1961183473382455, + "y": 7.408569816177152, + "heading": -2.8950909993625498, + "angularVelocity": -0.5625536987451474, + "velocityX": 3.56118581411015, + "velocityY": 0.3019461802664086, + "timestamp": 1.2977708878517369 + }, + { + "x": 7.452790863790213, + "y": 7.430332591265989, + "heading": -2.941640853640219, + "angularVelocity": -0.5380362748935424, + "velocityX": 2.966692952369079, + "velocityY": 0.251540259831949, + "timestamp": 1.3842889470418527 + }, + { + "x": 7.658073050822811, + "y": 7.4477380779456315, + "heading": -2.9817184792555684, + "angularVelocity": -0.46322844028762317, + "velocityX": 2.3727091078350337, + "velocityY": 0.2011774979995321, + "timestamp": 1.4708070062319685 + }, + { + "x": 7.812001701866268, + "y": 7.460789395861135, + "heading": -3.0132404006110005, + "angularVelocity": -0.3643392102239029, + "velocityX": 1.7791505320896364, + "velocityY": 0.15085079389985404, + "timestamp": 1.5573250654220843 + }, + { + "x": 7.914603145410721, + "y": 7.469488777355164, + "heading": -3.034982434060706, + "angularVelocity": -0.2513005221479756, + "velocityX": 1.18589626841948, + "velocityY": 0.10054989184295737, + "timestamp": 1.6438431246122 + }, + { + "x": 7.965896606445312, + "y": 7.473837852478027, + "heading": -3.046138770984786, + "angularVelocity": -0.12894807198073027, + "velocityX": 0.5928642125672009, + "velocityY": 0.05026783036482732, + "timestamp": 1.7303611838023158 + }, + { + "x": 7.965896606445312, + "y": 7.473837852478027, + "heading": -3.046138770984786, + "angularVelocity": -1.6061739574692185e-27, + "velocityX": 2.405650666496034e-19, + "velocityY": 2.5667630572731595e-18, + "timestamp": 1.8168792429924316 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/A123.3.traj b/src/main/deploy/choreo/A123.3.traj new file mode 100644 index 0000000..b8275c3 --- /dev/null +++ b/src/main/deploy/choreo/A123.3.traj @@ -0,0 +1,130 @@ +{ + "samples": [ + { + "x": 7.965896606445312, + "y": 7.473837852478027, + "heading": -3.046138770984786, + "angularVelocity": -1.6061739574692185e-27, + "velocityX": 2.405650666496034e-19, + "velocityY": 2.5667630572731595e-18, + "timestamp": 0 + }, + { + "x": 7.905970693449125, + "y": 7.456889105738656, + "heading": -3.045547301520618, + "angularVelocity": 0.006224276082072812, + "velocityX": -0.6306249934360915, + "velocityY": -0.17835862262033683, + "timestamp": 0.09502622576004782 + }, + { + "x": 7.786118868818083, + "y": 7.422991612644937, + "heading": -3.044364362729114, + "angularVelocity": 0.012448550724206106, + "velocityX": -1.261249972546328, + "velocityY": -0.3567172411889158, + "timestamp": 0.19005245152009564 + }, + { + "x": 7.606341134594192, + "y": 7.372145373774407, + "heading": -3.0425899548091673, + "angularVelocity": 0.018672823273306667, + "velocityX": -1.8918749301677051, + "velocityY": -0.5350758536798363, + "timestamp": 0.28507867728014347 + }, + { + "x": 7.366637494180796, + "y": 7.304350390089628, + "heading": -3.0402240780931336, + "angularVelocity": 0.0248970923248922, + "velocityX": -2.522499851974286, + "velocityY": -0.7134344560413174, + "timestamp": 0.3801049030401913 + }, + { + "x": 7.067007954384597, + "y": 7.219606663515728, + "heading": -3.03726673324464, + "angularVelocity": 0.031121354392853406, + "velocityX": -3.153124702151143, + "velocityY": -0.8917930381438821, + "timestamp": 0.4751311288002391 + }, + { + "x": 6.7074525356259445, + "y": 7.11791419982816, + "heading": -3.033717922068974, + "angularVelocity": 0.03734559746303509, + "velocityX": -3.7837493374363023, + "velocityY": -1.0701515594689839, + "timestamp": 0.5701573545602869 + }, + { + "x": 6.347897116868929, + "y": 7.016221736141056, + "heading": -3.030169108902523, + "angularVelocity": 0.03734561841288037, + "velocityX": -3.7837493374190543, + "velocityY": -1.0701515594641053, + "timestamp": 0.6651835803203348 + }, + { + "x": 6.048267577074264, + "y": 6.9314780095675905, + "heading": -3.0272117621899937, + "angularVelocity": 0.031121374008861453, + "velocityX": -3.153124702135004, + "velocityY": -0.8917930381393175, + "timestamp": 0.7602098060803826 + }, + { + "x": 5.8085639366622, + "y": 6.863683025883188, + "heading": -3.0248458838523082, + "angularVelocity": 0.02489710939019601, + "velocityX": -2.5224998519602666, + "velocityY": -0.7134344560373521, + "timestamp": 0.8552360318404304 + }, + { + "x": 5.628786202439365, + "y": 6.812836787012956, + "heading": -3.0230714746436687, + "angularVelocity": 0.01867283683475372, + "velocityX": -1.8918749301565887, + "velocityY": -0.5350758536766922, + "timestamp": 0.9502622576004782 + }, + { + "x": 5.508934377809051, + "y": 6.778939293919443, + "heading": -3.0218885349617404, + "angularVelocity": 0.012448560094509599, + "velocityX": -1.2612499725386697, + "velocityY": -0.35671724118674986, + "timestamp": 1.045288483360526 + }, + { + "x": 5.449008464813232, + "y": 6.761990547180176, + "heading": -3.0212970650452653, + "angularVelocity": 0.006224280841886529, + "velocityX": -0.6306249934322161, + "velocityY": -0.1783586226192408, + "timestamp": 1.1403147091205739 + }, + { + "x": 5.449008464813232, + "y": 6.761990547180176, + "heading": -3.0212970650452653, + "angularVelocity": 2.0281055022755585e-29, + "velocityX": 1.1430879114890998e-18, + "velocityY": -1.6176856268439436e-18, + "timestamp": 1.2353409348806217 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/A123.4.traj b/src/main/deploy/choreo/A123.4.traj new file mode 100644 index 0000000..0607ac7 --- /dev/null +++ b/src/main/deploy/choreo/A123.4.traj @@ -0,0 +1,139 @@ +{ + "samples": [ + { + "x": 5.449008464813232, + "y": 6.761990547180176, + "heading": -3.0212970650452653, + "angularVelocity": 2.0281055022755585e-29, + "velocityX": 1.1430879114890998e-18, + "velocityY": -1.6176856268439436e-18, + "timestamp": 0 + }, + { + "x": 5.49985252708244, + "y": 6.742794318695081, + "heading": -3.0259671249570506, + "angularVelocity": -0.052596426193898003, + "velocityX": 0.572629906051452, + "velocityY": -0.2161970154107702, + "timestamp": 0.08879044166554362 + }, + { + "x": 5.601543140419982, + "y": 6.704400922076198, + "heading": -3.0350465824550086, + "angularVelocity": -0.10225714984230815, + "velocityX": 1.1452878421372228, + "velocityY": -0.4324046135900921, + "timestamp": 0.17758088333108724 + }, + { + "x": 5.75408366076328, + "y": 6.64680909028587, + "heading": -3.048157311029397, + "angularVelocity": -0.14765923367938932, + "velocityX": 1.7179835743794172, + "velocityY": -0.6486264817475036, + "timestamp": 0.26637132499663085 + }, + { + "x": 5.957478781668101, + "y": 6.570017051267244, + "heading": -3.064701200025727, + "angularVelocity": -0.1863251120953702, + "velocityX": 2.2907321676692405, + "velocityY": -0.8648683076483163, + "timestamp": 0.35516176666217447 + }, + { + "x": 6.211735113142484, + "y": 6.4740223094049245, + "heading": -3.0835897723567736, + "angularVelocity": -0.2127320461159122, + "velocityX": 2.863555205999725, + "velocityY": -1.0811382403515035, + "timestamp": 0.4439522083277181 + }, + { + "x": 6.516858357819224, + "y": 6.358822711663981, + "heading": -3.102223690599022, + "angularVelocity": -0.2098640111785739, + "velocityX": 3.436442470081179, + "velocityY": -1.2974324215536346, + "timestamp": 0.5327426499932617 + }, + { + "x": 6.872622299397898, + "y": 6.224503666243447, + "heading": -3.1061843374394424, + "angularVelocity": -0.044606680247623424, + "velocityX": 4.006781979064447, + "velocityY": -1.512764695173915, + "timestamp": 0.6215330916588053 + }, + { + "x": 7.177745788997514, + "y": 6.109303976031618, + "heading": -3.124829178589315, + "angularVelocity": -0.20998703013669084, + "velocityX": 3.4364452285185845, + "velocityY": -1.2974334630045405, + "timestamp": 0.7103235333243489 + }, + { + "x": 7.43200235076056, + "y": 6.013309147223575, + "heading": -3.1436995015656892, + "angularVelocity": -0.21252651324176217, + "velocityX": 2.8635577996196915, + "velocityY": -1.081139219575414, + "timestamp": 0.7991139749898926 + }, + { + "x": 7.635397748239775, + "y": 5.936517003784, + "heading": -3.160215730785825, + "angularVelocity": -0.18601359459782157, + "velocityX": 2.2907352825810445, + "velocityY": -0.8648694836865012, + "timestamp": 0.8879044166554362 + }, + { + "x": 7.787938540074382, + "y": 5.878925069491846, + "heading": -3.1732992809366647, + "angularVelocity": -0.1473531374032643, + "velocityX": 1.7179866320430934, + "velocityY": -0.6486276361715984, + "timestamp": 0.9766948583209798 + }, + { + "x": 7.889629370431644, + "y": 5.840531590936942, + "heading": -3.1823575431703564, + "angularVelocity": -0.10201843873929947, + "velocityX": 1.145290286316077, + "velocityY": -0.43240553639235546, + "timestamp": 1.0654852999865234 + }, + { + "x": 7.940473556518555, + "y": 5.821335315704346, + "heading": -3.1870158092601475, + "angularVelocity": -0.052463598585734586, + "velocityX": 0.5726313005450608, + "velocityY": -0.2161975419032796, + "timestamp": 1.154275741652067 + }, + { + "x": 7.940473556518555, + "y": 5.821335315704346, + "heading": -3.1870158092601475, + "angularVelocity": 1.5550002280971524e-27, + "velocityX": -4.5599517029668704e-20, + "velocityY": -1.2345517643295343e-19, + "timestamp": 1.2430661833176107 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/A123.5.traj b/src/main/deploy/choreo/A123.5.traj new file mode 100644 index 0000000..4271e1f --- /dev/null +++ b/src/main/deploy/choreo/A123.5.traj @@ -0,0 +1,139 @@ +{ + "samples": [ + { + "x": 7.940473556518555, + "y": 5.821335315704346, + "heading": -3.1870158092601475, + "angularVelocity": 1.5550002280971524e-27, + "velocityX": -4.5599517029668704e-20, + "velocityY": -1.2345517643295343e-19, + "timestamp": 0 + }, + { + "x": 7.889367117787343, + "y": 5.841051497608848, + "heading": -3.182401245414936, + "angularVelocity": 0.05176721882064578, + "velocityX": -0.5733235654953367, + "velocityY": 0.22118057896568025, + "timestamp": 0.089140656004707 + }, + { + "x": 7.78715191484382, + "y": 5.880484758557419, + "heading": -3.1734212335230674, + "angularVelocity": 0.10073980038237511, + "velocityX": -1.146673218762545, + "velocityY": 0.4423712222455345, + "timestamp": 0.178281312009414 + }, + { + "x": 7.63382478470437, + "y": 5.93963631878692, + "heading": -3.160439385829673, + "angularVelocity": 0.14563329770322905, + "velocityX": -1.7200583550939212, + "velocityY": 0.6635755544179243, + "timestamp": 0.267421968014121 + }, + { + "x": 7.429381246451849, + "y": 6.018507906975374, + "heading": -3.1440362842645486, + "angularVelocity": 0.18401369588595168, + "velocityX": -2.293493759365243, + "velocityY": 0.8847992792905789, + "timestamp": 0.356562624018828 + }, + { + "x": 7.173814855056923, + "y": 6.117102009529074, + "heading": -3.1252866571110136, + "angularVelocity": 0.21033754959740159, + "velocityX": -2.867001465430438, + "velocityY": 1.106050897230241, + "timestamp": 0.445703280023535 + }, + { + "x": 6.867120001482762, + "y": 6.235420790339557, + "heading": -3.106859923030452, + "angularVelocity": 0.20671526222096004, + "velocityX": -3.440572094936845, + "velocityY": 1.3273267901936336, + "timestamp": 0.534843936028242 + }, + { + "x": 6.509649519466585, + "y": 6.373328130860556, + "heading": -3.106859612143933, + "angularVelocity": 0.0000034875951447068007, + "velocityX": -4.01018455593707, + "velocityY": 1.5470756745801426, + "timestamp": 0.623984592032949 + }, + { + "x": 6.202954670903185, + "y": 6.491646909737954, + "heading": -3.0884387572905028, + "angularVelocity": 0.20664930772393342, + "velocityX": -3.4405720387249987, + "velocityY": 1.3273267685078536, + "timestamp": 0.713125248037656 + }, + { + "x": 5.947388281937679, + "y": 6.590241011354414, + "heading": -3.069691188796424, + "angularVelocity": 0.2103144550909352, + "velocityX": -2.867001438176646, + "velocityY": 1.1060508867160919, + "timestamp": 0.802265904042363 + }, + { + "x": 5.742944749611789, + "y": 6.669112597256453, + "heading": -3.0532893829542354, + "angularVelocity": 0.18399916017358833, + "velocityX": -2.2934936928789664, + "velocityY": 0.8847992536410609, + "timestamp": 0.89140656004707 + }, + { + "x": 5.589617624363911, + "y": 6.728264155598851, + "heading": -3.040308767682206, + "angularVelocity": 0.14561947212217205, + "velocityX": -1.720058300219161, + "velocityY": 0.6635755332479741, + "timestamp": 0.980547216051777 + }, + { + "x": 5.487402424003547, + "y": 6.767697415550875, + "heading": -3.031329850849583, + "angularVelocity": 0.10072751576057763, + "velocityX": -1.1466731897840863, + "velocityY": 0.44237121106603183, + "timestamp": 1.069687872056484 + }, + { + "x": 5.436295986175537, + "y": 6.787413597106934, + "heading": -3.0267159569026636, + "angularVelocity": 0.05175970375039749, + "velocityX": -0.5733235553630177, + "velocityY": 0.2211805750567668, + "timestamp": 1.158828528061191 + }, + { + "x": 5.436295986175537, + "y": 6.787413597106934, + "heading": -3.0267159569026636, + "angularVelocity": 6.854677049309127e-18, + "velocityX": -8.559616674403826e-18, + "velocityY": -7.1969388527604e-18, + "timestamp": 1.247969184065898 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/A123.6.traj b/src/main/deploy/choreo/A123.6.traj new file mode 100644 index 0000000..355a79e --- /dev/null +++ b/src/main/deploy/choreo/A123.6.traj @@ -0,0 +1,220 @@ +{ + "samples": [ + { + "x": 5.436295986175537, + "y": 6.787413597106934, + "heading": -3.0267159569026636, + "angularVelocity": 6.854677049309127e-18, + "velocityX": -8.559616674403826e-18, + "velocityY": -7.1969388527604e-18, + "timestamp": 0 + }, + { + "x": 5.460898661874906, + "y": 6.770891554233058, + "heading": -3.031032530385192, + "angularVelocity": -0.06580682525696466, + "velocityX": 0.3750715670995127, + "velocityY": -0.25188107944466487, + "timestamp": 0.06559461675443057 + }, + { + "x": 5.510104016852139, + "y": 6.737847475934615, + "heading": -3.039665750319823, + "angularVelocity": -0.13161476294628496, + "velocityX": 0.7501431887537074, + "velocityY": -0.5037620453235327, + "timestamp": 0.13118923350886114 + }, + { + "x": 5.583912055468166, + "y": 6.688281373798517, + "heading": -3.052615800766114, + "angularVelocity": -0.197425506651816, + "velocityX": 1.1252148768909827, + "velocityY": -0.7556428345580732, + "timestamp": 0.1967838502632917 + }, + { + "x": 5.682322782278138, + "y": 6.622193263600659, + "heading": -3.0698830093365843, + "angularVelocity": -0.26324124486047346, + "velocityX": 1.5002866344722108, + "velocityY": -1.007523383286727, + "timestamp": 0.26237846701772227 + }, + { + "x": 5.805336201198385, + "y": 6.5395831653172145, + "heading": -3.091467888045328, + "angularVelocity": -0.32906478880259576, + "velocityX": 1.8753584517584796, + "velocityY": -1.2594036274755147, + "timestamp": 0.32797308377215284 + }, + { + "x": 5.952952314377682, + "y": 6.440451103068376, + "heading": -3.117371178377928, + "angularVelocity": -0.3948996368062194, + "velocityX": 2.250430301803197, + "velocityY": -1.5112835039482873, + "timestamp": 0.3935677005265834 + }, + { + "x": 6.125171120641939, + "y": 6.324797105039988, + "heading": -3.1475938920598048, + "angularVelocity": -0.46074990871916927, + "velocityX": 2.625502133948695, + "velocityY": -1.7631629507235327, + "timestamp": 0.459162317281014 + }, + { + "x": 6.321992612980133, + "y": 6.1926212038165795, + "heading": -3.182137324335163, + "angularVelocity": -0.5266199268288194, + "velocityX": 3.000573859210087, + "velocityY": -2.0150419007426175, + "timestamp": 0.5247569340354445 + }, + { + "x": 6.5434167711211595, + "y": 6.0439234407720175, + "heading": -3.2210029154552626, + "angularVelocity": -0.5925119017899734, + "velocityX": 3.3756452754353816, + "velocityY": -2.2669202200107508, + "timestamp": 0.5903515507898751 + }, + { + "x": 6.768454422425858, + "y": 5.874071349001229, + "heading": -3.221002927063026, + "angularVelocity": -1.7696213344774776e-7, + "velocityX": 3.4307335333175626, + "velocityY": -2.589421208250734, + "timestamp": 0.6559461675443057 + }, + { + "x": 6.97439432144165, + "y": 5.681508541107178, + "heading": -3.221002939203244, + "angularVelocity": -1.8507949182524416e-7, + "velocityX": 3.1395853685157755, + "velocityY": -2.935649561230799, + "timestamp": 0.7215407842987362 + }, + { + "x": 7.15472976461684, + "y": 5.474395114221533, + "heading": -3.221002948946015, + "angularVelocity": -1.5248985870081709e-7, + "velocityX": 2.82253640171159, + "velocityY": -3.2416544211944602, + "timestamp": 0.7854320570794942 + }, + { + "x": 7.312913869889132, + "y": 5.249907624667417, + "heading": -3.2210029583127433, + "angularVelocity": -1.4660418880845298e-7, + "velocityX": 2.4758327450338733, + "velocityY": -3.5135861250456206, + "timestamp": 0.849323329860252 + }, + { + "x": 7.448597158749216, + "y": 5.036236251986827, + "heading": -3.2541109060242546, + "angularVelocity": -0.5181920201467125, + "velocityX": 2.123659194042101, + "velocityY": -3.344296699392882, + "timestamp": 0.91321460264101 + }, + { + "x": 7.569203088849137, + "y": 4.846305269940681, + "heading": -3.28354394578579, + "angularVelocity": -0.46067386797572535, + "velocityX": 1.8876745578959737, + "velocityY": -2.972721841024385, + "timestamp": 0.9771058754217679 + }, + { + "x": 7.674732721316318, + "y": 4.680115323289921, + "heading": -3.3092993426884116, + "angularVelocity": -0.40311291013356876, + "velocityX": 1.6517065300803815, + "velocityY": -2.601136891122462, + "timestamp": 1.0409971482025258 + }, + { + "x": 7.765186402550063, + "y": 4.537666622961828, + "heading": -3.33137623392435, + "angularVelocity": -0.3455384479786788, + "velocityX": 1.4157439239644065, + "velocityY": -2.229548639873397, + "timestamp": 1.1048884209832837 + }, + { + "x": 7.840564301968412, + "y": 4.418959271788617, + "heading": -3.3497741734870097, + "angularVelocity": -0.28795700511180566, + "velocityX": 1.1797839695099621, + "velocityY": -1.857958779136701, + "timestamp": 1.1687796937640416 + }, + { + "x": 7.900866519469214, + "y": 4.323993329938929, + "heading": -3.364492865366424, + "angularVelocity": -0.23037092921851174, + "velocityX": 0.943825578615652, + "velocityY": -1.486367976665167, + "timestamp": 1.2326709665447995 + }, + { + "x": 7.946093121157217, + "y": 4.252768836889262, + "heading": -3.3755320899639822, + "angularVelocity": -0.17278141625794857, + "velocityX": 0.7078682223656876, + "velocityY": -1.1147765563237297, + "timestamp": 1.2965622393255574 + }, + { + "x": 7.976244154590328, + "y": 4.205285820920615, + "heading": -3.382891681018417, + "angularVelocity": -0.115189301044308, + "velocityX": 0.47191161047254004, + "velocityY": -0.7431846933397034, + "timestamp": 1.3604535121063153 + }, + { + "x": 7.99131965637207, + "y": 4.181544303894043, + "heading": -3.386571517316088, + "angularVelocity": -0.05759528864455178, + "velocityX": 0.2359555714827114, + "velocityY": -0.37159248819540497, + "timestamp": 1.4243447848870732 + }, + { + "x": 7.99131965637207, + "y": 4.181544303894043, + "heading": -3.386571517316088, + "angularVelocity": -4.708903555929205e-19, + "velocityX": 3.4384821500622114e-19, + "velocityY": -1.405030037847007e-18, + "timestamp": 1.488236057667831 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/A123.7.traj b/src/main/deploy/choreo/A123.7.traj new file mode 100644 index 0000000..d68603b --- /dev/null +++ b/src/main/deploy/choreo/A123.7.traj @@ -0,0 +1,220 @@ +{ + "samples": [ + { + "x": 7.99131965637207, + "y": 4.181544303894043, + "heading": -3.386571517316088, + "angularVelocity": -4.708903555929205e-19, + "velocityX": 3.4384821500622114e-19, + "velocityY": -1.405030037847007e-18, + "timestamp": 0 + }, + { + "x": 7.978610314810739, + "y": 4.201678078384227, + "heading": -3.383016641435549, + "angularVelocity": 0.06046067739254296, + "velocityX": -0.21615815174249978, + "velocityY": 0.3424315461502773, + "timestamp": 0.05879649441336454 + }, + { + "x": 7.953187149381424, + "y": 4.241942992732508, + "heading": -3.3759159768404734, + "angularVelocity": 0.1207668019313348, + "velocityX": -0.4323925377349786, + "velocityY": 0.684818282960888, + "timestamp": 0.11759298882672908 + }, + { + "x": 7.915044562001454, + "y": 4.302335753884612, + "heading": -3.365280870024358, + "angularVelocity": 0.18087994738845797, + "velocityX": -0.6487221348914206, + "velocityY": 1.0271490121080764, + "timestamp": 0.17638948324009363 + }, + { + "x": 7.864175361270239, + "y": 4.382852126965025, + "heading": -3.3511258737287886, + "angularVelocity": 0.24074558248411781, + "velocityX": -0.8651740420710002, + "velocityY": 1.3694077152686683, + "timestamp": 0.23518597765345817 + }, + { + "x": 7.8005699678526, + "y": 4.483486462884797, + "heading": -3.333470331964337, + "angularVelocity": 0.3002822181935773, + "velocityX": -1.0817888728274418, + "velocityY": 1.711570339759891, + "timestamp": 0.2939824720668227 + }, + { + "x": 7.724214987314268, + "y": 4.6042308479007135, + "heading": -3.3123412474297145, + "angularVelocity": 0.3593595969527817, + "velocityX": -1.29863151366687, + "velocityY": 2.0535983687570325, + "timestamp": 0.35277896648018725 + }, + { + "x": 7.635090364852527, + "y": 4.745073402195538, + "heading": -3.2877790375536318, + "angularVelocity": 0.4177495634926825, + "velocityX": -1.5158152429149414, + "velocityY": 2.3954243480000983, + "timestamp": 0.4115754608935518 + }, + { + "x": 7.533162778967564, + "y": 4.9059943073690455, + "heading": -3.259850978018229, + "angularVelocity": 0.47499531756189717, + "velocityX": -1.73356569812425, + "velocityY": 2.736913259524701, + "timestamp": 0.47037195530691633 + }, + { + "x": 7.4183659938859625, + "y": 5.086953873899936, + "heading": -3.228691402285513, + "angularVelocity": 0.5299563527316968, + "velocityX": -1.9524426792272918, + "velocityY": 3.07772713894586, + "timestamp": 0.5291684497202809 + }, + { + "x": 7.290504654847313, + "y": 5.287832710065626, + "heading": -3.19470019151555, + "angularVelocity": 0.5781162824264673, + "velocityX": -2.174642218288233, + "velocityY": 3.4165104258329766, + "timestamp": 0.5879649441336454 + }, + { + "x": 7.14174455003787, + "y": 5.492134292200882, + "heading": -3.1947001763002003, + "angularVelocity": 2.587798737144401e-7, + "velocityX": -2.5300845959216063, + "velocityY": 3.4747238619181893, + "timestamp": 0.64676143854701 + }, + { + "x": 6.97439432144165, + "y": 5.681508541107178, + "heading": -3.194700163553092, + "angularVelocity": 2.16800485848848e-7, + "velocityX": -2.8462620138485737, + "velocityY": 3.2208425144348545, + "timestamp": 0.7055579329603745 + }, + { + "x": 6.745847699273536, + "y": 5.886976114675493, + "heading": -3.194700153288452, + "angularVelocity": 1.4356027628687907e-7, + "velocityX": -3.196431195632974, + "velocityY": 2.8736498295812707, + "timestamp": 0.7770584934856117 + }, + { + "x": 6.495272369615725, + "y": 6.064915674656995, + "heading": -3.194700143450967, + "angularVelocity": 1.3758612071046523e-7, + "velocityX": -3.5045225913909417, + "velocityY": 2.4886456647943835, + "timestamp": 0.848559054010849 + }, + { + "x": 6.259944286847746, + "y": 6.219821063005782, + "heading": -3.1557407815558105, + "angularVelocity": 0.5448819087425905, + "velocityX": -3.2912760548906967, + "velocityY": 2.1664919437115477, + "timestamp": 0.9200596145360862 + }, + { + "x": 6.054032202615157, + "y": 6.3553633517659796, + "heading": -3.121653268018477, + "angularVelocity": 0.47674470363489235, + "velocityX": -2.8798667132114026, + "velocityY": 1.8956814850752688, + "timestamp": 0.9915601750613234 + }, + { + "x": 5.877536132289455, + "y": 6.471542504568667, + "heading": -3.092436592251897, + "angularVelocity": 0.4086216325012881, + "velocityX": -2.468457156547779, + "velocityY": 1.624870517786765, + "timestamp": 1.0630607355865607 + }, + { + "x": 5.73045607813, + "y": 6.568358496250266, + "heading": -3.068090035157399, + "angularVelocity": 0.3405086186129201, + "velocityX": -2.057047568285011, + "velocityY": 1.3540591985628594, + "timestamp": 1.134561296111798 + }, + { + "x": 5.612792038739406, + "y": 6.645811307792749, + "heading": -3.048613117858015, + "angularVelocity": 0.27240230225201295, + "velocityX": -1.6456379995659083, + "velocityY": 1.0832476133546296, + "timestamp": 1.2060618566370351 + }, + { + "x": 5.524544011657242, + "y": 6.703900925363133, + "heading": -3.0340055479534027, + "angularVelocity": 0.20430007537431713, + "velocityX": -1.2342284652581796, + "velocityY": 0.8124358346796499, + "timestamp": 1.2775624171622724 + }, + { + "x": 5.465711994604923, + "y": 6.742627339952768, + "heading": -3.0242671752077235, + "angularVelocity": 0.13619994968069762, + "velocityX": -0.8228189628185227, + "velocityY": 0.5416239300105247, + "timestamp": 1.3490629776875096 + }, + { + "x": 5.436295986175537, + "y": 6.761990547180176, + "heading": -3.019397960613997, + "angularVelocity": 0.06810036953497778, + "velocityX": -0.41140948005578704, + "velocityY": 0.2708119640624844, + "timestamp": 1.4205635382127468 + }, + { + "x": 5.436295986175537, + "y": 6.761990547180176, + "heading": -3.019397960613997, + "angularVelocity": 3.952492189308371e-23, + "velocityX": -1.106885338110057e-22, + "velocityY": -1.8566252974507848e-22, + "timestamp": 1.492064098737984 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/A123.traj b/src/main/deploy/choreo/A123.traj new file mode 100644 index 0000000..33df8b5 --- /dev/null +++ b/src/main/deploy/choreo/A123.traj @@ -0,0 +1,1075 @@ +{ + "samples": [ + { + "x": 1.775368571281433, + "y": 6.0120086669921875, + "heading": -2.6482728621413125, + "angularVelocity": -8.923254857024236e-25, + "velocityX": -9.616936178650736e-23, + "velocityY": -9.058247523111096e-23, + "timestamp": 0 + }, + { + "x": 1.8190963321489977, + "y": 6.053194106989434, + "heading": -2.644950811196117, + "angularVelocity": 0.035592499333761476, + "velocityX": 0.468499828937473, + "velocityY": 0.4412613683070305, + "timestamp": 0.09333570295369455 + }, + { + "x": 1.906551852113573, + "y": 6.1355649856042165, + "heading": -2.6383066719214034, + "angularVelocity": 0.07118539920366639, + "velocityX": 0.9369996389052054, + "velocityY": 0.8825227218318404, + "timestamp": 0.1866714059073891 + }, + { + "x": 2.0377351280171383, + "y": 6.259121300336279, + "heading": -2.6283403806935786, + "angularVelocity": 0.10677898073762115, + "velocityX": 1.4054994150378615, + "velocityY": 1.323784048568862, + "timestamp": 0.28000710886108365 + }, + { + "x": 2.2126461527991568, + "y": 6.42386304535477, + "heading": -2.615051832718829, + "angularVelocity": 0.1423736850339289, + "velocityX": 1.8739991155238322, + "velocityY": 1.7650453128340622, + "timestamp": 0.3733428118147782 + }, + { + "x": 2.4312848931630335, + "y": 6.629790191733643, + "heading": -2.5984407382107015, + "angularVelocity": 0.1779714941062607, + "velocityX": 2.342498459269624, + "velocityY": 2.206306267185214, + "timestamp": 0.4666785147684727 + }, + { + "x": 2.606195913698415, + "y": 6.794531937816056, + "heading": -2.585151593446195, + "angularVelocity": 0.14238007904755712, + "velocityX": 1.8739990700253055, + "velocityY": 1.7650453242329325, + "timestamp": 0.5600142177221673 + }, + { + "x": 2.7373791849088662, + "y": 6.918088253515589, + "heading": -2.57518466420097, + "angularVelocity": 0.1067858164647894, + "velocityX": 1.405499364755771, + "velocityY": 1.3237840589343497, + "timestamp": 0.6533499206758618 + }, + { + "x": 2.8248347011284207, + "y": 7.000459132839087, + "heading": -2.5685400217250343, + "angularVelocity": 0.07119079050845539, + "velocityX": 0.9369995987810015, + "velocityY": 0.8825227294250255, + "timestamp": 0.7466856236295564 + }, + { + "x": 2.8685624599456787, + "y": 7.04164457321167, + "heading": -2.5652176967040865, + "angularVelocity": 0.03559543578512434, + "velocityX": 0.468499806970458, + "velocityY": 0.4412613723284018, + "timestamp": 0.8400213265832509 + }, + { + "x": 2.8685624599456787, + "y": 7.04164457321167, + "heading": -2.5652176967040865, + "angularVelocity": 8.96777028872441e-25, + "velocityX": 9.964414836663175e-23, + "velocityY": 4.965750215054413e-23, + "timestamp": 0.9333570295369454 + }, + { + "x": 2.9198374533140026, + "y": 7.045992082496191, + "heading": -2.5770293802157602, + "angularVelocity": -0.13652275169185538, + "velocityX": 0.5926507580995501, + "velocityY": 0.050249731965998315, + "timestamp": 1.0198750887270611 + }, + { + "x": 3.022404200010661, + "y": 7.054688522110406, + "heading": -2.6000376852804123, + "angularVelocity": -0.26593644471489375, + "velocityX": 1.1854952325187602, + "velocityY": 0.10051588877075252, + "timestamp": 1.106393147917177 + }, + { + "x": 3.176284936967033, + "y": 7.067735777481485, + "heading": -2.633369012545948, + "angularVelocity": -0.3852528313457996, + "velocityX": 1.7785967276292372, + "velocityY": 0.15080383787167673, + "timestamp": 1.1929112071072927 + }, + { + "x": 3.381510228293591, + "y": 7.085136440082294, + "heading": -2.675682452707487, + "angularVelocity": -0.4890706120505913, + "velocityX": 2.3720514913030337, + "velocityY": 0.20112173994300755, + "timestamp": 1.2794292662974085 + }, + { + "x": 3.6381227114829775, + "y": 7.106894125064837, + "heading": -2.7246679320884843, + "angularVelocity": -0.5661879131310169, + "velocityX": 2.965999071078366, + "velocityY": 0.2514814269554226, + "timestamp": 1.3659473254875243 + }, + { + "x": 3.9461710841169118, + "y": 7.133012961404564, + "heading": -2.775445026128338, + "angularVelocity": -0.5868958979798145, + "velocityX": 3.5605095111649008, + "velocityY": 0.3018888378244019, + "timestamp": 1.45246538467764 + }, + { + "x": 4.305378922938188, + "y": 7.163469511698595, + "heading": -2.810773174167225, + "angularVelocity": -0.40833264603470154, + "velocityX": 4.151824973696521, + "velocityY": 0.3520253526157542, + "timestamp": 1.5389834438677559 + }, + { + "x": 4.675926456784465, + "y": 7.194887533192645, + "heading": -2.810773196454242, + "angularVelocity": -2.5759959514533893e-7, + "velocityX": 4.282892350047188, + "velocityY": 0.3631383064778689, + "timestamp": 1.6255015030578717 + }, + { + "x": 5.046473990630761, + "y": 7.226305554686697, + "heading": -2.810773218741196, + "angularVelocity": -2.5759886662698844e-7, + "velocityX": 4.28289235004742, + "velocityY": 0.3631383064778886, + "timestamp": 1.7120195622479875 + }, + { + "x": 5.4170215244770565, + "y": 7.25772357618075, + "heading": -2.8107732410281496, + "angularVelocity": -2.575988646921058e-7, + "velocityX": 4.282892350047419, + "velocityY": 0.3631383064778885, + "timestamp": 1.7985376214381033 + }, + { + "x": 5.787569058323353, + "y": 7.289141597674803, + "heading": -2.8107732633151037, + "angularVelocity": -2.575988664941198e-7, + "velocityX": 4.28289235004742, + "velocityY": 0.3631383064778886, + "timestamp": 1.885055680628219 + }, + { + "x": 6.158116592169649, + "y": 7.320559619168855, + "heading": -2.810773285602058, + "angularVelocity": -2.575988666257598e-7, + "velocityX": 4.28289235004742, + "velocityY": 0.36313830647788864, + "timestamp": 1.9715737398183348 + }, + { + "x": 6.528664126015926, + "y": 7.351977640662906, + "heading": -2.810773307889075, + "angularVelocity": -2.575995940422885e-7, + "velocityX": 4.28289235004719, + "velocityY": 0.36313830647786904, + "timestamp": 2.0580917990084506 + }, + { + "x": 6.888011462286063, + "y": 7.382446018680633, + "heading": -2.8464199451568986, + "angularVelocity": -0.41201383389210466, + "velocityX": 4.153437324345239, + "velocityY": 0.3521620607643999, + "timestamp": 2.1446098581985664 + }, + { + "x": 7.1961183473382455, + "y": 7.408569816177152, + "heading": -2.8950909993625498, + "angularVelocity": -0.5625536987451474, + "velocityX": 3.56118581411015, + "velocityY": 0.3019461802664086, + "timestamp": 2.231127917388682 + }, + { + "x": 7.452790863790213, + "y": 7.430332591265989, + "heading": -2.941640853640219, + "angularVelocity": -0.5380362748935424, + "velocityX": 2.966692952369079, + "velocityY": 0.251540259831949, + "timestamp": 2.317645976578798 + }, + { + "x": 7.658073050822811, + "y": 7.4477380779456315, + "heading": -2.9817184792555684, + "angularVelocity": -0.46322844028762317, + "velocityX": 2.3727091078350337, + "velocityY": 0.2011774979995321, + "timestamp": 2.404164035768914 + }, + { + "x": 7.812001701866268, + "y": 7.460789395861135, + "heading": -3.0132404006110005, + "angularVelocity": -0.3643392102239029, + "velocityX": 1.7791505320896364, + "velocityY": 0.15085079389985404, + "timestamp": 2.4906820949590296 + }, + { + "x": 7.914603145410721, + "y": 7.469488777355164, + "heading": -3.034982434060706, + "angularVelocity": -0.2513005221479756, + "velocityX": 1.18589626841948, + "velocityY": 0.10054989184295737, + "timestamp": 2.5772001541491454 + }, + { + "x": 7.965896606445312, + "y": 7.473837852478027, + "heading": -3.046138770984786, + "angularVelocity": -0.12894807198073027, + "velocityX": 0.5928642125672009, + "velocityY": 0.05026783036482732, + "timestamp": 2.663718213339261 + }, + { + "x": 7.965896606445312, + "y": 7.473837852478027, + "heading": -3.046138770984786, + "angularVelocity": -1.6061739574692185e-27, + "velocityX": 2.405650666496034e-19, + "velocityY": 2.5667630572731595e-18, + "timestamp": 2.750236272529377 + }, + { + "x": 7.905970693449125, + "y": 7.456889105738656, + "heading": -3.045547301520618, + "angularVelocity": 0.006224276082072812, + "velocityX": -0.6306249934360915, + "velocityY": -0.17835862262033683, + "timestamp": 2.845262498289425 + }, + { + "x": 7.786118868818083, + "y": 7.422991612644937, + "heading": -3.044364362729114, + "angularVelocity": 0.012448550724206106, + "velocityX": -1.261249972546328, + "velocityY": -0.3567172411889158, + "timestamp": 2.9402887240494726 + }, + { + "x": 7.606341134594192, + "y": 7.372145373774407, + "heading": -3.0425899548091673, + "angularVelocity": 0.018672823273306667, + "velocityX": -1.8918749301677051, + "velocityY": -0.5350758536798363, + "timestamp": 3.0353149498095204 + }, + { + "x": 7.366637494180796, + "y": 7.304350390089628, + "heading": -3.0402240780931336, + "angularVelocity": 0.0248970923248922, + "velocityX": -2.522499851974286, + "velocityY": -0.7134344560413174, + "timestamp": 3.1303411755695683 + }, + { + "x": 7.067007954384597, + "y": 7.219606663515728, + "heading": -3.03726673324464, + "angularVelocity": 0.031121354392853406, + "velocityX": -3.153124702151143, + "velocityY": -0.8917930381438821, + "timestamp": 3.225367401329616 + }, + { + "x": 6.7074525356259445, + "y": 7.11791419982816, + "heading": -3.033717922068974, + "angularVelocity": 0.03734559746303509, + "velocityX": -3.7837493374363023, + "velocityY": -1.0701515594689839, + "timestamp": 3.320393627089664 + }, + { + "x": 6.347897116868929, + "y": 7.016221736141056, + "heading": -3.030169108902523, + "angularVelocity": 0.03734561841288037, + "velocityX": -3.7837493374190543, + "velocityY": -1.0701515594641053, + "timestamp": 3.4154198528497117 + }, + { + "x": 6.048267577074264, + "y": 6.9314780095675905, + "heading": -3.0272117621899937, + "angularVelocity": 0.031121374008861453, + "velocityX": -3.153124702135004, + "velocityY": -0.8917930381393175, + "timestamp": 3.5104460786097595 + }, + { + "x": 5.8085639366622, + "y": 6.863683025883188, + "heading": -3.0248458838523082, + "angularVelocity": 0.02489710939019601, + "velocityX": -2.5224998519602666, + "velocityY": -0.7134344560373521, + "timestamp": 3.6054723043698074 + }, + { + "x": 5.628786202439365, + "y": 6.812836787012956, + "heading": -3.0230714746436687, + "angularVelocity": 0.01867283683475372, + "velocityX": -1.8918749301565887, + "velocityY": -0.5350758536766922, + "timestamp": 3.700498530129855 + }, + { + "x": 5.508934377809051, + "y": 6.778939293919443, + "heading": -3.0218885349617404, + "angularVelocity": 0.012448560094509599, + "velocityX": -1.2612499725386697, + "velocityY": -0.35671724118674986, + "timestamp": 3.795524755889903 + }, + { + "x": 5.449008464813232, + "y": 6.761990547180176, + "heading": -3.0212970650452653, + "angularVelocity": 0.006224280841886529, + "velocityX": -0.6306249934322161, + "velocityY": -0.1783586226192408, + "timestamp": 3.890550981649951 + }, + { + "x": 5.449008464813232, + "y": 6.761990547180176, + "heading": -3.0212970650452653, + "angularVelocity": 2.0281055022755585e-29, + "velocityX": 1.1430879114890998e-18, + "velocityY": -1.6176856268439436e-18, + "timestamp": 3.9855772074099987 + }, + { + "x": 5.49985252708244, + "y": 6.742794318695081, + "heading": -3.0259671249570506, + "angularVelocity": -0.052596426193898003, + "velocityX": 0.572629906051452, + "velocityY": -0.2161970154107702, + "timestamp": 4.074367649075542 + }, + { + "x": 5.601543140419982, + "y": 6.704400922076198, + "heading": -3.0350465824550086, + "angularVelocity": -0.10225714984230815, + "velocityX": 1.1452878421372228, + "velocityY": -0.4324046135900921, + "timestamp": 4.163158090741086 + }, + { + "x": 5.75408366076328, + "y": 6.64680909028587, + "heading": -3.048157311029397, + "angularVelocity": -0.14765923367938932, + "velocityX": 1.7179835743794172, + "velocityY": -0.6486264817475036, + "timestamp": 4.2519485324066295 + }, + { + "x": 5.957478781668101, + "y": 6.570017051267244, + "heading": -3.064701200025727, + "angularVelocity": -0.1863251120953702, + "velocityX": 2.2907321676692405, + "velocityY": -0.8648683076483163, + "timestamp": 4.340738974072173 + }, + { + "x": 6.211735113142484, + "y": 6.4740223094049245, + "heading": -3.0835897723567736, + "angularVelocity": -0.2127320461159122, + "velocityX": 2.863555205999725, + "velocityY": -1.0811382403515035, + "timestamp": 4.429529415737717 + }, + { + "x": 6.516858357819224, + "y": 6.358822711663981, + "heading": -3.102223690599022, + "angularVelocity": -0.2098640111785739, + "velocityX": 3.436442470081179, + "velocityY": -1.2974324215536346, + "timestamp": 4.51831985740326 + }, + { + "x": 6.872622299397898, + "y": 6.224503666243447, + "heading": -3.1061843374394424, + "angularVelocity": -0.044606680247623424, + "velocityX": 4.006781979064447, + "velocityY": -1.512764695173915, + "timestamp": 4.607110299068804 + }, + { + "x": 7.177745788997514, + "y": 6.109303976031618, + "heading": -3.124829178589315, + "angularVelocity": -0.20998703013669084, + "velocityX": 3.4364452285185845, + "velocityY": -1.2974334630045405, + "timestamp": 4.695900740734348 + }, + { + "x": 7.43200235076056, + "y": 6.013309147223575, + "heading": -3.1436995015656892, + "angularVelocity": -0.21252651324176217, + "velocityX": 2.8635577996196915, + "velocityY": -1.081139219575414, + "timestamp": 4.784691182399891 + }, + { + "x": 7.635397748239775, + "y": 5.936517003784, + "heading": -3.160215730785825, + "angularVelocity": -0.18601359459782157, + "velocityX": 2.2907352825810445, + "velocityY": -0.8648694836865012, + "timestamp": 4.873481624065435 + }, + { + "x": 7.787938540074382, + "y": 5.878925069491846, + "heading": -3.1732992809366647, + "angularVelocity": -0.1473531374032643, + "velocityX": 1.7179866320430934, + "velocityY": -0.6486276361715984, + "timestamp": 4.9622720657309785 + }, + { + "x": 7.889629370431644, + "y": 5.840531590936942, + "heading": -3.1823575431703564, + "angularVelocity": -0.10201843873929947, + "velocityX": 1.145290286316077, + "velocityY": -0.43240553639235546, + "timestamp": 5.051062507396522 + }, + { + "x": 7.940473556518555, + "y": 5.821335315704346, + "heading": -3.1870158092601475, + "angularVelocity": -0.052463598585734586, + "velocityX": 0.5726313005450608, + "velocityY": -0.2161975419032796, + "timestamp": 5.139852949062066 + }, + { + "x": 7.940473556518555, + "y": 5.821335315704346, + "heading": -3.1870158092601475, + "angularVelocity": 1.5550002280971524e-27, + "velocityX": -4.5599517029668704e-20, + "velocityY": -1.2345517643295343e-19, + "timestamp": 5.228643390727609 + }, + { + "x": 7.889367117787343, + "y": 5.841051497608848, + "heading": -3.182401245414936, + "angularVelocity": 0.05176721882064578, + "velocityX": -0.5733235654953367, + "velocityY": 0.22118057896568025, + "timestamp": 5.317784046732316 + }, + { + "x": 7.78715191484382, + "y": 5.880484758557419, + "heading": -3.1734212335230674, + "angularVelocity": 0.10073980038237511, + "velocityX": -1.146673218762545, + "velocityY": 0.4423712222455345, + "timestamp": 5.406924702737023 + }, + { + "x": 7.63382478470437, + "y": 5.93963631878692, + "heading": -3.160439385829673, + "angularVelocity": 0.14563329770322905, + "velocityX": -1.7200583550939212, + "velocityY": 0.6635755544179243, + "timestamp": 5.49606535874173 + }, + { + "x": 7.429381246451849, + "y": 6.018507906975374, + "heading": -3.1440362842645486, + "angularVelocity": 0.18401369588595168, + "velocityX": -2.293493759365243, + "velocityY": 0.8847992792905789, + "timestamp": 5.585206014746437 + }, + { + "x": 7.173814855056923, + "y": 6.117102009529074, + "heading": -3.1252866571110136, + "angularVelocity": 0.21033754959740159, + "velocityX": -2.867001465430438, + "velocityY": 1.106050897230241, + "timestamp": 5.674346670751144 + }, + { + "x": 6.867120001482762, + "y": 6.235420790339557, + "heading": -3.106859923030452, + "angularVelocity": 0.20671526222096004, + "velocityX": -3.440572094936845, + "velocityY": 1.3273267901936336, + "timestamp": 5.763487326755851 + }, + { + "x": 6.509649519466585, + "y": 6.373328130860556, + "heading": -3.106859612143933, + "angularVelocity": 0.0000034875951447068007, + "velocityX": -4.01018455593707, + "velocityY": 1.5470756745801426, + "timestamp": 5.852627982760558 + }, + { + "x": 6.202954670903185, + "y": 6.491646909737954, + "heading": -3.0884387572905028, + "angularVelocity": 0.20664930772393342, + "velocityX": -3.4405720387249987, + "velocityY": 1.3273267685078536, + "timestamp": 5.941768638765265 + }, + { + "x": 5.947388281937679, + "y": 6.590241011354414, + "heading": -3.069691188796424, + "angularVelocity": 0.2103144550909352, + "velocityX": -2.867001438176646, + "velocityY": 1.1060508867160919, + "timestamp": 6.030909294769972 + }, + { + "x": 5.742944749611789, + "y": 6.669112597256453, + "heading": -3.0532893829542354, + "angularVelocity": 0.18399916017358833, + "velocityX": -2.2934936928789664, + "velocityY": 0.8847992536410609, + "timestamp": 6.120049950774679 + }, + { + "x": 5.589617624363911, + "y": 6.728264155598851, + "heading": -3.040308767682206, + "angularVelocity": 0.14561947212217205, + "velocityX": -1.720058300219161, + "velocityY": 0.6635755332479741, + "timestamp": 6.209190606779386 + }, + { + "x": 5.487402424003547, + "y": 6.767697415550875, + "heading": -3.031329850849583, + "angularVelocity": 0.10072751576057763, + "velocityX": -1.1466731897840863, + "velocityY": 0.44237121106603183, + "timestamp": 6.298331262784093 + }, + { + "x": 5.436295986175537, + "y": 6.787413597106934, + "heading": -3.0267159569026636, + "angularVelocity": 0.05175970375039749, + "velocityX": -0.5733235553630177, + "velocityY": 0.2211805750567668, + "timestamp": 6.3874719187888 + }, + { + "x": 5.436295986175537, + "y": 6.787413597106934, + "heading": -3.0267159569026636, + "angularVelocity": 6.854677049309127e-18, + "velocityX": -8.559616674403826e-18, + "velocityY": -7.1969388527604e-18, + "timestamp": 6.476612574793507 + }, + { + "x": 5.460898661874906, + "y": 6.770891554233058, + "heading": -3.031032530385192, + "angularVelocity": -0.06580682525696466, + "velocityX": 0.3750715670995127, + "velocityY": -0.25188107944466487, + "timestamp": 6.542207191547938 + }, + { + "x": 5.510104016852139, + "y": 6.737847475934615, + "heading": -3.039665750319823, + "angularVelocity": -0.13161476294628496, + "velocityX": 0.7501431887537074, + "velocityY": -0.5037620453235327, + "timestamp": 6.607801808302368 + }, + { + "x": 5.583912055468166, + "y": 6.688281373798517, + "heading": -3.052615800766114, + "angularVelocity": -0.197425506651816, + "velocityX": 1.1252148768909827, + "velocityY": -0.7556428345580732, + "timestamp": 6.673396425056799 + }, + { + "x": 5.682322782278138, + "y": 6.622193263600659, + "heading": -3.0698830093365843, + "angularVelocity": -0.26324124486047346, + "velocityX": 1.5002866344722108, + "velocityY": -1.007523383286727, + "timestamp": 6.7389910418112295 + }, + { + "x": 5.805336201198385, + "y": 6.5395831653172145, + "heading": -3.091467888045328, + "angularVelocity": -0.32906478880259576, + "velocityX": 1.8753584517584796, + "velocityY": -1.2594036274755147, + "timestamp": 6.80458565856566 + }, + { + "x": 5.952952314377682, + "y": 6.440451103068376, + "heading": -3.117371178377928, + "angularVelocity": -0.3948996368062194, + "velocityX": 2.250430301803197, + "velocityY": -1.5112835039482873, + "timestamp": 6.870180275320091 + }, + { + "x": 6.125171120641939, + "y": 6.324797105039988, + "heading": -3.1475938920598048, + "angularVelocity": -0.46074990871916927, + "velocityX": 2.625502133948695, + "velocityY": -1.7631629507235327, + "timestamp": 6.935774892074521 + }, + { + "x": 6.321992612980133, + "y": 6.1926212038165795, + "heading": -3.182137324335163, + "angularVelocity": -0.5266199268288194, + "velocityX": 3.000573859210087, + "velocityY": -2.0150419007426175, + "timestamp": 7.001369508828952 + }, + { + "x": 6.5434167711211595, + "y": 6.0439234407720175, + "heading": -3.2210029154552626, + "angularVelocity": -0.5925119017899734, + "velocityX": 3.3756452754353816, + "velocityY": -2.2669202200107508, + "timestamp": 7.066964125583382 + }, + { + "x": 6.768454422425858, + "y": 5.874071349001229, + "heading": -3.221002927063026, + "angularVelocity": -1.7696213344774776e-7, + "velocityX": 3.4307335333175626, + "velocityY": -2.589421208250734, + "timestamp": 7.132558742337813 + }, + { + "x": 6.97439432144165, + "y": 5.681508541107178, + "heading": -3.221002939203244, + "angularVelocity": -1.8507949182524416e-7, + "velocityX": 3.1395853685157755, + "velocityY": -2.935649561230799, + "timestamp": 7.1981533590922435 + }, + { + "x": 7.15472976461684, + "y": 5.474395114221533, + "heading": -3.221002948946015, + "angularVelocity": -1.5248985870081709e-7, + "velocityX": 2.82253640171159, + "velocityY": -3.2416544211944602, + "timestamp": 7.262044631873001 + }, + { + "x": 7.312913869889132, + "y": 5.249907624667417, + "heading": -3.2210029583127433, + "angularVelocity": -1.4660418880845298e-7, + "velocityX": 2.4758327450338733, + "velocityY": -3.5135861250456206, + "timestamp": 7.325935904653759 + }, + { + "x": 7.448597158749216, + "y": 5.036236251986827, + "heading": -3.2541109060242546, + "angularVelocity": -0.5181920201467125, + "velocityX": 2.123659194042101, + "velocityY": -3.344296699392882, + "timestamp": 7.389827177434517 + }, + { + "x": 7.569203088849137, + "y": 4.846305269940681, + "heading": -3.28354394578579, + "angularVelocity": -0.46067386797572535, + "velocityX": 1.8876745578959737, + "velocityY": -2.972721841024385, + "timestamp": 7.453718450215275 + }, + { + "x": 7.674732721316318, + "y": 4.680115323289921, + "heading": -3.3092993426884116, + "angularVelocity": -0.40311291013356876, + "velocityX": 1.6517065300803815, + "velocityY": -2.601136891122462, + "timestamp": 7.517609722996033 + }, + { + "x": 7.765186402550063, + "y": 4.537666622961828, + "heading": -3.33137623392435, + "angularVelocity": -0.3455384479786788, + "velocityX": 1.4157439239644065, + "velocityY": -2.229548639873397, + "timestamp": 7.581500995776791 + }, + { + "x": 7.840564301968412, + "y": 4.418959271788617, + "heading": -3.3497741734870097, + "angularVelocity": -0.28795700511180566, + "velocityX": 1.1797839695099621, + "velocityY": -1.857958779136701, + "timestamp": 7.645392268557549 + }, + { + "x": 7.900866519469214, + "y": 4.323993329938929, + "heading": -3.364492865366424, + "angularVelocity": -0.23037092921851174, + "velocityX": 0.943825578615652, + "velocityY": -1.486367976665167, + "timestamp": 7.709283541338307 + }, + { + "x": 7.946093121157217, + "y": 4.252768836889262, + "heading": -3.3755320899639822, + "angularVelocity": -0.17278141625794857, + "velocityX": 0.7078682223656876, + "velocityY": -1.1147765563237297, + "timestamp": 7.773174814119065 + }, + { + "x": 7.976244154590328, + "y": 4.205285820920615, + "heading": -3.382891681018417, + "angularVelocity": -0.115189301044308, + "velocityX": 0.47191161047254004, + "velocityY": -0.7431846933397034, + "timestamp": 7.8370660868998225 + }, + { + "x": 7.99131965637207, + "y": 4.181544303894043, + "heading": -3.386571517316088, + "angularVelocity": -0.05759528864455178, + "velocityX": 0.2359555714827114, + "velocityY": -0.37159248819540497, + "timestamp": 7.90095735968058 + }, + { + "x": 7.99131965637207, + "y": 4.181544303894043, + "heading": -3.386571517316088, + "angularVelocity": -4.708903555929205e-19, + "velocityX": 3.4384821500622114e-19, + "velocityY": -1.405030037847007e-18, + "timestamp": 7.964848632461338 + }, + { + "x": 7.978610314810739, + "y": 4.201678078384227, + "heading": -3.383016641435549, + "angularVelocity": 0.06046067739254296, + "velocityX": -0.21615815174249978, + "velocityY": 0.3424315461502773, + "timestamp": 8.023645126874703 + }, + { + "x": 7.953187149381424, + "y": 4.241942992732508, + "heading": -3.3759159768404734, + "angularVelocity": 0.1207668019313348, + "velocityX": -0.4323925377349786, + "velocityY": 0.684818282960888, + "timestamp": 8.082441621288067 + }, + { + "x": 7.915044562001454, + "y": 4.302335753884612, + "heading": -3.365280870024358, + "angularVelocity": 0.18087994738845797, + "velocityX": -0.6487221348914206, + "velocityY": 1.0271490121080764, + "timestamp": 8.141238115701432 + }, + { + "x": 7.864175361270239, + "y": 4.382852126965025, + "heading": -3.3511258737287886, + "angularVelocity": 0.24074558248411781, + "velocityX": -0.8651740420710002, + "velocityY": 1.3694077152686683, + "timestamp": 8.200034610114797 + }, + { + "x": 7.8005699678526, + "y": 4.483486462884797, + "heading": -3.333470331964337, + "angularVelocity": 0.3002822181935773, + "velocityX": -1.0817888728274418, + "velocityY": 1.711570339759891, + "timestamp": 8.258831104528161 + }, + { + "x": 7.724214987314268, + "y": 4.6042308479007135, + "heading": -3.3123412474297145, + "angularVelocity": 0.3593595969527817, + "velocityX": -1.29863151366687, + "velocityY": 2.0535983687570325, + "timestamp": 8.317627598941526 + }, + { + "x": 7.635090364852527, + "y": 4.745073402195538, + "heading": -3.2877790375536318, + "angularVelocity": 0.4177495634926825, + "velocityX": -1.5158152429149414, + "velocityY": 2.3954243480000983, + "timestamp": 8.37642409335489 + }, + { + "x": 7.533162778967564, + "y": 4.9059943073690455, + "heading": -3.259850978018229, + "angularVelocity": 0.47499531756189717, + "velocityX": -1.73356569812425, + "velocityY": 2.736913259524701, + "timestamp": 8.435220587768255 + }, + { + "x": 7.4183659938859625, + "y": 5.086953873899936, + "heading": -3.228691402285513, + "angularVelocity": 0.5299563527316968, + "velocityX": -1.9524426792272918, + "velocityY": 3.07772713894586, + "timestamp": 8.49401708218162 + }, + { + "x": 7.290504654847313, + "y": 5.287832710065626, + "heading": -3.19470019151555, + "angularVelocity": 0.5781162824264673, + "velocityX": -2.174642218288233, + "velocityY": 3.4165104258329766, + "timestamp": 8.552813576594984 + }, + { + "x": 7.14174455003787, + "y": 5.492134292200882, + "heading": -3.1947001763002003, + "angularVelocity": 2.587798737144401e-7, + "velocityX": -2.5300845959216063, + "velocityY": 3.4747238619181893, + "timestamp": 8.611610071008348 + }, + { + "x": 6.97439432144165, + "y": 5.681508541107178, + "heading": -3.194700163553092, + "angularVelocity": 2.16800485848848e-7, + "velocityX": -2.8462620138485737, + "velocityY": 3.2208425144348545, + "timestamp": 8.670406565421713 + }, + { + "x": 6.745847699273536, + "y": 5.886976114675493, + "heading": -3.194700153288452, + "angularVelocity": 1.4356027628687907e-7, + "velocityX": -3.196431195632974, + "velocityY": 2.8736498295812707, + "timestamp": 8.74190712594695 + }, + { + "x": 6.495272369615725, + "y": 6.064915674656995, + "heading": -3.194700143450967, + "angularVelocity": 1.3758612071046523e-7, + "velocityX": -3.5045225913909417, + "velocityY": 2.4886456647943835, + "timestamp": 8.813407686472187 + }, + { + "x": 6.259944286847746, + "y": 6.219821063005782, + "heading": -3.1557407815558105, + "angularVelocity": 0.5448819087425905, + "velocityX": -3.2912760548906967, + "velocityY": 2.1664919437115477, + "timestamp": 8.884908246997425 + }, + { + "x": 6.054032202615157, + "y": 6.3553633517659796, + "heading": -3.121653268018477, + "angularVelocity": 0.47674470363489235, + "velocityX": -2.8798667132114026, + "velocityY": 1.8956814850752688, + "timestamp": 8.956408807522662 + }, + { + "x": 5.877536132289455, + "y": 6.471542504568667, + "heading": -3.092436592251897, + "angularVelocity": 0.4086216325012881, + "velocityX": -2.468457156547779, + "velocityY": 1.624870517786765, + "timestamp": 9.027909368047899 + }, + { + "x": 5.73045607813, + "y": 6.568358496250266, + "heading": -3.068090035157399, + "angularVelocity": 0.3405086186129201, + "velocityX": -2.057047568285011, + "velocityY": 1.3540591985628594, + "timestamp": 9.099409928573136 + }, + { + "x": 5.612792038739406, + "y": 6.645811307792749, + "heading": -3.048613117858015, + "angularVelocity": 0.27240230225201295, + "velocityX": -1.6456379995659083, + "velocityY": 1.0832476133546296, + "timestamp": 9.170910489098373 + }, + { + "x": 5.524544011657242, + "y": 6.703900925363133, + "heading": -3.0340055479534027, + "angularVelocity": 0.20430007537431713, + "velocityX": -1.2342284652581796, + "velocityY": 0.8124358346796499, + "timestamp": 9.24241104962361 + }, + { + "x": 5.465711994604923, + "y": 6.742627339952768, + "heading": -3.0242671752077235, + "angularVelocity": 0.13619994968069762, + "velocityX": -0.8228189628185227, + "velocityY": 0.5416239300105247, + "timestamp": 9.313911610148848 + }, + { + "x": 5.436295986175537, + "y": 6.761990547180176, + "heading": -3.019397960613997, + "angularVelocity": 0.06810036953497778, + "velocityX": -0.41140948005578704, + "velocityY": 0.2708119640624844, + "timestamp": 9.385412170674085 + }, + { + "x": 5.436295986175537, + "y": 6.761990547180176, + "heading": -3.019397960613997, + "angularVelocity": 3.952492189308371e-23, + "velocityX": -1.106885338110057e-22, + "velocityY": -1.8566252974507848e-22, + "timestamp": 9.456912731199322 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/ABC45.1.traj b/src/main/deploy/choreo/ABC45.1.traj new file mode 100644 index 0000000..82014c2 --- /dev/null +++ b/src/main/deploy/choreo/ABC45.1.traj @@ -0,0 +1,85 @@ +{ + "samples": [ + { + "x": 1.8084948062896729, + "y": 6.153295993804932, + "heading": 0.4048918189668609, + "angularVelocity": -4.1069892817306778e-25, + "velocityX": 2.8401756027512354e-22, + "velocityY": 1.242538955366428e-22, + "timestamp": 0 + }, + { + "x": 1.8839619227340805, + "y": 6.186312827447586, + "heading": 0.40399906423823373, + "angularVelocity": -0.009313836888129048, + "velocityX": 0.7873253318540663, + "velocityY": 0.34445452177336483, + "timestamp": 0.09585251914439309 + }, + { + "x": 2.0348961520152846, + "y": 6.252346493154345, + "heading": 0.40221355483146876, + "angularVelocity": -0.01862767324951869, + "velocityX": 1.5746506260710342, + "velocityY": 0.6889090270782012, + "timestamp": 0.19170503828878618 + }, + { + "x": 2.261297485715463, + "y": 6.351396987242034, + "heading": 0.399535290851602, + "angularVelocity": -0.027941508515099304, + "velocityX": 2.361975832467427, + "velocityY": 1.0333634939576128, + "timestamp": 0.2875575574331793 + }, + { + "x": 2.5631658817449776, + "y": 6.4834642912954665, + "heading": 0.3959642726544128, + "angularVelocity": -0.037255340068942634, + "velocityX": 3.1493005997555192, + "velocityY": 1.3778177687170161, + "timestamp": 0.38341007657757237 + }, + { + "x": 2.7895672154458406, + "y": 6.582514785381733, + "heading": 0.3932860087457274, + "angularVelocity": -0.027941507772485746, + "velocityX": 2.361975832474573, + "velocityY": 1.033363493942764, + "timestamp": 0.47926259572196545 + }, + { + "x": 2.9405014447276976, + "y": 6.64854845108714, + "heading": 0.3915004994042191, + "angularVelocity": -0.018627672568715576, + "velocityX": 1.5746506260778408, + "velocityY": 0.6889090270641033, + "timestamp": 0.5751151148663586 + }, + { + "x": 3.015968561172486, + "y": 6.681565284729004, + "heading": 0.3906077447120181, + "angularVelocity": -0.00931383650810643, + "velocityX": 0.7873253318580392, + "velocityY": 0.34445452176511515, + "timestamp": 0.6709676340107517 + }, + { + "x": 3.015968561172486, + "y": 6.681565284729004, + "heading": 0.3906077447120181, + "angularVelocity": 4.111882350756928e-25, + "velocityX": -2.840154314690188e-22, + "velocityY": -1.242602326089003e-22, + "timestamp": 0.7668201531551447 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/ABC45.2.traj b/src/main/deploy/choreo/ABC45.2.traj new file mode 100644 index 0000000..27177bd --- /dev/null +++ b/src/main/deploy/choreo/ABC45.2.traj @@ -0,0 +1,175 @@ +{ + "samples": [ + { + "x": 3.015968561172486, + "y": 6.681565284729004, + "heading": 0.3906077447120181, + "angularVelocity": 4.111882350756928e-25, + "velocityX": -2.840154314690188e-22, + "velocityY": -1.242602326089003e-22, + "timestamp": 0 + }, + { + "x": 2.983681753290401, + "y": 6.656482270532227, + "heading": 0.3885085466960412, + "angularVelocity": -0.031083324268668066, + "velocityX": -0.47807844298671504, + "velocityY": -0.3714101566312668, + "timestamp": 0.06753454031597472 + }, + { + "x": 2.91946323897797, + "y": 6.605867942187028, + "heading": 0.38409939745193045, + "angularVelocity": -0.06528732147256167, + "velocityX": -0.9508988143247918, + "velocityY": -0.7494583972644088, + "timestamp": 0.13506908063194945 + }, + { + "x": 2.8238901627601707, + "y": 6.529020599984131, + "heading": 0.3770464356050328, + "angularVelocity": -0.10443488345221376, + "velocityX": -1.415173269420963, + "velocityY": -1.1378968723759795, + "timestamp": 0.20260362094792417 + }, + { + "x": 2.698059890766503, + "y": 6.4246897450492995, + "heading": 0.36674191788660143, + "angularVelocity": -0.15258144454999606, + "velocityX": -1.8631987632542386, + "velocityY": -1.5448517817208405, + "timestamp": 0.270138161263899 + }, + { + "x": 2.544814664199375, + "y": 6.290062549484542, + "heading": 0.35175074410661816, + "angularVelocity": -0.22197787546704062, + "velocityX": -2.269138515641584, + "velocityY": -1.9934569027178601, + "timestamp": 0.33767270157987384 + }, + { + "x": 2.3816997436952163, + "y": 6.115936226118254, + "heading": 0.3258566979876119, + "angularVelocity": -0.38341929918905854, + "velocityX": -2.415281421047535, + "velocityY": -2.57832988203667, + "timestamp": 0.40520724189584867 + }, + { + "x": 2.259148112580847, + "y": 5.946533385595093, + "heading": 0.2939302799726612, + "angularVelocity": -0.4727420645135983, + "velocityX": -1.814651148004934, + "velocityY": -2.508388148206443, + "timestamp": 0.4727417822118235 + }, + { + "x": 2.174267210882179, + "y": 5.793024676618523, + "heading": 0.2601376621599889, + "angularVelocity": -0.5003753287512784, + "velocityX": -1.2568516984277258, + "velocityY": -2.273039962341445, + "timestamp": 0.5402763225277983 + }, + { + "x": 2.125764027427053, + "y": 5.658190290207638, + "heading": 0.22561638717429328, + "angularVelocity": -0.5111647288066291, + "velocityX": -0.7181981727897014, + "velocityY": -1.9965248268520615, + "timestamp": 0.6078108628437732 + }, + { + "x": 2.1129797017821503, + "y": 5.5432629827508135, + "heading": 0.19088640125278653, + "angularVelocity": -0.5142551612702936, + "velocityX": -0.1893005502827007, + "velocityY": -1.7017559743371586, + "timestamp": 0.675345403159748 + }, + { + "x": 2.135519027709961, + "y": 5.4489359855651855, + "heading": 0.15624465539833177, + "angularVelocity": -0.5129485696115788, + "velocityX": 0.3337451594747791, + "velocityY": -1.3967222808402766, + "timestamp": 0.7428799434757228 + }, + { + "x": 2.2257168252781243, + "y": 5.361309601565465, + "heading": 0.11119576514583607, + "angularVelocity": -0.5074966662099387, + "velocityX": 1.0161200710773546, + "velocityY": -0.9871519032464061, + "timestamp": 0.8316468128010478 + }, + { + "x": 2.3751569105405435, + "y": 5.312165927437898, + "heading": 0.06755158237683569, + "angularVelocity": -0.4916719841616492, + "velocityX": 1.6835119498777174, + "velocityY": -0.5536263078903791, + "timestamp": 0.9204136821263728 + }, + { + "x": 2.579774143297148, + "y": 5.307117553721901, + "heading": 0.027843482574793646, + "angularVelocity": -0.4473301819005732, + "velocityX": 2.305108136760973, + "velocityY": -0.056872274018082446, + "timestamp": 1.0091805514516978 + }, + { + "x": 2.780786706373995, + "y": 5.372145722523604, + "heading": 0.011713308423261262, + "angularVelocity": -0.18171390152801614, + "velocityX": 2.2644998590650816, + "velocityY": 0.7325725160293697, + "timestamp": 1.0979474207770226 + }, + { + "x": 2.912768986132865, + "y": 5.4224129115782524, + "heading": 0.0035448462930992987, + "angularVelocity": -0.09202151875183366, + "velocityX": 1.486841664711225, + "velocityY": 0.5662832252247418, + "timestamp": 1.1867142901023477 + }, + { + "x": 2.9782350063323975, + "y": 5.4489359855651855, + "heading": 1.7901422536582138e-29, + "angularVelocity": -0.039934339467438684, + "velocityX": 0.7375051153331075, + "velocityY": 0.29879474390076116, + "timestamp": 1.275481159427673 + }, + { + "x": 2.9782350063323975, + "y": 5.4489359855651855, + "heading": 4.312734924466337e-29, + "angularVelocity": 4.121027857802032e-29, + "velocityX": -1.3933248034746343e-27, + "velocityY": 5.3737543827325536e-27, + "timestamp": 1.364248028752998 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/ABC45.3.traj b/src/main/deploy/choreo/ABC45.3.traj new file mode 100644 index 0000000..9a1bd62 --- /dev/null +++ b/src/main/deploy/choreo/ABC45.3.traj @@ -0,0 +1,166 @@ +{ + "samples": [ + { + "x": 2.9782350063323975, + "y": 5.4489359855651855, + "heading": 4.312734924466337e-29, + "angularVelocity": 4.121027857802032e-29, + "velocityX": -1.3933248034746343e-27, + "velocityY": 5.3737543827325536e-27, + "timestamp": 0 + }, + { + "x": 2.947418627923645, + "y": 5.436521144229905, + "heading": -0.0030772565128669963, + "angularVelocity": -0.050538387039760685, + "velocityX": -0.5061034244864614, + "velocityY": -0.20389137331130347, + "timestamp": 0.060889488033046835 + }, + { + "x": 2.8861123333033722, + "y": 5.410919794564595, + "heading": -0.009557988296382394, + "angularVelocity": -0.10643432869723066, + "velocityX": -1.0068452963013872, + "velocityY": -0.42045598497092856, + "timestamp": 0.12177897606609367 + }, + { + "x": 2.7948983345431926, + "y": 5.370876506509467, + "heading": -0.019975431563290066, + "angularVelocity": -0.1710877132232376, + "velocityX": -1.4980253851153167, + "velocityY": -0.6576387706428947, + "timestamp": 0.1826684640991405 + }, + { + "x": 2.6750654864425063, + "y": 5.314013771366736, + "heading": -0.035347834138682914, + "angularVelocity": -0.252463981419087, + "velocityX": -1.9680383588649908, + "velocityY": -0.9338678477945207, + "timestamp": 0.24355795213218734 + }, + { + "x": 2.531072638838683, + "y": 5.234473020720821, + "heading": -0.058246205865832686, + "angularVelocity": -0.3760644483448775, + "velocityX": -2.3648227675304687, + "velocityY": -1.3063133426700135, + "timestamp": 0.3044474401652342 + }, + { + "x": 2.3962313630013896, + "y": 5.12331458197545, + "heading": -0.09446041492550344, + "angularVelocity": -0.5947530555687393, + "velocityX": -2.2145247101455428, + "velocityY": -1.8255768333122084, + "timestamp": 0.365336928198281 + }, + { + "x": 2.293989410314787, + "y": 5.0058501774188935, + "heading": -0.1351581368489454, + "angularVelocity": -0.6683866663709493, + "velocityX": -1.6791396345969074, + "velocityY": -1.9291409461811224, + "timestamp": 0.42622641623132784 + }, + { + "x": 2.2249673724348735, + "y": 4.889416773729017, + "heading": -0.17744432065306703, + "angularVelocity": -0.6944742872722379, + "velocityX": -1.1335624606081902, + "velocityY": -1.9122086168089227, + "timestamp": 0.4871159042643747 + }, + { + "x": 2.1889755700122517, + "y": 4.776712553076558, + "heading": -0.22023539636010306, + "angularVelocity": -0.7027662259832412, + "velocityX": -0.591100427763291, + "velocityY": -1.850963512639343, + "timestamp": 0.5480053922974215 + }, + { + "x": 2.185830593109131, + "y": 4.669109344482422, + "heading": -0.2629755537909918, + "angularVelocity": -0.7019299851510028, + "velocityX": -0.051650572286206946, + "velocityY": -1.7671885914976975, + "timestamp": 0.6088948803304683 + }, + { + "x": 2.2388582509094666, + "y": 4.537191758347884, + "heading": -0.31881238460124556, + "angularVelocity": -0.6935165819611967, + "velocityX": 0.6586254888295611, + "velocityY": -1.6384710971059544, + "timestamp": 0.6894074913661736 + }, + { + "x": 2.348281455132165, + "y": 4.419298471255977, + "heading": -0.3724787817570206, + "angularVelocity": -0.6665588963693566, + "velocityX": 1.359081550270082, + "velocityY": -1.4642834901929145, + "timestamp": 0.7699201024018789 + }, + { + "x": 2.5107665753721937, + "y": 4.325016992749106, + "heading": -0.41999287361953114, + "angularVelocity": -0.5901447146142, + "velocityX": 2.0181325403540815, + "velocityY": -1.1710150409239681, + "timestamp": 0.8504327134375842 + }, + { + "x": 2.679639946901716, + "y": 4.287961175699886, + "heading": -0.4448569227991737, + "angularVelocity": -0.3088217964837327, + "velocityX": 2.0974772691775088, + "velocityY": -0.460248606678342, + "timestamp": 0.9309453244732895 + }, + { + "x": 2.7946574935809596, + "y": 4.272360261865433, + "heading": -0.45791764540448965, + "angularVelocity": -0.16221958817760687, + "velocityX": 1.4285656023282574, + "velocityY": -0.1937698161041459, + "timestamp": 1.0114579355089948 + }, + { + "x": 2.852456569671631, + "y": 4.266618251800537, + "heading": -0.4636470674172049, + "angularVelocity": -0.07116179613370584, + "velocityX": 0.7178884816570968, + "velocityY": -0.07131814495930158, + "timestamp": 1.0919705465447 + }, + { + "x": 2.852456569671631, + "y": 4.266618251800537, + "heading": -0.4636470674172049, + "angularVelocity": -1.5737839666867323e-21, + "velocityX": 1.5895733095832658e-21, + "velocityY": -2.75601680097891e-21, + "timestamp": 1.1724831575804053 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/ABC45.4.traj b/src/main/deploy/choreo/ABC45.4.traj new file mode 100644 index 0000000..2bed44e --- /dev/null +++ b/src/main/deploy/choreo/ABC45.4.traj @@ -0,0 +1,256 @@ +{ + "samples": [ + { + "x": 2.852456569671631, + "y": 4.266618251800537, + "heading": -0.4636470674172049, + "angularVelocity": -1.5737839666867323e-21, + "velocityX": 1.5895733095832658e-21, + "velocityY": -2.75601680097891e-21, + "timestamp": 0 + }, + { + "x": 2.8689048567574886, + "y": 4.23799010747831, + "heading": -0.45294466367353325, + "angularVelocity": 0.17579953205783594, + "velocityX": 0.27018240407502114, + "velocityY": -0.47025084234068465, + "timestamp": 0.06087845410277115 + }, + { + "x": 2.9018119865044953, + "y": 4.180712651802012, + "heading": -0.4321543072253841, + "angularVelocity": 0.3415059852382609, + "velocityX": 0.5405381958525926, + "velocityY": -0.9408493780017206, + "timestamp": 0.1217569082055423 + }, + { + "x": 2.9511915332190393, + "y": 4.0947593541190255, + "heading": -0.4021080613371361, + "angularVelocity": 0.49354482355162504, + "velocityX": 0.8111169615309952, + "velocityY": -1.4118837107441309, + "timestamp": 0.18263536230831345 + }, + { + "x": 3.0170613770801658, + "y": 3.9800965305554366, + "heading": -0.3639950889659017, + "angularVelocity": 0.6260502657786707, + "velocityX": 1.0819894301180755, + "velocityY": -1.8834713406162191, + "timestamp": 0.2435138164110846 + }, + { + "x": 3.099445061058461, + "y": 3.836681439978771, + "heading": -0.31966267435514817, + "angularVelocity": 0.7282118980208427, + "velocityX": 1.3532486196055649, + "velocityY": -2.3557610437111562, + "timestamp": 0.30439227051385576 + }, + { + "x": 3.1983697154830506, + "y": 3.664465851176401, + "heading": -0.2723848967228083, + "angularVelocity": 0.77659293963886, + "velocityX": 1.624953456564302, + "velocityY": -2.8288430010336207, + "timestamp": 0.3652707246166269 + }, + { + "x": 3.3138250883276226, + "y": 3.4634661057081613, + "heading": -0.2294811416838526, + "angularVelocity": 0.704744489183798, + "velocityX": 1.8964898919684818, + "velocityY": -3.301656529072257, + "timestamp": 0.42614917871939806 + }, + { + "x": 3.444774395245181, + "y": 3.2352057247444876, + "heading": -0.21759605684332275, + "angularVelocity": 0.19522645598830476, + "velocityX": 2.150995928649858, + "velocityY": -3.7494444352732685, + "timestamp": 0.4870276328221692 + }, + { + "x": 3.5780508183938564, + "y": 3.0031703133896683, + "heading": -0.21759602088514662, + "angularVelocity": 5.90655210268061e-7, + "velocityX": 2.189221541724547, + "velocityY": -3.8114537363762975, + "timestamp": 0.5479060869249404 + }, + { + "x": 3.728273640162981, + "y": 2.7817292587041615, + "heading": -0.21759598431777455, + "angularVelocity": 6.006619680592617e-7, + "velocityX": 2.467586011884088, + "velocityY": -3.63742900422019, + "timestamp": 0.6087845410277115 + }, + { + "x": 3.90478285571631, + "y": 2.5806135390044367, + "heading": -0.21759594496836868, + "angularVelocity": 6.463601364304612e-7, + "velocityX": 2.8993708554977, + "velocityY": -3.3035615418258524, + "timestamp": 0.6696629951304827 + }, + { + "x": 4.104857048532899, + "y": 2.4029246222668372, + "heading": -0.21759589931675047, + "angularVelocity": 7.498813641677747e-7, + "velocityX": 3.2864532413854817, + "velocityY": -2.9187488308694074, + "timestamp": 0.7305414492332538 + }, + { + "x": 4.3254111104710535, + "y": 2.2514025588257867, + "heading": -0.21759584129637696, + "angularVelocity": 9.530526747957596e-7, + "velocityX": 3.6228591081802017, + "velocityY": -2.4889275799490753, + "timestamp": 0.791419903336025 + }, + { + "x": 4.56304407119751, + "y": 2.1283838748931885, + "heading": -0.21759574850610755, + "angularVelocity": 0.00000152418898854055, + "velocityX": 3.903400048977931, + "velocityY": -2.0207261459846824, + "timestamp": 0.8522983574387961 + }, + { + "x": 4.902202186268838, + "y": 2.0189477752190697, + "heading": -0.2175956812439266, + "angularVelocity": 8.295900167746209e-7, + "velocityX": 4.18306665431843, + "velocityY": -1.3497495091019698, + "timestamp": 0.9333771790543199 + }, + { + "x": 5.254758904415192, + "y": 1.966906867036995, + "heading": -0.21759563566622517, + "angularVelocity": 5.621406490335741e-7, + "velocityX": 4.34832069733551, + "velocityY": -0.6418557540075404, + "timestamp": 1.0144560006698438 + }, + { + "x": 5.611071357664952, + "y": 1.9736844281466508, + "heading": -0.21759559923618, + "angularVelocity": 4.493164112652634e-7, + "velocityX": 4.394642721121389, + "velocityY": 0.08359224979606977, + "timestamp": 1.0955348222853676 + }, + { + "x": 5.9613941658135525, + "y": 2.0390941463847105, + "heading": -0.21759556616125786, + "angularVelocity": 4.0793540776338416e-7, + "velocityX": 4.320768372903036, + "velocityY": 0.8067423395499419, + "timestamp": 1.1766136439008914 + }, + { + "x": 6.308967238779479, + "y": 2.1178180959591244, + "heading": -0.2175955334410667, + "angularVelocity": 4.035602699984555e-7, + "velocityX": 4.2868540272344084, + "velocityY": 0.9709557687915533, + "timestamp": 1.2576924655164152 + }, + { + "x": 6.6565402898896915, + "y": 2.1965421420233393, + "heading": -0.21759550069927944, + "angularVelocity": 4.0382662916818605e-7, + "velocityX": 4.286853757673087, + "velocityY": 0.9709569588655932, + "timestamp": 1.338771287131939 + }, + { + "x": 6.999243847826658, + "y": 2.2737319873915527, + "heading": -0.20304490740468692, + "angularVelocity": 0.1794623183300735, + "velocityX": 4.226795001561201, + "velocityY": 0.9520346229777302, + "timestamp": 1.4198501087474629 + }, + { + "x": 7.285901071427658, + "y": 2.3385216642106497, + "heading": -0.14981861782250458, + "angularVelocity": 0.6564758653570657, + "velocityX": 3.5355376149931237, + "velocityY": 0.7990949489415372, + "timestamp": 1.5009289303629867 + }, + { + "x": 7.5150671181162325, + "y": 2.3903352141587755, + "heading": -0.09559859222973717, + "angularVelocity": 0.6687322843674137, + "velocityX": 2.8264600067238423, + "velocityY": 0.6390515909792822, + "timestamp": 1.5820077519785105 + }, + { + "x": 7.686818195833645, + "y": 2.429173448097675, + "heading": -0.049917856900545725, + "angularVelocity": 0.5634114361677534, + "velocityX": 2.118322322589465, + "velocityY": 0.4790182339239137, + "timestamp": 1.6630865735940343 + }, + { + "x": 7.801251328421352, + "y": 2.455052252125321, + "heading": -0.017196471477353776, + "angularVelocity": 0.40357499987304796, + "velocityX": 1.4113812991800678, + "velocityY": 0.3191808108702338, + "timestamp": 1.7441653952095582 + }, + { + "x": 7.858440399169922, + "y": 2.4679858684539795, + "heading": -4.255719043582451e-22, + "angularVelocity": 0.21209572530419235, + "velocityX": 0.7053515284146717, + "velocityY": 0.15951904666288638, + "timestamp": 1.825244216825082 + }, + { + "x": 7.858440399169922, + "y": 2.4679858684539795, + "heading": -2.0406216662022407e-22, + "angularVelocity": 2.6720609917197666e-22, + "velocityX": 2.9911493145803203e-21, + "velocityY": -6.582645355508566e-21, + "timestamp": 1.9063230384406058 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/ABC45.5.traj b/src/main/deploy/choreo/ABC45.5.traj new file mode 100644 index 0000000..fed78f3 --- /dev/null +++ b/src/main/deploy/choreo/ABC45.5.traj @@ -0,0 +1,130 @@ +{ + "samples": [ + { + "x": 7.858440399169922, + "y": 2.4679858684539795, + "heading": -2.0406216662022407e-22, + "angularVelocity": 2.6720609917197666e-22, + "velocityX": 2.9911493145803203e-21, + "velocityY": -6.582645355508566e-21, + "timestamp": 0 + }, + { + "x": 7.802193679434275, + "y": 2.461403799955679, + "heading": -0.016098430275066133, + "angularVelocity": -0.20206538801050114, + "velocityX": -0.7060014581238274, + "velocityY": -0.08261726157741077, + "timestamp": 0.0796694101526656 + }, + { + "x": 7.68965716373804, + "y": 2.448234622119993, + "heading": -0.0469697530087514, + "angularVelocity": -0.38749279898681865, + "velocityX": -1.4125436033803618, + "velocityY": -0.16529779510668557, + "timestamp": 0.1593388203053312 + }, + { + "x": 7.520772265984366, + "y": 2.4284714791212965, + "heading": -0.09059926194440926, + "angularVelocity": -0.5476318809446852, + "velocityX": -2.1198211136501484, + "velocityY": -0.24806438206114204, + "timestamp": 0.2390082304579968 + }, + { + "x": 7.295458516959397, + "y": 2.402104954341516, + "heading": -0.14355233187997238, + "angularVelocity": -0.664659997282425, + "velocityX": -2.828108662951245, + "velocityY": -0.3309491651721261, + "timestamp": 0.3186776406106624 + }, + { + "x": 7.01362778562075, + "y": 2.369124734564868, + "heading": -0.19855218692859372, + "angularVelocity": -0.6903509759044087, + "velocityX": -3.5375024215516575, + "velocityY": -0.4139633984166606, + "timestamp": 0.398347050763328 + }, + { + "x": 6.67593552739275, + "y": 2.3296075220865387, + "heading": -0.22861244284967144, + "angularVelocity": -0.3773123946000743, + "velocityX": -4.238668989526869, + "velocityY": -0.49601487449956566, + "timestamp": 0.4780164609159936 + }, + { + "x": 6.338395700294933, + "y": 2.290108147308699, + "heading": -0.25872535016931825, + "angularVelocity": -0.3779732680578816, + "velocityX": -4.236755693948404, + "velocityY": -0.4957909780196619, + "timestamp": 0.5576858710686592 + }, + { + "x": 6.056622492133523, + "y": 2.2571346589733614, + "heading": -0.31544747859363725, + "angularVelocity": -0.7119687257082221, + "velocityX": -3.5367803981661914, + "velocityY": -0.41387890624711093, + "timestamp": 0.6373552812213248 + }, + { + "x": 5.831366504071045, + "y": 2.2307748934608496, + "heading": -0.37017810696303155, + "angularVelocity": -0.6869716778939038, + "velocityX": -2.8273836549164097, + "velocityY": -0.3308643237348025, + "timestamp": 0.7170246913739904 + }, + { + "x": 5.662532030674234, + "y": 2.21101765118974, + "heading": -0.41523336760412627, + "angularVelocity": -0.565527729586024, + "velocityX": -2.119188193728102, + "velocityY": -0.24799031690142373, + "timestamp": 0.796694101526656 + }, + { + "x": 5.550032213198717, + "y": 2.197852767830313, + "heading": -0.4470679159995858, + "angularVelocity": -0.39958308131636616, + "velocityX": -1.4120829721211736, + "velocityY": -0.1652438914032397, + "timestamp": 0.8763635116793216 + }, + { + "x": 5.493804931640625, + "y": 2.1912729740142822, + "heading": -0.4636476090008061, + "angularVelocity": -0.2081061347065287, + "velocityX": -0.705757472665446, + "velocityY": -0.0825887100635233, + "timestamp": 0.9560329218319872 + }, + { + "x": 5.493804931640625, + "y": 2.1912729740142822, + "heading": -0.4636476090008061, + "angularVelocity": -1.2345673166708835e-28, + "velocityX": 3.2849380716344713e-19, + "velocityY": -6.694198418913451e-19, + "timestamp": 1.0357023319846528 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/ABC45.6.traj b/src/main/deploy/choreo/ABC45.6.traj new file mode 100644 index 0000000..876f947 --- /dev/null +++ b/src/main/deploy/choreo/ABC45.6.traj @@ -0,0 +1,139 @@ +{ + "samples": [ + { + "x": 5.493804931640625, + "y": 2.1912729740142822, + "heading": -0.4636476090008061, + "angularVelocity": -1.2345673166708835e-28, + "velocityX": 3.2849380716344713e-19, + "velocityY": -6.694198418913451e-19, + "timestamp": 0 + }, + { + "x": 5.543582685258912, + "y": 2.165029659101282, + "heading": -0.4482440745746598, + "angularVelocity": 0.19399238755066553, + "velocityX": 0.6269019177137097, + "velocityY": -0.3305087765165978, + "timestamp": 0.07940277771014781 + }, + { + "x": 5.64317094411675, + "y": 2.112525762302623, + "heading": -0.41860709064605006, + "angularVelocity": 0.3732487046838187, + "velocityX": 1.2542163099303083, + "velocityY": -0.6612350135951144, + "timestamp": 0.15880555542029562 + }, + { + "x": 5.792614693595038, + "y": 2.03373756688881, + "heading": -0.37651729342487117, + "angularVelocity": 0.5300796576011967, + "velocityX": 1.8820972488370478, + "velocityY": -0.9922599395882876, + "timestamp": 0.23820833313044343 + }, + { + "x": 5.991977025965071, + "y": 1.9286318100019635, + "heading": -0.32501075303183175, + "angularVelocity": 0.6486742892176808, + "velocityX": 2.510772772934815, + "velocityY": -1.3237037786074133, + "timestamp": 0.31761111084059124 + }, + { + "x": 6.2413338476395746, + "y": 1.797168473044452, + "heading": -0.27048250127476103, + "angularVelocity": 0.6867297760806446, + "velocityX": 3.1404042637495326, + "velocityY": -1.6556516125595298, + "timestamp": 0.39701388855073905 + }, + { + "x": 6.540301582790788, + "y": 1.639549780916139, + "heading": -0.23627695302130516, + "angularVelocity": 0.4307852853500927, + "velocityX": 3.7652049937417553, + "velocityY": -1.9850526225125906, + "timestamp": 0.47641666626088686 + }, + { + "x": 6.849033036309926, + "y": 1.4767835616853349, + "heading": -0.23627692283455695, + "angularVelocity": 3.801724461520209e-7, + "velocityX": 3.8881694371717592, + "velocityY": -2.0498806707363233, + "timestamp": 0.5558194439710347 + }, + { + "x": 7.147834515438973, + "y": 1.3192525213457986, + "heading": -0.20213671352117563, + "angularVelocity": 0.4299624055723467, + "velocityX": 3.7631111624305413, + "velocityY": -1.9839487343199602, + "timestamp": 0.6352222216811825 + }, + { + "x": 7.397124245415781, + "y": 1.1878245557825884, + "heading": -0.14527435813644463, + "angularVelocity": 0.7161255188364037, + "velocityX": 3.139559309711022, + "velocityY": -1.655206144588241, + "timestamp": 0.7146249993913303 + }, + { + "x": 7.596418820634183, + "y": 1.0827545211238188, + "heading": -0.09131606420392813, + "angularVelocity": 0.6795517170631776, + "velocityX": 2.5099194381575356, + "velocityY": -1.3232538922292918, + "timestamp": 0.7940277771014781 + }, + { + "x": 7.745802743363532, + "y": 1.0039978669527994, + "heading": -0.04719562685693115, + "angularVelocity": 0.5556535755972488, + "velocityX": 1.8813437896928724, + "velocityY": -0.991862708613457, + "timestamp": 0.8734305548116259 + }, + { + "x": 7.845346710030626, + "y": 0.9515173214270564, + "heading": -0.016137296106261478, + "angularVelocity": 0.39114917193508325, + "velocityX": 1.2536584932893564, + "velocityY": -0.6609409272471353, + "timestamp": 0.9528333325217737 + }, + { + "x": 7.8951005935668945, + "y": 0.9252865910530091, + "heading": 1.5551406670300721e-27, + "angularVelocity": 0.20323339524933484, + "velocityX": 0.626601297474638, + "velocityY": -0.3303502866083613, + "timestamp": 1.0322361102319215 + }, + { + "x": 7.8951005935668945, + "y": 0.9252865910530091, + "heading": 1.630969921544442e-28, + "angularVelocity": -7.571955354473746e-28, + "velocityX": -1.1329628056058385e-19, + "velocityY": -2.1259646408689862e-19, + "timestamp": 1.1116388879420693 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/ABC45.7.traj b/src/main/deploy/choreo/ABC45.7.traj new file mode 100644 index 0000000..008e9dc --- /dev/null +++ b/src/main/deploy/choreo/ABC45.7.traj @@ -0,0 +1,139 @@ +{ + "samples": [ + { + "x": 7.8951005935668945, + "y": 0.9252865910530091, + "heading": 1.630969921544442e-28, + "angularVelocity": -7.571955354473746e-28, + "velocityX": -1.1329628056058385e-19, + "velocityY": -2.1259646408689862e-19, + "timestamp": 0 + }, + { + "x": 7.84555389678355, + "y": 0.9511267368499824, + "heading": -0.01567005287137935, + "angularVelocity": -0.19801439170088264, + "velocityX": -0.6260961022193674, + "velocityY": 0.32652862076778605, + "timestamp": 0.07913592914524248 + }, + { + "x": 7.7464285124002075, + "y": 1.0028237126513568, + "heading": -0.045919773216236644, + "angularVelocity": -0.3822501444235059, + "velocityX": -1.2525964559209537, + "velocityY": 0.6532680712763489, + "timestamp": 0.15827185829048496 + }, + { + "x": 7.597679757083218, + "y": 1.0804008222075567, + "heading": -0.08906812274324855, + "angularVelocity": -0.5452434816026405, + "velocityX": -1.879661449908333, + "velocityY": 0.980301999283009, + "timestamp": 0.23740778743572744 + }, + { + "x": 7.399243123676918, + "y": 1.1838917080097164, + "heading": -0.1421918009547825, + "angularVelocity": -0.6712965752134299, + "velocityX": -2.507541587615634, + "velocityY": 1.3077610501320729, + "timestamp": 0.3165437165809699 + }, + { + "x": 7.1510360344029475, + "y": 1.3133394369416234, + "heading": -0.19903793538493186, + "angularVelocity": -0.718335338248402, + "velocityX": -3.1364652181997132, + "velocityY": 1.6357643150221244, + "timestamp": 0.3956796457262124 + }, + { + "x": 6.853395338676586, + "y": 1.4685683315400455, + "heading": -0.23677487813290127, + "angularVelocity": -0.4768623197524946, + "velocityX": -3.7611322561220595, + "velocityY": 1.9615476342423674, + "timestamp": 0.47481557487145487 + }, + { + "x": 6.5449819848297945, + "y": 1.629415502926325, + "heading": -0.23677490685606856, + "angularVelocity": -3.629598795386413e-7, + "velocityX": -3.897260791375105, + "velocityY": 2.0325429059039406, + "timestamp": 0.5539515040166973 + }, + { + "x": 6.247171783916489, + "y": 1.7847327997599787, + "heading": -0.2747757478843015, + "angularVelocity": -0.4801970664739135, + "velocityX": -3.7632742059137043, + "velocityY": 1.9626647277823852, + "timestamp": 0.6330874331619398 + }, + { + "x": 5.998891519729611, + "y": 1.914218691688432, + "heading": -0.3290453030567638, + "angularVelocity": -0.685776432508405, + "velocityX": -3.1373898919060603, + "velocityY": 1.6362465611644073, + "timestamp": 0.7122233623071823 + }, + { + "x": 5.800380326481861, + "y": 2.0177484627705145, + "heading": -0.3794039803321187, + "angularVelocity": -0.6363566817156947, + "velocityX": -2.508483761951052, + "velocityY": 1.308252423397577, + "timestamp": 0.7913592914524257 + }, + { + "x": 5.651565385396731, + "y": 2.095360090266546, + "heading": -0.42022509607547454, + "angularVelocity": -0.515835426263014, + "velocityX": -1.8804978053900325, + "velocityY": 0.9807381847199456, + "timestamp": 0.870495220597669 + }, + { + "x": 5.552390828881883, + "y": 2.1470827108662562, + "heading": -0.4488296892120089, + "angularVelocity": -0.36146151875003285, + "velocityX": -1.2532178188345666, + "velocityY": 0.6535921313918152, + "timestamp": 0.9496311497429124 + }, + { + "x": 5.502817630767822, + "y": 2.172936677932739, + "heading": -0.4636478282562344, + "angularVelocity": -0.18724919520473388, + "velocityX": -0.6264309858936119, + "velocityY": 0.3267032730358341, + "timestamp": 1.0287670788881558 + }, + { + "x": 5.502817630767822, + "y": 2.1729366779327393, + "heading": -0.4636478282562344, + "angularVelocity": -1.4057086340898478e-23, + "velocityX": 6.82652158657282e-19, + "velocityY": 1.3089918117095413e-18, + "timestamp": 1.1079030080333991 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/ABC45.traj b/src/main/deploy/choreo/ABC45.traj new file mode 100644 index 0000000..67dac11 --- /dev/null +++ b/src/main/deploy/choreo/ABC45.traj @@ -0,0 +1,1012 @@ +{ + "samples": [ + { + "x": 1.8084948062896729, + "y": 6.153295993804932, + "heading": 0.4048918189668609, + "angularVelocity": -4.1069892817306778e-25, + "velocityX": 2.8401756027512354e-22, + "velocityY": 1.242538955366428e-22, + "timestamp": 0 + }, + { + "x": 1.8839619227340805, + "y": 6.186312827447586, + "heading": 0.40399906423823373, + "angularVelocity": -0.009313836888129048, + "velocityX": 0.7873253318540663, + "velocityY": 0.34445452177336483, + "timestamp": 0.09585251914439309 + }, + { + "x": 2.0348961520152846, + "y": 6.252346493154345, + "heading": 0.40221355483146876, + "angularVelocity": -0.01862767324951869, + "velocityX": 1.5746506260710342, + "velocityY": 0.6889090270782012, + "timestamp": 0.19170503828878618 + }, + { + "x": 2.261297485715463, + "y": 6.351396987242034, + "heading": 0.399535290851602, + "angularVelocity": -0.027941508515099304, + "velocityX": 2.361975832467427, + "velocityY": 1.0333634939576128, + "timestamp": 0.2875575574331793 + }, + { + "x": 2.5631658817449776, + "y": 6.4834642912954665, + "heading": 0.3959642726544128, + "angularVelocity": -0.037255340068942634, + "velocityX": 3.1493005997555192, + "velocityY": 1.3778177687170161, + "timestamp": 0.38341007657757237 + }, + { + "x": 2.7895672154458406, + "y": 6.582514785381733, + "heading": 0.3932860087457274, + "angularVelocity": -0.027941507772485746, + "velocityX": 2.361975832474573, + "velocityY": 1.033363493942764, + "timestamp": 0.47926259572196545 + }, + { + "x": 2.9405014447276976, + "y": 6.64854845108714, + "heading": 0.3915004994042191, + "angularVelocity": -0.018627672568715576, + "velocityX": 1.5746506260778408, + "velocityY": 0.6889090270641033, + "timestamp": 0.5751151148663586 + }, + { + "x": 3.015968561172486, + "y": 6.681565284729004, + "heading": 0.3906077447120181, + "angularVelocity": -0.00931383650810643, + "velocityX": 0.7873253318580392, + "velocityY": 0.34445452176511515, + "timestamp": 0.6709676340107517 + }, + { + "x": 3.015968561172486, + "y": 6.681565284729004, + "heading": 0.3906077447120181, + "angularVelocity": 4.111882350756928e-25, + "velocityX": -2.840154314690188e-22, + "velocityY": -1.242602326089003e-22, + "timestamp": 0.7668201531551447 + }, + { + "x": 2.983681753290401, + "y": 6.656482270532227, + "heading": 0.3885085466960412, + "angularVelocity": -0.031083324268668066, + "velocityX": -0.47807844298671504, + "velocityY": -0.3714101566312668, + "timestamp": 0.8343546934711195 + }, + { + "x": 2.91946323897797, + "y": 6.605867942187028, + "heading": 0.38409939745193045, + "angularVelocity": -0.06528732147256167, + "velocityX": -0.9508988143247918, + "velocityY": -0.7494583972644088, + "timestamp": 0.9018892337870942 + }, + { + "x": 2.8238901627601707, + "y": 6.529020599984131, + "heading": 0.3770464356050328, + "angularVelocity": -0.10443488345221376, + "velocityX": -1.415173269420963, + "velocityY": -1.1378968723759795, + "timestamp": 0.9694237741030689 + }, + { + "x": 2.698059890766503, + "y": 6.4246897450492995, + "heading": 0.36674191788660143, + "angularVelocity": -0.15258144454999606, + "velocityX": -1.8631987632542386, + "velocityY": -1.5448517817208405, + "timestamp": 1.0369583144190437 + }, + { + "x": 2.544814664199375, + "y": 6.290062549484542, + "heading": 0.35175074410661816, + "angularVelocity": -0.22197787546704062, + "velocityX": -2.269138515641584, + "velocityY": -1.9934569027178601, + "timestamp": 1.1044928547350186 + }, + { + "x": 2.3816997436952163, + "y": 6.115936226118254, + "heading": 0.3258566979876119, + "angularVelocity": -0.38341929918905854, + "velocityX": -2.415281421047535, + "velocityY": -2.57832988203667, + "timestamp": 1.1720273950509934 + }, + { + "x": 2.259148112580847, + "y": 5.946533385595093, + "heading": 0.2939302799726612, + "angularVelocity": -0.4727420645135983, + "velocityX": -1.814651148004934, + "velocityY": -2.508388148206443, + "timestamp": 1.2395619353669682 + }, + { + "x": 2.174267210882179, + "y": 5.793024676618523, + "heading": 0.2601376621599889, + "angularVelocity": -0.5003753287512784, + "velocityX": -1.2568516984277258, + "velocityY": -2.273039962341445, + "timestamp": 1.307096475682943 + }, + { + "x": 2.125764027427053, + "y": 5.658190290207638, + "heading": 0.22561638717429328, + "angularVelocity": -0.5111647288066291, + "velocityX": -0.7181981727897014, + "velocityY": -1.9965248268520615, + "timestamp": 1.374631015998918 + }, + { + "x": 2.1129797017821503, + "y": 5.5432629827508135, + "heading": 0.19088640125278653, + "angularVelocity": -0.5142551612702936, + "velocityX": -0.1893005502827007, + "velocityY": -1.7017559743371586, + "timestamp": 1.4421655563148927 + }, + { + "x": 2.135519027709961, + "y": 5.4489359855651855, + "heading": 0.15624465539833177, + "angularVelocity": -0.5129485696115788, + "velocityX": 0.3337451594747791, + "velocityY": -1.3967222808402766, + "timestamp": 1.5097000966308676 + }, + { + "x": 2.2257168252781243, + "y": 5.361309601565465, + "heading": 0.11119576514583607, + "angularVelocity": -0.5074966662099387, + "velocityX": 1.0161200710773546, + "velocityY": -0.9871519032464061, + "timestamp": 1.5984669659561925 + }, + { + "x": 2.3751569105405435, + "y": 5.312165927437898, + "heading": 0.06755158237683569, + "angularVelocity": -0.4916719841616492, + "velocityX": 1.6835119498777174, + "velocityY": -0.5536263078903791, + "timestamp": 1.6872338352815175 + }, + { + "x": 2.579774143297148, + "y": 5.307117553721901, + "heading": 0.027843482574793646, + "angularVelocity": -0.4473301819005732, + "velocityX": 2.305108136760973, + "velocityY": -0.056872274018082446, + "timestamp": 1.7760007046068425 + }, + { + "x": 2.780786706373995, + "y": 5.372145722523604, + "heading": 0.011713308423261262, + "angularVelocity": -0.18171390152801614, + "velocityX": 2.2644998590650816, + "velocityY": 0.7325725160293697, + "timestamp": 1.8647675739321674 + }, + { + "x": 2.912768986132865, + "y": 5.4224129115782524, + "heading": 0.0035448462930992987, + "angularVelocity": -0.09202151875183366, + "velocityX": 1.486841664711225, + "velocityY": 0.5662832252247418, + "timestamp": 1.9535344432574924 + }, + { + "x": 2.9782350063323975, + "y": 5.4489359855651855, + "heading": 1.7901422536582138e-29, + "angularVelocity": -0.039934339467438684, + "velocityX": 0.7375051153331075, + "velocityY": 0.29879474390076116, + "timestamp": 2.0423013125828176 + }, + { + "x": 2.9782350063323975, + "y": 5.4489359855651855, + "heading": 4.312734924466337e-29, + "angularVelocity": 4.121027857802032e-29, + "velocityX": -1.3933248034746343e-27, + "velocityY": 5.3737543827325536e-27, + "timestamp": 2.1310681819081427 + }, + { + "x": 2.947418627923645, + "y": 5.436521144229905, + "heading": -0.0030772565128669963, + "angularVelocity": -0.050538387039760685, + "velocityX": -0.5061034244864614, + "velocityY": -0.20389137331130347, + "timestamp": 2.1919576699411896 + }, + { + "x": 2.8861123333033722, + "y": 5.410919794564595, + "heading": -0.009557988296382394, + "angularVelocity": -0.10643432869723066, + "velocityX": -1.0068452963013872, + "velocityY": -0.42045598497092856, + "timestamp": 2.2528471579742364 + }, + { + "x": 2.7948983345431926, + "y": 5.370876506509467, + "heading": -0.019975431563290066, + "angularVelocity": -0.1710877132232376, + "velocityX": -1.4980253851153167, + "velocityY": -0.6576387706428947, + "timestamp": 2.3137366460072832 + }, + { + "x": 2.6750654864425063, + "y": 5.314013771366736, + "heading": -0.035347834138682914, + "angularVelocity": -0.252463981419087, + "velocityX": -1.9680383588649908, + "velocityY": -0.9338678477945207, + "timestamp": 2.37462613404033 + }, + { + "x": 2.531072638838683, + "y": 5.234473020720821, + "heading": -0.058246205865832686, + "angularVelocity": -0.3760644483448775, + "velocityX": -2.3648227675304687, + "velocityY": -1.3063133426700135, + "timestamp": 2.435515622073377 + }, + { + "x": 2.3962313630013896, + "y": 5.12331458197545, + "heading": -0.09446041492550344, + "angularVelocity": -0.5947530555687393, + "velocityX": -2.2145247101455428, + "velocityY": -1.8255768333122084, + "timestamp": 2.4964051101064237 + }, + { + "x": 2.293989410314787, + "y": 5.0058501774188935, + "heading": -0.1351581368489454, + "angularVelocity": -0.6683866663709493, + "velocityX": -1.6791396345969074, + "velocityY": -1.9291409461811224, + "timestamp": 2.5572945981394706 + }, + { + "x": 2.2249673724348735, + "y": 4.889416773729017, + "heading": -0.17744432065306703, + "angularVelocity": -0.6944742872722379, + "velocityX": -1.1335624606081902, + "velocityY": -1.9122086168089227, + "timestamp": 2.6181840861725174 + }, + { + "x": 2.1889755700122517, + "y": 4.776712553076558, + "heading": -0.22023539636010306, + "angularVelocity": -0.7027662259832412, + "velocityX": -0.591100427763291, + "velocityY": -1.850963512639343, + "timestamp": 2.6790735742055642 + }, + { + "x": 2.185830593109131, + "y": 4.669109344482422, + "heading": -0.2629755537909918, + "angularVelocity": -0.7019299851510028, + "velocityX": -0.051650572286206946, + "velocityY": -1.7671885914976975, + "timestamp": 2.739963062238611 + }, + { + "x": 2.2388582509094666, + "y": 4.537191758347884, + "heading": -0.31881238460124556, + "angularVelocity": -0.6935165819611967, + "velocityX": 0.6586254888295611, + "velocityY": -1.6384710971059544, + "timestamp": 2.8204756732743164 + }, + { + "x": 2.348281455132165, + "y": 4.419298471255977, + "heading": -0.3724787817570206, + "angularVelocity": -0.6665588963693566, + "velocityX": 1.359081550270082, + "velocityY": -1.4642834901929145, + "timestamp": 2.9009882843100216 + }, + { + "x": 2.5107665753721937, + "y": 4.325016992749106, + "heading": -0.41999287361953114, + "angularVelocity": -0.5901447146142, + "velocityX": 2.0181325403540815, + "velocityY": -1.1710150409239681, + "timestamp": 2.981500895345727 + }, + { + "x": 2.679639946901716, + "y": 4.287961175699886, + "heading": -0.4448569227991737, + "angularVelocity": -0.3088217964837327, + "velocityX": 2.0974772691775088, + "velocityY": -0.460248606678342, + "timestamp": 3.062013506381432 + }, + { + "x": 2.7946574935809596, + "y": 4.272360261865433, + "heading": -0.45791764540448965, + "angularVelocity": -0.16221958817760687, + "velocityX": 1.4285656023282574, + "velocityY": -0.1937698161041459, + "timestamp": 3.1425261174171375 + }, + { + "x": 2.852456569671631, + "y": 4.266618251800537, + "heading": -0.4636470674172049, + "angularVelocity": -0.07116179613370584, + "velocityX": 0.7178884816570968, + "velocityY": -0.07131814495930158, + "timestamp": 3.223038728452843 + }, + { + "x": 2.852456569671631, + "y": 4.266618251800537, + "heading": -0.4636470674172049, + "angularVelocity": -1.5737839666867323e-21, + "velocityX": 1.5895733095832658e-21, + "velocityY": -2.75601680097891e-21, + "timestamp": 3.303551339488548 + }, + { + "x": 2.8689048567574886, + "y": 4.23799010747831, + "heading": -0.45294466367353325, + "angularVelocity": 0.17579953205783594, + "velocityX": 0.27018240407502114, + "velocityY": -0.47025084234068465, + "timestamp": 3.364429793591319 + }, + { + "x": 2.9018119865044953, + "y": 4.180712651802012, + "heading": -0.4321543072253841, + "angularVelocity": 0.3415059852382609, + "velocityX": 0.5405381958525926, + "velocityY": -0.9408493780017206, + "timestamp": 3.4253082476940904 + }, + { + "x": 2.9511915332190393, + "y": 4.0947593541190255, + "heading": -0.4021080613371361, + "angularVelocity": 0.49354482355162504, + "velocityX": 0.8111169615309952, + "velocityY": -1.4118837107441309, + "timestamp": 3.4861867017968615 + }, + { + "x": 3.0170613770801658, + "y": 3.9800965305554366, + "heading": -0.3639950889659017, + "angularVelocity": 0.6260502657786707, + "velocityX": 1.0819894301180755, + "velocityY": -1.8834713406162191, + "timestamp": 3.5470651558996327 + }, + { + "x": 3.099445061058461, + "y": 3.836681439978771, + "heading": -0.31966267435514817, + "angularVelocity": 0.7282118980208427, + "velocityX": 1.3532486196055649, + "velocityY": -2.3557610437111562, + "timestamp": 3.607943610002404 + }, + { + "x": 3.1983697154830506, + "y": 3.664465851176401, + "heading": -0.2723848967228083, + "angularVelocity": 0.77659293963886, + "velocityX": 1.624953456564302, + "velocityY": -2.8288430010336207, + "timestamp": 3.668822064105175 + }, + { + "x": 3.3138250883276226, + "y": 3.4634661057081613, + "heading": -0.2294811416838526, + "angularVelocity": 0.704744489183798, + "velocityX": 1.8964898919684818, + "velocityY": -3.301656529072257, + "timestamp": 3.729700518207946 + }, + { + "x": 3.444774395245181, + "y": 3.2352057247444876, + "heading": -0.21759605684332275, + "angularVelocity": 0.19522645598830476, + "velocityX": 2.150995928649858, + "velocityY": -3.7494444352732685, + "timestamp": 3.7905789723107173 + }, + { + "x": 3.5780508183938564, + "y": 3.0031703133896683, + "heading": -0.21759602088514662, + "angularVelocity": 5.90655210268061e-7, + "velocityX": 2.189221541724547, + "velocityY": -3.8114537363762975, + "timestamp": 3.8514574264134884 + }, + { + "x": 3.728273640162981, + "y": 2.7817292587041615, + "heading": -0.21759598431777455, + "angularVelocity": 6.006619680592617e-7, + "velocityX": 2.467586011884088, + "velocityY": -3.63742900422019, + "timestamp": 3.9123358805162596 + }, + { + "x": 3.90478285571631, + "y": 2.5806135390044367, + "heading": -0.21759594496836868, + "angularVelocity": 6.463601364304612e-7, + "velocityX": 2.8993708554977, + "velocityY": -3.3035615418258524, + "timestamp": 3.9732143346190307 + }, + { + "x": 4.104857048532899, + "y": 2.4029246222668372, + "heading": -0.21759589931675047, + "angularVelocity": 7.498813641677747e-7, + "velocityX": 3.2864532413854817, + "velocityY": -2.9187488308694074, + "timestamp": 4.034092788721802 + }, + { + "x": 4.3254111104710535, + "y": 2.2514025588257867, + "heading": -0.21759584129637696, + "angularVelocity": 9.530526747957596e-7, + "velocityX": 3.6228591081802017, + "velocityY": -2.4889275799490753, + "timestamp": 4.094971242824573 + }, + { + "x": 4.56304407119751, + "y": 2.1283838748931885, + "heading": -0.21759574850610755, + "angularVelocity": 0.00000152418898854055, + "velocityX": 3.903400048977931, + "velocityY": -2.0207261459846824, + "timestamp": 4.155849696927344 + }, + { + "x": 4.902202186268838, + "y": 2.0189477752190697, + "heading": -0.2175956812439266, + "angularVelocity": 8.295900167746209e-7, + "velocityX": 4.18306665431843, + "velocityY": -1.3497495091019698, + "timestamp": 4.236928518542868 + }, + { + "x": 5.254758904415192, + "y": 1.966906867036995, + "heading": -0.21759563566622517, + "angularVelocity": 5.621406490335741e-7, + "velocityX": 4.34832069733551, + "velocityY": -0.6418557540075404, + "timestamp": 4.318007340158392 + }, + { + "x": 5.611071357664952, + "y": 1.9736844281466508, + "heading": -0.21759559923618, + "angularVelocity": 4.493164112652634e-7, + "velocityX": 4.394642721121389, + "velocityY": 0.08359224979606977, + "timestamp": 4.399086161773916 + }, + { + "x": 5.9613941658135525, + "y": 2.0390941463847105, + "heading": -0.21759556616125786, + "angularVelocity": 4.0793540776338416e-7, + "velocityX": 4.320768372903036, + "velocityY": 0.8067423395499419, + "timestamp": 4.4801649833894395 + }, + { + "x": 6.308967238779479, + "y": 2.1178180959591244, + "heading": -0.2175955334410667, + "angularVelocity": 4.035602699984555e-7, + "velocityX": 4.2868540272344084, + "velocityY": 0.9709557687915533, + "timestamp": 4.561243805004963 + }, + { + "x": 6.6565402898896915, + "y": 2.1965421420233393, + "heading": -0.21759550069927944, + "angularVelocity": 4.0382662916818605e-7, + "velocityX": 4.286853757673087, + "velocityY": 0.9709569588655932, + "timestamp": 4.642322626620487 + }, + { + "x": 6.999243847826658, + "y": 2.2737319873915527, + "heading": -0.20304490740468692, + "angularVelocity": 0.1794623183300735, + "velocityX": 4.226795001561201, + "velocityY": 0.9520346229777302, + "timestamp": 4.723401448236011 + }, + { + "x": 7.285901071427658, + "y": 2.3385216642106497, + "heading": -0.14981861782250458, + "angularVelocity": 0.6564758653570657, + "velocityX": 3.5355376149931237, + "velocityY": 0.7990949489415372, + "timestamp": 4.804480269851535 + }, + { + "x": 7.5150671181162325, + "y": 2.3903352141587755, + "heading": -0.09559859222973717, + "angularVelocity": 0.6687322843674137, + "velocityX": 2.8264600067238423, + "velocityY": 0.6390515909792822, + "timestamp": 4.885559091467059 + }, + { + "x": 7.686818195833645, + "y": 2.429173448097675, + "heading": -0.049917856900545725, + "angularVelocity": 0.5634114361677534, + "velocityX": 2.118322322589465, + "velocityY": 0.4790182339239137, + "timestamp": 4.966637913082582 + }, + { + "x": 7.801251328421352, + "y": 2.455052252125321, + "heading": -0.017196471477353776, + "angularVelocity": 0.40357499987304796, + "velocityX": 1.4113812991800678, + "velocityY": 0.3191808108702338, + "timestamp": 5.047716734698106 + }, + { + "x": 7.858440399169922, + "y": 2.4679858684539795, + "heading": -4.255719043582451e-22, + "angularVelocity": 0.21209572530419235, + "velocityX": 0.7053515284146717, + "velocityY": 0.15951904666288638, + "timestamp": 5.12879555631363 + }, + { + "x": 7.858440399169922, + "y": 2.4679858684539795, + "heading": -2.0406216662022407e-22, + "angularVelocity": 2.6720609917197666e-22, + "velocityX": 2.9911493145803203e-21, + "velocityY": -6.582645355508566e-21, + "timestamp": 5.209874377929154 + }, + { + "x": 7.802193679434275, + "y": 2.461403799955679, + "heading": -0.016098430275066133, + "angularVelocity": -0.20206538801050114, + "velocityX": -0.7060014581238274, + "velocityY": -0.08261726157741077, + "timestamp": 5.2895437880818195 + }, + { + "x": 7.68965716373804, + "y": 2.448234622119993, + "heading": -0.0469697530087514, + "angularVelocity": -0.38749279898681865, + "velocityX": -1.4125436033803618, + "velocityY": -0.16529779510668557, + "timestamp": 5.369213198234485 + }, + { + "x": 7.520772265984366, + "y": 2.4284714791212965, + "heading": -0.09059926194440926, + "angularVelocity": -0.5476318809446852, + "velocityX": -2.1198211136501484, + "velocityY": -0.24806438206114204, + "timestamp": 5.448882608387151 + }, + { + "x": 7.295458516959397, + "y": 2.402104954341516, + "heading": -0.14355233187997238, + "angularVelocity": -0.664659997282425, + "velocityX": -2.828108662951245, + "velocityY": -0.3309491651721261, + "timestamp": 5.528552018539816 + }, + { + "x": 7.01362778562075, + "y": 2.369124734564868, + "heading": -0.19855218692859372, + "angularVelocity": -0.6903509759044087, + "velocityX": -3.5375024215516575, + "velocityY": -0.4139633984166606, + "timestamp": 5.608221428692482 + }, + { + "x": 6.67593552739275, + "y": 2.3296075220865387, + "heading": -0.22861244284967144, + "angularVelocity": -0.3773123946000743, + "velocityX": -4.238668989526869, + "velocityY": -0.49601487449956566, + "timestamp": 5.6878908388451475 + }, + { + "x": 6.338395700294933, + "y": 2.290108147308699, + "heading": -0.25872535016931825, + "angularVelocity": -0.3779732680578816, + "velocityX": -4.236755693948404, + "velocityY": -0.4957909780196619, + "timestamp": 5.767560248997813 + }, + { + "x": 6.056622492133523, + "y": 2.2571346589733614, + "heading": -0.31544747859363725, + "angularVelocity": -0.7119687257082221, + "velocityX": -3.5367803981661914, + "velocityY": -0.41387890624711093, + "timestamp": 5.847229659150479 + }, + { + "x": 5.831366504071045, + "y": 2.2307748934608496, + "heading": -0.37017810696303155, + "angularVelocity": -0.6869716778939038, + "velocityX": -2.8273836549164097, + "velocityY": -0.3308643237348025, + "timestamp": 5.926899069303144 + }, + { + "x": 5.662532030674234, + "y": 2.21101765118974, + "heading": -0.41523336760412627, + "angularVelocity": -0.565527729586024, + "velocityX": -2.119188193728102, + "velocityY": -0.24799031690142373, + "timestamp": 6.00656847945581 + }, + { + "x": 5.550032213198717, + "y": 2.197852767830313, + "heading": -0.4470679159995858, + "angularVelocity": -0.39958308131636616, + "velocityX": -1.4120829721211736, + "velocityY": -0.1652438914032397, + "timestamp": 6.0862378896084754 + }, + { + "x": 5.493804931640625, + "y": 2.1912729740142822, + "heading": -0.4636476090008061, + "angularVelocity": -0.2081061347065287, + "velocityX": -0.705757472665446, + "velocityY": -0.0825887100635233, + "timestamp": 6.165907299761141 + }, + { + "x": 5.493804931640625, + "y": 2.1912729740142822, + "heading": -0.4636476090008061, + "angularVelocity": -1.2345673166708835e-28, + "velocityX": 3.2849380716344713e-19, + "velocityY": -6.694198418913451e-19, + "timestamp": 6.245576709913807 + }, + { + "x": 5.543582685258912, + "y": 2.165029659101282, + "heading": -0.4482440745746598, + "angularVelocity": 0.19399238755066553, + "velocityX": 0.6269019177137097, + "velocityY": -0.3305087765165978, + "timestamp": 6.3249794876239545 + }, + { + "x": 5.64317094411675, + "y": 2.112525762302623, + "heading": -0.41860709064605006, + "angularVelocity": 0.3732487046838187, + "velocityX": 1.2542163099303083, + "velocityY": -0.6612350135951144, + "timestamp": 6.404382265334102 + }, + { + "x": 5.792614693595038, + "y": 2.03373756688881, + "heading": -0.37651729342487117, + "angularVelocity": 0.5300796576011967, + "velocityX": 1.8820972488370478, + "velocityY": -0.9922599395882876, + "timestamp": 6.48378504304425 + }, + { + "x": 5.991977025965071, + "y": 1.9286318100019635, + "heading": -0.32501075303183175, + "angularVelocity": 0.6486742892176808, + "velocityX": 2.510772772934815, + "velocityY": -1.3237037786074133, + "timestamp": 6.563187820754398 + }, + { + "x": 6.2413338476395746, + "y": 1.797168473044452, + "heading": -0.27048250127476103, + "angularVelocity": 0.6867297760806446, + "velocityX": 3.1404042637495326, + "velocityY": -1.6556516125595298, + "timestamp": 6.642590598464546 + }, + { + "x": 6.540301582790788, + "y": 1.639549780916139, + "heading": -0.23627695302130516, + "angularVelocity": 0.4307852853500927, + "velocityX": 3.7652049937417553, + "velocityY": -1.9850526225125906, + "timestamp": 6.7219933761746935 + }, + { + "x": 6.849033036309926, + "y": 1.4767835616853349, + "heading": -0.23627692283455695, + "angularVelocity": 3.801724461520209e-7, + "velocityX": 3.8881694371717592, + "velocityY": -2.0498806707363233, + "timestamp": 6.801396153884841 + }, + { + "x": 7.147834515438973, + "y": 1.3192525213457986, + "heading": -0.20213671352117563, + "angularVelocity": 0.4299624055723467, + "velocityX": 3.7631111624305413, + "velocityY": -1.9839487343199602, + "timestamp": 6.880798931594989 + }, + { + "x": 7.397124245415781, + "y": 1.1878245557825884, + "heading": -0.14527435813644463, + "angularVelocity": 0.7161255188364037, + "velocityX": 3.139559309711022, + "velocityY": -1.655206144588241, + "timestamp": 6.960201709305137 + }, + { + "x": 7.596418820634183, + "y": 1.0827545211238188, + "heading": -0.09131606420392813, + "angularVelocity": 0.6795517170631776, + "velocityX": 2.5099194381575356, + "velocityY": -1.3232538922292918, + "timestamp": 7.039604487015285 + }, + { + "x": 7.745802743363532, + "y": 1.0039978669527994, + "heading": -0.04719562685693115, + "angularVelocity": 0.5556535755972488, + "velocityX": 1.8813437896928724, + "velocityY": -0.991862708613457, + "timestamp": 7.1190072647254325 + }, + { + "x": 7.845346710030626, + "y": 0.9515173214270564, + "heading": -0.016137296106261478, + "angularVelocity": 0.39114917193508325, + "velocityX": 1.2536584932893564, + "velocityY": -0.6609409272471353, + "timestamp": 7.19841004243558 + }, + { + "x": 7.8951005935668945, + "y": 0.9252865910530091, + "heading": 1.5551406670300721e-27, + "angularVelocity": 0.20323339524933484, + "velocityX": 0.626601297474638, + "velocityY": -0.3303502866083613, + "timestamp": 7.277812820145728 + }, + { + "x": 7.8951005935668945, + "y": 0.9252865910530091, + "heading": 1.630969921544442e-28, + "angularVelocity": -7.571955354473746e-28, + "velocityX": -1.1329628056058385e-19, + "velocityY": -2.1259646408689862e-19, + "timestamp": 7.357215597855876 + }, + { + "x": 7.84555389678355, + "y": 0.9511267368499824, + "heading": -0.01567005287137935, + "angularVelocity": -0.19801439170088264, + "velocityX": -0.6260961022193674, + "velocityY": 0.32652862076778605, + "timestamp": 7.4363515270011185 + }, + { + "x": 7.7464285124002075, + "y": 1.0028237126513568, + "heading": -0.045919773216236644, + "angularVelocity": -0.3822501444235059, + "velocityX": -1.2525964559209537, + "velocityY": 0.6532680712763489, + "timestamp": 7.515487456146361 + }, + { + "x": 7.597679757083218, + "y": 1.0804008222075567, + "heading": -0.08906812274324855, + "angularVelocity": -0.5452434816026405, + "velocityX": -1.879661449908333, + "velocityY": 0.980301999283009, + "timestamp": 7.594623385291603 + }, + { + "x": 7.399243123676918, + "y": 1.1838917080097164, + "heading": -0.1421918009547825, + "angularVelocity": -0.6712965752134299, + "velocityX": -2.507541587615634, + "velocityY": 1.3077610501320729, + "timestamp": 7.673759314436846 + }, + { + "x": 7.1510360344029475, + "y": 1.3133394369416234, + "heading": -0.19903793538493186, + "angularVelocity": -0.718335338248402, + "velocityX": -3.1364652181997132, + "velocityY": 1.6357643150221244, + "timestamp": 7.752895243582088 + }, + { + "x": 6.853395338676586, + "y": 1.4685683315400455, + "heading": -0.23677487813290127, + "angularVelocity": -0.4768623197524946, + "velocityX": -3.7611322561220595, + "velocityY": 1.9615476342423674, + "timestamp": 7.832031172727331 + }, + { + "x": 6.5449819848297945, + "y": 1.629415502926325, + "heading": -0.23677490685606856, + "angularVelocity": -3.629598795386413e-7, + "velocityX": -3.897260791375105, + "velocityY": 2.0325429059039406, + "timestamp": 7.911167101872573 + }, + { + "x": 6.247171783916489, + "y": 1.7847327997599787, + "heading": -0.2747757478843015, + "angularVelocity": -0.4801970664739135, + "velocityX": -3.7632742059137043, + "velocityY": 1.9626647277823852, + "timestamp": 7.990303031017816 + }, + { + "x": 5.998891519729611, + "y": 1.914218691688432, + "heading": -0.3290453030567638, + "angularVelocity": -0.685776432508405, + "velocityX": -3.1373898919060603, + "velocityY": 1.6362465611644073, + "timestamp": 8.069438960163058 + }, + { + "x": 5.800380326481861, + "y": 2.0177484627705145, + "heading": -0.3794039803321187, + "angularVelocity": -0.6363566817156947, + "velocityX": -2.508483761951052, + "velocityY": 1.308252423397577, + "timestamp": 8.148574889308302 + }, + { + "x": 5.651565385396731, + "y": 2.095360090266546, + "heading": -0.42022509607547454, + "angularVelocity": -0.515835426263014, + "velocityX": -1.8804978053900325, + "velocityY": 0.9807381847199456, + "timestamp": 8.227710818453545 + }, + { + "x": 5.552390828881883, + "y": 2.1470827108662562, + "heading": -0.4488296892120089, + "angularVelocity": -0.36146151875003285, + "velocityX": -1.2532178188345666, + "velocityY": 0.6535921313918152, + "timestamp": 8.306846747598788 + }, + { + "x": 5.502817630767822, + "y": 2.172936677932739, + "heading": -0.4636478282562344, + "angularVelocity": -0.18724919520473388, + "velocityX": -0.6264309858936119, + "velocityY": 0.3267032730358341, + "timestamp": 8.385982676744032 + }, + { + "x": 5.502817630767822, + "y": 2.1729366779327393, + "heading": -0.4636478282562344, + "angularVelocity": -1.4057086340898478e-23, + "velocityX": 6.82652158657282e-19, + "velocityY": 1.3089918117095413e-18, + "timestamp": 8.465118605889275 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/BA123.1.traj b/src/main/deploy/choreo/BA123.1.traj new file mode 100644 index 0000000..e51445b --- /dev/null +++ b/src/main/deploy/choreo/BA123.1.traj @@ -0,0 +1,76 @@ +{ + "samples": [ + { + "x": 1.483002781867981, + "y": 5.5289692878723145, + "heading": 3.141592653589793, + "angularVelocity": 4.804188938729379e-42, + "velocityX": -1.0059577979228262e-42, + "velocityY": 6.365083529423221e-42, + "timestamp": 0 + }, + { + "x": 1.5755119282180663, + "y": 5.536620736608523, + "heading": 3.1400776717900736, + "angularVelocity": -0.016530919629225094, + "velocityX": 1.009425501722026, + "velocityY": 0.08348977138129623, + "timestamp": 0.0916453430117127 + }, + { + "x": 1.7605302151370883, + "y": 5.551923633600031, + "heading": 3.1370477085137116, + "angularVelocity": -0.03306183573315699, + "velocityX": 2.0188509403623054, + "velocityY": 0.16697953751509448, + "timestamp": 0.1832906860234254 + }, + { + "x": 2.038057625280542, + "y": 5.57487797740817, + "heading": 3.1325027643437484, + "angularVelocity": -0.04959274547515668, + "velocityX": 3.0282761897457733, + "velocityY": 0.25046928795068285, + "timestamp": 0.27493602903513814 + }, + { + "x": 2.3155850354358676, + "y": 5.597832321183343, + "heading": 3.1279578246361175, + "angularVelocity": -0.0495926967838387, + "velocityX": 3.0282761898753168, + "velocityY": 0.2504692875909585, + "timestamp": 0.3665813720468508 + }, + { + "x": 2.500603322364312, + "y": 5.613135218148452, + "heading": 3.1249278648972543, + "angularVelocity": -0.033061797133284165, + "velocityX": 2.018850940465115, + "velocityY": 0.16697953722703365, + "timestamp": 0.4582267150585635 + }, + { + "x": 2.5931124687194824, + "y": 5.620786666870117, + "heading": 3.1234128850040452, + "angularVelocity": -0.01653089882609372, + "velocityX": 1.0094255017775111, + "velocityY": 0.08348977122260563, + "timestamp": 0.5498720580702762 + }, + { + "x": 2.5931124687194824, + "y": 5.620786666870117, + "heading": 3.1234128850040452, + "angularVelocity": 6.797549086666313e-39, + "velocityX": 1.0538796562779821e-38, + "velocityY": 4.37631798735499e-39, + "timestamp": 0.6415174010819888 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/BA123.2.traj b/src/main/deploy/choreo/BA123.2.traj new file mode 100644 index 0000000..72bbc3b --- /dev/null +++ b/src/main/deploy/choreo/BA123.2.traj @@ -0,0 +1,679 @@ +{ + "samples": [ + { + "x": 2.5931124687194824, + "y": 5.620786666870117, + "heading": 3.1234128850040452, + "angularVelocity": 6.797549086666313e-39, + "velocityX": 1.0538796562779821e-38, + "velocityY": 4.37631798735499e-39, + "timestamp": 0 + }, + { + "x": 2.5902191633660006, + "y": 5.622172308178305, + "heading": 3.1236231846500075, + "angularVelocity": 0.012341535996375805, + "velocityX": -0.16979501798548408, + "velocityY": 0.08131702744820536, + "timestamp": 0.017039989675842193 + }, + { + "x": 2.5844404753484342, + "y": 5.624959867845441, + "heading": 3.1240494020264102, + "angularVelocity": 0.02501277198581982, + "velocityX": -0.33912508912835815, + "velocityY": 0.1635892814587969, + "timestamp": 0.034079979351684386 + }, + { + "x": 2.5757853965549486, + "y": 5.629167537767336, + "heading": 3.124697825770354, + "angularVelocity": 0.03805305967192536, + "velocityX": -0.5079274669840959, + "velocityY": 0.24692913563558913, + "timestamp": 0.05111996902752658 + }, + { + "x": 2.5642642159968103, + "y": 5.634815780702077, + "heading": 3.125575541744695, + "angularVelocity": 0.0515091846320203, + "velocityX": -0.6761260292588179, + "velocityY": 0.3314698566248499, + "timestamp": 0.06815995870336877 + }, + { + "x": 2.5498888162624387, + "y": 5.64192777941668, + "heading": 3.126690593435854, + "angularVelocity": 0.06543734546625592, + "velocityX": -0.843627256109882, + "velocityY": 0.4173710694605735, + "timestamp": 0.08519994837921097 + }, + { + "x": 2.5326730658717955, + "y": 5.650530011128654, + "heading": 3.1280521879604875, + "angularVelocity": 0.07990583037522597, + "velocityX": -1.0103146021884275, + "velocityY": 0.5048261105562488, + "timestamp": 0.10223993805505316 + }, + { + "x": 2.512633348908364, + "y": 5.660652993489354, + "heading": 3.1296709648936383, + "angularVelocity": 0.09499870386926744, + "velocityX": -1.1760404404377187, + "velocityY": 0.5940720947179435, + "timestamp": 0.11927992773089535 + }, + { + "x": 2.489789296285424, + "y": 5.672332270018097, + "heading": 3.131559353416261, + "angularVelocity": 0.11082098983309922, + "velocityX": -1.3406142290876446, + "velocityY": 0.6854039674273565, + "timestamp": 0.13631991740673755 + }, + { + "x": 2.464164822040195, + "y": 5.685609737022312, + "heading": 3.133732056508738, + "angularVelocity": 0.12750612728111888, + "velocityX": -1.503784610947115, + "velocityY": 0.7791945451140279, + "timestamp": 0.15335990708257974 + }, + { + "x": 2.435789637049594, + "y": 5.7005354691556045, + "heading": 3.136206722659641, + "angularVelocity": 0.14522697477989688, + "velocityX": -1.6652113956869623, + "velocityY": 0.8759237779617546, + "timestamp": 0.17039989675842193 + }, + { + "x": 2.404701540197862, + "y": 5.717170292002654, + "heading": 3.1390049024308224, + "angularVelocity": 0.16421252737895475, + "velocityX": -1.8244199347025483, + "velocityY": 0.9762225895379132, + "timestamp": 0.18743988643426412 + }, + { + "x": 2.370950035804977, + "y": 5.735589504313645, + "heading": 3.142153451841552, + "angularVelocity": 0.18477413840181903, + "velocityX": -1.9807232888606294, + "velocityY": 1.0809403445298984, + "timestamp": 0.20447987611010632 + }, + { + "x": 2.33460234106864, + "y": 5.755888415439336, + "heading": 3.145686661546861, + "angularVelocity": 0.20734811302834996, + "velocityX": -2.1330819694021232, + "velocityY": 1.1912513746688418, + "timestamp": 0.2215198657859485 + }, + { + "x": 2.2957539884735723, + "y": 5.778190797790987, + "heading": 3.149649608554467, + "angularVelocity": 0.23256745356039296, + "velocityX": -2.2798342800725706, + "velocityY": 1.3088260483672556, + "timestamp": 0.2385598554617907 + }, + { + "x": 2.2545489517182054, + "y": 5.802661947482074, + "heading": 3.154103636346379, + "angularVelocity": 0.26138676587500986, + "velocityX": -2.41813742491779, + "velocityY": 1.4361012040857848, + "timestamp": 0.2555998451376329 + }, + { + "x": 2.2112211753037756, + "y": 5.829528007071594, + "heading": 3.1591355992300465, + "angularVelocity": 0.29530316504839027, + "velocityX": -2.5427114240483375, + "velocityY": 1.5766476447816522, + "timestamp": 0.2726398348134751 + }, + { + "x": 2.166187163497284, + "y": 5.859096368777116, + "heading": 3.164873306208298, + "angularVelocity": 0.33672009710108897, + "velocityX": -2.642842669695792, + "velocityY": 1.7352335457950308, + "timestamp": 0.2896798244893173 + }, + { + "x": 2.1202483707966637, + "y": 5.891721818786653, + "heading": 3.171501741715199, + "angularVelocity": 0.3889929297491645, + "velocityX": -2.695940172179118, + "velocityY": 1.9146402451048057, + "timestamp": 0.3067198141651595 + }, + { + "x": 2.0748263621986163, + "y": 5.927485928507909, + "heading": 3.1791652437298565, + "angularVelocity": 0.4497363062092675, + "velocityX": -2.665612448254168, + "velocityY": 2.0988340017575653, + "timestamp": 0.32375980384100167 + }, + { + "x": 2.0314525713511236, + "y": 5.965692244208046, + "heading": 3.1877298171032367, + "angularVelocity": 0.5026161128209019, + "velocityX": -2.5454118032116115, + "velocityY": 2.2421560357106713, + "timestamp": 0.34079979351684386 + }, + { + "x": 1.990913443115171, + "y": 6.005377227458498, + "heading": 3.1969662928737232, + "angularVelocity": 0.5420470285601927, + "velocityX": -2.3790582627773333, + "velocityY": 2.32893235297639, + "timestamp": 0.35783978319268606 + }, + { + "x": 1.953477877153394, + "y": 6.045854258534539, + "heading": 3.2066830981783285, + "angularVelocity": 0.5702353985801418, + "velocityX": -2.196924216148433, + "velocityY": 2.3754140610440646, + "timestamp": 0.37487977286852825 + }, + { + "x": 1.919228244872485, + "y": 6.086688482249977, + "heading": 3.2167506488100557, + "angularVelocity": 0.5908190570091756, + "velocityX": -2.009956163850568, + "velocityY": 2.3963760830987972, + "timestamp": 0.39191976254437044 + }, + { + "x": 1.888185091203371, + "y": 6.127595876679071, + "heading": 3.2270820659423944, + "angularVelocity": 0.6063041896665977, + "velocityX": -1.8217824223875096, + "velocityY": 2.40067014166615, + "timestamp": 0.40895975222021264 + }, + { + "x": 1.860347665740482, + "y": 6.1683807623094, + "heading": 3.2376165950788, + "angularVelocity": 0.6182239154370359, + "velocityX": -1.6336527188367116, + "velocityY": 2.3934806538145956, + "timestamp": 0.42599974189605483 + }, + { + "x": 1.835707853847385, + "y": 6.208901611745203, + "heading": 3.248309830617314, + "angularVelocity": 0.6275376770722889, + "velocityX": -1.4459992266327082, + "velocityY": 2.3779855625881225, + "timestamp": 0.443039731571897 + }, + { + "x": 1.8142553226903924, + "y": 6.249051887575358, + "heading": 3.259128074403703, + "angularVelocity": 0.634873846298503, + "velocityX": -1.2589521217495907, + "velocityY": 2.3562382720851454, + "timestamp": 0.4600797212477392 + }, + { + "x": 1.7959795058005092, + "y": 6.288748754453674, + "heading": 3.2700449558991145, + "angularVelocity": 0.6406624477530337, + "velocityX": -1.0725251151878987, + "velocityY": 2.3296297494002878, + "timestamp": 0.4771197109235814 + }, + { + "x": 1.7808703618932606, + "y": 6.3279260885864845, + "heading": 3.2810393135811178, + "angularVelocity": 0.6452091750730349, + "velocityX": -0.8866873862411462, + "velocityY": 2.2991407200412497, + "timestamp": 0.4941597005994236 + }, + { + "x": 1.7689186333603624, + "y": 6.366529951104452, + "heading": 3.2920938098527754, + "angularVelocity": 0.6487384371675794, + "velocityX": -0.7013929444947057, + "velocityY": 2.2654862621599268, + "timestamp": 0.5111996902752658 + }, + { + "x": 1.7601158958397196, + "y": 6.404515541778896, + "heading": 3.3031939914863275, + "angularVelocity": 0.6514195046308695, + "velocityX": -0.5165928904946825, + "velocityY": 2.229202681284329, + "timestamp": 0.528239679951108 + }, + { + "x": 1.7544545221137668, + "y": 6.441845080083907, + "heading": 3.314327631669556, + "angularVelocity": 0.653383035731085, + "velocityX": -0.3322404434304942, + "velocityY": 2.1907019320518635, + "timestamp": 0.5452796696269502 + }, + { + "x": 1.7519276141866935, + "y": 6.478486288821362, + "heading": 3.325484256418616, + "angularVelocity": 0.6547318960455363, + "velocityX": -0.14829280857227845, + "velocityY": 2.1503069799039145, + "timestamp": 0.5623196593027924 + }, + { + "x": 1.7525289273083395, + "y": 6.514411281966431, + "heading": 3.336654795480751, + "angularVelocity": 0.6555484642089668, + "velocityX": 0.03528835011552745, + "velocityY": 2.108275522959999, + "timestamp": 0.5793596489786346 + }, + { + "x": 1.7562527962036754, + "y": 6.5495957313476785, + "heading": 3.3478313196027307, + "angularVelocity": 0.6558997003281736, + "velocityX": 0.2185370394100181, + "velocityY": 2.064816355559754, + "timestamp": 0.5963996386544768 + }, + { + "x": 1.7630940675735474, + "y": 6.584018230438232, + "heading": 3.3590068391602883, + "angularVelocity": 0.6558407469812757, + "velocityX": 0.4014833048620338, + "velocityY": 2.0201009358213167, + "timestamp": 0.613439628330319 + }, + { + "x": 1.7720599747049364, + "y": 6.6151238881747885, + "heading": 3.3693153572951067, + "angularVelocity": 0.6554501418707033, + "velocityX": 0.5700824332276426, + "velocityY": 1.9778019992556979, + "timestamp": 0.6291670154743616 + }, + { + "x": 1.7836731787465967, + "y": 6.64554729228533, + "heading": 3.3796121703026714, + "angularVelocity": 0.6547058906389773, + "velocityX": 0.7384064457305151, + "velocityY": 1.9344220264880285, + "timestamp": 0.6448944026184042 + }, + { + "x": 1.7979286920774356, + "y": 6.6752693973823, + "heading": 3.389891037190624, + "angularVelocity": 0.6535648161904469, + "velocityX": 0.9064133285635219, + "velocityY": 1.8898310841307224, + "timestamp": 0.6606217897624469 + }, + { + "x": 1.8148207108436947, + "y": 6.704268726353282, + "heading": 3.400144906665288, + "angularVelocity": 0.651975396850814, + "velocityX": 1.0740511829174035, + "velocityY": 1.8438745549648863, + "timestamp": 0.6763491769064895 + }, + { + "x": 1.834342408767159, + "y": 6.732520878383194, + "heading": 3.4103657501447873, + "angularVelocity": 0.6498754933600988, + "velocityX": 1.2412549996175823, + "velocityY": 1.7963665401733704, + "timestamp": 0.6920765640505322 + }, + { + "x": 1.8564856585052998, + "y": 6.7599978965922585, + "heading": 3.42054434613586, + "angularVelocity": 0.6471892564130288, + "velocityX": 1.4079420526332154, + "velocityY": 1.7470809332415638, + "timestamp": 0.7078039511945748 + }, + { + "x": 1.8812406470329832, + "y": 6.786667442295124, + "heading": 3.4306699971764414, + "angularVelocity": 0.6438228389651354, + "velocityX": 1.5740051606130965, + "velocityY": 1.6957391242808437, + "timestamp": 0.7235313383386175 + }, + { + "x": 1.9085953317289672, + "y": 6.812491697509257, + "heading": 3.440730151284119, + "angularVelocity": 0.6396583243954679, + "velocityX": 1.7393025583620485, + "velocityY": 1.6419927212076624, + "timestamp": 0.7392587254826601 + }, + { + "x": 1.9385346494863533, + "y": 6.837425877048513, + "heading": 3.4507098849378357, + "angularVelocity": 0.6345449223265929, + "velocityX": 1.9036421932759835, + "velocityY": 1.5853987258588957, + "timestamp": 0.7549861126267028 + }, + { + "x": 1.9710393290071817, + "y": 6.861416165928424, + "heading": 3.4605911798929663, + "angularVelocity": 0.6282858598590166, + "velocityX": 2.0667564944594514, + "velocityY": 1.5253829933854495, + "timestamp": 0.7707134997707454 + }, + { + "x": 2.0060840386824674, + "y": 6.884396788302994, + "heading": 3.470351883817563, + "angularVelocity": 0.6206182778614892, + "velocityX": 2.228260127020535, + "velocityY": 1.4611850121127599, + "timestamp": 0.7864408869147881 + }, + { + "x": 2.0436343673142363, + "y": 6.906285727611734, + "heading": 3.479964169940124, + "angularVelocity": 0.611181376443841, + "velocityX": 2.3875757802523783, + "velocityY": 1.3917721429672245, + "timestamp": 0.8021682740588307 + }, + { + "x": 2.0836416366671413, + "y": 6.926978297915, + "heading": 3.489392174533464, + "angularVelocity": 0.5994641390201337, + "velocityX": 2.543796308089173, + "velocityY": 1.3157029908241744, + "timestamp": 0.8178956612028734 + }, + { + "x": 2.1260334179669322, + "y": 6.946337239436048, + "heading": 3.498588236932545, + "angularVelocity": 0.5847164767330048, + "velocityX": 2.69541157164483, + "velocityY": 1.2309064019181353, + "timestamp": 0.833623048346916 + }, + { + "x": 2.170694904668439, + "y": 6.964177314379111, + "heading": 3.5074866974204353, + "angularVelocity": 0.5657939495220478, + "velocityX": 2.8397270501746186, + "velocityY": 1.13433177295568, + "timestamp": 0.8493504354909587 + }, + { + "x": 2.217429447746538, + "y": 6.980242597216452, + "heading": 3.5159934565423607, + "angularVelocity": 0.5408882635121971, + "velocityX": 2.9715389244297503, + "velocityY": 1.021484540960543, + "timestamp": 0.8650778226350013 + }, + { + "x": 2.2658706589785127, + "y": 6.994183492523358, + "heading": 3.5239696234664595, + "angularVelocity": 0.5071514327870872, + "velocityX": 3.0800546071839623, + "velocityY": 0.8864088598585902, + "timestamp": 0.880805209779044 + }, + { + "x": 2.315306439317678, + "y": 7.005595211136875, + "heading": 3.531224973833355, + "angularVelocity": 0.46131949957394686, + "velocityX": 3.1432926452688266, + "velocityY": 0.7255953267379982, + "timestamp": 0.8965325969230866 + }, + { + "x": 2.364549276525335, + "y": 7.014302767113342, + "heading": 3.537627214320393, + "angularVelocity": 0.4070759133988072, + "velocityX": 3.1310246741340757, + "velocityY": 0.5536556006867284, + "timestamp": 0.9122599840671293 + }, + { + "x": 2.4123908439730872, + "y": 7.020687456759953, + "heading": 3.5432119576128693, + "angularVelocity": 0.3550967011447554, + "velocityX": 3.041927245103382, + "velocityY": 0.4059599721260442, + "timestamp": 0.9279873712111719 + }, + { + "x": 2.458132378842044, + "y": 7.025345704452024, + "heading": 3.5480975346239485, + "angularVelocity": 0.31064136504896245, + "velocityX": 2.9084001334756193, + "velocityY": 0.2961870048347796, + "timestamp": 0.9437147583552146 + }, + { + "x": 2.5014553638181054, + "y": 7.028751699569263, + "heading": 3.552407775238728, + "angularVelocity": 0.27405954818200595, + "velocityX": 2.7546206232019532, + "velocityY": 0.21656458800457026, + "timestamp": 0.9594421454992572 + }, + { + "x": 2.542213092217696, + "y": 7.031233755116593, + "heading": 3.5562362031090795, + "angularVelocity": 0.24342427863497068, + "velocityX": 2.5915130101587813, + "velocityY": 0.15781741268256558, + "timestamp": 0.9751695326432999 + }, + { + "x": 2.5803326470423813, + "y": 7.033019833738538, + "heading": 3.559649640158312, + "angularVelocity": 0.21703777098952132, + "velocityX": 2.423769089904064, + "velocityY": 0.11356486653424694, + "timestamp": 0.9908969197873425 + }, + { + "x": 2.6157749798098076, + "y": 7.03427367869847, + "heading": 3.5626967208417395, + "angularVelocity": 0.19374360505786095, + "velocityX": 2.253542336232949, + "velocityY": 0.07972366601316434, + "timestamp": 1.0066243069313852 + }, + { + "x": 2.6485179067049858, + "y": 7.035117253167883, + "heading": 3.565413972482842, + "angularVelocity": 0.17277196880933401, + "velocityX": 2.081905061234573, + "velocityY": 0.0536372928120376, + "timestamp": 1.0223516940754278 + }, + { + "x": 2.6785482690846774, + "y": 7.035644367883773, + "heading": 3.5678296659091027, + "angularVelocity": 0.15359788654883516, + "velocityX": 1.909431115585295, + "velocityY": 0.03351572076550796, + "timestamp": 1.0380790812194705 + }, + { + "x": 2.7058580326316437, + "y": 7.035929166009351, + "heading": 3.5699662569746473, + "angularVelocity": 0.1358516227760129, + "velocityX": 1.7364463211113188, + "velocityY": 0.01810841959755818, + "timestamp": 1.053806468363513 + }, + { + "x": 2.730442214470581, + "y": 7.036031594182232, + "heading": 3.571841977675455, + "angularVelocity": 0.11926461042947216, + "velocityX": 1.56314469872061, + "velocityY": 0.006512726617774275, + "timestamp": 1.0695338555075558 + }, + { + "x": 2.752297719343406, + "y": 7.036001055460871, + "heading": 3.573471906610007, + "angularVelocity": 0.10363634592473486, + "velocityX": 1.3896462694442941, + "velocityY": -0.001941754283852309, + "timestamp": 1.0852612426515984 + }, + { + "x": 2.7714226555411328, + "y": 7.035878927369517, + "heading": 3.5748687113578734, + "angularVelocity": 0.08881352859654948, + "velocityX": 1.216027558968734, + "velocityY": -0.00776531347745605, + "timestamp": 1.100988629795641 + }, + { + "x": 2.787815917152703, + "y": 7.035700347508605, + "heading": 3.576043177627596, + "angularVelocity": 0.0746765027760899, + "velocityX": 1.042338530960605, + "velocityY": -0.011354706237983625, + "timestamp": 1.1167160169396837 + }, + { + "x": 2.801476920645093, + "y": 7.035495511762398, + "heading": 3.57700459568957, + "angularVelocity": 0.06113018349256862, + "velocityX": 0.8686123999665686, + "velocityY": -0.013024143446794291, + "timestamp": 1.1324434040837263 + }, + { + "x": 2.8124054342268368, + "y": 7.035290639091272, + "heading": 3.5777610486984153, + "angularVelocity": 0.04809781827819563, + "velocityX": 0.6948715308939885, + "velocityY": -0.013026491256922158, + "timestamp": 1.148170791227769 + }, + { + "x": 2.82060146478212, + "y": 7.035108702556254, + "heading": 3.5783196319263704, + "angularVelocity": 0.03551659425937697, + "velocityX": 0.5211310995410608, + "velocityY": -0.011568134830788418, + "timestamp": 1.1638981783718116 + }, + { + "x": 2.8260651815141027, + "y": 7.034969992798228, + "heading": 3.5786866222858342, + "angularVelocity": 0.023334477373928582, + "velocityX": 0.347401426692329, + "velocityY": -0.008819631433772254, + "timestamp": 1.1796255655158543 + }, + { + "x": 2.828796863555908, + "y": 7.034892559051514, + "heading": 3.57886761138801, + "angularVelocity": 0.011507893874428679, + "velocityX": 0.1736895020633055, + "velocityY": -0.004923497209367752, + "timestamp": 1.195352952659897 + }, + { + "x": 2.828796863555908, + "y": 7.034892559051514, + "heading": 3.57886761138801, + "angularVelocity": 2.338404634182572e-38, + "velocityX": -5.320180341501415e-40, + "velocityY": -3.3698131372568e-38, + "timestamp": 1.2110803398039396 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/BA123.3.traj b/src/main/deploy/choreo/BA123.3.traj new file mode 100644 index 0000000..3d0597a --- /dev/null +++ b/src/main/deploy/choreo/BA123.3.traj @@ -0,0 +1,175 @@ +{ + "samples": [ + { + "x": 2.828796863555908, + "y": 7.034892559051514, + "heading": 3.57886761138801, + "angularVelocity": 2.338404634182572e-38, + "velocityX": -5.320180341501415e-40, + "velocityY": -3.3698131372568e-38, + "timestamp": 0 + }, + { + "x": 2.9084775174891906, + "y": 7.041040428082855, + "heading": 3.5613533790014733, + "angularVelocity": -0.2055483996105686, + "velocityX": 0.9351383797156885, + "velocityY": 0.07215187126209009, + "timestamp": 0.08520734007036235 + }, + { + "x": 3.0678383714410953, + "y": 7.053335880908181, + "heading": 3.526306324951356, + "angularVelocity": -0.41131496442884247, + "velocityX": 1.8702714322534668, + "velocityY": 0.14430039495625907, + "timestamp": 0.1704146801407247 + }, + { + "x": 3.3068786906868026, + "y": 7.0717782673609095, + "heading": 3.473697273457424, + "angularVelocity": -0.6174239384833393, + "velocityX": 2.805395862003363, + "velocityY": 0.21644128824465056, + "timestamp": 0.2556220202110868 + }, + { + "x": 3.6255976049312144, + "y": 7.096366357645069, + "heading": 3.4034914541196217, + "angularVelocity": -0.8239409806693618, + "velocityX": 3.74051007790198, + "velocityY": 0.28856774855141604, + "timestamp": 0.34082936028144895 + }, + { + "x": 4.006634355263943, + "y": 7.12573524545498, + "heading": 3.403491430543163, + "angularVelocity": -2.766951616622872e-7, + "velocityX": 4.471877070896435, + "velocityY": 0.34467556181966225, + "timestamp": 0.42603670035181107 + }, + { + "x": 4.387671106257462, + "y": 7.155104124693663, + "heading": 3.4034914069671127, + "angularVelocity": -2.766903705713447e-7, + "velocityX": 4.471877078651531, + "velocityY": 0.3446754612270608, + "timestamp": 0.5112440404221732 + }, + { + "x": 4.768707857250984, + "y": 7.184473003932312, + "heading": 3.4034913833910623, + "angularVelocity": -2.7669037102811926e-7, + "velocityX": 4.471877078651563, + "velocityY": 0.3446754612266613, + "timestamp": 0.5964513804925353 + }, + { + "x": 5.149744608244506, + "y": 7.213841883170961, + "heading": 3.4034913598150114, + "angularVelocity": -2.766903719221037e-7, + "velocityX": 4.471877078651562, + "velocityY": 0.3446754612266611, + "timestamp": 0.6816587205628974 + }, + { + "x": 5.530781359238029, + "y": 7.2432107624096105, + "heading": 3.403491336238961, + "angularVelocity": -2.7669037233593665e-7, + "velocityX": 4.471877078651563, + "velocityY": 0.34467546122666126, + "timestamp": 0.7668660606332596 + }, + { + "x": 5.91181811023155, + "y": 7.272579641648259, + "heading": 3.4034913126629105, + "angularVelocity": -2.766903719198649e-7, + "velocityX": 4.471877078651562, + "velocityY": 0.3446754612266609, + "timestamp": 0.8520734007036217 + }, + { + "x": 6.292854861225073, + "y": 7.301948520886908, + "heading": 3.40349128908686, + "angularVelocity": -2.766903702987286e-7, + "velocityX": 4.471877078651562, + "velocityY": 0.3446754612266614, + "timestamp": 0.9372807407739838 + }, + { + "x": 6.673891612218593, + "y": 7.331317400125578, + "heading": 3.4034912655108096, + "angularVelocity": -2.7669037110747106e-7, + "velocityX": 4.471877078651543, + "velocityY": 0.3446754612269092, + "timestamp": 1.022488080844346 + }, + { + "x": 7.054928362802186, + "y": 7.360686284680735, + "heading": 3.4034912419343524, + "angularVelocity": -2.7669514250103454e-7, + "velocityX": 4.471877073840595, + "velocityY": 0.34467552362161347, + "timestamp": 1.107695420914708 + }, + { + "x": 7.373639419387511, + "y": 7.385254037823193, + "heading": 3.3329373560097224, + "angularVelocity": -0.8280259173255274, + "velocityX": 3.7404178598010502, + "velocityY": 0.2883290702675442, + "timestamp": 1.1929027609850702 + }, + { + "x": 7.612672703715564, + "y": 7.403678948176451, + "heading": 3.2800183747872396, + "angularVelocity": -0.6210612979912687, + "velocityX": 2.8053132996601575, + "velocityY": 0.21623618737591582, + "timestamp": 1.2781101010554323 + }, + { + "x": 7.772028248697012, + "y": 7.415961919015887, + "heading": 3.244738714983509, + "angularVelocity": -0.4140448437258724, + "velocityX": 1.8702091257614122, + "velocityY": 0.14415390539467693, + "timestamp": 1.3633174411257944 + }, + { + "x": 7.851706027984619, + "y": 7.422103404998779, + "heading": 3.2270990200422704, + "angularVelocity": -0.20702083795448067, + "velocityX": 0.9351046426494596, + "velocityY": 0.07207695930680914, + "timestamp": 1.4485247811961566 + }, + { + "x": 7.851706027984619, + "y": 7.422103404998779, + "heading": 3.2270990200422704, + "angularVelocity": 2.395667816010299e-38, + "velocityX": 2.50451146033937e-40, + "velocityY": 2.3151739947273847e-39, + "timestamp": 1.5337321212665187 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/BA123.4.traj b/src/main/deploy/choreo/BA123.4.traj new file mode 100644 index 0000000..32093d3 --- /dev/null +++ b/src/main/deploy/choreo/BA123.4.traj @@ -0,0 +1,112 @@ +{ + "samples": [ + { + "x": 7.851706027984619, + "y": 7.422103404998779, + "heading": 3.2270990200422704, + "angularVelocity": 2.395667816010299e-38, + "velocityX": 2.50451146033937e-40, + "velocityY": 2.3151739947273847e-39, + "timestamp": 0 + }, + { + "x": 7.7535783430697425, + "y": 7.402064102143109, + "heading": 3.232020757481131, + "angularVelocity": 0.05169742228556919, + "velocityX": -1.030723078580944, + "velocityY": -0.2104907697550398, + "timestamp": 0.09520276294771124 + }, + { + "x": 7.557322977332363, + "y": 7.361985496490512, + "heading": 3.241864259728923, + "angularVelocity": 0.1033951320635361, + "velocityX": -2.061446114176018, + "velocityY": -0.4209815388930545, + "timestamp": 0.19040552589542248 + }, + { + "x": 7.26293993880598, + "y": 7.301867588491817, + "heading": 3.256629564983045, + "angularVelocity": 0.15509324306303482, + "velocityX": -3.0921690653880356, + "velocityY": -0.6314723032955863, + "timestamp": 0.2856082888431337 + }, + { + "x": 6.870429250696279, + "y": 7.2217103812612065, + "heading": 3.276316699257686, + "angularVelocity": 0.20679162731289896, + "velocityX": -4.122891772849927, + "velocityY": -0.8419630349870816, + "timestamp": 0.38081105179084496 + }, + { + "x": 6.452066171420709, + "y": 7.136273653088981, + "heading": 3.276316704861539, + "angularVelocity": 5.886229324112331e-8, + "velocityX": -4.394442622483034, + "velocityY": -0.8974185782732974, + "timestamp": 0.4760138147385562 + }, + { + "x": 6.033703092145166, + "y": 7.050836924916623, + "heading": 3.2763167104653923, + "angularVelocity": 5.886229546144486e-8, + "velocityX": -4.394442622482746, + "velocityY": -0.8974185782747038, + "timestamp": 0.5712165776862674 + }, + { + "x": 5.641192412144091, + "y": 6.970679696216733, + "heading": 3.2960044413177485, + "angularVelocity": 0.20679789370366783, + "velocityX": -4.122891687677771, + "velocityY": -0.8419632604981744, + "timestamp": 0.6664193406339782 + }, + { + "x": 5.346809380757658, + "y": 6.910561769475596, + "heading": 3.310770277891182, + "angularVelocity": 0.15509882398626432, + "velocityX": -3.0921689903907406, + "velocityY": -0.6314725001642677, + "timestamp": 0.761622103581689 + }, + { + "x": 5.150554020304434, + "y": 6.87048315009937, + "heading": 3.3206141794938233, + "angularVelocity": 0.10339932684567515, + "velocityX": -2.0614460586717827, + "velocityY": -0.42098168304462, + "timestamp": 0.8568248665293998 + }, + { + "x": 5.052426338195801, + "y": 6.8504438400268555, + "heading": 3.3255361324496757, + "angularVelocity": 0.05169968605381748, + "velocityX": -1.0307230491044557, + "velocityY": -0.21049084556003192, + "timestamp": 0.9520276294771106 + }, + { + "x": 5.052426338195801, + "y": 6.8504438400268555, + "heading": 3.3255361324496757, + "angularVelocity": -3.315402276831609e-39, + "velocityX": -2.4414050222730828e-37, + "velocityY": -1.3667234180646677e-37, + "timestamp": 1.0472303924248214 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/BA123.5.traj b/src/main/deploy/choreo/BA123.5.traj new file mode 100644 index 0000000..5ae10c0 --- /dev/null +++ b/src/main/deploy/choreo/BA123.5.traj @@ -0,0 +1,130 @@ +{ + "samples": [ + { + "x": 5.052426338195801, + "y": 6.8504438400268555, + "heading": 3.3255361324496757, + "angularVelocity": -3.315402276831609e-39, + "velocityX": -2.4414050222730828e-37, + "velocityY": -1.3667234180646677e-37, + "timestamp": 0 + }, + { + "x": 5.125592947632546, + "y": 6.825900629691664, + "heading": 3.3051967120755084, + "angularVelocity": -0.24269122019378206, + "velocityX": 0.8730285030244984, + "velocityY": -0.2928511016609534, + "timestamp": 0.08380781289874051 + }, + { + "x": 5.271925300123515, + "y": 6.776815141553044, + "heading": 3.2644814434878833, + "angularVelocity": -0.4858170996159881, + "velocityX": 1.746046668319264, + "velocityY": -0.5856910762953288, + "timestamp": 0.16761562579748102 + }, + { + "x": 5.4914215364585965, + "y": 6.703188291356257, + "heading": 3.203326609050582, + "angularVelocity": -0.7297032618091427, + "velocityX": 2.619042649404126, + "velocityY": -0.8785201242006593, + "timestamp": 0.2514234386962215 + }, + { + "x": 5.7840786491345275, + "y": 6.605020184452078, + "heading": 3.1216446060354786, + "angularVelocity": -0.9746347051651971, + "velocityX": 3.4920027447742963, + "velocityY": -1.1713479150540527, + "timestamp": 0.33523125159496203 + }, + { + "x": 6.140433700365838, + "y": 6.485419873586866, + "heading": 3.121644578142224, + "angularVelocity": -3.32824030604796e-7, + "velocityX": 4.252050481998292, + "velocityY": -1.427078296503421, + "timestamp": 0.41903906449370254 + }, + { + "x": 6.496788713608013, + "y": 6.36581944952913, + "heading": 3.1216445502509234, + "angularVelocity": -3.3280072167583106e-7, + "velocityX": 4.252050028709587, + "velocityY": -1.4270796471236313, + "timestamp": 0.502846877392443 + }, + { + "x": 6.853143726850153, + "y": 6.246219025471286, + "heading": 3.1216445223596225, + "angularVelocity": -3.3280072190686506e-7, + "velocityX": 4.252050028709163, + "velocityY": -1.4270796471248925, + "timestamp": 0.5866546902911836 + }, + { + "x": 7.20949876939429, + "y": 6.126618688722239, + "heading": 3.1216444944663726, + "angularVelocity": -3.3282397956897397e-7, + "velocityX": 4.252050378342396, + "velocityY": -1.4270786053509494, + "timestamp": 0.6704625031899241 + }, + { + "x": 7.502115016202162, + "y": 6.028435572271006, + "heading": 3.038925832747677, + "angularVelocity": -0.9870041808469556, + "velocityX": 3.4915151307124925, + "velocityY": -1.1715270098965787, + "timestamp": 0.7542703160886646 + }, + { + "x": 7.721575871144278, + "y": 5.954796687958475, + "heading": 2.9768631098050315, + "angularVelocity": -0.740536243531756, + "velocityX": 2.6186204764378944, + "velocityY": -0.8786637160130223, + "timestamp": 0.8380781289874051 + }, + { + "x": 7.867882744457675, + "y": 5.9057035200132155, + "heading": 2.935482546952977, + "angularVelocity": -0.4937554318718741, + "velocityX": 1.7457426491987094, + "velocityY": -0.5857827122224969, + "timestamp": 0.9218859418861456 + }, + { + "x": 7.941036224365234, + "y": 5.881156921386719, + "heading": 2.91479324575467, + "angularVelocity": -0.24686601979823689, + "velocityX": 0.8728718406713037, + "velocityY": -0.29289153096208925, + "timestamp": 1.005693754784886 + }, + { + "x": 7.941036224365234, + "y": 5.881156921386719, + "heading": 2.91479324575467, + "angularVelocity": -8.307370174213132e-35, + "velocityX": -3.1349419451622814e-36, + "velocityY": -3.3120539075020083e-35, + "timestamp": 1.0895015676836266 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/BA123.6.traj b/src/main/deploy/choreo/BA123.6.traj new file mode 100644 index 0000000..d9ac49d --- /dev/null +++ b/src/main/deploy/choreo/BA123.6.traj @@ -0,0 +1,130 @@ +{ + "samples": [ + { + "x": 7.941036224365234, + "y": 5.881156921386719, + "heading": 2.91479324575467, + "angularVelocity": -8.307370174213132e-35, + "velocityX": -3.1349419451622814e-36, + "velocityY": -3.3120539075020083e-35, + "timestamp": 0 + }, + { + "x": 7.8675727783640985, + "y": 5.90546036695194, + "heading": 2.935525873781026, + "angularVelocity": 0.24703644978061876, + "velocityX": -0.8753424247857141, + "velocityY": 0.28958397855960943, + "timestamp": 0.0839253804236888 + }, + { + "x": 7.720644724842832, + "y": 5.954065645656709, + "heading": 2.9769714499394753, + "angularVelocity": 0.4938384067991731, + "velocityX": -1.7506986894729044, + "velocityY": 0.5791487445084075, + "timestamp": 0.1678507608473776 + }, + { + "x": 7.5002499438728485, + "y": 6.026970898531862, + "heading": 3.0390854497042903, + "angularVelocity": 0.7401098386595186, + "velocityX": -2.6260802138440438, + "velocityY": 0.8686913601951816, + "timestamp": 0.2517761412710664 + }, + { + "x": 7.206385265272288, + "y": 6.1241748112836225, + "heading": 3.121788392181331, + "angularVelocity": 0.9854342281145816, + "velocityX": -3.501499512030981, + "velocityY": 1.1582183156160495, + "timestamp": 0.3357015216947552 + }, + { + "x": 6.849025327552005, + "y": 6.242427627098492, + "heading": 3.121788420122234, + "angularVelocity": 3.3292554576834236e-7, + "velocityX": -4.258067534709846, + "velocityY": 1.4090232920945023, + "timestamp": 0.419626902118444 + }, + { + "x": 6.4916654234974125, + "y": 6.360680544652888, + "heading": 3.1217884480614018, + "angularVelocity": 3.3290486828329673e-7, + "velocityX": -4.258067133571475, + "velocityY": 1.409024504356219, + "timestamp": 0.5035522825421328 + }, + { + "x": 6.134305519442798, + "y": 6.478933462207215, + "heading": 3.1217884760005696, + "angularVelocity": 3.329048698919558e-7, + "velocityX": -4.258067133571742, + "velocityY": 1.4090245043554115, + "timestamp": 0.5874776629658216 + }, + { + "x": 5.7769455762145565, + "y": 6.597186261377052, + "heading": 3.1217885039414885, + "angularVelocity": 3.3292573662864275e-7, + "velocityX": -4.258067600339077, + "velocityY": 1.4090230937631703, + "timestamp": 0.6714030433895104 + }, + { + "x": 5.483040254012964, + "y": 6.69438933225271, + "heading": 3.2033287309225806, + "angularVelocity": 0.9715800699316991, + "velocityX": -3.5019837946261267, + "velocityY": 1.1582082843704404, + "timestamp": 0.7553284238131992 + }, + { + "x": 5.26261008660972, + "y": 6.7672927025778, + "heading": 3.264440440566138, + "angularVelocity": 0.7281672044266055, + "velocityX": -2.6265018554628505, + "velocityY": 0.8686689289586099, + "timestamp": 0.839253804236888 + }, + { + "x": 5.115656365698194, + "y": 6.815895328669379, + "heading": 3.3051694863922827, + "angularVelocity": 0.4853006995086357, + "velocityX": -1.7510045253252935, + "velocityY": 0.579117137702728, + "timestamp": 0.9231791846605768 + }, + { + "x": 5.042179584503174, + "y": 6.84019660949707, + "heading": 3.325535953868165, + "angularVelocity": 0.24267351989426952, + "velocityX": -0.8755013182434166, + "velocityY": 0.2895581849615564, + "timestamp": 1.0071045650842656 + }, + { + "x": 5.042179584503174, + "y": 6.84019660949707, + "heading": 3.325535953868165, + "angularVelocity": 0, + "velocityX": 1.3923094056611315e-34, + "velocityY": 2.5569606606573608e-34, + "timestamp": 1.0910299455079544 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/BA123.7.traj b/src/main/deploy/choreo/BA123.7.traj new file mode 100644 index 0000000..1a3319f --- /dev/null +++ b/src/main/deploy/choreo/BA123.7.traj @@ -0,0 +1,211 @@ +{ + "samples": [ + { + "x": 5.042179584503174, + "y": 6.84019660949707, + "heading": 3.325535953868165, + "angularVelocity": 0, + "velocityX": 1.3923094056611315e-34, + "velocityY": 2.5569606606573608e-34, + "timestamp": 0 + }, + { + "x": 5.073880207055287, + "y": 6.82779809617603, + "heading": 3.305483362781089, + "angularVelocity": -0.3561840736590214, + "velocityX": 0.5630821886861347, + "velocityY": -0.2202285461677885, + "timestamp": 0.05629839335902531 + }, + { + "x": 5.1373374485970675, + "y": 6.802993369110707, + "heading": 3.265889881043017, + "angularVelocity": -0.7032790702494384, + "velocityX": 1.1271590138834517, + "velocityY": -0.44059387107441605, + "timestamp": 0.11259678671805062 + }, + { + "x": 5.232628574600268, + "y": 6.765767526946016, + "heading": 3.207512854637883, + "angularVelocity": -1.0369217116526357, + "velocityX": 1.6926082667317153, + "velocityY": -0.6612238812446238, + "timestamp": 0.16889518007707593 + }, + { + "x": 5.35987690908377, + "y": 6.716091063237897, + "heading": 3.131715460945362, + "angularVelocity": -1.346350919983584, + "velocityX": 2.2602480620009318, + "velocityY": -0.882377999516305, + "timestamp": 0.22519357343610125 + }, + { + "x": 5.519307136483051, + "y": 6.653925120393696, + "heading": 3.0413668880518268, + "angularVelocity": -1.6048161857366978, + "velocityX": 2.831878813709029, + "velocityY": -1.104222325631158, + "timestamp": 0.28149196679512656 + }, + { + "x": 5.711343989073416, + "y": 6.579302532140423, + "heading": 2.943828403584053, + "angularVelocity": -1.7325269629943616, + "velocityX": 3.411053870857564, + "velocityY": -1.325483442793676, + "timestamp": 0.33779036015415187 + }, + { + "x": 5.932593273975757, + "y": 6.495118363659999, + "heading": 2.890818284301493, + "angularVelocity": -0.94159204410159, + "velocityX": 3.9299395897746816, + "velocityY": -1.4953209755661327, + "timestamp": 0.3940887535131772 + }, + { + "x": 6.167130335408207, + "y": 6.401567639682382, + "heading": 2.8908181970163076, + "angularVelocity": -0.0000015504027732778302, + "velocityX": 4.165963670344261, + "velocityY": -1.6616943823072647, + "timestamp": 0.4503871468722025 + }, + { + "x": 6.392413339713847, + "y": 6.287519672451602, + "heading": 2.890818109850187, + "angularVelocity": -0.0000015482878884317716, + "velocityX": 4.001588515483356, + "velocityY": -2.0257765883917123, + "timestamp": 0.5066855402312278 + }, + { + "x": 6.59974479675293, + "y": 6.143391132354736, + "heading": 2.890818017389062, + "angularVelocity": -0.0000016423403859213353, + "velocityX": 3.6827242247730267, + "velocityY": -2.5600826506314496, + "timestamp": 0.5629839335902531 + }, + { + "x": 6.802098168556436, + "y": 5.950611470632589, + "heading": 2.8908179477513425, + "angularVelocity": -0.0000011175444046256971, + "velocityX": 3.2473619220198997, + "velocityY": -3.093723258657997, + "timestamp": 0.6252970901347634 + }, + { + "x": 6.972552082866989, + "y": 5.729124789979766, + "heading": 2.8908178853871664, + "angularVelocity": -0.0000010008187654827336, + "velocityX": 2.735440214600562, + "velocityY": -3.554412790734118, + "timestamp": 0.6876102466792737 + }, + { + "x": 7.131045654507235, + "y": 5.498927755764986, + "heading": 2.8908178239788898, + "angularVelocity": -9.854785101241336e-7, + "velocityX": 2.5435009302896496, + "velocityY": -3.694196329957235, + "timestamp": 0.749923403223784 + }, + { + "x": 7.289539085272484, + "y": 5.268730624556049, + "heading": 2.890817762570617, + "angularVelocity": -9.85478446229405e-7, + "velocityX": 2.5434986695311594, + "velocityY": -3.694197886517073, + "timestamp": 0.8122365597682943 + }, + { + "x": 7.448032587450016, + "y": 5.038533542515831, + "heading": 2.89081770116102, + "angularVelocity": -9.854996960990413e-7, + "velocityX": 2.5434998155537434, + "velocityY": -3.6941970974587077, + "timestamp": 0.8745497163128046 + }, + { + "x": 7.592777439099975, + "y": 4.829594632078275, + "heading": 2.8224347675081725, + "angularVelocity": -1.0974076333944311, + "velocityX": 2.32286181083715, + "velocityY": -3.353046483663694, + "timestamp": 0.936862872857315 + }, + { + "x": 7.713212504447541, + "y": 4.655502027335128, + "heading": 2.739432491996474, + "angularVelocity": -1.332018471129938, + "velocityX": 1.9327389595733324, + "velocityY": -2.793833828957021, + "timestamp": 0.9991760294018253 + }, + { + "x": 7.8093302684945165, + "y": 4.516554189275233, + "heading": 2.6658856506758575, + "angularVelocity": -1.1802778963393161, + "velocityX": 1.5424955078037048, + "velocityY": -2.2298314796594534, + "timestamp": 1.0614891859463356 + }, + { + "x": 7.881306607138764, + "y": 4.412523050354705, + "heading": 2.607642999795528, + "angularVelocity": -0.9346766254527145, + "velocityX": 1.1550745081070457, + "velocityY": -1.669489152683501, + "timestamp": 1.1238023424908459 + }, + { + "x": 7.929238314866953, + "y": 4.343258360969765, + "heading": 2.56743105109566, + "angularVelocity": -0.6453203613773755, + "velocityX": 0.7692068639461747, + "velocityY": -1.1115580276448371, + "timestamp": 1.1861154990353562 + }, + { + "x": 7.953185081481934, + "y": 4.30866003036499, + "heading": 2.5468332187039406, + "angularVelocity": -0.33055350641732584, + "velocityX": 0.3842971202698623, + "velocityY": -0.5552331565816471, + "timestamp": 1.2484286555798665 + }, + { + "x": 7.953185081481934, + "y": 4.30866003036499, + "heading": 2.5468332187039406, + "angularVelocity": 1.05622156239882e-34, + "velocityX": 2.693478782190868e-36, + "velocityY": -1.766740346712715e-35, + "timestamp": 1.3107418121243768 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/BA123.8.traj b/src/main/deploy/choreo/BA123.8.traj new file mode 100644 index 0000000..0ffadd4 --- /dev/null +++ b/src/main/deploy/choreo/BA123.8.traj @@ -0,0 +1,211 @@ +{ + "samples": [ + { + "x": 7.953185081481934, + "y": 4.30866003036499, + "heading": 2.5468332187039406, + "angularVelocity": 1.05622156239882e-34, + "velocityX": 2.693478782190868e-36, + "velocityY": -1.766740346712715e-35, + "timestamp": 0 + }, + { + "x": 7.933444800519874, + "y": 4.337679051528667, + "heading": 2.567386408846941, + "angularVelocity": 0.3596766726497821, + "velocityX": -0.34545092631390906, + "velocityY": 0.5078270041334514, + "timestamp": 0.05714351723614097 + }, + { + "x": 7.8939272389266275, + "y": 4.3958144902629535, + "heading": 2.6073640293286684, + "angularVelocity": 0.6996002769049587, + "velocityX": -0.69154933935801, + "velocityY": 1.017358425699386, + "timestamp": 0.11428703447228195 + }, + { + "x": 7.834584299123902, + "y": 4.483196881040095, + "heading": 2.665183110722398, + "angularVelocity": 1.0118222361916747, + "velocityX": -1.038489450299254, + "velocityY": 1.529174174141952, + "timestamp": 0.17143055170842203 + }, + { + "x": 7.755346884542635, + "y": 4.599998807677701, + "heading": 2.738526660809834, + "angularVelocity": 1.2834973000410625, + "velocityX": -1.3866387372311242, + "velocityY": 2.0440101045046273, + "timestamp": 0.22857406894456211 + }, + { + "x": 7.656097596776391, + "y": 4.746453323880735, + "heading": 2.8233938773370038, + "angularVelocity": 1.4851591332130114, + "velocityX": -1.7368424725433986, + "velocityY": 2.562924427592089, + "timestamp": 0.2857175861807022 + }, + { + "x": 7.536677822319239, + "y": 4.922827818699696, + "heading": 2.909865886749634, + "angularVelocity": 1.513242684297716, + "velocityX": -2.089821912145504, + "velocityY": 3.086518005010237, + "timestamp": 0.3428611034168423 + }, + { + "x": 7.400726981561679, + "y": 5.123361927462954, + "heading": 2.9474259118798702, + "angularVelocity": 0.657292846973742, + "velocityX": -2.3791122306272374, + "velocityY": 3.5093063651396728, + "timestamp": 0.40000462065298237 + }, + { + "x": 7.25730486839932, + "y": 5.335772109492621, + "heading": 2.9474259955604354, + "angularVelocity": 0.0000014643929737205378, + "velocityX": -2.509857987384268, + "velocityY": 3.7171352465390086, + "timestamp": 0.45714813788912245 + }, + { + "x": 7.113882763503483, + "y": 5.548182297104135, + "heading": 2.9474260792405436, + "angularVelocity": 0.0000014643849797763275, + "velocityX": -2.509857842721803, + "velocityY": 3.717135344220187, + "timestamp": 0.5142916551252625 + }, + { + "x": 6.970460146387983, + "y": 5.760592138857399, + "heading": 2.9474261629206717, + "angularVelocity": 0.0000014643853274041011, + "velocityX": -2.50986680646239, + "velocityY": 3.7171292917707364, + "timestamp": 0.5714351723614026 + }, + { + "x": 6.808497246757101, + "y": 5.959227499928339, + "heading": 2.947426248883836, + "angularVelocity": 0.000001504337989820676, + "velocityX": -2.8343180025405768, + "velocityY": 3.4760786643583015, + "timestamp": 0.6285786895975427 + }, + { + "x": 6.6202392578125, + "y": 6.133143901824951, + "heading": 2.947426346913631, + "angularVelocity": 0.0000017155015966502543, + "velocityX": -3.2944767499459546, + "velocityY": 3.043501875775644, + "timestamp": 0.6857222068336828 + }, + { + "x": 6.387030430534219, + "y": 6.28974049469131, + "heading": 2.9474264250351503, + "angularVelocity": 0.000001247337128965759, + "velocityX": -3.7235582712366577, + "velocityY": 2.500319329333199, + "timestamp": 0.7483528440603777 + }, + { + "x": 6.132503069560848, + "y": 6.40858776606532, + "heading": 2.9474265000949957, + "angularVelocity": 0.0000011984525335388143, + "velocityX": -4.063943338978, + "velocityY": 1.8975900076481367, + "timestamp": 0.8109834812870726 + }, + { + "x": 5.871983809603483, + "y": 6.513652349670096, + "heading": 2.9474265754258733, + "angularVelocity": 0.0000012027799908761018, + "velocityX": -4.159613752841165, + "velocityY": 1.6775269781223834, + "timestamp": 0.8736141185137676 + }, + { + "x": 5.633984277453578, + "y": 6.60624851988278, + "heading": 3.0254965551351023, + "angularVelocity": 1.2465142167826126, + "velocityX": -3.800049667201303, + "velocityY": 1.4784484768617097, + "timestamp": 0.9362447557404625 + }, + { + "x": 5.4360193740701925, + "y": 6.684287207108514, + "heading": 3.1232338980771237, + "angularVelocity": 1.5605356622551558, + "velocityX": -3.160831697541927, + "velocityY": 1.2460145813823962, + "timestamp": 0.9988753929671574 + }, + { + "x": 5.278191507828268, + "y": 6.7466944701474745, + "heading": 3.209038101470214, + "angularVelocity": 1.3700036785913317, + "velocityX": -2.5199786115964247, + "velocityY": 0.9964334677463785, + "timestamp": 1.0615060301938524 + }, + { + "x": 5.160074587729763, + "y": 6.7934662468242815, + "heading": 3.2763629711209266, + "angularVelocity": 1.0749510564139328, + "velocityX": -1.8859287615257068, + "velocityY": 0.7467874948727645, + "timestamp": 1.1241366674205473 + }, + { + "x": 5.081447933998166, + "y": 6.824627137180405, + "heading": 3.322538510654522, + "angularVelocity": 0.7372675990260279, + "velocityX": -1.2554024230506353, + "velocityY": 0.4975343016762813, + "timestamp": 1.1867673046472422 + }, + { + "x": 5.042179584503174, + "y": 6.84019660949707, + "heading": 3.346115957859533, + "angularVelocity": 0.3764522963365605, + "velocityX": -0.6269830746389843, + "velocityY": 0.2485919512571829, + "timestamp": 1.2493979418739372 + }, + { + "x": 5.042179584503174, + "y": 6.84019660949707, + "heading": 3.346115957859533, + "angularVelocity": 3.173544538115536e-35, + "velocityX": -2.8108210003831088e-36, + "velocityY": 9.453581075103361e-36, + "timestamp": 1.312028579100632 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/BA123.9.traj b/src/main/deploy/choreo/BA123.9.traj new file mode 100644 index 0000000..68658a3 --- /dev/null +++ b/src/main/deploy/choreo/BA123.9.traj @@ -0,0 +1,13 @@ +{ + "samples": [ + { + "x": 5.042179584503174, + "y": 6.84019660949707, + "heading": 3.346115957859533, + "angularVelocity": 3.173544538115536e-35, + "velocityX": -2.8108210003831088e-36, + "velocityY": 9.453581075103361e-36, + "timestamp": 0 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/BA123.traj b/src/main/deploy/choreo/BA123.traj new file mode 100644 index 0000000..c2303d3 --- /dev/null +++ b/src/main/deploy/choreo/BA123.traj @@ -0,0 +1,1633 @@ +{ + "samples": [ + { + "x": 1.483002781867981, + "y": 5.5289692878723145, + "heading": 3.141592653589793, + "angularVelocity": 4.804188938729379e-42, + "velocityX": -1.0059577979228262e-42, + "velocityY": 6.365083529423221e-42, + "timestamp": 0 + }, + { + "x": 1.5755119282180663, + "y": 5.536620736608523, + "heading": 3.1400776717900736, + "angularVelocity": -0.016530919629225094, + "velocityX": 1.009425501722026, + "velocityY": 0.08348977138129623, + "timestamp": 0.0916453430117127 + }, + { + "x": 1.7605302151370883, + "y": 5.551923633600031, + "heading": 3.1370477085137116, + "angularVelocity": -0.03306183573315699, + "velocityX": 2.0188509403623054, + "velocityY": 0.16697953751509448, + "timestamp": 0.1832906860234254 + }, + { + "x": 2.038057625280542, + "y": 5.57487797740817, + "heading": 3.1325027643437484, + "angularVelocity": -0.04959274547515668, + "velocityX": 3.0282761897457733, + "velocityY": 0.25046928795068285, + "timestamp": 0.27493602903513814 + }, + { + "x": 2.3155850354358676, + "y": 5.597832321183343, + "heading": 3.1279578246361175, + "angularVelocity": -0.0495926967838387, + "velocityX": 3.0282761898753168, + "velocityY": 0.2504692875909585, + "timestamp": 0.3665813720468508 + }, + { + "x": 2.500603322364312, + "y": 5.613135218148452, + "heading": 3.1249278648972543, + "angularVelocity": -0.033061797133284165, + "velocityX": 2.018850940465115, + "velocityY": 0.16697953722703365, + "timestamp": 0.4582267150585635 + }, + { + "x": 2.5931124687194824, + "y": 5.620786666870117, + "heading": 3.1234128850040452, + "angularVelocity": -0.01653089882609372, + "velocityX": 1.0094255017775111, + "velocityY": 0.08348977122260563, + "timestamp": 0.5498720580702762 + }, + { + "x": 2.5931124687194824, + "y": 5.620786666870117, + "heading": 3.1234128850040452, + "angularVelocity": 6.797549086666313e-39, + "velocityX": 1.0538796562779821e-38, + "velocityY": 4.37631798735499e-39, + "timestamp": 0.6415174010819888 + }, + { + "x": 2.5902191633660006, + "y": 5.622172308178305, + "heading": 3.1236231846500075, + "angularVelocity": 0.012341535996375805, + "velocityX": -0.16979501798548408, + "velocityY": 0.08131702744820536, + "timestamp": 0.658557390757831 + }, + { + "x": 2.5844404753484342, + "y": 5.624959867845441, + "heading": 3.1240494020264102, + "angularVelocity": 0.02501277198581982, + "velocityX": -0.33912508912835815, + "velocityY": 0.1635892814587969, + "timestamp": 0.6755973804336732 + }, + { + "x": 2.5757853965549486, + "y": 5.629167537767336, + "heading": 3.124697825770354, + "angularVelocity": 0.03805305967192536, + "velocityX": -0.5079274669840959, + "velocityY": 0.24692913563558913, + "timestamp": 0.6926373701095154 + }, + { + "x": 2.5642642159968103, + "y": 5.634815780702077, + "heading": 3.125575541744695, + "angularVelocity": 0.0515091846320203, + "velocityX": -0.6761260292588179, + "velocityY": 0.3314698566248499, + "timestamp": 0.7096773597853576 + }, + { + "x": 2.5498888162624387, + "y": 5.64192777941668, + "heading": 3.126690593435854, + "angularVelocity": 0.06543734546625592, + "velocityX": -0.843627256109882, + "velocityY": 0.4173710694605735, + "timestamp": 0.7267173494611998 + }, + { + "x": 2.5326730658717955, + "y": 5.650530011128654, + "heading": 3.1280521879604875, + "angularVelocity": 0.07990583037522597, + "velocityX": -1.0103146021884275, + "velocityY": 0.5048261105562488, + "timestamp": 0.743757339137042 + }, + { + "x": 2.512633348908364, + "y": 5.660652993489354, + "heading": 3.1296709648936383, + "angularVelocity": 0.09499870386926744, + "velocityX": -1.1760404404377187, + "velocityY": 0.5940720947179435, + "timestamp": 0.7607973288128842 + }, + { + "x": 2.489789296285424, + "y": 5.672332270018097, + "heading": 3.131559353416261, + "angularVelocity": 0.11082098983309922, + "velocityX": -1.3406142290876446, + "velocityY": 0.6854039674273565, + "timestamp": 0.7778373184887264 + }, + { + "x": 2.464164822040195, + "y": 5.685609737022312, + "heading": 3.133732056508738, + "angularVelocity": 0.12750612728111888, + "velocityX": -1.503784610947115, + "velocityY": 0.7791945451140279, + "timestamp": 0.7948773081645686 + }, + { + "x": 2.435789637049594, + "y": 5.7005354691556045, + "heading": 3.136206722659641, + "angularVelocity": 0.14522697477989688, + "velocityX": -1.6652113956869623, + "velocityY": 0.8759237779617546, + "timestamp": 0.8119172978404108 + }, + { + "x": 2.404701540197862, + "y": 5.717170292002654, + "heading": 3.1390049024308224, + "angularVelocity": 0.16421252737895475, + "velocityX": -1.8244199347025483, + "velocityY": 0.9762225895379132, + "timestamp": 0.828957287516253 + }, + { + "x": 2.370950035804977, + "y": 5.735589504313645, + "heading": 3.142153451841552, + "angularVelocity": 0.18477413840181903, + "velocityX": -1.9807232888606294, + "velocityY": 1.0809403445298984, + "timestamp": 0.8459972771920952 + }, + { + "x": 2.33460234106864, + "y": 5.755888415439336, + "heading": 3.145686661546861, + "angularVelocity": 0.20734811302834996, + "velocityX": -2.1330819694021232, + "velocityY": 1.1912513746688418, + "timestamp": 0.8630372668679374 + }, + { + "x": 2.2957539884735723, + "y": 5.778190797790987, + "heading": 3.149649608554467, + "angularVelocity": 0.23256745356039296, + "velocityX": -2.2798342800725706, + "velocityY": 1.3088260483672556, + "timestamp": 0.8800772565437796 + }, + { + "x": 2.2545489517182054, + "y": 5.802661947482074, + "heading": 3.154103636346379, + "angularVelocity": 0.26138676587500986, + "velocityX": -2.41813742491779, + "velocityY": 1.4361012040857848, + "timestamp": 0.8971172462196217 + }, + { + "x": 2.2112211753037756, + "y": 5.829528007071594, + "heading": 3.1591355992300465, + "angularVelocity": 0.29530316504839027, + "velocityX": -2.5427114240483375, + "velocityY": 1.5766476447816522, + "timestamp": 0.9141572358954639 + }, + { + "x": 2.166187163497284, + "y": 5.859096368777116, + "heading": 3.164873306208298, + "angularVelocity": 0.33672009710108897, + "velocityX": -2.642842669695792, + "velocityY": 1.7352335457950308, + "timestamp": 0.9311972255713061 + }, + { + "x": 2.1202483707966637, + "y": 5.891721818786653, + "heading": 3.171501741715199, + "angularVelocity": 0.3889929297491645, + "velocityX": -2.695940172179118, + "velocityY": 1.9146402451048057, + "timestamp": 0.9482372152471483 + }, + { + "x": 2.0748263621986163, + "y": 5.927485928507909, + "heading": 3.1791652437298565, + "angularVelocity": 0.4497363062092675, + "velocityX": -2.665612448254168, + "velocityY": 2.0988340017575653, + "timestamp": 0.9652772049229905 + }, + { + "x": 2.0314525713511236, + "y": 5.965692244208046, + "heading": 3.1877298171032367, + "angularVelocity": 0.5026161128209019, + "velocityX": -2.5454118032116115, + "velocityY": 2.2421560357106713, + "timestamp": 0.9823171945988327 + }, + { + "x": 1.990913443115171, + "y": 6.005377227458498, + "heading": 3.1969662928737232, + "angularVelocity": 0.5420470285601927, + "velocityX": -2.3790582627773333, + "velocityY": 2.32893235297639, + "timestamp": 0.9993571842746749 + }, + { + "x": 1.953477877153394, + "y": 6.045854258534539, + "heading": 3.2066830981783285, + "angularVelocity": 0.5702353985801418, + "velocityX": -2.196924216148433, + "velocityY": 2.3754140610440646, + "timestamp": 1.016397173950517 + }, + { + "x": 1.919228244872485, + "y": 6.086688482249977, + "heading": 3.2167506488100557, + "angularVelocity": 0.5908190570091756, + "velocityX": -2.009956163850568, + "velocityY": 2.3963760830987972, + "timestamp": 1.0334371636263593 + }, + { + "x": 1.888185091203371, + "y": 6.127595876679071, + "heading": 3.2270820659423944, + "angularVelocity": 0.6063041896665977, + "velocityX": -1.8217824223875096, + "velocityY": 2.40067014166615, + "timestamp": 1.0504771533022015 + }, + { + "x": 1.860347665740482, + "y": 6.1683807623094, + "heading": 3.2376165950788, + "angularVelocity": 0.6182239154370359, + "velocityX": -1.6336527188367116, + "velocityY": 2.3934806538145956, + "timestamp": 1.0675171429780437 + }, + { + "x": 1.835707853847385, + "y": 6.208901611745203, + "heading": 3.248309830617314, + "angularVelocity": 0.6275376770722889, + "velocityX": -1.4459992266327082, + "velocityY": 2.3779855625881225, + "timestamp": 1.0845571326538859 + }, + { + "x": 1.8142553226903924, + "y": 6.249051887575358, + "heading": 3.259128074403703, + "angularVelocity": 0.634873846298503, + "velocityX": -1.2589521217495907, + "velocityY": 2.3562382720851454, + "timestamp": 1.101597122329728 + }, + { + "x": 1.7959795058005092, + "y": 6.288748754453674, + "heading": 3.2700449558991145, + "angularVelocity": 0.6406624477530337, + "velocityX": -1.0725251151878987, + "velocityY": 2.3296297494002878, + "timestamp": 1.1186371120055703 + }, + { + "x": 1.7808703618932606, + "y": 6.3279260885864845, + "heading": 3.2810393135811178, + "angularVelocity": 0.6452091750730349, + "velocityX": -0.8866873862411462, + "velocityY": 2.2991407200412497, + "timestamp": 1.1356771016814124 + }, + { + "x": 1.7689186333603624, + "y": 6.366529951104452, + "heading": 3.2920938098527754, + "angularVelocity": 0.6487384371675794, + "velocityX": -0.7013929444947057, + "velocityY": 2.2654862621599268, + "timestamp": 1.1527170913572546 + }, + { + "x": 1.7601158958397196, + "y": 6.404515541778896, + "heading": 3.3031939914863275, + "angularVelocity": 0.6514195046308695, + "velocityX": -0.5165928904946825, + "velocityY": 2.229202681284329, + "timestamp": 1.1697570810330968 + }, + { + "x": 1.7544545221137668, + "y": 6.441845080083907, + "heading": 3.314327631669556, + "angularVelocity": 0.653383035731085, + "velocityX": -0.3322404434304942, + "velocityY": 2.1907019320518635, + "timestamp": 1.186797070708939 + }, + { + "x": 1.7519276141866935, + "y": 6.478486288821362, + "heading": 3.325484256418616, + "angularVelocity": 0.6547318960455363, + "velocityX": -0.14829280857227845, + "velocityY": 2.1503069799039145, + "timestamp": 1.2038370603847812 + }, + { + "x": 1.7525289273083395, + "y": 6.514411281966431, + "heading": 3.336654795480751, + "angularVelocity": 0.6555484642089668, + "velocityX": 0.03528835011552745, + "velocityY": 2.108275522959999, + "timestamp": 1.2208770500606234 + }, + { + "x": 1.7562527962036754, + "y": 6.5495957313476785, + "heading": 3.3478313196027307, + "angularVelocity": 0.6558997003281736, + "velocityX": 0.2185370394100181, + "velocityY": 2.064816355559754, + "timestamp": 1.2379170397364656 + }, + { + "x": 1.7630940675735474, + "y": 6.584018230438232, + "heading": 3.3590068391602883, + "angularVelocity": 0.6558407469812757, + "velocityX": 0.4014833048620338, + "velocityY": 2.0201009358213167, + "timestamp": 1.2549570294123078 + }, + { + "x": 1.7720599747049364, + "y": 6.6151238881747885, + "heading": 3.3693153572951067, + "angularVelocity": 0.6554501418707033, + "velocityX": 0.5700824332276426, + "velocityY": 1.9778019992556979, + "timestamp": 1.2706844165563504 + }, + { + "x": 1.7836731787465967, + "y": 6.64554729228533, + "heading": 3.3796121703026714, + "angularVelocity": 0.6547058906389773, + "velocityX": 0.7384064457305151, + "velocityY": 1.9344220264880285, + "timestamp": 1.286411803700393 + }, + { + "x": 1.7979286920774356, + "y": 6.6752693973823, + "heading": 3.389891037190624, + "angularVelocity": 0.6535648161904469, + "velocityX": 0.9064133285635219, + "velocityY": 1.8898310841307224, + "timestamp": 1.3021391908444357 + }, + { + "x": 1.8148207108436947, + "y": 6.704268726353282, + "heading": 3.400144906665288, + "angularVelocity": 0.651975396850814, + "velocityX": 1.0740511829174035, + "velocityY": 1.8438745549648863, + "timestamp": 1.3178665779884784 + }, + { + "x": 1.834342408767159, + "y": 6.732520878383194, + "heading": 3.4103657501447873, + "angularVelocity": 0.6498754933600988, + "velocityX": 1.2412549996175823, + "velocityY": 1.7963665401733704, + "timestamp": 1.333593965132521 + }, + { + "x": 1.8564856585052998, + "y": 6.7599978965922585, + "heading": 3.42054434613586, + "angularVelocity": 0.6471892564130288, + "velocityX": 1.4079420526332154, + "velocityY": 1.7470809332415638, + "timestamp": 1.3493213522765637 + }, + { + "x": 1.8812406470329832, + "y": 6.786667442295124, + "heading": 3.4306699971764414, + "angularVelocity": 0.6438228389651354, + "velocityX": 1.5740051606130965, + "velocityY": 1.6957391242808437, + "timestamp": 1.3650487394206063 + }, + { + "x": 1.9085953317289672, + "y": 6.812491697509257, + "heading": 3.440730151284119, + "angularVelocity": 0.6396583243954679, + "velocityX": 1.7393025583620485, + "velocityY": 1.6419927212076624, + "timestamp": 1.380776126564649 + }, + { + "x": 1.9385346494863533, + "y": 6.837425877048513, + "heading": 3.4507098849378357, + "angularVelocity": 0.6345449223265929, + "velocityX": 1.9036421932759835, + "velocityY": 1.5853987258588957, + "timestamp": 1.3965035137086916 + }, + { + "x": 1.9710393290071817, + "y": 6.861416165928424, + "heading": 3.4605911798929663, + "angularVelocity": 0.6282858598590166, + "velocityX": 2.0667564944594514, + "velocityY": 1.5253829933854495, + "timestamp": 1.4122309008527343 + }, + { + "x": 2.0060840386824674, + "y": 6.884396788302994, + "heading": 3.470351883817563, + "angularVelocity": 0.6206182778614892, + "velocityX": 2.228260127020535, + "velocityY": 1.4611850121127599, + "timestamp": 1.427958287996777 + }, + { + "x": 2.0436343673142363, + "y": 6.906285727611734, + "heading": 3.479964169940124, + "angularVelocity": 0.611181376443841, + "velocityX": 2.3875757802523783, + "velocityY": 1.3917721429672245, + "timestamp": 1.4436856751408196 + }, + { + "x": 2.0836416366671413, + "y": 6.926978297915, + "heading": 3.489392174533464, + "angularVelocity": 0.5994641390201337, + "velocityX": 2.543796308089173, + "velocityY": 1.3157029908241744, + "timestamp": 1.4594130622848622 + }, + { + "x": 2.1260334179669322, + "y": 6.946337239436048, + "heading": 3.498588236932545, + "angularVelocity": 0.5847164767330048, + "velocityX": 2.69541157164483, + "velocityY": 1.2309064019181353, + "timestamp": 1.4751404494289049 + }, + { + "x": 2.170694904668439, + "y": 6.964177314379111, + "heading": 3.5074866974204353, + "angularVelocity": 0.5657939495220478, + "velocityX": 2.8397270501746186, + "velocityY": 1.13433177295568, + "timestamp": 1.4908678365729475 + }, + { + "x": 2.217429447746538, + "y": 6.980242597216452, + "heading": 3.5159934565423607, + "angularVelocity": 0.5408882635121971, + "velocityX": 2.9715389244297503, + "velocityY": 1.021484540960543, + "timestamp": 1.5065952237169902 + }, + { + "x": 2.2658706589785127, + "y": 6.994183492523358, + "heading": 3.5239696234664595, + "angularVelocity": 0.5071514327870872, + "velocityX": 3.0800546071839623, + "velocityY": 0.8864088598585902, + "timestamp": 1.5223226108610328 + }, + { + "x": 2.315306439317678, + "y": 7.005595211136875, + "heading": 3.531224973833355, + "angularVelocity": 0.46131949957394686, + "velocityX": 3.1432926452688266, + "velocityY": 0.7255953267379982, + "timestamp": 1.5380499980050755 + }, + { + "x": 2.364549276525335, + "y": 7.014302767113342, + "heading": 3.537627214320393, + "angularVelocity": 0.4070759133988072, + "velocityX": 3.1310246741340757, + "velocityY": 0.5536556006867284, + "timestamp": 1.5537773851491181 + }, + { + "x": 2.4123908439730872, + "y": 7.020687456759953, + "heading": 3.5432119576128693, + "angularVelocity": 0.3550967011447554, + "velocityX": 3.041927245103382, + "velocityY": 0.4059599721260442, + "timestamp": 1.5695047722931608 + }, + { + "x": 2.458132378842044, + "y": 7.025345704452024, + "heading": 3.5480975346239485, + "angularVelocity": 0.31064136504896245, + "velocityX": 2.9084001334756193, + "velocityY": 0.2961870048347796, + "timestamp": 1.5852321594372034 + }, + { + "x": 2.5014553638181054, + "y": 7.028751699569263, + "heading": 3.552407775238728, + "angularVelocity": 0.27405954818200595, + "velocityX": 2.7546206232019532, + "velocityY": 0.21656458800457026, + "timestamp": 1.600959546581246 + }, + { + "x": 2.542213092217696, + "y": 7.031233755116593, + "heading": 3.5562362031090795, + "angularVelocity": 0.24342427863497068, + "velocityX": 2.5915130101587813, + "velocityY": 0.15781741268256558, + "timestamp": 1.6166869337252887 + }, + { + "x": 2.5803326470423813, + "y": 7.033019833738538, + "heading": 3.559649640158312, + "angularVelocity": 0.21703777098952132, + "velocityX": 2.423769089904064, + "velocityY": 0.11356486653424694, + "timestamp": 1.6324143208693314 + }, + { + "x": 2.6157749798098076, + "y": 7.03427367869847, + "heading": 3.5626967208417395, + "angularVelocity": 0.19374360505786095, + "velocityX": 2.253542336232949, + "velocityY": 0.07972366601316434, + "timestamp": 1.648141708013374 + }, + { + "x": 2.6485179067049858, + "y": 7.035117253167883, + "heading": 3.565413972482842, + "angularVelocity": 0.17277196880933401, + "velocityX": 2.081905061234573, + "velocityY": 0.0536372928120376, + "timestamp": 1.6638690951574167 + }, + { + "x": 2.6785482690846774, + "y": 7.035644367883773, + "heading": 3.5678296659091027, + "angularVelocity": 0.15359788654883516, + "velocityX": 1.909431115585295, + "velocityY": 0.03351572076550796, + "timestamp": 1.6795964823014593 + }, + { + "x": 2.7058580326316437, + "y": 7.035929166009351, + "heading": 3.5699662569746473, + "angularVelocity": 0.1358516227760129, + "velocityX": 1.7364463211113188, + "velocityY": 0.01810841959755818, + "timestamp": 1.695323869445502 + }, + { + "x": 2.730442214470581, + "y": 7.036031594182232, + "heading": 3.571841977675455, + "angularVelocity": 0.11926461042947216, + "velocityX": 1.56314469872061, + "velocityY": 0.006512726617774275, + "timestamp": 1.7110512565895446 + }, + { + "x": 2.752297719343406, + "y": 7.036001055460871, + "heading": 3.573471906610007, + "angularVelocity": 0.10363634592473486, + "velocityX": 1.3896462694442941, + "velocityY": -0.001941754283852309, + "timestamp": 1.7267786437335872 + }, + { + "x": 2.7714226555411328, + "y": 7.035878927369517, + "heading": 3.5748687113578734, + "angularVelocity": 0.08881352859654948, + "velocityX": 1.216027558968734, + "velocityY": -0.00776531347745605, + "timestamp": 1.74250603087763 + }, + { + "x": 2.787815917152703, + "y": 7.035700347508605, + "heading": 3.576043177627596, + "angularVelocity": 0.0746765027760899, + "velocityX": 1.042338530960605, + "velocityY": -0.011354706237983625, + "timestamp": 1.7582334180216725 + }, + { + "x": 2.801476920645093, + "y": 7.035495511762398, + "heading": 3.57700459568957, + "angularVelocity": 0.06113018349256862, + "velocityX": 0.8686123999665686, + "velocityY": -0.013024143446794291, + "timestamp": 1.7739608051657152 + }, + { + "x": 2.8124054342268368, + "y": 7.035290639091272, + "heading": 3.5777610486984153, + "angularVelocity": 0.04809781827819563, + "velocityX": 0.6948715308939885, + "velocityY": -0.013026491256922158, + "timestamp": 1.7896881923097578 + }, + { + "x": 2.82060146478212, + "y": 7.035108702556254, + "heading": 3.5783196319263704, + "angularVelocity": 0.03551659425937697, + "velocityX": 0.5211310995410608, + "velocityY": -0.011568134830788418, + "timestamp": 1.8054155794538005 + }, + { + "x": 2.8260651815141027, + "y": 7.034969992798228, + "heading": 3.5786866222858342, + "angularVelocity": 0.023334477373928582, + "velocityX": 0.347401426692329, + "velocityY": -0.008819631433772254, + "timestamp": 1.8211429665978431 + }, + { + "x": 2.828796863555908, + "y": 7.034892559051514, + "heading": 3.57886761138801, + "angularVelocity": 0.011507893874428679, + "velocityX": 0.1736895020633055, + "velocityY": -0.004923497209367752, + "timestamp": 1.8368703537418858 + }, + { + "x": 2.828796863555908, + "y": 7.034892559051514, + "heading": 3.57886761138801, + "angularVelocity": 2.338404634182572e-38, + "velocityX": -5.320180341501415e-40, + "velocityY": -3.3698131372568e-38, + "timestamp": 1.8525977408859284 + }, + { + "x": 2.9084775174891906, + "y": 7.041040428082855, + "heading": 3.5613533790014733, + "angularVelocity": -0.2055483996105686, + "velocityX": 0.9351383797156885, + "velocityY": 0.07215187126209009, + "timestamp": 1.9378050809562908 + }, + { + "x": 3.0678383714410953, + "y": 7.053335880908181, + "heading": 3.526306324951356, + "angularVelocity": -0.41131496442884247, + "velocityX": 1.8702714322534668, + "velocityY": 0.14430039495625907, + "timestamp": 2.023012421026653 + }, + { + "x": 3.3068786906868026, + "y": 7.0717782673609095, + "heading": 3.473697273457424, + "angularVelocity": -0.6174239384833393, + "velocityX": 2.805395862003363, + "velocityY": 0.21644128824465056, + "timestamp": 2.1082197610970153 + }, + { + "x": 3.6255976049312144, + "y": 7.096366357645069, + "heading": 3.4034914541196217, + "angularVelocity": -0.8239409806693618, + "velocityX": 3.74051007790198, + "velocityY": 0.28856774855141604, + "timestamp": 2.1934271011673774 + }, + { + "x": 4.006634355263943, + "y": 7.12573524545498, + "heading": 3.403491430543163, + "angularVelocity": -2.766951616622872e-7, + "velocityX": 4.471877070896435, + "velocityY": 0.34467556181966225, + "timestamp": 2.2786344412377395 + }, + { + "x": 4.387671106257462, + "y": 7.155104124693663, + "heading": 3.4034914069671127, + "angularVelocity": -2.766903705713447e-7, + "velocityX": 4.471877078651531, + "velocityY": 0.3446754612270608, + "timestamp": 2.3638417813081016 + }, + { + "x": 4.768707857250984, + "y": 7.184473003932312, + "heading": 3.4034913833910623, + "angularVelocity": -2.7669037102811926e-7, + "velocityX": 4.471877078651563, + "velocityY": 0.3446754612266613, + "timestamp": 2.4490491213784638 + }, + { + "x": 5.149744608244506, + "y": 7.213841883170961, + "heading": 3.4034913598150114, + "angularVelocity": -2.766903719221037e-7, + "velocityX": 4.471877078651562, + "velocityY": 0.3446754612266611, + "timestamp": 2.534256461448826 + }, + { + "x": 5.530781359238029, + "y": 7.2432107624096105, + "heading": 3.403491336238961, + "angularVelocity": -2.7669037233593665e-7, + "velocityX": 4.471877078651563, + "velocityY": 0.34467546122666126, + "timestamp": 2.619463801519188 + }, + { + "x": 5.91181811023155, + "y": 7.272579641648259, + "heading": 3.4034913126629105, + "angularVelocity": -2.766903719198649e-7, + "velocityX": 4.471877078651562, + "velocityY": 0.3446754612266609, + "timestamp": 2.70467114158955 + }, + { + "x": 6.292854861225073, + "y": 7.301948520886908, + "heading": 3.40349128908686, + "angularVelocity": -2.766903702987286e-7, + "velocityX": 4.471877078651562, + "velocityY": 0.3446754612266614, + "timestamp": 2.7898784816599123 + }, + { + "x": 6.673891612218593, + "y": 7.331317400125578, + "heading": 3.4034912655108096, + "angularVelocity": -2.7669037110747106e-7, + "velocityX": 4.471877078651543, + "velocityY": 0.3446754612269092, + "timestamp": 2.8750858217302744 + }, + { + "x": 7.054928362802186, + "y": 7.360686284680735, + "heading": 3.4034912419343524, + "angularVelocity": -2.7669514250103454e-7, + "velocityX": 4.471877073840595, + "velocityY": 0.34467552362161347, + "timestamp": 2.9602931618006365 + }, + { + "x": 7.373639419387511, + "y": 7.385254037823193, + "heading": 3.3329373560097224, + "angularVelocity": -0.8280259173255274, + "velocityX": 3.7404178598010502, + "velocityY": 0.2883290702675442, + "timestamp": 3.0455005018709986 + }, + { + "x": 7.612672703715564, + "y": 7.403678948176451, + "heading": 3.2800183747872396, + "angularVelocity": -0.6210612979912687, + "velocityX": 2.8053132996601575, + "velocityY": 0.21623618737591582, + "timestamp": 3.1307078419413608 + }, + { + "x": 7.772028248697012, + "y": 7.415961919015887, + "heading": 3.244738714983509, + "angularVelocity": -0.4140448437258724, + "velocityX": 1.8702091257614122, + "velocityY": 0.14415390539467693, + "timestamp": 3.215915182011723 + }, + { + "x": 7.851706027984619, + "y": 7.422103404998779, + "heading": 3.2270990200422704, + "angularVelocity": -0.20702083795448067, + "velocityX": 0.9351046426494596, + "velocityY": 0.07207695930680914, + "timestamp": 3.301122522082085 + }, + { + "x": 7.851706027984619, + "y": 7.422103404998779, + "heading": 3.2270990200422704, + "angularVelocity": 2.395667816010299e-38, + "velocityX": 2.50451146033937e-40, + "velocityY": 2.3151739947273847e-39, + "timestamp": 3.386329862152447 + }, + { + "x": 7.7535783430697425, + "y": 7.402064102143109, + "heading": 3.232020757481131, + "angularVelocity": 0.05169742228556919, + "velocityX": -1.030723078580944, + "velocityY": -0.2104907697550398, + "timestamp": 3.4815326251001584 + }, + { + "x": 7.557322977332363, + "y": 7.361985496490512, + "heading": 3.241864259728923, + "angularVelocity": 0.1033951320635361, + "velocityX": -2.061446114176018, + "velocityY": -0.4209815388930545, + "timestamp": 3.5767353880478696 + }, + { + "x": 7.26293993880598, + "y": 7.301867588491817, + "heading": 3.256629564983045, + "angularVelocity": 0.15509324306303482, + "velocityX": -3.0921690653880356, + "velocityY": -0.6314723032955863, + "timestamp": 3.671938150995581 + }, + { + "x": 6.870429250696279, + "y": 7.2217103812612065, + "heading": 3.276316699257686, + "angularVelocity": 0.20679162731289896, + "velocityX": -4.122891772849927, + "velocityY": -0.8419630349870816, + "timestamp": 3.767140913943292 + }, + { + "x": 6.452066171420709, + "y": 7.136273653088981, + "heading": 3.276316704861539, + "angularVelocity": 5.886229324112331e-8, + "velocityX": -4.394442622483034, + "velocityY": -0.8974185782732974, + "timestamp": 3.8623436768910033 + }, + { + "x": 6.033703092145166, + "y": 7.050836924916623, + "heading": 3.2763167104653923, + "angularVelocity": 5.886229546144486e-8, + "velocityX": -4.394442622482746, + "velocityY": -0.8974185782747038, + "timestamp": 3.9575464398387146 + }, + { + "x": 5.641192412144091, + "y": 6.970679696216733, + "heading": 3.2960044413177485, + "angularVelocity": 0.20679789370366783, + "velocityX": -4.122891687677771, + "velocityY": -0.8419632604981744, + "timestamp": 4.052749202786425 + }, + { + "x": 5.346809380757658, + "y": 6.910561769475596, + "heading": 3.310770277891182, + "angularVelocity": 0.15509882398626432, + "velocityX": -3.0921689903907406, + "velocityY": -0.6314725001642677, + "timestamp": 4.147951965734136 + }, + { + "x": 5.150554020304434, + "y": 6.87048315009937, + "heading": 3.3206141794938233, + "angularVelocity": 0.10339932684567515, + "velocityX": -2.0614460586717827, + "velocityY": -0.42098168304462, + "timestamp": 4.243154728681847 + }, + { + "x": 5.052426338195801, + "y": 6.8504438400268555, + "heading": 3.3255361324496757, + "angularVelocity": 0.05169968605381748, + "velocityX": -1.0307230491044557, + "velocityY": -0.21049084556003192, + "timestamp": 4.338357491629558 + }, + { + "x": 5.052426338195801, + "y": 6.8504438400268555, + "heading": 3.3255361324496757, + "angularVelocity": -3.315402276831609e-39, + "velocityX": -2.4414050222730828e-37, + "velocityY": -1.3667234180646677e-37, + "timestamp": 4.4335602545772685 + }, + { + "x": 5.125592947632546, + "y": 6.825900629691664, + "heading": 3.3051967120755084, + "angularVelocity": -0.24269122019378206, + "velocityX": 0.8730285030244984, + "velocityY": -0.2928511016609534, + "timestamp": 4.517368067476009 + }, + { + "x": 5.271925300123515, + "y": 6.776815141553044, + "heading": 3.2644814434878833, + "angularVelocity": -0.4858170996159881, + "velocityX": 1.746046668319264, + "velocityY": -0.5856910762953288, + "timestamp": 4.60117588037475 + }, + { + "x": 5.4914215364585965, + "y": 6.703188291356257, + "heading": 3.203326609050582, + "angularVelocity": -0.7297032618091427, + "velocityX": 2.619042649404126, + "velocityY": -0.8785201242006593, + "timestamp": 4.68498369327349 + }, + { + "x": 5.7840786491345275, + "y": 6.605020184452078, + "heading": 3.1216446060354786, + "angularVelocity": -0.9746347051651971, + "velocityX": 3.4920027447742963, + "velocityY": -1.1713479150540527, + "timestamp": 4.768791506172231 + }, + { + "x": 6.140433700365838, + "y": 6.485419873586866, + "heading": 3.121644578142224, + "angularVelocity": -3.32824030604796e-7, + "velocityX": 4.252050481998292, + "velocityY": -1.427078296503421, + "timestamp": 4.852599319070971 + }, + { + "x": 6.496788713608013, + "y": 6.36581944952913, + "heading": 3.1216445502509234, + "angularVelocity": -3.3280072167583106e-7, + "velocityX": 4.252050028709587, + "velocityY": -1.4270796471236313, + "timestamp": 4.936407131969712 + }, + { + "x": 6.853143726850153, + "y": 6.246219025471286, + "heading": 3.1216445223596225, + "angularVelocity": -3.3280072190686506e-7, + "velocityX": 4.252050028709163, + "velocityY": -1.4270796471248925, + "timestamp": 5.020214944868452 + }, + { + "x": 7.20949876939429, + "y": 6.126618688722239, + "heading": 3.1216444944663726, + "angularVelocity": -3.3282397956897397e-7, + "velocityX": 4.252050378342396, + "velocityY": -1.4270786053509494, + "timestamp": 5.104022757767193 + }, + { + "x": 7.502115016202162, + "y": 6.028435572271006, + "heading": 3.038925832747677, + "angularVelocity": -0.9870041808469556, + "velocityX": 3.4915151307124925, + "velocityY": -1.1715270098965787, + "timestamp": 5.187830570665933 + }, + { + "x": 7.721575871144278, + "y": 5.954796687958475, + "heading": 2.9768631098050315, + "angularVelocity": -0.740536243531756, + "velocityX": 2.6186204764378944, + "velocityY": -0.8786637160130223, + "timestamp": 5.271638383564674 + }, + { + "x": 7.867882744457675, + "y": 5.9057035200132155, + "heading": 2.935482546952977, + "angularVelocity": -0.4937554318718741, + "velocityX": 1.7457426491987094, + "velocityY": -0.5857827122224969, + "timestamp": 5.355446196463414 + }, + { + "x": 7.941036224365234, + "y": 5.881156921386719, + "heading": 2.91479324575467, + "angularVelocity": -0.24686601979823689, + "velocityX": 0.8728718406713037, + "velocityY": -0.29289153096208925, + "timestamp": 5.439254009362155 + }, + { + "x": 7.941036224365234, + "y": 5.881156921386719, + "heading": 2.91479324575467, + "angularVelocity": -8.307370174213132e-35, + "velocityX": -3.1349419451622814e-36, + "velocityY": -3.3120539075020083e-35, + "timestamp": 5.523061822260895 + }, + { + "x": 7.8675727783640985, + "y": 5.90546036695194, + "heading": 2.935525873781026, + "angularVelocity": 0.24703644978061876, + "velocityX": -0.8753424247857141, + "velocityY": 0.28958397855960943, + "timestamp": 5.606987202684584 + }, + { + "x": 7.720644724842832, + "y": 5.954065645656709, + "heading": 2.9769714499394753, + "angularVelocity": 0.4938384067991731, + "velocityX": -1.7506986894729044, + "velocityY": 0.5791487445084075, + "timestamp": 5.690912583108273 + }, + { + "x": 7.5002499438728485, + "y": 6.026970898531862, + "heading": 3.0390854497042903, + "angularVelocity": 0.7401098386595186, + "velocityX": -2.6260802138440438, + "velocityY": 0.8686913601951816, + "timestamp": 5.7748379635319615 + }, + { + "x": 7.206385265272288, + "y": 6.1241748112836225, + "heading": 3.121788392181331, + "angularVelocity": 0.9854342281145816, + "velocityX": -3.501499512030981, + "velocityY": 1.1582183156160495, + "timestamp": 5.85876334395565 + }, + { + "x": 6.849025327552005, + "y": 6.242427627098492, + "heading": 3.121788420122234, + "angularVelocity": 3.3292554576834236e-7, + "velocityX": -4.258067534709846, + "velocityY": 1.4090232920945023, + "timestamp": 5.942688724379339 + }, + { + "x": 6.4916654234974125, + "y": 6.360680544652888, + "heading": 3.1217884480614018, + "angularVelocity": 3.3290486828329673e-7, + "velocityX": -4.258067133571475, + "velocityY": 1.409024504356219, + "timestamp": 6.026614104803028 + }, + { + "x": 6.134305519442798, + "y": 6.478933462207215, + "heading": 3.1217884760005696, + "angularVelocity": 3.329048698919558e-7, + "velocityX": -4.258067133571742, + "velocityY": 1.4090245043554115, + "timestamp": 6.110539485226717 + }, + { + "x": 5.7769455762145565, + "y": 6.597186261377052, + "heading": 3.1217885039414885, + "angularVelocity": 3.3292573662864275e-7, + "velocityX": -4.258067600339077, + "velocityY": 1.4090230937631703, + "timestamp": 6.1944648656504055 + }, + { + "x": 5.483040254012964, + "y": 6.69438933225271, + "heading": 3.2033287309225806, + "angularVelocity": 0.9715800699316991, + "velocityX": -3.5019837946261267, + "velocityY": 1.1582082843704404, + "timestamp": 6.278390246074094 + }, + { + "x": 5.26261008660972, + "y": 6.7672927025778, + "heading": 3.264440440566138, + "angularVelocity": 0.7281672044266055, + "velocityX": -2.6265018554628505, + "velocityY": 0.8686689289586099, + "timestamp": 6.362315626497783 + }, + { + "x": 5.115656365698194, + "y": 6.815895328669379, + "heading": 3.3051694863922827, + "angularVelocity": 0.4853006995086357, + "velocityX": -1.7510045253252935, + "velocityY": 0.579117137702728, + "timestamp": 6.446241006921472 + }, + { + "x": 5.042179584503174, + "y": 6.84019660949707, + "heading": 3.325535953868165, + "angularVelocity": 0.24267351989426952, + "velocityX": -0.8755013182434166, + "velocityY": 0.2895581849615564, + "timestamp": 6.530166387345161 + }, + { + "x": 5.042179584503174, + "y": 6.84019660949707, + "heading": 3.325535953868165, + "angularVelocity": 0, + "velocityX": 1.3923094056611315e-34, + "velocityY": 2.5569606606573608e-34, + "timestamp": 6.6140917677688496 + }, + { + "x": 5.073880207055287, + "y": 6.82779809617603, + "heading": 3.305483362781089, + "angularVelocity": -0.3561840736590214, + "velocityX": 0.5630821886861347, + "velocityY": -0.2202285461677885, + "timestamp": 6.670390161127875 + }, + { + "x": 5.1373374485970675, + "y": 6.802993369110707, + "heading": 3.265889881043017, + "angularVelocity": -0.7032790702494384, + "velocityX": 1.1271590138834517, + "velocityY": -0.44059387107441605, + "timestamp": 6.7266885544869 + }, + { + "x": 5.232628574600268, + "y": 6.765767526946016, + "heading": 3.207512854637883, + "angularVelocity": -1.0369217116526357, + "velocityX": 1.6926082667317153, + "velocityY": -0.6612238812446238, + "timestamp": 6.7829869478459255 + }, + { + "x": 5.35987690908377, + "y": 6.716091063237897, + "heading": 3.131715460945362, + "angularVelocity": -1.346350919983584, + "velocityX": 2.2602480620009318, + "velocityY": -0.882377999516305, + "timestamp": 6.839285341204951 + }, + { + "x": 5.519307136483051, + "y": 6.653925120393696, + "heading": 3.0413668880518268, + "angularVelocity": -1.6048161857366978, + "velocityX": 2.831878813709029, + "velocityY": -1.104222325631158, + "timestamp": 6.895583734563976 + }, + { + "x": 5.711343989073416, + "y": 6.579302532140423, + "heading": 2.943828403584053, + "angularVelocity": -1.7325269629943616, + "velocityX": 3.411053870857564, + "velocityY": -1.325483442793676, + "timestamp": 6.951882127923001 + }, + { + "x": 5.932593273975757, + "y": 6.495118363659999, + "heading": 2.890818284301493, + "angularVelocity": -0.94159204410159, + "velocityX": 3.9299395897746816, + "velocityY": -1.4953209755661327, + "timestamp": 7.008180521282027 + }, + { + "x": 6.167130335408207, + "y": 6.401567639682382, + "heading": 2.8908181970163076, + "angularVelocity": -0.0000015504027732778302, + "velocityX": 4.165963670344261, + "velocityY": -1.6616943823072647, + "timestamp": 7.064478914641052 + }, + { + "x": 6.392413339713847, + "y": 6.287519672451602, + "heading": 2.890818109850187, + "angularVelocity": -0.0000015482878884317716, + "velocityX": 4.001588515483356, + "velocityY": -2.0257765883917123, + "timestamp": 7.120777308000077 + }, + { + "x": 6.59974479675293, + "y": 6.143391132354736, + "heading": 2.890818017389062, + "angularVelocity": -0.0000016423403859213353, + "velocityX": 3.6827242247730267, + "velocityY": -2.5600826506314496, + "timestamp": 7.177075701359103 + }, + { + "x": 6.802098168556436, + "y": 5.950611470632589, + "heading": 2.8908179477513425, + "angularVelocity": -0.0000011175444046256971, + "velocityX": 3.2473619220198997, + "velocityY": -3.093723258657997, + "timestamp": 7.239388857903613 + }, + { + "x": 6.972552082866989, + "y": 5.729124789979766, + "heading": 2.8908178853871664, + "angularVelocity": -0.0000010008187654827336, + "velocityX": 2.735440214600562, + "velocityY": -3.554412790734118, + "timestamp": 7.301702014448123 + }, + { + "x": 7.131045654507235, + "y": 5.498927755764986, + "heading": 2.8908178239788898, + "angularVelocity": -9.854785101241336e-7, + "velocityX": 2.5435009302896496, + "velocityY": -3.694196329957235, + "timestamp": 7.364015170992634 + }, + { + "x": 7.289539085272484, + "y": 5.268730624556049, + "heading": 2.890817762570617, + "angularVelocity": -9.85478446229405e-7, + "velocityX": 2.5434986695311594, + "velocityY": -3.694197886517073, + "timestamp": 7.426328327537144 + }, + { + "x": 7.448032587450016, + "y": 5.038533542515831, + "heading": 2.89081770116102, + "angularVelocity": -9.854996960990413e-7, + "velocityX": 2.5434998155537434, + "velocityY": -3.6941970974587077, + "timestamp": 7.488641484081654 + }, + { + "x": 7.592777439099975, + "y": 4.829594632078275, + "heading": 2.8224347675081725, + "angularVelocity": -1.0974076333944311, + "velocityX": 2.32286181083715, + "velocityY": -3.353046483663694, + "timestamp": 7.5509546406261645 + }, + { + "x": 7.713212504447541, + "y": 4.655502027335128, + "heading": 2.739432491996474, + "angularVelocity": -1.332018471129938, + "velocityX": 1.9327389595733324, + "velocityY": -2.793833828957021, + "timestamp": 7.613267797170675 + }, + { + "x": 7.8093302684945165, + "y": 4.516554189275233, + "heading": 2.6658856506758575, + "angularVelocity": -1.1802778963393161, + "velocityX": 1.5424955078037048, + "velocityY": -2.2298314796594534, + "timestamp": 7.675580953715185 + }, + { + "x": 7.881306607138764, + "y": 4.412523050354705, + "heading": 2.607642999795528, + "angularVelocity": -0.9346766254527145, + "velocityX": 1.1550745081070457, + "velocityY": -1.669489152683501, + "timestamp": 7.737894110259695 + }, + { + "x": 7.929238314866953, + "y": 4.343258360969765, + "heading": 2.56743105109566, + "angularVelocity": -0.6453203613773755, + "velocityX": 0.7692068639461747, + "velocityY": -1.1115580276448371, + "timestamp": 7.800207266804206 + }, + { + "x": 7.953185081481934, + "y": 4.30866003036499, + "heading": 2.5468332187039406, + "angularVelocity": -0.33055350641732584, + "velocityX": 0.3842971202698623, + "velocityY": -0.5552331565816471, + "timestamp": 7.862520423348716 + }, + { + "x": 7.953185081481934, + "y": 4.30866003036499, + "heading": 2.5468332187039406, + "angularVelocity": 1.05622156239882e-34, + "velocityX": 2.693478782190868e-36, + "velocityY": -1.766740346712715e-35, + "timestamp": 7.924833579893226 + }, + { + "x": 7.933444800519874, + "y": 4.337679051528667, + "heading": 2.567386408846941, + "angularVelocity": 0.3596766726497821, + "velocityX": -0.34545092631390906, + "velocityY": 0.5078270041334514, + "timestamp": 7.981977097129367 + }, + { + "x": 7.8939272389266275, + "y": 4.3958144902629535, + "heading": 2.6073640293286684, + "angularVelocity": 0.6996002769049587, + "velocityX": -0.69154933935801, + "velocityY": 1.017358425699386, + "timestamp": 8.039120614365508 + }, + { + "x": 7.834584299123902, + "y": 4.483196881040095, + "heading": 2.665183110722398, + "angularVelocity": 1.0118222361916747, + "velocityX": -1.038489450299254, + "velocityY": 1.529174174141952, + "timestamp": 8.096264131601648 + }, + { + "x": 7.755346884542635, + "y": 4.599998807677701, + "heading": 2.738526660809834, + "angularVelocity": 1.2834973000410625, + "velocityX": -1.3866387372311242, + "velocityY": 2.0440101045046273, + "timestamp": 8.153407648837788 + }, + { + "x": 7.656097596776391, + "y": 4.746453323880735, + "heading": 2.8233938773370038, + "angularVelocity": 1.4851591332130114, + "velocityX": -1.7368424725433986, + "velocityY": 2.562924427592089, + "timestamp": 8.210551166073929 + }, + { + "x": 7.536677822319239, + "y": 4.922827818699696, + "heading": 2.909865886749634, + "angularVelocity": 1.513242684297716, + "velocityX": -2.089821912145504, + "velocityY": 3.086518005010237, + "timestamp": 8.267694683310069 + }, + { + "x": 7.400726981561679, + "y": 5.123361927462954, + "heading": 2.9474259118798702, + "angularVelocity": 0.657292846973742, + "velocityX": -2.3791122306272374, + "velocityY": 3.5093063651396728, + "timestamp": 8.324838200546209 + }, + { + "x": 7.25730486839932, + "y": 5.335772109492621, + "heading": 2.9474259955604354, + "angularVelocity": 0.0000014643929737205378, + "velocityX": -2.509857987384268, + "velocityY": 3.7171352465390086, + "timestamp": 8.381981717782349 + }, + { + "x": 7.113882763503483, + "y": 5.548182297104135, + "heading": 2.9474260792405436, + "angularVelocity": 0.0000014643849797763275, + "velocityX": -2.509857842721803, + "velocityY": 3.717135344220187, + "timestamp": 8.439125235018489 + }, + { + "x": 6.970460146387983, + "y": 5.760592138857399, + "heading": 2.9474261629206717, + "angularVelocity": 0.0000014643853274041011, + "velocityX": -2.50986680646239, + "velocityY": 3.7171292917707364, + "timestamp": 8.496268752254629 + }, + { + "x": 6.808497246757101, + "y": 5.959227499928339, + "heading": 2.947426248883836, + "angularVelocity": 0.000001504337989820676, + "velocityX": -2.8343180025405768, + "velocityY": 3.4760786643583015, + "timestamp": 8.553412269490769 + }, + { + "x": 6.6202392578125, + "y": 6.133143901824951, + "heading": 2.947426346913631, + "angularVelocity": 0.0000017155015966502543, + "velocityX": -3.2944767499459546, + "velocityY": 3.043501875775644, + "timestamp": 8.61055578672691 + }, + { + "x": 6.387030430534219, + "y": 6.28974049469131, + "heading": 2.9474264250351503, + "angularVelocity": 0.000001247337128965759, + "velocityX": -3.7235582712366577, + "velocityY": 2.500319329333199, + "timestamp": 8.673186423953604 + }, + { + "x": 6.132503069560848, + "y": 6.40858776606532, + "heading": 2.9474265000949957, + "angularVelocity": 0.0000011984525335388143, + "velocityX": -4.063943338978, + "velocityY": 1.8975900076481367, + "timestamp": 8.735817061180299 + }, + { + "x": 5.871983809603483, + "y": 6.513652349670096, + "heading": 2.9474265754258733, + "angularVelocity": 0.0000012027799908761018, + "velocityX": -4.159613752841165, + "velocityY": 1.6775269781223834, + "timestamp": 8.798447698406994 + }, + { + "x": 5.633984277453578, + "y": 6.60624851988278, + "heading": 3.0254965551351023, + "angularVelocity": 1.2465142167826126, + "velocityX": -3.800049667201303, + "velocityY": 1.4784484768617097, + "timestamp": 8.861078335633689 + }, + { + "x": 5.4360193740701925, + "y": 6.684287207108514, + "heading": 3.1232338980771237, + "angularVelocity": 1.5605356622551558, + "velocityX": -3.160831697541927, + "velocityY": 1.2460145813823962, + "timestamp": 8.923708972860384 + }, + { + "x": 5.278191507828268, + "y": 6.7466944701474745, + "heading": 3.209038101470214, + "angularVelocity": 1.3700036785913317, + "velocityX": -2.5199786115964247, + "velocityY": 0.9964334677463785, + "timestamp": 8.986339610087079 + }, + { + "x": 5.160074587729763, + "y": 6.7934662468242815, + "heading": 3.2763629711209266, + "angularVelocity": 1.0749510564139328, + "velocityX": -1.8859287615257068, + "velocityY": 0.7467874948727645, + "timestamp": 9.048970247313774 + }, + { + "x": 5.081447933998166, + "y": 6.824627137180405, + "heading": 3.322538510654522, + "angularVelocity": 0.7372675990260279, + "velocityX": -1.2554024230506353, + "velocityY": 0.4975343016762813, + "timestamp": 9.111600884540469 + }, + { + "x": 5.042179584503174, + "y": 6.84019660949707, + "heading": 3.346115957859533, + "angularVelocity": 0.3764522963365605, + "velocityX": -0.6269830746389843, + "velocityY": 0.2485919512571829, + "timestamp": 9.174231521767163 + }, + { + "x": 5.042179584503174, + "y": 6.84019660949707, + "heading": 3.346115957859533, + "angularVelocity": 3.173544538115536e-35, + "velocityX": -2.8108210003831088e-36, + "velocityY": 9.453581075103361e-36, + "timestamp": 9.236862158993858 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/BAC.1.traj b/src/main/deploy/choreo/BAC.1.traj new file mode 100644 index 0000000..8ddd0a4 --- /dev/null +++ b/src/main/deploy/choreo/BAC.1.traj @@ -0,0 +1,94 @@ +{ + "samples": [ + { + "x": 1.4672101736068726, + "y": 5.529117584228516, + "heading": 3.1342399032514394, + "angularVelocity": 2.0409137584686925e-22, + "velocityX": -1.7776643305965262e-19, + "velocityY": -1.391980365780906e-28, + "timestamp": 0 + }, + { + "x": 1.523226330857495, + "y": 5.529117584228516, + "heading": 3.1346075407729113, + "angularVelocity": 0.004079272970523933, + "velocityX": 0.6215502576293102, + "velocityY": 4.339386975653289e-19, + "timestamp": 0.09012329504017398 + }, + { + "x": 1.635258643051257, + "y": 5.529117584228516, + "heading": 3.135342815812323, + "angularVelocity": 0.008158545901854228, + "velocityX": 1.2431004896549982, + "velocityY": 4.339386972818906e-19, + "timestamp": 0.18024659008034796 + }, + { + "x": 1.8033071055731926, + "y": 5.529117584228516, + "heading": 3.1364457283639133, + "angularVelocity": 0.012237818769259935, + "velocityX": 1.8646506704734345, + "velocityY": 4.339386972878163e-19, + "timestamp": 0.27036988512052196 + }, + { + "x": 2.0273717045784014, + "y": 5.529117584228516, + "heading": 3.1379162784172956, + "angularVelocity": 0.01631709152142235, + "velocityX": 2.486200697670099, + "velocityY": 4.3393869727945e-19, + "timestamp": 0.3604931801606959 + }, + { + "x": 2.251436303583609, + "y": 5.529117584228516, + "heading": 3.1393868284729205, + "angularVelocity": 0.016317091546299674, + "velocityX": 2.4862006976700846, + "velocityY": 4.339386972843219e-19, + "timestamp": 0.4506164752008699 + }, + { + "x": 2.419484766105543, + "y": 5.529117584228516, + "heading": 3.140489741026528, + "angularVelocity": 0.012237818791640459, + "velocityX": 1.8646506704734211, + "velocityY": 4.33938697282966e-19, + "timestamp": 0.5407397702410439 + }, + { + "x": 2.531517078299305, + "y": 5.529117584228516, + "heading": 3.141225016067478, + "angularVelocity": 0.00815854591892204, + "velocityX": 1.2431004896549878, + "velocityY": 4.339386972855822e-19, + "timestamp": 0.6308630652812179 + }, + { + "x": 2.5875332355499268, + "y": 5.529117584228516, + "heading": 3.141592653589793, + "angularVelocity": 0.00407927297987919, + "velocityX": 0.6215502576293045, + "velocityY": 4.339386912872934e-19, + "timestamp": 0.720986360321392 + }, + { + "x": 2.5875332355499268, + "y": 5.529117584228516, + "heading": 3.141592653589793, + "angularVelocity": -9.270392387787844e-21, + "velocityX": 2.4145496004036355e-19, + "velocityY": 3.352969834197908e-19, + "timestamp": 0.811109655361566 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/BAC.2.traj b/src/main/deploy/choreo/BAC.2.traj new file mode 100644 index 0000000..08590d4 --- /dev/null +++ b/src/main/deploy/choreo/BAC.2.traj @@ -0,0 +1,166 @@ +{ + "samples": [ + { + "x": 2.5875332355499268, + "y": 5.529117584228516, + "heading": 3.141592653589793, + "angularVelocity": -9.270392387787844e-21, + "velocityX": 2.4145496004036355e-19, + "velocityY": 3.352969834197908e-19, + "timestamp": 0 + }, + { + "x": 2.5600362053555803, + "y": 5.521017447160045, + "heading": 3.1346676285921653, + "angularVelocity": -0.10722467179780332, + "velocityX": -0.4257544252927914, + "velocityY": -0.1254196972547403, + "timestamp": 0.06458424988874056 + }, + { + "x": 2.505584296099502, + "y": 5.503247214690995, + "heading": 3.1199154860472977, + "angularVelocity": -0.2284170300078041, + "velocityX": -0.8431143715361332, + "velocityY": -0.2751480817639561, + "timestamp": 0.12916849977748113 + }, + { + "x": 2.4254159929252994, + "y": 5.47300835305198, + "heading": 3.095784519832863, + "angularVelocity": -0.3736354646218692, + "velocityX": -1.2412980457667073, + "velocityY": -0.46820798710379297, + "timestamp": 0.1937527496662217 + }, + { + "x": 2.3235496503060364, + "y": 5.424392303348171, + "heading": 3.0592201476968186, + "angularVelocity": -0.5661499854691174, + "velocityX": -1.577262920832063, + "velocityY": -0.7527539576221648, + "timestamp": 0.25833699955496225 + }, + { + "x": 2.2192056112863816, + "y": 5.347815141414236, + "heading": 3.0042606838359207, + "angularVelocity": -0.8509731700155467, + "velocityX": -1.615626707740792, + "velocityY": -1.1856940672974423, + "timestamp": 0.3229212494437028 + }, + { + "x": 2.1375558423359533, + "y": 5.253827787929466, + "heading": 2.94052551006143, + "angularVelocity": -0.9868532015822344, + "velocityX": -1.2642365451497315, + "velocityY": -1.4552674010564646, + "timestamp": 0.3875054993324434 + }, + { + "x": 2.0832915247049715, + "y": 5.151104465797474, + "heading": 2.8735827182257894, + "angularVelocity": -1.0365188409087829, + "velocityX": -0.8402097682401357, + "velocityY": -1.590532092715386, + "timestamp": 0.45208974922118395 + }, + { + "x": 2.0573530197143555, + "y": 5.043372631072998, + "heading": 2.8057381761859457, + "angularVelocity": -1.0504812265640555, + "velocityX": -0.40162276461057256, + "velocityY": -1.6680821548607452, + "timestamp": 0.5166739991099245 + }, + { + "x": 2.0586542274012434, + "y": 4.9377546147115945, + "heading": 2.741238422539913, + "angularVelocity": -1.046306862028587, + "velocityX": 0.021108026849010322, + "velocityY": -1.7133221295579422, + "timestamp": 0.5783191588491812 + }, + { + "x": 2.0861459577947783, + "y": 4.831317346870753, + "heading": 2.6781925434091516, + "angularVelocity": -1.0227222931602278, + "velocityX": 0.44596738024231, + "velocityY": -1.726611923645656, + "timestamp": 0.6399643185884378 + }, + { + "x": 2.139636348302225, + "y": 4.727915639653612, + "heading": 2.618905454039189, + "angularVelocity": -0.9617476801217085, + "velocityX": 0.8677143628745139, + "velocityY": -1.6773694423780108, + "timestamp": 0.7016094783276945 + }, + { + "x": 2.216058800258476, + "y": 4.636794871451547, + "heading": 2.568746805299111, + "angularVelocity": -0.8136672684803807, + "velocityX": 1.2397153690492175, + "velocityY": -1.478149599862866, + "timestamp": 0.7632546380669512 + }, + { + "x": 2.29514952461487, + "y": 4.571163115176212, + "heading": 2.5343102765661243, + "angularVelocity": -0.5586250222830847, + "velocityX": 1.2829997471160317, + "velocityY": -1.0646700657917056, + "timestamp": 0.8248997978062078 + }, + { + "x": 2.3591967583576956, + "y": 4.526591007980059, + "heading": 2.511766520325431, + "angularVelocity": -0.36570196810337247, + "velocityX": 1.0389661412790356, + "velocityY": -0.7230430967278133, + "timestamp": 0.8865449575454645 + }, + { + "x": 2.4033666080540748, + "y": 4.498819329963111, + "heading": 2.498100704779462, + "angularVelocity": -0.22168513479032978, + "velocityX": 0.7165177263422866, + "velocityY": -0.45050865525234624, + "timestamp": 0.9481901172847211 + }, + { + "x": 2.425884485244751, + "y": 4.485595226287842, + "heading": 2.4917219840272407, + "angularVelocity": -0.10347480287506021, + "velocityX": 0.36528216142063263, + "velocityY": -0.21451974057985182, + "timestamp": 1.0098352770239778 + }, + { + "x": 2.425884485244751, + "y": 4.485595226287842, + "heading": 2.4917219840272407, + "angularVelocity": -2.0238555050332036e-19, + "velocityX": -6.328087147231851e-19, + "velocityY": 2.0228670555604575e-19, + "timestamp": 1.0714804367632345 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/BAC.3.traj b/src/main/deploy/choreo/BAC.3.traj new file mode 100644 index 0000000..c8fdf0f --- /dev/null +++ b/src/main/deploy/choreo/BAC.3.traj @@ -0,0 +1,229 @@ +{ + "samples": [ + { + "x": 2.425884485244751, + "y": 4.485595226287842, + "heading": 2.4917219840272407, + "angularVelocity": -2.0238555050332036e-19, + "velocityX": -6.328087147231851e-19, + "velocityY": 2.0228670555604575e-19, + "timestamp": 0 + }, + { + "x": 2.4163281961687897, + "y": 4.4968944124354655, + "heading": 2.4986289124106955, + "angularVelocity": 0.1481481520431064, + "velocityX": -0.2049748438661491, + "velocityY": 0.24235860782506102, + "timestamp": 0.04662176536259022 + }, + { + "x": 2.397636354370071, + "y": 4.519824349042913, + "heading": 2.5125926065530826, + "angularVelocity": 0.2995101973034969, + "velocityX": -0.40092522566117333, + "velocityY": 0.4918290079561581, + "timestamp": 0.09324353072518043 + }, + { + "x": 2.3703488844596947, + "y": 4.554777605276144, + "heading": 2.5337687464712286, + "angularVelocity": 0.45421145581798067, + "velocityX": -0.5852946515035189, + "velocityY": 0.7497197062657466, + "timestamp": 0.13986529608777087 + }, + { + "x": 2.3351784028550076, + "y": 4.602220667216977, + "heading": 2.5623123992809558, + "angularVelocity": 0.6122387813446211, + "velocityX": -0.7543790187084568, + "velocityY": 1.0176161621477637, + "timestamp": 0.1864870614503611 + }, + { + "x": 2.2930964709980497, + "y": 4.662703934333856, + "heading": 2.5983734318145917, + "angularVelocity": 0.7734806319147236, + "velocityX": -0.9026241612619152, + "velocityY": 1.297318251389327, + "timestamp": 0.2331088268129513 + }, + { + "x": 2.2454743619949378, + "y": 4.7368536333295514, + "heading": 2.642110926002794, + "angularVelocity": 0.93813466410043, + "velocityX": -1.0214565800488513, + "velocityY": 1.590452408204886, + "timestamp": 0.2797305921755415 + }, + { + "x": 2.194302030246623, + "y": 4.825300290392434, + "heading": 2.6937611323595516, + "angularVelocity": 1.107856082991702, + "velocityX": -1.09760605052883, + "velocityY": 1.8971108531607184, + "timestamp": 0.32635235753813174 + }, + { + "x": 2.1424596494575834, + "y": 4.928442895157424, + "heading": 2.7536921937797003, + "angularVelocity": 1.2854738758613833, + "velocityX": -1.1119780725986486, + "velocityY": 2.212327310277112, + "timestamp": 0.37297412290072196 + }, + { + "x": 2.0938433909209753, + "y": 5.045947403781979, + "heading": 2.821868168120998, + "angularVelocity": 1.4623207382018693, + "velocityX": -1.0427803014001404, + "velocityY": 2.5203787911223454, + "timestamp": 0.4195958882633122 + }, + { + "x": 2.052916437371428, + "y": 5.176103289286733, + "heading": 2.8968212910198883, + "angularVelocity": 1.607685215606043, + "velocityX": -0.8778507898885197, + "velocityY": 2.7917408208911634, + "timestamp": 0.4662176536259024 + }, + { + "x": 2.0234217298246264, + "y": 5.3158158161345135, + "heading": 2.976459686173169, + "angularVelocity": 1.708180600496579, + "velocityX": -0.632638153390655, + "velocityY": 2.9967232206073193, + "timestamp": 0.5128394189884926 + }, + { + "x": 2.007551670074463, + "y": 5.461705684661865, + "heading": 3.0588507582207694, + "angularVelocity": 1.7672233431492963, + "velocityX": -0.3404002320962656, + "velocityY": 3.1292223147863565, + "timestamp": 0.5594611843510828 + }, + { + "x": 2.0103349998249294, + "y": 5.6478465131717925, + "heading": 3.1628344443744427, + "angularVelocity": 1.798056030810364, + "velocityX": 0.04812853851097616, + "velocityY": 3.218693736127869, + "timestamp": 0.6172923555559133 + }, + { + "x": 2.0361655835802854, + "y": 5.834881164361853, + "heading": 3.266208023207822, + "angularVelocity": 1.7875062302861198, + "velocityX": 0.44665503425250636, + "velocityY": 3.234149461154913, + "timestamp": 0.6751235267607438 + }, + { + "x": 2.084020418179946, + "y": 6.015266139989275, + "heading": 3.364851393635821, + "angularVelocity": 1.7057128253311047, + "velocityX": 0.8274920532071655, + "velocityY": 3.1191651814991155, + "timestamp": 0.7329546979655743 + }, + { + "x": 2.1473687763134053, + "y": 6.178930316392185, + "heading": 3.4533973919365337, + "angularVelocity": 1.5311119670582805, + "velocityX": 1.0954016115130607, + "velocityY": 2.8300339244943253, + "timestamp": 0.7907858691704048 + }, + { + "x": 2.2155511491160462, + "y": 6.320505490764638, + "heading": 3.528711384633538, + "angularVelocity": 1.3023079271601148, + "velocityX": 1.1789900045625517, + "velocityY": 2.4480772466290195, + "timestamp": 0.8486170403752353 + }, + { + "x": 2.2809856519910747, + "y": 6.439584670224959, + "heading": 3.5917636657644656, + "angularVelocity": 1.0902819330358189, + "velocityX": 1.1314746271222493, + "velocityY": 2.0590829647658215, + "timestamp": 0.9064482115800658 + }, + { + "x": 2.339420243806818, + "y": 6.537054712272287, + "heading": 3.6433952707526793, + "angularVelocity": 0.8927988818580544, + "velocityX": 1.0104341758664286, + "velocityY": 1.6854239680206609, + "timestamp": 0.9642793827848963 + }, + { + "x": 2.3884097396170962, + "y": 6.613832541544583, + "heading": 3.6840951991671647, + "angularVelocity": 0.7037714707580782, + "velocityX": 0.8471122889205226, + "velocityY": 1.3276201687211013, + "timestamp": 1.0221105539897268 + }, + { + "x": 2.4264268318884348, + "y": 6.670660345764589, + "heading": 3.714220994181463, + "angularVelocity": 0.5209265935078019, + "velocityX": 0.6573806388372562, + "velocityY": 0.98265006632375, + "timestamp": 1.0799417251945573 + }, + { + "x": 2.4524434068905587, + "y": 6.708118580157331, + "heading": 3.734065890519564, + "angularVelocity": 0.343152246870689, + "velocityX": 0.4498711414641213, + "velocityY": 0.6477170289370383, + "timestamp": 1.1377728963993878 + }, + { + "x": 2.4657256603240967, + "y": 6.7266645431518555, + "heading": 3.743879884568966, + "angularVelocity": 0.16970076595271175, + "velocityX": 0.22967291093749073, + "velocityY": 0.32069146462271875, + "timestamp": 1.1956040676042183 + }, + { + "x": 2.4657256603240967, + "y": 6.7266645431518555, + "heading": 3.743879884568966, + "angularVelocity": -1.4269853033258e-19, + "velocityX": 1.5862987647763193e-19, + "velocityY": -3.281295415241875e-19, + "timestamp": 1.2534352388090488 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/BAC.traj b/src/main/deploy/choreo/BAC.traj new file mode 100644 index 0000000..d9866d2 --- /dev/null +++ b/src/main/deploy/choreo/BAC.traj @@ -0,0 +1,463 @@ +{ + "samples": [ + { + "x": 1.4672101736068726, + "y": 5.529117584228516, + "heading": 3.1342399032514394, + "angularVelocity": 2.0409137584686925e-22, + "velocityX": -1.7776643305965262e-19, + "velocityY": -1.391980365780906e-28, + "timestamp": 0 + }, + { + "x": 1.523226330857495, + "y": 5.529117584228516, + "heading": 3.1346075407729113, + "angularVelocity": 0.004079272970523933, + "velocityX": 0.6215502576293102, + "velocityY": 4.339386975653289e-19, + "timestamp": 0.09012329504017398 + }, + { + "x": 1.635258643051257, + "y": 5.529117584228516, + "heading": 3.135342815812323, + "angularVelocity": 0.008158545901854228, + "velocityX": 1.2431004896549982, + "velocityY": 4.339386972818906e-19, + "timestamp": 0.18024659008034796 + }, + { + "x": 1.8033071055731926, + "y": 5.529117584228516, + "heading": 3.1364457283639133, + "angularVelocity": 0.012237818769259935, + "velocityX": 1.8646506704734345, + "velocityY": 4.339386972878163e-19, + "timestamp": 0.27036988512052196 + }, + { + "x": 2.0273717045784014, + "y": 5.529117584228516, + "heading": 3.1379162784172956, + "angularVelocity": 0.01631709152142235, + "velocityX": 2.486200697670099, + "velocityY": 4.3393869727945e-19, + "timestamp": 0.3604931801606959 + }, + { + "x": 2.251436303583609, + "y": 5.529117584228516, + "heading": 3.1393868284729205, + "angularVelocity": 0.016317091546299674, + "velocityX": 2.4862006976700846, + "velocityY": 4.339386972843219e-19, + "timestamp": 0.4506164752008699 + }, + { + "x": 2.419484766105543, + "y": 5.529117584228516, + "heading": 3.140489741026528, + "angularVelocity": 0.012237818791640459, + "velocityX": 1.8646506704734211, + "velocityY": 4.33938697282966e-19, + "timestamp": 0.5407397702410439 + }, + { + "x": 2.531517078299305, + "y": 5.529117584228516, + "heading": 3.141225016067478, + "angularVelocity": 0.00815854591892204, + "velocityX": 1.2431004896549878, + "velocityY": 4.339386972855822e-19, + "timestamp": 0.6308630652812179 + }, + { + "x": 2.5875332355499268, + "y": 5.529117584228516, + "heading": 3.141592653589793, + "angularVelocity": 0.00407927297987919, + "velocityX": 0.6215502576293045, + "velocityY": 4.339386912872934e-19, + "timestamp": 0.720986360321392 + }, + { + "x": 2.5875332355499268, + "y": 5.529117584228516, + "heading": 3.141592653589793, + "angularVelocity": -9.270392387787844e-21, + "velocityX": 2.4145496004036355e-19, + "velocityY": 3.352969834197908e-19, + "timestamp": 0.811109655361566 + }, + { + "x": 2.5600362053555803, + "y": 5.521017447160045, + "heading": 3.1346676285921653, + "angularVelocity": -0.10722467179780332, + "velocityX": -0.4257544252927914, + "velocityY": -0.1254196972547403, + "timestamp": 0.8756939052503065 + }, + { + "x": 2.505584296099502, + "y": 5.503247214690995, + "heading": 3.1199154860472977, + "angularVelocity": -0.2284170300078041, + "velocityX": -0.8431143715361332, + "velocityY": -0.2751480817639561, + "timestamp": 0.9402781551390471 + }, + { + "x": 2.4254159929252994, + "y": 5.47300835305198, + "heading": 3.095784519832863, + "angularVelocity": -0.3736354646218692, + "velocityX": -1.2412980457667073, + "velocityY": -0.46820798710379297, + "timestamp": 1.0048624050277877 + }, + { + "x": 2.3235496503060364, + "y": 5.424392303348171, + "heading": 3.0592201476968186, + "angularVelocity": -0.5661499854691174, + "velocityX": -1.577262920832063, + "velocityY": -0.7527539576221648, + "timestamp": 1.0694466549165282 + }, + { + "x": 2.2192056112863816, + "y": 5.347815141414236, + "heading": 3.0042606838359207, + "angularVelocity": -0.8509731700155467, + "velocityX": -1.615626707740792, + "velocityY": -1.1856940672974423, + "timestamp": 1.1340309048052688 + }, + { + "x": 2.1375558423359533, + "y": 5.253827787929466, + "heading": 2.94052551006143, + "angularVelocity": -0.9868532015822344, + "velocityX": -1.2642365451497315, + "velocityY": -1.4552674010564646, + "timestamp": 1.1986151546940094 + }, + { + "x": 2.0832915247049715, + "y": 5.151104465797474, + "heading": 2.8735827182257894, + "angularVelocity": -1.0365188409087829, + "velocityX": -0.8402097682401357, + "velocityY": -1.590532092715386, + "timestamp": 1.26319940458275 + }, + { + "x": 2.0573530197143555, + "y": 5.043372631072998, + "heading": 2.8057381761859457, + "angularVelocity": -1.0504812265640555, + "velocityX": -0.40162276461057256, + "velocityY": -1.6680821548607452, + "timestamp": 1.3277836544714905 + }, + { + "x": 2.0586542274012434, + "y": 4.9377546147115945, + "heading": 2.741238422539913, + "angularVelocity": -1.046306862028587, + "velocityX": 0.021108026849010322, + "velocityY": -1.7133221295579422, + "timestamp": 1.3894288142107472 + }, + { + "x": 2.0861459577947783, + "y": 4.831317346870753, + "heading": 2.6781925434091516, + "angularVelocity": -1.0227222931602278, + "velocityX": 0.44596738024231, + "velocityY": -1.726611923645656, + "timestamp": 1.4510739739500038 + }, + { + "x": 2.139636348302225, + "y": 4.727915639653612, + "heading": 2.618905454039189, + "angularVelocity": -0.9617476801217085, + "velocityX": 0.8677143628745139, + "velocityY": -1.6773694423780108, + "timestamp": 1.5127191336892605 + }, + { + "x": 2.216058800258476, + "y": 4.636794871451547, + "heading": 2.568746805299111, + "angularVelocity": -0.8136672684803807, + "velocityX": 1.2397153690492175, + "velocityY": -1.478149599862866, + "timestamp": 1.5743642934285171 + }, + { + "x": 2.29514952461487, + "y": 4.571163115176212, + "heading": 2.5343102765661243, + "angularVelocity": -0.5586250222830847, + "velocityX": 1.2829997471160317, + "velocityY": -1.0646700657917056, + "timestamp": 1.6360094531677738 + }, + { + "x": 2.3591967583576956, + "y": 4.526591007980059, + "heading": 2.511766520325431, + "angularVelocity": -0.36570196810337247, + "velocityX": 1.0389661412790356, + "velocityY": -0.7230430967278133, + "timestamp": 1.6976546129070305 + }, + { + "x": 2.4033666080540748, + "y": 4.498819329963111, + "heading": 2.498100704779462, + "angularVelocity": -0.22168513479032978, + "velocityX": 0.7165177263422866, + "velocityY": -0.45050865525234624, + "timestamp": 1.7592997726462871 + }, + { + "x": 2.425884485244751, + "y": 4.485595226287842, + "heading": 2.4917219840272407, + "angularVelocity": -0.10347480287506021, + "velocityX": 0.36528216142063263, + "velocityY": -0.21451974057985182, + "timestamp": 1.8209449323855438 + }, + { + "x": 2.425884485244751, + "y": 4.485595226287842, + "heading": 2.4917219840272407, + "angularVelocity": -2.0238555050332036e-19, + "velocityX": -6.328087147231851e-19, + "velocityY": 2.0228670555604575e-19, + "timestamp": 1.8825900921248004 + }, + { + "x": 2.4163281961687897, + "y": 4.4968944124354655, + "heading": 2.4986289124106955, + "angularVelocity": 0.1481481520431064, + "velocityX": -0.2049748438661491, + "velocityY": 0.24235860782506102, + "timestamp": 1.9292118574873907 + }, + { + "x": 2.397636354370071, + "y": 4.519824349042913, + "heading": 2.5125926065530826, + "angularVelocity": 0.2995101973034969, + "velocityX": -0.40092522566117333, + "velocityY": 0.4918290079561581, + "timestamp": 1.9758336228499809 + }, + { + "x": 2.3703488844596947, + "y": 4.554777605276144, + "heading": 2.5337687464712286, + "angularVelocity": 0.45421145581798067, + "velocityX": -0.5852946515035189, + "velocityY": 0.7497197062657466, + "timestamp": 2.0224553882125713 + }, + { + "x": 2.3351784028550076, + "y": 4.602220667216977, + "heading": 2.5623123992809558, + "angularVelocity": 0.6122387813446211, + "velocityX": -0.7543790187084568, + "velocityY": 1.0176161621477637, + "timestamp": 2.0690771535751615 + }, + { + "x": 2.2930964709980497, + "y": 4.662703934333856, + "heading": 2.5983734318145917, + "angularVelocity": 0.7734806319147236, + "velocityX": -0.9026241612619152, + "velocityY": 1.297318251389327, + "timestamp": 2.1156989189377517 + }, + { + "x": 2.2454743619949378, + "y": 4.7368536333295514, + "heading": 2.642110926002794, + "angularVelocity": 0.93813466410043, + "velocityX": -1.0214565800488513, + "velocityY": 1.590452408204886, + "timestamp": 2.162320684300342 + }, + { + "x": 2.194302030246623, + "y": 4.825300290392434, + "heading": 2.6937611323595516, + "angularVelocity": 1.107856082991702, + "velocityX": -1.09760605052883, + "velocityY": 1.8971108531607184, + "timestamp": 2.208942449662932 + }, + { + "x": 2.1424596494575834, + "y": 4.928442895157424, + "heading": 2.7536921937797003, + "angularVelocity": 1.2854738758613833, + "velocityX": -1.1119780725986486, + "velocityY": 2.212327310277112, + "timestamp": 2.2555642150255224 + }, + { + "x": 2.0938433909209753, + "y": 5.045947403781979, + "heading": 2.821868168120998, + "angularVelocity": 1.4623207382018693, + "velocityX": -1.0427803014001404, + "velocityY": 2.5203787911223454, + "timestamp": 2.3021859803881126 + }, + { + "x": 2.052916437371428, + "y": 5.176103289286733, + "heading": 2.8968212910198883, + "angularVelocity": 1.607685215606043, + "velocityX": -0.8778507898885197, + "velocityY": 2.7917408208911634, + "timestamp": 2.348807745750703 + }, + { + "x": 2.0234217298246264, + "y": 5.3158158161345135, + "heading": 2.976459686173169, + "angularVelocity": 1.708180600496579, + "velocityX": -0.632638153390655, + "velocityY": 2.9967232206073193, + "timestamp": 2.395429511113293 + }, + { + "x": 2.007551670074463, + "y": 5.461705684661865, + "heading": 3.0588507582207694, + "angularVelocity": 1.7672233431492963, + "velocityX": -0.3404002320962656, + "velocityY": 3.1292223147863565, + "timestamp": 2.4420512764758833 + }, + { + "x": 2.0103349998249294, + "y": 5.6478465131717925, + "heading": 3.1628344443744427, + "angularVelocity": 1.798056030810364, + "velocityX": 0.04812853851097616, + "velocityY": 3.218693736127869, + "timestamp": 2.4998824476807138 + }, + { + "x": 2.0361655835802854, + "y": 5.834881164361853, + "heading": 3.266208023207822, + "angularVelocity": 1.7875062302861198, + "velocityX": 0.44665503425250636, + "velocityY": 3.234149461154913, + "timestamp": 2.5577136188855443 + }, + { + "x": 2.084020418179946, + "y": 6.015266139989275, + "heading": 3.364851393635821, + "angularVelocity": 1.7057128253311047, + "velocityX": 0.8274920532071655, + "velocityY": 3.1191651814991155, + "timestamp": 2.6155447900903748 + }, + { + "x": 2.1473687763134053, + "y": 6.178930316392185, + "heading": 3.4533973919365337, + "angularVelocity": 1.5311119670582805, + "velocityX": 1.0954016115130607, + "velocityY": 2.8300339244943253, + "timestamp": 2.6733759612952053 + }, + { + "x": 2.2155511491160462, + "y": 6.320505490764638, + "heading": 3.528711384633538, + "angularVelocity": 1.3023079271601148, + "velocityX": 1.1789900045625517, + "velocityY": 2.4480772466290195, + "timestamp": 2.7312071325000358 + }, + { + "x": 2.2809856519910747, + "y": 6.439584670224959, + "heading": 3.5917636657644656, + "angularVelocity": 1.0902819330358189, + "velocityX": 1.1314746271222493, + "velocityY": 2.0590829647658215, + "timestamp": 2.7890383037048663 + }, + { + "x": 2.339420243806818, + "y": 6.537054712272287, + "heading": 3.6433952707526793, + "angularVelocity": 0.8927988818580544, + "velocityX": 1.0104341758664286, + "velocityY": 1.6854239680206609, + "timestamp": 2.8468694749096968 + }, + { + "x": 2.3884097396170962, + "y": 6.613832541544583, + "heading": 3.6840951991671647, + "angularVelocity": 0.7037714707580782, + "velocityX": 0.8471122889205226, + "velocityY": 1.3276201687211013, + "timestamp": 2.9047006461145273 + }, + { + "x": 2.4264268318884348, + "y": 6.670660345764589, + "heading": 3.714220994181463, + "angularVelocity": 0.5209265935078019, + "velocityX": 0.6573806388372562, + "velocityY": 0.98265006632375, + "timestamp": 2.9625318173193578 + }, + { + "x": 2.4524434068905587, + "y": 6.708118580157331, + "heading": 3.734065890519564, + "angularVelocity": 0.343152246870689, + "velocityX": 0.4498711414641213, + "velocityY": 0.6477170289370383, + "timestamp": 3.0203629885241883 + }, + { + "x": 2.4657256603240967, + "y": 6.7266645431518555, + "heading": 3.743879884568966, + "angularVelocity": 0.16970076595271175, + "velocityX": 0.22967291093749073, + "velocityY": 0.32069146462271875, + "timestamp": 3.0781941597290188 + }, + { + "x": 2.4657256603240967, + "y": 6.7266645431518555, + "heading": 3.743879884568966, + "angularVelocity": -1.4269853033258e-19, + "velocityX": 1.5862987647763193e-19, + "velocityY": -3.281295415241875e-19, + "timestamp": 3.1360253309338493 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/C543.1.traj b/src/main/deploy/choreo/C543.1.traj new file mode 100644 index 0000000..c98f495 --- /dev/null +++ b/src/main/deploy/choreo/C543.1.traj @@ -0,0 +1,373 @@ +{ + "samples": [ + { + "x": 1.8994296789169312, + "y": 4.536521911621094, + "heading": -0.31521512283034503, + "angularVelocity": 1.856559392483535e-29, + "velocityX": 2.4789185943182768e-27, + "velocityY": 5.839609049576754e-27, + "timestamp": 0 + }, + { + "x": 1.9781729924240876, + "y": 4.508277851628998, + "heading": -0.31370252846473695, + "angularVelocity": 0.015853113541521, + "velocityX": 0.8252884699611387, + "velocityY": -0.2960187477283183, + "timestamp": 0.09541307842440148 + }, + { + "x": 2.135659614712038, + "y": 4.451789733342148, + "heading": -0.3106773398781248, + "angularVelocity": 0.031706225567484585, + "velocityX": 1.6505768903864868, + "velocityY": -0.5920374776672492, + "timestamp": 0.19082615684880297 + }, + { + "x": 2.37188953160045, + "y": 4.367057561849908, + "heading": -0.3061395573376708, + "angularVelocity": 0.04755933479338926, + "velocityX": 2.4758651621914, + "velocityY": -0.8880561542658498, + "timestamp": 0.28623923527320444 + }, + { + "x": 2.6081194484995667, + "y": 4.282325390379507, + "heading": -0.3016017764892536, + "angularVelocity": 0.047559317059584855, + "velocityX": 2.475865162303598, + "velocityY": -0.888056154036968, + "timestamp": 0.38165231369760594 + }, + { + "x": 2.7656060707960486, + "y": 4.2258372721101445, + "heading": -0.2985765892352557, + "angularVelocity": 0.03170621160069682, + "velocityX": 1.650576890475903, + "velocityY": -0.5920374774839596, + "timestamp": 0.47706539212200744 + }, + { + "x": 2.8443493843078613, + "y": 4.1975932121276855, + "heading": -0.2970639955809382, + "angularVelocity": 0.01585310608666701, + "velocityX": 0.8252884700099399, + "velocityY": -0.296018747627325, + "timestamp": 0.5724784705464089 + }, + { + "x": 2.8443493843078613, + "y": 4.1975932121276855, + "heading": -0.2970639955809382, + "angularVelocity": -1.5327250624446661e-19, + "velocityX": 1.0883761859326842e-17, + "velocityY": 1.9079842229343454e-18, + "timestamp": 0 + }, + { + "x": 2.862655277787623, + "y": 4.166331737606692, + "heading": -0.2914406979851438, + "angularVelocity": 0.08949618859577375, + "velocityX": 0.291342876198692, + "velocityY": -0.49753419089995243, + "timestamp": 0.06283281650341255 + }, + { + "x": 2.899267048439286, + "y": 4.103808779235663, + "heading": -0.2801941781965803, + "angularVelocity": 0.178991177133531, + "velocityX": 0.5826854928537122, + "velocityY": -0.9950685302739267, + "timestamp": 0.1256656330068251 + }, + { + "x": 2.954184671588178, + "y": 4.010024323455344, + "heading": -0.2633244984279256, + "angularVelocity": 0.26848517554094714, + "velocityX": 0.8740277168051342, + "velocityY": -1.49260308544684, + "timestamp": 0.18849844951023764 + }, + { + "x": 3.0274081129021373, + "y": 3.8849783537261198, + "heading": -0.24083160924593064, + "angularVelocity": 0.35797996069089505, + "velocityX": 1.1653693943516235, + "velocityY": -1.9901379038518732, + "timestamp": 0.2513312660136502 + }, + { + "x": 3.118937326277501, + "y": 3.7286708524637473, + "heading": -0.2127152128195461, + "angularVelocity": 0.44747948589653275, + "velocityX": 1.456710338146768, + "velocityY": -2.4876730021145357, + "timestamp": 0.31416408251706274 + }, + { + "x": 3.2287722489867865, + "y": 3.541101804086028, + "heading": -0.17897457541099993, + "angularVelocity": 0.5369906887224499, + "velocityX": 1.7480502836183356, + "velocityY": -2.9852083483717826, + "timestamp": 0.3769968990204753 + }, + { + "x": 3.3569127792467097, + "y": 3.322271201658846, + "heading": -0.1396081808955156, + "angularVelocity": 0.6265260210536343, + "velocityX": 2.039388609183459, + "velocityY": -3.482743804987216, + "timestamp": 0.4398297155238877 + }, + { + "x": 3.5021716708255193, + "y": 3.0807779610466595, + "heading": -0.13960817173262668, + "angularVelocity": 1.4582967072784403e-7, + "velocityX": 2.3118316138276516, + "velocityY": -3.8434253635487554, + "timestamp": 0.5026625320273002 + }, + { + "x": 3.6769461709495137, + "y": 2.8597055646944223, + "heading": -0.1396081622048658, + "angularVelocity": 1.5163669897442772e-7, + "velocityX": 2.7815799107867174, + "velocityY": -3.518422516364424, + "timestamp": 0.5654953485307126 + }, + { + "x": 3.8786730073685605, + "y": 2.6629178305639787, + "heading": -0.13960815157343132, + "angularVelocity": 1.692019405455223e-7, + "velocityX": 3.2105330883604766, + "velocityY": -3.1319260393128068, + "timestamp": 0.628328165034125 + }, + { + "x": 4.104009176413799, + "y": 2.493676177995021, + "heading": -0.13960813870475786, + "angularVelocity": 2.0480815916440042e-7, + "velocityX": 3.5862815258812284, + "velocityY": -2.6935232572252708, + "timestamp": 0.6911609815375375 + }, + { + "x": 4.349220275878906, + "y": 2.354785442352295, + "heading": -0.13960812002410117, + "angularVelocity": 2.9730732619979236e-7, + "velocityX": 3.902596017668889, + "velocityY": -2.2104808183336777, + "timestamp": 0.7539937980409499 + }, + { + "x": 4.702138462369536, + "y": 2.2283930249367065, + "heading": -0.13960810657818157, + "angularVelocity": 1.608747054811707e-7, + "velocityX": 4.222515893538795, + "velocityY": -1.512231479673413, + "timestamp": 0.8375738710089728 + }, + { + "x": 5.071446484340789, + "y": 2.164066664880277, + "heading": -0.13960809680390723, + "angularVelocity": 1.1694503221449552e-7, + "velocityX": 4.418613299279326, + "velocityY": -0.769637519712364, + "timestamp": 0.9211539439769958 + }, + { + "x": 5.446314672461798, + "y": 2.1636924757230216, + "heading": -0.13960808851101342, + "angularVelocity": 9.922094519449148e-8, + "velocityX": 4.4851383207625375, + "velocityY": -0.004477014006162678, + "timestamp": 1.0047340169450187 + }, + { + "x": 5.817540831504603, + "y": 2.2158215275129325, + "heading": -0.13960808058309485, + "angularVelocity": 9.485417105515293e-8, + "velocityX": 4.441562992949752, + "velocityY": 0.6237019176785743, + "timestamp": 1.0883140899130417 + }, + { + "x": 6.188772462196687, + "y": 2.2679115992313883, + "heading": -0.13960807265505407, + "angularVelocity": 9.485563374022525e-8, + "velocityX": 4.441628458904423, + "velocityY": 0.6232355377151022, + "timestamp": 1.1718941628810646 + }, + { + "x": 6.560004093493723, + "y": 2.320001666638522, + "heading": -0.13960806472701331, + "angularVelocity": 9.485563327837965e-8, + "velocityX": 4.441628466142408, + "velocityY": 0.6232354861319785, + "timestamp": 1.2554742358490876 + }, + { + "x": 6.931235730753671, + "y": 2.3720916915356227, + "heading": -0.1396080567965601, + "angularVelocity": 9.488449751076888e-8, + "velocityX": 4.441628537486111, + "velocityY": 0.6232349775175141, + "timestamp": 1.3390543088171107 + }, + { + "x": 7.24868918264303, + "y": 2.416634284578295, + "heading": -0.09307656790464995, + "angularVelocity": 0.5567294600197072, + "velocityX": 3.798195438412857, + "velocityY": 0.5329331676911383, + "timestamp": 1.422634381785134 + }, + { + "x": 7.502651903227993, + "y": 2.4522684080177837, + "heading": -0.055847435634415084, + "angularVelocity": 0.44543072227840985, + "velocityX": 3.0385558610618424, + "velocityY": 0.4263471204810751, + "timestamp": 1.506214454753157 + }, + { + "x": 7.693123925820933, + "y": 2.478994027732418, + "heading": -0.027924037956266926, + "angularVelocity": 0.3340915685587962, + "velocityX": 2.278916682279189, + "velocityY": 0.3197606650197457, + "timestamp": 1.5897945277211802 + }, + { + "x": 7.820105269972474, + "y": 2.496811117044124, + "heading": -0.009307999867762826, + "angularVelocity": 0.22273297243501328, + "velocityX": 1.519277737411431, + "velocityY": 0.21317389036661577, + "timestamp": 1.6733746006892034 + }, + { + "x": 7.883595943450928, + "y": 2.5057196617126465, + "heading": 8.357390447577733e-19, + "angularVelocity": 0.11136625677897814, + "velocityX": 0.7596388854882326, + "velocityY": 0.10658694533489864, + "timestamp": 1.7569546736572266 + }, + { + "x": 7.883595943450928, + "y": 2.5057196617126465, + "heading": 4.17243901206371e-19, + "angularVelocity": -1.3675284747643194e-20, + "velocityX": -7.031289010518118e-19, + "velocityY": -4.150996756775015e-18, + "timestamp": 0 + }, + { + "x": 7.8360314621897995, + "y": 2.492731063817999, + "heading": -0.016719867482953874, + "angularVelocity": -0.2274817131266118, + "velocityX": -0.6471372869606266, + "velocityY": -0.17671602380817322, + "timestamp": 0.07349983105520197 + }, + { + "x": 7.740883620690362, + "y": 2.466748712680915, + "heading": -0.04965695904893551, + "angularVelocity": -0.44812472482071825, + "velocityX": -1.2945314313440597, + "velocityY": -0.3535021885638062, + "timestamp": 0.14699966211040394 + }, + { + "x": 7.598125002725434, + "y": 2.427765121657134, + "heading": -0.09805551215786533, + "angularVelocity": -0.6584852293412782, + "velocityX": -1.942298586478529, + "velocityY": -0.5303902126591636, + "timestamp": 0.2204994931656059 + }, + { + "x": 7.407713015886342, + "y": 2.375768659889871, + "heading": -0.16067295956756167, + "angularVelocity": -0.851940018238513, + "velocityX": -2.5906452314983186, + "velocityY": -0.707436480067701, + "timestamp": 0.2939993242208079 + }, + { + "x": 7.169573094076045, + "y": 2.310738965356344, + "heading": -0.2350465818507183, + "angularVelocity": -1.011888343352769, + "velocityX": -3.2400063835717154, + "velocityY": -0.8847597824365953, + "timestamp": 0.36749915527600985 + }, + { + "x": 6.883562344935437, + "y": 2.2326370179443717, + "heading": -0.31343736426140006, + "angularVelocity": -1.0665437088121537, + "velocityX": -3.891311653843135, + "velocityY": -1.0626139719057854, + "timestamp": 0.4409989863312118 + }, + { + "x": 6.597543030833985, + "y": 2.1545327316688314, + "heading": -0.3908962792736383, + "angularVelocity": -1.0538652116637235, + "velocityX": -3.891428184190491, + "velocityY": -1.0626457932519704, + "timestamp": 0.5144988173864138 + }, + { + "x": 6.359399124094035, + "y": 2.089501948956741, + "heading": -0.464859231230505, + "angularVelocity": -1.0063009791317343, + "velocityX": -3.240060600426301, + "velocityY": -0.8847745876211502, + "timestamp": 0.5879986484416158 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/C543.2.traj b/src/main/deploy/choreo/C543.2.traj new file mode 100644 index 0000000..b6c40cd --- /dev/null +++ b/src/main/deploy/choreo/C543.2.traj @@ -0,0 +1,571 @@ +{ + "samples": [ + { + "x": 6.359399124094035, + "y": 2.089501948956741, + "heading": -0.464859231230505, + "angularVelocity": -1.0063009791317343, + "velocityX": -3.240060600426301, + "velocityY": -0.8847745876211502, + "timestamp": 0 + }, + { + "x": 6.16898551010948, + "y": 2.037505042859222, + "heading": -0.5271660011253505, + "angularVelocity": -0.847713103558702, + "velocityX": -2.5906673695827256, + "velocityY": -0.7074425253912049, + "timestamp": 0.07349983105520197 + }, + { + "x": 6.02622555720923, + "y": 1.9985210873000216, + "heading": -0.5752994073778341, + "angularVelocity": -0.6548777808255529, + "velocityX": -1.9423167489055984, + "velocityY": -0.5303951723361273, + "timestamp": 0.14699966211040394 + }, + { + "x": 5.931076427258362, + "y": 1.972538384321036, + "heading": -0.6080315357610515, + "angularVelocity": -0.44533610367939375, + "velocityX": -1.2945489613357886, + "velocityY": -0.3535069755394575, + "timestamp": 0.2204994931656059 + }, + { + "x": 5.883511066436768, + "y": 1.9595495462417605, + "heading": -0.6246352364103855, + "angularVelocity": -0.2259012083560281, + "velocityX": -0.6471492537972026, + "velocityY": -0.17671929163374886, + "timestamp": 0.2939993242208079 + }, + { + "x": 5.883511066436768, + "y": 1.9595495462417605, + "heading": -0.6246352364103855, + "angularVelocity": 1.1039025848691765e-27, + "velocityX": 4.761636458443322e-19, + "velocityY": -7.033891840854684e-19, + "timestamp": -0.5879986484416158 + }, + { + "x": 5.9240926304528285, + "y": 1.9383422846339131, + "heading": -0.6075035710729456, + "angularVelocity": 0.24169946083236432, + "velocityX": 0.5725387432697568, + "velocityY": -0.2991993828612573, + "timestamp": -0.5171186171894071 + }, + { + "x": 6.005283451616443, + "y": 1.895913289440645, + "heading": -0.573965380823692, + "angularVelocity": 0.47316838969662034, + "velocityX": 1.145468190818343, + "velocityY": -0.5986029413883209, + "timestamp": -0.44623858593719845 + }, + { + "x": 6.127122045835839, + "y": 1.8322424328782785, + "heading": -0.5250879063858699, + "angularVelocity": 0.6895803172533067, + "velocityX": 1.7189410341237554, + "velocityY": -0.8982904696501893, + "timestamp": -0.3753585546849898 + }, + { + "x": 6.289665004875087, + "y": 1.747300141016517, + "heading": -0.4625905881083867, + "angularVelocity": 0.8817337855721652, + "velocityX": 2.29321229361314, + "velocityY": -1.1983952371510074, + "timestamp": -0.30447852343278115 + }, + { + "x": 6.492999718397039, + "y": 1.6410407454468894, + "heading": -0.3896566443511683, + "angularVelocity": 1.0289773081180171, + "velocityX": 2.868716476696195, + "velocityY": -1.4991443103563342, + "timestamp": -0.2335984921805725 + }, + { + "x": 6.737244110099108, + "y": 1.5134026211738723, + "heading": -0.3141954589351258, + "angularVelocity": 1.0646325076738916, + "velocityX": 3.4458843680949864, + "velocityY": -1.8007628103160618, + "timestamp": -0.16271846092836384 + }, + { + "x": 7.01729581033793, + "y": 1.3670521831193727, + "heading": -0.30876142690130054, + "angularVelocity": 0.07666520369453061, + "velocityX": 3.9510662635338036, + "velocityY": -2.0647626061809827, + "timestamp": -0.09183842967615519 + }, + { + "x": 7.26154055242165, + "y": 1.2394138757426358, + "heading": -0.2339960472360153, + "angularVelocity": 1.0548158394463982, + "velocityX": 3.4458893114005127, + "velocityY": -1.8007653936066776, + "timestamp": -0.020958398423946534 + }, + { + "x": 7.464873571099522, + "y": 1.1331553658708038, + "heading": -0.16137914172841886, + "angularVelocity": 1.0245044228212563, + "velocityX": 2.86869256524963, + "velocityY": -1.499131814625452, + "timestamp": 0.04992163282826212 + }, + { + "x": 7.627413516143391, + "y": 1.0482146490737314, + "heading": -0.09912632593980694, + "angularVelocity": 0.8782842598799258, + "velocityX": 2.293169771123716, + "velocityY": -1.1983730155935266, + "timestamp": 0.12080166408047077 + }, + { + "x": 7.749249434578068, + "y": 0.9845451908326943, + "heading": -0.05045468749596772, + "angularVelocity": 0.6866763118466136, + "velocityX": 1.7189032832273226, + "velocityY": -0.8982707416491623, + "timestamp": 0.19168169533267942 + }, + { + "x": 7.830438177887822, + "y": 0.9421172814918687, + "heading": -0.017061880048381956, + "angularVelocity": 0.471117278839311, + "velocityX": 1.145438875737296, + "velocityY": -0.5985876218064394, + "timestamp": 0.2625617265848881 + }, + { + "x": 7.871018409729004, + "y": 0.9209107160568237, + "heading": 9.50951307503468e-28, + "angularVelocity": 0.24071490583393818, + "velocityX": 0.5725199484857437, + "velocityY": -0.29918956101453714, + "timestamp": 0.33344175783709673 + }, + { + "x": 7.871018409729004, + "y": 0.9209107160568236, + "heading": 8.137519168881531e-28, + "angularVelocity": 2.362827828754998e-28, + "velocityX": -3.9701162432574964e-19, + "velocityY": -7.597092312783658e-19, + "timestamp": -0.5879986484416158 + }, + { + "x": 7.823797894101561, + "y": 0.9455873851909241, + "heading": -0.018482752277338615, + "angularVelocity": -0.24186482202526205, + "velocityX": -0.6179264558004075, + "velocityY": 0.32291825907407035, + "timestamp": -0.5115809531249811 + }, + { + "x": 7.729321961493796, + "y": 0.9949589623352963, + "heading": -0.054483658821666704, + "angularVelocity": -0.47110693923912733, + "velocityX": -1.2363096298089762, + "velocityY": 0.6460751916137003, + "timestamp": -0.4351632578083464 + }, + { + "x": 7.58754066258398, + "y": 1.069051550188512, + "heading": -0.10651385730845997, + "angularVelocity": -0.6808658422791649, + "velocityX": -1.8553464393599934, + "velocityY": 0.9695737033970333, + "timestamp": -0.35874556249171174 + }, + { + "x": 7.398377800873555, + "y": 1.167904967794458, + "heading": -0.17197008608174008, + "angularVelocity": -0.8565585300899715, + "velocityX": -2.4753803543359076, + "velocityY": 1.2935932861668868, + "timestamp": -0.2823278671750771 + }, + { + "x": 7.161716233228508, + "y": 1.2915804322389899, + "heading": -0.2453029387583975, + "angularVelocity": -0.9596318283717464, + "velocityX": -3.0969472013575574, + "velocityY": 1.6184139541513987, + "timestamp": -0.2059101718584424 + }, + { + "x": 6.877624632886902, + "y": 1.4400420560111074, + "heading": -0.30717062717430227, + "angularVelocity": -0.8095989830569665, + "velocityX": -3.717615392148104, + "velocityY": 1.9427649990878437, + "timestamp": -0.12949247654180773 + }, + { + "x": 6.593321734084911, + "y": 1.5886141009026509, + "heading": -0.3710961793728399, + "angularVelocity": -0.8365281357107608, + "velocityX": -3.7203804383786885, + "velocityY": 1.9442099670231996, + "timestamp": -0.05307478122517306 + }, + { + "x": 6.356540113748248, + "y": 1.712352302920254, + "heading": -0.44128880578466156, + "angularVelocity": -0.9185389080497671, + "velocityX": -3.098518207799423, + "velocityY": 1.619234936422734, + "timestamp": 0.02334291409146161 + }, + { + "x": 6.167245984491196, + "y": 1.8112743187990714, + "heading": -0.5029133636260013, + "angularVelocity": -0.8064173825970512, + "velocityX": -2.4770981180826768, + "velocityY": 1.2944909614054212, + "timestamp": 0.09976060940809628 + }, + { + "x": 6.025345568601143, + "y": 1.8854291552378972, + "heading": -0.5515597119481043, + "angularVelocity": -0.6365848658551928, + "velocityX": -1.8569052011067917, + "velocityY": 0.9703882867909994, + "timestamp": 0.17617830472473095 + }, + { + "x": 5.930780146751048, + "y": 1.9348474979966368, + "heading": -0.5851017568980736, + "angularVelocity": -0.4389303395108766, + "velocityX": -1.237480683737782, + "velocityY": 0.6466871652432838, + "timestamp": 0.2525960000413656 + }, + { + "x": 5.883511066436768, + "y": 1.9595495462417605, + "heading": -0.6022878896800522, + "angularVelocity": -0.22489729258083782, + "velocityX": -0.6185619720461629, + "velocityY": 0.32325036946967856, + "timestamp": 0.3290136953580003 + }, + { + "x": 5.883511066436768, + "y": 1.9595495462417605, + "heading": -0.6022878896800522, + "angularVelocity": -4.575007650652195e-29, + "velocityX": -4.1249629587352285e-19, + "velocityY": -5.74436540517081e-19, + "timestamp": -0.5879986484416158 + }, + { + "x": 5.9208106594012895, + "y": 1.9980075042526808, + "heading": -0.5853908134983155, + "angularVelocity": 0.22064510061169645, + "velocityX": 0.4870648835286477, + "velocityY": 0.502191025439745, + "timestamp": -0.5114183113532649 + }, + { + "x": 5.995436106794604, + "y": 2.074950497307001, + "heading": -0.5526457044120064, + "angularVelocity": 0.4275916029010293, + "velocityX": 0.9744726940444085, + "velocityY": 1.0047356277049464, + "timestamp": -0.434837974264914 + }, + { + "x": 6.10742397990414, + "y": 2.190416232439158, + "heading": -0.5056350196037447, + "angularVelocity": 0.6138740908651978, + "velocityX": 1.4623580590972747, + "velocityY": 1.5077726152986792, + "timestamp": -0.3582576371765631 + }, + { + "x": 6.256827319095995, + "y": 2.3444593972210575, + "heading": -0.44701478852585097, + "angularVelocity": 0.7654736621263952, + "velocityX": 1.9509360349183227, + "velocityY": 2.01152372317426, + "timestamp": -0.28167730008821223 + }, + { + "x": 6.443721455693994, + "y": 2.537157662443671, + "heading": -0.38225639564594427, + "angularVelocity": 0.845626897740015, + "velocityX": 2.4404977008964956, + "velocityY": 2.5162890703954135, + "timestamp": -0.20509696299986135 + }, + { + "x": 6.668014936261544, + "y": 2.768416734519728, + "heading": -0.3299586648556644, + "angularVelocity": 0.6829133009684187, + "velocityX": 2.9288651512304407, + "velocityY": 3.0198231147670853, + "timestamp": -0.12851662591151047 + }, + { + "x": 6.907145850728067, + "y": 3.014974027421391, + "heading": -0.3299586409292499, + "angularVelocity": 3.1243548135653975e-7, + "velocityX": 3.1226150675027364, + "velocityY": 3.2195900707150122, + "timestamp": -0.05193628882315959 + }, + { + "x": 7.146276765194591, + "y": 3.2615313203230536, + "heading": -0.32995861700283524, + "angularVelocity": 3.1243548345466456e-7, + "velocityX": 3.1226150675027333, + "velocityY": 3.2195900707150087, + "timestamp": 0.024644048265191287 + }, + { + "x": 7.3704559389808395, + "y": 3.492672535740464, + "heading": -0.2780946692696562, + "angularVelocity": 0.6772488827431452, + "velocityX": 2.927372512445502, + "velocityY": 3.018284121036746, + "timestamp": 0.10122438535354217 + }, + { + "x": 7.557410981992869, + "y": 3.6854335988685567, + "heading": -0.20676278837663634, + "angularVelocity": 0.9314647023651023, + "velocityX": 2.441293027952323, + "velocityY": 2.517109096891334, + "timestamp": 0.17780472244189305 + }, + { + "x": 7.706857595788799, + "y": 3.8395213821777414, + "heading": -0.13930245424786591, + "angularVelocity": 0.8809093390505888, + "velocityX": 1.9515011225859873, + "velocityY": 2.0121063600361744, + "timestamp": 0.25438505953024393 + }, + { + "x": 7.818870508401949, + "y": 3.955012934432877, + "heading": -0.08237188944912294, + "angularVelocity": 0.7434096918777223, + "velocityX": 1.4626850294994265, + "velocityY": 1.5081097399962253, + "timestamp": 0.3309653966185948 + }, + { + "x": 7.89350857290413, + "y": 4.031968936429238, + "heading": -0.039083183822383134, + "angularVelocity": 0.5652718083076284, + "velocityX": 0.9746374505517081, + "velocityY": 1.0049055008412506, + "timestamp": 0.4075457337069457 + }, + { + "x": 7.930812835693359, + "y": 4.070431709289551, + "heading": -0.011235090118986577, + "angularVelocity": 0.3636454834518186, + "velocityX": 0.4871258629508457, + "velocityY": 0.5022538986207112, + "timestamp": 0.48412607079529657 + }, + { + "x": 7.930812835693359, + "y": 4.070431709289551, + "heading": 6.119117243912058e-27, + "angularVelocity": 0.14670985459393632, + "velocityX": 2.1916141083836675e-16, + "velocityY": 2.2186710331296502e-16, + "timestamp": 0.5607064078836475 + }, + { + "x": 7.893341474761217, + "y": 4.032497740542634, + "heading": -0.00904576584972626, + "angularVelocity": -0.118238999129348, + "velocityX": -0.489795589034084, + "velocityY": -0.49584242751266944, + "timestamp": 0.6372104878939204 + }, + { + "x": 7.818363459726622, + "y": 3.9565940741618237, + "heading": -0.03724245303127927, + "angularVelocity": -0.3685644893418323, + "velocityX": -0.9800525020956713, + "velocityY": -0.9921518743917612, + "timestamp": 0.7137145679041934 + }, + { + "x": 7.705828672647867, + "y": 3.842669973467507, + "heading": -0.0828753801074409, + "angularVelocity": -0.5964770384799597, + "velocityX": -1.4709645167113123, + "velocityY": -1.4891245104708004, + "timestamp": 0.7902186479144664 + }, + { + "x": 7.555661516697107, + "y": 3.690648908340853, + "heading": -0.14296397524076854, + "angularVelocity": -0.7854299421058165, + "velocityX": -1.9628646724540015, + "velocityY": -1.9870974869084101, + "timestamp": 0.8667227279247394 + }, + { + "x": 7.367747717848417, + "y": 3.500415193970309, + "heading": -0.2110723502689409, + "angularVelocity": -0.8902580753735824, + "velocityX": -2.4562585266492567, + "velocityY": -2.4865826024572626, + "timestamp": 0.9432268079350123 + }, + { + "x": 7.142210019187645, + "y": 3.27209308878329, + "heading": -0.2643456870251724, + "angularVelocity": -0.6963463484441335, + "velocityX": -2.94804798163034, + "velocityY": -2.9844435114618633, + "timestamp": 1.0197308879452853 + }, + { + "x": 6.901072440933301, + "y": 3.0279785135541535, + "heading": -0.2643457148851664, + "angularVelocity": -3.641635060622556e-7, + "velocityX": -3.151957101137135, + "velocityY": -3.19087001891084, + "timestamp": 1.0962349679555583 + }, + { + "x": 6.659934862678956, + "y": 2.783863938325017, + "heading": -0.26434574274516087, + "angularVelocity": -3.641635119748583e-7, + "velocityX": -3.1519571011371346, + "velocityY": -3.1908700189108394, + "timestamp": 1.1727390479658313 + }, + { + "x": 6.434341905968575, + "y": 2.555485892892574, + "heading": -0.31825319174835115, + "angularVelocity": -0.7046349553638372, + "velocityX": -2.948770270554044, + "velocityY": -2.9851747175023284, + "timestamp": 1.2492431279761043 + }, + { + "x": 6.246271560267497, + "y": 2.3650936989990523, + "heading": -0.3908662829242579, + "angularVelocity": -0.9491401133920705, + "velocityX": -2.45830478159887, + "velocityY": -2.4886541197274097, + "timestamp": 1.3257472079863772 + }, + { + "x": 6.095971059225887, + "y": 2.212937642551479, + "heading": -0.45757371770508454, + "angularVelocity": -0.8719461076045745, + "velocityX": -1.9646076525778429, + "velocityY": -1.9888619852319178, + "timestamp": 1.4022512879966502 + }, + { + "x": 5.983338099139899, + "y": 2.098914156841632, + "heading": -0.5114190364388432, + "angularVelocity": -0.7038228382921298, + "velocityX": -1.4722477555558267, + "velocityY": -1.490423591716096, + "timestamp": 1.4787553680069232 + }, + { + "x": 5.908297013054152, + "y": 2.022946640757376, + "heading": -0.5490598757520303, + "angularVelocity": -0.4920108745590113, + "velocityX": -0.9808769163117926, + "velocityY": -0.9929864665264183, + "timestamp": 1.5552594480171962 + }, + { + "x": 5.870795249938965, + "y": 1.984981894493103, + "heading": -0.5685244655425081, + "angularVelocity": -0.2544255128336178, + "velocityX": -0.49019298199717704, + "velocityY": -0.49624472654497614, + "timestamp": 1.6317635280274692 + }, + { + "x": 5.870795249938965, + "y": 1.984981894493103, + "heading": -0.5685244655425081, + "angularVelocity": 2.232507914728541e-27, + "velocityX": 2.548089958327824e-19, + "velocityY": -2.5170158061752717e-19, + "timestamp": 7.1579421839986 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/C543.traj b/src/main/deploy/choreo/C543.traj new file mode 100644 index 0000000..9c00580 --- /dev/null +++ b/src/main/deploy/choreo/C543.traj @@ -0,0 +1,931 @@ +{ + "samples": [ + { + "x": 1.8994296789169312, + "y": 4.536521911621094, + "heading": -0.31521512283034503, + "angularVelocity": 1.856559392483535e-29, + "velocityX": 2.4789185943182768e-27, + "velocityY": 5.839609049576754e-27, + "timestamp": 0 + }, + { + "x": 1.9781729924240876, + "y": 4.508277851628998, + "heading": -0.31370252846473695, + "angularVelocity": 0.015853113541521, + "velocityX": 0.8252884699611387, + "velocityY": -0.2960187477283183, + "timestamp": 0.09541307842440148 + }, + { + "x": 2.135659614712038, + "y": 4.451789733342148, + "heading": -0.3106773398781248, + "angularVelocity": 0.031706225567484585, + "velocityX": 1.6505768903864868, + "velocityY": -0.5920374776672492, + "timestamp": 0.19082615684880297 + }, + { + "x": 2.37188953160045, + "y": 4.367057561849908, + "heading": -0.3061395573376708, + "angularVelocity": 0.04755933479338926, + "velocityX": 2.4758651621914, + "velocityY": -0.8880561542658498, + "timestamp": 0.28623923527320444 + }, + { + "x": 2.6081194484995667, + "y": 4.282325390379507, + "heading": -0.3016017764892536, + "angularVelocity": 0.047559317059584855, + "velocityX": 2.475865162303598, + "velocityY": -0.888056154036968, + "timestamp": 0.38165231369760594 + }, + { + "x": 2.7656060707960486, + "y": 4.2258372721101445, + "heading": -0.2985765892352557, + "angularVelocity": 0.03170621160069682, + "velocityX": 1.650576890475903, + "velocityY": -0.5920374774839596, + "timestamp": 0.47706539212200744 + }, + { + "x": 2.8443493843078613, + "y": 4.1975932121276855, + "heading": -0.2970639955809382, + "angularVelocity": 0.01585310608666701, + "velocityX": 0.8252884700099399, + "velocityY": -0.296018747627325, + "timestamp": 0.5724784705464089 + }, + { + "x": 2.8443493843078613, + "y": 4.1975932121276855, + "heading": -0.2970639955809382, + "angularVelocity": -1.5327250624446661e-19, + "velocityX": 1.0883761859326842e-17, + "velocityY": 1.9079842229343454e-18, + "timestamp": 0 + }, + { + "x": 2.862655277787623, + "y": 4.166331737606692, + "heading": -0.2914406979851438, + "angularVelocity": 0.08949618859577375, + "velocityX": 0.291342876198692, + "velocityY": -0.49753419089995243, + "timestamp": 0.06283281650341255 + }, + { + "x": 2.899267048439286, + "y": 4.103808779235663, + "heading": -0.2801941781965803, + "angularVelocity": 0.178991177133531, + "velocityX": 0.5826854928537122, + "velocityY": -0.9950685302739267, + "timestamp": 0.1256656330068251 + }, + { + "x": 2.954184671588178, + "y": 4.010024323455344, + "heading": -0.2633244984279256, + "angularVelocity": 0.26848517554094714, + "velocityX": 0.8740277168051342, + "velocityY": -1.49260308544684, + "timestamp": 0.18849844951023764 + }, + { + "x": 3.0274081129021373, + "y": 3.8849783537261198, + "heading": -0.24083160924593064, + "angularVelocity": 0.35797996069089505, + "velocityX": 1.1653693943516235, + "velocityY": -1.9901379038518732, + "timestamp": 0.2513312660136502 + }, + { + "x": 3.118937326277501, + "y": 3.7286708524637473, + "heading": -0.2127152128195461, + "angularVelocity": 0.44747948589653275, + "velocityX": 1.456710338146768, + "velocityY": -2.4876730021145357, + "timestamp": 0.31416408251706274 + }, + { + "x": 3.2287722489867865, + "y": 3.541101804086028, + "heading": -0.17897457541099993, + "angularVelocity": 0.5369906887224499, + "velocityX": 1.7480502836183356, + "velocityY": -2.9852083483717826, + "timestamp": 0.3769968990204753 + }, + { + "x": 3.3569127792467097, + "y": 3.322271201658846, + "heading": -0.1396081808955156, + "angularVelocity": 0.6265260210536343, + "velocityX": 2.039388609183459, + "velocityY": -3.482743804987216, + "timestamp": 0.4398297155238877 + }, + { + "x": 3.5021716708255193, + "y": 3.0807779610466595, + "heading": -0.13960817173262668, + "angularVelocity": 1.4582967072784403e-7, + "velocityX": 2.3118316138276516, + "velocityY": -3.8434253635487554, + "timestamp": 0.5026625320273002 + }, + { + "x": 3.6769461709495137, + "y": 2.8597055646944223, + "heading": -0.1396081622048658, + "angularVelocity": 1.5163669897442772e-7, + "velocityX": 2.7815799107867174, + "velocityY": -3.518422516364424, + "timestamp": 0.5654953485307126 + }, + { + "x": 3.8786730073685605, + "y": 2.6629178305639787, + "heading": -0.13960815157343132, + "angularVelocity": 1.692019405455223e-7, + "velocityX": 3.2105330883604766, + "velocityY": -3.1319260393128068, + "timestamp": 0.628328165034125 + }, + { + "x": 4.104009176413799, + "y": 2.493676177995021, + "heading": -0.13960813870475786, + "angularVelocity": 2.0480815916440042e-7, + "velocityX": 3.5862815258812284, + "velocityY": -2.6935232572252708, + "timestamp": 0.6911609815375375 + }, + { + "x": 4.349220275878906, + "y": 2.354785442352295, + "heading": -0.13960812002410117, + "angularVelocity": 2.9730732619979236e-7, + "velocityX": 3.902596017668889, + "velocityY": -2.2104808183336777, + "timestamp": 0.7539937980409499 + }, + { + "x": 4.702138462369536, + "y": 2.2283930249367065, + "heading": -0.13960810657818157, + "angularVelocity": 1.608747054811707e-7, + "velocityX": 4.222515893538795, + "velocityY": -1.512231479673413, + "timestamp": 0.8375738710089728 + }, + { + "x": 5.071446484340789, + "y": 2.164066664880277, + "heading": -0.13960809680390723, + "angularVelocity": 1.1694503221449552e-7, + "velocityX": 4.418613299279326, + "velocityY": -0.769637519712364, + "timestamp": 0.9211539439769958 + }, + { + "x": 5.446314672461798, + "y": 2.1636924757230216, + "heading": -0.13960808851101342, + "angularVelocity": 9.922094519449148e-8, + "velocityX": 4.4851383207625375, + "velocityY": -0.004477014006162678, + "timestamp": 1.0047340169450187 + }, + { + "x": 5.817540831504603, + "y": 2.2158215275129325, + "heading": -0.13960808058309485, + "angularVelocity": 9.485417105515293e-8, + "velocityX": 4.441562992949752, + "velocityY": 0.6237019176785743, + "timestamp": 1.0883140899130417 + }, + { + "x": 6.188772462196687, + "y": 2.2679115992313883, + "heading": -0.13960807265505407, + "angularVelocity": 9.485563374022525e-8, + "velocityX": 4.441628458904423, + "velocityY": 0.6232355377151022, + "timestamp": 1.1718941628810646 + }, + { + "x": 6.560004093493723, + "y": 2.320001666638522, + "heading": -0.13960806472701331, + "angularVelocity": 9.485563327837965e-8, + "velocityX": 4.441628466142408, + "velocityY": 0.6232354861319785, + "timestamp": 1.2554742358490876 + }, + { + "x": 6.931235730753671, + "y": 2.3720916915356227, + "heading": -0.1396080567965601, + "angularVelocity": 9.488449751076888e-8, + "velocityX": 4.441628537486111, + "velocityY": 0.6232349775175141, + "timestamp": 1.3390543088171107 + }, + { + "x": 7.24868918264303, + "y": 2.416634284578295, + "heading": -0.09307656790464995, + "angularVelocity": 0.5567294600197072, + "velocityX": 3.798195438412857, + "velocityY": 0.5329331676911383, + "timestamp": 1.422634381785134 + }, + { + "x": 7.502651903227993, + "y": 2.4522684080177837, + "heading": -0.055847435634415084, + "angularVelocity": 0.44543072227840985, + "velocityX": 3.0385558610618424, + "velocityY": 0.4263471204810751, + "timestamp": 1.506214454753157 + }, + { + "x": 7.693123925820933, + "y": 2.478994027732418, + "heading": -0.027924037956266926, + "angularVelocity": 0.3340915685587962, + "velocityX": 2.278916682279189, + "velocityY": 0.3197606650197457, + "timestamp": 1.5897945277211802 + }, + { + "x": 7.820105269972474, + "y": 2.496811117044124, + "heading": -0.009307999867762826, + "angularVelocity": 0.22273297243501328, + "velocityX": 1.519277737411431, + "velocityY": 0.21317389036661577, + "timestamp": 1.6733746006892034 + }, + { + "x": 7.883595943450928, + "y": 2.5057196617126465, + "heading": 8.357390447577733e-19, + "angularVelocity": 0.11136625677897814, + "velocityX": 0.7596388854882326, + "velocityY": 0.10658694533489864, + "timestamp": 1.7569546736572266 + }, + { + "x": 7.883595943450928, + "y": 2.5057196617126465, + "heading": 4.17243901206371e-19, + "angularVelocity": -1.3675284747643194e-20, + "velocityX": -7.031289010518118e-19, + "velocityY": -4.150996756775015e-18, + "timestamp": 0 + }, + { + "x": 7.8360314621897995, + "y": 2.492731063817999, + "heading": -0.016719867482953874, + "angularVelocity": -0.2274817131266118, + "velocityX": -0.6471372869606266, + "velocityY": -0.17671602380817322, + "timestamp": 0.07349983105520197 + }, + { + "x": 7.740883620690362, + "y": 2.466748712680915, + "heading": -0.04965695904893551, + "angularVelocity": -0.44812472482071825, + "velocityX": -1.2945314313440597, + "velocityY": -0.3535021885638062, + "timestamp": 0.14699966211040394 + }, + { + "x": 7.598125002725434, + "y": 2.427765121657134, + "heading": -0.09805551215786533, + "angularVelocity": -0.6584852293412782, + "velocityX": -1.942298586478529, + "velocityY": -0.5303902126591636, + "timestamp": 0.2204994931656059 + }, + { + "x": 7.407713015886342, + "y": 2.375768659889871, + "heading": -0.16067295956756167, + "angularVelocity": -0.851940018238513, + "velocityX": -2.5906452314983186, + "velocityY": -0.707436480067701, + "timestamp": 0.2939993242208079 + }, + { + "x": 7.169573094076045, + "y": 2.310738965356344, + "heading": -0.2350465818507183, + "angularVelocity": -1.011888343352769, + "velocityX": -3.2400063835717154, + "velocityY": -0.8847597824365953, + "timestamp": 0.36749915527600985 + }, + { + "x": 6.883562344935437, + "y": 2.2326370179443717, + "heading": -0.31343736426140006, + "angularVelocity": -1.0665437088121537, + "velocityX": -3.891311653843135, + "velocityY": -1.0626139719057854, + "timestamp": 0.4409989863312118 + }, + { + "x": 6.597543030833985, + "y": 2.1545327316688314, + "heading": -0.3908962792736383, + "angularVelocity": -1.0538652116637235, + "velocityX": -3.891428184190491, + "velocityY": -1.0626457932519704, + "timestamp": 0.5144988173864138 + }, + { + "x": 6.359399124094035, + "y": 2.089501948956741, + "heading": -0.464859231230505, + "angularVelocity": -1.0063009791317343, + "velocityX": -3.240060600426301, + "velocityY": -0.8847745876211502, + "timestamp": 0.5879986484416158 + }, + { + "x": 6.16898551010948, + "y": 2.037505042859222, + "heading": -0.5271660011253505, + "angularVelocity": -0.847713103558702, + "velocityX": -2.5906673695827256, + "velocityY": -0.7074425253912049, + "timestamp": 0.6614984794968177 + }, + { + "x": 6.02622555720923, + "y": 1.9985210873000216, + "heading": -0.5752994073778341, + "angularVelocity": -0.6548777808255529, + "velocityX": -1.9423167489055984, + "velocityY": -0.5303951723361273, + "timestamp": 0.7349983105520197 + }, + { + "x": 5.931076427258362, + "y": 1.972538384321036, + "heading": -0.6080315357610515, + "angularVelocity": -0.44533610367939375, + "velocityX": -1.2945489613357886, + "velocityY": -0.3535069755394575, + "timestamp": 0.8084981416072217 + }, + { + "x": 5.883511066436768, + "y": 1.9595495462417605, + "heading": -0.6246352364103855, + "angularVelocity": -0.2259012083560281, + "velocityX": -0.6471492537972026, + "velocityY": -0.17671929163374886, + "timestamp": 0.8819979726624236 + }, + { + "x": 5.883511066436768, + "y": 1.9595495462417605, + "heading": -0.6246352364103855, + "angularVelocity": 1.1039025848691765e-27, + "velocityX": 4.761636458443322e-19, + "velocityY": -7.033891840854684e-19, + "timestamp": 0 + }, + { + "x": 5.9240926304528285, + "y": 1.9383422846339131, + "heading": -0.6075035710729456, + "angularVelocity": 0.24169946083236432, + "velocityX": 0.5725387432697568, + "velocityY": -0.2991993828612573, + "timestamp": 0.07088003125220865 + }, + { + "x": 6.005283451616443, + "y": 1.895913289440645, + "heading": -0.573965380823692, + "angularVelocity": 0.47316838969662034, + "velocityX": 1.145468190818343, + "velocityY": -0.5986029413883209, + "timestamp": 0.1417600625044173 + }, + { + "x": 6.127122045835839, + "y": 1.8322424328782785, + "heading": -0.5250879063858699, + "angularVelocity": 0.6895803172533067, + "velocityX": 1.7189410341237554, + "velocityY": -0.8982904696501893, + "timestamp": 0.21264009375662596 + }, + { + "x": 6.289665004875087, + "y": 1.747300141016517, + "heading": -0.4625905881083867, + "angularVelocity": 0.8817337855721652, + "velocityX": 2.29321229361314, + "velocityY": -1.1983952371510074, + "timestamp": 0.2835201250088346 + }, + { + "x": 6.492999718397039, + "y": 1.6410407454468894, + "heading": -0.3896566443511683, + "angularVelocity": 1.0289773081180171, + "velocityX": 2.868716476696195, + "velocityY": -1.4991443103563342, + "timestamp": 0.35440015626104326 + }, + { + "x": 6.737244110099108, + "y": 1.5134026211738723, + "heading": -0.3141954589351258, + "angularVelocity": 1.0646325076738916, + "velocityX": 3.4458843680949864, + "velocityY": -1.8007628103160618, + "timestamp": 0.4252801875132519 + }, + { + "x": 7.01729581033793, + "y": 1.3670521831193727, + "heading": -0.30876142690130054, + "angularVelocity": 0.07666520369453061, + "velocityX": 3.9510662635338036, + "velocityY": -2.0647626061809827, + "timestamp": 0.49616021876546057 + }, + { + "x": 7.26154055242165, + "y": 1.2394138757426358, + "heading": -0.2339960472360153, + "angularVelocity": 1.0548158394463982, + "velocityX": 3.4458893114005127, + "velocityY": -1.8007653936066776, + "timestamp": 0.5670402500176692 + }, + { + "x": 7.464873571099522, + "y": 1.1331553658708038, + "heading": -0.16137914172841886, + "angularVelocity": 1.0245044228212563, + "velocityX": 2.86869256524963, + "velocityY": -1.499131814625452, + "timestamp": 0.6379202812698779 + }, + { + "x": 7.627413516143391, + "y": 1.0482146490737314, + "heading": -0.09912632593980694, + "angularVelocity": 0.8782842598799258, + "velocityX": 2.293169771123716, + "velocityY": -1.1983730155935266, + "timestamp": 0.7088003125220865 + }, + { + "x": 7.749249434578068, + "y": 0.9845451908326943, + "heading": -0.05045468749596772, + "angularVelocity": 0.6866763118466136, + "velocityX": 1.7189032832273226, + "velocityY": -0.8982707416491623, + "timestamp": 0.7796803437742952 + }, + { + "x": 7.830438177887822, + "y": 0.9421172814918687, + "heading": -0.017061880048381956, + "angularVelocity": 0.471117278839311, + "velocityX": 1.145438875737296, + "velocityY": -0.5985876218064394, + "timestamp": 0.8505603750265038 + }, + { + "x": 7.871018409729004, + "y": 0.9209107160568237, + "heading": 9.50951307503468e-28, + "angularVelocity": 0.24071490583393818, + "velocityX": 0.5725199484857437, + "velocityY": -0.29918956101453714, + "timestamp": 0.9214404062787125 + }, + { + "x": 7.871018409729004, + "y": 0.9209107160568236, + "heading": 8.137519168881531e-28, + "angularVelocity": 2.362827828754998e-28, + "velocityX": -3.9701162432574964e-19, + "velocityY": -7.597092312783658e-19, + "timestamp": 0 + }, + { + "x": 7.823797894101561, + "y": 0.9455873851909241, + "heading": -0.018482752277338615, + "angularVelocity": -0.24186482202526205, + "velocityX": -0.6179264558004075, + "velocityY": 0.32291825907407035, + "timestamp": 0.07641769531663467 + }, + { + "x": 7.729321961493796, + "y": 0.9949589623352963, + "heading": -0.054483658821666704, + "angularVelocity": -0.47110693923912733, + "velocityX": -1.2363096298089762, + "velocityY": 0.6460751916137003, + "timestamp": 0.15283539063326934 + }, + { + "x": 7.58754066258398, + "y": 1.069051550188512, + "heading": -0.10651385730845997, + "angularVelocity": -0.6808658422791649, + "velocityX": -1.8553464393599934, + "velocityY": 0.9695737033970333, + "timestamp": 0.229253085949904 + }, + { + "x": 7.398377800873555, + "y": 1.167904967794458, + "heading": -0.17197008608174008, + "angularVelocity": -0.8565585300899715, + "velocityX": -2.4753803543359076, + "velocityY": 1.2935932861668868, + "timestamp": 0.3056707812665387 + }, + { + "x": 7.161716233228508, + "y": 1.2915804322389899, + "heading": -0.2453029387583975, + "angularVelocity": -0.9596318283717464, + "velocityX": -3.0969472013575574, + "velocityY": 1.6184139541513987, + "timestamp": 0.38208847658317335 + }, + { + "x": 6.877624632886902, + "y": 1.4400420560111074, + "heading": -0.30717062717430227, + "angularVelocity": -0.8095989830569665, + "velocityX": -3.717615392148104, + "velocityY": 1.9427649990878437, + "timestamp": 0.458506171899808 + }, + { + "x": 6.593321734084911, + "y": 1.5886141009026509, + "heading": -0.3710961793728399, + "angularVelocity": -0.8365281357107608, + "velocityX": -3.7203804383786885, + "velocityY": 1.9442099670231996, + "timestamp": 0.5349238672164427 + }, + { + "x": 6.356540113748248, + "y": 1.712352302920254, + "heading": -0.44128880578466156, + "angularVelocity": -0.9185389080497671, + "velocityX": -3.098518207799423, + "velocityY": 1.619234936422734, + "timestamp": 0.6113415625330774 + }, + { + "x": 6.167245984491196, + "y": 1.8112743187990714, + "heading": -0.5029133636260013, + "angularVelocity": -0.8064173825970512, + "velocityX": -2.4770981180826768, + "velocityY": 1.2944909614054212, + "timestamp": 0.687759257849712 + }, + { + "x": 6.025345568601143, + "y": 1.8854291552378972, + "heading": -0.5515597119481043, + "angularVelocity": -0.6365848658551928, + "velocityX": -1.8569052011067917, + "velocityY": 0.9703882867909994, + "timestamp": 0.7641769531663467 + }, + { + "x": 5.930780146751048, + "y": 1.9348474979966368, + "heading": -0.5851017568980736, + "angularVelocity": -0.4389303395108766, + "velocityX": -1.237480683737782, + "velocityY": 0.6466871652432838, + "timestamp": 0.8405946484829814 + }, + { + "x": 5.883511066436768, + "y": 1.9595495462417605, + "heading": -0.6022878896800522, + "angularVelocity": -0.22489729258083782, + "velocityX": -0.6185619720461629, + "velocityY": 0.32325036946967856, + "timestamp": 0.917012343799616 + }, + { + "x": 5.883511066436768, + "y": 1.9595495462417605, + "heading": -0.6022878896800522, + "angularVelocity": -4.575007650652195e-29, + "velocityX": -4.1249629587352285e-19, + "velocityY": -5.74436540517081e-19, + "timestamp": 0 + }, + { + "x": 5.9208106594012895, + "y": 1.9980075042526808, + "heading": -0.5853908134983155, + "angularVelocity": 0.22064510061169645, + "velocityX": 0.4870648835286477, + "velocityY": 0.502191025439745, + "timestamp": 0.07658033708835088 + }, + { + "x": 5.995436106794604, + "y": 2.074950497307001, + "heading": -0.5526457044120064, + "angularVelocity": 0.4275916029010293, + "velocityX": 0.9744726940444085, + "velocityY": 1.0047356277049464, + "timestamp": 0.15316067417670176 + }, + { + "x": 6.10742397990414, + "y": 2.190416232439158, + "heading": -0.5056350196037447, + "angularVelocity": 0.6138740908651978, + "velocityX": 1.4623580590972747, + "velocityY": 1.5077726152986792, + "timestamp": 0.22974101126505264 + }, + { + "x": 6.256827319095995, + "y": 2.3444593972210575, + "heading": -0.44701478852585097, + "angularVelocity": 0.7654736621263952, + "velocityX": 1.9509360349183227, + "velocityY": 2.01152372317426, + "timestamp": 0.3063213483534035 + }, + { + "x": 6.443721455693994, + "y": 2.537157662443671, + "heading": -0.38225639564594427, + "angularVelocity": 0.845626897740015, + "velocityX": 2.4404977008964956, + "velocityY": 2.5162890703954135, + "timestamp": 0.3829016854417544 + }, + { + "x": 6.668014936261544, + "y": 2.768416734519728, + "heading": -0.3299586648556644, + "angularVelocity": 0.6829133009684187, + "velocityX": 2.9288651512304407, + "velocityY": 3.0198231147670853, + "timestamp": 0.4594820225301053 + }, + { + "x": 6.907145850728067, + "y": 3.014974027421391, + "heading": -0.3299586409292499, + "angularVelocity": 3.1243548135653975e-7, + "velocityX": 3.1226150675027364, + "velocityY": 3.2195900707150122, + "timestamp": 0.5360623596184562 + }, + { + "x": 7.146276765194591, + "y": 3.2615313203230536, + "heading": -0.32995861700283524, + "angularVelocity": 3.1243548345466456e-7, + "velocityX": 3.1226150675027333, + "velocityY": 3.2195900707150087, + "timestamp": 0.612642696706807 + }, + { + "x": 7.3704559389808395, + "y": 3.492672535740464, + "heading": -0.2780946692696562, + "angularVelocity": 0.6772488827431452, + "velocityX": 2.927372512445502, + "velocityY": 3.018284121036746, + "timestamp": 0.6892230337951579 + }, + { + "x": 7.557410981992869, + "y": 3.6854335988685567, + "heading": -0.20676278837663634, + "angularVelocity": 0.9314647023651023, + "velocityX": 2.441293027952323, + "velocityY": 2.517109096891334, + "timestamp": 0.7658033708835088 + }, + { + "x": 7.706857595788799, + "y": 3.8395213821777414, + "heading": -0.13930245424786591, + "angularVelocity": 0.8809093390505888, + "velocityX": 1.9515011225859873, + "velocityY": 2.0121063600361744, + "timestamp": 0.8423837079718597 + }, + { + "x": 7.818870508401949, + "y": 3.955012934432877, + "heading": -0.08237188944912294, + "angularVelocity": 0.7434096918777223, + "velocityX": 1.4626850294994265, + "velocityY": 1.5081097399962253, + "timestamp": 0.9189640450602106 + }, + { + "x": 7.89350857290413, + "y": 4.031968936429238, + "heading": -0.039083183822383134, + "angularVelocity": 0.5652718083076284, + "velocityX": 0.9746374505517081, + "velocityY": 1.0049055008412506, + "timestamp": 0.9955443821485614 + }, + { + "x": 7.930812835693359, + "y": 4.070431709289551, + "heading": -0.011235090118986577, + "angularVelocity": 0.3636454834518186, + "velocityX": 0.4871258629508457, + "velocityY": 0.5022538986207112, + "timestamp": 1.0721247192369123 + }, + { + "x": 7.930812835693359, + "y": 4.070431709289551, + "heading": 6.119117243912058e-27, + "angularVelocity": 0.14670985459393632, + "velocityX": 2.1916141083836675e-16, + "velocityY": 2.2186710331296502e-16, + "timestamp": 1.1487050563252632 + }, + { + "x": 7.893341474761217, + "y": 4.032497740542634, + "heading": -0.00904576584972626, + "angularVelocity": -0.118238999129348, + "velocityX": -0.489795589034084, + "velocityY": -0.49584242751266944, + "timestamp": 1.2252091363355362 + }, + { + "x": 7.818363459726622, + "y": 3.9565940741618237, + "heading": -0.03724245303127927, + "angularVelocity": -0.3685644893418323, + "velocityX": -0.9800525020956713, + "velocityY": -0.9921518743917612, + "timestamp": 1.3017132163458092 + }, + { + "x": 7.705828672647867, + "y": 3.842669973467507, + "heading": -0.0828753801074409, + "angularVelocity": -0.5964770384799597, + "velocityX": -1.4709645167113123, + "velocityY": -1.4891245104708004, + "timestamp": 1.3782172963560821 + }, + { + "x": 7.555661516697107, + "y": 3.690648908340853, + "heading": -0.14296397524076854, + "angularVelocity": -0.7854299421058165, + "velocityX": -1.9628646724540015, + "velocityY": -1.9870974869084101, + "timestamp": 1.4547213763663551 + }, + { + "x": 7.367747717848417, + "y": 3.500415193970309, + "heading": -0.2110723502689409, + "angularVelocity": -0.8902580753735824, + "velocityX": -2.4562585266492567, + "velocityY": -2.4865826024572626, + "timestamp": 1.531225456376628 + }, + { + "x": 7.142210019187645, + "y": 3.27209308878329, + "heading": -0.2643456870251724, + "angularVelocity": -0.6963463484441335, + "velocityX": -2.94804798163034, + "velocityY": -2.9844435114618633, + "timestamp": 1.607729536386901 + }, + { + "x": 6.901072440933301, + "y": 3.0279785135541535, + "heading": -0.2643457148851664, + "angularVelocity": -3.641635060622556e-7, + "velocityX": -3.151957101137135, + "velocityY": -3.19087001891084, + "timestamp": 1.684233616397174 + }, + { + "x": 6.659934862678956, + "y": 2.783863938325017, + "heading": -0.26434574274516087, + "angularVelocity": -3.641635119748583e-7, + "velocityX": -3.1519571011371346, + "velocityY": -3.1908700189108394, + "timestamp": 1.760737696407447 + }, + { + "x": 6.434341905968575, + "y": 2.555485892892574, + "heading": -0.31825319174835115, + "angularVelocity": -0.7046349553638372, + "velocityX": -2.948770270554044, + "velocityY": -2.9851747175023284, + "timestamp": 1.83724177641772 + }, + { + "x": 6.246271560267497, + "y": 2.3650936989990523, + "heading": -0.3908662829242579, + "angularVelocity": -0.9491401133920705, + "velocityX": -2.45830478159887, + "velocityY": -2.4886541197274097, + "timestamp": 1.913745856427993 + }, + { + "x": 6.095971059225887, + "y": 2.212937642551479, + "heading": -0.45757371770508454, + "angularVelocity": -0.8719461076045745, + "velocityX": -1.9646076525778429, + "velocityY": -1.9888619852319178, + "timestamp": 1.990249936438266 + }, + { + "x": 5.983338099139899, + "y": 2.098914156841632, + "heading": -0.5114190364388432, + "angularVelocity": -0.7038228382921298, + "velocityX": -1.4722477555558267, + "velocityY": -1.490423591716096, + "timestamp": 2.066754016448539 + }, + { + "x": 5.908297013054152, + "y": 2.022946640757376, + "heading": -0.5490598757520303, + "angularVelocity": -0.4920108745590113, + "velocityX": -0.9808769163117926, + "velocityY": -0.9929864665264183, + "timestamp": 2.143258096458812 + }, + { + "x": 5.870795249938965, + "y": 1.984981894493103, + "heading": -0.5685244655425081, + "angularVelocity": -0.2544255128336178, + "velocityX": -0.49019298199717704, + "velocityY": -0.49624472654497614, + "timestamp": 2.219762176469085 + }, + { + "x": 5.870795249938965, + "y": 1.984981894493103, + "heading": -0.5685244655425081, + "angularVelocity": 2.232507914728541e-27, + "velocityX": 2.548089958327824e-19, + "velocityY": -2.5170158061752717e-19, + "timestamp": 7.7459408324402155 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA12.1.traj b/src/main/deploy/choreo/CBA12.1.traj new file mode 100644 index 0000000..cfeb616 --- /dev/null +++ b/src/main/deploy/choreo/CBA12.1.traj @@ -0,0 +1,94 @@ +{ + "samples": [ + { + "x": 1.8578436374664309, + "y": 4.599852085113525, + "heading": -0.5133989700043752, + "angularVelocity": -4.422567769122659e-34, + "velocityX": 6.892691337281804e-32, + "velocityY": -6.099714772366126e-32, + "timestamp": 0 + }, + { + "x": 1.9087008015316704, + "y": 4.578261769990847, + "heading": -0.5085324968126388, + "angularVelocity": 0.05356306031002483, + "velocityX": 0.5597617080577004, + "velocityY": -0.23763479330209838, + "timestamp": 0.09085502515294563 + }, + { + "x": 2.010415127878634, + "y": 4.53508114233636, + "heading": -0.49879948903440047, + "angularVelocity": 0.10712679636435032, + "velocityX": 1.1195233964850195, + "velocityY": -0.47526955808766275, + "timestamp": 0.18171005030589127 + }, + { + "x": 2.162986612791464, + "y": 4.470310206532365, + "heading": -0.4841998555543846, + "angularVelocity": 0.16069153528312977, + "velocityX": 1.6792850440135332, + "velocityY": -0.712904274639219, + "timestamp": 0.2725650754588369 + }, + { + "x": 2.366415244510088, + "y": 4.383948971397735, + "heading": -0.46473349227048705, + "angularVelocity": 0.2142574200064854, + "velocityX": 2.2390465621041016, + "velocityY": -0.9505388941253716, + "timestamp": 0.36342010061178254 + }, + { + "x": 2.5698438888179167, + "y": 4.29758778527856, + "heading": -0.4452660257493059, + "angularVelocity": 0.21426956283823098, + "velocityX": 2.2390467006677612, + "velocityY": -0.9505383546345164, + "timestamp": 0.45427512576472817 + }, + { + "x": 2.7224153846121193, + "y": 4.232816892345644, + "heading": -0.4306654086114036, + "angularVelocity": 0.16070236195875645, + "velocityX": 1.6792851637798603, + "velocityY": -0.7129038027767155, + "timestamp": 0.5451301509176738 + }, + { + "x": 2.824129718825871, + "y": 4.1896362961575, + "heading": -0.42093166030816187, + "angularVelocity": 0.10713494698675294, + "velocityX": 1.1195234830711887, + "velocityY": -0.47526921175192277, + "timestamp": 0.6359851760706194 + }, + { + "x": 2.8749868869781494, + "y": 4.168045997619629, + "heading": -0.41606478679198744, + "angularVelocity": 0.0535674664993065, + "velocityX": 0.5597617530418804, + "velocityY": -0.237634610760666, + "timestamp": 0.7268402012235651 + }, + { + "x": 2.8749868869781494, + "y": 4.168045997619629, + "heading": -0.41606478679198744, + "angularVelocity": 2.252467346090552e-31, + "velocityX": 0, + "velocityY": 2.011568947036027e-33, + "timestamp": 0.8176952263765107 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA12.2.traj b/src/main/deploy/choreo/CBA12.2.traj new file mode 100644 index 0000000..366a2f5 --- /dev/null +++ b/src/main/deploy/choreo/CBA12.2.traj @@ -0,0 +1,184 @@ +{ + "samples": [ + { + "x": 2.8749868869781494, + "y": 4.168045997619629, + "heading": -0.41606478679198744, + "angularVelocity": 2.252467346090552e-31, + "velocityX": 0, + "velocityY": 2.011568947036027e-33, + "timestamp": 0 + }, + { + "x": 2.847917154575131, + "y": 4.18857218481624, + "heading": -0.413849846792703, + "angularVelocity": 0.031092848081314524, + "velocityX": -0.3799990417258383, + "velocityY": 0.2881421710740943, + "timestamp": 0.0712363175445675 + }, + { + "x": 2.794194937505566, + "y": 4.2301609083697675, + "heading": -0.40920002029536984, + "angularVelocity": 0.06527325748450821, + "velocityX": -0.7541408500789326, + "velocityY": 0.5838134955181548, + "timestamp": 0.142472635089135 + }, + { + "x": 2.7144991090947443, + "y": 4.293639466516727, + "heading": -0.40177096724490385, + "angularVelocity": 0.10428743801670697, + "velocityX": -1.1187527816965028, + "velocityY": 0.8910982534611612, + "timestamp": 0.2137089526337026 + }, + { + "x": 2.610117428439824, + "y": 4.380441782920026, + "heading": -0.390950053544995, + "angularVelocity": 0.1519016433315657, + "velocityX": -1.465287429962691, + "velocityY": 1.2185121212785794, + "timestamp": 0.2849452701782702 + }, + { + "x": 2.484314130174985, + "y": 4.493574980926992, + "heading": -0.37537452674099403, + "angularVelocity": 0.2186458725109274, + "velocityX": -1.7659994592796813, + "velocityY": 1.5881393354756128, + "timestamp": 0.3561815877228378 + }, + { + "x": 2.3531824903728396, + "y": 4.640132668431153, + "heading": -0.3506074155168294, + "angularVelocity": 0.34767534423229424, + "velocityX": -1.8407975639677778, + "velocityY": 2.0573450812136223, + "timestamp": 0.4274179052674054 + }, + { + "x": 2.255641935949165, + "y": 4.791390952468689, + "heading": -0.31960088612658566, + "angularVelocity": 0.43526294534848725, + "velocityX": -1.3692531813234, + "velocityY": 2.1233310374704324, + "timestamp": 0.498654222811973 + }, + { + "x": 2.1909191464919413, + "y": 4.933866453881316, + "heading": -0.2864853718680139, + "angularVelocity": 0.464868418245244, + "velocityX": -0.9085645031656491, + "velocityY": 2.0000402368278873, + "timestamp": 0.5698905403565406 + }, + { + "x": 2.157828961709761, + "y": 5.063935572853814, + "heading": -0.25251089537480154, + "angularVelocity": 0.47692634409332263, + "velocityX": -0.46451284854095054, + "velocityY": 1.8258821266426113, + "timestamp": 0.6411268579011082 + }, + { + "x": 2.155695590887111, + "y": 5.17999447692524, + "heading": -0.21825186452030718, + "angularVelocity": 0.48092085659890615, + "velocityX": -0.029947797643166435, + "velocityY": 1.6292097636684717, + "timestamp": 0.7123631754456758 + }, + { + "x": 2.1840972900390625, + "y": 5.281146049499512, + "heading": -0.18403581352931628, + "angularVelocity": 0.48031751458202726, + "velocityX": 0.3986969025197261, + "velocityY": 1.4199438721823479, + "timestamp": 0.7835994929902435 + }, + { + "x": 2.2699927273833724, + "y": 5.385248234413548, + "heading": -0.14066170396559097, + "angularVelocity": 0.4757513996325097, + "velocityX": 0.9421490135390499, + "velocityY": 1.1418507647951859, + "timestamp": 0.8747691870725334 + }, + { + "x": 2.4043082811295378, + "y": 5.4619094452636885, + "heading": -0.09848696785057999, + "angularVelocity": 0.462596003414656, + "velocityX": 1.4732478275610528, + "velocityY": 0.8408628724904812, + "timestamp": 0.9659388811548231 + }, + { + "x": 2.584301381513317, + "y": 5.506784832670758, + "heading": -0.05919882536632615, + "angularVelocity": 0.43093423620338894, + "velocityX": 1.9742646083827076, + "velocityY": 0.4922182514569118, + "timestamp": 1.0571085752371128 + }, + { + "x": 2.7972441579561136, + "y": 5.506892793506414, + "heading": -0.028482819148575467, + "angularVelocity": 0.3369102696563002, + "velocityX": 2.3356750133495874, + "velocityY": 0.0011841745959921836, + "timestamp": 1.1482782693194025 + }, + { + "x": 2.9615578598610925, + "y": 5.4803731996380725, + "heading": -0.012705291548085192, + "angularVelocity": 0.17305671319082472, + "velocityX": 1.8022842300715867, + "velocityY": -0.2908816809715155, + "timestamp": 1.2394479634016922 + }, + { + "x": 3.0703651150223847, + "y": 5.457240379251342, + "heading": -0.003927703556954177, + "angularVelocity": 0.09627747553054532, + "velocityX": 1.1934585967029847, + "velocityY": -0.2537336624803672, + "timestamp": 1.3306176574839819 + }, + { + "x": 3.1244747638702393, + "y": 5.444272518157959, + "heading": -2.807663106755401e-33, + "angularVelocity": 0.04308124093749151, + "velocityX": 0.593504775819655, + "velocityY": -0.14223872553163855, + "timestamp": 1.4217873515662716 + }, + { + "x": 3.1244747638702393, + "y": 5.444272518157959, + "heading": 3.0521202750813426e-33, + "angularVelocity": 6.160790687932971e-32, + "velocityX": 0, + "velocityY": 2.787060388735749e-34, + "timestamp": 1.5129570456485613 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA12.3.traj b/src/main/deploy/choreo/CBA12.3.traj new file mode 100644 index 0000000..fc99ed7 --- /dev/null +++ b/src/main/deploy/choreo/CBA12.3.traj @@ -0,0 +1,193 @@ +{ + "samples": [ + { + "x": 3.1244747638702393, + "y": 5.444272518157959, + "heading": 3.0521202750813426e-33, + "angularVelocity": 6.160790687932971e-32, + "velocityX": 0, + "velocityY": 2.787060388735749e-34, + "timestamp": 0 + }, + { + "x": 3.098534078468127, + "y": 5.457485358892098, + "heading": 0.0026617246220334858, + "angularVelocity": 0.040358929233321114, + "velocityX": -0.39333080429903416, + "velocityY": 0.20034232683050138, + "timestamp": 0.06595131914048169 + }, + { + "x": 3.0469970111830578, + "y": 5.4845604751866395, + "heading": 0.008254014019529202, + "angularVelocity": 0.08479420078900869, + "velocityX": -0.781441037976515, + "velocityY": 0.4105318384440523, + "timestamp": 0.13190263828096338 + }, + { + "x": 2.9704316321323962, + "y": 5.526489654118093, + "heading": 0.017190641221102865, + "angularVelocity": 0.1355033882270096, + "velocityX": -1.1609377954601798, + "velocityY": 0.6357595189590637, + "timestamp": 0.19785395742144507 + }, + { + "x": 2.8699314986072157, + "y": 5.584960341950904, + "heading": 0.030185152262255092, + "angularVelocity": 0.19703185941549203, + "velocityX": -1.5238532729129166, + "velocityY": 0.8865734392405011, + "timestamp": 0.26380527656192676 + }, + { + "x": 2.748274594463794, + "y": 5.663364989965539, + "heading": 0.04871787407063448, + "angularVelocity": 0.2810060822119608, + "velocityX": -1.8446470173587592, + "velocityY": 1.1888260771196033, + "timestamp": 0.32975659570240845 + }, + { + "x": 2.6175674532641646, + "y": 5.769278046533218, + "heading": 0.07678121127700592, + "angularVelocity": 0.4255159346635774, + "velocityX": -1.981873037613281, + "velocityY": 1.6059277956523261, + "timestamp": 0.39570791484289014 + }, + { + "x": 2.5114580249111134, + "y": 5.890576272725935, + "heading": 0.11264970936545414, + "angularVelocity": 0.5438632396726933, + "velocityX": -1.6089053219239324, + "velocityY": 1.8392084915585034, + "timestamp": 0.46165923398337183 + }, + { + "x": 2.434422801586928, + "y": 6.0132815023129025, + "heading": 0.15152547101477512, + "angularVelocity": 0.5894614718243135, + "velocityX": -1.1680619027511843, + "velocityY": 1.8605424605009222, + "timestamp": 0.5276105531238535 + }, + { + "x": 2.3863480483215023, + "y": 6.1329283805448345, + "heading": 0.1917455671848182, + "angularVelocity": 0.6098452115014477, + "velocityX": -0.7289430126944083, + "velocityY": 1.8141695995045146, + "timestamp": 0.5935618722643352 + }, + { + "x": 2.366951842118672, + "y": 6.247499705389461, + "heading": 0.23254149094602206, + "angularVelocity": 0.6185763119353173, + "velocityX": -0.2940988361662238, + "velocityY": 1.7372105112944605, + "timestamp": 0.6595131914048169 + }, + { + "x": 2.3760111331939697, + "y": 6.355863094329834, + "heading": 0.2734764838636355, + "angularVelocity": 0.6206849756925088, + "velocityX": 0.1373633036207332, + "velocityY": 1.6430814478404059, + "timestamp": 0.7254645105452986 + }, + { + "x": 2.43019040241965, + "y": 6.478943079024278, + "heading": 0.32373567599245034, + "angularVelocity": 0.6180408931068141, + "velocityX": 0.6662463625418199, + "velocityY": 1.5135234062090714, + "timestamp": 0.8067846831897865 + }, + { + "x": 2.526899563141218, + "y": 6.589698861716159, + "heading": 0.37307788513968465, + "angularVelocity": 0.6067646875632248, + "velocityX": 1.1892394909727473, + "velocityY": 1.3619718095782953, + "timestamp": 0.8881048558342743 + }, + { + "x": 2.664996244898422, + "y": 6.684729235285451, + "heading": 0.4201486669681804, + "angularVelocity": 0.5788327827864446, + "velocityX": 1.6981848078569475, + "velocityY": 1.1685953248610919, + "timestamp": 0.9694250284787622 + }, + { + "x": 2.8401007366822744, + "y": 6.75551919172687, + "heading": 0.4614250409919158, + "angularVelocity": 0.5075785341009543, + "velocityX": 2.1532725040970155, + "velocityY": 0.8705091755136839, + "timestamp": 1.05074520112325 + }, + { + "x": 3.013182726978644, + "y": 6.782401959259476, + "heading": 0.4862766709248573, + "angularVelocity": 0.3056022770830774, + "velocityX": 2.128401658134633, + "velocityY": 0.33057932193726985, + "timestamp": 1.132065373767738 + }, + { + "x": 3.1453404362811956, + "y": 6.792674782529316, + "heading": 0.5013972962017155, + "angularVelocity": 0.1859394143562305, + "velocityX": 1.625152837295322, + "velocityY": 0.1263256451111805, + "timestamp": 1.2133855464122258 + }, + { + "x": 3.2337546883673167, + "y": 6.79637553946723, + "heading": 0.5103347181148908, + "angularVelocity": 0.10990411877569715, + "velocityX": 1.0872364040917817, + "velocityY": 0.045508473698305255, + "timestamp": 1.2947057190567137 + }, + { + "x": 3.278005838394165, + "y": 6.79726505279541, + "heading": 0.5144513011934931, + "angularVelocity": 0.05062191759726958, + "velocityX": 0.5441595681345003, + "velocityY": 0.010938409244131192, + "timestamp": 1.3760258917012016 + }, + { + "x": 3.278005838394165, + "y": 6.79726505279541, + "heading": 0.5144513011934931, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 3.975812450296696e-33, + "timestamp": 1.4573460643456895 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA12.4.traj b/src/main/deploy/choreo/CBA12.4.traj new file mode 100644 index 0000000..97b0a81 --- /dev/null +++ b/src/main/deploy/choreo/CBA12.4.traj @@ -0,0 +1,211 @@ +{ + "samples": [ + { + "x": 3.278005838394165, + "y": 6.79726505279541, + "heading": 0.5144513011934931, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 3.975812450296696e-33, + "timestamp": 0 + }, + { + "x": 3.3272756579842873, + "y": 6.80124671096813, + "heading": 0.502259354445405, + "angularVelocity": -0.14167110798768545, + "velocityX": 0.5725180790102878, + "velocityY": 0.0462670922541433, + "timestamp": 0.08605810261099611 + }, + { + "x": 3.4258151809145874, + "y": 6.8092100324777185, + "heading": 0.4778687342193166, + "angularVelocity": -0.2834203809553646, + "velocityX": 1.145034807189669, + "velocityY": 0.09253424451600453, + "timestamp": 0.17211620522199222 + }, + { + "x": 3.573624189931311, + "y": 6.821154977138411, + "heading": 0.44126757500898656, + "angularVelocity": -0.4253075317731932, + "velocityX": 1.7175490108681681, + "velocityY": 0.13880092981701167, + "timestamp": 0.2581743078329879 + }, + { + "x": 3.7707023793841614, + "y": 6.837081406357962, + "heading": 0.39243932813761767, + "angularVelocity": -0.567386979146327, + "velocityX": 2.2900596628730017, + "velocityY": 0.18506600466831086, + "timestamp": 0.34423241044398356 + }, + { + "x": 4.017049385854338, + "y": 6.856989019626697, + "heading": 0.3313641468295269, + "angularVelocity": -0.7096970471683371, + "velocityX": 2.8625660919308524, + "velocityY": 0.23132758758031108, + "timestamp": 0.43029051305497923 + }, + { + "x": 4.312664842319635, + "y": 6.880877294918687, + "heading": 0.25802182339028534, + "angularVelocity": -0.8522419297304485, + "velocityX": 3.4350682561699637, + "velocityY": 0.27758310452160395, + "timestamp": 0.5163486156659749 + }, + { + "x": 4.637160999178445, + "y": 6.907084038065497, + "heading": 0.25802181146651204, + "angularVelocity": -1.385550100052896e-7, + "velocityX": 3.7706636215948364, + "velocityY": 0.304523831594114, + "timestamp": 0.6024067182769706 + }, + { + "x": 4.9616571567181005, + "y": 6.933290772782941, + "heading": 0.25802179954292714, + "angularVelocity": -1.3855287596436928e-7, + "velocityX": 3.770663629506321, + "velocityY": 0.3045237336441463, + "timestamp": 0.6884648208879662 + }, + { + "x": 5.286153314257592, + "y": 6.959497507502428, + "heading": 0.2580217876193422, + "angularVelocity": -1.3855287690850846e-7, + "velocityX": 3.7706636295043996, + "velocityY": 0.30452373366790714, + "timestamp": 0.7745229234989619 + }, + { + "x": 5.610649471797095, + "y": 6.985704242221776, + "heading": 0.2580217756957572, + "angularVelocity": -1.385528773415127e-7, + "velocityX": 3.7706636295045306, + "velocityY": 0.3045237336662797, + "timestamp": 0.8605810261099576 + }, + { + "x": 5.935145629336598, + "y": 7.011910976941122, + "heading": 0.25802176377217223, + "angularVelocity": -1.3855287632634293e-7, + "velocityX": 3.7706636295045324, + "velocityY": 0.30452373366626134, + "timestamp": 0.9466391287209532 + }, + { + "x": 6.259641786876101, + "y": 7.038117711660469, + "heading": 0.25802175184858733, + "angularVelocity": -1.3855287660850554e-7, + "velocityX": 3.7706636295045315, + "velocityY": 0.30452373366627317, + "timestamp": 1.032697231331949 + }, + { + "x": 6.584137944415597, + "y": 7.064324446379899, + "heading": 0.2580217399250024, + "angularVelocity": -1.3855287639179296e-7, + "velocityX": 3.7706636295044533, + "velocityY": 0.3045237336672401, + "timestamp": 1.1187553339429446 + }, + { + "x": 6.908634101955186, + "y": 7.090531181098188, + "heading": 0.2580217280014175, + "angularVelocity": -1.3855287562699177e-7, + "velocityX": 3.7706636295055262, + "velocityY": 0.3045237336539718, + "timestamp": 1.2048134365539402 + }, + { + "x": 7.233130259094438, + "y": 7.116737920772481, + "heading": 0.25802171607764257, + "angularVelocity": -1.3855502925167896e-7, + "velocityX": 3.7706636248536065, + "velocityY": 0.30452379124316004, + "timestamp": 1.290871539164936 + }, + { + "x": 7.52873971550584, + "y": 7.140609322656376, + "heading": 0.18429681518138608, + "angularVelocity": -0.8566875013371068, + "velocityX": 3.4349985352055685, + "velocityY": 0.2773870345689411, + "timestamp": 1.3769296417759316 + }, + { + "x": 7.775081036606935, + "y": 7.160501331656927, + "heading": 0.12286043910547154, + "angularVelocity": -0.7138941507190695, + "velocityX": 2.8625000276218113, + "velocityY": 0.23114626510492656, + "timestamp": 1.4629877443869272 + }, + { + "x": 7.972154196137362, + "y": 7.176414449461249, + "heading": 0.07371418459504951, + "angularVelocity": -0.5710822458267809, + "velocityX": 2.2900012148910633, + "velocityY": 0.1849113252734793, + "timestamp": 1.549045846997923 + }, + { + "x": 8.119959129631837, + "y": 7.188349047759648, + "heading": 0.036856619638336335, + "angularVelocity": -0.4282869809901411, + "velocityX": 1.7175016530700593, + "velocityY": 0.13868070450431355, + "timestamp": 1.6351039496089186 + }, + { + "x": 8.218495773200091, + "y": 7.1963053677311315, + "heading": 0.012285644906453631, + "angularVelocity": -0.28551611047234304, + "velocityX": 1.1450013488404933, + "velocityY": 0.09245288624879405, + "timestamp": 1.7211620522199143 + }, + { + "x": 8.2677640914917, + "y": 7.200283527374268, + "heading": 0, + "angularVelocity": -0.14275988586599633, + "velocityX": 0.5725006338369774, + "velocityY": 0.046226439143295044, + "timestamp": 1.80722015483091 + }, + { + "x": 8.2677640914917, + "y": 7.200283527374268, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -3.7403039540233026e-33, + "timestamp": 1.8932782574419056 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA12.5.traj b/src/main/deploy/choreo/CBA12.5.traj new file mode 100644 index 0000000..23daf9c --- /dev/null +++ b/src/main/deploy/choreo/CBA12.5.traj @@ -0,0 +1,139 @@ +{ + "samples": [ + { + "x": 8.2677640914917, + "y": 7.200283527374268, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -3.7403039540233026e-33, + "timestamp": 0 + }, + { + "x": 8.209402430057438, + "y": 7.190289670117731, + "heading": 0.0036095788362639895, + "angularVelocity": 0.03838131856351764, + "velocityX": -0.6205703272376543, + "velocityY": -0.10626653038374882, + "timestamp": 0.09404520144243467 + }, + { + "x": 8.092676418351024, + "y": 7.1703016582639245, + "heading": 0.010387056823911094, + "angularVelocity": 0.07206617544671082, + "velocityX": -1.2411692453853074, + "velocityY": -0.21253622244646914, + "timestamp": 0.18809040288486933 + }, + { + "x": 7.917582655378263, + "y": 7.140319157705864, + "heading": 0.019665971369599126, + "angularVelocity": 0.09866441245990722, + "velocityX": -1.8618043269324378, + "velocityY": -0.31880946713051367, + "timestamp": 0.282135604327304 + }, + { + "x": 7.684117200932665, + "y": 7.10034191479348, + "heading": 0.030324783936255515, + "angularVelocity": 0.11333712302739128, + "velocityX": -2.4824813054230472, + "velocityY": -0.42508540892271196, + "timestamp": 0.37618080576973867 + }, + { + "x": 7.39227895399914, + "y": 7.0503706021181705, + "heading": 0.04007805146811309, + "angularVelocity": 0.10370829539357472, + "velocityX": -3.1031699912104123, + "velocityY": -0.5313541989264866, + "timestamp": 0.47022600721217334 + }, + { + "x": 7.042125768342003, + "y": 6.990417993094679, + "heading": 0.041681192088546266, + "angularVelocity": 0.01704649036837675, + "velocityX": -3.7232435072345225, + "velocityY": -0.6374871668522564, + "timestamp": 0.564271208654608 + }, + { + "x": 6.6914633234862775, + "y": 6.930365555430834, + "heading": 0.041681197110018026, + "angularVelocity": 5.33942963783108e-8, + "velocityX": -3.728658554369243, + "velocityY": -0.6385486632238303, + "timestamp": 0.6583164100970427 + }, + { + "x": 6.341309093988729, + "y": 6.870412719224656, + "heading": 0.04328301737471915, + "angularVelocity": 0.017032450782756198, + "velocityX": -3.723254606582202, + "velocityY": -0.6374895825277324, + "timestamp": 0.7523616115394773 + }, + { + "x": 6.0494705333637215, + "y": 6.820441331819641, + "heading": 0.05298413270489232, + "angularVelocity": 0.10315375141099561, + "velocityX": -3.1031733267498307, + "velocityY": -0.5313549935412731, + "timestamp": 0.846406812981912 + }, + { + "x": 5.81600477469543, + "y": 6.780464021198124, + "heading": 0.06358433999708792, + "angularVelocity": 0.11271396233125523, + "velocityX": -2.482484540279067, + "velocityY": -0.4250861288864222, + "timestamp": 0.9404520144243467 + }, + { + "x": 5.640910724528676, + "y": 6.750481459365072, + "heading": 0.07281183701492056, + "angularVelocity": 0.09811768039955485, + "velocityX": -1.8618073807191666, + "velocityY": -0.31881011867883496, + "timestamp": 1.0344972158667813 + }, + { + "x": 5.524184486258963, + "y": 6.730493399783685, + "heading": 0.07955165767088834, + "angularVelocity": 0.07166575809109048, + "velocityX": -1.2411716544746547, + "velocityY": -0.21253672994263784, + "timestamp": 1.128542417309216 + }, + { + "x": 5.465822696685791, + "y": 6.720499515533447, + "heading": 0.08314116334656949, + "angularVelocity": 0.03816787694349407, + "velocityX": -0.6205716897623333, + "velocityY": -0.10626681741275645, + "timestamp": 1.2225876187516507 + }, + { + "x": 5.465822696685791, + "y": 6.720499515533447, + "heading": 0.08314116334656949, + "angularVelocity": 8.412036964343254e-33, + "velocityX": 0, + "velocityY": -2.9297022179051264e-34, + "timestamp": 1.3166328201940853 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA12.6.traj b/src/main/deploy/choreo/CBA12.6.traj new file mode 100644 index 0000000..d946932 --- /dev/null +++ b/src/main/deploy/choreo/CBA12.6.traj @@ -0,0 +1,148 @@ +{ + "samples": [ + { + "x": 5.465822696685791, + "y": 6.720499515533447, + "heading": 0.08314116334656949, + "angularVelocity": 8.412036964343254e-33, + "velocityX": 0, + "velocityY": -2.9297022179051264e-34, + "timestamp": 0 + }, + { + "x": 5.517857585692567, + "y": 6.7009202818586795, + "heading": 0.08116164183957295, + "angularVelocity": -0.0217235022928802, + "velocityX": 0.5710370039563939, + "velocityY": -0.21486481764081972, + "timestamp": 0.09112349750761162 + }, + { + "x": 5.621927362497105, + "y": 6.661761814969241, + "heading": 0.0772026019010839, + "angularVelocity": -0.04344697083467331, + "velocityX": 1.1420739946449114, + "velocityY": -0.4297296302324916, + "timestamp": 0.18224699501522323 + }, + { + "x": 5.778032025282058, + "y": 6.603024115556083, + "heading": 0.07126404712081705, + "angularVelocity": -0.06517039998230038, + "velocityX": 1.7131109653896246, + "velocityY": -0.6445944352415494, + "timestamp": 0.27337049252283485 + }, + { + "x": 5.986171571020342, + "y": 6.524707184767147, + "heading": 0.06334598490916293, + "angularVelocity": -0.08689374780707436, + "velocityX": 2.284147902914729, + "velocityY": -0.8594592276529842, + "timestamp": 0.36449399003044647 + }, + { + "x": 6.2463459936710395, + "y": 6.426811024886955, + "heading": 0.05344843633991816, + "angularVelocity": -0.1086168643648252, + "velocityX": 2.855184774146016, + "velocityY": -1.074323994993733, + "timestamp": 0.4556174875380581 + }, + { + "x": 6.558555275153954, + "y": 6.30933564274033, + "heading": 0.04157148359406043, + "angularVelocity": -0.13033907905684416, + "velocityX": 3.42622144696285, + "velocityY": -1.2891886874380052, + "timestamp": 0.5467409850456697 + }, + { + "x": 6.881186726040969, + "y": 6.187938689228568, + "heading": 0.041571481800887045, + "angularVelocity": -1.9678505938061922e-8, + "velocityX": 3.5405955621931517, + "velocityY": -1.3322244737325157, + "timestamp": 0.6378644825532813 + }, + { + "x": 7.203818176927985, + "y": 6.066541735716808, + "heading": 0.041571480007713664, + "angularVelocity": -1.967850585555319e-8, + "velocityX": 3.5405955621931553, + "velocityY": -1.3322244737325053, + "timestamp": 0.7289879800608929 + }, + { + "x": 7.516027457256037, + "y": 5.949066354035002, + "heading": 0.029694083932865854, + "angularVelocity": -0.13034394420143097, + "velocityX": 3.426221434289254, + "velocityY": -1.289188682337032, + "timestamp": 0.8201114775685046 + }, + { + "x": 7.77620187882914, + "y": 5.85117019459155, + "heading": 0.01979612092341288, + "angularVelocity": -0.1086214124805027, + "velocityX": 2.85518476232035, + "velocityY": -1.074323990200902, + "timestamp": 0.9112349750761162 + }, + { + "x": 7.984341423634275, + "y": 5.772853264185132, + "heading": 0.011877698718430987, + "angularVelocity": -0.08689769841626724, + "velocityX": 2.284147892674251, + "velocityY": -0.8594592234551758, + "timestamp": 1.0023584725837278 + }, + { + "x": 8.140446085681976, + "y": 5.714115565078498, + "heading": 0.005938858336697638, + "angularVelocity": -0.06517353420644122, + "velocityX": 1.7131109572989367, + "velocityY": -0.644594431877729, + "timestamp": 1.0934819700913394 + }, + { + "x": 8.244515861980604, + "y": 5.674957098402801, + "heading": 0.0019796214029937777, + "angularVelocity": -0.04344913268336865, + "velocityX": 1.142073989092992, + "velocityY": -0.4297296278868681, + "timestamp": 1.184605467598951 + }, + { + "x": 8.296550750732422, + "y": 5.6553778648376465, + "heading": -1.364378444746575e-32, + "angularVelocity": -0.02172459856332236, + "velocityX": 0.5710370011584592, + "velocityY": -0.2148648164379124, + "timestamp": 1.2757289651065626 + }, + { + "x": 8.296550750732422, + "y": 5.6553778648376465, + "heading": -2.7953004517751144e-32, + "angularVelocity": -1.5703141365783988e-31, + "velocityX": 0, + "velocityY": 3.434094774968783e-33, + "timestamp": 1.3668524626141743 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA12.7.traj b/src/main/deploy/choreo/CBA12.7.traj new file mode 100644 index 0000000..4cf8273 --- /dev/null +++ b/src/main/deploy/choreo/CBA12.7.traj @@ -0,0 +1,139 @@ +{ + "samples": [ + { + "x": 8.296550750732422, + "y": 5.6553778648376465, + "heading": -2.7953004517751144e-32, + "angularVelocity": -1.5703141365783988e-31, + "velocityX": 0, + "velocityY": 3.434094774968783e-33, + "timestamp": 0 + }, + { + "x": 8.237994852774355, + "y": 5.664172303219014, + "heading": 0.007226618869060547, + "angularVelocity": 0.07681811545724591, + "velocityX": -0.6224423636571395, + "velocityY": 0.09348385395871893, + "timestamp": 0.09407440973976122 + }, + { + "x": 8.120872431236778, + "y": 5.681762484801859, + "heading": 0.02080471998001932, + "angularVelocity": 0.1443336306696198, + "velocityX": -1.2449976764304742, + "velocityY": 0.1869815780029983, + "timestamp": 0.18814881947952244 + }, + { + "x": 7.945169976448883, + "y": 5.708149977355932, + "heading": 0.03940846636956739, + "angularVelocity": 0.19775565364675843, + "velocityX": -1.867696595428377, + "velocityY": 0.28049596725681497, + "timestamp": 0.28222322921928367 + }, + { + "x": 7.710871729254762, + "y": 5.743336322554468, + "heading": 0.06079052814790254, + "angularVelocity": 0.22728882208972376, + "velocityX": -2.4905630324148915, + "velocityY": 0.3740267443172734, + "timestamp": 0.3762976389590449 + }, + { + "x": 7.417973686541333, + "y": 5.787320356921691, + "heading": 0.0802978988189348, + "angularVelocity": 0.2073610743406771, + "velocityX": -3.113472022026445, + "velocityY": 0.4675451537660226, + "timestamp": 0.4703720486988061 + }, + { + "x": 7.066742108885601, + "y": 5.840051326513902, + "heading": 0.08234056213406721, + "angularVelocity": 0.021713272726732794, + "velocityX": -3.7335506927685165, + "velocityY": 0.5605240547147593, + "timestamp": 0.5644464584385673 + }, + { + "x": 6.714812786263884, + "y": 5.892917473581175, + "heading": 0.0823405720722638, + "angularVelocity": 1.0564201978235854e-7, + "velocityX": -3.740967640352154, + "velocityY": 0.5619609755029853, + "timestamp": 0.6585208681783286 + }, + { + "x": 6.363585735336453, + "y": 5.945647087732873, + "heading": 0.0843940929667477, + "angularVelocity": 0.02182868752812118, + "velocityX": -3.7335025741749357, + "velocityY": 0.5605096465402537, + "timestamp": 0.7525952779180898 + }, + { + "x": 6.070689025568267, + "y": 5.989630737024261, + "heading": 0.10401416670033654, + "angularVelocity": 0.20855909474314377, + "velocityX": -3.1134578529740558, + "velocityY": 0.46754106045483973, + "timestamp": 0.846669687657851 + }, + { + "x": 5.836392076431593, + "y": 6.024816723804097, + "heading": 0.1255202540864113, + "angularVelocity": 0.2286071998262574, + "velocityX": -2.4905492342157323, + "velocityY": 0.3740229343683141, + "timestamp": 0.9407440973976122 + }, + { + "x": 5.6606908312498, + "y": 6.051203885635972, + "heading": 0.14422987535302242, + "angularVelocity": 0.1988810912343688, + "velocityX": -1.8676837374566488, + "velocityY": 0.2804924517184302, + "timestamp": 1.0348185071373734 + }, + { + "x": 5.543569343329225, + "y": 6.068793800338837, + "heading": 0.15788326554426388, + "angularVelocity": 0.14513394480677122, + "velocityX": -1.2449877521907207, + "velocityY": 0.186978741099956, + "timestamp": 1.1288929168771347 + }, + { + "x": 5.485013961791992, + "y": 6.077588081359863, + "heading": 0.16514897587001573, + "angularVelocity": 0.07723365307987065, + "velocityX": -0.6224368741639325, + "velocityY": 0.09348218123665668, + "timestamp": 1.2229673266168959 + }, + { + "x": 5.485013961791992, + "y": 6.077588081359863, + "heading": 0.16514897587001573, + "angularVelocity": -1.1689527354691748e-31, + "velocityX": 3.442040180158371e-34, + "velocityY": 0, + "timestamp": 1.3170417363566571 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA12.traj b/src/main/deploy/choreo/CBA12.traj new file mode 100644 index 0000000..e4d9aef --- /dev/null +++ b/src/main/deploy/choreo/CBA12.traj @@ -0,0 +1,1030 @@ +{ + "samples": [ + { + "x": 1.8578436374664309, + "y": 4.599852085113525, + "heading": -0.5133989700043752, + "angularVelocity": -4.422567769122659e-34, + "velocityX": 6.892691337281804e-32, + "velocityY": -6.099714772366126e-32, + "timestamp": 0 + }, + { + "x": 1.9087008015316704, + "y": 4.578261769990847, + "heading": -0.5085324968126388, + "angularVelocity": 0.05356306031002483, + "velocityX": 0.5597617080577004, + "velocityY": -0.23763479330209838, + "timestamp": 0.09085502515294563 + }, + { + "x": 2.010415127878634, + "y": 4.53508114233636, + "heading": -0.49879948903440047, + "angularVelocity": 0.10712679636435032, + "velocityX": 1.1195233964850195, + "velocityY": -0.47526955808766275, + "timestamp": 0.18171005030589127 + }, + { + "x": 2.162986612791464, + "y": 4.470310206532365, + "heading": -0.4841998555543846, + "angularVelocity": 0.16069153528312977, + "velocityX": 1.6792850440135332, + "velocityY": -0.712904274639219, + "timestamp": 0.2725650754588369 + }, + { + "x": 2.366415244510088, + "y": 4.383948971397735, + "heading": -0.46473349227048705, + "angularVelocity": 0.2142574200064854, + "velocityX": 2.2390465621041016, + "velocityY": -0.9505388941253716, + "timestamp": 0.36342010061178254 + }, + { + "x": 2.5698438888179167, + "y": 4.29758778527856, + "heading": -0.4452660257493059, + "angularVelocity": 0.21426956283823098, + "velocityX": 2.2390467006677612, + "velocityY": -0.9505383546345164, + "timestamp": 0.45427512576472817 + }, + { + "x": 2.7224153846121193, + "y": 4.232816892345644, + "heading": -0.4306654086114036, + "angularVelocity": 0.16070236195875645, + "velocityX": 1.6792851637798603, + "velocityY": -0.7129038027767155, + "timestamp": 0.5451301509176738 + }, + { + "x": 2.824129718825871, + "y": 4.1896362961575, + "heading": -0.42093166030816187, + "angularVelocity": 0.10713494698675294, + "velocityX": 1.1195234830711887, + "velocityY": -0.47526921175192277, + "timestamp": 0.6359851760706194 + }, + { + "x": 2.8749868869781494, + "y": 4.168045997619629, + "heading": -0.41606478679198744, + "angularVelocity": 0.0535674664993065, + "velocityX": 0.5597617530418804, + "velocityY": -0.237634610760666, + "timestamp": 0.7268402012235651 + }, + { + "x": 2.8749868869781494, + "y": 4.168045997619629, + "heading": -0.41606478679198744, + "angularVelocity": 2.252467346090552e-31, + "velocityX": 0, + "velocityY": 2.011568947036027e-33, + "timestamp": 0.8176952263765107 + }, + { + "x": 2.847917154575131, + "y": 4.18857218481624, + "heading": -0.413849846792703, + "angularVelocity": 0.031092848081314524, + "velocityX": -0.3799990417258383, + "velocityY": 0.2881421710740943, + "timestamp": 0.8889315439210782 + }, + { + "x": 2.794194937505566, + "y": 4.2301609083697675, + "heading": -0.40920002029536984, + "angularVelocity": 0.06527325748450821, + "velocityX": -0.7541408500789326, + "velocityY": 0.5838134955181548, + "timestamp": 0.9601678614656457 + }, + { + "x": 2.7144991090947443, + "y": 4.293639466516727, + "heading": -0.40177096724490385, + "angularVelocity": 0.10428743801670697, + "velocityX": -1.1187527816965028, + "velocityY": 0.8910982534611612, + "timestamp": 1.0314041790102133 + }, + { + "x": 2.610117428439824, + "y": 4.380441782920026, + "heading": -0.390950053544995, + "angularVelocity": 0.1519016433315657, + "velocityX": -1.465287429962691, + "velocityY": 1.2185121212785794, + "timestamp": 1.102640496554781 + }, + { + "x": 2.484314130174985, + "y": 4.493574980926992, + "heading": -0.37537452674099403, + "angularVelocity": 0.2186458725109274, + "velocityX": -1.7659994592796813, + "velocityY": 1.5881393354756128, + "timestamp": 1.1738768140993485 + }, + { + "x": 2.3531824903728396, + "y": 4.640132668431153, + "heading": -0.3506074155168294, + "angularVelocity": 0.34767534423229424, + "velocityX": -1.8407975639677778, + "velocityY": 2.0573450812136223, + "timestamp": 1.2451131316439161 + }, + { + "x": 2.255641935949165, + "y": 4.791390952468689, + "heading": -0.31960088612658566, + "angularVelocity": 0.43526294534848725, + "velocityX": -1.3692531813234, + "velocityY": 2.1233310374704324, + "timestamp": 1.3163494491884837 + }, + { + "x": 2.1909191464919413, + "y": 4.933866453881316, + "heading": -0.2864853718680139, + "angularVelocity": 0.464868418245244, + "velocityX": -0.9085645031656491, + "velocityY": 2.0000402368278873, + "timestamp": 1.3875857667330513 + }, + { + "x": 2.157828961709761, + "y": 5.063935572853814, + "heading": -0.25251089537480154, + "angularVelocity": 0.47692634409332263, + "velocityX": -0.46451284854095054, + "velocityY": 1.8258821266426113, + "timestamp": 1.458822084277619 + }, + { + "x": 2.155695590887111, + "y": 5.17999447692524, + "heading": -0.21825186452030718, + "angularVelocity": 0.48092085659890615, + "velocityX": -0.029947797643166435, + "velocityY": 1.6292097636684717, + "timestamp": 1.5300584018221866 + }, + { + "x": 2.1840972900390625, + "y": 5.281146049499512, + "heading": -0.18403581352931628, + "angularVelocity": 0.48031751458202726, + "velocityX": 0.3986969025197261, + "velocityY": 1.4199438721823479, + "timestamp": 1.6012947193667542 + }, + { + "x": 2.2699927273833724, + "y": 5.385248234413548, + "heading": -0.14066170396559097, + "angularVelocity": 0.4757513996325097, + "velocityX": 0.9421490135390499, + "velocityY": 1.1418507647951859, + "timestamp": 1.692464413449044 + }, + { + "x": 2.4043082811295378, + "y": 5.4619094452636885, + "heading": -0.09848696785057999, + "angularVelocity": 0.462596003414656, + "velocityX": 1.4732478275610528, + "velocityY": 0.8408628724904812, + "timestamp": 1.7836341075313338 + }, + { + "x": 2.584301381513317, + "y": 5.506784832670758, + "heading": -0.05919882536632615, + "angularVelocity": 0.43093423620338894, + "velocityX": 1.9742646083827076, + "velocityY": 0.4922182514569118, + "timestamp": 1.8748038016136235 + }, + { + "x": 2.7972441579561136, + "y": 5.506892793506414, + "heading": -0.028482819148575467, + "angularVelocity": 0.3369102696563002, + "velocityX": 2.3356750133495874, + "velocityY": 0.0011841745959921836, + "timestamp": 1.9659734956959132 + }, + { + "x": 2.9615578598610925, + "y": 5.4803731996380725, + "heading": -0.012705291548085192, + "angularVelocity": 0.17305671319082472, + "velocityX": 1.8022842300715867, + "velocityY": -0.2908816809715155, + "timestamp": 2.057143189778203 + }, + { + "x": 3.0703651150223847, + "y": 5.457240379251342, + "heading": -0.003927703556954177, + "angularVelocity": 0.09627747553054532, + "velocityX": 1.1934585967029847, + "velocityY": -0.2537336624803672, + "timestamp": 2.1483128838604926 + }, + { + "x": 3.1244747638702393, + "y": 5.444272518157959, + "heading": -2.807663106755401e-33, + "angularVelocity": 0.04308124093749151, + "velocityX": 0.593504775819655, + "velocityY": -0.14223872553163855, + "timestamp": 2.2394825779427823 + }, + { + "x": 3.1244747638702393, + "y": 5.444272518157959, + "heading": 3.0521202750813426e-33, + "angularVelocity": 6.160790687932971e-32, + "velocityX": 0, + "velocityY": 2.787060388735749e-34, + "timestamp": 2.330652272025072 + }, + { + "x": 3.098534078468127, + "y": 5.457485358892098, + "heading": 0.0026617246220334858, + "angularVelocity": 0.040358929233321114, + "velocityX": -0.39333080429903416, + "velocityY": 0.20034232683050138, + "timestamp": 2.3966035911655537 + }, + { + "x": 3.0469970111830578, + "y": 5.4845604751866395, + "heading": 0.008254014019529202, + "angularVelocity": 0.08479420078900869, + "velocityX": -0.781441037976515, + "velocityY": 0.4105318384440523, + "timestamp": 2.4625549103060353 + }, + { + "x": 2.9704316321323962, + "y": 5.526489654118093, + "heading": 0.017190641221102865, + "angularVelocity": 0.1355033882270096, + "velocityX": -1.1609377954601798, + "velocityY": 0.6357595189590637, + "timestamp": 2.528506229446517 + }, + { + "x": 2.8699314986072157, + "y": 5.584960341950904, + "heading": 0.030185152262255092, + "angularVelocity": 0.19703185941549203, + "velocityX": -1.5238532729129166, + "velocityY": 0.8865734392405011, + "timestamp": 2.5944575485869987 + }, + { + "x": 2.748274594463794, + "y": 5.663364989965539, + "heading": 0.04871787407063448, + "angularVelocity": 0.2810060822119608, + "velocityX": -1.8446470173587592, + "velocityY": 1.1888260771196033, + "timestamp": 2.6604088677274804 + }, + { + "x": 2.6175674532641646, + "y": 5.769278046533218, + "heading": 0.07678121127700592, + "angularVelocity": 0.4255159346635774, + "velocityX": -1.981873037613281, + "velocityY": 1.6059277956523261, + "timestamp": 2.726360186867962 + }, + { + "x": 2.5114580249111134, + "y": 5.890576272725935, + "heading": 0.11264970936545414, + "angularVelocity": 0.5438632396726933, + "velocityX": -1.6089053219239324, + "velocityY": 1.8392084915585034, + "timestamp": 2.792311506008444 + }, + { + "x": 2.434422801586928, + "y": 6.0132815023129025, + "heading": 0.15152547101477512, + "angularVelocity": 0.5894614718243135, + "velocityX": -1.1680619027511843, + "velocityY": 1.8605424605009222, + "timestamp": 2.8582628251489255 + }, + { + "x": 2.3863480483215023, + "y": 6.1329283805448345, + "heading": 0.1917455671848182, + "angularVelocity": 0.6098452115014477, + "velocityX": -0.7289430126944083, + "velocityY": 1.8141695995045146, + "timestamp": 2.924214144289407 + }, + { + "x": 2.366951842118672, + "y": 6.247499705389461, + "heading": 0.23254149094602206, + "angularVelocity": 0.6185763119353173, + "velocityX": -0.2940988361662238, + "velocityY": 1.7372105112944605, + "timestamp": 2.990165463429889 + }, + { + "x": 2.3760111331939697, + "y": 6.355863094329834, + "heading": 0.2734764838636355, + "angularVelocity": 0.6206849756925088, + "velocityX": 0.1373633036207332, + "velocityY": 1.6430814478404059, + "timestamp": 3.0561167825703706 + }, + { + "x": 2.43019040241965, + "y": 6.478943079024278, + "heading": 0.32373567599245034, + "angularVelocity": 0.6180408931068141, + "velocityX": 0.6662463625418199, + "velocityY": 1.5135234062090714, + "timestamp": 3.1374369552148584 + }, + { + "x": 2.526899563141218, + "y": 6.589698861716159, + "heading": 0.37307788513968465, + "angularVelocity": 0.6067646875632248, + "velocityX": 1.1892394909727473, + "velocityY": 1.3619718095782953, + "timestamp": 3.2187571278593463 + }, + { + "x": 2.664996244898422, + "y": 6.684729235285451, + "heading": 0.4201486669681804, + "angularVelocity": 0.5788327827864446, + "velocityX": 1.6981848078569475, + "velocityY": 1.1685953248610919, + "timestamp": 3.300077300503834 + }, + { + "x": 2.8401007366822744, + "y": 6.75551919172687, + "heading": 0.4614250409919158, + "angularVelocity": 0.5075785341009543, + "velocityX": 2.1532725040970155, + "velocityY": 0.8705091755136839, + "timestamp": 3.381397473148322 + }, + { + "x": 3.013182726978644, + "y": 6.782401959259476, + "heading": 0.4862766709248573, + "angularVelocity": 0.3056022770830774, + "velocityX": 2.128401658134633, + "velocityY": 0.33057932193726985, + "timestamp": 3.46271764579281 + }, + { + "x": 3.1453404362811956, + "y": 6.792674782529316, + "heading": 0.5013972962017155, + "angularVelocity": 0.1859394143562305, + "velocityX": 1.625152837295322, + "velocityY": 0.1263256451111805, + "timestamp": 3.544037818437298 + }, + { + "x": 3.2337546883673167, + "y": 6.79637553946723, + "heading": 0.5103347181148908, + "angularVelocity": 0.10990411877569715, + "velocityX": 1.0872364040917817, + "velocityY": 0.045508473698305255, + "timestamp": 3.6253579910817857 + }, + { + "x": 3.278005838394165, + "y": 6.79726505279541, + "heading": 0.5144513011934931, + "angularVelocity": 0.05062191759726958, + "velocityX": 0.5441595681345003, + "velocityY": 0.010938409244131192, + "timestamp": 3.7066781637262736 + }, + { + "x": 3.278005838394165, + "y": 6.79726505279541, + "heading": 0.5144513011934931, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 3.975812450296696e-33, + "timestamp": 3.7879983363707614 + }, + { + "x": 3.3272756579842873, + "y": 6.80124671096813, + "heading": 0.502259354445405, + "angularVelocity": -0.14167110798768545, + "velocityX": 0.5725180790102878, + "velocityY": 0.0462670922541433, + "timestamp": 3.8740564389817576 + }, + { + "x": 3.4258151809145874, + "y": 6.8092100324777185, + "heading": 0.4778687342193166, + "angularVelocity": -0.2834203809553646, + "velocityX": 1.145034807189669, + "velocityY": 0.09253424451600453, + "timestamp": 3.9601145415927537 + }, + { + "x": 3.573624189931311, + "y": 6.821154977138411, + "heading": 0.44126757500898656, + "angularVelocity": -0.4253075317731932, + "velocityX": 1.7175490108681681, + "velocityY": 0.13880092981701167, + "timestamp": 4.046172644203749 + }, + { + "x": 3.7707023793841614, + "y": 6.837081406357962, + "heading": 0.39243932813761767, + "angularVelocity": -0.567386979146327, + "velocityX": 2.2900596628730017, + "velocityY": 0.18506600466831086, + "timestamp": 4.132230746814745 + }, + { + "x": 4.017049385854338, + "y": 6.856989019626697, + "heading": 0.3313641468295269, + "angularVelocity": -0.7096970471683371, + "velocityX": 2.8625660919308524, + "velocityY": 0.23132758758031108, + "timestamp": 4.218288849425741 + }, + { + "x": 4.312664842319635, + "y": 6.880877294918687, + "heading": 0.25802182339028534, + "angularVelocity": -0.8522419297304485, + "velocityX": 3.4350682561699637, + "velocityY": 0.27758310452160395, + "timestamp": 4.304346952036736 + }, + { + "x": 4.637160999178445, + "y": 6.907084038065497, + "heading": 0.25802181146651204, + "angularVelocity": -1.385550100052896e-7, + "velocityX": 3.7706636215948364, + "velocityY": 0.304523831594114, + "timestamp": 4.390405054647732 + }, + { + "x": 4.9616571567181005, + "y": 6.933290772782941, + "heading": 0.25802179954292714, + "angularVelocity": -1.3855287596436928e-7, + "velocityX": 3.770663629506321, + "velocityY": 0.3045237336441463, + "timestamp": 4.476463157258728 + }, + { + "x": 5.286153314257592, + "y": 6.959497507502428, + "heading": 0.2580217876193422, + "angularVelocity": -1.3855287690850846e-7, + "velocityX": 3.7706636295043996, + "velocityY": 0.30452373366790714, + "timestamp": 4.562521259869723 + }, + { + "x": 5.610649471797095, + "y": 6.985704242221776, + "heading": 0.2580217756957572, + "angularVelocity": -1.385528773415127e-7, + "velocityX": 3.7706636295045306, + "velocityY": 0.3045237336662797, + "timestamp": 4.648579362480719 + }, + { + "x": 5.935145629336598, + "y": 7.011910976941122, + "heading": 0.25802176377217223, + "angularVelocity": -1.3855287632634293e-7, + "velocityX": 3.7706636295045324, + "velocityY": 0.30452373366626134, + "timestamp": 4.734637465091715 + }, + { + "x": 6.259641786876101, + "y": 7.038117711660469, + "heading": 0.25802175184858733, + "angularVelocity": -1.3855287660850554e-7, + "velocityX": 3.7706636295045315, + "velocityY": 0.30452373366627317, + "timestamp": 4.82069556770271 + }, + { + "x": 6.584137944415597, + "y": 7.064324446379899, + "heading": 0.2580217399250024, + "angularVelocity": -1.3855287639179296e-7, + "velocityX": 3.7706636295044533, + "velocityY": 0.3045237336672401, + "timestamp": 4.906753670313706 + }, + { + "x": 6.908634101955186, + "y": 7.090531181098188, + "heading": 0.2580217280014175, + "angularVelocity": -1.3855287562699177e-7, + "velocityX": 3.7706636295055262, + "velocityY": 0.3045237336539718, + "timestamp": 4.992811772924702 + }, + { + "x": 7.233130259094438, + "y": 7.116737920772481, + "heading": 0.25802171607764257, + "angularVelocity": -1.3855502925167896e-7, + "velocityX": 3.7706636248536065, + "velocityY": 0.30452379124316004, + "timestamp": 5.078869875535697 + }, + { + "x": 7.52873971550584, + "y": 7.140609322656376, + "heading": 0.18429681518138608, + "angularVelocity": -0.8566875013371068, + "velocityX": 3.4349985352055685, + "velocityY": 0.2773870345689411, + "timestamp": 5.164927978146693 + }, + { + "x": 7.775081036606935, + "y": 7.160501331656927, + "heading": 0.12286043910547154, + "angularVelocity": -0.7138941507190695, + "velocityX": 2.8625000276218113, + "velocityY": 0.23114626510492656, + "timestamp": 5.250986080757689 + }, + { + "x": 7.972154196137362, + "y": 7.176414449461249, + "heading": 0.07371418459504951, + "angularVelocity": -0.5710822458267809, + "velocityX": 2.2900012148910633, + "velocityY": 0.1849113252734793, + "timestamp": 5.337044183368684 + }, + { + "x": 8.119959129631837, + "y": 7.188349047759648, + "heading": 0.036856619638336335, + "angularVelocity": -0.4282869809901411, + "velocityX": 1.7175016530700593, + "velocityY": 0.13868070450431355, + "timestamp": 5.42310228597968 + }, + { + "x": 8.218495773200091, + "y": 7.1963053677311315, + "heading": 0.012285644906453631, + "angularVelocity": -0.28551611047234304, + "velocityX": 1.1450013488404933, + "velocityY": 0.09245288624879405, + "timestamp": 5.509160388590676 + }, + { + "x": 8.2677640914917, + "y": 7.200283527374268, + "heading": 0, + "angularVelocity": -0.14275988586599633, + "velocityX": 0.5725006338369774, + "velocityY": 0.046226439143295044, + "timestamp": 5.595218491201671 + }, + { + "x": 8.2677640914917, + "y": 7.200283527374268, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -3.7403039540233026e-33, + "timestamp": 5.681276593812667 + }, + { + "x": 8.209402430057438, + "y": 7.190289670117731, + "heading": 0.0036095788362639895, + "angularVelocity": 0.03838131856351764, + "velocityX": -0.6205703272376543, + "velocityY": -0.10626653038374882, + "timestamp": 5.775321795255102 + }, + { + "x": 8.092676418351024, + "y": 7.1703016582639245, + "heading": 0.010387056823911094, + "angularVelocity": 0.07206617544671082, + "velocityX": -1.2411692453853074, + "velocityY": -0.21253622244646914, + "timestamp": 5.869366996697536 + }, + { + "x": 7.917582655378263, + "y": 7.140319157705864, + "heading": 0.019665971369599126, + "angularVelocity": 0.09866441245990722, + "velocityX": -1.8618043269324378, + "velocityY": -0.31880946713051367, + "timestamp": 5.963412198139971 + }, + { + "x": 7.684117200932665, + "y": 7.10034191479348, + "heading": 0.030324783936255515, + "angularVelocity": 0.11333712302739128, + "velocityX": -2.4824813054230472, + "velocityY": -0.42508540892271196, + "timestamp": 6.057457399582406 + }, + { + "x": 7.39227895399914, + "y": 7.0503706021181705, + "heading": 0.04007805146811309, + "angularVelocity": 0.10370829539357472, + "velocityX": -3.1031699912104123, + "velocityY": -0.5313541989264866, + "timestamp": 6.15150260102484 + }, + { + "x": 7.042125768342003, + "y": 6.990417993094679, + "heading": 0.041681192088546266, + "angularVelocity": 0.01704649036837675, + "velocityX": -3.7232435072345225, + "velocityY": -0.6374871668522564, + "timestamp": 6.245547802467275 + }, + { + "x": 6.6914633234862775, + "y": 6.930365555430834, + "heading": 0.041681197110018026, + "angularVelocity": 5.33942963783108e-8, + "velocityX": -3.728658554369243, + "velocityY": -0.6385486632238303, + "timestamp": 6.33959300390971 + }, + { + "x": 6.341309093988729, + "y": 6.870412719224656, + "heading": 0.04328301737471915, + "angularVelocity": 0.017032450782756198, + "velocityX": -3.723254606582202, + "velocityY": -0.6374895825277324, + "timestamp": 6.433638205352144 + }, + { + "x": 6.0494705333637215, + "y": 6.820441331819641, + "heading": 0.05298413270489232, + "angularVelocity": 0.10315375141099561, + "velocityX": -3.1031733267498307, + "velocityY": -0.5313549935412731, + "timestamp": 6.527683406794579 + }, + { + "x": 5.81600477469543, + "y": 6.780464021198124, + "heading": 0.06358433999708792, + "angularVelocity": 0.11271396233125523, + "velocityX": -2.482484540279067, + "velocityY": -0.4250861288864222, + "timestamp": 6.621728608237014 + }, + { + "x": 5.640910724528676, + "y": 6.750481459365072, + "heading": 0.07281183701492056, + "angularVelocity": 0.09811768039955485, + "velocityX": -1.8618073807191666, + "velocityY": -0.31881011867883496, + "timestamp": 6.715773809679448 + }, + { + "x": 5.524184486258963, + "y": 6.730493399783685, + "heading": 0.07955165767088834, + "angularVelocity": 0.07166575809109048, + "velocityX": -1.2411716544746547, + "velocityY": -0.21253672994263784, + "timestamp": 6.809819011121883 + }, + { + "x": 5.465822696685791, + "y": 6.720499515533447, + "heading": 0.08314116334656949, + "angularVelocity": 0.03816787694349407, + "velocityX": -0.6205716897623333, + "velocityY": -0.10626681741275645, + "timestamp": 6.903864212564318 + }, + { + "x": 5.465822696685791, + "y": 6.720499515533447, + "heading": 0.08314116334656949, + "angularVelocity": 8.412036964343254e-33, + "velocityX": 0, + "velocityY": -2.9297022179051264e-34, + "timestamp": 6.997909414006752 + }, + { + "x": 5.517857585692567, + "y": 6.7009202818586795, + "heading": 0.08116164183957295, + "angularVelocity": -0.0217235022928802, + "velocityX": 0.5710370039563939, + "velocityY": -0.21486481764081972, + "timestamp": 7.089032911514364 + }, + { + "x": 5.621927362497105, + "y": 6.661761814969241, + "heading": 0.0772026019010839, + "angularVelocity": -0.04344697083467331, + "velocityX": 1.1420739946449114, + "velocityY": -0.4297296302324916, + "timestamp": 7.180156409021976 + }, + { + "x": 5.778032025282058, + "y": 6.603024115556083, + "heading": 0.07126404712081705, + "angularVelocity": -0.06517039998230038, + "velocityX": 1.7131109653896246, + "velocityY": -0.6445944352415494, + "timestamp": 7.271279906529587 + }, + { + "x": 5.986171571020342, + "y": 6.524707184767147, + "heading": 0.06334598490916293, + "angularVelocity": -0.08689374780707436, + "velocityX": 2.284147902914729, + "velocityY": -0.8594592276529842, + "timestamp": 7.362403404037199 + }, + { + "x": 6.2463459936710395, + "y": 6.426811024886955, + "heading": 0.05344843633991816, + "angularVelocity": -0.1086168643648252, + "velocityX": 2.855184774146016, + "velocityY": -1.074323994993733, + "timestamp": 7.4535269015448105 + }, + { + "x": 6.558555275153954, + "y": 6.30933564274033, + "heading": 0.04157148359406043, + "angularVelocity": -0.13033907905684416, + "velocityX": 3.42622144696285, + "velocityY": -1.2891886874380052, + "timestamp": 7.544650399052422 + }, + { + "x": 6.881186726040969, + "y": 6.187938689228568, + "heading": 0.041571481800887045, + "angularVelocity": -1.9678505938061922e-8, + "velocityX": 3.5405955621931517, + "velocityY": -1.3322244737325157, + "timestamp": 7.635773896560034 + }, + { + "x": 7.203818176927985, + "y": 6.066541735716808, + "heading": 0.041571480007713664, + "angularVelocity": -1.967850585555319e-8, + "velocityX": 3.5405955621931553, + "velocityY": -1.3322244737325053, + "timestamp": 7.726897394067645 + }, + { + "x": 7.516027457256037, + "y": 5.949066354035002, + "heading": 0.029694083932865854, + "angularVelocity": -0.13034394420143097, + "velocityX": 3.426221434289254, + "velocityY": -1.289188682337032, + "timestamp": 7.818020891575257 + }, + { + "x": 7.77620187882914, + "y": 5.85117019459155, + "heading": 0.01979612092341288, + "angularVelocity": -0.1086214124805027, + "velocityX": 2.85518476232035, + "velocityY": -1.074323990200902, + "timestamp": 7.9091443890828685 + }, + { + "x": 7.984341423634275, + "y": 5.772853264185132, + "heading": 0.011877698718430987, + "angularVelocity": -0.08689769841626724, + "velocityX": 2.284147892674251, + "velocityY": -0.8594592234551758, + "timestamp": 8.00026788659048 + }, + { + "x": 8.140446085681976, + "y": 5.714115565078498, + "heading": 0.005938858336697638, + "angularVelocity": -0.06517353420644122, + "velocityX": 1.7131109572989367, + "velocityY": -0.644594431877729, + "timestamp": 8.091391384098092 + }, + { + "x": 8.244515861980604, + "y": 5.674957098402801, + "heading": 0.0019796214029937777, + "angularVelocity": -0.04344913268336865, + "velocityX": 1.142073989092992, + "velocityY": -0.4297296278868681, + "timestamp": 8.182514881605703 + }, + { + "x": 8.296550750732422, + "y": 5.6553778648376465, + "heading": -1.364378444746575e-32, + "angularVelocity": -0.02172459856332236, + "velocityX": 0.5710370011584592, + "velocityY": -0.2148648164379124, + "timestamp": 8.273638379113315 + }, + { + "x": 8.296550750732422, + "y": 5.6553778648376465, + "heading": -2.7953004517751144e-32, + "angularVelocity": -1.5703141365783988e-31, + "velocityX": 0, + "velocityY": 3.434094774968783e-33, + "timestamp": 8.364761876620927 + }, + { + "x": 8.237994852774355, + "y": 5.664172303219014, + "heading": 0.007226618869060547, + "angularVelocity": 0.07681811545724591, + "velocityX": -0.6224423636571395, + "velocityY": 0.09348385395871893, + "timestamp": 8.458836286360688 + }, + { + "x": 8.120872431236778, + "y": 5.681762484801859, + "heading": 0.02080471998001932, + "angularVelocity": 0.1443336306696198, + "velocityX": -1.2449976764304742, + "velocityY": 0.1869815780029983, + "timestamp": 8.552910696100449 + }, + { + "x": 7.945169976448883, + "y": 5.708149977355932, + "heading": 0.03940846636956739, + "angularVelocity": 0.19775565364675843, + "velocityX": -1.867696595428377, + "velocityY": 0.28049596725681497, + "timestamp": 8.64698510584021 + }, + { + "x": 7.710871729254762, + "y": 5.743336322554468, + "heading": 0.06079052814790254, + "angularVelocity": 0.22728882208972376, + "velocityX": -2.4905630324148915, + "velocityY": 0.3740267443172734, + "timestamp": 8.741059515579972 + }, + { + "x": 7.417973686541333, + "y": 5.787320356921691, + "heading": 0.0802978988189348, + "angularVelocity": 0.2073610743406771, + "velocityX": -3.113472022026445, + "velocityY": 0.4675451537660226, + "timestamp": 8.835133925319733 + }, + { + "x": 7.066742108885601, + "y": 5.840051326513902, + "heading": 0.08234056213406721, + "angularVelocity": 0.021713272726732794, + "velocityX": -3.7335506927685165, + "velocityY": 0.5605240547147593, + "timestamp": 8.929208335059494 + }, + { + "x": 6.714812786263884, + "y": 5.892917473581175, + "heading": 0.0823405720722638, + "angularVelocity": 1.0564201978235854e-7, + "velocityX": -3.740967640352154, + "velocityY": 0.5619609755029853, + "timestamp": 9.023282744799255 + }, + { + "x": 6.363585735336453, + "y": 5.945647087732873, + "heading": 0.0843940929667477, + "angularVelocity": 0.02182868752812118, + "velocityX": -3.7335025741749357, + "velocityY": 0.5605096465402537, + "timestamp": 9.117357154539016 + }, + { + "x": 6.070689025568267, + "y": 5.989630737024261, + "heading": 0.10401416670033654, + "angularVelocity": 0.20855909474314377, + "velocityX": -3.1134578529740558, + "velocityY": 0.46754106045483973, + "timestamp": 9.211431564278778 + }, + { + "x": 5.836392076431593, + "y": 6.024816723804097, + "heading": 0.1255202540864113, + "angularVelocity": 0.2286071998262574, + "velocityX": -2.4905492342157323, + "velocityY": 0.3740229343683141, + "timestamp": 9.305505974018539 + }, + { + "x": 5.6606908312498, + "y": 6.051203885635972, + "heading": 0.14422987535302242, + "angularVelocity": 0.1988810912343688, + "velocityX": -1.8676837374566488, + "velocityY": 0.2804924517184302, + "timestamp": 9.3995803837583 + }, + { + "x": 5.543569343329225, + "y": 6.068793800338837, + "heading": 0.15788326554426388, + "angularVelocity": 0.14513394480677122, + "velocityX": -1.2449877521907207, + "velocityY": 0.186978741099956, + "timestamp": 9.493654793498061 + }, + { + "x": 5.485013961791992, + "y": 6.077588081359863, + "heading": 0.16514897587001573, + "angularVelocity": 0.07723365307987065, + "velocityX": -0.6224368741639325, + "velocityY": 0.09348218123665668, + "timestamp": 9.587729203237823 + }, + { + "x": 5.485013961791992, + "y": 6.077588081359863, + "heading": 0.16514897587001573, + "angularVelocity": -1.1689527354691748e-31, + "velocityX": 3.442040180158371e-34, + "velocityY": 0, + "timestamp": 9.681803612977584 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA123.1.traj b/src/main/deploy/choreo/CBA123.1.traj new file mode 100644 index 0000000..f092a31 --- /dev/null +++ b/src/main/deploy/choreo/CBA123.1.traj @@ -0,0 +1,85 @@ +{ + "samples": [ + { + "x": 1.3479586839675903, + "y": 4.920444488525391, + "heading": 2.622765580104145, + "angularVelocity": 1.5385294760575736e-37, + "velocityX": -4.525960803191592e-37, + "velocityY": 5.824643031906921e-39, + "timestamp": 0 + }, + { + "x": 1.4254576260477565, + "y": 4.880807633074535, + "heading": 2.6267034277668575, + "angularVelocity": 0.04436815590241169, + "velocityX": 0.8731889699653917, + "velocityY": -0.44659274120153025, + "timestamp": 0.08875391781831346 + }, + { + "x": 1.5804555064265071, + "y": 4.801533924729678, + "heading": 2.6345791298116996, + "angularVelocity": 0.08873638751321193, + "velocityX": 1.746377897323294, + "velocityY": -0.893185453594704, + "timestamp": 0.17750783563662692 + }, + { + "x": 1.8129523161590906, + "y": 4.68262336913622, + "heading": 2.646392694564592, + "angularVelocity": 0.13310471293307716, + "velocityX": 2.619566723899709, + "velocityY": -1.3397781023805322, + "timestamp": 0.2662617534549404 + }, + { + "x": 2.1229480096662936, + "y": 4.524075992400786, + "heading": 2.6621441294728503, + "angularVelocity": 0.17747312226264264, + "velocityX": 3.4927550369301996, + "velocityY": -1.7863704570201997, + "timestamp": 0.35501567127325384 + }, + { + "x": 2.355444821301696, + "y": 4.405165440711646, + "heading": 2.6739577129781713, + "angularVelocity": 0.13310492421872042, + "velocityX": 2.6195667453389717, + "velocityY": -1.3397780583901595, + "timestamp": 0.4437695890915673 + }, + { + "x": 2.510442703464613, + "y": 4.325891736084506, + "heading": 2.6818334361924214, + "angularVelocity": 0.0887366260312266, + "velocityX": 1.7463779174256908, + "velocityY": -0.8931854117067864, + "timestamp": 0.5325235069098807 + }, + { + "x": 2.5879416465759277, + "y": 4.2862548828125, + "heading": 2.6857712983829183, + "angularVelocity": 0.04436831958852742, + "velocityX": 0.873188981583452, + "velocityY": -0.4465927166521856, + "timestamp": 0.6212774247281941 + }, + { + "x": 2.5879416465759277, + "y": 4.2862548828125, + "heading": 2.6857712983829183, + "angularVelocity": 5.3847700504342055e-37, + "velocityX": 8.698817388407438e-39, + "velocityY": -2.770926604636497e-38, + "timestamp": 0.7100313425465076 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA123.2.traj b/src/main/deploy/choreo/CBA123.2.traj new file mode 100644 index 0000000..892d83e --- /dev/null +++ b/src/main/deploy/choreo/CBA123.2.traj @@ -0,0 +1,157 @@ +{ + "samples": [ + { + "x": 2.5879416465759277, + "y": 4.2862548828125, + "heading": 2.6857712983829183, + "angularVelocity": 5.3847700504342055e-37, + "velocityX": 8.698817388407438e-39, + "velocityY": -2.770926604636497e-38, + "timestamp": 0 + }, + { + "x": 2.557860644240392, + "y": 4.318524288125325, + "heading": 2.68919118628525, + "angularVelocity": 0.05411676522637325, + "velocityX": -0.47600581880369225, + "velocityY": 0.510635401270978, + "timestamp": 0.06319461054307274 + }, + { + "x": 2.498282487303618, + "y": 4.383591603835094, + "heading": 2.696354861077149, + "angularVelocity": 0.11335895150451837, + "velocityX": -0.9427727526885523, + "velocityY": 1.0296339379355688, + "timestamp": 0.12638922108614548 + }, + { + "x": 2.4102046272873507, + "y": 4.482316225754566, + "heading": 2.707800615933889, + "angularVelocity": 0.18111916124458857, + "velocityX": -1.3937558798029874, + "velocityY": 1.5622316693000622, + "timestamp": 0.1895838316292182 + }, + { + "x": 2.295704109453543, + "y": 4.616327372903078, + "heading": 2.7245932961796777, + "angularVelocity": 0.26572962633170844, + "velocityX": -1.8118715638854261, + "velocityY": 2.120610381120576, + "timestamp": 0.25277844217229095 + }, + { + "x": 2.1614517035876886, + "y": 4.789659527185801, + "heading": 2.7497269864105105, + "angularVelocity": 0.39771888796912597, + "velocityX": -2.124428091448555, + "velocityY": 2.742831276166849, + "timestamp": 0.3159730527153636 + }, + { + "x": 2.061575493662025, + "y": 4.989709507594673, + "heading": 2.792410295358657, + "angularVelocity": 0.6754264102799412, + "velocityX": -1.5804545524904996, + "velocityY": 3.165617743185872, + "timestamp": 0.3791676632584363 + }, + { + "x": 2.001798110386581, + "y": 5.171379149434758, + "heading": 2.8388621903309694, + "angularVelocity": 0.7350610213928208, + "velocityX": -0.9459253370143008, + "velocityY": 2.8747647984357916, + "timestamp": 0.44236227380150905 + }, + { + "x": 1.978911997185236, + "y": 5.328819797631775, + "heading": 2.886301638585647, + "angularVelocity": 0.7506881970946474, + "velocityX": -0.36215292735677707, + "velocityY": 2.4913619507110902, + "timestamp": 0.5055568843445818 + }, + { + "x": 1.9915421022340005, + "y": 5.4600540508319835, + "heading": 2.933718928886097, + "angularVelocity": 0.7503375666526398, + "velocityX": 0.1998604776613994, + "velocityY": 2.0766684385333893, + "timestamp": 0.5687514948876545 + }, + { + "x": 2.0389416217803955, + "y": 5.5640997886657715, + "heading": 2.9805967354734015, + "angularVelocity": 0.7418007039596033, + "velocityX": 0.750056359855052, + "velocityY": 1.6464337217945526, + "timestamp": 0.6319461054307273 + }, + { + "x": 2.163265986983934, + "y": 5.6534822842831955, + "heading": 3.041629416668671, + "angularVelocity": 0.7237838294156185, + "velocityX": 1.4743570718904808, + "velocityY": 1.059983007361601, + "timestamp": 0.7162705662915722 + }, + { + "x": 2.3447602212049414, + "y": 5.688965385139213, + "heading": 3.0987012296523844, + "angularVelocity": 0.6768120709113685, + "velocityX": 2.152331984909044, + "velocityY": 0.42079250188830075, + "timestamp": 0.800595027152417 + }, + { + "x": 2.566977292741073, + "y": 5.65742824832104, + "heading": 3.1433966419125996, + "angularVelocity": 0.530040889724432, + "velocityX": 2.635262286501201, + "velocityY": -0.3739974913117593, + "timestamp": 0.8849194880132619 + }, + { + "x": 2.7144228017968293, + "y": 5.604225780565181, + "heading": 3.1618565972535455, + "angularVelocity": 0.21891578259135683, + "velocityX": 1.748549680016051, + "velocityY": -0.6309256793666975, + "timestamp": 0.9692439488741068 + }, + { + "x": 2.786717653274536, + "y": 5.573565483093262, + "heading": 3.1693638048208683, + "angularVelocity": 0.08902763789633576, + "velocityX": 0.8573414017672781, + "velocityY": -0.3635990928245075, + "timestamp": 1.0535684097349516 + }, + { + "x": 2.786717653274536, + "y": 5.573565483093262, + "heading": 3.1693638048208683, + "angularVelocity": 2.931866046366828e-38, + "velocityX": -1.1847240425690822e-39, + "velocityY": -7.043293922543416e-40, + "timestamp": 1.1378928705957967 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA123.3.traj b/src/main/deploy/choreo/CBA123.3.traj new file mode 100644 index 0000000..540669f --- /dev/null +++ b/src/main/deploy/choreo/CBA123.3.traj @@ -0,0 +1,688 @@ +{ + "samples": [ + { + "x": 2.786717653274536, + "y": 5.573565483093262, + "heading": 3.1693638048208683, + "angularVelocity": 2.931866046366828e-38, + "velocityX": -1.1847240425690822e-39, + "velocityY": -7.043293922543416e-40, + "timestamp": 0 + }, + { + "x": 2.7838634682756775, + "y": 5.575306183326131, + "heading": 3.1696268104013123, + "angularVelocity": 0.015118256006559196, + "velocityX": -0.16406609863566005, + "velocityY": 0.1000600508429731, + "timestamp": 0.017396555550434956 + }, + { + "x": 2.7781635414165, + "y": 5.578801206347808, + "heading": 3.170158474700968, + "angularVelocity": 0.030561469373312153, + "velocityX": -0.32764686334906284, + "velocityY": 0.20090316221186844, + "timestamp": 0.03479311110086991 + }, + { + "x": 2.7696273846815345, + "y": 5.5840657232897595, + "heading": 3.170965105092508, + "angularVelocity": 0.0463672471944818, + "velocityX": -0.4906808540471063, + "velocityY": 0.30261835032165785, + "timestamp": 0.05218966665130487 + }, + { + "x": 2.758265793234076, + "y": 5.5911167319231625, + "heading": 3.172053783081744, + "angularVelocity": 0.06258008868939276, + "velocityX": -0.6530943102224854, + "velocityY": 0.40531061525144174, + "timestamp": 0.06958622220173982 + }, + { + "x": 2.744091120487868, + "y": 5.599973404947456, + "heading": 3.1734325151970073, + "angularVelocity": 0.07925316659762915, + "velocityX": -0.8147976595202109, + "velocityY": 0.5091049776271751, + "timestamp": 0.08698277775217478 + }, + { + "x": 2.7271176368126726, + "y": 5.610657531920939, + "heading": 3.1751104254879197, + "angularVelocity": 0.09645071899710177, + "velocityX": -0.9756807102410053, + "velocityY": 0.6141518614134843, + "timestamp": 0.10437933330260973 + }, + { + "x": 2.7073620058359746, + "y": 5.6231940881572715, + "heading": 3.1770980048968873, + "angularVelocity": 0.11425131849838457, + "velocityX": -1.1356058916044396, + "velocityY": 0.7206343922501016, + "timestamp": 0.12177588885304469 + }, + { + "x": 2.684843930071724, + "y": 5.637611978983044, + "heading": 3.1794074399377976, + "angularVelocity": 0.1327524310323994, + "velocityX": -1.2943985203834083, + "velocityY": 0.8287784776689098, + "timestamp": 0.13917244440347964 + }, + { + "x": 2.6595870470813243, + "y": 5.653945031227922, + "heading": 3.182053054495388, + "angularVelocity": 0.152076918325605, + "velocityX": -1.4518323996481046, + "velocityY": 0.9388670186765858, + "timestamp": 0.15656899995391438 + }, + { + "x": 2.6316202081179507, + "y": 5.672233341323654, + "heading": 3.1850519171745817, + "angularVelocity": 0.17238255414985487, + "velocityX": -1.6076078326134229, + "velocityY": 1.0512604085740558, + "timestamp": 0.1739655555043491 + }, + { + "x": 2.600979362390935, + "y": 5.69252515090379, + "heading": 3.1884246981498414, + "angularVelocity": 0.19387636624280877, + "velocityX": -1.76131680999629, + "velocityY": 1.16642685509252, + "timestamp": 0.19136211105478385 + }, + { + "x": 2.56771044224616, + "y": 5.714879524325921, + "heading": 3.1921969149485707, + "angularVelocity": 0.21683699326533024, + "velocityX": -1.9123854747179205, + "velocityY": 1.2849884770190956, + "timestamp": 0.20875866660521858 + }, + { + "x": 2.531873988844058, + "y": 5.739370280413082, + "heading": 3.19640080864036, + "angularVelocity": 0.24165092219559348, + "velocityX": -2.0599740735002126, + "velocityY": 1.4077934000301997, + "timestamp": 0.2261552221556533 + }, + { + "x": 2.4935529941684056, + "y": 5.766091937198378, + "heading": 3.2010782889997142, + "angularVelocity": 0.26887393575087964, + "velocityX": -2.202792073669703, + "velocityY": 1.5360314694380732, + "timestamp": 0.24355177770608805 + }, + { + "x": 2.4528671363352914, + "y": 5.79516892673793, + "heading": 3.2062857888830454, + "angularVelocity": 0.29934085907027813, + "velocityX": -2.338730659363013, + "velocityY": 1.6714222223620452, + "timestamp": 0.2609483332565228 + }, + { + "x": 2.4100008652071403, + "y": 5.826769900846777, + "heading": 3.212102729976151, + "angularVelocity": 0.334373150836745, + "velocityX": -2.464066579379806, + "velocityY": 1.8165075274373275, + "timestamp": 0.2783448888069575 + }, + { + "x": 2.365264409264796, + "y": 5.861127730914518, + "heading": 3.2186471517684527, + "angularVelocity": 0.37619066448691335, + "velocityX": -2.571569746243583, + "velocityY": 1.9749788955711667, + "timestamp": 0.29574144435739225 + }, + { + "x": 2.3192374036487657, + "y": 5.898548491541854, + "heading": 3.2261037658581118, + "angularVelocity": 0.42862588907565374, + "velocityX": -2.645753952993267, + "velocityY": 2.151044240846847, + "timestamp": 0.313137999907827 + }, + { + "x": 2.2730712823306805, + "y": 5.939277233096012, + "heading": 3.234722066355753, + "angularVelocity": 0.49540269466881787, + "velocityX": -2.6537506912930886, + "velocityY": 2.3411957290096908, + "timestamp": 0.3305345554582617 + }, + { + "x": 2.2285555727829873, + "y": 5.9828746912671305, + "heading": 3.2445111987990307, + "angularVelocity": 0.5627052099421548, + "velocityX": -2.5588806599465155, + "velocityY": 2.5060971434674393, + "timestamp": 0.34793111100869645 + }, + { + "x": 2.18692929945952, + "y": 6.028114885994637, + "heading": 3.255210019353813, + "angularVelocity": 0.614996487308366, + "velocityX": -2.39278822769182, + "velocityY": 2.6005259832240544, + "timestamp": 0.3653276665591312 + }, + { + "x": 2.1485716857950186, + "y": 6.074017979779568, + "heading": 3.2665254860070387, + "angularVelocity": 0.6504429351213081, + "velocityX": -2.2048970299492767, + "velocityY": 2.638631173386685, + "timestamp": 0.3827242221095659 + }, + { + "x": 2.113553488124031, + "y": 6.12000095046398, + "heading": 3.2782612178988115, + "angularVelocity": 0.6746008919840083, + "velocityX": -2.0129385710559187, + "velocityY": 2.6432227087196347, + "timestamp": 0.40012077766000065 + }, + { + "x": 2.0818666977257543, + "y": 6.165710242696975, + "heading": 3.290293518998621, + "angularVelocity": 0.6916484740284482, + "velocityX": -1.8214404746050021, + "velocityY": 2.6274909478763533, + "timestamp": 0.4175173332104354 + }, + { + "x": 2.053485472561832, + "y": 6.210915958519219, + "heading": 3.30254009499683, + "angularVelocity": 0.7039655616138741, + "velocityX": -1.6314278468310237, + "velocityY": 2.598544044606179, + "timestamp": 0.4349138887608701 + }, + { + "x": 2.028382147011791, + "y": 6.255458802658021, + "heading": 3.31494295383331, + "angularVelocity": 0.7129491122838909, + "velocityX": -1.443005512054579, + "velocityY": 2.560440427972457, + "timestamp": 0.45231044431130485 + }, + { + "x": 2.006531387313589, + "y": 6.299222662006888, + "heading": 3.3274592724833716, + "angularVelocity": 0.719471082294136, + "velocityX": -1.2560394288888732, + "velocityY": 2.5156623230380273, + "timestamp": 0.4697069998617396 + }, + { + "x": 1.9879109996275754, + "y": 6.34211946303162, + "heading": 3.3400562374477403, + "angularVelocity": 0.7241068456251942, + "velocityX": -1.070349106294682, + "velocityY": 2.4658215185396353, + "timestamp": 0.4871035554121743 + }, + { + "x": 1.9725017779583829, + "y": 6.384080257529668, + "heading": 3.3527079517381098, + "angularVelocity": 0.7272539816109163, + "velocityX": -0.8857627950842983, + "velocityY": 2.412017389097379, + "timestamp": 0.504500110962609 + }, + { + "x": 1.9602871128958277, + "y": 6.4250496778901445, + "heading": 3.365393481996193, + "angularVelocity": 0.7291978128260018, + "velocityX": -0.7021312366774745, + "velocityY": 2.355030583019786, + "timestamp": 0.5218966665130438 + }, + { + "x": 1.9512525840043362, + "y": 6.464982324825169, + "heading": 3.3780955702982447, + "angularVelocity": 0.7301496129637277, + "velocityX": -0.5193286030271496, + "velocityY": 2.295434105863935, + "timestamp": 0.5392932220634785 + }, + { + "x": 1.9453855996438743, + "y": 6.503840320779697, + "heading": 3.3907997524885256, + "angularVelocity": 0.730269975194225, + "velocityX": -0.33724977013138463, + "velocityY": 2.2336603267166004, + "timestamp": 0.5566897776139133 + }, + { + "x": 1.9426750959440404, + "y": 6.541591597502305, + "heading": 3.403493735583254, + "angularVelocity": 0.7296837042210303, + "velocityX": -0.1558069177530964, + "velocityY": 2.1700431797065427, + "timestamp": 0.574086333164348 + }, + { + "x": 1.943111290335544, + "y": 6.5782086634174055, + "heading": 3.4161669462041386, + "angularVelocity": 0.7284896475134609, + "velocityX": 0.025073606682601935, + "velocityY": 2.104845744259196, + "timestamp": 0.5914828887147827 + }, + { + "x": 1.946685480968561, + "y": 6.6136676947141995, + "heading": 3.4288101954137264, + "angularVelocity": 0.7267673863905522, + "velocityX": 0.20545392578748464, + "velocityY": 2.0382788531898433, + "timestamp": 0.6088794442652175 + }, + { + "x": 1.953389883365602, + "y": 6.647947850963729, + "heading": 3.441415424900957, + "angularVelocity": 0.7245819122461787, + "velocityX": 0.3853867725484022, + "velocityY": 1.9705139991733958, + "timestamp": 0.6262759998156522 + }, + { + "x": 1.963217496871948, + "y": 6.681030750274658, + "heading": 3.453975511355717, + "angularVelocity": 0.7219869714062964, + "velocityX": 0.5649172031701521, + "velocityY": 1.901692505451284, + "timestamp": 0.6436725553660869 + }, + { + "x": 1.972254759310776, + "y": 6.704973117440469, + "heading": 3.4632918988879884, + "angularVelocity": 0.7197850864325751, + "velocityX": 0.698219852181241, + "velocityY": 1.8497898203727527, + "timestamp": 0.656615845950844 + }, + { + "x": 1.9830118282595994, + "y": 6.72822960606617, + "heading": 3.4725743160294034, + "angularVelocity": 0.7171605304408409, + "velocityX": 0.8310922851018642, + "velocityY": 1.7967987717968175, + "timestamp": 0.6695591365356011 + }, + { + "x": 1.995482319152762, + "y": 6.750784474341597, + "heading": 3.4818166306621796, + "angularVelocity": 0.7140622063805339, + "velocityX": 0.9634714458043958, + "velocityY": 1.7425915093019795, + "timestamp": 0.6825024271203581 + }, + { + "x": 2.009658846146705, + "y": 6.772620021168416, + "heading": 3.4910119236181756, + "angularVelocity": 0.7104293066575377, + "velocityX": 1.095279975452167, + "velocityY": 1.687016658077186, + "timestamp": 0.6954457177051152 + }, + { + "x": 2.0255327787443296, + "y": 6.793716203810744, + "heading": 3.5001523295995915, + "angularVelocity": 0.7061887331943414, + "velocityX": 1.2264217119809064, + "velocityY": 1.6298933029574858, + "timestamp": 0.7083890082898723 + }, + { + "x": 2.0430939161796475, + "y": 6.814050151845941, + "heading": 3.5092288330791357, + "angularVelocity": 0.7012516191387282, + "velocityX": 1.3567753362501818, + "velocityY": 1.5710029765647726, + "timestamp": 0.7213322988746294 + }, + { + "x": 2.0623300431045437, + "y": 6.833595541010598, + "heading": 3.5182310024639936, + "angularVelocity": 0.6955085591185912, + "velocityX": 1.4861852014316905, + "velocityY": 1.510078834796003, + "timestamp": 0.7342755894593864 + }, + { + "x": 2.0832263096675794, + "y": 6.852321774889104, + "heading": 3.5271466380085204, + "angularVelocity": 0.6888229454592012, + "velocityX": 1.614447765519879, + "velocityY": 1.4467908107199958, + "timestamp": 0.7472188800441435 + }, + { + "x": 2.105764344470479, + "y": 6.87019289901877, + "heading": 3.5359612967387575, + "angularVelocity": 0.6810214661036674, + "velocityX": 1.7412909534335712, + "velocityY": 1.3807249410526374, + "timestamp": 0.7601621706289006 + }, + { + "x": 2.129920948302108, + "y": 6.887166137027593, + "heading": 3.544657638059313, + "angularVelocity": 0.6718802505134741, + "velocityX": 1.866341767840478, + "velocityY": 1.311354164358459, + "timestamp": 0.7731054612136576 + }, + { + "x": 2.1556661064734146, + "y": 6.903189887018867, + "heading": 3.5532145015981276, + "angularVelocity": 0.6611041823391853, + "velocityX": 1.9890736441956522, + "velocityY": 1.2379966196651884, + "timestamp": 0.7860487517984147 + }, + { + "x": 2.1829598496443197, + "y": 6.91820094556084, + "heading": 3.5616055752204656, + "angularVelocity": 0.6482952358513759, + "velocityX": 2.1087174851075265, + "velocityY": 1.1597559711477912, + "timestamp": 0.7989920423831718 + }, + { + "x": 2.2117470850673566, + "y": 6.9321206491026155, + "heading": 3.5697974211323467, + "angularVelocity": 0.6329028818628413, + "velocityX": 2.2241048545211815, + "velocityY": 1.075437768365362, + "timestamp": 0.8119353329679289 + }, + { + "x": 2.2419486962848367, + "y": 6.944849624778968, + "heading": 3.577746483191445, + "angularVelocity": 0.6141453757099231, + "velocityX": 2.333379678043173, + "velocityY": 0.9834420075017508, + "timestamp": 0.8248786235526859 + }, + { + "x": 2.2734455518567573, + "y": 6.956261311165807, + "heading": 3.585394523284631, + "angularVelocity": 0.5908883867749319, + "velocityX": 2.433450393906261, + "velocityY": 0.8816680976225674, + "timestamp": 0.837821914137443 + }, + { + "x": 2.3060491115565114, + "y": 6.966196859192603, + "heading": 3.592662176763274, + "angularVelocity": 0.561499676689782, + "velocityX": 2.5189544719137884, + "velocityY": 0.7676214917477436, + "timestamp": 0.8507652047222001 + }, + { + "x": 2.3394501625904085, + "y": 6.9744733277760504, + "heading": 3.599444288228278, + "angularVelocity": 0.523986649344906, + "velocityX": 2.5805687367656036, + "velocityY": 0.6394408384213112, + "timestamp": 0.8637084953069571 + }, + { + "x": 2.3731543733238665, + "y": 6.9809397435508185, + "heading": 3.6056298261391637, + "angularVelocity": 0.477895313435202, + "velocityX": 2.6039908872284765, + "velocityY": 0.49959596691611147, + "timestamp": 0.8766517858917142 + }, + { + "x": 2.4064949438216043, + "y": 6.985610811483352, + "heading": 3.6111636065879185, + "angularVelocity": 0.4275404629539523, + "velocityX": 2.5758960041429493, + "velocityY": 0.3608872026742504, + "timestamp": 0.8895950764764713 + }, + { + "x": 2.438840596898612, + "y": 6.988743182156231, + "heading": 3.6160526389087924, + "angularVelocity": 0.37772715437845295, + "velocityX": 2.499028578953463, + "velocityY": 0.2420072895966135, + "timestamp": 0.9025383670612284 + }, + { + "x": 2.4697821805679805, + "y": 6.990692528960885, + "heading": 3.6203582923022264, + "angularVelocity": 0.3326552367219884, + "velocityX": 2.3905500279663556, + "velocityY": 0.15060674037166785, + "timestamp": 0.9154816576459854 + }, + { + "x": 2.4991030303812836, + "y": 6.991770372494304, + "heading": 3.6241647361521734, + "angularVelocity": 0.2940862545749717, + "velocityX": 2.2653319587704477, + "velocityY": 0.08327430542956869, + "timestamp": 0.9284249482307425 + }, + { + "x": 2.5266936990356643, + "y": 6.992212575691012, + "heading": 3.6275448907630823, + "angularVelocity": 0.26115110286480986, + "velocityX": 2.1316579793760706, + "velocityY": 0.03416466576350692, + "timestamp": 0.9413682388154996 + }, + { + "x": 2.552497875190756, + "y": 6.992192727058408, + "heading": 3.6305547138750143, + "angularVelocity": 0.23253925207214746, + "velocityX": 1.993633379867123, + "velocityY": -0.0015335074549528095, + "timestamp": 0.9543115294002567 + }, + { + "x": 2.5764856506570815, + "y": 6.991840108609927, + "heading": 3.6332366293718588, + "angularVelocity": 0.2072050750373046, + "velocityX": 1.8532980704747994, + "velocityY": -0.027243338637266013, + "timestamp": 0.9672548199850137 + }, + { + "x": 2.598640768592086, + "y": 6.991253252700003, + "heading": 3.635623341291647, + "angularVelocity": 0.1843976154409533, + "velocityX": 1.7117067557067622, + "velocityY": -0.04534054969106539, + "timestamp": 0.9801981105697708 + }, + { + "x": 2.6189543568583464, + "y": 6.990509100455072, + "heading": 3.6377406220206474, + "angularVelocity": 0.16358133313438186, + "velocityX": 1.5694299786626271, + "velocityY": -0.05749328117587533, + "timestamp": 0.9931414011545279 + }, + { + "x": 2.637421701817117, + "y": 6.989669089846847, + "heading": 3.6396092140952665, + "angularVelocity": 0.14436762138521886, + "velocityX": 1.426789025390412, + "velocityY": -0.064899308465973, + "timestamp": 1.006084691739285 + }, + { + "x": 2.6540405079363083, + "y": 6.988783254129454, + "heading": 3.641246127061458, + "angularVelocity": 0.12646806895606091, + "velocityX": 1.2839707190660168, + "velocityY": -0.06843976124870382, + "timestamp": 1.019027982324042 + }, + { + "x": 2.668809918789073, + "y": 6.987893040987183, + "heading": 3.642665537885756, + "angularVelocity": 0.10966383046128683, + "velocityX": 1.1410862451127886, + "velocityY": -0.06877796155789115, + "timestamp": 1.031971272908799 + }, + { + "x": 2.6817299462844417, + "y": 6.987033297747524, + "heading": 3.6438794301898154, + "angularVelocity": 0.09378544784344418, + "velocityX": 0.9982026912525851, + "velocityY": -0.06642385365831636, + "timestamp": 1.0449145634935562 + }, + { + "x": 2.692801127848911, + "y": 6.986233701516961, + "heading": 3.6448980582213584, + "angularVelocity": 0.07869930948955245, + "velocityX": 0.8553606590202942, + "velocityY": -0.06177688937186473, + "timestamp": 1.0578578540783132 + }, + { + "x": 2.70202431549513, + "y": 6.985519811426032, + "heading": 3.6457302906268625, + "angularVelocity": 0.06429836370082181, + "velocityX": 0.7125844533754433, + "velocityY": -0.05515522395592016, + "timestamp": 1.0708011446630703 + }, + { + "x": 2.7094005435757, + "y": 6.9849138578690955, + "heading": 3.646383869984044, + "angularVelocity": 0.05049561028563871, + "velocityX": 0.5698881619220394, + "velocityY": -0.04681603591984091, + "timestamp": 1.0837444352478274 + }, + { + "x": 2.71493094473244, + "y": 6.984435344858166, + "heading": 3.6468656120769536, + "angularVelocity": 0.037219445067315414, + "velocityX": 0.42727937849538145, + "velocityY": -0.036969965851833674, + "timestamp": 1.0966877258325844 + }, + { + "x": 2.71861669603319, + "y": 6.984101517009836, + "heading": 3.6471815612498695, + "angularVelocity": 0.024410266527419688, + "velocityX": 0.2847615354545603, + "velocityY": -0.02579157488156991, + "timestamp": 1.1096310164173415 + }, + { + "x": 2.720458984375, + "y": 6.9839277267456055, + "heading": 3.6473371131923193, + "angularVelocity": 0.012017959531342692, + "velocityX": 0.14233539220539884, + "velocityY": -0.013427054201720174, + "timestamp": 1.1225743070020986 + }, + { + "x": 2.720458984375, + "y": 6.9839277267456055, + "heading": 3.6473371131923193, + "angularVelocity": 2.1725782853578066e-36, + "velocityX": 0, + "velocityY": -3.4267891550955994e-37, + "timestamp": 1.1355175975868557 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA123.4.traj b/src/main/deploy/choreo/CBA123.4.traj new file mode 100644 index 0000000..429a5f1 --- /dev/null +++ b/src/main/deploy/choreo/CBA123.4.traj @@ -0,0 +1,175 @@ +{ + "samples": [ + { + "x": 2.720458984375, + "y": 6.9839277267456055, + "heading": 3.6473371131923193, + "angularVelocity": 2.1725782853578066e-36, + "velocityX": 0, + "velocityY": -3.4267891550955994e-37, + "timestamp": 0 + }, + { + "x": 2.8037165851892607, + "y": 6.9904871579633525, + "heading": 3.6321201190303314, + "angularVelocity": -0.17480730059204416, + "velocityX": 0.9564330706301294, + "velocityY": 0.07535236278514325, + "timestamp": 0.08705010666288215 + }, + { + "x": 2.97023156700119, + "y": 7.003606007121592, + "heading": 3.6016759055237015, + "angularVelocity": -0.3497320643676068, + "velocityX": 1.9128636160871058, + "velocityY": 0.15070457304600782, + "timestamp": 0.1741002133257643 + }, + { + "x": 3.220003551699753, + "y": 7.023284160432856, + "heading": 3.5559874461834298, + "angularVelocity": -0.5248524222630633, + "velocityX": 2.8692898179418815, + "velocityY": 0.2260554761577808, + "timestamp": 0.26115031998864646 + }, + { + "x": 3.5530320286012063, + "y": 7.049521300592341, + "heading": 3.4950321381493312, + "angularVelocity": -0.7002324336046938, + "velocityX": 3.825710153247341, + "velocityY": 0.3014027341872559, + "timestamp": 0.3482004266515286 + }, + { + "x": 3.942259099478333, + "y": 7.080171032524024, + "heading": 3.495032118406403, + "angularVelocity": -2.267995872008394e-7, + "velocityX": 4.471299183865228, + "velocityY": 0.3520929853696892, + "timestamp": 0.43525053331441077 + }, + { + "x": 4.331486170555169, + "y": 7.1108207619204435, + "heading": 3.4950320986636445, + "angularVelocity": -2.267976380492203e-7, + "velocityX": 4.471299186159417, + "velocityY": 0.3520929562454923, + "timestamp": 0.5223006399772929 + }, + { + "x": 4.720713241632006, + "y": 7.141470491316854, + "heading": 3.495032078920886, + "angularVelocity": -2.2679763752591037e-7, + "velocityX": 4.471299186159426, + "velocityY": 0.3520929562453827, + "timestamp": 0.6093507466401751 + }, + { + "x": 5.109940312708843, + "y": 7.172120220713263, + "heading": 3.4950320591781274, + "angularVelocity": -2.267976369220769e-7, + "velocityX": 4.471299186159426, + "velocityY": 0.35209295624538245, + "timestamp": 0.6964008533030572 + }, + { + "x": 5.4991673837856805, + "y": 7.202769950109673, + "heading": 3.495032039435369, + "angularVelocity": -2.26797638537719e-7, + "velocityX": 4.471299186159426, + "velocityY": 0.3520929562453829, + "timestamp": 0.7834509599659394 + }, + { + "x": 5.888394454862517, + "y": 7.233419679506083, + "heading": 3.4950320196926103, + "angularVelocity": -2.2679763777636123e-7, + "velocityX": 4.471299186159426, + "velocityY": 0.35209295624538245, + "timestamp": 0.8705010666288215 + }, + { + "x": 6.277621525939354, + "y": 7.264069408902492, + "heading": 3.4950319999498514, + "angularVelocity": -2.2679763839745266e-7, + "velocityX": 4.471299186159426, + "velocityY": 0.3520929562453826, + "timestamp": 0.9575511732917037 + }, + { + "x": 6.66684859701619, + "y": 7.29471913829891, + "heading": 3.495031980207093, + "angularVelocity": -2.267976392668664e-7, + "velocityX": 4.471299186159418, + "velocityY": 0.35209295624547304, + "timestamp": 1.0446012799545863 + }, + { + "x": 7.056075667928158, + "y": 7.32536886978814, + "heading": 3.495031960464165, + "angularVelocity": -2.267995828480058e-7, + "velocityX": 4.471299184265469, + "velocityY": 0.35209298028694325, + "timestamp": 1.131651386617469 + }, + { + "x": 7.38909841721707, + "y": 7.3515996523388205, + "heading": 3.433802950980574, + "angularVelocity": -0.7033766164206079, + "velocityX": 3.8256443565153195, + "velocityY": 0.30132970028703093, + "timestamp": 1.2187014932803515 + }, + { + "x": 7.638865368158441, + "y": 7.371272393933456, + "heading": 3.3878742662647525, + "angularVelocity": -0.5276120441033825, + "velocityX": 2.869231991968013, + "velocityY": 0.225993308323234, + "timestamp": 1.305751599943234 + }, + { + "x": 7.805376647095578, + "y": 7.3843874359578985, + "heading": 3.35725340940439, + "angularVelocity": -0.3517612790406731, + "velocityX": 1.9128210788067521, + "velocityY": 0.15066083807608674, + "timestamp": 1.3928017066061167 + }, + { + "x": 7.888632297515869, + "y": 7.390944957733154, + "heading": 3.3419432761879757, + "angularVelocity": -0.1758772482118357, + "velocityX": 0.9564106652127833, + "velocityY": 0.0753304278035119, + "timestamp": 1.4798518132689993 + }, + { + "x": 7.888632297515869, + "y": 7.390944957733154, + "heading": 3.3419432761879757, + "angularVelocity": 3.9030345535831066e-35, + "velocityX": 8.804773966904886e-37, + "velocityY": 1.1323533960883966e-35, + "timestamp": 1.5669019199318819 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA123.5.traj b/src/main/deploy/choreo/CBA123.5.traj new file mode 100644 index 0000000..ef13fde --- /dev/null +++ b/src/main/deploy/choreo/CBA123.5.traj @@ -0,0 +1,103 @@ +{ + "samples": [ + { + "x": 7.888632297515869, + "y": 7.390944957733154, + "heading": 3.3419432761879757, + "angularVelocity": 3.9030345535831066e-35, + "velocityX": 8.804773966904886e-37, + "velocityY": 1.1323533960883966e-35, + "timestamp": 0 + }, + { + "x": 7.782881118230512, + "y": 7.362639799509245, + "heading": 3.3425923011938354, + "angularVelocity": 0.006521281545109612, + "velocityX": -1.0625680175965528, + "velocityY": -0.2844049216754242, + "timestamp": 0.09952415048643992 + }, + { + "x": 7.57137876356566, + "y": 7.306029484106841, + "heading": 3.343890344728581, + "angularVelocity": 0.01304249801079575, + "velocityX": -2.1251359959477387, + "velocityY": -0.5688098328467204, + "timestamp": 0.19904830097287984 + }, + { + "x": 7.254125241333047, + "y": 7.221114013616789, + "heading": 3.3458373938423662, + "angularVelocity": 0.019563584358860877, + "velocityX": -3.187703895808074, + "velocityY": -0.8532147230096001, + "timestamp": 0.29857245145931977 + }, + { + "x": 6.8311205749680965, + "y": 7.107893394311742, + "heading": 3.348433409842623, + "angularVelocity": 0.026084281931252454, + "velocityX": -4.2502715601936885, + "velocityY": -1.1376195501460042, + "timestamp": 0.3980966019457597 + }, + { + "x": 6.399919425946622, + "y": 6.992478920143246, + "heading": 3.348433410548539, + "angularVelocity": 7.092910467358784e-9, + "velocityX": -4.332628280813384, + "velocityY": -1.1596629923932005, + "timestamp": 0.4976207524321996 + }, + { + "x": 5.976914759581955, + "y": 6.879258300837546, + "heading": 3.3510294266908165, + "angularVelocity": 0.026084283358251074, + "velocityX": -4.250271560190831, + "velocityY": -1.1376195501525759, + "timestamp": 0.5971449029186395 + }, + { + "x": 5.659661237349591, + "y": 6.794342830346922, + "heading": 3.3529764759284917, + "angularVelocity": 0.019563585603683077, + "velocityX": -3.187703895805583, + "velocityY": -0.8532147230153324, + "timestamp": 0.6966690534050795 + }, + { + "x": 5.44815888268492, + "y": 6.7377325149441, + "heading": 3.3542745195538526, + "angularVelocity": 0.013042498921281074, + "velocityX": -2.1251359959459153, + "velocityY": -0.568809832850927, + "timestamp": 0.7961932038915194 + }, + { + "x": 5.342407703399658, + "y": 6.709427356719971, + "heading": 3.354923544607161, + "angularVelocity": 0.006521282021865445, + "velocityX": -1.0625680175955954, + "velocityY": -0.28440492167764353, + "timestamp": 0.8957173543779593 + }, + { + "x": 5.342407703399658, + "y": 6.709427356719971, + "heading": 3.354923544607161, + "angularVelocity": -8.321210302525092e-35, + "velocityX": 3.972717142215159e-38, + "velocityY": 2.156619931918946e-38, + "timestamp": 0.9952415048643992 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA123.6.traj b/src/main/deploy/choreo/CBA123.6.traj new file mode 100644 index 0000000..e8ea8ab --- /dev/null +++ b/src/main/deploy/choreo/CBA123.6.traj @@ -0,0 +1,121 @@ +{ + "samples": [ + { + "x": 5.342407703399658, + "y": 6.709427356719971, + "heading": 3.354923544607161, + "angularVelocity": -8.321210302525092e-35, + "velocityX": 3.972717142215159e-38, + "velocityY": 2.156619931918946e-38, + "timestamp": 0 + }, + { + "x": 5.412646620847983, + "y": 6.685272362409357, + "heading": 3.3376723045170427, + "angularVelocity": -0.20995551242929855, + "velocityX": 0.8548398740209807, + "velocityY": -0.29397737100168153, + "timestamp": 0.08216616887316697 + }, + { + "x": 5.553160406591399, + "y": 6.636944177185238, + "heading": 3.3042040470339473, + "angularVelocity": -0.40732405979348324, + "velocityX": 1.710117286353154, + "velocityY": -0.5881762030151344, + "timestamp": 0.16433233774633393 + }, + { + "x": 5.764011811265675, + "y": 6.564410113443709, + "heading": 3.256520385123487, + "angularVelocity": -0.5803320583690068, + "velocityX": 2.5661584027332363, + "velocityY": -0.8827728581758841, + "timestamp": 0.2464985066195009 + }, + { + "x": 6.045327886786299, + "y": 6.467599426347047, + "heading": 3.200063171820206, + "angularVelocity": -0.6871102070053786, + "velocityX": 3.4237457992579357, + "velocityY": -1.1782305104927078, + "timestamp": 0.32866467549266787 + }, + { + "x": 6.393950264167201, + "y": 6.3481237380245314, + "heading": 3.2000631015766148, + "angularVelocity": -8.548967570988147e-7, + "velocityX": 4.242894395125564, + "velocityY": -1.4540739815548667, + "timestamp": 0.41083084436583484 + }, + { + "x": 6.742456977163128, + "y": 6.228311034623432, + "heading": 3.200063071871244, + "angularVelocity": -3.615279995739255e-7, + "velocityX": 4.241486706455621, + "velocityY": -1.4581756097943859, + "timestamp": 0.4929970132390018 + }, + { + "x": 7.091069051642512, + "y": 6.108805287445655, + "heading": 3.200063001812038, + "angularVelocity": -8.526527023394516e-7, + "velocityX": 4.242769004083742, + "velocityY": -1.454439811624267, + "timestamp": 0.5751631821121688 + }, + { + "x": 7.372382443797803, + "y": 6.012003983976649, + "heading": 3.14332703013963, + "angularVelocity": -0.690502824343514, + "velocityX": 3.4237131414698108, + "velocityY": -1.1781163074358554, + "timestamp": 0.6573293509853357 + }, + { + "x": 7.583230269821529, + "y": 5.939474882226305, + "heading": 3.095396790832798, + "angularVelocity": -0.5833330184935077, + "velocityX": 2.5661148489129793, + "velocityY": -0.8827124684649698, + "timestamp": 0.7394955198585027 + }, + { + "x": 7.723740293307972, + "y": 5.8911496882585785, + "heading": 3.0617167669372236, + "angularVelocity": -0.409901354261302, + "velocityX": 1.710071497958437, + "velocityY": -0.5881397980514556, + "timestamp": 0.8216616887316697 + }, + { + "x": 7.793976783752441, + "y": 5.8669962882995605, + "heading": 3.044340108418373, + "angularVelocity": -0.21148191229012508, + "velocityX": 0.8548103362697581, + "velocityY": -0.29395796701075205, + "timestamp": 0.9038278576048366 + }, + { + "x": 7.793976783752441, + "y": 5.8669962882995605, + "heading": 3.044340108418373, + "angularVelocity": 0, + "velocityX": -4.398446739190714e-37, + "velocityY": 0, + "timestamp": 0.9859940264780036 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA123.7.traj b/src/main/deploy/choreo/CBA123.7.traj new file mode 100644 index 0000000..b2a8247 --- /dev/null +++ b/src/main/deploy/choreo/CBA123.7.traj @@ -0,0 +1,121 @@ +{ + "samples": [ + { + "x": 7.793976783752441, + "y": 5.8669962882995605, + "heading": 3.044340108418373, + "angularVelocity": 0, + "velocityX": -4.398446739190714e-37, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 7.724452143365902, + "y": 5.891359995748724, + "heading": 3.064192130892063, + "angularVelocity": 0.2424150971155188, + "velocityX": -0.8489725655691124, + "velocityY": 0.29750774840248856, + "timestamp": 0.08189268205614297 + }, + { + "x": 7.585289292630555, + "y": 5.940133616180372, + "heading": 3.100973644408784, + "angularVelocity": 0.44914286103738393, + "velocityX": -1.699331945679124, + "velocityY": 0.5955797173453272, + "timestamp": 0.16378536411228595 + }, + { + "x": 7.3763326141186685, + "y": 6.0133792242054085, + "heading": 3.14960943502028, + "angularVelocity": 0.5938966631738083, + "velocityX": -2.5515915862742524, + "velocityY": 0.8944096857741486, + "timestamp": 0.24567804616842892 + }, + { + "x": 7.097432154332273, + "y": 6.111156462191388, + "heading": 3.198862048472999, + "angularVelocity": 0.6014287506050079, + "velocityX": -3.405682324523109, + "velocityY": 1.1939679533141472, + "timestamp": 0.3275707282245719 + }, + { + "x": 6.7509713925352335, + "y": 6.232193154508266, + "heading": 3.199747773581081, + "angularVelocity": 0.010815680789089665, + "velocityX": -4.230668151270443, + "velocityY": 1.4779915528215268, + "timestamp": 0.40946341028071487 + }, + { + "x": 6.404350241550633, + "y": 6.353697070710262, + "heading": 3.199747821125258, + "angularVelocity": 5.805668609663509e-7, + "velocityX": -4.232626680207759, + "velocityY": 1.4836968719463455, + "timestamp": 0.49135609233685784 + }, + { + "x": 6.057887361130367, + "y": 6.47472492007914, + "heading": 3.2006358854014487, + "angularVelocity": 0.01084424461249623, + "velocityX": -4.230694021997506, + "velocityY": 1.4778835706700362, + "timestamp": 0.5732487743930008 + }, + { + "x": 5.77898688869625, + "y": 6.572504725430993, + "heading": 3.2497074160560264, + "angularVelocity": 0.5992175298512812, + "velocityX": -3.4056824789657414, + "velocityY": 1.193999303684035, + "timestamp": 0.6551414564491438 + }, + { + "x": 5.57003008056901, + "y": 6.645752368291244, + "heading": 3.298210403791235, + "angularVelocity": 0.5922749935330796, + "velocityX": -2.551593169020725, + "velocityY": 0.8944345333571375, + "timestamp": 0.7370341385052868 + }, + { + "x": 5.430865596155991, + "y": 6.694528008588771, + "heading": 3.3348189416352247, + "angularVelocity": 0.4470306372294954, + "velocityX": -1.6993518946859398, + "velocityY": 0.5956043821362281, + "timestamp": 0.8189268205614297 + }, + { + "x": 5.3613386154174805, + "y": 6.718893051147461, + "heading": 3.354531552893077, + "angularVelocity": 0.24071273235791338, + "velocityX": -0.8490011438487919, + "velocityY": 0.2975240515628335, + "timestamp": 0.9008195026175727 + }, + { + "x": 5.3613386154174805, + "y": 6.718893051147461, + "heading": 3.354531552893077, + "angularVelocity": -3.752319250871441e-31, + "velocityX": -2.3322961891501556e-32, + "velocityY": -6.418789255803219e-32, + "timestamp": 0.9827121846737157 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA123.8.traj b/src/main/deploy/choreo/CBA123.8.traj new file mode 100644 index 0000000..5f1ab27 --- /dev/null +++ b/src/main/deploy/choreo/CBA123.8.traj @@ -0,0 +1,202 @@ +{ + "samples": [ + { + "x": 5.3613386154174805, + "y": 6.718893051147461, + "heading": 3.354531552893077, + "angularVelocity": -3.752319250871441e-31, + "velocityX": -2.3322961891501556e-32, + "velocityY": -6.418789255803219e-32, + "timestamp": 0 + }, + { + "x": 5.381762554826202, + "y": 6.705904304585873, + "heading": 3.3390037774751677, + "angularVelocity": -0.32613159738048514, + "velocityX": 0.4289662752646431, + "velocityY": -0.2728040913844985, + "timestamp": 0.047611993264769836 + }, + { + "x": 5.422670945546834, + "y": 6.679911372646232, + "heading": 3.3084755716638385, + "angularVelocity": -0.6411873084489506, + "velocityX": 0.859203488774904, + "velocityY": -0.5459324459516255, + "timestamp": 0.09522398652953967 + }, + { + "x": 5.4841375959919265, + "y": 6.640889729584984, + "heading": 3.2636438109088237, + "angularVelocity": -0.9416064667929949, + "velocityX": 1.290990908598487, + "velocityY": -0.8195759174435114, + "timestamp": 0.1428359797943095 + }, + { + "x": 5.566254301369102, + "y": 6.588794024470335, + "heading": 3.2055157827984404, + "angularVelocity": -1.2208694516763405, + "velocityX": 1.7247063133965845, + "velocityY": -1.09417189960937, + "timestamp": 0.19044797305907935 + }, + { + "x": 5.669144917681803, + "y": 6.52354420886013, + "heading": 3.135745985842297, + "angularVelocity": -1.4653828199996848, + "velocityX": 2.1610230796364758, + "velocityY": -1.3704491481257148, + "timestamp": 0.23805996632384918 + }, + { + "x": 5.792991373719124, + "y": 6.445019863790107, + "heading": 3.0574508369011313, + "angularVelocity": -1.6444417377309697, + "velocityX": 2.601160916506215, + "velocityY": -1.6492555695668922, + "timestamp": 0.285671959588619 + }, + { + "x": 5.938012207911987, + "y": 6.3531182794458365, + "heading": 2.9775771179710966, + "angularVelocity": -1.6775966191089577, + "velocityX": 3.0458887403926016, + "velocityY": -1.9302192166856003, + "timestamp": 0.33328395285338885 + }, + { + "x": 6.10288509502327, + "y": 6.249471412942973, + "heading": 2.9223714814313397, + "angularVelocity": -1.159490135872572, + "velocityX": 3.462843619975461, + "velocityY": -2.176906686653277, + "timestamp": 0.3808959461181587 + }, + { + "x": 6.283648559689557, + "y": 6.138505943088611, + "heading": 2.918071321671212, + "angularVelocity": -0.0903167346134664, + "velocityX": 3.796595191068823, + "velocityY": -2.3306201283654526, + "timestamp": 0.4285079393829285 + }, + { + "x": 6.4636502982527535, + "y": 6.023608344983735, + "heading": 2.9180712113581024, + "angularVelocity": -0.000002316918529011022, + "velocityX": 3.780596572845407, + "velocityY": -2.4132070561726313, + "timestamp": 0.47611993264769836 + }, + { + "x": 6.6436519645721965, + "y": 5.908710633699827, + "heading": 2.9180711010451206, + "angularVelocity": -0.000002316915849069208, + "velocityX": 3.780595055501843, + "velocityY": -2.4132094332844187, + "timestamp": 0.5237319259124673 + }, + { + "x": 6.8228001378837595, + "y": 5.79248664277841, + "heading": 2.918070991032562, + "angularVelocity": -0.000002310606033986578, + "velocityX": 3.762669046769599, + "velocityY": -2.4410654322719396, + "timestamp": 0.5713439191772371 + }, + { + "x": 6.987106800079346, + "y": 5.656085014343262, + "heading": 2.9180708819744035, + "angularVelocity": -0.000002290560660276634, + "velocityX": 3.4509511349701865, + "velocityY": -2.864858601424802, + "timestamp": 0.618955912442007 + }, + { + "x": 7.197673412932475, + "y": 5.404866525743537, + "heading": 2.91807080868153, + "angularVelocity": -0.0000010028513094998122, + "velocityX": 2.8811396466166426, + "velocityY": -3.4373708996697796, + "timestamp": 0.6920403993745134 + }, + { + "x": 7.389061641704391, + "y": 5.138747052991589, + "heading": 2.9180707373970454, + "angularVelocity": -9.75370940633925e-7, + "velocityX": 2.618725762533503, + "velocityY": -3.6412580004544863, + "timestamp": 0.7651248863070199 + }, + { + "x": 7.559618144405977, + "y": 4.904145918307005, + "heading": 2.815675073371891, + "angularVelocity": -1.4010588063603617, + "velocityX": 2.333689540146932, + "velocityY": -3.2099990645263112, + "timestamp": 0.8382093732395264 + }, + { + "x": 7.695410928944271, + "y": 4.716969709749763, + "heading": 2.700604666277338, + "angularVelocity": -1.574484708373056, + "velocityX": 1.8580247359955884, + "velocityY": -2.561093556423498, + "timestamp": 0.9112938601720328 + }, + { + "x": 7.796892129440441, + "y": 4.57711072358559, + "heading": 2.60568153422841, + "angularVelocity": -1.2988136885542398, + "velocityX": 1.3885463900126565, + "velocityY": -1.9136617363602635, + "timestamp": 0.9843783471045393 + }, + { + "x": 7.864395197078724, + "y": 4.484114526017619, + "heading": 2.5390881205921088, + "angularVelocity": -0.9111839794097845, + "velocityX": 0.9236305879871393, + "velocityY": -1.2724478404549935, + "timestamp": 1.0574628340370458 + }, + { + "x": 7.898097515106201, + "y": 4.4377031326293945, + "heading": 2.504728002453472, + "angularVelocity": -0.47014242804166056, + "velocityX": 0.4611418844407282, + "velocityY": -0.6350375481340379, + "timestamp": 1.1305473209695522 + }, + { + "x": 7.898097515106201, + "y": 4.4377031326293945, + "heading": 2.504728002453472, + "angularVelocity": -1.3198901080890972e-29, + "velocityX": -5.557969546153989e-30, + "velocityY": 1.6398907339851722e-31, + "timestamp": 1.2036318079020587 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA123.9.traj b/src/main/deploy/choreo/CBA123.9.traj new file mode 100644 index 0000000..4b1ea63 --- /dev/null +++ b/src/main/deploy/choreo/CBA123.9.traj @@ -0,0 +1,202 @@ +{ + "samples": [ + { + "x": 7.898097515106201, + "y": 4.4377031326293945, + "heading": 2.504728002453472, + "angularVelocity": -1.3198901080890972e-29, + "velocityX": -5.557969546153989e-30, + "velocityY": 1.6398907339851722e-31, + "timestamp": 0 + }, + { + "x": 7.872617208829989, + "y": 4.473100868040156, + "heading": 2.533632642169352, + "angularVelocity": 0.4521476679265757, + "velocityX": -0.3985817216228667, + "velocityY": 0.5537174541243508, + "timestamp": 0.06392743293010383 + }, + { + "x": 7.821576408126655, + "y": 4.544074102448603, + "heading": 2.589474507633189, + "angularVelocity": 0.8735195972109739, + "velocityX": -0.7984178053753025, + "velocityY": 1.1102156172303905, + "timestamp": 0.12785486586020767 + }, + { + "x": 7.744874668782088, + "y": 4.650876974268225, + "heading": 2.669362107193564, + "angularVelocity": 1.2496606839777096, + "velocityX": -1.1998251115201943, + "velocityY": 1.6706892006190956, + "timestamp": 0.1917822987903115 + }, + { + "x": 7.642361003877687, + "y": 4.793861579473503, + "heading": 2.7687102276641724, + "angularVelocity": 1.554076488246139, + "velocityX": -1.6035942662750133, + "velocityY": 2.2366705286229, + "timestamp": 0.25570973172041533 + }, + { + "x": 7.513746244350758, + "y": 4.973514163207665, + "heading": 2.877208848057101, + "angularVelocity": 1.6972153490279336, + "velocityX": -2.0118868165337256, + "velocityY": 2.810258061364581, + "timestamp": 0.31963716465051917 + }, + { + "x": 7.36085249101928, + "y": 5.1866403519008655, + "heading": 2.9426744108217315, + "angularVelocity": 1.0240605599822166, + "velocityX": -2.3916767234910896, + "velocityY": 3.3338768494932727, + "timestamp": 0.383564597580623 + }, + { + "x": 7.194539581784471, + "y": 5.420200579055916, + "heading": 2.9426745032862103, + "angularVelocity": 0.0000014463974957387205, + "velocityX": -2.601589045764413, + "velocityY": 3.6535211324756753, + "timestamp": 0.44749203051072683 + }, + { + "x": 6.999818325042725, + "y": 5.630661964416504, + "heading": 2.9426746005935125, + "angularVelocity": 0.0000015221525015916737, + "velocityX": -3.045973345350628, + "velocityY": 3.292192032654593, + "timestamp": 0.5114194634408307 + }, + { + "x": 6.821050073226294, + "y": 5.780060077850853, + "heading": 2.9426746891736677, + "angularVelocity": 0.0000017053007611025479, + "velocityX": -3.4415568043145366, + "velocityY": 2.8761376173765214, + "timestamp": 0.5633634707552808 + }, + { + "x": 6.624664762679586, + "y": 5.905402386843963, + "heading": 2.942674778221278, + "angularVelocity": 0.0000017142999717232747, + "velocityX": -3.7807115911927003, + "velocityY": 2.4130273244857414, + "timestamp": 0.615307478069731 + }, + { + "x": 6.4270459836651215, + "y": 6.028790809329787, + "heading": 2.942674867501059, + "angularVelocity": 0.0000017187696207326967, + "velocityX": -3.804457708049898, + "velocityY": 2.375412080528933, + "timestamp": 0.6672514853841811 + }, + { + "x": 6.22942713407172, + "y": 6.152179118776215, + "heading": 2.9426749567811457, + "angularVelocity": 0.0000017187754837333519, + "velocityX": -3.8044590668002334, + "velocityY": 2.3754099043511037, + "timestamp": 0.7191954926986313 + }, + { + "x": 6.035443701378057, + "y": 6.268251221439266, + "heading": 2.962950698672798, + "angularVelocity": 0.3903384228503762, + "velocityX": -3.734471842331197, + "velocityY": 2.2345619574630913, + "timestamp": 0.7711395000130814 + }, + { + "x": 5.861314358992964, + "y": 6.376637011473068, + "heading": 3.0412855386044892, + "angularVelocity": 1.5080630852656163, + "velocityX": -3.3522508444713, + "velocityY": 2.086588918288052, + "timestamp": 0.8230835073275315 + }, + { + "x": 5.712466276366555, + "y": 6.469340902724069, + "heading": 3.1246025769956893, + "angularVelocity": 1.6039778734595374, + "velocityX": -2.8655486998786075, + "velocityY": 1.7846888610231364, + "timestamp": 0.8750275146419817 + }, + { + "x": 5.588796508227056, + "y": 6.546382737169229, + "heading": 3.2002505066448665, + "angularVelocity": 1.4563360349006294, + "velocityX": -2.3808284060727742, + "velocityY": 1.4831707915559873, + "timestamp": 0.9269715219564318 + }, + { + "x": 5.490084330824383, + "y": 6.607894755873726, + "heading": 3.2637725507230906, + "angularVelocity": 1.222894562094215, + "velocityX": -1.9003573753003593, + "velocityY": 1.1841985608104344, + "timestamp": 0.978915529270882 + }, + { + "x": 5.416180600283462, + "y": 6.6539627808027575, + "heading": 3.3129789015972246, + "angularVelocity": 0.9472960100335697, + "velocityX": -1.4227575876757712, + "velocityY": 0.8868785315340437, + "timestamp": 1.0308595365853321 + }, + { + "x": 5.366980486180744, + "y": 6.684639660698046, + "heading": 3.3465688664099242, + "angularVelocity": 0.6466571708524164, + "velocityX": -0.9471759428354473, + "velocityY": 0.5905759197511072, + "timestamp": 1.0828035438997823 + }, + { + "x": 5.342407703399658, + "y": 6.699962139129639, + "heading": 3.3636740355255, + "angularVelocity": 0.32930014451959316, + "velocityX": -0.4730629008334152, + "velocityY": 0.294980676766733, + "timestamp": 1.1347475512142324 + }, + { + "x": 5.342407703399658, + "y": 6.699962139129639, + "heading": 3.3636740355255, + "angularVelocity": -1.0197070578567015e-31, + "velocityX": 4.5771839799212747e-32, + "velocityY": -2.780302289877669e-33, + "timestamp": 1.1866915585286826 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA123.traj b/src/main/deploy/choreo/CBA123.traj new file mode 100644 index 0000000..a12f956 --- /dev/null +++ b/src/main/deploy/choreo/CBA123.traj @@ -0,0 +1,1750 @@ +{ + "samples": [ + { + "x": 1.3479586839675903, + "y": 4.920444488525391, + "heading": 2.622765580104145, + "angularVelocity": 1.5385294760575736e-37, + "velocityX": -4.525960803191592e-37, + "velocityY": 5.824643031906921e-39, + "timestamp": 0 + }, + { + "x": 1.4254576260477565, + "y": 4.880807633074535, + "heading": 2.6267034277668575, + "angularVelocity": 0.04436815590241169, + "velocityX": 0.8731889699653917, + "velocityY": -0.44659274120153025, + "timestamp": 0.08875391781831346 + }, + { + "x": 1.5804555064265071, + "y": 4.801533924729678, + "heading": 2.6345791298116996, + "angularVelocity": 0.08873638751321193, + "velocityX": 1.746377897323294, + "velocityY": -0.893185453594704, + "timestamp": 0.17750783563662692 + }, + { + "x": 1.8129523161590906, + "y": 4.68262336913622, + "heading": 2.646392694564592, + "angularVelocity": 0.13310471293307716, + "velocityX": 2.619566723899709, + "velocityY": -1.3397781023805322, + "timestamp": 0.2662617534549404 + }, + { + "x": 2.1229480096662936, + "y": 4.524075992400786, + "heading": 2.6621441294728503, + "angularVelocity": 0.17747312226264264, + "velocityX": 3.4927550369301996, + "velocityY": -1.7863704570201997, + "timestamp": 0.35501567127325384 + }, + { + "x": 2.355444821301696, + "y": 4.405165440711646, + "heading": 2.6739577129781713, + "angularVelocity": 0.13310492421872042, + "velocityX": 2.6195667453389717, + "velocityY": -1.3397780583901595, + "timestamp": 0.4437695890915673 + }, + { + "x": 2.510442703464613, + "y": 4.325891736084506, + "heading": 2.6818334361924214, + "angularVelocity": 0.0887366260312266, + "velocityX": 1.7463779174256908, + "velocityY": -0.8931854117067864, + "timestamp": 0.5325235069098807 + }, + { + "x": 2.5879416465759277, + "y": 4.2862548828125, + "heading": 2.6857712983829183, + "angularVelocity": 0.04436831958852742, + "velocityX": 0.873188981583452, + "velocityY": -0.4465927166521856, + "timestamp": 0.6212774247281941 + }, + { + "x": 2.5879416465759277, + "y": 4.2862548828125, + "heading": 2.6857712983829183, + "angularVelocity": 5.3847700504342055e-37, + "velocityX": 8.698817388407438e-39, + "velocityY": -2.770926604636497e-38, + "timestamp": 0.7100313425465076 + }, + { + "x": 2.557860644240392, + "y": 4.318524288125325, + "heading": 2.68919118628525, + "angularVelocity": 0.05411676522637325, + "velocityX": -0.47600581880369225, + "velocityY": 0.510635401270978, + "timestamp": 0.7732259530895803 + }, + { + "x": 2.498282487303618, + "y": 4.383591603835094, + "heading": 2.696354861077149, + "angularVelocity": 0.11335895150451837, + "velocityX": -0.9427727526885523, + "velocityY": 1.0296339379355688, + "timestamp": 0.836420563632653 + }, + { + "x": 2.4102046272873507, + "y": 4.482316225754566, + "heading": 2.707800615933889, + "angularVelocity": 0.18111916124458857, + "velocityX": -1.3937558798029874, + "velocityY": 1.5622316693000622, + "timestamp": 0.8996151741757258 + }, + { + "x": 2.295704109453543, + "y": 4.616327372903078, + "heading": 2.7245932961796777, + "angularVelocity": 0.26572962633170844, + "velocityX": -1.8118715638854261, + "velocityY": 2.120610381120576, + "timestamp": 0.9628097847187985 + }, + { + "x": 2.1614517035876886, + "y": 4.789659527185801, + "heading": 2.7497269864105105, + "angularVelocity": 0.39771888796912597, + "velocityX": -2.124428091448555, + "velocityY": 2.742831276166849, + "timestamp": 1.0260043952618711 + }, + { + "x": 2.061575493662025, + "y": 4.989709507594673, + "heading": 2.792410295358657, + "angularVelocity": 0.6754264102799412, + "velocityX": -1.5804545524904996, + "velocityY": 3.165617743185872, + "timestamp": 1.0891990058049439 + }, + { + "x": 2.001798110386581, + "y": 5.171379149434758, + "heading": 2.8388621903309694, + "angularVelocity": 0.7350610213928208, + "velocityX": -0.9459253370143008, + "velocityY": 2.8747647984357916, + "timestamp": 1.1523936163480166 + }, + { + "x": 1.978911997185236, + "y": 5.328819797631775, + "heading": 2.886301638585647, + "angularVelocity": 0.7506881970946474, + "velocityX": -0.36215292735677707, + "velocityY": 2.4913619507110902, + "timestamp": 1.2155882268910894 + }, + { + "x": 1.9915421022340005, + "y": 5.4600540508319835, + "heading": 2.933718928886097, + "angularVelocity": 0.7503375666526398, + "velocityX": 0.1998604776613994, + "velocityY": 2.0766684385333893, + "timestamp": 1.278782837434162 + }, + { + "x": 2.0389416217803955, + "y": 5.5640997886657715, + "heading": 2.9805967354734015, + "angularVelocity": 0.7418007039596033, + "velocityX": 0.750056359855052, + "velocityY": 1.6464337217945526, + "timestamp": 1.3419774479772348 + }, + { + "x": 2.163265986983934, + "y": 5.6534822842831955, + "heading": 3.041629416668671, + "angularVelocity": 0.7237838294156185, + "velocityX": 1.4743570718904808, + "velocityY": 1.059983007361601, + "timestamp": 1.4263019088380797 + }, + { + "x": 2.3447602212049414, + "y": 5.688965385139213, + "heading": 3.0987012296523844, + "angularVelocity": 0.6768120709113685, + "velocityX": 2.152331984909044, + "velocityY": 0.42079250188830075, + "timestamp": 1.5106263696989246 + }, + { + "x": 2.566977292741073, + "y": 5.65742824832104, + "heading": 3.1433966419125996, + "angularVelocity": 0.530040889724432, + "velocityX": 2.635262286501201, + "velocityY": -0.3739974913117593, + "timestamp": 1.5949508305597695 + }, + { + "x": 2.7144228017968293, + "y": 5.604225780565181, + "heading": 3.1618565972535455, + "angularVelocity": 0.21891578259135683, + "velocityX": 1.748549680016051, + "velocityY": -0.6309256793666975, + "timestamp": 1.6792752914206144 + }, + { + "x": 2.786717653274536, + "y": 5.573565483093262, + "heading": 3.1693638048208683, + "angularVelocity": 0.08902763789633576, + "velocityX": 0.8573414017672781, + "velocityY": -0.3635990928245075, + "timestamp": 1.7635997522814593 + }, + { + "x": 2.786717653274536, + "y": 5.573565483093262, + "heading": 3.1693638048208683, + "angularVelocity": 2.931866046366828e-38, + "velocityX": -1.1847240425690822e-39, + "velocityY": -7.043293922543416e-40, + "timestamp": 1.8479242131423042 + }, + { + "x": 2.7838634682756775, + "y": 5.575306183326131, + "heading": 3.1696268104013123, + "angularVelocity": 0.015118256006559196, + "velocityX": -0.16406609863566005, + "velocityY": 0.1000600508429731, + "timestamp": 1.865320768692739 + }, + { + "x": 2.7781635414165, + "y": 5.578801206347808, + "heading": 3.170158474700968, + "angularVelocity": 0.030561469373312153, + "velocityX": -0.32764686334906284, + "velocityY": 0.20090316221186844, + "timestamp": 1.882717324243174 + }, + { + "x": 2.7696273846815345, + "y": 5.5840657232897595, + "heading": 3.170965105092508, + "angularVelocity": 0.0463672471944818, + "velocityX": -0.4906808540471063, + "velocityY": 0.30261835032165785, + "timestamp": 1.900113879793609 + }, + { + "x": 2.758265793234076, + "y": 5.5911167319231625, + "heading": 3.172053783081744, + "angularVelocity": 0.06258008868939276, + "velocityX": -0.6530943102224854, + "velocityY": 0.40531061525144174, + "timestamp": 1.917510435344044 + }, + { + "x": 2.744091120487868, + "y": 5.599973404947456, + "heading": 3.1734325151970073, + "angularVelocity": 0.07925316659762915, + "velocityX": -0.8147976595202109, + "velocityY": 0.5091049776271751, + "timestamp": 1.934906990894479 + }, + { + "x": 2.7271176368126726, + "y": 5.610657531920939, + "heading": 3.1751104254879197, + "angularVelocity": 0.09645071899710177, + "velocityX": -0.9756807102410053, + "velocityY": 0.6141518614134843, + "timestamp": 1.9523035464449139 + }, + { + "x": 2.7073620058359746, + "y": 5.6231940881572715, + "heading": 3.1770980048968873, + "angularVelocity": 0.11425131849838457, + "velocityX": -1.1356058916044396, + "velocityY": 0.7206343922501016, + "timestamp": 1.9697001019953488 + }, + { + "x": 2.684843930071724, + "y": 5.637611978983044, + "heading": 3.1794074399377976, + "angularVelocity": 0.1327524310323994, + "velocityX": -1.2943985203834083, + "velocityY": 0.8287784776689098, + "timestamp": 1.9870966575457838 + }, + { + "x": 2.6595870470813243, + "y": 5.653945031227922, + "heading": 3.182053054495388, + "angularVelocity": 0.152076918325605, + "velocityX": -1.4518323996481046, + "velocityY": 0.9388670186765858, + "timestamp": 2.0044932130962185 + }, + { + "x": 2.6316202081179507, + "y": 5.672233341323654, + "heading": 3.1850519171745817, + "angularVelocity": 0.17238255414985487, + "velocityX": -1.6076078326134229, + "velocityY": 1.0512604085740558, + "timestamp": 2.0218897686466533 + }, + { + "x": 2.600979362390935, + "y": 5.69252515090379, + "heading": 3.1884246981498414, + "angularVelocity": 0.19387636624280877, + "velocityX": -1.76131680999629, + "velocityY": 1.16642685509252, + "timestamp": 2.039286324197088 + }, + { + "x": 2.56771044224616, + "y": 5.714879524325921, + "heading": 3.1921969149485707, + "angularVelocity": 0.21683699326533024, + "velocityX": -1.9123854747179205, + "velocityY": 1.2849884770190956, + "timestamp": 2.0566828797475227 + }, + { + "x": 2.531873988844058, + "y": 5.739370280413082, + "heading": 3.19640080864036, + "angularVelocity": 0.24165092219559348, + "velocityX": -2.0599740735002126, + "velocityY": 1.4077934000301997, + "timestamp": 2.0740794352979575 + }, + { + "x": 2.4935529941684056, + "y": 5.766091937198378, + "heading": 3.2010782889997142, + "angularVelocity": 0.26887393575087964, + "velocityX": -2.202792073669703, + "velocityY": 1.5360314694380732, + "timestamp": 2.091475990848392 + }, + { + "x": 2.4528671363352914, + "y": 5.79516892673793, + "heading": 3.2062857888830454, + "angularVelocity": 0.29934085907027813, + "velocityX": -2.338730659363013, + "velocityY": 1.6714222223620452, + "timestamp": 2.108872546398827 + }, + { + "x": 2.4100008652071403, + "y": 5.826769900846777, + "heading": 3.212102729976151, + "angularVelocity": 0.334373150836745, + "velocityX": -2.464066579379806, + "velocityY": 1.8165075274373275, + "timestamp": 2.1262691019492617 + }, + { + "x": 2.365264409264796, + "y": 5.861127730914518, + "heading": 3.2186471517684527, + "angularVelocity": 0.37619066448691335, + "velocityX": -2.571569746243583, + "velocityY": 1.9749788955711667, + "timestamp": 2.1436656574996964 + }, + { + "x": 2.3192374036487657, + "y": 5.898548491541854, + "heading": 3.2261037658581118, + "angularVelocity": 0.42862588907565374, + "velocityX": -2.645753952993267, + "velocityY": 2.151044240846847, + "timestamp": 2.161062213050131 + }, + { + "x": 2.2730712823306805, + "y": 5.939277233096012, + "heading": 3.234722066355753, + "angularVelocity": 0.49540269466881787, + "velocityX": -2.6537506912930886, + "velocityY": 2.3411957290096908, + "timestamp": 2.178458768600566 + }, + { + "x": 2.2285555727829873, + "y": 5.9828746912671305, + "heading": 3.2445111987990307, + "angularVelocity": 0.5627052099421548, + "velocityX": -2.5588806599465155, + "velocityY": 2.5060971434674393, + "timestamp": 2.1958553241510006 + }, + { + "x": 2.18692929945952, + "y": 6.028114885994637, + "heading": 3.255210019353813, + "angularVelocity": 0.614996487308366, + "velocityX": -2.39278822769182, + "velocityY": 2.6005259832240544, + "timestamp": 2.2132518797014353 + }, + { + "x": 2.1485716857950186, + "y": 6.074017979779568, + "heading": 3.2665254860070387, + "angularVelocity": 0.6504429351213081, + "velocityX": -2.2048970299492767, + "velocityY": 2.638631173386685, + "timestamp": 2.23064843525187 + }, + { + "x": 2.113553488124031, + "y": 6.12000095046398, + "heading": 3.2782612178988115, + "angularVelocity": 0.6746008919840083, + "velocityX": -2.0129385710559187, + "velocityY": 2.6432227087196347, + "timestamp": 2.248044990802305 + }, + { + "x": 2.0818666977257543, + "y": 6.165710242696975, + "heading": 3.290293518998621, + "angularVelocity": 0.6916484740284482, + "velocityX": -1.8214404746050021, + "velocityY": 2.6274909478763533, + "timestamp": 2.2654415463527395 + }, + { + "x": 2.053485472561832, + "y": 6.210915958519219, + "heading": 3.30254009499683, + "angularVelocity": 0.7039655616138741, + "velocityX": -1.6314278468310237, + "velocityY": 2.598544044606179, + "timestamp": 2.2828381019031743 + }, + { + "x": 2.028382147011791, + "y": 6.255458802658021, + "heading": 3.31494295383331, + "angularVelocity": 0.7129491122838909, + "velocityX": -1.443005512054579, + "velocityY": 2.560440427972457, + "timestamp": 2.300234657453609 + }, + { + "x": 2.006531387313589, + "y": 6.299222662006888, + "heading": 3.3274592724833716, + "angularVelocity": 0.719471082294136, + "velocityX": -1.2560394288888732, + "velocityY": 2.5156623230380273, + "timestamp": 2.3176312130040437 + }, + { + "x": 1.9879109996275754, + "y": 6.34211946303162, + "heading": 3.3400562374477403, + "angularVelocity": 0.7241068456251942, + "velocityX": -1.070349106294682, + "velocityY": 2.4658215185396353, + "timestamp": 2.3350277685544785 + }, + { + "x": 1.9725017779583829, + "y": 6.384080257529668, + "heading": 3.3527079517381098, + "angularVelocity": 0.7272539816109163, + "velocityX": -0.8857627950842983, + "velocityY": 2.412017389097379, + "timestamp": 2.352424324104913 + }, + { + "x": 1.9602871128958277, + "y": 6.4250496778901445, + "heading": 3.365393481996193, + "angularVelocity": 0.7291978128260018, + "velocityX": -0.7021312366774745, + "velocityY": 2.355030583019786, + "timestamp": 2.369820879655348 + }, + { + "x": 1.9512525840043362, + "y": 6.464982324825169, + "heading": 3.3780955702982447, + "angularVelocity": 0.7301496129637277, + "velocityX": -0.5193286030271496, + "velocityY": 2.295434105863935, + "timestamp": 2.3872174352057827 + }, + { + "x": 1.9453855996438743, + "y": 6.503840320779697, + "heading": 3.3907997524885256, + "angularVelocity": 0.730269975194225, + "velocityX": -0.33724977013138463, + "velocityY": 2.2336603267166004, + "timestamp": 2.4046139907562174 + }, + { + "x": 1.9426750959440404, + "y": 6.541591597502305, + "heading": 3.403493735583254, + "angularVelocity": 0.7296837042210303, + "velocityX": -0.1558069177530964, + "velocityY": 2.1700431797065427, + "timestamp": 2.422010546306652 + }, + { + "x": 1.943111290335544, + "y": 6.5782086634174055, + "heading": 3.4161669462041386, + "angularVelocity": 0.7284896475134609, + "velocityX": 0.025073606682601935, + "velocityY": 2.104845744259196, + "timestamp": 2.439407101857087 + }, + { + "x": 1.946685480968561, + "y": 6.6136676947141995, + "heading": 3.4288101954137264, + "angularVelocity": 0.7267673863905522, + "velocityX": 0.20545392578748464, + "velocityY": 2.0382788531898433, + "timestamp": 2.4568036574075216 + }, + { + "x": 1.953389883365602, + "y": 6.647947850963729, + "heading": 3.441415424900957, + "angularVelocity": 0.7245819122461787, + "velocityX": 0.3853867725484022, + "velocityY": 1.9705139991733958, + "timestamp": 2.4742002129579563 + }, + { + "x": 1.963217496871948, + "y": 6.681030750274658, + "heading": 3.453975511355717, + "angularVelocity": 0.7219869714062964, + "velocityX": 0.5649172031701521, + "velocityY": 1.901692505451284, + "timestamp": 2.491596768508391 + }, + { + "x": 1.972254759310776, + "y": 6.704973117440469, + "heading": 3.4632918988879884, + "angularVelocity": 0.7197850864325751, + "velocityX": 0.698219852181241, + "velocityY": 1.8497898203727527, + "timestamp": 2.504540059093148 + }, + { + "x": 1.9830118282595994, + "y": 6.72822960606617, + "heading": 3.4725743160294034, + "angularVelocity": 0.7171605304408409, + "velocityX": 0.8310922851018642, + "velocityY": 1.7967987717968175, + "timestamp": 2.517483349677905 + }, + { + "x": 1.995482319152762, + "y": 6.750784474341597, + "heading": 3.4818166306621796, + "angularVelocity": 0.7140622063805339, + "velocityX": 0.9634714458043958, + "velocityY": 1.7425915093019795, + "timestamp": 2.5304266402626623 + }, + { + "x": 2.009658846146705, + "y": 6.772620021168416, + "heading": 3.4910119236181756, + "angularVelocity": 0.7104293066575377, + "velocityX": 1.095279975452167, + "velocityY": 1.687016658077186, + "timestamp": 2.5433699308474194 + }, + { + "x": 2.0255327787443296, + "y": 6.793716203810744, + "heading": 3.5001523295995915, + "angularVelocity": 0.7061887331943414, + "velocityX": 1.2264217119809064, + "velocityY": 1.6298933029574858, + "timestamp": 2.5563132214321764 + }, + { + "x": 2.0430939161796475, + "y": 6.814050151845941, + "heading": 3.5092288330791357, + "angularVelocity": 0.7012516191387282, + "velocityX": 1.3567753362501818, + "velocityY": 1.5710029765647726, + "timestamp": 2.5692565120169335 + }, + { + "x": 2.0623300431045437, + "y": 6.833595541010598, + "heading": 3.5182310024639936, + "angularVelocity": 0.6955085591185912, + "velocityX": 1.4861852014316905, + "velocityY": 1.510078834796003, + "timestamp": 2.5821998026016906 + }, + { + "x": 2.0832263096675794, + "y": 6.852321774889104, + "heading": 3.5271466380085204, + "angularVelocity": 0.6888229454592012, + "velocityX": 1.614447765519879, + "velocityY": 1.4467908107199958, + "timestamp": 2.5951430931864476 + }, + { + "x": 2.105764344470479, + "y": 6.87019289901877, + "heading": 3.5359612967387575, + "angularVelocity": 0.6810214661036674, + "velocityX": 1.7412909534335712, + "velocityY": 1.3807249410526374, + "timestamp": 2.6080863837712047 + }, + { + "x": 2.129920948302108, + "y": 6.887166137027593, + "heading": 3.544657638059313, + "angularVelocity": 0.6718802505134741, + "velocityX": 1.866341767840478, + "velocityY": 1.311354164358459, + "timestamp": 2.621029674355962 + }, + { + "x": 2.1556661064734146, + "y": 6.903189887018867, + "heading": 3.5532145015981276, + "angularVelocity": 0.6611041823391853, + "velocityX": 1.9890736441956522, + "velocityY": 1.2379966196651884, + "timestamp": 2.633972964940719 + }, + { + "x": 2.1829598496443197, + "y": 6.91820094556084, + "heading": 3.5616055752204656, + "angularVelocity": 0.6482952358513759, + "velocityX": 2.1087174851075265, + "velocityY": 1.1597559711477912, + "timestamp": 2.646916255525476 + }, + { + "x": 2.2117470850673566, + "y": 6.9321206491026155, + "heading": 3.5697974211323467, + "angularVelocity": 0.6329028818628413, + "velocityX": 2.2241048545211815, + "velocityY": 1.075437768365362, + "timestamp": 2.659859546110233 + }, + { + "x": 2.2419486962848367, + "y": 6.944849624778968, + "heading": 3.577746483191445, + "angularVelocity": 0.6141453757099231, + "velocityX": 2.333379678043173, + "velocityY": 0.9834420075017508, + "timestamp": 2.67280283669499 + }, + { + "x": 2.2734455518567573, + "y": 6.956261311165807, + "heading": 3.585394523284631, + "angularVelocity": 0.5908883867749319, + "velocityX": 2.433450393906261, + "velocityY": 0.8816680976225674, + "timestamp": 2.685746127279747 + }, + { + "x": 2.3060491115565114, + "y": 6.966196859192603, + "heading": 3.592662176763274, + "angularVelocity": 0.561499676689782, + "velocityX": 2.5189544719137884, + "velocityY": 0.7676214917477436, + "timestamp": 2.6986894178645042 + }, + { + "x": 2.3394501625904085, + "y": 6.9744733277760504, + "heading": 3.599444288228278, + "angularVelocity": 0.523986649344906, + "velocityX": 2.5805687367656036, + "velocityY": 0.6394408384213112, + "timestamp": 2.7116327084492613 + }, + { + "x": 2.3731543733238665, + "y": 6.9809397435508185, + "heading": 3.6056298261391637, + "angularVelocity": 0.477895313435202, + "velocityX": 2.6039908872284765, + "velocityY": 0.49959596691611147, + "timestamp": 2.7245759990340184 + }, + { + "x": 2.4064949438216043, + "y": 6.985610811483352, + "heading": 3.6111636065879185, + "angularVelocity": 0.4275404629539523, + "velocityX": 2.5758960041429493, + "velocityY": 0.3608872026742504, + "timestamp": 2.7375192896187754 + }, + { + "x": 2.438840596898612, + "y": 6.988743182156231, + "heading": 3.6160526389087924, + "angularVelocity": 0.37772715437845295, + "velocityX": 2.499028578953463, + "velocityY": 0.2420072895966135, + "timestamp": 2.7504625802035325 + }, + { + "x": 2.4697821805679805, + "y": 6.990692528960885, + "heading": 3.6203582923022264, + "angularVelocity": 0.3326552367219884, + "velocityX": 2.3905500279663556, + "velocityY": 0.15060674037166785, + "timestamp": 2.7634058707882896 + }, + { + "x": 2.4991030303812836, + "y": 6.991770372494304, + "heading": 3.6241647361521734, + "angularVelocity": 0.2940862545749717, + "velocityX": 2.2653319587704477, + "velocityY": 0.08327430542956869, + "timestamp": 2.7763491613730467 + }, + { + "x": 2.5266936990356643, + "y": 6.992212575691012, + "heading": 3.6275448907630823, + "angularVelocity": 0.26115110286480986, + "velocityX": 2.1316579793760706, + "velocityY": 0.03416466576350692, + "timestamp": 2.7892924519578037 + }, + { + "x": 2.552497875190756, + "y": 6.992192727058408, + "heading": 3.6305547138750143, + "angularVelocity": 0.23253925207214746, + "velocityX": 1.993633379867123, + "velocityY": -0.0015335074549528095, + "timestamp": 2.802235742542561 + }, + { + "x": 2.5764856506570815, + "y": 6.991840108609927, + "heading": 3.6332366293718588, + "angularVelocity": 0.2072050750373046, + "velocityX": 1.8532980704747994, + "velocityY": -0.027243338637266013, + "timestamp": 2.815179033127318 + }, + { + "x": 2.598640768592086, + "y": 6.991253252700003, + "heading": 3.635623341291647, + "angularVelocity": 0.1843976154409533, + "velocityX": 1.7117067557067622, + "velocityY": -0.04534054969106539, + "timestamp": 2.828122323712075 + }, + { + "x": 2.6189543568583464, + "y": 6.990509100455072, + "heading": 3.6377406220206474, + "angularVelocity": 0.16358133313438186, + "velocityX": 1.5694299786626271, + "velocityY": -0.05749328117587533, + "timestamp": 2.841065614296832 + }, + { + "x": 2.637421701817117, + "y": 6.989669089846847, + "heading": 3.6396092140952665, + "angularVelocity": 0.14436762138521886, + "velocityX": 1.426789025390412, + "velocityY": -0.064899308465973, + "timestamp": 2.854008904881589 + }, + { + "x": 2.6540405079363083, + "y": 6.988783254129454, + "heading": 3.641246127061458, + "angularVelocity": 0.12646806895606091, + "velocityX": 1.2839707190660168, + "velocityY": -0.06843976124870382, + "timestamp": 2.866952195466346 + }, + { + "x": 2.668809918789073, + "y": 6.987893040987183, + "heading": 3.642665537885756, + "angularVelocity": 0.10966383046128683, + "velocityX": 1.1410862451127886, + "velocityY": -0.06877796155789115, + "timestamp": 2.8798954860511032 + }, + { + "x": 2.6817299462844417, + "y": 6.987033297747524, + "heading": 3.6438794301898154, + "angularVelocity": 0.09378544784344418, + "velocityX": 0.9982026912525851, + "velocityY": -0.06642385365831636, + "timestamp": 2.8928387766358603 + }, + { + "x": 2.692801127848911, + "y": 6.986233701516961, + "heading": 3.6448980582213584, + "angularVelocity": 0.07869930948955245, + "velocityX": 0.8553606590202942, + "velocityY": -0.06177688937186473, + "timestamp": 2.9057820672206174 + }, + { + "x": 2.70202431549513, + "y": 6.985519811426032, + "heading": 3.6457302906268625, + "angularVelocity": 0.06429836370082181, + "velocityX": 0.7125844533754433, + "velocityY": -0.05515522395592016, + "timestamp": 2.9187253578053745 + }, + { + "x": 2.7094005435757, + "y": 6.9849138578690955, + "heading": 3.646383869984044, + "angularVelocity": 0.05049561028563871, + "velocityX": 0.5698881619220394, + "velocityY": -0.04681603591984091, + "timestamp": 2.9316686483901315 + }, + { + "x": 2.71493094473244, + "y": 6.984435344858166, + "heading": 3.6468656120769536, + "angularVelocity": 0.037219445067315414, + "velocityX": 0.42727937849538145, + "velocityY": -0.036969965851833674, + "timestamp": 2.9446119389748886 + }, + { + "x": 2.71861669603319, + "y": 6.984101517009836, + "heading": 3.6471815612498695, + "angularVelocity": 0.024410266527419688, + "velocityX": 0.2847615354545603, + "velocityY": -0.02579157488156991, + "timestamp": 2.9575552295596457 + }, + { + "x": 2.720458984375, + "y": 6.9839277267456055, + "heading": 3.6473371131923193, + "angularVelocity": 0.012017959531342692, + "velocityX": 0.14233539220539884, + "velocityY": -0.013427054201720174, + "timestamp": 2.9704985201444027 + }, + { + "x": 2.720458984375, + "y": 6.9839277267456055, + "heading": 3.6473371131923193, + "angularVelocity": 2.1725782853578066e-36, + "velocityX": 0, + "velocityY": -3.4267891550955994e-37, + "timestamp": 2.98344181072916 + }, + { + "x": 2.8037165851892607, + "y": 6.9904871579633525, + "heading": 3.6321201190303314, + "angularVelocity": -0.17480730059204416, + "velocityX": 0.9564330706301294, + "velocityY": 0.07535236278514325, + "timestamp": 3.070491917392042 + }, + { + "x": 2.97023156700119, + "y": 7.003606007121592, + "heading": 3.6016759055237015, + "angularVelocity": -0.3497320643676068, + "velocityX": 1.9128636160871058, + "velocityY": 0.15070457304600782, + "timestamp": 3.157542024054924 + }, + { + "x": 3.220003551699753, + "y": 7.023284160432856, + "heading": 3.5559874461834298, + "angularVelocity": -0.5248524222630633, + "velocityX": 2.8692898179418815, + "velocityY": 0.2260554761577808, + "timestamp": 3.2445921307178063 + }, + { + "x": 3.5530320286012063, + "y": 7.049521300592341, + "heading": 3.4950321381493312, + "angularVelocity": -0.7002324336046938, + "velocityX": 3.825710153247341, + "velocityY": 0.3014027341872559, + "timestamp": 3.3316422373806884 + }, + { + "x": 3.942259099478333, + "y": 7.080171032524024, + "heading": 3.495032118406403, + "angularVelocity": -2.267995872008394e-7, + "velocityX": 4.471299183865228, + "velocityY": 0.3520929853696892, + "timestamp": 3.4186923440435706 + }, + { + "x": 4.331486170555169, + "y": 7.1108207619204435, + "heading": 3.4950320986636445, + "angularVelocity": -2.267976380492203e-7, + "velocityX": 4.471299186159417, + "velocityY": 0.3520929562454923, + "timestamp": 3.5057424507064527 + }, + { + "x": 4.720713241632006, + "y": 7.141470491316854, + "heading": 3.495032078920886, + "angularVelocity": -2.2679763752591037e-7, + "velocityX": 4.471299186159426, + "velocityY": 0.3520929562453827, + "timestamp": 3.592792557369335 + }, + { + "x": 5.109940312708843, + "y": 7.172120220713263, + "heading": 3.4950320591781274, + "angularVelocity": -2.267976369220769e-7, + "velocityX": 4.471299186159426, + "velocityY": 0.35209295624538245, + "timestamp": 3.679842664032217 + }, + { + "x": 5.4991673837856805, + "y": 7.202769950109673, + "heading": 3.495032039435369, + "angularVelocity": -2.26797638537719e-7, + "velocityX": 4.471299186159426, + "velocityY": 0.3520929562453829, + "timestamp": 3.766892770695099 + }, + { + "x": 5.888394454862517, + "y": 7.233419679506083, + "heading": 3.4950320196926103, + "angularVelocity": -2.2679763777636123e-7, + "velocityX": 4.471299186159426, + "velocityY": 0.35209295624538245, + "timestamp": 3.8539428773579814 + }, + { + "x": 6.277621525939354, + "y": 7.264069408902492, + "heading": 3.4950319999498514, + "angularVelocity": -2.2679763839745266e-7, + "velocityX": 4.471299186159426, + "velocityY": 0.3520929562453826, + "timestamp": 3.9409929840208635 + }, + { + "x": 6.66684859701619, + "y": 7.29471913829891, + "heading": 3.495031980207093, + "angularVelocity": -2.267976392668664e-7, + "velocityX": 4.471299186159418, + "velocityY": 0.35209295624547304, + "timestamp": 4.028043090683746 + }, + { + "x": 7.056075667928158, + "y": 7.32536886978814, + "heading": 3.495031960464165, + "angularVelocity": -2.267995828480058e-7, + "velocityX": 4.471299184265469, + "velocityY": 0.35209298028694325, + "timestamp": 4.115093197346629 + }, + { + "x": 7.38909841721707, + "y": 7.3515996523388205, + "heading": 3.433802950980574, + "angularVelocity": -0.7033766164206079, + "velocityX": 3.8256443565153195, + "velocityY": 0.30132970028703093, + "timestamp": 4.202143304009511 + }, + { + "x": 7.638865368158441, + "y": 7.371272393933456, + "heading": 3.3878742662647525, + "angularVelocity": -0.5276120441033825, + "velocityX": 2.869231991968013, + "velocityY": 0.225993308323234, + "timestamp": 4.289193410672394 + }, + { + "x": 7.805376647095578, + "y": 7.3843874359578985, + "heading": 3.35725340940439, + "angularVelocity": -0.3517612790406731, + "velocityX": 1.9128210788067521, + "velocityY": 0.15066083807608674, + "timestamp": 4.3762435173352765 + }, + { + "x": 7.888632297515869, + "y": 7.390944957733154, + "heading": 3.3419432761879757, + "angularVelocity": -0.1758772482118357, + "velocityX": 0.9564106652127833, + "velocityY": 0.0753304278035119, + "timestamp": 4.463293623998159 + }, + { + "x": 7.888632297515869, + "y": 7.390944957733154, + "heading": 3.3419432761879757, + "angularVelocity": 3.9030345535831066e-35, + "velocityX": 8.804773966904886e-37, + "velocityY": 1.1323533960883966e-35, + "timestamp": 4.550343730661042 + }, + { + "x": 7.782881118230512, + "y": 7.362639799509245, + "heading": 3.3425923011938354, + "angularVelocity": 0.006521281545109612, + "velocityX": -1.0625680175965528, + "velocityY": -0.2844049216754242, + "timestamp": 4.649867881147482 + }, + { + "x": 7.57137876356566, + "y": 7.306029484106841, + "heading": 3.343890344728581, + "angularVelocity": 0.01304249801079575, + "velocityX": -2.1251359959477387, + "velocityY": -0.5688098328467204, + "timestamp": 4.7493920316339215 + }, + { + "x": 7.254125241333047, + "y": 7.221114013616789, + "heading": 3.3458373938423662, + "angularVelocity": 0.019563584358860877, + "velocityX": -3.187703895808074, + "velocityY": -0.8532147230096001, + "timestamp": 4.8489161821203615 + }, + { + "x": 6.8311205749680965, + "y": 7.107893394311742, + "heading": 3.348433409842623, + "angularVelocity": 0.026084281931252454, + "velocityX": -4.2502715601936885, + "velocityY": -1.1376195501460042, + "timestamp": 4.948440332606801 + }, + { + "x": 6.399919425946622, + "y": 6.992478920143246, + "heading": 3.348433410548539, + "angularVelocity": 7.092910467358784e-9, + "velocityX": -4.332628280813384, + "velocityY": -1.1596629923932005, + "timestamp": 5.047964483093241 + }, + { + "x": 5.976914759581955, + "y": 6.879258300837546, + "heading": 3.3510294266908165, + "angularVelocity": 0.026084283358251074, + "velocityX": -4.250271560190831, + "velocityY": -1.1376195501525759, + "timestamp": 5.147488633579681 + }, + { + "x": 5.659661237349591, + "y": 6.794342830346922, + "heading": 3.3529764759284917, + "angularVelocity": 0.019563585603683077, + "velocityX": -3.187703895805583, + "velocityY": -0.8532147230153324, + "timestamp": 5.247012784066121 + }, + { + "x": 5.44815888268492, + "y": 6.7377325149441, + "heading": 3.3542745195538526, + "angularVelocity": 0.013042498921281074, + "velocityX": -2.1251359959459153, + "velocityY": -0.568809832850927, + "timestamp": 5.346536934552561 + }, + { + "x": 5.342407703399658, + "y": 6.709427356719971, + "heading": 3.354923544607161, + "angularVelocity": 0.006521282021865445, + "velocityX": -1.0625680175955954, + "velocityY": -0.28440492167764353, + "timestamp": 5.446061085039001 + }, + { + "x": 5.342407703399658, + "y": 6.709427356719971, + "heading": 3.354923544607161, + "angularVelocity": -8.321210302525092e-35, + "velocityX": 3.972717142215159e-38, + "velocityY": 2.156619931918946e-38, + "timestamp": 5.545585235525441 + }, + { + "x": 5.412646620847983, + "y": 6.685272362409357, + "heading": 3.3376723045170427, + "angularVelocity": -0.20995551242929855, + "velocityX": 0.8548398740209807, + "velocityY": -0.29397737100168153, + "timestamp": 5.627751404398608 + }, + { + "x": 5.553160406591399, + "y": 6.636944177185238, + "heading": 3.3042040470339473, + "angularVelocity": -0.40732405979348324, + "velocityX": 1.710117286353154, + "velocityY": -0.5881762030151344, + "timestamp": 5.709917573271775 + }, + { + "x": 5.764011811265675, + "y": 6.564410113443709, + "heading": 3.256520385123487, + "angularVelocity": -0.5803320583690068, + "velocityX": 2.5661584027332363, + "velocityY": -0.8827728581758841, + "timestamp": 5.792083742144942 + }, + { + "x": 6.045327886786299, + "y": 6.467599426347047, + "heading": 3.200063171820206, + "angularVelocity": -0.6871102070053786, + "velocityX": 3.4237457992579357, + "velocityY": -1.1782305104927078, + "timestamp": 5.874249911018109 + }, + { + "x": 6.393950264167201, + "y": 6.3481237380245314, + "heading": 3.2000631015766148, + "angularVelocity": -8.548967570988147e-7, + "velocityX": 4.242894395125564, + "velocityY": -1.4540739815548667, + "timestamp": 5.956416079891276 + }, + { + "x": 6.742456977163128, + "y": 6.228311034623432, + "heading": 3.200063071871244, + "angularVelocity": -3.615279995739255e-7, + "velocityX": 4.241486706455621, + "velocityY": -1.4581756097943859, + "timestamp": 6.038582248764443 + }, + { + "x": 7.091069051642512, + "y": 6.108805287445655, + "heading": 3.200063001812038, + "angularVelocity": -8.526527023394516e-7, + "velocityX": 4.242769004083742, + "velocityY": -1.454439811624267, + "timestamp": 6.12074841763761 + }, + { + "x": 7.372382443797803, + "y": 6.012003983976649, + "heading": 3.14332703013963, + "angularVelocity": -0.690502824343514, + "velocityX": 3.4237131414698108, + "velocityY": -1.1781163074358554, + "timestamp": 6.202914586510777 + }, + { + "x": 7.583230269821529, + "y": 5.939474882226305, + "heading": 3.095396790832798, + "angularVelocity": -0.5833330184935077, + "velocityX": 2.5661148489129793, + "velocityY": -0.8827124684649698, + "timestamp": 6.285080755383944 + }, + { + "x": 7.723740293307972, + "y": 5.8911496882585785, + "heading": 3.0617167669372236, + "angularVelocity": -0.409901354261302, + "velocityX": 1.710071497958437, + "velocityY": -0.5881397980514556, + "timestamp": 6.367246924257111 + }, + { + "x": 7.793976783752441, + "y": 5.8669962882995605, + "heading": 3.044340108418373, + "angularVelocity": -0.21148191229012508, + "velocityX": 0.8548103362697581, + "velocityY": -0.29395796701075205, + "timestamp": 6.4494130931302776 + }, + { + "x": 7.793976783752441, + "y": 5.8669962882995605, + "heading": 3.044340108418373, + "angularVelocity": 0, + "velocityX": -4.398446739190714e-37, + "velocityY": 0, + "timestamp": 6.5315792620034445 + }, + { + "x": 7.724452143365902, + "y": 5.891359995748724, + "heading": 3.064192130892063, + "angularVelocity": 0.2424150971155188, + "velocityX": -0.8489725655691124, + "velocityY": 0.29750774840248856, + "timestamp": 6.6134719440595875 + }, + { + "x": 7.585289292630555, + "y": 5.940133616180372, + "heading": 3.100973644408784, + "angularVelocity": 0.44914286103738393, + "velocityX": -1.699331945679124, + "velocityY": 0.5955797173453272, + "timestamp": 6.6953646261157305 + }, + { + "x": 7.3763326141186685, + "y": 6.0133792242054085, + "heading": 3.14960943502028, + "angularVelocity": 0.5938966631738083, + "velocityX": -2.5515915862742524, + "velocityY": 0.8944096857741486, + "timestamp": 6.777257308171873 + }, + { + "x": 7.097432154332273, + "y": 6.111156462191388, + "heading": 3.198862048472999, + "angularVelocity": 0.6014287506050079, + "velocityX": -3.405682324523109, + "velocityY": 1.1939679533141472, + "timestamp": 6.859149990228016 + }, + { + "x": 6.7509713925352335, + "y": 6.232193154508266, + "heading": 3.199747773581081, + "angularVelocity": 0.010815680789089665, + "velocityX": -4.230668151270443, + "velocityY": 1.4779915528215268, + "timestamp": 6.941042672284159 + }, + { + "x": 6.404350241550633, + "y": 6.353697070710262, + "heading": 3.199747821125258, + "angularVelocity": 5.805668609663509e-7, + "velocityX": -4.232626680207759, + "velocityY": 1.4836968719463455, + "timestamp": 7.022935354340302 + }, + { + "x": 6.057887361130367, + "y": 6.47472492007914, + "heading": 3.2006358854014487, + "angularVelocity": 0.01084424461249623, + "velocityX": -4.230694021997506, + "velocityY": 1.4778835706700362, + "timestamp": 7.104828036396445 + }, + { + "x": 5.77898688869625, + "y": 6.572504725430993, + "heading": 3.2497074160560264, + "angularVelocity": 0.5992175298512812, + "velocityX": -3.4056824789657414, + "velocityY": 1.193999303684035, + "timestamp": 7.186720718452588 + }, + { + "x": 5.57003008056901, + "y": 6.645752368291244, + "heading": 3.298210403791235, + "angularVelocity": 0.5922749935330796, + "velocityX": -2.551593169020725, + "velocityY": 0.8944345333571375, + "timestamp": 7.268613400508731 + }, + { + "x": 5.430865596155991, + "y": 6.694528008588771, + "heading": 3.3348189416352247, + "angularVelocity": 0.4470306372294954, + "velocityX": -1.6993518946859398, + "velocityY": 0.5956043821362281, + "timestamp": 7.350506082564874 + }, + { + "x": 5.3613386154174805, + "y": 6.718893051147461, + "heading": 3.354531552893077, + "angularVelocity": 0.24071273235791338, + "velocityX": -0.8490011438487919, + "velocityY": 0.2975240515628335, + "timestamp": 7.432398764621017 + }, + { + "x": 5.3613386154174805, + "y": 6.718893051147461, + "heading": 3.354531552893077, + "angularVelocity": -3.752319250871441e-31, + "velocityX": -2.3322961891501556e-32, + "velocityY": -6.418789255803219e-32, + "timestamp": 7.51429144667716 + }, + { + "x": 5.381762554826202, + "y": 6.705904304585873, + "heading": 3.3390037774751677, + "angularVelocity": -0.32613159738048514, + "velocityX": 0.4289662752646431, + "velocityY": -0.2728040913844985, + "timestamp": 7.56190343994193 + }, + { + "x": 5.422670945546834, + "y": 6.679911372646232, + "heading": 3.3084755716638385, + "angularVelocity": -0.6411873084489506, + "velocityX": 0.859203488774904, + "velocityY": -0.5459324459516255, + "timestamp": 7.6095154332067 + }, + { + "x": 5.4841375959919265, + "y": 6.640889729584984, + "heading": 3.2636438109088237, + "angularVelocity": -0.9416064667929949, + "velocityX": 1.290990908598487, + "velocityY": -0.8195759174435114, + "timestamp": 7.65712742647147 + }, + { + "x": 5.566254301369102, + "y": 6.588794024470335, + "heading": 3.2055157827984404, + "angularVelocity": -1.2208694516763405, + "velocityX": 1.7247063133965845, + "velocityY": -1.09417189960937, + "timestamp": 7.7047394197362395 + }, + { + "x": 5.669144917681803, + "y": 6.52354420886013, + "heading": 3.135745985842297, + "angularVelocity": -1.4653828199996848, + "velocityX": 2.1610230796364758, + "velocityY": -1.3704491481257148, + "timestamp": 7.752351413001009 + }, + { + "x": 5.792991373719124, + "y": 6.445019863790107, + "heading": 3.0574508369011313, + "angularVelocity": -1.6444417377309697, + "velocityX": 2.601160916506215, + "velocityY": -1.6492555695668922, + "timestamp": 7.799963406265779 + }, + { + "x": 5.938012207911987, + "y": 6.3531182794458365, + "heading": 2.9775771179710966, + "angularVelocity": -1.6775966191089577, + "velocityX": 3.0458887403926016, + "velocityY": -1.9302192166856003, + "timestamp": 7.847575399530549 + }, + { + "x": 6.10288509502327, + "y": 6.249471412942973, + "heading": 2.9223714814313397, + "angularVelocity": -1.159490135872572, + "velocityX": 3.462843619975461, + "velocityY": -2.176906686653277, + "timestamp": 7.895187392795319 + }, + { + "x": 6.283648559689557, + "y": 6.138505943088611, + "heading": 2.918071321671212, + "angularVelocity": -0.0903167346134664, + "velocityX": 3.796595191068823, + "velocityY": -2.3306201283654526, + "timestamp": 7.942799386060089 + }, + { + "x": 6.4636502982527535, + "y": 6.023608344983735, + "heading": 2.9180712113581024, + "angularVelocity": -0.000002316918529011022, + "velocityX": 3.780596572845407, + "velocityY": -2.4132070561726313, + "timestamp": 7.990411379324859 + }, + { + "x": 6.6436519645721965, + "y": 5.908710633699827, + "heading": 2.9180711010451206, + "angularVelocity": -0.000002316915849069208, + "velocityX": 3.780595055501843, + "velocityY": -2.4132094332844187, + "timestamp": 8.038023372589628 + }, + { + "x": 6.8228001378837595, + "y": 5.79248664277841, + "heading": 2.918070991032562, + "angularVelocity": -0.000002310606033986578, + "velocityX": 3.762669046769599, + "velocityY": -2.4410654322719396, + "timestamp": 8.085635365854397 + }, + { + "x": 6.987106800079346, + "y": 5.656085014343262, + "heading": 2.9180708819744035, + "angularVelocity": -0.000002290560660276634, + "velocityX": 3.4509511349701865, + "velocityY": -2.864858601424802, + "timestamp": 8.133247359119167 + }, + { + "x": 7.197673412932475, + "y": 5.404866525743537, + "heading": 2.91807080868153, + "angularVelocity": -0.0000010028513094998122, + "velocityX": 2.8811396466166426, + "velocityY": -3.4373708996697796, + "timestamp": 8.206331846051674 + }, + { + "x": 7.389061641704391, + "y": 5.138747052991589, + "heading": 2.9180707373970454, + "angularVelocity": -9.75370940633925e-7, + "velocityX": 2.618725762533503, + "velocityY": -3.6412580004544863, + "timestamp": 8.27941633298418 + }, + { + "x": 7.559618144405977, + "y": 4.904145918307005, + "heading": 2.815675073371891, + "angularVelocity": -1.4010588063603617, + "velocityX": 2.333689540146932, + "velocityY": -3.2099990645263112, + "timestamp": 8.352500819916687 + }, + { + "x": 7.695410928944271, + "y": 4.716969709749763, + "heading": 2.700604666277338, + "angularVelocity": -1.574484708373056, + "velocityX": 1.8580247359955884, + "velocityY": -2.561093556423498, + "timestamp": 8.425585306849193 + }, + { + "x": 7.796892129440441, + "y": 4.57711072358559, + "heading": 2.60568153422841, + "angularVelocity": -1.2988136885542398, + "velocityX": 1.3885463900126565, + "velocityY": -1.9136617363602635, + "timestamp": 8.4986697937817 + }, + { + "x": 7.864395197078724, + "y": 4.484114526017619, + "heading": 2.5390881205921088, + "angularVelocity": -0.9111839794097845, + "velocityX": 0.9236305879871393, + "velocityY": -1.2724478404549935, + "timestamp": 8.571754280714206 + }, + { + "x": 7.898097515106201, + "y": 4.4377031326293945, + "heading": 2.504728002453472, + "angularVelocity": -0.47014242804166056, + "velocityX": 0.4611418844407282, + "velocityY": -0.6350375481340379, + "timestamp": 8.644838767646712 + }, + { + "x": 7.898097515106201, + "y": 4.4377031326293945, + "heading": 2.504728002453472, + "angularVelocity": -1.3198901080890972e-29, + "velocityX": -5.557969546153989e-30, + "velocityY": 1.6398907339851722e-31, + "timestamp": 8.717923254579219 + }, + { + "x": 7.872617208829989, + "y": 4.473100868040156, + "heading": 2.533632642169352, + "angularVelocity": 0.4521476679265757, + "velocityX": -0.3985817216228667, + "velocityY": 0.5537174541243508, + "timestamp": 8.781850687509323 + }, + { + "x": 7.821576408126655, + "y": 4.544074102448603, + "heading": 2.589474507633189, + "angularVelocity": 0.8735195972109739, + "velocityX": -0.7984178053753025, + "velocityY": 1.1102156172303905, + "timestamp": 8.845778120439427 + }, + { + "x": 7.744874668782088, + "y": 4.650876974268225, + "heading": 2.669362107193564, + "angularVelocity": 1.2496606839777096, + "velocityX": -1.1998251115201943, + "velocityY": 1.6706892006190956, + "timestamp": 8.90970555336953 + }, + { + "x": 7.642361003877687, + "y": 4.793861579473503, + "heading": 2.7687102276641724, + "angularVelocity": 1.554076488246139, + "velocityX": -1.6035942662750133, + "velocityY": 2.2366705286229, + "timestamp": 8.973632986299634 + }, + { + "x": 7.513746244350758, + "y": 4.973514163207665, + "heading": 2.877208848057101, + "angularVelocity": 1.6972153490279336, + "velocityX": -2.0118868165337256, + "velocityY": 2.810258061364581, + "timestamp": 9.037560419229738 + }, + { + "x": 7.36085249101928, + "y": 5.1866403519008655, + "heading": 2.9426744108217315, + "angularVelocity": 1.0240605599822166, + "velocityX": -2.3916767234910896, + "velocityY": 3.3338768494932727, + "timestamp": 9.101487852159842 + }, + { + "x": 7.194539581784471, + "y": 5.420200579055916, + "heading": 2.9426745032862103, + "angularVelocity": 0.0000014463974957387205, + "velocityX": -2.601589045764413, + "velocityY": 3.6535211324756753, + "timestamp": 9.165415285089946 + }, + { + "x": 6.999818325042725, + "y": 5.630661964416504, + "heading": 2.9426746005935125, + "angularVelocity": 0.0000015221525015916737, + "velocityX": -3.045973345350628, + "velocityY": 3.292192032654593, + "timestamp": 9.22934271802005 + }, + { + "x": 6.821050073226294, + "y": 5.780060077850853, + "heading": 2.9426746891736677, + "angularVelocity": 0.0000017053007611025479, + "velocityX": -3.4415568043145366, + "velocityY": 2.8761376173765214, + "timestamp": 9.2812867253345 + }, + { + "x": 6.624664762679586, + "y": 5.905402386843963, + "heading": 2.942674778221278, + "angularVelocity": 0.0000017142999717232747, + "velocityX": -3.7807115911927003, + "velocityY": 2.4130273244857414, + "timestamp": 9.33323073264895 + }, + { + "x": 6.4270459836651215, + "y": 6.028790809329787, + "heading": 2.942674867501059, + "angularVelocity": 0.0000017187696207326967, + "velocityX": -3.804457708049898, + "velocityY": 2.375412080528933, + "timestamp": 9.3851747399634 + }, + { + "x": 6.22942713407172, + "y": 6.152179118776215, + "heading": 2.9426749567811457, + "angularVelocity": 0.0000017187754837333519, + "velocityX": -3.8044590668002334, + "velocityY": 2.3754099043511037, + "timestamp": 9.43711874727785 + }, + { + "x": 6.035443701378057, + "y": 6.268251221439266, + "heading": 2.962950698672798, + "angularVelocity": 0.3903384228503762, + "velocityX": -3.734471842331197, + "velocityY": 2.2345619574630913, + "timestamp": 9.4890627545923 + }, + { + "x": 5.861314358992964, + "y": 6.376637011473068, + "heading": 3.0412855386044892, + "angularVelocity": 1.5080630852656163, + "velocityX": -3.3522508444713, + "velocityY": 2.086588918288052, + "timestamp": 9.54100676190675 + }, + { + "x": 5.712466276366555, + "y": 6.469340902724069, + "heading": 3.1246025769956893, + "angularVelocity": 1.6039778734595374, + "velocityX": -2.8655486998786075, + "velocityY": 1.7846888610231364, + "timestamp": 9.5929507692212 + }, + { + "x": 5.588796508227056, + "y": 6.546382737169229, + "heading": 3.2002505066448665, + "angularVelocity": 1.4563360349006294, + "velocityX": -2.3808284060727742, + "velocityY": 1.4831707915559873, + "timestamp": 9.64489477653565 + }, + { + "x": 5.490084330824383, + "y": 6.607894755873726, + "heading": 3.2637725507230906, + "angularVelocity": 1.222894562094215, + "velocityX": -1.9003573753003593, + "velocityY": 1.1841985608104344, + "timestamp": 9.696838783850101 + }, + { + "x": 5.416180600283462, + "y": 6.6539627808027575, + "heading": 3.3129789015972246, + "angularVelocity": 0.9472960100335697, + "velocityX": -1.4227575876757712, + "velocityY": 0.8868785315340437, + "timestamp": 9.748782791164551 + }, + { + "x": 5.366980486180744, + "y": 6.684639660698046, + "heading": 3.3465688664099242, + "angularVelocity": 0.6466571708524164, + "velocityX": -0.9471759428354473, + "velocityY": 0.5905759197511072, + "timestamp": 9.800726798479001 + }, + { + "x": 5.342407703399658, + "y": 6.699962139129639, + "heading": 3.3636740355255, + "angularVelocity": 0.32930014451959316, + "velocityX": -0.4730629008334152, + "velocityY": 0.294980676766733, + "timestamp": 9.852670805793451 + }, + { + "x": 5.342407703399658, + "y": 6.699962139129639, + "heading": 3.3636740355255, + "angularVelocity": -1.0197070578567015e-31, + "velocityX": 4.5771839799212747e-32, + "velocityY": -2.780302289877669e-33, + "timestamp": 9.904614813107901 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA123_2.1.traj b/src/main/deploy/choreo/CBA123_2.1.traj new file mode 100644 index 0000000..2604107 --- /dev/null +++ b/src/main/deploy/choreo/CBA123_2.1.traj @@ -0,0 +1,85 @@ +{ + "samples": [ + { + "x": 1.363219976425171, + "y": 4.730286121368408, + "heading": 2.611103760491661, + "angularVelocity": 2.856687734403895e-35, + "velocityX": 3.1220671934678776e-35, + "velocityY": 6.181424868568757e-35, + "timestamp": 0 + }, + { + "x": 1.402998270947258, + "y": 4.683184154208398, + "heading": 2.6049915833998276, + "angularVelocity": -0.08180300703051442, + "velocityX": 0.5323772622360481, + "velocityY": -0.6303944556661406, + "timestamp": 0.07471824464293149 + }, + { + "x": 1.4839770811789443, + "y": 4.590200438242412, + "heading": 2.5936512198440655, + "angularVelocity": -0.15177502643372132, + "velocityX": 1.08378898110727, + "velocityY": -1.2444579822550972, + "timestamp": 0.14943648928586298 + }, + { + "x": 1.6089330089555856, + "y": 4.453958116559408, + "heading": 2.578899291635344, + "angularVelocity": -0.19743408426173076, + "velocityX": 1.6723616617840638, + "velocityY": -1.823414379367287, + "timestamp": 0.22415473392879448 + }, + { + "x": 1.7852840218103743, + "y": 4.283584114299459, + "heading": 2.5663808674297406, + "angularVelocity": -0.16754173315269297, + "velocityX": 2.3602135421883452, + "velocityY": -2.280219551116922, + "timestamp": 0.29887297857172596 + }, + { + "x": 1.9970725289507456, + "y": 4.162112267088438, + "heading": 2.585076657998306, + "angularVelocity": 0.2502172080983726, + "velocityX": 2.834495218035172, + "velocityY": -1.625732079112933, + "timestamp": 0.37359122321465743 + }, + { + "x": 2.1898805805828863, + "y": 4.098958983843109, + "heading": 2.6201560463541953, + "angularVelocity": 0.4694889250079303, + "velocityX": 2.5804681648176344, + "velocityY": -0.8452190431818932, + "timestamp": 0.4483094678575889 + }, + { + "x": 2.3558360300545305, + "y": 4.091099567826289, + "heading": 2.668216435711751, + "angularVelocity": 0.6432216065464311, + "velocityX": 2.221083354737834, + "velocityY": -0.1051873749761943, + "timestamp": 0.5230277125005204 + }, + { + "x": 2.4921278953552246, + "y": 4.137131214141846, + "heading": 2.727903726186528, + "angularVelocity": 0.7988315405429405, + "velocityX": 1.8240774519264265, + "velocityY": 0.6160696967057429, + "timestamp": 0.5977459571434519 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA123_2.2.traj b/src/main/deploy/choreo/CBA123_2.2.traj new file mode 100644 index 0000000..19601b0 --- /dev/null +++ b/src/main/deploy/choreo/CBA123_2.2.traj @@ -0,0 +1,679 @@ +{ + "samples": [ + { + "x": 2.4921278953552246, + "y": 4.137131214141846, + "heading": 2.727903726186528, + "angularVelocity": 0.7988315405429405, + "velocityX": 1.8240774519264265, + "velocityY": 0.6160696967057429, + "timestamp": 0 + }, + { + "x": 2.5678257820915626, + "y": 4.189783493438115, + "heading": 2.771498377016999, + "angularVelocity": 0.8941007357134214, + "velocityX": 1.5525192869672992, + "velocityY": 1.0798673864564463, + "timestamp": 0.048758097481807816 + }, + { + "x": 2.630126753501051, + "y": 4.264971550188619, + "heading": 2.8193191143009715, + "angularVelocity": 0.9807752917720928, + "velocityX": 1.277756406158655, + "velocityY": 1.5420629727925397, + "timestamp": 0.09751619496361563 + }, + { + "x": 2.678828008144544, + "y": 4.362590092597177, + "heading": 2.8708181793814673, + "angularVelocity": 1.0562156388425759, + "velocityX": 0.9988341867043516, + "velocityY": 2.002099086105245, + "timestamp": 0.14627429244542345 + }, + { + "x": 2.7136560652750763, + "y": 4.482489991621694, + "heading": 2.9252472290103584, + "angularVelocity": 1.116307904532149, + "velocityX": 0.7143030374293637, + "velocityY": 2.4590766501759567, + "timestamp": 0.19503238992723126 + }, + { + "x": 2.734222752794457, + "y": 4.624444876515601, + "heading": 2.9815222182693346, + "angularVelocity": 1.1541670443555083, + "velocityX": 0.4218107059459087, + "velocityY": 2.911411482920796, + "timestamp": 0.24379048740903908 + }, + { + "x": 2.739936088083405, + "y": 4.788073833618392, + "heading": 3.0379379742173653, + "angularVelocity": 1.1570540866382242, + "velocityX": 0.11717715792910317, + "velocityY": 3.355933999759601, + "timestamp": 0.2925485848908469 + }, + { + "x": 2.7297924832395113, + "y": 4.972619509286156, + "heading": 3.091451470522054, + "angularVelocity": 1.0975304425004457, + "velocityX": -0.20803938971734517, + "velocityY": 3.7849236372813815, + "timestamp": 0.3413066823726547 + }, + { + "x": 2.7018025256512415, + "y": 5.176036576333896, + "heading": 3.1353828525002947, + "angularVelocity": 0.9010068941806458, + "velocityX": -0.5740576239405811, + "velocityY": 4.171964813098734, + "timestamp": 0.39006477985446253 + }, + { + "x": 2.652767431235754, + "y": 5.3866681773925515, + "heading": 3.1430169415371583, + "angularVelocity": 0.15657069145718175, + "velocityX": -1.0056810447491957, + "velocityY": 4.319930676894127, + "timestamp": 0.43882287733627023 + }, + { + "x": 2.578230857849121, + "y": 5.591317176818848, + "heading": 3.145641116687763, + "angularVelocity": 0.05382029418977674, + "velocityX": -1.5287014308637321, + "velocityY": 4.197231024090984, + "timestamp": 0.48758097481807805 + }, + { + "x": 2.5300923576495236, + "y": 5.715831604580908, + "heading": 3.1692857866621438, + "angularVelocity": 0.7493726813277178, + "velocityX": -1.5256578759083332, + "velocityY": 3.946247111807484, + "timestamp": 0.5191335928458499 + }, + { + "x": 2.489698719124449, + "y": 5.83712547611349, + "heading": 3.211129453316311, + "angularVelocity": 1.326155142414398, + "velocityX": -1.280199268711112, + "velocityY": 3.844177729588811, + "timestamp": 0.5506862108736218 + }, + { + "x": 2.4586027844574416, + "y": 5.957244854458578, + "heading": 3.2681567689882636, + "angularVelocity": 1.8073719151215417, + "velocityX": -0.9855262926086568, + "velocityY": 3.806954409911735, + "timestamp": 0.5822388289013937 + }, + { + "x": 2.4376767560186225, + "y": 6.077594195526883, + "heading": 3.335908594784813, + "angularVelocity": 2.1472647923197865, + "velocityX": -0.6632105272659387, + "velocityY": 3.814242639465817, + "timestamp": 0.6137914469291655 + }, + { + "x": 2.4276354965926332, + "y": 6.1965351647529765, + "heading": 3.4055549427565963, + "angularVelocity": 2.2073080563546945, + "velocityX": -0.318238550511125, + "velocityY": 3.769606982260691, + "timestamp": 0.6453440649569374 + }, + { + "x": 2.4285189105865825, + "y": 6.314163311794461, + "heading": 3.475107035399847, + "angularVelocity": 2.204320813633675, + "velocityX": 0.02799812025650875, + "velocityY": 3.7279995890658006, + "timestamp": 0.6768966829847093 + }, + { + "x": 2.4403099616706636, + "y": 6.4304564053655255, + "heading": 3.54330154735025, + "angularVelocity": 2.1612948849562805, + "velocityX": 0.3736948570702671, + "velocityY": 3.685687617702767, + "timestamp": 0.7084493010124812 + }, + { + "x": 2.462968721480692, + "y": 6.5453156721591546, + "heading": 3.6091874198616924, + "angularVelocity": 2.088126964724473, + "velocityX": 0.7181261405974296, + "velocityY": 3.6402452149147515, + "timestamp": 0.740001919040253 + }, + { + "x": 2.496280419314515, + "y": 6.658059875645555, + "heading": 3.670815010978431, + "angularVelocity": 1.9531688642285017, + "velocityX": 1.0557506766792757, + "velocityY": 3.573212320675413, + "timestamp": 0.7715545370680249 + }, + { + "x": 2.5399627685546875, + "y": 6.768060207366943, + "heading": 3.726552604780115, + "angularVelocity": 1.7664966422952548, + "velocityX": 1.3844286772566394, + "velocityY": 3.4862505426512955, + "timestamp": 0.8031071550957968 + }, + { + "x": 2.711472402265462, + "y": 7.022141137893455, + "heading": 3.8205671676033472, + "angularVelocity": 1.1954904742675398, + "velocityX": 2.180918861813592, + "velocityY": 3.2308966080988553, + "timestamp": 0.8817481521148934 + }, + { + "x": 2.9440584277570823, + "y": 7.251420784545613, + "heading": 3.870489572159589, + "angularVelocity": 0.6348139831457024, + "velocityX": 2.957567099958574, + "velocityY": 2.91552314114839, + "timestamp": 0.9603891491339901 + }, + { + "x": 3.2336142292756485, + "y": 7.4463705200531045, + "heading": 3.8816828308541447, + "angularVelocity": 0.14233363155146306, + "velocityX": 3.6819955556801096, + "velocityY": 2.478983518738352, + "timestamp": 1.0390301461530869 + }, + { + "x": 3.5587364292560837, + "y": 7.583133241747301, + "heading": 3.881682648772359, + "angularVelocity": -0.0000023153544915760507, + "velocityX": 4.134258367826705, + "velocityY": 1.7390766505794137, + "timestamp": 1.1176711431721835 + }, + { + "x": 3.9041321281628165, + "y": 7.654619714094976, + "heading": 3.881682528586368, + "angularVelocity": -0.000001528286715346348, + "velocityX": 4.392056459086593, + "velocityY": 0.9090229658496732, + "timestamp": 1.1963121401912802 + }, + { + "x": 4.25683041113255, + "y": 7.658145635695571, + "heading": 3.881682433362879, + "angularVelocity": -0.0000012108631983957567, + "velocityX": 4.484916218497164, + "velocityY": 0.0448356675810983, + "timestamp": 1.2749531372103768 + }, + { + "x": 4.607642012696934, + "y": 7.621543283163485, + "heading": 3.8816823417099404, + "angularVelocity": -0.000001165460028483155, + "velocityX": 4.460925151790678, + "velocityY": -0.46543601835564014, + "timestamp": 1.3535941342294735 + }, + { + "x": 4.958453577460718, + "y": 7.584940577921394, + "heading": 3.8816822500570063, + "angularVelocity": -0.0000011654599646464362, + "velocityX": 4.460924683833743, + "velocityY": -0.4654405034209098, + "timestamp": 1.4322351312485702 + }, + { + "x": 5.30926514222431, + "y": 7.548337872677454, + "heading": 3.8816821584040735, + "angularVelocity": -0.0000011654599583899375, + "velocityX": 4.46092468383129, + "velocityY": -0.46544050344442334, + "timestamp": 1.5108761282676668 + }, + { + "x": 5.6600767069879, + "y": 7.511735167433515, + "heading": 3.8816820667511402, + "angularVelocity": -0.0000011654599544160961, + "velocityX": 4.4609246838312915, + "velocityY": -0.46544050344441834, + "timestamp": 1.5895171252867635 + }, + { + "x": 6.010888271751492, + "y": 7.475132462189576, + "heading": 3.881681975098208, + "angularVelocity": -0.0000011654599477733292, + "velocityX": 4.4609246838312915, + "velocityY": -0.46544050344441307, + "timestamp": 1.6681581223058601 + }, + { + "x": 6.36169983651515, + "y": 7.438529756946278, + "heading": 3.881681883445276, + "angularVelocity": -0.000001165459943904857, + "velocityX": 4.460924683832142, + "velocityY": -0.4654405034362651, + "timestamp": 1.7467991193249568 + }, + { + "x": 6.712511414020599, + "y": 7.401927173824582, + "heading": 3.881681791792296, + "angularVelocity": -0.000001165460557447994, + "velocityX": 4.460924845856925, + "velocityY": -0.465438950536295, + "timestamp": 1.8254401163440535 + }, + { + "x": 7.057780054096872, + "y": 7.3861277126995235, + "heading": 3.862699181090406, + "angularVelocity": -0.2413831388388956, + "velocityX": 4.390440777250461, + "velocityY": -0.20090616502766034, + "timestamp": 1.9040811133631501 + }, + { + "x": 7.357828304832814, + "y": 7.380096983401069, + "heading": 3.7196702717700116, + "angularVelocity": -1.8187575786413597, + "velocityX": 3.8154176842783394, + "velocityY": -0.07668683672704285, + "timestamp": 1.9827221103822468 + }, + { + "x": 7.591253431232368, + "y": 7.384293260233195, + "heading": 3.54918164843041, + "angularVelocity": -2.167935679887186, + "velocityX": 2.968237118647807, + "velocityY": 0.05335991392769182, + "timestamp": 2.0613631074013434 + }, + { + "x": 7.757049647154371, + "y": 7.397291202866586, + "heading": 3.3897603884068133, + "angularVelocity": -2.0272029357013897, + "velocityX": 2.1082669626091244, + "velocityY": 0.16528201734566547, + "timestamp": 2.14000410442044 + }, + { + "x": 7.855769551197074, + "y": 7.41842241781004, + "heading": 3.2555384895151875, + "angularVelocity": -1.7067675128665036, + "velocityX": 1.2553236579481564, + "velocityY": 0.26870481993409817, + "timestamp": 2.2186451014395367 + }, + { + "x": 7.8879241943359375, + "y": 7.44731855392456, + "heading": 3.1544127998908698, + "angularVelocity": -1.2859156605016193, + "velocityX": 0.4088788845219625, + "velocityY": 0.36744366437142384, + "timestamp": 2.2972860984586334 + }, + { + "x": 7.853095293464832, + "y": 7.484128621336326, + "heading": 3.0912302182855265, + "angularVelocity": -0.7969541696453112, + "velocityX": -0.4393147140895529, + "velocityY": 0.4643041794647338, + "timestamp": 2.3765661675163847 + }, + { + "x": 7.751024990133268, + "y": 7.528622420567753, + "heading": 3.066855075648433, + "angularVelocity": -0.30745612266479316, + "velocityX": -1.287464864053165, + "velocityY": 0.5612230130503048, + "timestamp": 2.455846236574136 + }, + { + "x": 7.5817055236517445, + "y": 7.580769856739841, + "heading": 3.0812339833070395, + "angularVelocity": 0.18136850572281352, + "velocityX": -2.135712903556922, + "velocityY": 0.6577622445573384, + "timestamp": 2.5351263056318873 + }, + { + "x": 7.345111485587916, + "y": 7.640496291283522, + "heading": 3.1341452918793684, + "angularVelocity": 0.6673973572574197, + "velocityX": -2.9842814325941345, + "velocityY": 0.7533600216742169, + "timestamp": 2.6144063746896387 + }, + { + "x": 7.0411691500814175, + "y": 7.707635289062441, + "heading": 3.224871988420523, + "angularVelocity": 1.1443821583337042, + "velocityX": -3.833779903560516, + "velocityY": 0.8468584674164656, + "timestamp": 2.69368644374739 + }, + { + "x": 6.687353000496664, + "y": 7.743031104298496, + "heading": 3.224872054818683, + "angularVelocity": 8.3751389593832e-7, + "velocityX": -4.462863791491119, + "velocityY": 0.44646549450242334, + "timestamp": 2.7729665128051413 + }, + { + "x": 6.333406667177098, + "y": 7.708961786842931, + "heading": 3.2248721535215856, + "angularVelocity": 0.0000012449901260127418, + "velocityX": -4.4645058654241945, + "velocityY": -0.42973370054392757, + "timestamp": 2.8522465818628926 + }, + { + "x": 6.001048260775448, + "y": 7.609896901121636, + "heading": 3.253324179663097, + "angularVelocity": 0.3588799364035036, + "velocityX": -4.192206318079056, + "velocityY": -1.2495559968436876, + "timestamp": 2.931526650920644 + }, + { + "x": 5.725196978606951, + "y": 7.471944548035669, + "heading": 3.308251967986784, + "angularVelocity": 0.6928322461938681, + "velocityX": -3.4794531014794283, + "velocityY": -1.7400634828594161, + "timestamp": 3.010806719978395 + }, + { + "x": 5.514912201266955, + "y": 7.311259163266165, + "heading": 3.355532174812349, + "angularVelocity": 0.5963693950761302, + "velocityX": -2.652429290731509, + "velocityY": -2.0268068214276393, + "timestamp": 3.0900867890361465 + }, + { + "x": 5.37147700444608, + "y": 7.133177495561375, + "heading": 3.384976056212167, + "angularVelocity": 0.37139071332505014, + "velocityX": -1.80922139102062, + "velocityY": -2.246235022513233, + "timestamp": 3.169366858093898 + }, + { + "x": 5.295263290405273, + "y": 6.9402666091918945, + "heading": 3.391793203834301, + "angularVelocity": 0.08598816453058325, + "velocityX": -0.9613224981588709, + "velocityY": -2.4332835309332914, + "timestamp": 3.248646927151649 + }, + { + "x": 5.2851018227414315, + "y": 6.739082812486844, + "heading": 3.374132057887834, + "angularVelocity": -0.2280237523514783, + "velocityX": -0.1311951100529265, + "velocityY": -2.597491939427625, + "timestamp": 3.3261000247537087 + }, + { + "x": 5.339706721305988, + "y": 6.526786224855968, + "heading": 3.3350290350932306, + "angularVelocity": -0.5048606705894175, + "velocityX": 0.7050060004714964, + "velocityY": -2.7409696216621224, + "timestamp": 3.4035531223557682 + }, + { + "x": 5.459717296048167, + "y": 6.306034851708146, + "heading": 3.279334015838865, + "angularVelocity": -0.7190805917216783, + "velocityX": 1.5494612669821415, + "velocityY": -2.8501296911584357, + "timestamp": 3.4810062199578273 + }, + { + "x": 5.645882142703712, + "y": 6.081995955148106, + "heading": 3.216428172598459, + "angularVelocity": -0.8121798247967432, + "velocityX": 2.4035816825819953, + "velocityY": -2.8925750356830533, + "timestamp": 3.5584593175598864 + }, + { + "x": 5.897235284038749, + "y": 5.867927132656735, + "heading": 3.170219120243653, + "angularVelocity": -0.5966068986965503, + "velocityX": 3.245230327990835, + "velocityY": -2.763851015891185, + "timestamp": 3.6359124151619455 + }, + { + "x": 6.197857568827006, + "y": 5.693844894960139, + "heading": 3.1702188407942256, + "angularVelocity": -0.000003607982588359873, + "velocityX": 3.8813461836323366, + "velocityY": -2.2475826414458036, + "timestamp": 3.7133655127640046 + }, + { + "x": 6.526078207722115, + "y": 5.5800483992181364, + "heading": 3.1702187626348843, + "angularVelocity": -0.0000010091183420138395, + "velocityX": 4.237669622736715, + "velocityY": -1.4692310477583403, + "timestamp": 3.7908186103660637 + }, + { + "x": 6.869940640875451, + "y": 5.530683043384077, + "heading": 3.1702180851447475, + "angularVelocity": -0.000008747101898111188, + "velocityX": 4.4396214457430965, + "velocityY": -0.6373580574877811, + "timestamp": 3.868271707968123 + }, + { + "x": 7.194451452823388, + "y": 5.538769828048966, + "heading": 3.1028210554472024, + "angularVelocity": -0.8701657103995888, + "velocityX": 4.189771900605136, + "velocityY": 0.10440879597142798, + "timestamp": 3.945724805570182 + }, + { + "x": 7.4640579985911675, + "y": 5.583997607667359, + "heading": 3.038950860040275, + "angularVelocity": -0.8246306136790209, + "velocityX": 3.4809007530334206, + "velocityY": 0.5839376476686109, + "timestamp": 4.0231779031722406 + }, + { + "x": 7.674947972014166, + "y": 5.659124971358623, + "heading": 2.9929962955809506, + "angularVelocity": -0.5933211954340547, + "velocityX": 2.722808770109035, + "velocityY": 0.9699723576874354, + "timestamp": 4.1006310007743 + }, + { + "x": 7.825975185916945, + "y": 5.761003986073113, + "heading": 2.9710435386371308, + "angularVelocity": -0.28343291131633563, + "velocityX": 1.9499183193257243, + "velocityY": 1.3153639798620549, + "timestamp": 4.178084098376359 + }, + { + "x": 7.916625499725342, + "y": 5.887895107269287, + "heading": 2.9764432064175574, + "angularVelocity": 0.06971532382306395, + "velocityX": 1.1703897792976932, + "velocityY": 1.638296273805844, + "timestamp": 4.255537195978418 + }, + { + "x": 7.94630282852553, + "y": 6.040403998758151, + "heading": 3.011837013902778, + "angularVelocity": 0.45255035690956674, + "velocityX": 0.37945863118162615, + "velocityY": 1.9500008102826474, + "timestamp": 4.333746851423001 + }, + { + "x": 7.913500037298791, + "y": 6.216302578213154, + "heading": 3.073900572334669, + "angularVelocity": 0.7935536613617653, + "velocityX": -0.41942124716278933, + "velocityY": 2.249064753643349, + "timestamp": 4.411956506867584 + }, + { + "x": 7.817295457201863, + "y": 6.41392635869847, + "heading": 3.157507299357982, + "angularVelocity": 1.0690077401319518, + "velocityX": -1.2300857170390516, + "velocityY": 2.5268463255837528, + "timestamp": 4.490166162312168 + }, + { + "x": 7.656224060083962, + "y": 6.629924805879377, + "heading": 3.2532317866303937, + "angularVelocity": 1.2239471805401183, + "velocityX": -2.059482249375602, + "velocityY": 2.76178747947505, + "timestamp": 4.568375817756751 + }, + { + "x": 7.428338063568708, + "y": 6.854853143948175, + "heading": 3.3364989443789064, + "angularVelocity": 1.0646659581247357, + "velocityX": -2.9137834097316344, + "velocityY": 2.875966359782434, + "timestamp": 4.646585473201334 + }, + { + "x": 7.1440021632714545, + "y": 7.060280821700023, + "heading": 3.3364993135305636, + "angularVelocity": 0.0000047200266382700315, + "velocityX": -3.6355600683949207, + "velocityY": 2.6266280881061843, + "timestamp": 4.724795128645917 + }, + { + "x": 6.825539959505244, + "y": 7.207349961467594, + "heading": 3.3364993910547764, + "angularVelocity": 9.91235834204802e-7, + "velocityX": -4.071903935082064, + "velocityY": 1.8804473556564736, + "timestamp": 4.803004784090501 + }, + { + "x": 6.4847801251228745, + "y": 7.290597931535019, + "heading": 3.3364995530878274, + "angularVelocity": 0.000002071778097721289, + "velocityX": -4.357004674746592, + "velocityY": 1.0644206216508802, + "timestamp": 4.881214439535084 + }, + { + "x": 6.145899111390599, + "y": 7.307772798176174, + "heading": 3.371414041514827, + "angularVelocity": 0.4464217138987176, + "velocityX": -4.3329818013633306, + "velocityY": 0.21960033634625267, + "timestamp": 4.959424094979667 + }, + { + "x": 5.850082833849242, + "y": 7.2728348025531115, + "heading": 3.406732202989882, + "angularVelocity": 0.4515831360500068, + "velocityX": -3.782349837239242, + "velocityY": -0.4467222803176537, + "timestamp": 5.03763375042425 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CBA123_2.3.traj b/src/main/deploy/choreo/CBA123_2.3.traj new file mode 100644 index 0000000..3c41373 --- /dev/null +++ b/src/main/deploy/choreo/CBA123_2.3.traj @@ -0,0 +1,346 @@ +{ + "samples": [ + { + "x": 5.850082833849242, + "y": 7.2728348025531115, + "heading": 3.406732202989882, + "angularVelocity": 0.4515831360500068, + "velocityX": -3.782349837239242, + "velocityY": -0.4467222803176537, + "timestamp": 0 + }, + { + "x": 5.606595892031962, + "y": 7.195869181059609, + "heading": 3.42245414187671, + "angularVelocity": 0.20102299131042597, + "velocityX": -3.1132593595148474, + "velocityY": -0.9840936014356655, + "timestamp": 0.07820965544458325 + }, + { + "x": 5.418067324322377, + "y": 7.081075803625522, + "heading": 3.4118514245412515, + "angularVelocity": -0.13556788193461983, + "velocityX": -2.4105536156359846, + "velocityY": -1.4677647763763115, + "timestamp": 0.1564193108891665 + }, + { + "x": 5.285696029663086, + "y": 6.930699348449707, + "heading": 3.3716790500485776, + "angularVelocity": -0.513649807869807, + "velocityX": -1.692518575958756, + "velocityY": -1.922735170242076, + "timestamp": 0.23462896633374974 + }, + { + "x": 5.211770963738827, + "y": 6.761432922153251, + "heading": 3.3070654449235826, + "angularVelocity": -0.8889516609228225, + "velocityX": -1.017058404496571, + "velocityY": -2.3287614195744344, + "timestamp": 0.30731413917871553 + }, + { + "x": 5.187822713728538, + "y": 6.5636194922724185, + "heading": 3.218656242633811, + "angularVelocity": -1.2163306329111174, + "velocityX": -0.3294791643595513, + "velocityY": -2.7215100705993875, + "timestamp": 0.3799993120236813 + }, + { + "x": 5.215169628706364, + "y": 6.338875849604383, + "heading": 3.1125853834185557, + "angularVelocity": -1.459319075177811, + "velocityX": 0.37623787503615364, + "velocityY": -3.0920149718485574, + "timestamp": 0.4526844848686471 + }, + { + "x": 5.295906684702748, + "y": 6.090585587432419, + "heading": 3.0013469191655093, + "angularVelocity": -1.5304147998700217, + "velocityX": 1.110777519489342, + "velocityY": -3.4159685181124213, + "timestamp": 0.5253696577136129 + }, + { + "x": 5.432252758121741, + "y": 5.8287501096854, + "heading": 2.9186116329618734, + "angularVelocity": -1.1382690989826274, + "velocityX": 1.875844385894381, + "velocityY": -3.602323107981056, + "timestamp": 0.5980548305585787 + }, + { + "x": 5.619462820953339, + "y": 5.565267386356212, + "heading": 2.911140590833299, + "angularVelocity": -0.10278632953807085, + "velocityX": 2.575629327193151, + "velocityY": -3.6249858535960753, + "timestamp": 0.6707400034035444 + }, + { + "x": 5.852735915847676, + "y": 5.337535645485744, + "heading": 2.911140489766906, + "angularVelocity": -0.0000013904678033080708, + "velocityX": 3.20936286953458, + "velocityY": -3.13312511970232, + "timestamp": 0.7434251762485102 + }, + { + "x": 6.115332523824382, + "y": 5.144348933291394, + "heading": 2.9111403928217827, + "angularVelocity": -0.0000013337675307223419, + "velocityX": 3.612794710371133, + "velocityY": -2.6578558546790183, + "timestamp": 0.816110349093476 + }, + { + "x": 6.377929653343084, + "y": 4.9511629300254425, + "heading": 2.9111402958765833, + "angularVelocity": -0.000001333768574825737, + "velocityX": 3.6128018857272206, + "velocityY": -2.657846101267551, + "timestamp": 0.8887955219384418 + }, + { + "x": 6.640526801778387, + "y": 4.757976952472771, + "heading": 2.9111401989313768, + "angularVelocity": -0.000001333768673260765, + "velocityX": 3.6128021459811346, + "velocityY": -2.6578457475051644, + "timestamp": 0.9614806947834076 + }, + { + "x": 6.905953257537394, + "y": 4.568697053971164, + "heading": 2.911140097019875, + "angularVelocity": -0.000001402094788599936, + "velocityX": 3.651727654622886, + "velocityY": -2.6041060520738193, + "timestamp": 1.0341658676283734 + }, + { + "x": 7.180063648762743, + "y": 4.431733581091024, + "heading": 2.8478362805193025, + "angularVelocity": -0.8709316360242585, + "velocityX": 3.7712009271823845, + "velocityY": -1.8843385455280675, + "timestamp": 1.1068510404733392 + }, + { + "x": 7.418521838834014, + "y": 4.334906711770402, + "heading": 2.727627639883835, + "angularVelocity": -1.653826164682401, + "velocityX": 3.2806992229335634, + "velocityY": -1.3321405938890518, + "timestamp": 1.179536213318305 + }, + { + "x": 7.61265483800419, + "y": 4.27560693688884, + "heading": 2.622104533916382, + "angularVelocity": -1.451783105648361, + "velocityX": 2.670874836938915, + "velocityY": -0.8158441750980653, + "timestamp": 1.2522213861632707 + }, + { + "x": 7.7619919385944485, + "y": 4.252060561408317, + "heading": 2.544047453790352, + "angularVelocity": -1.0739065076246306, + "velocityX": 2.0545744715884093, + "velocityY": -0.323950188998603, + "timestamp": 1.3249065590082365 + }, + { + "x": 7.866497658152248, + "y": 4.26345832148744, + "heading": 2.4991268437341323, + "angularVelocity": -0.6180161413667277, + "velocityX": 1.437785939928983, + "velocityY": 0.15680997420799672, + "timestamp": 1.3975917318532023 + }, + { + "x": 7.926192283630371, + "y": 4.309337139129639, + "heading": 2.49051536007312, + "angularVelocity": -0.11847648322141355, + "velocityX": 0.8212765154380016, + "velocityY": 0.6311991269533881, + "timestamp": 1.470276904698168 + }, + { + "x": 7.940348457773753, + "y": 4.392089087396041, + "heading": 2.5218533921200885, + "angularVelocity": 0.4211885410835337, + "velocityX": 0.19026141545325317, + "velocityY": 1.1122004186449108, + "timestamp": 1.5446807143705525 + }, + { + "x": 7.907090158619606, + "y": 4.510564365474328, + "heading": 2.5899769617808053, + "angularVelocity": 0.915592493995659, + "velocityX": -0.44699726130410805, + "velocityY": 1.592328115992432, + "timestamp": 1.619084524042937 + }, + { + "x": 7.825756439789389, + "y": 4.664661562939824, + "heading": 2.6895832213386424, + "angularVelocity": 1.338725261467457, + "velocityX": -1.0931391710766756, + "velocityY": 2.071092839788974, + "timestamp": 1.6934883337153215 + }, + { + "x": 7.695327922577603, + "y": 4.854090188986528, + "heading": 2.811653971659537, + "angularVelocity": 1.6406518813807833, + "velocityX": -1.7529817059918138, + "velocityY": 2.5459533171862905, + "timestamp": 1.767892143387706 + }, + { + "x": 7.513956823036736, + "y": 5.077399643654063, + "heading": 2.9354969579556736, + "angularVelocity": 1.664471037725669, + "velocityX": -2.4376587749939413, + "velocityY": 3.001317481602277, + "timestamp": 1.8422959530600904 + }, + { + "x": 7.288806907542069, + "y": 5.316870197892342, + "heading": 2.9491156101085165, + "angularVelocity": 0.18303702744266304, + "velocityX": -3.026053591691752, + "velocityY": 3.2185254396612124, + "timestamp": 1.9166997627324749 + }, + { + "x": 7.027854499811058, + "y": 5.524877943910136, + "heading": 2.949115709026995, + "angularVelocity": 0.0000013294813606110081, + "velocityX": -3.507245245640513, + "velocityY": 2.795659885343188, + "timestamp": 1.9911035724048594 + }, + { + "x": 6.766901538587652, + "y": 5.732884995552106, + "heading": 2.9491158079453674, + "angularVelocity": 0.000001329479944148138, + "velocityX": -3.507252684673519, + "velocityY": 2.7956505528126785, + "timestamp": 2.065507382077244 + }, + { + "x": 6.505948577360842, + "y": 5.940892047189807, + "heading": 2.949115906863738, + "angularVelocity": 0.0000013294799098389838, + "velocityX": -3.5072526847192558, + "velocityY": 2.7956505527553035, + "timestamp": 2.1399111917496283 + }, + { + "x": 6.244995616133647, + "y": 6.148899098827026, + "heading": 2.949116005782105, + "angularVelocity": 0.0000013294798761008448, + "velocityX": -3.5072526847244223, + "velocityY": 2.7956505527488273, + "timestamp": 2.2143150014220128 + }, + { + "x": 5.9840425923910745, + "y": 6.356906072035521, + "heading": 2.9491161047018837, + "angularVelocity": 0.0000013294988413652688, + "velocityX": -3.50725352494185, + "velocityY": 2.795649498653279, + "timestamp": 2.2887188110943972 + }, + { + "x": 5.7499825511088805, + "y": 6.541544824622768, + "heading": 3.0464017069157494, + "angularVelocity": 1.3075352275943224, + "velocityX": -3.145807214883363, + "velocityY": 2.4815765939976933, + "timestamp": 2.3631226207667817 + }, + { + "x": 5.563295634077935, + "y": 6.689913978052536, + "heading": 3.176337898527034, + "angularVelocity": 1.7463647652374488, + "velocityX": -2.509104276420351, + "velocityY": 1.9941069426830298, + "timestamp": 2.4375264304391653 + }, + { + "x": 5.424144731034325, + "y": 6.80085426856286, + "heading": 3.2842877383245876, + "angularVelocity": 1.4508644150464807, + "velocityX": -1.8702120718861284, + "velocityY": 1.4910565870056691, + "timestamp": 2.5119302401115498 + }, + { + "x": 5.331760336370482, + "y": 6.874690623270579, + "heading": 3.3599442811717215, + "angularVelocity": 1.0168369493479672, + "velocityX": -1.2416621550782305, + "velocityY": 0.9923733076684759, + "timestamp": 2.586334049783934 + }, + { + "x": 5.285696029663086, + "y": 6.91156530380249, + "heading": 3.3989154646935043, + "angularVelocity": 0.5237794098632975, + "velocityX": -0.6191122055473569, + "velocityY": 0.4956020490654649, + "timestamp": 2.6607378594563187 + }, + { + "x": 5.285696029663086, + "y": 6.91156530380249, + "heading": 3.3989154646935043, + "angularVelocity": 6.118242255374946e-35, + "velocityX": -2.3977836939513236e-35, + "velocityY": -5.304923927286292e-35, + "timestamp": 2.735141669128703 + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java new file mode 100644 index 0000000..0815e5a --- /dev/null +++ b/src/main/java/frc/robot/Autos.java @@ -0,0 +1,222 @@ +package frc.robot; + +import java.util.List; + +import com.pathplanner.lib.path.PathPlannerPath; +import com.techhounds.houndutil.houndauto.AutoRoutine; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants.Intake.IntakePosition; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.ShooterTilt; + +public class Autos { + + public static AutoRoutine autoA123(Drivetrain drivetrain, Intake intake, Shooter shooter, ShooterTilt shooterTilt) { + PathPlannerPath pathStartToA = PathPlannerPath.fromChoreoTrajectory("A123.1"); + PathPlannerPath pathATo1 = PathPlannerPath.fromChoreoTrajectory("A123.2"); + PathPlannerPath path1ToScore = PathPlannerPath.fromChoreoTrajectory("A123.3"); + PathPlannerPath pathScoreTo2 = PathPlannerPath.fromChoreoTrajectory("A123.4"); + PathPlannerPath path2ToScore = PathPlannerPath.fromChoreoTrajectory("A123.5"); + PathPlannerPath pathScoreTo3 = PathPlannerPath.fromChoreoTrajectory("A123.6"); + PathPlannerPath path3ToScore = PathPlannerPath.fromChoreoTrajectory("A123.7"); + + Command command = Commands.parallel( + Commands.sequence( + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt) + .alongWith(intake.moveToPositionCommand(() -> IntakePosition.GROUND)), + drivetrain.followPathCommand(pathStartToA).alongWith(intake.intakeNoteAutoCommand()), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathATo1).alongWith(intake.intakeNoteAutoCommand()), + drivetrain.followPathCommand(path1ToScore), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathScoreTo2).alongWith(intake.intakeNoteAutoCommand()), + drivetrain.followPathCommand(path2ToScore), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathScoreTo3).alongWith(intake.intakeNoteAutoCommand()), + drivetrain.followPathCommand(path3ToScore), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt)), + shooterTilt.targetSpeakerCommand(drivetrain::getPose)); + + return new AutoRoutine("A123", command, + List.of(pathStartToA, pathATo1, path1ToScore, pathScoreTo2, path2ToScore, pathScoreTo3, path3ToScore), + new Pose2d(1.775, 6.012, new Rotation2d(-2.648))); + } + + public static AutoRoutine autoBA123(Drivetrain drivetrain, Intake intake, Shooter shooter, + ShooterTilt shooterTilt) { + PathPlannerPath pathStartToB = PathPlannerPath.fromChoreoTrajectory("BA123.1"); + PathPlannerPath pathBToA = PathPlannerPath.fromChoreoTrajectory("BA123.2"); + PathPlannerPath pathATo1 = PathPlannerPath.fromChoreoTrajectory("BA123.3"); + PathPlannerPath path1ToScore = PathPlannerPath.fromChoreoTrajectory("BA123.4"); + PathPlannerPath pathScoreTo2 = PathPlannerPath.fromChoreoTrajectory("BA123.5"); + PathPlannerPath path2ToScore = PathPlannerPath.fromChoreoTrajectory("BA123.6"); + PathPlannerPath pathScoreTo3 = PathPlannerPath.fromChoreoTrajectory("BA123.7"); + PathPlannerPath path3ToScore = PathPlannerPath.fromChoreoTrajectory("BA123.8"); + + Pose2d startingPose = pathStartToB.getPreviewStartingHolonomicPose(); + + Command command = Commands.parallel( + Commands.sequence( + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt) + .alongWith(intake.moveToPositionCommand(() -> IntakePosition.GROUND)), + drivetrain.followPathCommand(pathStartToB).alongWith(intake.intakeNoteAutoCommand()), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathBToA).alongWith(intake.intakeNoteAutoCommand()), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathATo1).alongWith(intake.intakeNoteAutoCommand()), + drivetrain.followPathCommand(path1ToScore), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathScoreTo2).alongWith(intake.intakeNoteAutoCommand()), + drivetrain.followPathCommand(path2ToScore), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathScoreTo3).alongWith(intake.intakeNoteAutoCommand()), + drivetrain.followPathCommand(path3ToScore), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt)), + shooterTilt.targetSpeakerCommand(drivetrain::getPose)); + + return new AutoRoutine("BA123", command, + List.of(pathStartToB, pathBToA, pathATo1, path1ToScore, pathScoreTo2, path2ToScore, pathScoreTo3, + path3ToScore), + new Pose2d(startingPose.getX(), startingPose.getY(), new Rotation2d(Math.PI))); + } + + public static AutoRoutine autoCBA123(Drivetrain drivetrain, Intake intake, Shooter shooter, + ShooterTilt shooterTilt) { + PathPlannerPath pathStartToC = PathPlannerPath.fromChoreoTrajectory("CBA123.1"); + PathPlannerPath pathCToB = PathPlannerPath.fromChoreoTrajectory("CBA123.2"); + PathPlannerPath pathBToA = PathPlannerPath.fromChoreoTrajectory("CBA123.3"); + PathPlannerPath pathATo1 = PathPlannerPath.fromChoreoTrajectory("CBA123.4"); + PathPlannerPath path1ToScore = PathPlannerPath.fromChoreoTrajectory("CBA123.5"); + PathPlannerPath pathScoreTo2 = PathPlannerPath.fromChoreoTrajectory("CBA123.6"); + PathPlannerPath path2ToScore = PathPlannerPath.fromChoreoTrajectory("CBA123.7"); + PathPlannerPath pathScoreTo3 = PathPlannerPath.fromChoreoTrajectory("CBA123.8"); + PathPlannerPath path3ToScore = PathPlannerPath.fromChoreoTrajectory("CBA123.9"); + + Pose2d startingPose = pathStartToC.getPreviewStartingHolonomicPose(); + + Command command = Commands.parallel( + Commands.sequence( + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt) + .alongWith(intake.moveToPositionCommand(() -> IntakePosition.GROUND)), + drivetrain.followPathCommand(pathStartToC).alongWith(intake.intakeNoteAutoCommand()), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathCToB).alongWith(intake.intakeNoteAutoCommand()), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathBToA).alongWith(intake.intakeNoteAutoCommand()), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathATo1).alongWith(intake.intakeNoteAutoCommand()), + drivetrain.followPathCommand(path1ToScore), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathScoreTo2).alongWith(intake.intakeNoteAutoCommand()), + drivetrain.followPathCommand(path2ToScore), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathScoreTo3).alongWith(intake.intakeNoteAutoCommand()), + drivetrain.followPathCommand(path3ToScore), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt)), + shooterTilt.targetSpeakerCommand(drivetrain::getPose)); + + return new AutoRoutine("CBA123", command, + List.of(pathStartToC, pathCToB, pathBToA, pathATo1, path1ToScore, pathScoreTo2, path2ToScore, + pathScoreTo3, + path3ToScore), + new Pose2d(startingPose.getX(), startingPose.getY(), new Rotation2d(2.62276))); + } + + public static AutoRoutine auto453(Drivetrain drivetrain, Intake intake, Shooter shooter, ShooterTilt shooterTilt) { + PathPlannerPath pathStartTo4 = PathPlannerPath.fromChoreoTrajectory("453.1"); + PathPlannerPath path4ToScore = PathPlannerPath.fromChoreoTrajectory("453.2"); + PathPlannerPath pathScoreTo5 = PathPlannerPath.fromChoreoTrajectory("453.3"); + PathPlannerPath path5ToScore = PathPlannerPath.fromChoreoTrajectory("453.4"); + PathPlannerPath pathScoreTo3 = PathPlannerPath.fromChoreoTrajectory("453.5"); + PathPlannerPath path3ToScore = PathPlannerPath.fromChoreoTrajectory("453.6"); + + Pose2d startingPose = pathStartTo4.getPreviewStartingHolonomicPose(); + + Command command = Commands.parallel( + Commands.sequence( + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt) + .alongWith(intake.moveToPositionCommand(() -> IntakePosition.GROUND)), + drivetrain.followPathCommand(pathStartTo4).alongWith(intake.intakeNoteAutoCommand()), + drivetrain.followPathCommand(path4ToScore), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathScoreTo5).alongWith(intake.intakeNoteAutoCommand()), + drivetrain.followPathCommand(path5ToScore), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathScoreTo3).alongWith(intake.intakeNoteAutoCommand()), + drivetrain.followPathCommand(path3ToScore), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt)), + shooterTilt.targetSpeakerCommand(drivetrain::getPose)); + + return new AutoRoutine("453", command, + List.of(pathStartTo4, path4ToScore, pathScoreTo5, path5ToScore, pathScoreTo3, path3ToScore), + new Pose2d(startingPose.getX(), startingPose.getY(), new Rotation2d(1.986288))); + } + + public static AutoRoutine auto1234(Drivetrain drivetrain, Intake intake, Shooter shooter, ShooterTilt shooterTilt) { + PathPlannerPath pathStartTo1 = PathPlannerPath.fromChoreoTrajectory("1234.1"); + PathPlannerPath path1ToScore = PathPlannerPath.fromChoreoTrajectory("1234.2"); + PathPlannerPath pathScoreTo2 = PathPlannerPath.fromChoreoTrajectory("1234.3"); + PathPlannerPath path2ToScore = PathPlannerPath.fromChoreoTrajectory("1234.4"); + PathPlannerPath pathScoreTo3 = PathPlannerPath.fromChoreoTrajectory("1234.5"); + PathPlannerPath path3ToScore = PathPlannerPath.fromChoreoTrajectory("1234.6"); + PathPlannerPath pathScoreTo4 = PathPlannerPath.fromChoreoTrajectory("1234.7"); + PathPlannerPath path4ToScore = PathPlannerPath.fromChoreoTrajectory("1234.8"); + + Pose2d startingPose = pathStartTo1.getPreviewStartingHolonomicPose(); + + Command command = Commands.parallel( + Commands.sequence( + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt) + .alongWith(intake.moveToPositionCommand(() -> IntakePosition.GROUND)), + drivetrain.followPathCommand(pathStartTo1).alongWith(intake.intakeNoteAutoCommand()), + drivetrain.followPathCommand(path1ToScore), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathScoreTo2).alongWith(intake.intakeNoteAutoCommand()), + drivetrain.followPathCommand(path2ToScore), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathScoreTo3).alongWith(intake.intakeNoteAutoCommand()), + drivetrain.followPathCommand(path3ToScore), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathScoreTo4).alongWith(intake.intakeNoteAutoCommand()), + drivetrain.followPathCommand(path4ToScore), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt) + + ), + shooterTilt.targetSpeakerCommand(drivetrain::getPose)); + + return new AutoRoutine("1234", command, + List.of(pathStartTo1, path1ToScore, pathScoreTo2, path2ToScore, pathScoreTo3, path3ToScore, + pathScoreTo4, path4ToScore), + new Pose2d(startingPose.getX(), startingPose.getY(), new Rotation2d(-2.32049))); + } + + public static AutoRoutine autoBAC(Drivetrain drivetrain, Intake intake, Shooter shooter, ShooterTilt shooterTilt) { + PathPlannerPath pathStartToB = PathPlannerPath.fromChoreoTrajectory("BAC.1"); + PathPlannerPath pathBToC = PathPlannerPath.fromChoreoTrajectory("BAC.2"); + PathPlannerPath pathCToA = PathPlannerPath.fromChoreoTrajectory("BAC.3"); + + Pose2d startingPose = pathStartToB.getPreviewStartingHolonomicPose(); + + Command command = Commands.parallel( + Commands.sequence( + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt) + .alongWith(intake.moveToPositionCommand(() -> IntakePosition.GROUND)), + drivetrain.followPathCommand(pathStartToB).alongWith(intake.intakeNoteAutoCommand()), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathBToC).alongWith(intake.intakeNoteAutoCommand()), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt), + drivetrain.followPathCommand(pathCToA).alongWith(intake.intakeNoteAutoCommand()), + RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt)), + shooterTilt.targetSpeakerCommand(drivetrain::getPose)); + + return new AutoRoutine("BAC", command, + List.of(pathStartToB, pathBToC, pathCToA), + new Pose2d(startingPose.getX(), startingPose.getY(), new Rotation2d(Math.PI))); + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..3a202c2 --- /dev/null +++ b/src/main/java/frc/robot/Constants.java @@ -0,0 +1,579 @@ +package frc.robot; + +import com.techhounds.houndutil.houndlib.AprilTagPhotonCamera.PhotonCameraConstants; +import com.techhounds.houndutil.houndlib.leds.BaseLEDSection; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; + +import com.techhounds.houndutil.houndlib.swerve.CoaxialSwerveModule.SwerveConstants; +import edu.wpi.first.math.trajectory.TrapezoidProfile; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.system.plant.DCMotor; + +public class Constants { + enum ControllerType { + XboxController, + FlightStick + } + + public static final boolean DEBUG_MODE = false; + + public static final ControllerType CONTROLLER_TYPE = ControllerType.FlightStick; + + public static final class Drivetrain { + public static final int FRONT_LEFT_DRIVE_MOTOR_ID = 1; + public static final int FRONT_LEFT_STEER_MOTOR_ID = 2; + public static final int FRONT_RIGHT_DRIVE_MOTOR_ID = 3; + public static final int FRONT_RIGHT_STEER_MOTOR_ID = 4; + public static final int BACK_LEFT_DRIVE_MOTOR_ID = 5; + public static final int BACK_LEFT_STEER_MOTOR_ID = 6; + public static final int BACK_RIGHT_DRIVE_MOTOR_ID = 7; + public static final int BACK_RIGHT_STEER_MOTOR_ID = 8; + + public static final int FRONT_LEFT_STEER_ENCODER_ID = 0; + public static final int FRONT_RIGHT_STEER_ENCODER_ID = 1; + public static final int BACK_LEFT_STEER_ENCODER_ID = 2; + public static final int BACK_RIGHT_STEER_ENCODER_ID = 3; + + public static final String CAN_BUS_NAME = "canivore"; + + public static final int PIGEON_ID = 0; + + public static final boolean DRIVE_MOTORS_INVERTED = false; + public static final boolean STEER_MOTORS_INVERTED = true; + public static final boolean STEER_CANCODERS_INVERTED = true; + + // TODO + public static final double FRONT_LEFT_OFFSET = -0.001220703125; + public static final double FRONT_RIGHT_OFFSET = 0.201171875; + public static final double BACK_LEFT_OFFSET = 0.2403; + public static final double BACK_RIGHT_OFFSET = -0.4331; + + /** Distance between left and right wheels. */ + public static final double TRACK_WIDTH_METERS = 0.52832; + /** Distance between front and back wheels. */ + public static final double WHEEL_BASE_METERS = 0.52832; + + public static final SwerveConstants SWERVE_CONSTANTS = new SwerveConstants(); + static { + SWERVE_CONSTANTS.DRIVE_kP = 2.6794 * (2.0 * Math.PI * 0.0478); // TODO simvalue + SWERVE_CONSTANTS.DRIVE_kI = 0.0; + SWERVE_CONSTANTS.DRIVE_kD = 0.0; + SWERVE_CONSTANTS.DRIVE_kS = 0.0097954 * (2.0 * Math.PI * 0.0478); // TODO simvalue + SWERVE_CONSTANTS.DRIVE_kV = 2.1268 * (2.0 * Math.PI * 0.0478); + SWERVE_CONSTANTS.DRIVE_kA = 0.26277 * (2.0 * Math.PI * 0.0478); + SWERVE_CONSTANTS.STEER_kP = 40.0; // TODO simvalue + SWERVE_CONSTANTS.STEER_kI = 0.0; + SWERVE_CONSTANTS.STEER_kD = 1.0; // TODO simvalue + + SWERVE_CONSTANTS.DRIVE_GEARING = 5.357; + SWERVE_CONSTANTS.STEER_GEARING = 150.0 / 7.0; + SWERVE_CONSTANTS.WHEEL_CIRCUMFERENCE = 2.0 * Math.PI * 0.0478; // 0.3003362577 + SWERVE_CONSTANTS.DRIVE_ENCODER_ROTATIONS_TO_METERS = SWERVE_CONSTANTS.WHEEL_CIRCUMFERENCE + / SWERVE_CONSTANTS.DRIVE_GEARING; + SWERVE_CONSTANTS.STEER_ENCODER_ROTATIONS_TO_RADIANS = 2 * Math.PI + / SWERVE_CONSTANTS.STEER_GEARING; + + // 64.4 + SWERVE_CONSTANTS.MAX_DRIVING_VELOCITY_METERS_PER_SECOND = 5.583; // TODO simvalue + SWERVE_CONSTANTS.MAX_DRIVING_ACCELERATION_METERS_PER_SECOND_SQUARED = 19.3416549959; // TODO simvalue + SWERVE_CONSTANTS.MAX_STEER_VELOCITY_RADIANS_PER_SECOND = 100 * 2 * Math.PI; // TODO simvalue + // max velocity in 1/3 sec + SWERVE_CONSTANTS.MAX_STEER_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 3 * 100 * 2 * Math.PI; // TODO simvalue + + SWERVE_CONSTANTS.DRIVE_CURRENT_LIMIT = 50; // TODO simvalue + SWERVE_CONSTANTS.STEER_CURRENT_LIMIT = 20; // TODO simvalue + SWERVE_CONSTANTS.DRIVE_GEARBOX_REPR = DCMotor.getKrakenX60(1); + SWERVE_CONSTANTS.STEER_GEARBOX_REPR = DCMotor.getKrakenX60(1); + SWERVE_CONSTANTS.DRIVE_MOI = 0.04; // TODO simvalue + SWERVE_CONSTANTS.STEER_MOI = 0.025; // TODO simvalue + } + + public static final double MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND = 20 * Math.PI; // TODO + public static final double MAX_ANGULAR_ACCELERATION_RADIANS_PER_SECOND_SQUARED = 20 * Math.PI; // TODO + + public static final Translation2d[] SWERVE_MODULE_LOCATIONS = new Translation2d[] { + new Translation2d(WHEEL_BASE_METERS / 2, TRACK_WIDTH_METERS / 2), + new Translation2d(WHEEL_BASE_METERS / 2, -TRACK_WIDTH_METERS / 2), + new Translation2d(-WHEEL_BASE_METERS / 2, TRACK_WIDTH_METERS / 2), + new Translation2d(-WHEEL_BASE_METERS / 2, -TRACK_WIDTH_METERS / 2) }; + + public static final SwerveDriveKinematics KINEMATICS = new SwerveDriveKinematics( + SWERVE_MODULE_LOCATIONS[0], + SWERVE_MODULE_LOCATIONS[1], + SWERVE_MODULE_LOCATIONS[2], + SWERVE_MODULE_LOCATIONS[3]); + + public static final double XY_kP = 1; // TODO + public static final double XY_kI = 0; // TODO + public static final double XY_kD = 0; // TODO + public static final TrapezoidProfile.Constraints XY_CONSTRAINTS = new TrapezoidProfile.Constraints( + SWERVE_CONSTANTS.MAX_DRIVING_VELOCITY_METERS_PER_SECOND, + SWERVE_CONSTANTS.MAX_DRIVING_ACCELERATION_METERS_PER_SECOND_SQUARED); + + public static final double THETA_kP = 1.5; // TODO + public static final double THETA_kI = 0; // TODO + public static final double THETA_kD = 0.1; // TODO + public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND, MAX_ANGULAR_ACCELERATION_RADIANS_PER_SECOND_SQUARED); + + public static final double PATH_FOLLOWING_TRANSLATION_kP = 5; // TODO simvalue + public static final double PATH_FOLLOWING_ROTATION_kP = 1; // TODO simvalue + } + + public static final class Intake { + public static enum IntakePosition { + GROUND(Units.degreesToRadians(-35)), // TODO simvalue + AMP(Units.degreesToRadians(60)), // TODO simvalue + STOW(Units.degreesToRadians(95)); // TODO simvalue + + public final double value; + + private IntakePosition(double value) { + this.value = value; + } + } + + public static final int PRIMARY_ARM_MOTOR_ID = 9; + public static final int SECONDARY_ARM_MOTOR_ID = 10; + public static final int ROLLER_MOTOR_ID = 11; + + public static final int INTAKE_BEAM_ID = 0; + public static final int PRIMARY_SHOOTER_BEAM_ID = 1; + public static final int SECONDARY_SHOOTER_BEAM_ID = 2; + + public static final DCMotor MOTOR_GEARBOX_REPR = DCMotor.getNeoVortex(2); + public static final double GEARING = 36.0; + public static final double LENGTH_METERS = 0.23; // TODO simvalue + public static final double MASS_KG = 4.082; // TODO simvalue + public static final double MOMENT_OF_INERTIA_KG_METERS_SQUARED = SingleJointedArmSim.estimateMOI( + LENGTH_METERS, + MASS_KG); + + public static final double MIN_ANGLE_RADIANS = Units.degreesToRadians(-35); // TODO simvalue + public static final double MAX_ANGLE_RADIANS = Units.degreesToRadians(100); // TODO simvalue + + public static final double ENCODER_ROTATIONS_TO_RADIANS = 2 * Math.PI / GEARING; + public static final int ARM_CURRENT_LIMIT = 20; // TODO simvalue + public static final int ROLLER_CURRENT_LIMIT = 10; // TODO simvalue + + public static final double kP = 33.03; // TODO simvalue + public static final double kI = 0; + public static final double kD = 0.834; + public static final double kS = 0; + public static final double kG = 0.21316; // TODO simvalue + public static final double kV = 0.59658; // TODO simvalue + public static final double kA = 0.019195; // TODO simvalue + public static final double TOLERANCE = 0.01; + + public static final double MAX_VELOCITY_METERS_PER_SECOND = 7.5 * Math.PI; // TODO simvalue + public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 7.5 * 5 * Math.PI; // TODO simvalue + public static final TrapezoidProfile.Constraints MOVEMENT_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_VELOCITY_METERS_PER_SECOND, MAX_ACCELERATION_METERS_PER_SECOND_SQUARED); + + public static final Pose3d BASE_COMPONENT_POSE = new Pose3d(-0.19, 0, 0.299, + new Rotation3d(0, -Units.degreesToRadians(35), Math.PI)); + + } + + public static final class Shooter { + public static final int PRIMARY_MOTOR_ID = 12; + public static final int SECONDARY_MOTOR_ID = 13; + + public static final DCMotor MOTOR_GEARBOX_REPR = DCMotor.getNeoVortex(2); + public static final double GEARING = 1.0; + public static final double WHEEL_AXLE_MASS = Units.lbsToKilograms(2.5); + public static final double WHEEL_RADIUS = Units.inchesToMeters(2); + // 3lb, 2in radius, 1/2mr^2 + public static final double MOMENT_OF_INERTIA_KG_METERS_SQUARED = (1.0 / 2.0) * WHEEL_AXLE_MASS + * Math.pow(WHEEL_RADIUS, 2); + public static final int CURRENT_LIMIT = 40; + + public static final double IDLE_RPS = 8; + public static final double SHOOTING_RPS = 70; + + public static final double kP = 0.5; // TODO simvalue + public static final double kI = 0; + public static final double kD = 0; + public static final double kS = 0.011075; // TODO simvalue + public static final double kV = 0.10431; // TODO simvalue + public static final double kA = 0.036933; // TODO simvalue + public static final double TOLERANCE = 0.05; + + public static final double MAX_VELOCITY_METERS_PER_SECOND = 6600; // TODO simvalue + public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 1000; // TODO simvalue + public static final TrapezoidProfile.Constraints MOVEMENT_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_VELOCITY_METERS_PER_SECOND, MAX_ACCELERATION_METERS_PER_SECOND_SQUARED); + } + + public static final class ShooterTilt { + public static enum ShooterTiltPosition { + BOTTOM(Units.degreesToRadians(25)), // TODO simvalue + STOW(Units.degreesToRadians(80)); // TODO simvalue + + public final double value; + + private ShooterTiltPosition(double value) { + this.value = value; + } + } + + public static final int MOTOR_ID = 14; + + public static final DCMotor MOTOR_GEARBOX_REPR = DCMotor.getNeoVortex(1); + public static final double GEARING = 16; + public static final double MASS_KG = Units.lbsToKilograms(12); // TODO simvalue + // 1 rot = 12mm + public static final double ENCODER_ROTATIONS_TO_METERS = Units.inchesToMeters(0.5); + + public static final double MIN_HEIGHT_METERS = 0; // 9.921in // TODO simvalue + public static final double MAX_HEIGHT_METERS = 0.1445; // TODO simvalue + + public static final double MIN_ANGLE_RADIANS = Units.degreesToRadians(22.0097); // TODO simvalue + public static final double MAX_ANGLE_RADIANS = Units.degreesToRadians(72.4199); // TODO simvalue + + public static final int CURRENT_LIMIT = 40; + + public static final double kP = 1000; // TODO simvalue + public static final double kI = 0; + public static final double kD = 0; + public static final double kS = 0; + public static final double kG = 0.022466; // TODO simvalue + public static final double kV = 131.43; // TODO simvalue + public static final double kA = 6.5761; // TODO simvalue + public static final double TOLERANCE = 0.01; + + public static final double MAX_VELOCITY_METERS_PER_SECOND = 0.1; // TODO simvalue + public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 0.5; // TODO simvalue + public static final TrapezoidProfile.Constraints MOVEMENT_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_VELOCITY_METERS_PER_SECOND, MAX_ACCELERATION_METERS_PER_SECOND_SQUARED); + + public static final Pose3d BASE_SHOOTER_POSE = new Pose3d(-0.19, 0, 0.299, new Rotation3d(0, 0, 0)); + // public static final Pose3d BASE_OUTER_LEAD_SCREW_POSE = new Pose3d(1, 1, 1, + // new Rotation3d(0, 0, 0)); + public static final Pose3d BASE_OUTER_LEAD_SCREW_POSE = new Pose3d(-0.0915, + 0, 0.125, + new Rotation3d(0, 0, 0)); + + public static final Transform3d OUTER_LEAD_SCREW_TO_INNER_LEAD_SCREW = new Transform3d(0.047, 0, 0.0127, + new Rotation3d()); + public static final Transform3d LEAD_SCREW_PIVOT_TO_EXTENSION = new Transform3d(0, 0, 0.0127, new Rotation3d()); + public static final Transform3d SHOOTER_PIVOT_TO_TOP_LEAD_SCREW_PIVOT = new Transform3d(0.2572, 0, -0.0984, + new Rotation3d()); + public static final double INITIAL_LEAD_SCREW_LENGTH = 0.2519; + + public static final Transform3d SHOOTER_PIVOT_TO_BOTTOM_LEAD_SCREW_PIVOT = new Transform3d(0.09906, 0, -0.1737, + new Rotation3d()); + + public static final double getLinearActuatorLength(double angle) { + double shooterToBottomLeadScrewAngle = Math.atan(Math.abs(SHOOTER_PIVOT_TO_BOTTOM_LEAD_SCREW_PIVOT.getX()) + / Math.abs(SHOOTER_PIVOT_TO_BOTTOM_LEAD_SCREW_PIVOT.getZ())); + double shooterToTopLeadScrewAngle = Math.atan(Math.abs(SHOOTER_PIVOT_TO_TOP_LEAD_SCREW_PIVOT.getZ()) + / Math.abs(SHOOTER_PIVOT_TO_TOP_LEAD_SCREW_PIVOT.getX())); + + double shooterInteriorAngle = angle - shooterToBottomLeadScrewAngle - shooterToTopLeadScrewAngle + + Math.PI / 2.0; + + double shooterPivotToBottomPivot = SHOOTER_PIVOT_TO_BOTTOM_LEAD_SCREW_PIVOT.getTranslation().getNorm(); // a + double shooterPivotToTopPivot = SHOOTER_PIVOT_TO_TOP_LEAD_SCREW_PIVOT.getTranslation().getNorm(); // b + + return Math.sqrt( + Math.pow(shooterPivotToBottomPivot, 2) + - (2 * shooterPivotToBottomPivot * shooterPivotToTopPivot * Math.cos(shooterInteriorAngle)) + + Math.pow(shooterPivotToTopPivot, 2) - Math.pow(LEAD_SCREW_PIVOT_TO_EXTENSION.getZ(), 2)) + - INITIAL_LEAD_SCREW_LENGTH; + } + + // https://www.desmos.com/calculator/7ojeknrpiq + public static final double getShooterAngle(double linearActuatorLength) { + double fullLeadScrewLength = INITIAL_LEAD_SCREW_LENGTH + linearActuatorLength; + + double shooterPivotToBottomPivot = SHOOTER_PIVOT_TO_BOTTOM_LEAD_SCREW_PIVOT.getTranslation().getNorm(); // a + double shooterPivotToTopPivot = SHOOTER_PIVOT_TO_TOP_LEAD_SCREW_PIVOT.getTranslation().getNorm(); // b + double leadScrewHyp = Math + .sqrt(Math.pow(LEAD_SCREW_PIVOT_TO_EXTENSION.getZ(), 2) + Math.pow(fullLeadScrewLength, 2)); // c + + double shooterInteriorAngle = Math.acos( + (Math.pow(shooterPivotToBottomPivot, 2) + + Math.pow(shooterPivotToTopPivot, 2) + - Math.pow(leadScrewHyp, 2)) + / (2 * shooterPivotToBottomPivot * shooterPivotToTopPivot)); + + double shooterToBottomLeadScrewAngle = Math.atan(Math.abs(SHOOTER_PIVOT_TO_BOTTOM_LEAD_SCREW_PIVOT.getX()) + / Math.abs(SHOOTER_PIVOT_TO_BOTTOM_LEAD_SCREW_PIVOT.getZ())); + double shooterToTopLeadScrewAngle = Math.atan(Math.abs(SHOOTER_PIVOT_TO_TOP_LEAD_SCREW_PIVOT.getZ()) + / Math.abs(SHOOTER_PIVOT_TO_TOP_LEAD_SCREW_PIVOT.getX())); + + return shooterInteriorAngle + shooterToBottomLeadScrewAngle + shooterToTopLeadScrewAngle - Math.PI / 2.0; + } + + // used only in simulation + public static final double getLeadScrewAngle(double linearActuatorLength) { + double fullLeadScrewLength = INITIAL_LEAD_SCREW_LENGTH + linearActuatorLength; + + double shooterPivotToBottomPivot = SHOOTER_PIVOT_TO_BOTTOM_LEAD_SCREW_PIVOT.getTranslation().getNorm(); // a + double shooterPivotToTopPivot = SHOOTER_PIVOT_TO_TOP_LEAD_SCREW_PIVOT.getTranslation().getNorm(); // b + double leadScrewHyp = Math + .sqrt(Math.pow(LEAD_SCREW_PIVOT_TO_EXTENSION.getZ(), 2) + Math.pow(fullLeadScrewLength, 2)); // c + + double leadScrewInteriorAngle = Math.acos( + (Math.pow(shooterPivotToBottomPivot, 2) + + Math.pow(leadScrewHyp, 2) + - Math.pow(shooterPivotToTopPivot, 2)) + / (2 * shooterPivotToBottomPivot * leadScrewHyp)); + + double bottomLeadScrewToShooterAngle = Math.atan(Math.abs(SHOOTER_PIVOT_TO_BOTTOM_LEAD_SCREW_PIVOT.getZ()) + / Math.abs(SHOOTER_PIVOT_TO_BOTTOM_LEAD_SCREW_PIVOT.getX())); + + return Math.PI - leadScrewInteriorAngle - bottomLeadScrewToShooterAngle; + } + + // TODO i need to actually make sure these are right + public static final double LEAD_SCREW_RADIUS_METERS = Units.inchesToMeters(0.613); // ? + public static final double SHOOTER_PIVOT_RADIUS_METERS = Units.inchesToMeters(1.625); // ? + public static final double SHOOTER_OFFSET_ANGLE_RADIANS = 1337; + public static final double ANGLE_ALPHA = 1337; // ? + + public static final double BOTTOM_PIVOT_TO_TOP_PIVOT_LENGTH_METERS = Units.inchesToMeters(7.843); + public static final double SHOOTER_PIVOT_TO_ENDPOINT_PIVOT_LENGTH_METERS = Units.inchesToMeters(10.725); + + public static final double LEAD_SCREW_MIN_LENGTH_METERS = 1337; + public static final double HORIZONTAL_ANGLE_OFFSET_RADIANS = 1337; + + /* + * By treating the shooter + shooter tilt as a triangle with vertices at the 3 + * pivot points, we can use trig to get the shooter's approximate angle based on + * lead screw length + * units are radians and meters + */ + + public double getAngleFromLength(double length) { + length += LEAD_SCREW_MIN_LENGTH_METERS; + double theta = Math.acos((LEAD_SCREW_RADIUS_METERS * LEAD_SCREW_RADIUS_METERS + + length * length + - SHOOTER_PIVOT_TO_ENDPOINT_PIVOT_LENGTH_METERS * SHOOTER_PIVOT_TO_ENDPOINT_PIVOT_LENGTH_METERS - + BOTTOM_PIVOT_TO_TOP_PIVOT_LENGTH_METERS * BOTTOM_PIVOT_TO_TOP_PIVOT_LENGTH_METERS) + / (-2 * SHOOTER_PIVOT_TO_ENDPOINT_PIVOT_LENGTH_METERS * BOTTOM_PIVOT_TO_TOP_PIVOT_LENGTH_METERS)); + theta += HORIZONTAL_ANGLE_OFFSET_RADIANS; // to include or not to include... + return theta; + } + + /* + * The inverse equation of the method above + * units are meters and radians + */ + public double getLengthFromAngle(double angle) { + angle += HORIZONTAL_ANGLE_OFFSET_RADIANS; // angle to the horizontal + that offset that we need + double length = Math.sqrt(-2 * BOTTOM_PIVOT_TO_TOP_PIVOT_LENGTH_METERS + * SHOOTER_PIVOT_TO_ENDPOINT_PIVOT_LENGTH_METERS * Math.cos(angle) + + BOTTOM_PIVOT_TO_TOP_PIVOT_LENGTH_METERS * BOTTOM_PIVOT_TO_TOP_PIVOT_LENGTH_METERS + + SHOOTER_PIVOT_TO_ENDPOINT_PIVOT_LENGTH_METERS * SHOOTER_PIVOT_TO_ENDPOINT_PIVOT_LENGTH_METERS - + LEAD_SCREW_RADIUS_METERS * LEAD_SCREW_RADIUS_METERS); + length -= LEAD_SCREW_MIN_LENGTH_METERS; // include or... + return length; + } + + public double aimingRegression(double angle) { + // there's going to be a regression function here once we test the shooter + double fudgeFactor = 65537; + return fudgeFactor; + } + + /* + * this does effectively nothing except for account for miniscule positional + * changes in the entry point of the shooter, but it was fun to figure this out + * so im going to keep this in + * dx and dy are the target's horizontal and vertical distances from the shooter + * pivot + * units in meters and radians + */ + public double mathematicallyPerfectLengthFromTarget(double dx, double dy) { + + double h = 2 * Math.atan( + (dy - Math.sqrt(dx * dx + dy * dy - SHOOTER_PIVOT_RADIUS_METERS * SHOOTER_PIVOT_RADIUS_METERS)) + / (dx + SHOOTER_PIVOT_RADIUS_METERS)); + double desiredAngle = ANGLE_ALPHA - h + SHOOTER_OFFSET_ANGLE_RADIANS; + return getLengthFromAngle(desiredAngle); + } + } + + public static final class Climber { + public static enum ClimberPosition { + BOTTOM(0), // TODO + TOP(0.5); // TODO + + public final double value; + + private ClimberPosition(double value) { + this.value = value; + } + } + + public static final int MOTOR_ID = 15; + + public static final DCMotor MOTOR_GEARBOX_REPR = DCMotor.getNeoVortex(1); + public static final double GEARING = 20; + public static final double CARRIAGE_MASS_KG = Units.lbsToKilograms(3); + public static final double DRUM_RADIUS_METERS = Units.inchesToMeters(0.75); + public static final double WHEEL_CIRCUMFERENCE = 2.0 * Math.PI * DRUM_RADIUS_METERS; + public static final double ENCODER_ROTATIONS_TO_METERS = WHEEL_CIRCUMFERENCE / GEARING; + + public static final double MIN_HEIGHT_METERS = 0; // TODO ask nate + public static final double MAX_HEIGHT_METERS = 0.67; // TODO ask nate + + public static final int CURRENT_LIMIT = 40; + + public static final double kP = 0; // TODO + public static final double kI = 0; + public static final double kD = 0; + public static final double kS = 0; + public static final double kG = 0.062861; // TODO + public static final double kV = 78.237; // TODO + public static final double kA = 5.0061; // TODO + public static final double TOLERANCE = 0.01; + + public static final double MAX_VELOCITY_METERS_PER_SECOND = 0.152; // TODO + public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 0.152 * 10; // TODO + public static final TrapezoidProfile.Constraints MOVEMENT_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_VELOCITY_METERS_PER_SECOND, MAX_ACCELERATION_METERS_PER_SECOND_SQUARED); + + public static final Pose3d BASE_COMPONENT_POSE = new Pose3d(0.177, 0, 0.198, new Rotation3d(0, 0, 0)); + } + + public static final class NoteLift { + public static enum NoteLiftPosition { + BOTTOM(0), // TODO + TOP(0.5); // TODO + + public final double value; + + private NoteLiftPosition(double value) { + this.value = value; + } + } + + public static final int MOTOR_ID = 16; + + public static final DCMotor MOTOR_GEARBOX_REPR = DCMotor.getNeoVortex(1); + public static final double GEARING = 20; + public static final double CARRIAGE_MASS_KG = Units.lbsToKilograms(3); + public static final double DRUM_RADIUS_METERS = Units.inchesToMeters(0.75); + public static final double WHEEL_CIRCUMFERENCE = 2.0 * Math.PI * DRUM_RADIUS_METERS; + public static final double ENCODER_ROTATIONS_TO_METERS = WHEEL_CIRCUMFERENCE / GEARING; + + public static final double MIN_HEIGHT_METERS = 0; // TODO ask nate + public static final double MAX_HEIGHT_METERS = 0.67; // TODO ask nate + + public static final int CURRENT_LIMIT = 40; + + public static final double kP = 0; // TODO + public static final double kI = 0; + public static final double kD = 0; + public static final double kS = 0; + public static final double kG = 0.062861; // TODO + public static final double kV = 78.237; // TODO + public static final double kA = 5.0061; // TODO + public static final double TOLERANCE = 0.01; + + public static final double MAX_VELOCITY_METERS_PER_SECOND = 0.152; // TODO + public static final double MAX_ACCELERATION_METERS_PER_SECOND_SQUARED = 0.152 * 10; // TODO + public static final TrapezoidProfile.Constraints MOVEMENT_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_VELOCITY_METERS_PER_SECOND, MAX_ACCELERATION_METERS_PER_SECOND_SQUARED); + + public static final Pose3d BASE_COMPONENT_POSE = new Pose3d(0.177, 0, 0.2, new Rotation3d(0, 0, 0)); + } + + public static final class Vision { + public static final Matrix SINGLE_TAG_STD_DEVS = VecBuilder.fill(6, 6, 12); + public static final Matrix MULTI_TAG_STD_DEVS = VecBuilder.fill(0.5, 0.5, 1); + + public static final PhotonCameraConstants CAMERA_CONSTANTS = new PhotonCameraConstants(); + static { + CAMERA_CONSTANTS.WIDTH = 1600; + CAMERA_CONSTANTS.HEIGHT = 1200; + CAMERA_CONSTANTS.FOV = 95.39; + CAMERA_CONSTANTS.FPS = 35; + CAMERA_CONSTANTS.AVG_LATENCY = 30; + CAMERA_CONSTANTS.STDDEV_LATENCY = 15; + } + + // TODO + public static final Transform3d[] ROBOT_TO_CAMS = new Transform3d[] { + new Transform3d( + new Translation3d(0.26416, 0.26416, 0.25), + new Rotation3d(0, Units.degreesToRadians(-25), Units.degreesToRadians(-10))), + new Transform3d( + new Translation3d(-0.26416, 0.26416, 0.25), + new Rotation3d(0, Units.degreesToRadians(-25), Units.degreesToRadians(70))), + new Transform3d( + new Translation3d(-0.26416, -0.26416, 0.25), + new Rotation3d(0, Units.degreesToRadians(-25), Units.degreesToRadians(-70))) + }; + } + + public static final class Teleop { + /** + * A value inputted into the rate limiter (the joystick input) can move from 0 + * to 1 in 1/RATE_LIMIT seconds. + * + * A rate limit of 3, for example, means that 0->1 in 1/3 sec. + * Larger numbers mean less of a rate limit. + */ + public static final double JOYSTICK_INPUT_RATE_LIMIT = 15.0; // TODO + public static final double JOYSTICK_INPUT_DEADBAND = 0.05; // TODO + public static final double JOYSTICK_CURVE_EXP = 2; // TODO + } + + public static final class LEDs { + public static enum LEDSection implements BaseLEDSection { + SHOOTER_RIGHT(0, 10), + SHOOTER_TOP(0, 0), + SHOOTER_LEFT(0, 0, true), + ELEVATOR_LEFT(0, 0), + ELEVATOR_RIGHT(0, 0); + + private final int startIdx; + private final int endIdx; + private final boolean inverted; + + private LEDSection(int startIdx, int endIdx, boolean inverted) { + this.startIdx = startIdx; + this.endIdx = endIdx; + this.inverted = inverted; + } + + private LEDSection(int startIdx, int endIdx) { + this(startIdx, endIdx, false); + } + + @Override + public int getStart() { + return startIdx; + } + + @Override + public int getEnd() { + return endIdx; + } + + @Override + public boolean getInverted() { + return inverted; + } + } + + public static final int PORT = 0; + public static final int LENGTH = 100; // TODO + } +} diff --git a/src/main/java/frc/robot/Controls.java b/src/main/java/frc/robot/Controls.java new file mode 100644 index 0000000..3127bfe --- /dev/null +++ b/src/main/java/frc/robot/Controls.java @@ -0,0 +1,57 @@ +package frc.robot; + +import com.techhounds.houndutil.houndlib.oi.CommandVirpilJoystick; +import com.techhounds.houndutil.houndlib.subsystems.BaseSwerveDrive.DriveMode; + +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants.Intake.IntakePosition; +import frc.robot.subsystems.Climber; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.LEDs; +import frc.robot.subsystems.NoteLift; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.ShooterTilt; + +public class Controls { + private static double speedMultiplier = 1.0; + + public static void configureDriverControl(int port, Drivetrain drivetrain, Intake intake, Shooter shooter, + ShooterTilt shooterTilt, Climber climber, NoteLift noteLift, LEDs leds) { + CommandVirpilJoystick joystick = new CommandVirpilJoystick(port); + + drivetrain.setDefaultCommand( + drivetrain.teleopDriveCommand( + () -> -joystick.getY() * speedMultiplier, + () -> -joystick.getX() * speedMultiplier, + () -> -joystick.getTwist() * speedMultiplier)); + + new Trigger(() -> (Math.abs(joystick.getTwist()) > 0.05)).onTrue(drivetrain.disableControlledRotateCommand()); + + joystick.stickButton().onTrue(drivetrain.resetGyroCommand()); + + joystick.centerBottomHatUp() + .whileTrue(drivetrain.controlledRotateCommand(() -> Math.toRadians(0), + DriveMode.FIELD_ORIENTED)); + joystick.centerBottomHatLeft() + .whileTrue(drivetrain.controlledRotateCommand(() -> Math.toRadians(90), + DriveMode.FIELD_ORIENTED)); + joystick.centerBottomHatDown() + .whileTrue(drivetrain.controlledRotateCommand(() -> Math.toRadians(180), + DriveMode.FIELD_ORIENTED)); + joystick.centerBottomHatRight() + .whileTrue(drivetrain.controlledRotateCommand(() -> Math.toRadians(270), + DriveMode.FIELD_ORIENTED)); + + joystick.centerBottomHatButton().onTrue(drivetrain.wheelLockCommand()); + + joystick.centerTopHatUp().whileTrue(intake.moveToPositionCommand(() -> IntakePosition.STOW)); + joystick.centerTopHatLeft().whileTrue(intake.moveToPositionCommand(() -> IntakePosition.AMP)); + joystick.centerTopHatDown().whileTrue(intake.moveToPositionCommand(() -> IntakePosition.GROUND)); + + joystick.triggerSoftPress().whileTrue(RobotCommands.targetSpeakerCommand(drivetrain, shooter, shooterTilt)); + joystick.triggerHardPress().onTrue(RobotCommands.shootCommand(drivetrain, intake, shooter, shooterTilt)); + + joystick.blackThumbButton().whileTrue(intake.ampScoreCommand(() -> joystick.getHID().getPinkieButton())); + } +} diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java new file mode 100644 index 0000000..6d0d709 --- /dev/null +++ b/src/main/java/frc/robot/FieldConstants.java @@ -0,0 +1,44 @@ +package frc.robot; + +import com.techhounds.houndutil.houndlog.interfaces.Log; +import com.techhounds.houndutil.houndlog.interfaces.LoggedObject; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; + +@LoggedObject +public class FieldConstants { + public static final double FIELD_LENGTH = Units.inchesToMeters(651.223); + public static final double FIELD_WIDTH = Units.inchesToMeters(323.277); + + @Log + public static final Pose3d TOP_LEFT_OPENING = new Pose3d( + Units.inchesToMeters(18.11), + Units.inchesToMeters(197.73), + Units.inchesToMeters(82.90), + new Rotation3d()); + @Log + public static final Pose3d TOP_RIGHT_OPENING = new Pose3d( + Units.inchesToMeters(18.11), + Units.inchesToMeters(239.11), + Units.inchesToMeters(82.90), new Rotation3d()); + @Log + public static final Pose3d BOTTOM_LEFT_OPENING = new Pose3d( + 0, + Units.inchesToMeters(197.73), + Units.inchesToMeters(78.13), + new Rotation3d()); + @Log + public static final Pose3d BOTTOM_RIGHT_OPENING = new Pose3d( + 0, + Units.inchesToMeters(239.11), + Units.inchesToMeters(78.13), + new Rotation3d()); + @Log + public static final Pose3d TARGET = new Pose3d( + Units.inchesToMeters(6), + Units.inchesToMeters(218.42), + Units.inchesToMeters(78.13 + 1), + new Rotation3d()); +} diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index fe215d7..31617fa 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -4,12 +4,28 @@ package frc.robot; +import com.techhounds.houndutil.houndlib.robots.HoundRobot; + import edu.wpi.first.wpilibj.RobotBase; +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what + * you are doing, do not modify this file except to change the parameter class + * to the startRobot + * call. + */ public final class Main { - private Main() {} + private Main() { + } - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } + /** + * Main initialization function. Do not perform any initialization here. + * + *

+ * If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(() -> new HoundRobot(() -> new RobotContainer())); + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java deleted file mode 100644 index b0fafd4..0000000 --- a/src/main/java/frc/robot/Robot.java +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot; - -import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj2.command.CommandScheduler; - -public class Robot extends TimedRobot { - @Override - public void robotInit() { - } - - @Override - public void robotPeriodic() { - } - - @Override - public void disabledInit() { - } - - @Override - public void disabledPeriodic() { - } - - @Override - public void disabledExit() { - } - - @Override - public void autonomousInit() { - } - - @Override - public void autonomousPeriodic() { - } - - @Override - public void autonomousExit() { - } - - @Override - public void teleopInit() { - } - - @Override - public void teleopPeriodic() { - } - - @Override - public void teleopExit() { - } - - @Override - public void testInit() { - } - - @Override - public void testPeriodic() { - } - - @Override - public void testExit() { - } -} diff --git a/src/main/java/frc/robot/RobotCommands.java b/src/main/java/frc/robot/RobotCommands.java new file mode 100644 index 0000000..719bfc5 --- /dev/null +++ b/src/main/java/frc/robot/RobotCommands.java @@ -0,0 +1,40 @@ +package frc.robot; + +import static frc.robot.Constants.Shooter.SHOOTING_RPS; + +import com.techhounds.houndutil.houndlib.subsystems.BaseSwerveDrive.DriveMode; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.ShooterTilt; + +public class RobotCommands { + public static Command targetSpeakerCommand(Drivetrain drivetrain, Shooter shooter, ShooterTilt shooterTilt) { + return Commands.parallel( + drivetrain.controlledRotateCommand(() -> { + Transform2d diff = drivetrain.getPose().minus(FieldConstants.TARGET.toPose2d()); + Rotation2d rot = new Rotation2d(diff.getX(), diff.getY()); + if (DriverStation.getAlliance().isPresent() && + DriverStation.getAlliance().get() == Alliance.Blue) + rot = rot.plus(new Rotation2d(Math.PI)); + return rot.getRadians(); + }, DriveMode.FIELD_ORIENTED), + shooterTilt.targetSpeakerCommand(drivetrain::getPose).asProxy(), + shooter.spinAtVelocityCommand(() -> SHOOTING_RPS).asProxy()); + } + + public static Command shootCommand(Drivetrain drivetrain, Intake intake, Shooter shooter, ShooterTilt shooterTilt) { + return Commands.sequence( + shooter.spinAtVelocityCommand(() -> SHOOTING_RPS).asProxy() + .until(() -> shooterTilt.atGoal() && shooter.atGoal()), + shooter.spinAtVelocityCommand(() -> SHOOTING_RPS).asProxy().alongWith(intake.runRollersCommand()) + .withTimeout(0.2)); + } +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java new file mode 100644 index 0000000..ce50d49 --- /dev/null +++ b/src/main/java/frc/robot/RobotContainer.java @@ -0,0 +1,142 @@ +package frc.robot; + +import com.techhounds.houndutil.houndauto.AutoManager; +import com.techhounds.houndutil.houndlib.SparkConfigurator; +import com.techhounds.houndutil.houndlog.LoggingManager; +import com.techhounds.houndutil.houndlog.interfaces.Log; +import com.techhounds.houndutil.houndlog.interfaces.SendableLog; + +import edu.wpi.first.hal.AllianceStationID; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.wpilibj.DataLogManager; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.PneumaticHub; +import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.livewindow.LiveWindow; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.subsystems.Climber; +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.LEDs; +import frc.robot.subsystems.NoteLift; +import frc.robot.subsystems.Shooter; +import frc.robot.subsystems.ShooterTilt; +import frc.robot.subsystems.Vision; + +import java.util.ArrayList; +import java.util.function.Supplier; + +/** + * The container for the robot. Contains subsystems, OI devices, and commands. + */ +public class RobotContainer { + @SendableLog(groups = "wpilib") + public static Mechanism2d mechanisms = new Mechanism2d(5, 2); + + @Log(groups = "subsystems") + @SendableLog(groups = { "wpilib", "subsystems" }) + private final Drivetrain drivetrain = new Drivetrain(); + + @Log(groups = "subsystems") + @SendableLog(groups = { "wpilib", "subsystems" }) + private final Intake intake = new Intake(); + + @Log(groups = "subsystems") + @SendableLog(groups = { "wpilib", "subsystems" }) + private final Shooter shooter = new Shooter(); + + @Log(groups = "subsystems") + @SendableLog(groups = { "wpilib", "subsystems" }) + private final ShooterTilt shooterTilt = new ShooterTilt(); + + @Log(groups = "subsystems") + @SendableLog(groups = { "wpilib", "subsystems" }) + private final Climber climber = new Climber(); + + @Log(groups = "subsystems") + @SendableLog(groups = { "wpilib", "subsystems" }) + private final NoteLift noteLift = new NoteLift(); + + @Log(groups = "subsystems") + @SendableLog(groups = { "wpilib", "subsystems" }) + private final Vision vision = new Vision(); + + @Log(groups = "subsystems") + @SendableLog(groups = { "wpilib", "subsystems" }) + private final LEDs leds = new LEDs(); + + @Log(groups = { "subsystems", "misc" }) + private final PowerDistribution pdh = new PowerDistribution(); + + @Log(groups = { "subsystems", "misc" }) + private final PneumaticHub ph = new PneumaticHub(); + + @SendableLog(groups = "wpilib") + private final CommandScheduler commandScheduler = CommandScheduler.getInstance(); + + private double prevLoopTime = 0.0; + + @Log(groups = "wpilib") + private final Supplier loopTimeMs = () -> { + double timestamp = Timer.getFPGATimestamp(); + double loopTime = Timer.getFPGATimestamp() - prevLoopTime; + prevLoopTime = timestamp; + return loopTime * 1000.0; + }; + + /** + * Constructs the robot container. + */ + public RobotContainer() { + vision.setPoseEstimator(drivetrain.getPoseEstimator()); + vision.setSimPoseSupplier(drivetrain::getSimPose); + SparkConfigurator.safeBurnFlash(); + DataLogManager.logNetworkTables(true); + DriverStation.startDataLog(DataLogManager.getLog()); + DataLogManager.start(); + + LoggingManager.getInstance().registerRobotContainer(this); + LoggingManager.getInstance().registerClass(LoggingManager.class, "houndlog", new ArrayList<>()); + LoggingManager.getInstance().registerClass(FieldConstants.class, "fieldConstants", new ArrayList<>()); + + LiveWindow.disableAllTelemetry(); // livewindow is basically deprecated. using houndlog instead. + + if (RobotBase.isSimulation()) { + // prevents annoying joystick disconnected warning + DriverStation.silenceJoystickConnectionWarning(true); + } + configureButtonBindings(); + configureAuto(); + DriverStationSim.setAllianceStationId(AllianceStationID.Blue1); + DriverStationSim.setEnabled(true); + } + + private void configureButtonBindings() { + Controls.configureDriverControl(0, drivetrain, intake, shooter, shooterTilt, climber, noteLift, leds); + } + + private void configureAuto() { + AutoManager.getInstance().addRoutine(Autos.autoA123(drivetrain, intake, shooter, shooterTilt)); + AutoManager.getInstance().addRoutine(Autos.autoBA123(drivetrain, intake, shooter, shooterTilt)); + AutoManager.getInstance().addRoutine(Autos.autoCBA123(drivetrain, intake, shooter, shooterTilt)); + AutoManager.getInstance().addRoutine(Autos.auto453(drivetrain, intake, shooter, shooterTilt)); + AutoManager.getInstance().addRoutine(Autos.autoBAC(drivetrain, intake, shooter, shooterTilt)); + AutoManager.getInstance().addRoutine(Autos.auto1234(drivetrain, intake, shooter, shooterTilt)); + } + + @Log + private Pose3d[] getComponentPoses() { + return new Pose3d[] { + intake.getComponentPose(), + shooterTilt.getShooterComponentPose(), + shooterTilt.getOuterLeadScrewComponentPose(), + shooterTilt.getInnerLeadScrewComponentPose(), + climber.getComponentPose(), + noteLift.getComponentPose() + }; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java new file mode 100644 index 0000000..4309909 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -0,0 +1,200 @@ +package frc.robot.subsystems; + +import java.util.function.Supplier; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.techhounds.houndutil.houndlib.SparkConfigurator; +import com.techhounds.houndutil.houndlib.subsystems.BaseElevator; +import com.techhounds.houndutil.houndlog.interfaces.Log; +import com.techhounds.houndutil.houndlog.interfaces.LoggedObject; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.units.Distance; +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.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; +import frc.robot.Constants.Climber.ClimberPosition; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; +import static frc.robot.Constants.Climber.*; + +@LoggedObject +public class Climber extends SubsystemBase implements BaseElevator { + @Log + private CANSparkMax motor; + + @Log + private ProfiledPIDController pidController = new ProfiledPIDController(kP, kI, kD, MOVEMENT_CONSTRAINTS); + + @Log + private ElevatorFeedforward feedforwardController = new ElevatorFeedforward(kS, kG, kV, kA); + + private ElevatorSim elevatorSim = new ElevatorSim( + MOTOR_GEARBOX_REPR, + GEARING, + CARRIAGE_MASS_KG, + DRUM_RADIUS_METERS, + MIN_HEIGHT_METERS, + MAX_HEIGHT_METERS, + true, + 0); + + @Log(groups = "control") + private double feedbackVoltage = 0; + @Log(groups = "control") + private double feedforwardVoltage = 0; + + private double simVelocity = 0.0; + + private final MutableMeasure sysidAppliedVoltageMeasure = MutableMeasure.mutable(Volts.of(0)); + private final MutableMeasure sysidPositionMeasure = MutableMeasure.mutable(Meters.of(0)); + private final MutableMeasure> sysidVelocityMeasure = MutableMeasure + .mutable(MetersPerSecond.of(0)); + + private final SysIdRoutine sysIdRoutine; + + public Climber() { + motor = SparkConfigurator.createSparkMax( + MOTOR_ID, MotorType.kBrushless, false, + (s) -> s.setIdleMode(IdleMode.kBrake), + (s) -> s.setSmartCurrentLimit(CURRENT_LIMIT), + (s) -> s.getEncoder().setPositionConversionFactor(ENCODER_ROTATIONS_TO_METERS), + (s) -> s.getEncoder().setVelocityConversionFactor(ENCODER_ROTATIONS_TO_METERS / 60.0)); + + pidController.setTolerance(TOLERANCE); + + sysIdRoutine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> setVoltage(volts.magnitude()), + log -> { + log.motor("primary") + .voltage(sysidAppliedVoltageMeasure.mut_replace(motor.getAppliedOutput(), Volts)) + .linearPosition(sysidPositionMeasure.mut_replace(getPosition(), Meters)) + .linearVelocity(sysidVelocityMeasure.mut_replace(getVelocity(), MetersPerSecond)); + }, + this)); + + setDefaultCommand(moveToCurrentGoalCommand()); + } + + @Override + public void simulationPeriodic() { + elevatorSim.setInput(motor.getAppliedOutput()); + elevatorSim.update(0.020); + motor.getEncoder().setPosition(elevatorSim.getPositionMeters()); + simVelocity = elevatorSim.getVelocityMetersPerSecond(); + } + + @Override + @Log + public double getPosition() { + return motor.getEncoder().getPosition(); + } + + @Log + public double getVelocity() { + if (RobotBase.isReal()) + return motor.getEncoder().getVelocity(); + else + return simVelocity; + } + + @Log + public Pose3d getComponentPose() { + return BASE_COMPONENT_POSE.plus(new Transform3d(0, 0, getPosition(), new Rotation3d())); + } + + @Override + public void resetPosition() { + motor.getEncoder().setPosition(0); + } + + @Override + public void setVoltage(double voltage) { + motor.setVoltage(voltage); + } + + @Override + public Command moveToCurrentGoalCommand() { + return run(() -> { + feedbackVoltage = pidController.calculate(getPosition()); + feedforwardVoltage = feedforwardController.calculate(pidController.getSetpoint().position, + pidController.getSetpoint().velocity); + setVoltage(feedbackVoltage + feedforwardVoltage); + }).withName("climber.moveToCurrentGoal"); + } + + @Override + public Command moveToPositionCommand(Supplier goalPositionSupplier) { + return Commands.sequence( + runOnce(() -> pidController.reset(getPosition())), + runOnce(() -> pidController.setGoal(goalPositionSupplier.get().value)), + moveToCurrentGoalCommand().until(pidController::atGoal)).withName("climber.moveToPosition"); + } + + @Override + public Command moveToArbitraryPositionCommand(Supplier goalPositionSupplier) { + return Commands.sequence( + runOnce(() -> pidController.reset(getPosition())), + runOnce(() -> pidController.setGoal(goalPositionSupplier.get())), + moveToCurrentGoalCommand().until(pidController::atGoal)).withName("climber.moveToArbitraryPosition"); + } + + @Override + public Command movePositionDeltaCommand(Supplier delta) { + return moveToArbitraryPositionCommand(() -> pidController.getGoal().position + delta.get()) + .withName("climber.movePositionDelta"); + } + + @Override + public Command holdCurrentPositionCommand() { + return runOnce(() -> pidController.setGoal(getPosition())).andThen(moveToCurrentGoalCommand()) + .withName("climber.holdCurrentPosition"); + } + + @Override + public Command resetPositionCommand() { + return runOnce(this::resetPosition).withName("climber.resetPosition"); + } + + @Override + public Command setOverridenSpeedCommand(Supplier speed) { + return run(() -> setVoltage(12.0 * speed.get())) + .withName("climber.setOverriddenSpeed"); + } + + @Override + public Command coastMotorsCommand() { + return runOnce(motor::stopMotor) + .andThen(() -> motor.setIdleMode(IdleMode.kCoast)) + .finallyDo((d) -> { + motor.setIdleMode(IdleMode.kBrake); + pidController.reset(getPosition()); + }).withInterruptBehavior(InterruptionBehavior.kCancelIncoming) + .withName("climber.coastMotorsCommand"); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysIdRoutine.quasistatic(direction).withName("climber.sysIdQuasistatic"); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysIdRoutine.dynamic(direction).withName("climber.sysIdDynamic"); + } +} diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java new file mode 100644 index 0000000..a496757 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -0,0 +1,713 @@ +package frc.robot.subsystems; + +import java.util.Set; +import java.util.function.DoubleSupplier; +import com.ctre.phoenix6.hardware.Pigeon2; +import com.pathplanner.lib.commands.FollowPathHolonomic; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.ReplanningConfig; +import com.techhounds.houndutil.houndauto.AutoManager; +import com.techhounds.houndutil.houndlib.MotorHoldMode; +import com.techhounds.houndutil.houndlib.subsystems.BaseSwerveDrive; +import com.techhounds.houndutil.houndlib.swerve.KrakenCoaxialSwerveModule; +import com.techhounds.houndutil.houndlog.interfaces.Log; +import com.techhounds.houndutil.houndlog.interfaces.LoggedObject; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Distance; +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.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.DeferredCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; + +import static frc.robot.Constants.Drivetrain.*; +import static frc.robot.Constants.Teleop.*; +import static edu.wpi.first.units.MutableMeasure.mutable; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; + +@LoggedObject +public class Drivetrain extends SubsystemBase implements BaseSwerveDrive { + @Log(groups = "modules") + private final KrakenCoaxialSwerveModule frontLeft = new KrakenCoaxialSwerveModule( + FRONT_LEFT_DRIVE_MOTOR_ID, + FRONT_LEFT_STEER_MOTOR_ID, + FRONT_LEFT_STEER_ENCODER_ID, + CAN_BUS_NAME, + DRIVE_MOTORS_INVERTED, + STEER_MOTORS_INVERTED, + STEER_CANCODERS_INVERTED, + FRONT_LEFT_OFFSET, + SWERVE_CONSTANTS); + + @Log(groups = "modules") + private final KrakenCoaxialSwerveModule frontRight = new KrakenCoaxialSwerveModule( + FRONT_RIGHT_DRIVE_MOTOR_ID, + FRONT_RIGHT_STEER_MOTOR_ID, + FRONT_RIGHT_STEER_ENCODER_ID, + CAN_BUS_NAME, + DRIVE_MOTORS_INVERTED, + STEER_MOTORS_INVERTED, + STEER_CANCODERS_INVERTED, + FRONT_RIGHT_OFFSET, + SWERVE_CONSTANTS); + + @Log(groups = "modules") + private final KrakenCoaxialSwerveModule backLeft = new KrakenCoaxialSwerveModule( + BACK_LEFT_DRIVE_MOTOR_ID, + BACK_LEFT_STEER_MOTOR_ID, + BACK_LEFT_STEER_ENCODER_ID, + CAN_BUS_NAME, + DRIVE_MOTORS_INVERTED, + STEER_MOTORS_INVERTED, + STEER_CANCODERS_INVERTED, + BACK_LEFT_OFFSET, + SWERVE_CONSTANTS); + + @Log(groups = "modules") + private final KrakenCoaxialSwerveModule backRight = new KrakenCoaxialSwerveModule( + BACK_RIGHT_DRIVE_MOTOR_ID, + BACK_RIGHT_STEER_MOTOR_ID, + BACK_RIGHT_STEER_ENCODER_ID, + CAN_BUS_NAME, + DRIVE_MOTORS_INVERTED, + STEER_MOTORS_INVERTED, + STEER_CANCODERS_INVERTED, + BACK_RIGHT_OFFSET, + SWERVE_CONSTANTS); + + @Log + private Pigeon2 pigeon = new Pigeon2(0); + + @Log + private SwerveDrivePoseEstimator poseEstimator; + + @Log + private ProfiledPIDController xPositionController = new ProfiledPIDController( + XY_kP, XY_kI, XY_kD, XY_CONSTRAINTS); + + @Log + private ProfiledPIDController yPositionController = new ProfiledPIDController( + XY_kP, XY_kI, XY_kD, XY_CONSTRAINTS); + + @Log + private ProfiledPIDController thetaPositionController = new ProfiledPIDController( + THETA_kP, THETA_kI, THETA_kD, THETA_CONSTRAINTS); + + @Log(groups = "control") + private SwerveModuleState[] commandedModuleStates = new SwerveModuleState[] { new SwerveModuleState(), + new SwerveModuleState(), new SwerveModuleState(), new SwerveModuleState() }; + + @Log(groups = "control") + private ChassisSpeeds commandedChassisSpeeds = new ChassisSpeeds(); + + @Log + private DriveMode driveMode = DriveMode.FIELD_ORIENTED; + + @Log + private Twist2d twist = new Twist2d(); + + /** + * Whether to override the inputs of the driver for maintaining or turning to a + * specific angle. + */ + @Log + private boolean isControlledRotationEnabled = false; + + private SwerveDriveOdometry simOdometry; + private SwerveModulePosition[] lastModulePositions = getModulePositions(); + + private final MutableMeasure sysidDriveAppliedVoltageMeasure = mutable(Volts.of(0)); + private final MutableMeasure sysidDrivePositionMeasure = mutable(Meters.of(0)); + private final MutableMeasure> sysidDriveVelocityMeasure = mutable(MetersPerSecond.of(0)); + + private final SysIdRoutine sysIdDrive; + + private final MutableMeasure sysidSteerAppliedVoltageMeasure = mutable(Volts.of(0)); + private final MutableMeasure sysidSteerPositionMeasure = mutable(Rotations.of(0)); + private final MutableMeasure> sysidSteerVelocityMeasure = mutable(RotationsPerSecond.of(0)); + + private final SysIdRoutine sysIdSteer; + + /** Initializes the drivetrain. */ + public Drivetrain() { + resetGyro(); + poseEstimator = new SwerveDrivePoseEstimator( + KINEMATICS, + getRotation(), + getModulePositions(), + new Pose2d(0, 0, new Rotation2d())); + + thetaPositionController.setTolerance(0.05); + thetaPositionController.enableContinuousInput(0, 2 * Math.PI); + + AutoManager.getInstance().setResetOdometryConsumer(this::resetPoseEstimator); + + if (RobotBase.isSimulation()) { + simOdometry = new SwerveDriveOdometry(KINEMATICS, getRotation(), getModulePositions(), new Pose2d()); + } + + sysIdDrive = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> { + drive(new ChassisSpeeds( + SWERVE_CONSTANTS.MAX_DRIVING_VELOCITY_METERS_PER_SECOND * volts.magnitude() / 12.0, + 0, + 0)); + }, + log -> { + log.motor("frontLeft") + .voltage(sysidDriveAppliedVoltageMeasure + .mut_replace(frontLeft.getDriveMotorVoltage(), Volts)) + .linearPosition( + sysidDrivePositionMeasure.mut_replace(frontLeft.getDriveMotorPosition(), + Meters)) + .linearVelocity(sysidDriveVelocityMeasure + .mut_replace(frontLeft.getDriveMotorVelocity(), MetersPerSecond)); + log.motor("frontRight") + .voltage(sysidDriveAppliedVoltageMeasure.mut_replace( + frontRight.getDriveMotorVoltage(), + Volts)) + .linearPosition( + sysidDrivePositionMeasure.mut_replace(frontRight.getDriveMotorPosition(), + Meters)) + .linearVelocity( + sysidDriveVelocityMeasure.mut_replace(frontRight.getDriveMotorVelocity(), + MetersPerSecond)); + log.motor("backLeft") + .voltage( + sysidDriveAppliedVoltageMeasure.mut_replace(backLeft.getDriveMotorVoltage(), + Volts)) + .linearPosition( + sysidDrivePositionMeasure.mut_replace(backLeft.getDriveMotorPosition(), + Meters)) + .linearVelocity( + sysidDriveVelocityMeasure.mut_replace(backLeft.getDriveMotorVelocity(), + MetersPerSecond)); + log.motor("backRight") + .voltage( + sysidDriveAppliedVoltageMeasure.mut_replace( + backRight.getDriveMotorVoltage(), + Volts)) + .linearPosition( + sysidDrivePositionMeasure.mut_replace(backRight.getDriveMotorPosition(), + Meters)) + .linearVelocity( + sysidDriveVelocityMeasure.mut_replace(backRight.getDriveMotorVelocity(), + MetersPerSecond)); + }, + this)); + + sysIdSteer = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> { + drive(new ChassisSpeeds( + SWERVE_CONSTANTS.MAX_DRIVING_VELOCITY_METERS_PER_SECOND * volts.magnitude() / + 12.0, + 0, + 0)); + }, + log -> { + log.motor("frontLeft") + .voltage( + sysidSteerAppliedVoltageMeasure.mut_replace( + frontLeft.getSteerMotorVoltage(), + Volts)) + .angularPosition( + sysidSteerPositionMeasure.mut_replace(frontLeft.getSteerMotorPosition(), + Rotations)) + .angularVelocity( + sysidSteerVelocityMeasure.mut_replace(frontLeft.getSteerMotorVelocity(), + RotationsPerSecond)); + log.motor("frontRight") + .voltage( + sysidSteerAppliedVoltageMeasure.mut_replace( + frontRight.getSteerMotorVoltage(), + Volts)) + .angularPosition( + sysidSteerPositionMeasure.mut_replace(frontRight.getSteerMotorPosition(), + Rotations)) + .angularVelocity( + sysidSteerVelocityMeasure.mut_replace(frontRight.getSteerMotorVelocity(), + RotationsPerSecond)); + log.motor("backLeft") + .voltage( + sysidSteerAppliedVoltageMeasure.mut_replace(backLeft.getSteerMotorVoltage(), + Volts)) + .angularPosition( + sysidSteerPositionMeasure.mut_replace(backLeft.getSteerMotorPosition(), + Rotations)) + .angularVelocity( + sysidSteerVelocityMeasure.mut_replace(backLeft.getSteerMotorVelocity(), + RotationsPerSecond)); + log.motor("backRight") + .voltage( + sysidSteerAppliedVoltageMeasure.mut_replace( + backRight.getSteerMotorVoltage(), + Volts)) + .angularPosition( + sysidSteerPositionMeasure.mut_replace(backRight.getSteerMotorPosition(), + Rotations)) + .angularVelocity( + sysidSteerVelocityMeasure.mut_replace(backRight.getSteerMotorVelocity(), + RotationsPerSecond)); + }, + this)); + } + + @Override + public void periodic() { + updatePoseEstimator(); + } + + /** + * Updates simulation-specific variables. + */ + @Override + public void simulationPeriodic() { + SwerveModulePosition[] currentPositions = getModulePositions(); + SwerveModulePosition[] deltas = new SwerveModulePosition[4]; + + for (int i = 0; i < 4; i++) { + deltas[i] = new SwerveModulePosition( + currentPositions[i].distanceMeters - lastModulePositions[i].distanceMeters, + currentPositions[i].angle); + } + + twist = KINEMATICS.toTwist2d(deltas); + + pigeon.getSimState().setRawYaw(pigeon.getYaw().getValue() + + Units.radiansToDegrees(twist.dtheta)); + + lastModulePositions = currentPositions; + } + + @Override + public DriveMode getDriveMode() { + return driveMode; + } + + @Override + @Log + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + @Log + public Pose2d getSimPose() { + return simOdometry.getPoseMeters(); + } + + @Override + public Rotation2d getRotation() { + return Rotation2d.fromDegrees(pigeon.getYaw().getValue()); + } + + @Override + @Log(groups = "control") + public SwerveModulePosition[] getModulePositions() { + return new SwerveModulePosition[] { + frontLeft.getPosition(), + frontRight.getPosition(), + backLeft.getPosition(), + backRight.getPosition() + }; + } + + @Override + @Log(groups = "control") + public SwerveModuleState[] getModuleStates() { + return new SwerveModuleState[] { + frontLeft.getState(), + frontRight.getState(), + backLeft.getState(), + backRight.getState() + }; + } + + @Override + @Log(groups = "control") + public ChassisSpeeds getChassisSpeeds() { + return KINEMATICS.toChassisSpeeds(getModuleStates()); + } + + @Override + public SwerveDrivePoseEstimator getPoseEstimator() { + return poseEstimator; + } + + @Override + public void updatePoseEstimator() { + poseEstimator.update(getRotation(), getModulePositions()); + simOdometry.update(getRotation(), getModulePositions()); + drawRobotOnField(AutoManager.getInstance().getField()); + } + + @Override + public void resetPoseEstimator(Pose2d pose) { + poseEstimator.resetPosition(getRotation(), getModulePositions(), pose); + simOdometry.resetPosition(getRotation(), getModulePositions(), pose); + } + + @Override + public void resetGyro() { + pigeon.setYaw(0); + } + + @Override + public void setMotorHoldModes(MotorHoldMode motorHoldMode) { + frontLeft.setMotorHoldMode(motorHoldMode); + frontRight.setMotorHoldMode(motorHoldMode); + backLeft.setMotorHoldMode(motorHoldMode); + backRight.setMotorHoldMode(motorHoldMode); + } + + @Override + public void setDriveCurrentLimit(int currentLimit) { + frontLeft.setDriveCurrentLimit(currentLimit); + frontRight.setDriveCurrentLimit(currentLimit); + backLeft.setDriveCurrentLimit(currentLimit); + backRight.setDriveCurrentLimit(currentLimit); + } + + @Override + public void stop() { + frontLeft.stop(); + frontRight.stop(); + backLeft.stop(); + backRight.stop(); + } + + @Override + public void setStates(SwerveModuleState[] states) { + frontLeft.setState(states[0]); + frontRight.setState(states[1]); + backLeft.setState(states[2]); + backRight.setState(states[3]); + } + + @Override + public void setStatesClosedLoop(SwerveModuleState[] states) { + frontLeft.setStateClosedLoop(states[0]); + frontRight.setStateClosedLoop(states[1]); + backLeft.setStateClosedLoop(states[2]); + backRight.setStateClosedLoop(states[3]); + } + + public void drive(ChassisSpeeds speeds) { + drive(speeds, this.driveMode); + } + + @Override + public void drive(ChassisSpeeds speeds, DriveMode driveMode) { + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red + && driveMode == DriveMode.FIELD_ORIENTED) { + speeds.vxMetersPerSecond *= -1; + speeds.vyMetersPerSecond *= -1; + } + + commandedChassisSpeeds = speeds; + ChassisSpeeds adjustedChassisSpeeds = null; + switch (driveMode) { + case ROBOT_RELATIVE: + adjustedChassisSpeeds = speeds; + break; + case FIELD_ORIENTED: + adjustedChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds.vxMetersPerSecond, + speeds.vyMetersPerSecond, + speeds.omegaRadiansPerSecond, getRotation()); + break; + } + + // compensates for swerve skew + adjustedChassisSpeeds = ChassisSpeeds.discretize(adjustedChassisSpeeds, 0.02); + SwerveModuleState[] states = KINEMATICS.toSwerveModuleStates(adjustedChassisSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(states, + SWERVE_CONSTANTS.MAX_DRIVING_VELOCITY_METERS_PER_SECOND); + + states = new SwerveModuleState[] { + SwerveModuleState.optimize(states[0], frontLeft.getWheelAngle()), + SwerveModuleState.optimize(states[1], frontRight.getWheelAngle()), + SwerveModuleState.optimize(states[2], backLeft.getWheelAngle()), + SwerveModuleState.optimize(states[3], backRight.getWheelAngle()), + }; + + commandedModuleStates = states; + setStates(states); + } + + @Override + public void driveClosedLoop(ChassisSpeeds speeds, DriveMode driveMode) { + if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red + && driveMode == DriveMode.FIELD_ORIENTED) { + speeds.vxMetersPerSecond *= -1; + speeds.vyMetersPerSecond *= -1; + } + + commandedChassisSpeeds = speeds; + ChassisSpeeds adjustedChassisSpeeds = null; + switch (driveMode) { + case ROBOT_RELATIVE: + adjustedChassisSpeeds = speeds; + break; + case FIELD_ORIENTED: + adjustedChassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds.vxMetersPerSecond, + speeds.vyMetersPerSecond, + speeds.omegaRadiansPerSecond, getRotation()); + break; + } + + SwerveModuleState[] states = KINEMATICS.toSwerveModuleStates(adjustedChassisSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(states, + SWERVE_CONSTANTS.MAX_DRIVING_VELOCITY_METERS_PER_SECOND); + + states = new SwerveModuleState[] { + SwerveModuleState.optimize(states[0], frontLeft.getWheelAngle()), + SwerveModuleState.optimize(states[1], frontRight.getWheelAngle()), + SwerveModuleState.optimize(states[2], backLeft.getWheelAngle()), + SwerveModuleState.optimize(states[3], backRight.getWheelAngle()), + }; + + commandedModuleStates = states; + setStatesClosedLoop(states); + } + + @Override + public Command teleopDriveCommand(DoubleSupplier xSpeedSupplier, DoubleSupplier ySpeedSupplier, + DoubleSupplier thetaSpeedSupplier) { + SlewRateLimiter xSpeedLimiter = new SlewRateLimiter(JOYSTICK_INPUT_RATE_LIMIT); + SlewRateLimiter ySpeedLimiter = new SlewRateLimiter(JOYSTICK_INPUT_RATE_LIMIT); + SlewRateLimiter thetaSpeedLimiter = new SlewRateLimiter(JOYSTICK_INPUT_RATE_LIMIT); + + return run(() -> { + double xSpeed = xSpeedSupplier.getAsDouble(); + double ySpeed = ySpeedSupplier.getAsDouble(); + double thetaSpeed = thetaSpeedSupplier.getAsDouble(); + + xSpeed = MathUtil.applyDeadband(xSpeed, JOYSTICK_INPUT_DEADBAND); + ySpeed = MathUtil.applyDeadband(ySpeed, JOYSTICK_INPUT_DEADBAND); + thetaSpeed = MathUtil.applyDeadband(thetaSpeed, JOYSTICK_INPUT_DEADBAND); + + xSpeed = Math.copySign(Math.pow(xSpeed, JOYSTICK_CURVE_EXP), xSpeed); + ySpeed = Math.copySign(Math.pow(ySpeed, JOYSTICK_CURVE_EXP), ySpeed); + thetaSpeed = Math.copySign(Math.pow(thetaSpeed, JOYSTICK_CURVE_EXP), thetaSpeed); + + xSpeed = xSpeedLimiter.calculate(xSpeed); + ySpeed = ySpeedLimiter.calculate(ySpeed); + thetaSpeed = thetaSpeedLimiter.calculate(thetaSpeed); + + if (isControlledRotationEnabled) { + thetaSpeed = thetaPositionController.calculate(getRotation().getRadians()); + } + + // the speeds are initially values from -1.0 to 1.0, so we multiply by the max + // physical velocity to output in m/s. + xSpeed *= SWERVE_CONSTANTS.MAX_DRIVING_VELOCITY_METERS_PER_SECOND; + ySpeed *= SWERVE_CONSTANTS.MAX_DRIVING_VELOCITY_METERS_PER_SECOND; + thetaSpeed *= SWERVE_CONSTANTS.MAX_DRIVING_VELOCITY_METERS_PER_SECOND; + + drive(new ChassisSpeeds(xSpeed, ySpeed, thetaSpeed), driveMode); + }).withName("drivetrain.teleopDrive"); + } + + @Override + public Command controlledRotateCommand(DoubleSupplier angle, DriveMode driveMode) { + return Commands.run(() -> { + if (!isControlledRotationEnabled) { + thetaPositionController.reset(getRotation().getRadians()); + } + isControlledRotationEnabled = true; + if (driveMode == DriveMode.FIELD_ORIENTED && DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == Alliance.Red) + thetaPositionController.setGoal(angle.getAsDouble() + Math.PI); + else + thetaPositionController.setGoal(angle.getAsDouble()); + }).withName("drivetrain.controlledRotate"); + } + + @Override + public Command disableControlledRotateCommand() { + return Commands.runOnce( + () -> { + isControlledRotationEnabled = false; + }).withName("drivetrain.disableControlledRotate"); + } + + @Override + public Command wheelLockCommand() { + return run(() -> { + setStates(new SwerveModuleState[] { + new SwerveModuleState(0, Rotation2d.fromDegrees(45)), + new SwerveModuleState(0, Rotation2d.fromDegrees(-45)), + new SwerveModuleState(0, Rotation2d.fromDegrees(-45)), + new SwerveModuleState(0, Rotation2d.fromDegrees(45)) + }); + }).withName("drivetrain.wheelLock"); + } + + @Override + public Command turnWheelsToAngleCommand(double angle) { + return Commands.runOnce(() -> { + setStates(new SwerveModuleState[] { + new SwerveModuleState(0, new Rotation2d(angle)), + new SwerveModuleState(0, new Rotation2d(angle)), + new SwerveModuleState(0, new Rotation2d(angle)), + new SwerveModuleState(0, new Rotation2d(angle)) + + }); + }).withName("drivetrain.turnWheelsToAngle"); + } + + @Override + public Command driveToPoseCommand(Pose2d pose) { + return runOnce(() -> { + xPositionController.reset(getPose().getX()); + yPositionController.reset(getPose().getY()); + thetaPositionController.reset(getPose().getRotation().getRadians()); + }).andThen(run(() -> { + driveClosedLoop( + new ChassisSpeeds( + xPositionController.calculate(getPose().getX(), pose.getX()), + yPositionController.calculate(getPose().getX(), pose.getY()), + thetaPositionController.calculate(getPose().getX(), pose.getRotation().getRadians())), + DriveMode.FIELD_ORIENTED); + })).withName("drivetrain.driveToPose"); + } + + @Override + public Command followPathCommand(PathPlannerPath path) { + return new FollowPathHolonomic( + path, + this::getPose, + this::getChassisSpeeds, + (speeds) -> driveClosedLoop(speeds, DriveMode.ROBOT_RELATIVE), + new HolonomicPathFollowerConfig( + new PIDConstants(PATH_FOLLOWING_TRANSLATION_kP, 0, 0), + new PIDConstants(PATH_FOLLOWING_ROTATION_kP, 0, 0), + SWERVE_CONSTANTS.MAX_DRIVING_VELOCITY_METERS_PER_SECOND, + 0.41, + new ReplanningConfig()), + () -> false, + this).withName("drivetrain.followPath"); + } + + @Override + public Command driveDeltaCommand(Transform2d delta, PathConstraints constraints) { + return new DeferredCommand(() -> followPathCommand( + new PathPlannerPath( + PathPlannerPath.bezierFromPoses( + getPose(), getPose().plus(delta)), + constraints, + new GoalEndState(0, delta.getRotation().plus(getRotation())))), + Set.of()).withName("drivetrain.driveDelta"); + } + + @Override + public Command setDriveModeCommand(DriveMode driveMode) { + return runOnce(() -> this.driveMode = driveMode).withName("drivetrain.setDriveMode"); + } + + @Override + public Command resetGyroCommand() { + return runOnce(() -> resetGyro()).withName("drivetrain.resetGyro"); + } + + @Override + public Command setDriveCurrentLimitCommand(int currentLimit) { + return Commands.runOnce(() -> { + frontLeft.setDriveCurrentLimit(currentLimit); + frontRight.setDriveCurrentLimit(currentLimit); + backLeft.setDriveCurrentLimit(currentLimit); + backRight.setDriveCurrentLimit(currentLimit); + }).withName("drivetrain.setDriveCurrentLimit"); + } + + @Override + public Command coastMotorsCommand() { + return runOnce(this::stop) + .andThen(() -> setMotorHoldModes(MotorHoldMode.COAST)) + .finallyDo((d) -> setMotorHoldModes(MotorHoldMode.BRAKE)) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming) + .withName("drivetrain.coastMotors"); + } + + /** + * Draws the robot on a Field2d. This will include the angles of the swerve + * modules on the outsides of the robot box in Glass. + * + * @param field the field to draw the robot on (usually + * {@code AutoManager.getInstance().getField()}) + */ + public void drawRobotOnField(Field2d field) { + field.setRobotPose(getPose()); + field.getObject("simPose").setPose(simOdometry.getPoseMeters()); + + // Draw a pose that is based on the robot pose, but shifted by the + // translation of the module relative to robot center, + // then rotated around its own center by the angle of the module. + field.getObject("frontLeft").setPose( + getPose().transformBy( + new Transform2d(SWERVE_MODULE_LOCATIONS[0], + getModuleStates()[0].angle))); + field.getObject("frontRight").setPose( + getPose().transformBy( + new Transform2d(SWERVE_MODULE_LOCATIONS[1], + getModuleStates()[1].angle))); + field.getObject("backLeft").setPose( + getPose().transformBy( + new Transform2d(SWERVE_MODULE_LOCATIONS[2], + getModuleStates()[2].angle))); + field.getObject("backRight").setPose( + getPose().transformBy( + new Transform2d(SWERVE_MODULE_LOCATIONS[3], + getModuleStates()[3].angle))); + } + + public Command sysIdDriveQuasistatic(SysIdRoutine.Direction direction) { + return sysIdDrive.quasistatic(direction).withName("drivetrain.sysIdDriveQuasistatic"); + } + + public Command sysIdDriveDynamic(SysIdRoutine.Direction direction) { + return sysIdDrive.dynamic(direction).withName("drivetrain.sysIdDriveQuasistatic"); + } + + public Command sysIdSteerQuasistatic(SysIdRoutine.Direction direction) { + return sysIdSteer.quasistatic(direction).withName("drivetrain.sysIdDriveQuasistatic"); + } + + public Command sysIdSteerDynamic(SysIdRoutine.Direction direction) { + return sysIdSteer.dynamic(direction).withName("drivetrain.sysIdDriveQuasistatic"); + } +} diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java new file mode 100644 index 0000000..ab4189d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -0,0 +1,301 @@ +package frc.robot.subsystems; + +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.techhounds.houndutil.houndlib.SparkConfigurator; +import com.techhounds.houndutil.houndlib.subsystems.BaseSingleJointedArm; +import com.techhounds.houndutil.houndlog.interfaces.Log; +import com.techhounds.houndutil.houndlog.interfaces.LoggedObject; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +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.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.DIOSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants.Intake.IntakePosition; +import static frc.robot.Constants.Intake.*; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; + +/* + * The Intake subsystem, which pulls in a note and can be sent to a shooter or can be used to score the amp. + * @author rb, jq, ak + */ +@LoggedObject +public class Intake extends SubsystemBase implements BaseSingleJointedArm { + @Log + private final CANSparkFlex primaryArmMotor; + @Log + private final CANSparkFlex secondaryArmMotor; + + @Log + private final CANSparkFlex rollerMotor; + + @Log + private final DigitalInput intakeBeam = new DigitalInput(INTAKE_BEAM_ID); + @Log + private final DigitalInput shooterPrimaryBeam = new DigitalInput(PRIMARY_SHOOTER_BEAM_ID); + @Log + private final DigitalInput shooterSecondaryBeam = new DigitalInput(SECONDARY_SHOOTER_BEAM_ID); + + @Log + private final ProfiledPIDController pidController = new ProfiledPIDController(kP, kI, kD, MOVEMENT_CONSTRAINTS); + + @Log + private final ArmFeedforward feedforwardController = new ArmFeedforward(kS, kG, kV, kA); + + @Log + private final SingleJointedArmSim armSim = new SingleJointedArmSim( + MOTOR_GEARBOX_REPR, + GEARING, + MOMENT_OF_INERTIA_KG_METERS_SQUARED, + LENGTH_METERS, + MIN_ANGLE_RADIANS, + MAX_ANGLE_RADIANS, + true, + 0); + private final DIOSim intakeBeamSim = new DIOSim(intakeBeam); + private final DIOSim shooterPrimaryBeamSim = new DIOSim(shooterPrimaryBeam); + private final DIOSim shooterSecondaryBeamSim = new DIOSim(shooterSecondaryBeam); + + @Log(groups = "control") + private double feedbackVoltage = 0; + @Log(groups = "control") + private double feedforwardVoltage = 0; + + @Log + private double simVelocity = 0.0; + + private final MutableMeasure sysidAppliedVoltageMeasure = MutableMeasure.mutable(Volts.of(0)); + private final MutableMeasure sysidPositionMeasure = MutableMeasure.mutable(Radians.of(0)); + private final MutableMeasure> sysidVelocityMeasure = MutableMeasure.mutable(RadiansPerSecond.of(0)); + + private final SysIdRoutine sysIdRoutine; + + public Intake() { + primaryArmMotor = SparkConfigurator.createSparkFlex(PRIMARY_ARM_MOTOR_ID, MotorType.kBrushless, false, + (s) -> s.setIdleMode(IdleMode.kBrake), + (s) -> s.setSmartCurrentLimit(ARM_CURRENT_LIMIT), + (s) -> s.getEncoder().setPositionConversionFactor(ENCODER_ROTATIONS_TO_RADIANS), + (s) -> s.getEncoder().setVelocityConversionFactor(ENCODER_ROTATIONS_TO_RADIANS / 60.0)); + + secondaryArmMotor = SparkConfigurator.createSparkFlex(SECONDARY_ARM_MOTOR_ID, MotorType.kBrushless, false, + (s) -> s.setIdleMode(IdleMode.kBrake), + (s) -> s.setSmartCurrentLimit(ARM_CURRENT_LIMIT), + (s) -> s.follow(primaryArmMotor)); + + rollerMotor = SparkConfigurator.createSparkFlex(ROLLER_MOTOR_ID, MotorType.kBrushless, false, + (s) -> s.setIdleMode(IdleMode.kBrake), + (s) -> s.setSmartCurrentLimit(ROLLER_CURRENT_LIMIT)); + + sysIdRoutine = new SysIdRoutine( + new SysIdRoutine.Config(Volts.of(0.5).per(Seconds.of(1)), Volts.of(2), null, null), + new SysIdRoutine.Mechanism( + (Measure volts) -> setVoltage(volts.magnitude()), + log -> { + log.motor("primary") + .voltage(sysidAppliedVoltageMeasure.mut_replace(primaryArmMotor.getAppliedOutput(), + Volts)) + .angularPosition(sysidPositionMeasure.mut_replace(getPosition(), Radians)) + .angularVelocity(sysidVelocityMeasure.mut_replace(getVelocity(), RadiansPerSecond)); + }, + this)); + + pidController.setGoal(IntakePosition.STOW.value); + intakeBeamSim.setValue(false); + shooterPrimaryBeamSim.setValue(false); + shooterSecondaryBeamSim.setValue(false); + setDefaultCommand(moveToCurrentGoalCommand()); + } + + @Override + public void simulationPeriodic() { + armSim.setInput(primaryArmMotor.getAppliedOutput()); + armSim.update(0.020); + primaryArmMotor.getEncoder().setPosition(armSim.getAngleRads()); + simVelocity = armSim.getVelocityRadPerSec(); + } + + @Log + public Pose3d getComponentPose() { + return BASE_COMPONENT_POSE.plus(new Transform3d(0, 0, 0, new Rotation3d(0, -getPosition(), 0))); + } + + @Override + @Log + public double getPosition() { + return primaryArmMotor.getEncoder().getPosition(); + } + + @Log + public double getVelocity() { + if (RobotBase.isReal()) + return primaryArmMotor.getEncoder().getVelocity(); + else + return simVelocity; + } + + @Override + public void resetPosition() { + primaryArmMotor.getEncoder().setPosition(0); + } + + @Override + public void setVoltage(double voltage) { + primaryArmMotor.setVoltage(voltage > 12 ? 12 : voltage); + } + + public void setRollerVoltage(double voltage) { + rollerMotor.setVoltage(voltage); + } + + @Override + public Command moveToCurrentGoalCommand() { + return run(() -> { + feedbackVoltage = pidController.calculate(getPosition()); + feedforwardVoltage = feedforwardController.calculate(getPosition(), + pidController.getSetpoint().velocity); + setVoltage(feedbackVoltage + feedforwardVoltage); + }).withName("intake.moveToCurrentGoal"); + } + + @Override + public Command moveToPositionCommand(Supplier goalPositionSupplier) { + return Commands.sequence( + runOnce(() -> pidController.reset(getPosition())), + runOnce(() -> pidController.setGoal(goalPositionSupplier.get().value)), + moveToCurrentGoalCommand().until(pidController::atGoal)) + .withName("intake.moveToPosition"); + } + + @Override + public Command moveToArbitraryPositionCommand(Supplier goalPositionSupplier) { + return Commands.sequence( + runOnce(() -> pidController.reset(getPosition())), + runOnce(() -> pidController.setGoal(goalPositionSupplier.get())), + moveToCurrentGoalCommand().until(pidController::atGoal)) + .withName("intake.moveToArbitraryPosition"); + } + + @Override + public Command movePositionDeltaCommand(Supplier delta) { + return Commands.sequence( + runOnce(() -> pidController.reset(getPosition())), + runOnce(() -> pidController.setGoal(getPosition() + delta.get())), + moveToCurrentGoalCommand().until(pidController::atGoal)).withTimeout(2) + .withName("intake.movePositionDelta"); + } + + @Override + public Command holdCurrentPositionCommand() { + return runOnce(() -> pidController.setGoal(getPosition())) + .andThen(moveToCurrentGoalCommand()).withName("intake.holdCurrentPosition"); + } + + @Override + public Command resetPositionCommand() { + return runOnce(this::resetPosition).withName("intake.resetPosition"); + } + + @Override + public Command setOverridenSpeedCommand(Supplier speed) { + return run(() -> setVoltage(12.0 * speed.get())) + .withName("intake.setOverriddenSpeed"); + } + + @Override + public Command coastMotorsCommand() { + return runOnce(() -> primaryArmMotor.stopMotor()) + .andThen(() -> { + primaryArmMotor.setIdleMode(IdleMode.kCoast); + secondaryArmMotor.setIdleMode(IdleMode.kCoast); + }) + .finallyDo((d) -> { + primaryArmMotor.setIdleMode(IdleMode.kBrake); + secondaryArmMotor.setIdleMode(IdleMode.kBrake); + pidController.reset(getPosition()); + }) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming) + .withName("intake.coastMotors"); + } + + public Command runRollersCommand() { + return Commands.startEnd( + () -> rollerMotor.setVoltage(3), + () -> rollerMotor.setVoltage(0)) + .withName("intake.runRollers"); + } + + public Command reverseRollersCommand() { + return Commands.startEnd( + () -> rollerMotor.setVoltage(-3), + () -> rollerMotor.setVoltage(0)) + .withName("intake.reverseRollers"); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysIdRoutine.quasistatic(direction).withName("intake.sysIdQuasistatic"); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysIdRoutine.dynamic(direction).withName("intake.sysIdQuasistatic"); + } + + public Command intakeNoteCommand() { + return Commands.sequence( + moveToPositionCommand(() -> IntakePosition.GROUND), + moveToCurrentGoalCommand().alongWith(runRollersCommand()).until(shooterPrimaryBeam::get), + moveToPositionCommand(() -> IntakePosition.STOW)).withName("intake.intakeNote"); + } + + public Command intakeNoteAutoCommand() { + return Commands.sequence( + moveToPositionCommand(() -> IntakePosition.GROUND), + moveToCurrentGoalCommand().alongWith(runRollersCommand()).until(shooterPrimaryBeam::get)) + .withName("intake.intakeNoteAuto"); + } + + public Command ampScoreCommand(BooleanSupplier runRollers) { + return Commands.sequence( + moveToPositionCommand(() -> IntakePosition.GROUND), + moveToCurrentGoalCommand().alongWith(reverseRollersCommand()).until(intakeBeam::get), + moveToPositionCommand(() -> IntakePosition.AMP), + moveToCurrentGoalCommand().until(runRollers), + moveToCurrentGoalCommand().alongWith(reverseRollersCommand()).withTimeout(1), + moveToPositionCommand(() -> IntakePosition.STOW)).withName("intake.ampScore"); + } + + public Command simTriggerIntakeBeamCommand() { + return Commands.runOnce(() -> intakeBeamSim.setValue(true)) + .andThen(Commands.waitSeconds(1)) + .andThen(Commands.runOnce(() -> intakeBeamSim.setValue(false))) + .withName("intake.simTriggerIntakeBeam"); + } + + public Command simTriggerShooterBeamCommand() { + return Commands.runOnce(() -> shooterPrimaryBeamSim.setValue(true)) + .andThen(Commands.waitSeconds(1)) + .andThen(Commands.runOnce(() -> shooterPrimaryBeamSim.setValue(false))) + .withName("intake.simTriggerShooterBeam"); + } +} diff --git a/src/main/java/frc/robot/subsystems/LEDs.java b/src/main/java/frc/robot/subsystems/LEDs.java new file mode 100644 index 0000000..ae15eae --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LEDs.java @@ -0,0 +1,83 @@ +package frc.robot.subsystems; + +import java.util.ArrayList; +import java.util.function.Consumer; +import com.techhounds.houndutil.houndlog.interfaces.LoggedObject; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.AddressableLEDBuffer; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import static frc.robot.Constants.LEDs.*; +import static com.techhounds.houndutil.houndlib.leds.LEDPatterns.*; + +/** + * The LEDs subsystem, which controls the state of the LEDs through an internal + * state machine and continuously updates the LED's buffer. + * + * @author dr + */ +@LoggedObject +public class LEDs extends SubsystemBase { + /** The LEDs. */ + private AddressableLED leds = new AddressableLED(PORT); + private AddressableLEDBuffer buffer = new AddressableLEDBuffer(LENGTH); + + private ArrayList currentStates = new ArrayList(); + + public enum LEDState { + OFF(solid(Color.kBlack, LEDSection.SHOOTER_RIGHT)), + FLASHING_RED(flash(Color.kRed, 0.1, LEDSection.SHOOTER_RIGHT)), + FLASHING_ORANGE(flash(Color.kOrange, 1, LEDSection.SHOOTER_RIGHT)); + + private Consumer bufferConsumer; + + private LEDState(Consumer bufferConsumer) { + this.bufferConsumer = bufferConsumer; + } + } + + /** + * Initializes the LEDs. + */ + public LEDs() { + leds.setLength(LENGTH); + leds.setData(buffer); + leds.start(); + + setDefaultCommand(updateStateMachineCommand()); + } + + public Command requestFlashingRedCommand() { + return Commands.run(() -> currentStates.add(LEDState.FLASHING_RED)).ignoringDisable(true); + } + + public Command updateStateMachineCommand() { + return run(() -> { + clear(); + currentStates.sort((s1, s2) -> s2.ordinal() - s1.ordinal()); + currentStates.forEach((s) -> s.bufferConsumer.accept(buffer)); + leds.setData(buffer); + currentStates.clear(); + }) + .ignoringDisable(true) + .withName("Update State Machine"); + } + + public void clear() { + for (int i = 0; i < buffer.getLength(); i++) { + buffer.setLED(i, Color.kBlack); + } + } + + /** + * Sets the LEDs to the current state of the buffer. + */ + @Override + public void periodic() { + // super.periodic(); + // leds.setData(buffer); + } +} diff --git a/src/main/java/frc/robot/subsystems/NoteLift.java b/src/main/java/frc/robot/subsystems/NoteLift.java new file mode 100644 index 0000000..dec22a3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/NoteLift.java @@ -0,0 +1,200 @@ +package frc.robot.subsystems; + +import java.util.function.Supplier; + +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.techhounds.houndutil.houndlib.SparkConfigurator; +import com.techhounds.houndutil.houndlib.subsystems.BaseElevator; +import com.techhounds.houndutil.houndlog.interfaces.Log; +import com.techhounds.houndutil.houndlog.interfaces.LoggedObject; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.units.Distance; +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.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; +import frc.robot.Constants.NoteLift.NoteLiftPosition; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; +import static frc.robot.Constants.NoteLift.*; + +@LoggedObject +public class NoteLift extends SubsystemBase implements BaseElevator { + @Log + private CANSparkMax motor; + + @Log + private ProfiledPIDController pidController = new ProfiledPIDController(kP, kI, kD, MOVEMENT_CONSTRAINTS); + + @Log + private ElevatorFeedforward feedforwardController = new ElevatorFeedforward(kS, kG, kV, kA); + + private ElevatorSim elevatorSim = new ElevatorSim( + MOTOR_GEARBOX_REPR, + GEARING, + CARRIAGE_MASS_KG, + DRUM_RADIUS_METERS, + MIN_HEIGHT_METERS, + MAX_HEIGHT_METERS, + true, + 0); + + @Log(groups = "control") + private double feedbackVoltage = 0; + @Log(groups = "control") + private double feedforwardVoltage = 0; + + private double simVelocity = 0.0; + + private final MutableMeasure sysidAppliedVoltageMeasure = MutableMeasure.mutable(Volts.of(0)); + private final MutableMeasure sysidPositionMeasure = MutableMeasure.mutable(Meters.of(0)); + private final MutableMeasure> sysidVelocityMeasure = MutableMeasure + .mutable(MetersPerSecond.of(0)); + + private final SysIdRoutine sysIdRoutine; + + public NoteLift() { + motor = SparkConfigurator.createSparkMax( + MOTOR_ID, MotorType.kBrushless, false, + (s) -> s.setIdleMode(IdleMode.kBrake), + (s) -> s.setSmartCurrentLimit(CURRENT_LIMIT), + (s) -> s.getEncoder().setPositionConversionFactor(ENCODER_ROTATIONS_TO_METERS), + (s) -> s.getEncoder().setVelocityConversionFactor(ENCODER_ROTATIONS_TO_METERS / 60.0)); + + pidController.setTolerance(TOLERANCE); + + sysIdRoutine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> setVoltage(volts.magnitude()), + log -> { + log.motor("primary") + .voltage(sysidAppliedVoltageMeasure.mut_replace(motor.getAppliedOutput(), Volts)) + .linearPosition(sysidPositionMeasure.mut_replace(getPosition(), Meters)) + .linearVelocity(sysidVelocityMeasure.mut_replace(getVelocity(), MetersPerSecond)); + }, + this)); + + setDefaultCommand(moveToCurrentGoalCommand()); + } + + @Override + public void simulationPeriodic() { + elevatorSim.setInput(motor.getAppliedOutput()); + elevatorSim.update(0.020); + motor.getEncoder().setPosition(elevatorSim.getPositionMeters()); + simVelocity = elevatorSim.getVelocityMetersPerSecond(); + } + + @Override + @Log + public double getPosition() { + return motor.getEncoder().getPosition(); + } + + @Log + public double getVelocity() { + if (RobotBase.isReal()) + return motor.getEncoder().getVelocity(); + else + return simVelocity; + } + + @Log + public Pose3d getComponentPose() { + return BASE_COMPONENT_POSE.plus(new Transform3d(0, 0, getPosition(), new Rotation3d())); + } + + @Override + public void resetPosition() { + motor.getEncoder().setPosition(0); + } + + @Override + public void setVoltage(double voltage) { + motor.setVoltage(voltage); + } + + @Override + public Command moveToCurrentGoalCommand() { + return run(() -> { + feedbackVoltage = pidController.calculate(getPosition()); + feedforwardVoltage = feedforwardController.calculate(pidController.getSetpoint().position, + pidController.getSetpoint().velocity); + setVoltage(feedbackVoltage + feedforwardVoltage); + }).withName("noteLift.moveToCurrentGoal"); + } + + @Override + public Command moveToPositionCommand(Supplier goalPositionSupplier) { + return Commands.sequence( + runOnce(() -> pidController.reset(getPosition())), + runOnce(() -> pidController.setGoal(goalPositionSupplier.get().value)), + moveToCurrentGoalCommand().until(pidController::atGoal)).withName("noteLift.moveToPosition"); + } + + @Override + public Command moveToArbitraryPositionCommand(Supplier goalPositionSupplier) { + return Commands.sequence( + runOnce(() -> pidController.reset(getPosition())), + runOnce(() -> pidController.setGoal(goalPositionSupplier.get())), + moveToCurrentGoalCommand().until(pidController::atGoal)).withName("noteLift.moveToArbitraryPosition"); + } + + @Override + public Command movePositionDeltaCommand(Supplier delta) { + return moveToArbitraryPositionCommand(() -> pidController.getGoal().position + delta.get()) + .withName("noteLift.movePositionDelta"); + } + + @Override + public Command holdCurrentPositionCommand() { + return runOnce(() -> pidController.setGoal(getPosition())).andThen(moveToCurrentGoalCommand()) + .withName("noteLift.holdCurrentPosition"); + } + + @Override + public Command resetPositionCommand() { + return runOnce(this::resetPosition).withName("noteLift.resetPosition"); + } + + @Override + public Command setOverridenSpeedCommand(Supplier speed) { + return run(() -> setVoltage(12.0 * speed.get())) + .withName("noteLift.setOverriddenSpeed"); + } + + @Override + public Command coastMotorsCommand() { + return runOnce(motor::stopMotor) + .andThen(() -> motor.setIdleMode(IdleMode.kCoast)) + .finallyDo((d) -> { + motor.setIdleMode(IdleMode.kBrake); + pidController.reset(getPosition()); + }).withInterruptBehavior(InterruptionBehavior.kCancelIncoming) + .withName("noteLift.coastMotorsCommand"); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysIdRoutine.quasistatic(direction).withName("noteLift.sysIdQuasistatic"); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysIdRoutine.dynamic(direction).withName("noteLift.sysIdDynamic"); + } +} diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java new file mode 100644 index 0000000..8356e95 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -0,0 +1,170 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.techhounds.houndutil.houndlib.SparkConfigurator; +import com.techhounds.houndutil.houndlib.subsystems.BaseShooter; +import com.techhounds.houndutil.houndlog.interfaces.Log; +import com.techhounds.houndutil.houndlog.interfaces.LoggedObject; + +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Volts; +import static frc.robot.Constants.Shooter.*; + +import java.util.function.Supplier; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +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.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +@LoggedObject +public class Shooter extends SubsystemBase implements BaseShooter { + @Log + private final CANSparkFlex primaryMotor; + + @Log + private final CANSparkFlex secondaryMotor; + + @Log + private final PIDController pidController = new PIDController(kP, kI, kD); + @Log + private final SimpleMotorFeedforward feedforwardController = new SimpleMotorFeedforward(kS, kV, kA); + + private final FlywheelSim flywheelSim = new FlywheelSim(MOTOR_GEARBOX_REPR, GEARING, + MOMENT_OF_INERTIA_KG_METERS_SQUARED); + + @Log + private double feedforwardVoltage = 0.0; + @Log + private double feedbackVoltage = 0.0; + + private double simVelocity = 0.0; + + private final MutableMeasure sysidAppliedVoltageMeasure = MutableMeasure.mutable(Volts.of(0)); + private final MutableMeasure sysidPositionMeasure = MutableMeasure.mutable(Rotations.of(0)); + private final MutableMeasure> sysidVelocityMeasure = MutableMeasure + .mutable(RotationsPerSecond.of(0)); + + private final SysIdRoutine sysIdRoutine; + + public Shooter() { + primaryMotor = SparkConfigurator.createSparkFlex(PRIMARY_MOTOR_ID, MotorType.kBrushless, false, + (s) -> s.setIdleMode(IdleMode.kBrake), + (s) -> s.setSmartCurrentLimit(CURRENT_LIMIT), + (s) -> s.getEncoder().setPositionConversionFactor(1.0), + (s) -> s.getEncoder().setVelocityConversionFactor(1.0 / 60.0)); + + secondaryMotor = SparkConfigurator.createSparkFlex(SECONDARY_MOTOR_ID, MotorType.kBrushless, false, + (s) -> s.setIdleMode(IdleMode.kBrake), + (s) -> s.setSmartCurrentLimit(CURRENT_LIMIT), + (s) -> s.follow(primaryMotor, true)); + + sysIdRoutine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> setVoltage(volts.magnitude()), + log -> { + log.motor("primary") + .voltage(sysidAppliedVoltageMeasure.mut_replace(primaryMotor.getAppliedOutput(), + Volts)) + .angularPosition(sysidPositionMeasure.mut_replace(0, Rotations)) + .angularVelocity( + sysidVelocityMeasure.mut_replace(getVelocity(), RotationsPerSecond)); + }, + this)); + + setDefaultCommand(holdVelocityCommand(() -> IDLE_RPS)); + pidController.setTolerance(0.25); + + } + + @Override + public void simulationPeriodic() { + // set the input (the voltage of the motor) + flywheelSim.setInput(primaryMotor.getAppliedOutput()); + // update the sim + flywheelSim.update(0.020); + simVelocity = flywheelSim.getAngularVelocityRPM() / 60.0; + } + + @Override + public double getVelocity() { + if (RobotBase.isReal()) + return primaryMotor.getEncoder().getVelocity(); + else + return simVelocity; + } + + @Override + public void setVoltage(double voltage) { + primaryMotor.setVoltage(voltage > 12 ? 12 : (voltage < -12 ? -12 : voltage)); + } + + public void stop() { + primaryMotor.setVoltage(0); + } + + @Override + public Command spinAtVelocityCommand(Supplier goalVelocitySupplier) { + return run(() -> { + feedbackVoltage = pidController.calculate(getVelocity(), goalVelocitySupplier.get()); + feedforwardVoltage = feedforwardController.calculate(goalVelocitySupplier.get()); + setVoltage(feedbackVoltage + feedforwardVoltage); + }).withName("shooter.spinAtVelocity"); + } + + // will get up to speed, but slowly ramp down + public Command holdVelocityCommand(Supplier goalVelocitySupplier) { + return run(() -> { + feedbackVoltage = pidController.calculate(getVelocity(), goalVelocitySupplier.get()); + feedforwardVoltage = feedforwardController.calculate(goalVelocitySupplier.get()); + if (feedbackVoltage < 0) + feedbackVoltage = 0.0; + setVoltage(feedbackVoltage + feedforwardVoltage); + }).withName("shooter.holdVelocity"); + } + + @Override + public Command setOverridenSpeedCommand(Supplier speed) { + return run(() -> setVoltage(12.0 * speed.get())) + .withName("shooter.setOverriddenSpeed"); + } + + @Override + public Command coastMotorsCommand() { + return runOnce(() -> primaryMotor.stopMotor()) + .andThen(() -> { + primaryMotor.setIdleMode(IdleMode.kCoast); + secondaryMotor.setIdleMode(IdleMode.kCoast); + }) + .finallyDo((d) -> { + primaryMotor.setIdleMode(IdleMode.kBrake); + secondaryMotor.setIdleMode(IdleMode.kBrake); + }).withInterruptBehavior(InterruptionBehavior.kCancelIncoming) + .withName("shooter.coastMotors"); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysIdRoutine.quasistatic(direction).withName("shooter.sysIdQuasistatic"); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysIdRoutine.dynamic(direction).withName("shooter.sysIdDynamic"); + } + + public boolean atGoal() { + return pidController.atSetpoint(); + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterTilt.java b/src/main/java/frc/robot/subsystems/ShooterTilt.java new file mode 100644 index 0000000..24d5a0d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShooterTilt.java @@ -0,0 +1,244 @@ +package frc.robot.subsystems; + +import java.util.function.Supplier; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkFlex; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.techhounds.houndutil.houndlib.SparkConfigurator; +import com.techhounds.houndutil.houndlib.subsystems.BaseSingleJointedArm; +import com.techhounds.houndutil.houndlog.interfaces.Log; +import com.techhounds.houndutil.houndlog.interfaces.LoggedObject; + +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.units.Distance; +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.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants.ShooterTilt.ShooterTiltPosition; +import frc.robot.FieldConstants; + +import static frc.robot.Constants.ShooterTilt.*; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; + +@LoggedObject +public class ShooterTilt extends SubsystemBase implements BaseSingleJointedArm { + @Log + private final CANSparkFlex motor; + + @Log + private final ProfiledPIDController pidController = new ProfiledPIDController(kP, kI, kD, MOVEMENT_CONSTRAINTS); + + @Log + private final ElevatorFeedforward feedforwardController = new ElevatorFeedforward(kS, + kG, kV, kA); + + @Log + private final ElevatorSim elevatorSim = new ElevatorSim( + MOTOR_GEARBOX_REPR, + GEARING / ENCODER_ROTATIONS_TO_METERS, + MASS_KG, + 1 / (2.0 * Math.PI), + MIN_HEIGHT_METERS, + MAX_HEIGHT_METERS, + true, + 0); + + @Log(groups = "control") + private double feedbackVoltage = 0; + @Log(groups = "control") + private double feedforwardVoltage = 0; + + private double simVelocity = 0.0; + + private final MutableMeasure sysidAppliedVoltageMeasure = MutableMeasure.mutable(Volts.of(0)); + private final MutableMeasure sysidPositionMeasure = MutableMeasure.mutable(Meters.of(0)); + private final MutableMeasure> sysidVelocityMeasure = MutableMeasure + .mutable(MetersPerSecond.of(0)); + + private final SysIdRoutine sysIdRoutine; + + public ShooterTilt() { + motor = SparkConfigurator.createSparkFlex(MOTOR_ID, + MotorType.kBrushless, false, + (s) -> s.setIdleMode(IdleMode.kBrake), + (s) -> s.setSmartCurrentLimit(CURRENT_LIMIT), + (s) -> s.getEncoder().setPositionConversionFactor(ENCODER_ROTATIONS_TO_METERS), + (s) -> s.getEncoder().setVelocityConversionFactor(ENCODER_ROTATIONS_TO_METERS / + 60.0)); + + sysIdRoutine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> setVoltage(volts.magnitude()), + log -> { + log.motor("primary") + .voltage(sysidAppliedVoltageMeasure.mut_replace(motor.getAppliedOutput(), Volts)) + .linearPosition(sysidPositionMeasure.mut_replace(getPosition(), Meters)) + .linearVelocity(sysidVelocityMeasure.mut_replace(getVelocity(), MetersPerSecond)); + }, + this)); + + pidController.setTolerance(0.01); + + setDefaultCommand(moveToCurrentGoalCommand()); + } + + @Override + public void simulationPeriodic() { + elevatorSim.setInput(motor.getAppliedOutput()); + elevatorSim.update(0.020); + motor.getEncoder().setPosition(elevatorSim.getPositionMeters()); + simVelocity = elevatorSim.getVelocityMetersPerSecond(); + } + + @Log + public Pose3d getShooterComponentPose() { + return BASE_SHOOTER_POSE.plus(new Transform3d(0, 0, 0, new Rotation3d(0, -getAngle(), 0))); + } + + @Log + public Pose3d getOuterLeadScrewComponentPose() { + // small offset of 0.03rad added to fix an alignment issue between the two; + // since this is constant, it's likely a CAD zeroing problem + return BASE_OUTER_LEAD_SCREW_POSE + .plus(new Transform3d(0, 0, 0, new Rotation3d(0, -getLeadScrewAngle(getPosition()) + 0.03, 0))); + } + + @Log + public Pose3d getInnerLeadScrewComponentPose() { + return getOuterLeadScrewComponentPose().plus(OUTER_LEAD_SCREW_TO_INNER_LEAD_SCREW) + .plus(new Transform3d(getPosition(), 0, 0, new Rotation3d())); + } + + @Override + @Log + public double getPosition() { + return motor.getEncoder().getPosition(); + } + + @Log + public double getAngle() { + return getShooterAngle(getPosition()); + } + + @Log + public double getVelocity() { + if (RobotBase.isReal()) + return motor.getEncoder().getVelocity(); + else + return simVelocity; + } + + @Override + public void resetPosition() { + motor.getEncoder().setPosition(0); + } + + @Override + public void setVoltage(double voltage) { + motor.setVoltage(voltage > 12 ? 12 : voltage); + } + + @Override + public Command moveToCurrentGoalCommand() { + return run(() -> { + feedbackVoltage = pidController.calculate(getPosition()); + feedforwardVoltage = feedforwardController.calculate(pidController.getSetpoint().velocity); + setVoltage(feedbackVoltage + feedforwardVoltage); + }).withName("shooterTilt.moveToCurrentGoal"); + } + + @Override + public Command moveToPositionCommand(Supplier goalPositionSupplier) { + return Commands.sequence( + runOnce(() -> pidController.reset(getPosition())), + runOnce(() -> pidController.setGoal(getLinearActuatorLength(goalPositionSupplier.get().value))), + moveToCurrentGoalCommand().until(pidController::atGoal)) + .withName("shooterTilt.moveToPosition"); + } + + @Override + public Command moveToArbitraryPositionCommand(Supplier goalPositionSupplier) { + return Commands.sequence( + runOnce(() -> pidController.reset(getPosition())), + Commands.parallel( + Commands.run(() -> pidController.setGoal(getLinearActuatorLength(goalPositionSupplier.get()))), + moveToCurrentGoalCommand())) + .withName("shooterTilt.moveToArbitraryPosition"); + } + + public Command targetSpeakerCommand(Supplier poseSupplier) { + return moveToArbitraryPositionCommand(() -> { + Transform3d diff = new Pose3d(poseSupplier.get()).minus(FieldConstants.TARGET); + return new Rotation2d(Math.abs(diff.getX()), Math.abs(diff.getZ())).getRadians(); + }).withName("shooterTilt.targetSpeaker"); + } + + @Override + public Command movePositionDeltaCommand(Supplier delta) { + return Commands.sequence( + runOnce(() -> pidController.reset(getPosition())), + runOnce(() -> pidController.setGoal(getLinearActuatorLength(getAngle() + + delta.get()))), + moveToCurrentGoalCommand().until(pidController::atGoal)) + .withName("shooterTilt.movePositionDelta"); + } + + @Override + public Command holdCurrentPositionCommand() { + return runOnce(() -> pidController.setGoal(getPosition())) + .andThen(moveToCurrentGoalCommand()) + .withName("shooterTilt.holdCurrentPosition"); + } + + @Override + public Command resetPositionCommand() { + return runOnce(this::resetPosition).withName("shooterTilt.resetPosition"); + } + + @Override + public Command setOverridenSpeedCommand(Supplier speed) { + return run(() -> setVoltage(12.0 * speed.get())) + .withName("shooterTilt.setOverriddenSpeed"); + } + + @Override + public Command coastMotorsCommand() { + return runOnce(motor::stopMotor) + .andThen(() -> motor.setIdleMode(IdleMode.kCoast)) + .finallyDo((d) -> { + motor.setIdleMode(IdleMode.kBrake); + pidController.reset(getPosition()); + }).withInterruptBehavior(InterruptionBehavior.kCancelIncoming) + .withName("shooterTilt.coastMotors"); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysIdRoutine.quasistatic(direction).withName("shooterTilt.sysIdQuasistatic"); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysIdRoutine.dynamic(direction).withName("shooterTilt.sysIdDynamic"); + } + + public boolean atGoal() { + return pidController.atGoal(); + } +} diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java new file mode 100644 index 0000000..bf6ec2b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -0,0 +1,114 @@ +package frc.robot.subsystems; + +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import java.util.function.Supplier; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.simulation.VisionSystemSim; + +import com.techhounds.houndutil.houndlib.AprilTagPhotonCamera; +import com.techhounds.houndutil.houndlog.interfaces.Log; +import com.techhounds.houndutil.houndlog.interfaces.LoggedObject; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import static frc.robot.Constants.Vision.*; + +@LoggedObject +public class Vision extends SubsystemBase { + private SwerveDrivePoseEstimator poseEstimator = null; + private Supplier simPoseSupplier = null; + private final VisionSystemSim visionSim = new VisionSystemSim("main"); + + @Log(name = "houndeye01", groups = "cameras") + private final AprilTagPhotonCamera photonCamera1 = new AprilTagPhotonCamera("HoundEye01", + ROBOT_TO_CAMS[0], CAMERA_CONSTANTS, 0.64, 0.22); + @Log(name = "houndeye02", groups = "cameras") + private final AprilTagPhotonCamera photonCamera2 = new AprilTagPhotonCamera("HoundEye02", + ROBOT_TO_CAMS[1], CAMERA_CONSTANTS, 0.64, 0.22); + @Log(name = "houndeye03", groups = "cameras") + private final AprilTagPhotonCamera photonCamera3 = new AprilTagPhotonCamera("HoundEye03", + ROBOT_TO_CAMS[2], CAMERA_CONSTANTS, 0.64, 0.22); + + private final AprilTagPhotonCamera[] photonCameras = new AprilTagPhotonCamera[] { + photonCamera1, photonCamera2, photonCamera3 }; + + public Vision() { + AprilTagFieldLayout tagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + visionSim.addAprilTags(tagLayout); + for (AprilTagPhotonCamera photonCamera : photonCameras) { + visionSim.addCamera(photonCamera.getSim(), photonCamera.getRobotToCam()); + } + } + + @Override + public void periodic() { + updatePoseEstimator(); + } + + @Override + public void simulationPeriodic() { + visionSim.update(simPoseSupplier.get()); + } + + public void updatePoseEstimator() { + if (poseEstimator == null) { + return; + } + + Pose2d prevEstimatedRobotPose = poseEstimator.getEstimatedPosition(); + for (AprilTagPhotonCamera photonCamera : photonCameras) { + Optional result = photonCamera + .getEstimatedGlobalPose(prevEstimatedRobotPose); + + if (result.isPresent()) { + EstimatedRobotPose estPose = result.get(); + Matrix stddevs = photonCamera.getEstimationStdDevs(estPose.estimatedPose.toPose2d(), + SINGLE_TAG_STD_DEVS, MULTI_TAG_STD_DEVS); + poseEstimator.addVisionMeasurement(estPose.estimatedPose.toPose2d(), + estPose.timestampSeconds, stddevs); + } + } + } + + @Log + public Pose3d[] cameraPoses() { + List poses = new ArrayList(); + for (Transform3d transform : ROBOT_TO_CAMS) { + poses.add(new Pose3d(poseEstimator.getEstimatedPosition()).plus(transform) + .plus(new Transform3d(0, 0, 0, new Rotation3d(0, 0, Math.PI)))); + } + Pose3d[] poseArray = new Pose3d[poses.size()]; + return poses.toArray(poseArray); + } + + @Log + public Pose3d[] aprilTagPoses() { + List poses = new ArrayList(); + for (AprilTag tag : AprilTagFields.kDefaultField.loadAprilTagLayoutField().getTags()) { + poses.add(tag.pose); + } + Pose3d[] poseArray = new Pose3d[poses.size()]; + return poses.toArray(poseArray); + } + + public void setPoseEstimator(SwerveDrivePoseEstimator poseEstimator) { + this.poseEstimator = poseEstimator; + } + + public void setSimPoseSupplier(Supplier simPoseSupplier) { + this.simPoseSupplier = simPoseSupplier; + } +} diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json index 2a29835..e978a5f 100644 --- a/vendordeps/NavX.json +++ b/vendordeps/NavX.json @@ -1,7 +1,7 @@ { "fileName": "NavX.json", "name": "NavX", - "version": "2024.0.1-beta-4", + "version": "2024.1.0", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-frc-java", - "version": "2024.0.1-beta-4" + "version": "2024.1.0" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-frc-cpp", - "version": "2024.0.1-beta-4", + "version": "2024.1.0", "headerClassifier": "headers", "sourcesClassifier": "sources", "sharedLibrary": false, diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 3e18a45..ff15fab 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.0.0-beta-6.2", + "version": "2024.2.3", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.0.0-beta-6.2" + "version": "2024.2.3" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.0.0-beta-6.2", + "version": "2024.2.3", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 7020206..2b7d172 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,13 +1,13 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.0.0-beta-8", + "version": "24.2.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json", "conflictsWith": [ { "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.0.0-beta-8" + "version": "24.2.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.0.0-beta-8", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.0.0-beta-8", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.0.0-beta-8", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.0.0-beta-8", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.0.0-beta-8", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-8", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.0.0-beta-8", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.0.0-beta-8", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.0.0-beta-8", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.0.0-beta-8", + "version": "24.2.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.0.0-beta-8", + "version": "24.2.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.0.0-beta-8", + "version": "24.2.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.0.0-beta-8", + "version": "24.2.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.0.0-beta-8", + "version": "24.2.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.0.0-beta-8", + "version": "24.2.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.0.0-beta-8", + "version": "24.2.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.0.0-beta-8", + "version": "24.2.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.0.0-beta-8", + "version": "24.2.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.0.0-beta-8", + "version": "24.2.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.0.0-beta-8", + "version": "24.2.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.0.0-beta-8", + "version": "24.2.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.0.0-beta-8", + "version": "24.2.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 0f3520e..a829581 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.0", + "version": "2024.2.1", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.0" + "version": "2024.2.1" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.0", + "version": "2024.2.1", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.0", + "version": "2024.2.1", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index dfd18fd..46211fc 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,21 +1,36 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "dev-v2023.4.2-83-gd61225eb", + "version": "v2024.2.4", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ "https://maven.photonvision.org/repository/internal", "https://maven.photonvision.org/repository/snapshots" ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json", + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", "jniDependencies": [], "cppDependencies": [ { "groupId": "org.photonvision", - "artifactId": "PhotonLib-cpp", - "version": "dev-v2023.4.2-83-gd61225eb", - "libName": "Photon", + "artifactId": "photonlib-cpp", + "version": "v2024.2.4", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2024.2.4", + "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -30,13 +45,13 @@ "javaDependencies": [ { "groupId": "org.photonvision", - "artifactId": "PhotonLib-java", - "version": "dev-v2023.4.2-83-gd61225eb" + "artifactId": "photonlib-java", + "version": "v2024.2.4" }, { "groupId": "org.photonvision", - "artifactId": "PhotonTargeting-java", - "version": "dev-v2023.4.2-83-gd61225eb" + "artifactId": "photontargeting-java", + "version": "v2024.2.4" } ] } \ No newline at end of file