diff --git a/.AdvantageScope/2-7-24-copy-of-config.json b/.AdvantageScope/config.json similarity index 100% rename from .AdvantageScope/2-7-24-copy-of-config.json rename to .AdvantageScope/config.json diff --git a/.AdvantageScope/model.glb b/.AdvantageScope/model.glb new file mode 100644 index 0000000..a92882a Binary files /dev/null and b/.AdvantageScope/model.glb differ diff --git a/.AdvantageScope/model_0.glb b/.AdvantageScope/model_0.glb new file mode 100644 index 0000000..c0f7c86 Binary files /dev/null and b/.AdvantageScope/model_0.glb differ diff --git a/.AdvantageScope/model_1.glb b/.AdvantageScope/model_1.glb new file mode 100644 index 0000000..4a8b1dd Binary files /dev/null and b/.AdvantageScope/model_1.glb differ diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index ba0370f..47ce758 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -11,8 +11,10 @@ jobs: container: wpilib/roborio-cross-ubuntu:2024-22.04 steps: - name: Checkout repository - uses: actions/checkout@v2 + uses: actions/checkout@v4 - name: Grant execute permission run: chmod +x gradlew + - name: Check formatting + run: ./gradlew spotlessCheck - name: Build robot code run: ./gradlew build diff --git a/.gitignore b/.gitignore index cab2896..1ae6db4 100644 --- a/.gitignore +++ b/.gitignore @@ -180,4 +180,5 @@ logs/ # Folder that has CTRE Phoenix Sim device config storage ctre_sim/ -.SysId/ \ No newline at end of file +.SysId/ +.DataLogTool/ \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json index b4b9950..b8c1920 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -1,13 +1,6 @@ { "version": "0.2.0", "configurations": [ - { - "type": "java", - "name": "Main", - "request": "launch", - "mainClass": "frc.robot.Main", - "projectName": "2024-Crescendo" - }, { "type": "wpilib", "name": "WPILib Desktop Debug", diff --git a/.vscode/settings.json b/.vscode/settings.json index d9e9859..1127fa4 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -28,6 +28,15 @@ } ], "java.test.defaultConfig": "WPIlibUnitTests", + "java.format.enabled": false, + "editor.detectIndentation": false, + "editor.insertSpaces": true, + "editor.tabSize": 2, + "editor.formatOnSave": true, + "editor.formatOnPaste": true, + "editor.formatOnType": true, + "editor.formatOnSaveMode": "file", + "editor.autoIndent": "full", "spotlessGradle.format.enable": true, "spotlessGradle.diagnostics.enable": false, "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle", diff --git a/ChoreoProject.chor b/ChoreoProject.chor index c73ce5a..bb83d6b 100644 --- a/ChoreoProject.chor +++ b/ChoreoProject.chor @@ -1,19 +1,19 @@ { - "version": "v0.3", + "version": "v0.3.1", "robotConfiguration": { - "mass": 65.1974197627209, - "rotationalInertia": 15, - "motorMaxTorque": 1.1251759834368529, - "motorMaxVelocity": 3600, - "gearing": 6.593406593406593, + "mass": 71.61837776965554, + "rotationalInertia": 7, + "motorMaxTorque": 0.49, + "motorMaxVelocity": 4600, + "gearing": 6.122448979591837, "wheelbase": 0.40004978397311663, "trackWidth": 0.5270497153931537, "bumperLength": 0.8254998984, "bumperWidth": 0.8254998984, - "wheelRadius": 0.04914897345955433 + "wheelRadius": 0.04394197627133281 }, "paths": { - "Centerline Auto": { + "3PieceAmpSide": { "waypoints": [ { "x": 0.660783052444458, @@ -22,75 +22,84 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 22 }, { - "x": 2.486186981201172, - "y": 7.724460124969482, + "x": 2.8914051055908203, + "y": 7.769551753997803, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 21 + "controlIntervalCount": 15 }, { - "x": 8.360527038574219, - "y": 7.469053745269775, + "x": 4.91845178604126, + "y": 7.730569362640381, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 16 }, { - "x": 5.0970048904418945, - "y": 6.844727993011475, - "heading": 0.2938692977225419, + "x": 7.296333312988281, + "y": 7.438207149505615, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, + "headingConstrained": true, "controlIntervalCount": 11 }, { - "x": 2.897675037384033, - "y": 6.319726943969727, - "heading": 0.23441548450132177, + "x": 8.290366172790527, + "y": 7.457698345184326, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 24 }, { - "x": 4.501265048980713, - "y": 6.1953558921813965, - "heading": 0, + "x": 3.8659467697143555, + "y": 6.424684047698975, + "heading": 0.1608595979864649, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 6.263319492340088, + "y": 6.385702133178711, + "heading": -0.1882215588380072, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 15 + "controlIntervalCount": 16 }, { - "x": 8.275391578674316, - "y": 5.78053617477417, - "heading": 0, + "x": 8.270874977111816, + "y": 5.7814860343933105, + "heading": -0.4182234468320189, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 16 }, { - "x": 5.139572620391846, - "y": 6.3622941970825195, - "heading": 0, + "x": 6.263319492340088, + "y": 6.385702133178711, + "heading": -0.2551830834264048, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 12 + "headingConstrained": true, + "controlIntervalCount": 18 }, { - "x": 2.897675037384033, - "y": 6.319726943969727, - "heading": 0.23441548450132177, + "x": 3.8659467697143555, + "y": 6.424684047698975, + "heading": 0.1608595979864649, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -102,1018 +111,1414 @@ "x": 0.660783052444458, "y": 6.694385051727295, "heading": 1.0498621745809171, - "angularVelocity": 5.825284855617402e-28, - "velocityX": -1.9685293298057674e-27, - "velocityY": 1.4418564963898336e-27, + "angularVelocity": 5.036142496978594e-25, + "velocityX": 4.0909648611784826e-25, + "velocityY": -1.2650073502903171e-25, "timestamp": 0 }, { - "x": 0.6784036347143663, - "y": 6.707198268292706, - "heading": 1.0175475056665193, - "angularVelocity": -0.5991822637957332, - "velocityX": 0.32672283914932904, - "velocityY": 0.23758411786628814, - "timestamp": 0.05393128412995569 - }, - { - "x": 0.7135162843776108, - "y": 6.732573985913541, - "heading": 0.9516130808872657, - "angularVelocity": -1.2225635981589924, - "velocityX": 0.6510627408506594, - "velocityY": 0.4705194402508243, - "timestamp": 0.10786256825991138 - }, - { - "x": 0.7660994692609764, - "y": 6.770017684654193, - "heading": 0.8504346226500515, - "angularVelocity": -1.8760624722639487, - "velocityX": 0.9750033905489506, - "velocityY": 0.6942853177837462, - "timestamp": 0.16179385238986707 - }, - { - "x": 0.836479083356807, - "y": 6.818804043384565, - "heading": 0.7130270962325688, - "angularVelocity": -2.5478259721459318, - "velocityX": 1.3049868036933845, - "velocityY": 0.9046022084846815, - "timestamp": 0.21572513651982275 - }, - { - "x": 0.9255037273969096, - "y": 6.878132431612943, - "heading": 0.5402736783143448, - "angularVelocity": -3.2032135096569965, - "velocityX": 1.6507050680563082, - "velocityY": 1.1000737176110378, - "timestamp": 0.26965642064977846 - }, - { - "x": 1.035016950774323, - "y": 6.947674803955847, - "heading": 0.3381434888609651, - "angularVelocity": -3.747920946334524, - "velocityX": 2.0306066347970617, - "velocityY": 1.289462571952319, - "timestamp": 0.32358770477973414 - }, - { - "x": 1.162461605941772, - "y": 7.018689321892079, - "heading": 0.18269313635392612, - "angularVelocity": -2.882378104190095, - "velocityX": 2.3630932810787844, - "velocityY": 1.3167592628633178, - "timestamp": 0.3775189889096898 - }, - { - "x": 1.3050246421848908, - "y": 7.094859887834467, - "heading": 0.07841153426675496, - "angularVelocity": -1.9336013182235503, - "velocityX": 2.6434200212921195, - "velocityY": 1.4123632910138557, - "timestamp": 0.4314502730396455 - }, - { - "x": 1.4591047302314346, - "y": 7.181935811218979, - "heading": 0.022297438159640273, - "angularVelocity": -1.0404739477721165, - "velocityX": 2.8569705048235896, - "velocityY": 1.6145716681748061, - "timestamp": 0.4853815571696012 - }, - { - "x": 1.6255168678692307, - "y": 7.279018436792272, - "heading": 0.014123453422294747, - "angularVelocity": -0.15156295402959544, - "velocityX": 3.0856327699670643, - "velocityY": 1.800117077489892, - "timestamp": 0.5393128412995569 - }, - { - "x": 1.793858101376193, - "y": 7.378107838800647, - "heading": 0.014120593150970575, - "angularVelocity": -0.00005303547598235602, - "velocityX": 3.121402284828195, - "velocityY": 1.8373269542331496, - "timestamp": 0.5932441254295127 - }, - { - "x": 1.9621993268656661, - "y": 7.477197254469766, - "heading": 0.014117732942591941, - "angularVelocity": -0.000053034308839039934, - "velocityX": 3.121402136167001, - "velocityY": 1.8373272075322298, - "timestamp": 0.6471754095594684 - }, - { - "x": 2.130540737170093, - "y": 7.576286356115685, - "heading": 0.014114872666943619, - "angularVelocity": -0.00005303555616123241, - "velocityX": 3.121405563026878, - "velocityY": 1.8373213848783807, - "timestamp": 0.7011066936894241 - }, - { - "x": 2.3040566274888983, - "y": 7.663067241706454, - "heading": 0.010006762068371105, - "angularVelocity": -0.0761730536338314, - "velocityX": 3.217351359568822, - "velocityY": 1.609101043870144, - "timestamp": 0.7550379778193799 - }, - { - "x": 2.486186981201172, - "y": 7.724460124969482, - "heading": -5.2714780410694486e-27, - "angularVelocity": -0.18554651960925458, - "velocityX": 3.377081718903682, - "velocityY": 1.1383538191876352, - "timestamp": 0.8089692619493356 - }, - { - "x": 2.8066356107472474, - "y": 7.755704422012626, - "heading": -3.618373256811644e-9, - "angularVelocity": -4.070535151890212e-8, - "velocityX": 3.6049277350997357, - "velocityY": 0.3514867051672969, - "timestamp": 0.8978610936568053 - }, - { - "x": 3.128175858643877, - "y": 7.739109235929212, - "heading": -3.6183723834730783e-9, - "angularVelocity": 9.824733603484871e-15, - "velocityX": 3.6172080349831512, - "velocityY": -0.18668966275806864, - "timestamp": 0.9867529253642751 - }, - { - "x": 3.449716088164231, - "y": 7.722513693799808, - "heading": -3.6183716236893314e-9, - "angularVelocity": 8.547284180686901e-15, - "velocityX": 3.6172078282568996, - "velocityY": -0.18669366814284583, - "timestamp": 1.0756447570717447 - }, - { - "x": 3.771256317684507, - "y": 7.7059181516688815, - "heading": -3.6183708487107216e-9, - "angularVelocity": 8.718220728132132e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.18669366815997415, - "timestamp": 1.1645365887792143 - }, - { - "x": 4.092796547204784, - "y": 7.689322609537955, - "heading": -3.618370111441706e-9, - "angularVelocity": 8.29400184228923e-15, - "velocityX": 3.6172078282560163, - "velocityY": -0.18669366815997418, - "timestamp": 1.253428420486684 - }, - { - "x": 4.414336776725059, - "y": 7.6727270674070285, - "heading": -3.6183692900088547e-9, - "angularVelocity": 9.240813644117634e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.1866936681599742, - "timestamp": 1.3423202521941535 - }, - { - "x": 4.735877006245335, - "y": 7.656131525276102, - "heading": -3.6183684638446085e-9, - "angularVelocity": 9.294040074640308e-15, - "velocityX": 3.6172078282560163, - "velocityY": -0.1866936681599742, - "timestamp": 1.4312120839016231 - }, - { - "x": 5.0574172357656115, - "y": 7.6395359831451755, - "heading": -3.618367662939708e-9, - "angularVelocity": 9.009881839671523e-15, - "velocityX": 3.6172078282560163, - "velocityY": -0.1866936681599742, - "timestamp": 1.5201039156090927 - }, - { - "x": 5.378957465285888, - "y": 7.62294044101425, - "heading": -3.6183667762068362e-9, - "angularVelocity": 9.975414552450845e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.18669366815997418, - "timestamp": 1.6089957473165624 - }, - { - "x": 5.700497694806164, - "y": 7.606344898883323, - "heading": -3.6183660187139743e-9, - "angularVelocity": 8.521512579724434e-15, - "velocityX": 3.6172078282560163, - "velocityY": -0.18669366815997426, - "timestamp": 1.697887579024032 - }, - { - "x": 6.02203792432644, - "y": 7.589749356752397, - "heading": -3.618365173816459e-9, - "angularVelocity": 9.504782370286536e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.1866936681599742, - "timestamp": 1.7867794107315016 - }, - { - "x": 6.343578153846717, - "y": 7.57315381462147, - "heading": -3.6183643942346165e-9, - "angularVelocity": 8.770005387165316e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.1866936681599742, - "timestamp": 1.8756712424389712 - }, - { - "x": 6.665118383366993, - "y": 7.556558272490544, - "heading": -3.6183635769900644e-9, - "angularVelocity": 9.193696838193051e-15, - "velocityX": 3.6172078282560163, - "velocityY": -0.18669366815997424, - "timestamp": 1.9645630741464408 - }, - { - "x": 6.986658612887269, - "y": 7.539962730359617, - "heading": -3.6183627012139706e-9, - "angularVelocity": 9.85215488559423e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.1866936681599742, - "timestamp": 2.0534549058539104 - }, - { - "x": 7.3081988424075455, - "y": 7.523367188228691, - "heading": -3.618361818393418e-9, - "angularVelocity": 9.931402417986347e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.1866936681599742, - "timestamp": 2.1423467375613803 - }, - { - "x": 7.629739071927805, - "y": 7.506771646097765, - "heading": -3.6183609804060354e-9, - "angularVelocity": 9.427045955569345e-15, - "velocityX": 3.617207828255831, - "velocityY": -0.1866936681599647, - "timestamp": 2.23123856926885 - }, - { - "x": 7.922054241421621, - "y": 7.491684486652487, - "heading": -2.171017261999522e-9, - "angularVelocity": 1.6282077786061535e-8, - "velocityX": 3.288436787485536, - "velocityY": -0.16972492472567544, - "timestamp": 2.32013040097632 - }, - { - "x": 8.14129063555876, - "y": 7.480369116190248, - "heading": -1.0855088008297502e-9, - "angularVelocity": 1.221156590340072e-8, - "velocityX": 2.466327782046558, - "velocityY": -0.12729370342458965, - "timestamp": 2.40902223268379 - }, - { - "x": 8.287448236582586, - "y": 7.472825535627514, - "heading": -3.6183629850910975e-10, - "angularVelocity": 8.141046127861815e-9, - "velocityX": 1.6442185768520245, - "velocityY": -0.08486247181359102, - "timestamp": 2.4979140643912596 - }, - { - "x": 8.360527038574219, - "y": 7.469053745269775, - "heading": 6.2013561613280124e-27, - "angularVelocity": 4.070523596587155e-9, - "velocityX": 0.8221093050723073, - "velocityY": -0.042431236765954845, - "timestamp": 2.5868058960987295 - }, - { - "x": 8.360527038574219, - "y": 7.469053745269775, - "heading": 4.2684027559613635e-27, - "angularVelocity": -4.179791912699714e-28, - "velocityX": 2.3088842991913657e-27, - "velocityY": -4.6090179027433225e-27, - "timestamp": 2.6756977278061993 - }, - { - "x": 8.313753662903038, - "y": 7.460040831603572, - "heading": 0.010953449074032317, - "angularVelocity": 0.1521110200792272, - "velocityX": -0.6495438868437788, - "velocityY": -0.12516272111059268, - "timestamp": 2.74770729731883 - }, - { - "x": 8.220205864520521, - "y": 7.442024067957108, - "heading": 0.03284727412772724, - "angularVelocity": 0.30404049353266294, - "velocityX": -1.299102313979371, - "velocityY": -0.2501995744232807, - "timestamp": 2.819716866831461 - }, - { - "x": 8.079881048506913, - "y": 7.415017178316717, - "heading": 0.06565325181712828, - "angularVelocity": 0.45557802819035925, - "velocityX": -1.9486967768776025, - "velocityY": -0.37504584214539577, - "timestamp": 2.8917264363440918 - }, - { - "x": 7.892775351387043, - "y": 7.379038650911545, - "heading": 0.10933893425739379, - "angularVelocity": 0.6066649576706988, - "velocityX": -2.598344892022307, - "velocityY": -0.4996353630313118, - "timestamp": 2.9637360058567226 - }, - { - "x": 7.65888403905923, - "y": 7.3341118085455905, - "heading": 0.1638822878842525, - "angularVelocity": 0.7574459060929595, - "velocityX": -3.248058749841399, - "velocityY": -0.623900999131414, - "timestamp": 3.0357455753693534 - }, - { - "x": 7.402696123218692, - "y": 7.2851734321166655, - "heading": 0.1638823285777561, - "angularVelocity": 5.651124407641853e-7, - "velocityX": -3.557692645219899, - "velocityY": -0.6796093458153674, - "timestamp": 3.1077551448819842 - }, - { - "x": 7.146508206567032, - "y": 7.2362350599332865, - "heading": 0.16388236927081118, - "angularVelocity": 5.651062127144245e-7, - "velocityX": -3.557692656483975, - "velocityY": -0.6796092868572866, - "timestamp": 3.179764714394615 - }, - { - "x": 6.8903202899153655, - "y": 7.187296687749944, - "heading": 0.16388240996386633, - "angularVelocity": 5.651062132417643e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567837, - "timestamp": 3.251774283907246 - }, - { - "x": 6.634132373263698, - "y": 7.138358315566601, - "heading": 0.16388245065692145, - "angularVelocity": 5.651062127692066e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567831, - "timestamp": 3.3237838534198767 - }, - { - "x": 6.377944456612031, - "y": 7.089419943383258, - "heading": 0.1638824913499765, - "angularVelocity": 5.651062124923317e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567826, - "timestamp": 3.3957934229325075 - }, - { - "x": 6.121756539960365, - "y": 7.040481571199916, - "heading": 0.1638825320430316, - "angularVelocity": 5.65106212535781e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567821, - "timestamp": 3.4678029924451383 - }, - { - "x": 5.865568623308698, - "y": 6.991543199016573, - "heading": 0.16388257273608667, - "angularVelocity": 5.651062121582895e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567813, - "timestamp": 3.539812561957769 - }, - { - "x": 5.6093807066570305, - "y": 6.94260482683323, - "heading": 0.16388261342914182, - "angularVelocity": 5.651062135141695e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567811, - "timestamp": 3.6118221314704 - }, - { - "x": 5.35319279000551, - "y": 6.893666454649124, - "heading": 0.16388265412219696, - "angularVelocity": 5.651062132389692e-7, - "velocityX": -3.5576926564820495, - "velocityY": -0.6796092868673679, - "timestamp": 3.683831700983031 - }, - { - "x": 5.0970048904418945, - "y": 6.844727993011475, - "heading": 0.16388269481525294, - "angularVelocity": 5.651062246743821e-7, - "velocityX": -3.5576924191815573, - "velocityY": -0.6796105291126032, - "timestamp": 3.7558412704956616 - }, - { - "x": 4.821698218246571, - "y": 6.778961174770714, - "heading": 0.16388272951436206, - "angularVelocity": 4.4401904271117674e-7, - "velocityX": -3.522897507544664, - "velocityY": -0.8415697237266404, - "timestamp": 3.833989065487064 - }, - { - "x": 4.546391565680663, - "y": 6.7131942743592505, - "heading": 0.163882764213471, - "angularVelocity": 4.440190404819063e-7, - "velocityX": -3.5228972563614294, - "velocityY": -0.8415707752048409, - "timestamp": 3.9121368604784665 - }, - { - "x": 4.271084913114876, - "y": 6.647427373947279, - "heading": 0.16388279891257995, - "angularVelocity": 4.440190403154282e-7, - "velocityX": -3.522897256359876, - "velocityY": -0.8415707752113447, - "timestamp": 3.990284655469869 - }, - { - "x": 3.995778260549088, - "y": 6.5816604735353135, - "heading": 0.1638828336116889, - "angularVelocity": 4.440190406386135e-7, - "velocityX": -3.5228972563598964, - "velocityY": -0.8415707752112599, - "timestamp": 4.068432450461271 - }, - { - "x": 3.7204716077277675, - "y": 6.515893574193041, - "heading": 0.16388286831080823, - "angularVelocity": 4.440191733622333e-7, - "velocityX": -3.522897259629757, - "velocityY": -0.8415707615231752, - "timestamp": 4.146580245452674 - }, - { - "x": 3.446523890940015, - "y": 6.450633718978252, - "heading": 0.1690942590558449, - "angularVelocity": 0.06668634406908087, - "velocityX": -3.50550795218074, - "velocityY": -0.8350824898126525, - "timestamp": 4.224728040444077 - }, - { - "x": 3.2268656747177786, - "y": 6.398260340243826, - "heading": 0.190922516728536, - "angularVelocity": 0.27932019931071, - "velocityX": -2.8108050425018627, - "velocityY": -0.670183704353886, - "timestamp": 4.30287583543548 - }, - { - "x": 3.062219316413325, - "y": 6.3589866212900406, - "heading": 0.21130201229843995, - "angularVelocity": 0.2607814535540765, - "velocityX": -2.1068586557377182, - "velocityY": -0.5025569686016857, - "timestamp": 4.381023630426883 - }, - { - "x": 2.9525091880643317, - "y": 6.3328115268917164, - "heading": 0.22637932021512078, - "angularVelocity": 0.1929332480633594, - "velocityX": -1.4038800245235723, - "velocityY": -0.334943479866623, - "timestamp": 4.459171425418286 - }, - { - "x": 2.897675037384033, - "y": 6.319726943969727, - "heading": 0.23441548450132177, - "angularVelocity": 0.10283289870283706, - "velocityX": -0.7016723976195487, - "velocityY": -0.1674338082530608, - "timestamp": 4.537319220409689 - }, - { - "x": 2.897675037384033, - "y": 6.319726943969727, - "heading": 0.23441548450132177, - "angularVelocity": 2.348167343500178e-28, - "velocityX": 2.328514438851991e-27, - "velocityY": -3.011007529624761e-27, - "timestamp": 4.615467015401092 - }, - { - "x": 2.931995611661183, - "y": 6.317030660120305, - "heading": 0.22827399101803314, - "angularVelocity": -0.1004895685231517, - "velocityX": 0.5615669396966905, - "velocityY": -0.04411767290505802, - "timestamp": 4.6765827468501096 - }, - { - "x": 3.0006366597167062, - "y": 6.311640001770152, - "heading": 0.21599961790492203, - "angularVelocity": -0.20083819373658737, - "velocityX": 1.1231322349922719, - "velocityY": -0.08820410428451562, - "timestamp": 4.737698478299127 - }, - { - "x": 3.1035985534895527, - "y": 6.303558029226905, - "heading": 0.1976138856084477, - "angularVelocity": -0.30083469281246417, - "velocityX": 1.684703616101463, - "velocityY": -0.13224046168846443, - "timestamp": 4.798814209748145 - }, - { - "x": 3.2408820865427965, - "y": 6.29278900601246, - "heading": 0.173149008298291, - "angularVelocity": -0.4003040907816843, - "velocityX": 2.2462879818065202, - "velocityY": -0.17620705764487352, - "timestamp": 4.859929941197163 - }, - { - "x": 3.412488399373154, - "y": 6.279338451783206, - "heading": 0.14264484283477577, - "angularVelocity": -0.4991213348884074, - "velocityX": 2.807891008774219, - "velocityY": -0.22008333877955472, - "timestamp": 4.921045672646181 - }, - { - "x": 3.618418876396286, - "y": 6.263213190116882, - "heading": 0.10614503413984216, - "angularVelocity": -0.597224443355985, - "velocityX": 3.3695166881037326, - "velocityY": -0.26384796981077935, - "timestamp": 4.982161404095199 - }, - { - "x": 3.839130420023726, - "y": 6.246248871896369, - "heading": 0.10614500290272542, - "angularVelocity": -5.111141764182067e-7, - "velocityX": 3.611370401605275, - "velocityY": -0.2775769481653736, - "timestamp": 5.0432771355442165 - }, - { - "x": 4.059841964366672, - "y": 6.229284562983422, - "heading": 0.10614497166605225, - "angularVelocity": -5.111069183870487e-7, - "velocityX": 3.6113704133126814, - "velocityY": -0.2775767958712516, - "timestamp": 5.104392866993234 - }, - { - "x": 4.280553508709572, - "y": 6.212320254069855, - "heading": 0.10614494042937911, - "angularVelocity": -5.111069180282712e-7, - "velocityX": 3.6113704133119002, - "velocityY": -0.2775767958814193, - "timestamp": 5.165508598442252 - }, - { - "x": 4.501265048980713, - "y": 6.1953558921813965, - "heading": 0.1061449091927048, - "angularVelocity": -5.111069369418327e-7, - "velocityX": 3.611370346688179, - "velocityY": -0.27757766267774153, - "timestamp": 5.22662432989127 - }, - { - "x": 4.810869637193169, - "y": 6.16135848513122, - "heading": 0.10614488355477533, - "angularVelocity": -2.98142570668873e-7, - "velocityX": 3.6003807524808256, - "velocityY": -0.39535463826432365, - "timestamp": 5.312616509468145 - }, - { - "x": 5.120474220145008, - "y": 6.12736103017416, - "heading": 0.1061448579168455, - "angularVelocity": -2.9814257488756455e-7, - "velocityX": 3.600380691305304, - "velocityY": -0.39535519537177727, - "timestamp": 5.3986086890450204 - }, - { - "x": 5.430078803096823, - "y": 6.093363575216878, - "heading": 0.1061448322789156, - "angularVelocity": -2.981425755600158e-7, - "velocityX": 3.6003806913050207, - "velocityY": -0.39535519537435887, - "timestamp": 5.484600868621896 - }, - { - "x": 5.739683386048638, - "y": 6.059366120259595, - "heading": 0.10614480664098574, - "angularVelocity": -2.981425750224502e-7, - "velocityX": 3.60038069130502, - "velocityY": -0.39535519537435865, - "timestamp": 5.570593048198771 - }, - { - "x": 6.049287969000453, - "y": 6.025368665302312, - "heading": 0.10614478100305595, - "angularVelocity": -2.981425744595309e-7, - "velocityX": 3.60038069130502, - "velocityY": -0.3953551953743586, - "timestamp": 5.656585227775646 - }, - { - "x": 6.358892551952268, - "y": 5.991371210345029, - "heading": 0.1061447553651262, - "angularVelocity": -2.98142573863633e-7, - "velocityX": 3.6003806913050203, - "velocityY": -0.3953551953743583, - "timestamp": 5.742577407352521 - }, - { - "x": 6.668497134904083, - "y": 5.9573737553877475, - "heading": 0.10614472972719659, - "angularVelocity": -2.981425722498942e-7, - "velocityX": 3.6003806913050203, - "velocityY": -0.3953551953743583, - "timestamp": 5.8285695869293965 - }, - { - "x": 6.978101717855898, - "y": 5.923376300430465, - "heading": 0.10614470408926703, - "angularVelocity": -2.981425716615336e-7, - "velocityX": 3.6003806913050207, - "velocityY": -0.39535519537435826, - "timestamp": 5.914561766506272 - }, - { - "x": 7.287706300807711, - "y": 5.889378845473171, - "heading": 0.10614467845133739, - "angularVelocity": -2.9814257255193557e-7, - "velocityX": 3.600380691305006, - "velocityY": -0.39535519537448677, - "timestamp": 6.000553946083147 - }, - { - "x": 7.597310883497341, - "y": 5.855381388128685, - "heading": 0.10614465281325862, - "angularVelocity": -2.981443068468836e-7, - "velocityX": 3.600380688256082, - "velocityY": -0.39535522313506416, - "timestamp": 6.086546125660022 - }, - { - "x": 7.868541995298217, - "y": 5.825436456932061, - "heading": 0.06366050775847867, - "angularVelocity": -0.4940466128876299, - "velocityX": 3.154136959145237, - "velocityY": -0.34822854059484143, - "timestamp": 6.172538305236897 - }, - { - "x": 8.071966668862895, - "y": 5.802984598737895, - "heading": 0.03182759417957115, - "angularVelocity": -0.3701838206164972, - "velocityX": 2.365618298845671, - "velocityY": -0.2610918609650363, - "timestamp": 6.258530484813773 - }, - { - "x": 8.207583377724607, - "y": 5.788018984348283, - "heading": 0.01061131016646268, - "angularVelocity": -0.24672341272780052, - "velocityX": 1.577081887318283, - "velocityY": -0.17403459783495048, - "timestamp": 6.344522664390648 - }, - { - "x": 8.275391578674316, - "y": 5.78053617477417, - "heading": 6.762629528326585e-25, - "angularVelocity": -0.12339854878287405, - "velocityX": 0.7885391588323608, - "velocityY": -0.08701732658634945, - "timestamp": 6.430514843967523 - }, - { - "x": 8.275391578674316, - "y": 5.78053617477417, - "heading": 6.890807484355009e-25, - "angularVelocity": 1.714109587590018e-25, - "velocityX": -1.5632941987910777e-25, - "velocityY": 4.01842027987716e-26, - "timestamp": 6.516507023544398 - }, - { - "x": 8.217699478068795, - "y": 5.791359796793642, - "heading": 0.01104341509549353, - "angularVelocity": 0.13833447495381113, - "velocityX": -0.7226755833441465, - "velocityY": 0.1355812541876774, - "timestamp": 6.596338281075688 - }, - { - "x": 8.10231615629365, - "y": 5.8130147597317485, - "heading": 0.03315104134696604, - "angularVelocity": 0.276929450131827, - "velocityX": -1.4453401505033163, - "velocityY": 0.2712591985616573, - "timestamp": 6.676169538606978 - }, - { - "x": 7.929243964127431, - "y": 5.845512314072997, - "heading": 0.06637865596420629, - "angularVelocity": 0.4162231141632274, - "velocityX": -2.167975270818996, - "velocityY": 0.40707807124934986, - "timestamp": 6.756000796138268 - }, - { - "x": 7.698485965371198, - "y": 5.888866692718938, - "heading": 0.11081243304487726, - "angularVelocity": 0.5565962312851562, - "velocityX": -2.8905720126704284, - "velocityY": 0.5430752312645023, - "timestamp": 6.8358320536695585 - }, - { - "x": 7.41422585651022, - "y": 5.941821981236076, - "heading": 0.11081246572396981, - "angularVelocity": 4.0935209556931575e-7, - "velocityX": -3.5607620079085063, - "velocityY": 0.6633402774142934, - "timestamp": 6.9156633112008485 - }, - { - "x": 7.129965619426801, - "y": 5.994776581882574, - "heading": 0.11081249814230922, - "angularVelocity": 4.060857915201533e-7, - "velocityX": -3.5607636140768695, - "velocityY": 0.6633316608565432, - "timestamp": 6.995494568732139 - }, - { - "x": 6.8457053823426035, - "y": 6.047731182524891, - "heading": 0.11081253056064869, - "angularVelocity": 4.060857924168534e-7, - "velocityX": -3.560763614086623, - "velocityY": 0.6633316608041847, - "timestamp": 7.075325826263429 - }, - { - "x": 6.561445145258405, - "y": 6.100685783167208, - "heading": 0.11081256297898824, - "angularVelocity": 4.060857934452354e-7, - "velocityX": -3.5607636140866235, - "velocityY": 0.6633316608041844, - "timestamp": 7.155157083794719 - }, - { - "x": 6.277184908174208, - "y": 6.153640383809526, - "heading": 0.11081259539732799, - "angularVelocity": 4.060857956958186e-7, - "velocityX": -3.560763614086623, - "velocityY": 0.6633316608041847, - "timestamp": 7.234988341326009 - }, - { - "x": 5.99292467109001, - "y": 6.206594984451843, - "heading": 0.11081262781566778, - "angularVelocity": 4.060857964387038e-7, - "velocityX": -3.560763614086623, - "velocityY": 0.6633316608041848, - "timestamp": 7.314819598857299 - }, - { - "x": 5.708664434005791, - "y": 6.259549585094043, - "heading": 0.11081266023400761, - "angularVelocity": 4.060857968841719e-7, - "velocityX": -3.5607636140868966, - "velocityY": 0.6633316608027189, - "timestamp": 7.394650856388589 - }, - { - "x": 5.424404193332451, - "y": 6.312504166469845, - "heading": 0.11081269265234713, - "angularVelocity": 4.060857929641026e-7, - "velocityX": -3.5607636590457346, - "velocityY": 0.6633314194636794, - "timestamp": 7.474482113919879 - }, - { - "x": 5.139572620391846, - "y": 6.3622941970825195, - "heading": 0.11081272507571231, - "angularVelocity": 4.061487466795785e-7, - "velocityX": -3.567920407980088, - "velocityY": 0.6236909219820286, - "timestamp": 7.554313371451169 - }, - { - "x": 4.8822045552292, - "y": 6.360012293211402, - "heading": 0.1108127534765096, - "angularVelocity": 3.996777164840354e-7, - "velocityX": -3.6218800325558864, - "velocityY": -0.03211269456367951, - "timestamp": 7.625372617712571 - }, - { - "x": 4.624881625896748, - "y": 6.3546795533312395, - "heading": 0.11081278178743512, - "angularVelocity": 3.98412972255652e-7, - "velocityX": -3.6212448466713205, - "velocityY": -0.07504638960770253, - "timestamp": 7.696431863973974 - }, - { - "x": 4.367558697083316, - "y": 6.349346788406661, - "heading": 0.11081281009836048, - "angularVelocity": 3.9841297029584103e-7, - "velocityX": -3.62124483936728, - "velocityY": -0.07504674205185301, - "timestamp": 7.767491110235376 - }, - { - "x": 4.110235768269888, - "y": 6.344014023481877, - "heading": 0.11081283840928584, - "angularVelocity": 3.9841296996573985e-7, - "velocityX": -3.6212448393672196, - "velocityY": -0.075046742054734, - "timestamp": 7.8385503564967784 - }, - { - "x": 3.852912839456459, - "y": 6.338681258557136, - "heading": 0.11081286672021123, - "angularVelocity": 3.984129707844367e-7, - "velocityX": -3.621244839367232, - "velocityY": -0.07504674205415067, - "timestamp": 7.909609602758181 - }, - { - "x": 3.5955899105380604, - "y": 6.333348498701343, - "heading": 0.110812895031426, - "angularVelocity": 3.98417042898143e-7, - "velocityX": -3.6212448408444415, - "velocityY": -0.07504667072002474, - "timestamp": 7.980668849019583 - }, - { - "x": 3.3629508998707167, - "y": 6.328797181710379, - "heading": 0.15195588309543634, - "angularVelocity": 0.5789955597426332, - "velocityX": -3.2738738856241922, - "velocityY": -0.06404960973301323, - "timestamp": 8.051728095280986 - }, - { - "x": 3.176840519639808, - "y": 6.325164863882251, - "heading": 0.18492044940341654, - "angularVelocity": 0.4639025607830811, - "velocityX": -2.6190874519872214, - "velocityY": -0.05111675143253586, - "timestamp": 8.122787341542388 - }, - { - "x": 3.0372579905429093, - "y": 6.322444909479725, - "heading": 0.2096671002374919, - "angularVelocity": 0.3482537760538718, - "velocityX": -1.9643119852893276, - "velocityY": -0.03827727629589979, - "timestamp": 8.19384658780379 - }, - { - "x": 2.944202841875484, - "y": 6.320632970128358, - "heading": 0.22616886731909797, - "angularVelocity": 0.2322254731059452, - "velocityX": -1.3095431427052717, - "velocityY": -0.025498994806420823, - "timestamp": 8.264905834065193 - }, - { - "x": 2.897675037384033, - "y": 6.319726943969727, - "heading": 0.23441548450132177, - "angularVelocity": 0.11605269709570704, - "velocityX": -0.6547748103081646, - "velocityY": -0.012750292274397987, - "timestamp": 8.335965080326595 - }, - { - "x": 2.897675037384033, - "y": 6.319726943969727, - "heading": 0.23441548450132177, - "angularVelocity": 6.477597733926451e-26, - "velocityX": -1.5355730051117952e-25, - "velocityY": -1.2183012914286086e-25, - "timestamp": 8.407024326587997 + "x": 0.6757749919218088, + "y": 6.703069104575789, + "heading": 1.049862128706329, + "angularVelocity": -6.805633230382436e-7, + "velocityX": 0.2224099351379215, + "velocityY": 0.12883053814923912, + "timestamp": 0.06740678858637275 + }, + { + "x": 0.705758870441161, + "y": 6.720437210067111, + "heading": 1.0498620338020643, + "angularVelocity": -0.0000014079333376652512, + "velocityX": 0.4448198638173059, + "velocityY": 0.25766107324734056, + "timestamp": 0.1348135771727455 + }, + { + "x": 0.750734687393026, + "y": 6.746489367913325, + "heading": 1.049861885451002, + "angularVelocity": -0.000002200832664302106, + "velocityX": 0.6672297834547413, + "velocityY": 0.3864916040738485, + "timestamp": 0.20222036575911825 + }, + { + "x": 0.8107024418631711, + "y": 6.781225577682529, + "heading": 1.0498616770274682, + "angularVelocity": -0.000003092025867532283, + "velocityX": 0.8896396895292586, + "velocityY": 0.5153221284929648, + "timestamp": 0.269627154345491 + }, + { + "x": 0.8856621323278759, + "y": 6.82464583865489, + "heading": 1.0498613974886852, + "angularVelocity": -0.0000041470419969153815, + "velocityX": 1.1120495729989293, + "velocityY": 0.6441526422330909, + "timestamp": 0.33703394293186373 + }, + { + "x": 0.9756137557397047, + "y": 6.8767501493907295, + "heading": 1.0498610247491444, + "angularVelocity": -0.000005529703292486692, + "velocityX": 1.334459411258968, + "velocityY": 0.7729831346152221, + "timestamp": 0.40444073151823645 + }, + { + "x": 1.0805573029563829, + "y": 6.937538505571021, + "heading": 1.049860492552681, + "angularVelocity": -0.000007895294742633066, + "velocityX": 1.556869113890614, + "velocityY": 0.9018135629232547, + "timestamp": 0.4718475201046092 + }, + { + "x": 1.1972133032133725, + "y": 7.005111257740716, + "heading": 1.0334106168225663, + "angularVelocity": -0.2440388583271016, + "velocityX": 1.7306268805182825, + "velocityY": 1.0024621197182535, + "timestamp": 0.5392543086909819 + }, + { + "x": 1.3138692969010544, + "y": 7.072684021251592, + "heading": 0.9724418882550508, + "angularVelocity": -0.9044894415848371, + "velocityX": 1.7306267830606272, + "velocityY": 1.0024622879680827, + "timestamp": 0.6066610972773546 + }, + { + "x": 1.4305252906191748, + "y": 7.140256784709921, + "heading": 0.8706420088958982, + "angularVelocity": -1.5102318548926246, + "velocityX": 1.7306267835121878, + "velocityY": 1.0024622871885176, + "timestamp": 0.6740678858637273 + }, + { + "x": 1.5471812843420916, + "y": 7.207829548159968, + "heading": 0.7338773790646791, + "angularVelocity": -2.028944453509658, + "velocityX": 1.7306267835833422, + "velocityY": 1.0024622870656754, + "timestamp": 0.7414746744501001 + }, + { + "x": 1.6638372780224426, + "y": 7.2754023116835, + "heading": 0.5723600050127189, + "angularVelocity": -2.396158865289915, + "velocityX": 1.7306267829518702, + "velocityY": 1.002462288155831, + "timestamp": 0.8088814630364728 + }, + { + "x": 1.780493271638965, + "y": 7.3429750753172245, + "heading": 0.40418447555722015, + "angularVelocity": -2.4949346049916663, + "velocityX": 1.7306267820049508, + "velocityY": 1.0024622897905686, + "timestamp": 0.8762882516228455 + }, + { + "x": 1.8971492652070823, + "y": 7.410547839034513, + "heading": 0.25129496131963275, + "angularVelocity": -2.26816196771754, + "velocityX": 1.7306267812868497, + "velocityY": 1.0024622910302834, + "timestamp": 0.9436950402092182 + }, + { + "x": 2.013805258779043, + "y": 7.478120602745167, + "heading": 0.12816045902008172, + "angularVelocity": -1.8267374085292716, + "velocityX": 1.7306267813438676, + "velocityY": 1.0024622909318521, + "timestamp": 1.011101828795591 + }, + { + "x": 2.1304612523991975, + "y": 7.545693366372621, + "heading": 0.04280584220849794, + "angularVelocity": -1.266261434517286, + "velocityX": 1.7306267820588324, + "velocityY": 1.0024622896975566, + "timestamp": 1.0785086173819638 + }, + { + "x": 2.247117388567763, + "y": 7.613265883907332, + "heading": 0.000013466426965438275, + "angularVelocity": -0.6348377764162405, + "velocityX": 1.7306288968074217, + "velocityY": 1.002458638837633, + "timestamp": 1.1459154059683365 + }, + { + "x": 2.365737851453118, + "y": 7.677327455956403, + "heading": 0.000004459976492377945, + "angularVelocity": -0.00013361340396034107, + "velocityX": 1.759770274968055, + "velocityY": 0.9503727056657639, + "timestamp": 1.2133221945547092 + }, + { + "x": 2.491594313865089, + "y": 7.725647742212638, + "heading": 0.000001984433687850704, + "angularVelocity": -0.000036725422712508624, + "velocityX": 1.8671185061828453, + "velocityY": 0.7168459923634953, + "timestamp": 1.280728983141082 + }, + { + "x": 2.622608441209736, + "y": 7.7574282007435205, + "heading": 7.92136305089401e-7, + "angularVelocity": -0.00001768809058799076, + "velocityX": 1.9436340180599136, + "velocityY": 0.4714726691089775, + "timestamp": 1.3481357717274547 + }, + { + "x": 2.7566164536261604, + "y": 7.772143891629413, + "heading": 2.0780523669770732e-7, + "angularVelocity": -0.000008668727299520462, + "velocityX": 1.9880492043425424, + "velocityY": 0.2183117041251145, + "timestamp": 1.4155425603138274 + }, + { + "x": 2.8914051055908203, + "y": 7.769551753997803, + "heading": -8.09802518102299e-24, + "angularVelocity": -0.000003082853241575703, + "velocityX": 1.9996302270348674, + "velocityY": -0.03845514207057226, + "timestamp": 1.4829493489002001 + }, + { + "x": 2.9864482996127166, + "y": 7.767723966300088, + "heading": -1.0517201217515276e-7, + "angularVelocity": -0.0000023977731499342787, + "velocityX": 2.1668504195789944, + "velocityY": -0.04167097476524414, + "timestamp": 1.526811718523463 + }, + { + "x": 3.0888261669051587, + "y": 7.76575512457206, + "heading": -1.802948176200958e-7, + "angularVelocity": -0.0000017126937301878197, + "velocityX": 2.3340705979128114, + "velocityY": -0.04488680718663573, + "timestamp": 1.570674088146726 + }, + { + "x": 3.1985387066370827, + "y": 7.7636452288297, + "heading": -2.2536845598951497e-7, + "angularVelocity": -0.0000010276152145121185, + "velocityX": 2.5012907572995546, + "velocityY": -0.04810263924365375, + "timestamp": 1.614536457769989 + }, + { + "x": 3.315585917645001, + "y": 7.761394279095385, + "heading": -2.4039298339678027e-7, + "angularVelocity": -3.4253797814190234e-7, + "velocityX": 2.668510890160408, + "velocityY": -0.05131847079054902, + "timestamp": 1.658398827393252 + }, + { + "x": 3.4399677981836816, + "y": 7.759002275402676, + "heading": -2.2536848500409826e-7, + "angularVelocity": 3.4253731665043406e-7, + "velocityX": 2.835730983232458, + "velocityY": -0.05453430157226078, + "timestamp": 1.702261197016515 + }, + { + "x": 3.5716843453444107, + "y": 7.756469217807512, + "heading": -1.8029510454250157e-7, + "angularVelocity": 0.0000010276093345784838, + "velocityX": 3.0029510099899284, + "velocityY": -0.057750131078668464, + "timestamp": 1.7461235666397779 + }, + { + "x": 3.7107355533097754, + "y": 7.753795106421767, + "heading": -1.0517313285100412e-7, + "angularVelocity": 0.0000017126747217883014, + "velocityX": 3.1701709041186272, + "velocityY": -0.06096595803447545, + "timestamp": 1.7899859362630408 + }, + { + "x": 3.8571214046276947, + "y": 7.750979941581066, + "heading": -3.445312029122533e-12, + "angularVelocity": 0.000002397720151516781, + "velocityX": 3.337390400364549, + "velocityY": -0.06418177733854838, + "timestamp": 1.8338483058863038 + }, + { + "x": 4.008740030563312, + "y": 7.748064144589167, + "heading": -2.953693080850903e-12, + "angularVelocity": 1.1208216804044482e-11, + "velocityX": 3.456690261786607, + "velocityY": -0.06647604807818788, + "timestamp": 1.8777106755095667 + }, + { + "x": 4.160358656499413, + "y": 7.745148347597258, + "heading": -2.46204265095573e-12, + "angularVelocity": 1.1208934540541933e-11, + "velocityX": 3.4566902617976116, + "velocityY": -0.06647604807839952, + "timestamp": 1.9215730451328297 + }, + { + "x": 4.311977282435513, + "y": 7.742232550605349, + "heading": -1.970393895641841e-12, + "angularVelocity": 1.1208896362456744e-11, + "velocityX": 3.4566902617976116, + "velocityY": -0.06647604807839952, + "timestamp": 1.9654354147560926 + }, + { + "x": 4.463595908371613, + "y": 7.739316753613441, + "heading": -1.478772239262654e-12, + "angularVelocity": 1.1208278545067146e-11, + "velocityX": 3.456690261797612, + "velocityY": -0.06647604807839953, + "timestamp": 2.0092977843793554 + }, + { + "x": 4.615214534307714, + "y": 7.736400956621532, + "heading": -9.871132742331443e-13, + "angularVelocity": 1.1209129129534961e-11, + "velocityX": 3.4566902617976116, + "velocityY": -0.06647604807839952, + "timestamp": 2.0531601540026183 + }, + { + "x": 4.766833160243814, + "y": 7.733485159629623, + "heading": -4.955367585470695e-13, + "angularVelocity": 1.120724940109933e-11, + "velocityX": 3.456690261797612, + "velocityY": -0.06647604807839952, + "timestamp": 2.0970225236258813 + }, + { + "x": 4.91845178604126, + "y": 7.730569362640381, + "heading": 5.035858981265476e-24, + "angularVelocity": 1.1297537338117902e-11, + "velocityX": 3.4566902586364923, + "velocityY": -0.06647604801760763, + "timestamp": 2.140884893249144 + }, + { + "x": 5.07168754119456, + "y": 7.72009798657163, + "heading": -1.2237883220746723e-17, + "angularVelocity": -2.754707452373014e-16, + "velocityX": 3.4492852675356476, + "velocityY": -0.23570715051872318, + "timestamp": 2.1853102577139336 + }, + { + "x": 5.224226523742201, + "y": 7.702133987621307, + "heading": -3.220999671007562e-17, + "angularVelocity": -4.4956555179430785e-16, + "velocityX": 3.433601150724158, + "velocityY": -0.4043635694775183, + "timestamp": 2.229735622178723 + }, + { + "x": 5.375704105653677, + "y": 7.6767294867936675, + "heading": -8.086584933587513e-18, + "angularVelocity": 5.430098788639561e-16, + "velocityX": 3.4097093796840374, + "velocityY": -0.5718467621751168, + "timestamp": 2.2741609866435124 + }, + { + "x": 5.527091031475673, + "y": 7.65079022745198, + "heading": 1.237593714278334e-17, + "angularVelocity": 4.606044840123949e-16, + "velocityX": 3.4076687416257836, + "velocityY": -0.5838839963203132, + "timestamp": 2.318586351108302 + }, + { + "x": 5.678477940102667, + "y": 7.624850867756873, + "heading": -5.299489627912929e-18, + "angularVelocity": -3.9786790685233107e-16, + "velocityX": 3.4076683545720687, + "velocityY": -0.5838862552420002, + "timestamp": 2.363011715573091 + }, + { + "x": 5.82986484872645, + "y": 7.598911508043026, + "heading": -6.861733443062272e-18, + "angularVelocity": -3.51655824113109e-17, + "velocityX": 3.4076683544997888, + "velocityY": -0.5838862556638376, + "timestamp": 2.4074370800378806 + }, + { + "x": 5.981251757350232, + "y": 7.572972148329175, + "heading": 1.300275398298883e-18, + "angularVelocity": 1.8372407158846258e-16, + "velocityX": 3.4076683544997755, + "velocityY": -0.5838862556639163, + "timestamp": 2.45186244450267 + }, + { + "x": 6.13263866597402, + "y": 7.547032788615358, + "heading": 2.03304825021732e-17, + "angularVelocity": 4.2836355611375324e-16, + "velocityX": 3.407668354499907, + "velocityY": -0.5838862556631488, + "timestamp": 2.4962878089674594 + }, + { + "x": 6.284025574629089, + "y": 7.521093429084101, + "heading": 9.540525391081043e-18, + "angularVelocity": -2.428783025456181e-16, + "velocityX": 3.407668355204027, + "velocityY": -0.5838862515537734, + "timestamp": 2.540713173432249 + }, + { + "x": 6.435412650788291, + "y": 7.495155047156986, + "heading": 3.1121659748942844e-18, + "angularVelocity": -1.4470020659857382e-16, + "velocityX": 3.407672125665691, + "velocityY": -0.5838642460136346, + "timestamp": 2.585138537897038 + }, + { + "x": 6.587408261478287, + "y": 7.473060233149462, + "heading": 4.012412418719246e-18, + "angularVelocity": 2.0264244415020296e-17, + "velocityX": 3.4213700331139205, + "velocityY": -0.49734682593400187, + "timestamp": 2.6295639023618276 + }, + { + "x": 6.740303543651052, + "y": 7.458435646449454, + "heading": -2.1084051336842118e-18, + "angularVelocity": -1.377775427669848e-16, + "velocityX": 3.4416213353510248, + "velocityY": -0.3291945238085493, + "timestamp": 2.673989266826617 + }, + { + "x": 6.890285316295548, + "y": 7.450749433084636, + "heading": -1.4822341121773416e-18, + "angularVelocity": 1.40948989157144e-17, + "velocityX": 3.3760392165914315, + "velocityY": -0.17301407557185516, + "timestamp": 2.7184146312914064 + }, + { + "x": 7.032993253879116, + "y": 7.444993241304835, + "heading": -9.121457386741735e-19, + "angularVelocity": 1.2832497387232803e-17, + "velocityX": 3.212307637828718, + "velocityY": -0.1295699393611814, + "timestamp": 2.7628399957561958 + }, + { + "x": 7.168350422821424, + "y": 7.440849530732545, + "heading": -4.3340581409719474e-19, + "angularVelocity": 1.0776274550871163e-17, + "velocityX": 3.046844310069511, + "velocityY": -0.09327353016031785, + "timestamp": 2.807265360220985 + }, + { + "x": 7.296333312988281, + "y": 7.438207149505615, + "heading": -1.2424191874750213e-28, + "angularVelocity": 9.755818981171634e-18, + "velocityX": 2.880851777103443, + "velocityY": -0.05947911196145459, + "timestamp": 2.8516907246857746 + }, + { + "x": 7.476987619873736, + "y": 7.437582893575528, + "heading": 6.306246828045357e-19, + "angularVelocity": 9.156773444569897e-18, + "velocityX": 2.623130055966497, + "velocityY": -0.009064298112001053, + "timestamp": 2.9205604730516797 + }, + { + "x": 7.639661681076791, + "y": 7.438908313736393, + "heading": 7.717197233455447e-19, + "angularVelocity": 2.04872304504038e-18, + "velocityX": 2.362053950578807, + "velocityY": 0.019245317317299903, + "timestamp": 2.9894302214175847 + }, + { + "x": 7.78428303377631, + "y": 7.4413247257119215, + "heading": 8.357244621441239e-19, + "angularVelocity": 9.29359266725832e-19, + "velocityX": 2.0999256731873923, + "velocityY": 0.0350866967407844, + "timestamp": 3.05829996978349 + }, + { + "x": 7.91082689918953, + "y": 7.4442843222063875, + "heading": 7.220816869988264e-19, + "angularVelocity": -1.6501116644010886e-18, + "velocityX": 1.8374376038212434, + "velocityY": 0.042973824715339615, + "timestamp": 3.127169718149395 + }, + { + "x": 8.019285863587411, + "y": 7.447408189663725, + "heading": 5.712912024628398e-19, + "angularVelocity": -2.1895024749108468e-18, + "velocityX": 1.5748418858979918, + "velocityY": 0.04535906593908385, + "timestamp": 3.1960394665153 + }, + { + "x": 8.109659533807191, + "y": 7.450418969222921, + "heading": 4.341547519848611e-19, + "angularVelocity": -1.9912436692380873e-18, + "velocityX": 1.312240459187152, + "velocityY": 0.04371701117883307, + "timestamp": 3.264909214881205 + }, + { + "x": 8.181950471768632, + "y": 7.4531049914429115, + "heading": 2.769074151229353e-19, + "angularVelocity": -2.283257025771601e-18, + "velocityX": 1.0496762319699213, + "velocityY": 0.03900148154629361, + "timestamp": 3.33377896324711 + }, + { + "x": 8.236162441930201, + "y": 7.455299473800812, + "heading": 8.002361823653196e-20, + "angularVelocity": -2.858784902160653e-18, + "velocityX": 0.7871666653047966, + "velocityY": 0.03186424242820522, + "timestamp": 3.4026487116130153 + }, + { + "x": 8.27229961106123, + "y": 7.456867643223532, + "heading": 9.595707199020635e-21, + "angularVelocity": -1.0226247759898425e-18, + "velocityX": 0.5247176008111963, + "velocityY": 0.02277007626611394, + "timestamp": 3.4715184599789204 + }, + { + "x": 8.290366172790527, + "y": 7.457698345184326, + "heading": -2.737879233354451e-29, + "angularVelocity": -1.3933123630808857e-19, + "velocityX": 0.26232942849316815, + "velocityY": 0.012061928212380262, + "timestamp": 3.5403882083448255 + }, + { + "x": 8.290366172790527, + "y": 7.457698345184326, + "heading": -7.734690041039287e-29, + "angularVelocity": -1.1060203570884526e-28, + "velocityX": -8.232857746308476e-27, + "velocityY": 9.915594313558478e-27, + "timestamp": 3.6092579567107306 + }, + { + "x": 8.258529155042611, + "y": 7.450262882094732, + "heading": 0.0018141437015229199, + "angularVelocity": 0.019588491140668917, + "velocityX": -0.34376501683789845, + "velocityY": -0.0802855378738891, + "timestamp": 3.7018706897944167 + }, + { + "x": 8.19485511722891, + "y": 7.435392003543495, + "heading": 0.005442056461770318, + "angularVelocity": 0.03917293701902922, + "velocityX": -0.6875300587033178, + "velocityY": -0.16057056147775478, + "timestamp": 3.794483422878103 + }, + { + "x": 8.099344042603773, + "y": 7.413085782435483, + "heading": 0.010882224859340402, + "angularVelocity": 0.05874104150078671, + "velocityX": -1.0312952813824383, + "velocityY": -0.24085479788027828, + "timestamp": 3.887096155961789 + }, + { + "x": 7.9719959003584435, + "y": 7.383344317488874, + "heading": 0.018132064338159094, + "angularVelocity": 0.07828123884722897, + "velocityX": -1.3750608367237869, + "velocityY": -0.3211379683583422, + "timestamp": 3.979708889045475 + }, + { + "x": 7.8128106462112585, + "y": 7.346167733877292, + "heading": 0.027188013699287764, + "angularVelocity": 0.09778298360923647, + "velocityX": -1.7188268702030736, + "velocityY": -0.4014197872552755, + "timestamp": 4.072321622129161 + }, + { + "x": 7.621788223292644, + "y": 7.301556183975714, + "heading": 0.038045656619278445, + "angularVelocity": 0.11723704245051889, + "velocityX": -2.0625935177402037, + "velocityY": -0.481699960860308, + "timestamp": 4.164934355212847 + }, + { + "x": 7.3989285635202835, + "y": 7.24950984823631, + "heading": 0.050699870016136385, + "angularVelocity": 0.1366357840387178, + "velocityX": -2.406360900406452, + "velocityY": -0.5619781860057411, + "timestamp": 4.257547088296533 + }, + { + "x": 7.144231590225439, + "y": 7.190028936362276, + "heading": 0.06514499751032955, + "angularVelocity": 0.15597345001297352, + "velocityX": -2.750129110914985, + "velocityY": -0.6422541468491951, + "timestamp": 4.3501598213802195 + }, + { + "x": 6.857697227315765, + "y": 7.123113690113179, + "heading": 0.08137503414778206, + "angularVelocity": 0.17524627658690128, + "velocityX": -3.093898143042135, + "velocityY": -0.7225274972571053, + "timestamp": 4.442772554463906 + }, + { + "x": 6.545887160381392, + "y": 7.050327089705259, + "heading": 0.08137504044908753, + "angularVelocity": 6.803929929105723e-8, + "velocityX": -3.366816382069384, + "velocityY": -0.7859243322637903, + "timestamp": 4.535385287547592 + }, + { + "x": 6.234077092172867, + "y": 6.977540494754869, + "heading": 0.0813750467500712, + "angularVelocity": 6.80358247192299e-8, + "velocityX": -3.3668163958272355, + "velocityY": -0.7859242733352659, + "timestamp": 4.627998020631278 + }, + { + "x": 5.922267023964316, + "y": 6.904753899804592, + "heading": 0.08137505305105486, + "angularVelocity": 6.803582445105601e-8, + "velocityX": -3.3668163958275135, + "velocityY": -0.7859242733340743, + "timestamp": 4.720610753714964 + }, + { + "x": 5.610456955755783, + "y": 6.831967304854232, + "heading": 0.08137505935203854, + "angularVelocity": 6.803582480789322e-8, + "velocityX": -3.366816395827309, + "velocityY": -0.7859242733349515, + "timestamp": 4.81322348679865 + }, + { + "x": 5.298646888485436, + "y": 6.759180705885576, + "heading": 0.08137506565333762, + "angularVelocity": 6.803923039402274e-8, + "velocityX": -3.366816385697099, + "velocityY": -0.7859243167231056, + "timestamp": 4.905836219882336 + }, + { + "x": 5.012107236392116, + "y": 6.692280236782266, + "heading": 0.09728577795318676, + "angularVelocity": 0.17179832373019394, + "velocityX": -3.0939552538029402, + "velocityY": -0.7223679387893466, + "timestamp": 4.998448952966022 + }, + { + "x": 4.757405139637452, + "y": 6.632813653440834, + "heading": 0.1114212737800766, + "angularVelocity": 0.15263015523056464, + "velocityX": -2.75018443224769, + "velocityY": -0.642099432350162, + "timestamp": 5.091061686049708 + }, + { + "x": 4.534540680564304, + "y": 6.580780772308004, + "heading": 0.12378499174610678, + "angularVelocity": 0.1334991156654254, + "velocityX": -2.4064127215829467, + "velocityY": -0.561832907855257, + "timestamp": 5.1836744191333946 + }, + { + "x": 4.343513918608397, + "y": 6.536181432644826, + "heading": 0.13437950942576468, + "angularVelocity": 0.11439590785086255, + "velocityX": -2.0626403691519735, + "velocityY": -0.4815681189634867, + "timestamp": 5.276287152217081 + }, + { + "x": 4.184324899185163, + "y": 6.499015499114029, + "heading": 0.14320667584584268, + "angularVelocity": 0.0953126651828929, + "velocityX": -1.7188675263409952, + "velocityY": -0.4013047913963752, + "timestamp": 5.368899885300767 + }, + { + "x": 4.056973656080411, + "y": 6.469282862636605, + "heading": 0.15026773537457858, + "angularVelocity": 0.07624285876927316, + "velocityX": -1.3750943187226343, + "velocityY": -0.32104264162636753, + "timestamp": 5.461512618384453 + }, + { + "x": 3.9614602126260823, + "y": 6.446983440876931, + "heading": 0.1555634289688374, + "angularVelocity": 0.05718105294952869, + "velocityX": -1.0313208591740977, + "velocityY": -0.24078138088770026, + "timestamp": 5.554125351468139 + }, + { + "x": 3.8977845824113757, + "y": 6.432117178562742, + "heading": 0.15909407056957425, + "angularVelocity": 0.03812263695475346, + "velocityX": -0.6875472528941416, + "velocityY": -0.1605207169596795, + "timestamp": 5.646738084551825 + }, + { + "x": 3.865946769714355, + "y": 6.424684047698975, + "heading": 0.1608595979864649, + "angularVelocity": 0.019063549450541617, + "velocityX": -0.3437736004211391, + "velocityY": -0.08026035531260044, + "timestamp": 5.739350817635511 + }, + { + "x": 3.865946769714355, + "y": 6.424684047698975, + "heading": 0.1608595979864649, + "angularVelocity": -9.999505736171943e-25, + "velocityX": -5.156111906167998e-25, + "velocityY": -7.69774068808886e-24, + "timestamp": 5.831963550719197 + }, + { + "x": 3.8805908200661583, + "y": 6.424671235221358, + "heading": 0.15744018373663873, + "angularVelocity": -0.0550052976874839, + "velocityX": 0.23556676380826952, + "velocityY": -0.00020610376336075096, + "timestamp": 5.894128731048805 + }, + { + "x": 3.9098813602559557, + "y": 6.424646814464556, + "heading": 0.1506499345999589, + "angularVelocity": -0.10922913921711586, + "velocityX": 0.47117276961950644, + "velocityY": -0.0003928365794473002, + "timestamp": 5.956293911378412 + }, + { + "x": 3.9538212666067056, + "y": 6.424612381558881, + "heading": 0.14054735001329138, + "angularVelocity": -0.1625119485393969, + "velocityX": 0.7068250444666143, + "velocityY": -0.0005538937632522526, + "timestamp": 6.01845909170802 + }, + { + "x": 4.012413923406757, + "y": 6.424569899192806, + "heading": 0.1272024405085879, + "angularVelocity": -0.21466855615872318, + "velocityX": 0.9425317595699895, + "velocityY": -0.0006833787957962723, + "timestamp": 6.080624272037627 + }, + { + "x": 4.085663316044443, + "y": 6.4245216623134365, + "heading": 0.11069894640828985, + "angularVelocity": -0.2654781022558683, + "velocityX": 1.1783025843295045, + "velocityY": -0.0007759469065193845, + "timestamp": 6.142789452367235 + }, + { + "x": 4.173574156900502, + "y": 6.4244702509624405, + "heading": 0.09113759111571623, + "angularVelocity": -0.3146673940115171, + "velocityX": 1.414149213272517, + "velocityY": -0.0008270120141812696, + "timestamp": 6.204954632696842 + }, + { + "x": 4.276152059818016, + "y": 6.424418464281491, + "heading": 0.0686410579111913, + "angularVelocity": -0.36188318099047606, + "velocityX": 1.650086147480532, + "velocityY": -0.0008330496376791236, + "timestamp": 6.26711981302645 + }, + { + "x": 4.39340378630487, + "y": 6.424369226491044, + "heading": 0.04336192483995491, + "angularVelocity": -0.40664457075815247, + "velocityX": 1.886131848490915, + "velocityY": -0.0007920477377600693, + "timestamp": 6.329284993356057 + }, + { + "x": 4.525337595489836, + "y": 6.424325450771935, + "heading": 0.015495928724554285, + "angularVelocity": -0.4482573036489503, + "velocityX": 2.122310407296116, + "velocityY": -0.000704183899678652, + "timestamp": 6.391450173685665 + }, + { + "x": 4.671963731284715, + "y": 6.424289841531667, + "heading": -0.014694474575960468, + "angularVelocity": -0.4856481255333208, + "velocityX": 2.3586537514642383, + "velocityY": -0.0005728164879388929, + "timestamp": 6.453615354015272 + }, + { + "x": 4.833295019214238, + "y": 6.424264622086766, + "heading": -0.04683458533303072, + "angularVelocity": -0.5170114618289433, + "velocityX": 2.595203409273861, + "velocityY": -0.00040568441636908034, + "timestamp": 6.51578053434488 + }, + { + "x": 5.009347081398109, + "y": 6.424251287994793, + "heading": -0.08033775965322806, + "angularVelocity": -0.5389379415061509, + "velocityX": 2.832004367242584, + "velocityY": -0.00021449454344913397, + "timestamp": 6.577945714674487 + }, + { + "x": 5.200134146051008, + "y": 6.424251504702917, + "heading": -0.1141370339338056, + "angularVelocity": -0.5437010574306965, + "velocityX": 3.069034202769489, + "velocityY": 0.0000034860049153566075, + "timestamp": 6.640110895004095 + }, + { + "x": 5.405613054428805, + "y": 6.424281444650923, + "heading": -0.1455833954142087, + "angularVelocity": -0.5058516892200839, + "velocityX": 3.3053697791645313, + "velocityY": 0.0004816192577199728, + "timestamp": 6.702276075333702 + }, + { + "x": 5.620478420069855, + "y": 6.42365674720977, + "heading": -0.14578823417625966, + "angularVelocity": -0.003295072273013325, + "velocityX": 3.456361977907994, + "velocityY": -0.0100489926650518, + "timestamp": 6.76444125566331 + }, + { + "x": 5.8354039344878705, + "y": 6.423689815718493, + "heading": -0.1457882693852108, + "angularVelocity": -5.663773666898463e-7, + "velocityX": 3.4573295416896364, + "velocityY": 0.0005319458344337396, + "timestamp": 6.826606435992917 + }, + { + "x": 6.050013195110809, + "y": 6.4120346435837465, + "heading": -0.14578830470722368, + "angularVelocity": -5.68196097923049e-7, + "velocityX": 3.4522422276434255, + "velocityY": -0.1874871442976483, + "timestamp": 6.888771616322525 + }, + { + "x": 6.263319492340088, + "y": 6.385702133178711, + "heading": -0.14578834092456064, + "angularVelocity": -5.825984377491133e-7, + "velocityX": 3.4312825298390877, + "velocityY": -0.42358938340430125, + "timestamp": 6.950936796652132 + }, + { + "x": 6.49569885410855, + "y": 6.338969209681391, + "heading": -0.14578837085914265, + "angularVelocity": -4.366235413031805e-7, + "velocityX": 3.389467737301255, + "velocityY": -0.6816428759355991, + "timestamp": 7.019496045412968 + }, + { + "x": 6.723883016534425, + "y": 6.274812198093867, + "heading": -0.14578840022622835, + "angularVelocity": -4.28346084046259e-7, + "velocityX": 3.328276878031172, + "velocityY": -0.9357893026414394, + "timestamp": 7.088055294173803 + }, + { + "x": 6.944379070760873, + "y": 6.204658250666653, + "heading": -0.16522808025100505, + "angularVelocity": -0.28354569771601223, + "velocityX": 3.2161387152247607, + "velocityY": -1.023260153738583, + "timestamp": 7.156614542934639 + }, + { + "x": 7.148638930902927, + "y": 6.139545447418234, + "heading": -0.19715028206546042, + "angularVelocity": -0.465614813339248, + "velocityX": 2.979318820347672, + "velocityY": -0.949730407279711, + "timestamp": 7.225173791695474 + }, + { + "x": 7.3358170328874195, + "y": 6.0798421831912135, + "heading": -0.22994658912328278, + "angularVelocity": -0.4783644460899831, + "velocityX": 2.7301655920625914, + "velocityY": -0.870827281601222, + "timestamp": 7.29373304045631 + }, + { + "x": 7.5059237430887356, + "y": 6.025569482278596, + "heading": -0.26160545834967963, + "angularVelocity": -0.4617738641920779, + "velocityX": 2.481163566927677, + "velocityY": -0.7916174971803536, + "timestamp": 7.362292289217145 + }, + { + "x": 7.658979831941821, + "y": 5.976731413281962, + "heading": -0.291240609478123, + "angularVelocity": -0.43225606557947815, + "velocityX": 2.232464497780769, + "velocityY": -0.7123483684455512, + "timestamp": 7.430851537977981 + }, + { + "x": 7.795001086268662, + "y": 5.933327606851567, + "heading": -0.3183467386103757, + "angularVelocity": -0.39536794265075353, + "velocityX": 1.9839956940214167, + "velocityY": -0.633084627018105, + "timestamp": 7.4994107867388164 + }, + { + "x": 7.91399908186447, + "y": 5.895356201186841, + "heading": -0.34259459104051004, + "angularVelocity": -0.3536773355659347, + "velocityX": 1.7356957339326504, + "velocityY": -0.5538480416724806, + "timestamp": 7.567970035499652 + }, + { + "x": 8.015982502141213, + "y": 5.862814866579216, + "heading": -0.36375111540905714, + "angularVelocity": -0.3085874590363489, + "velocityX": 1.4875224294318823, + "velocityY": -0.4746454372792861, + "timestamp": 7.6365292842604875 + }, + { + "x": 8.100958056756967, + "y": 5.835701248475415, + "heading": -0.3816420491863885, + "angularVelocity": -0.26095580247302225, + "velocityX": 1.2394469914947026, + "velocityY": -0.39547717622147305, + "timestamp": 7.705088533021323 + }, + { + "x": 8.168931078031942, + "y": 5.814013189562108, + "heading": -0.3961322011943633, + "angularVelocity": -0.211352257643937, + "velocityX": 0.9914493303754669, + "velocityY": -0.31634038157220884, + "timestamp": 7.7736477817821585 + }, + { + "x": 8.219905912133822, + "y": 5.797748853681938, + "heading": -0.4071141507461263, + "angularVelocity": -0.16018188282769613, + "velocityX": 0.7435150621282579, + "velocityY": -0.237230368974835, + "timestamp": 7.842207030542994 + }, + { + "x": 8.253886183057102, + "y": 5.786906800693844, + "heading": -0.4145013188137657, + "angularVelocity": -0.10774867288013994, + "velocityX": 0.49563365318977964, + "velocityY": -0.1581413621656758, + "timestamp": 7.91076627930383 + }, + { + "x": 8.270874977111816, + "y": 5.7814860343933105, + "heading": -0.4182234468320189, + "angularVelocity": -0.05429067683103692, + "velocityX": 0.2477972609352767, + "velocityY": -0.07906688591998655, + "timestamp": 7.979325528064665 + }, + { + "x": 8.270874977111816, + "y": 5.7814860343933105, + "heading": -0.4182234468320189, + "angularVelocity": 1.2149869731922202e-24, + "velocityX": 1.6200325032236621e-24, + "velocityY": -9.81678651755837e-25, + "timestamp": 8.0478847768255 + }, + { + "x": 8.255885275596246, + "y": 5.786297415399053, + "heading": -0.4161957197256105, + "angularVelocity": 0.03152694238731417, + "velocityX": -0.233058706268173, + "velocityY": 0.07480697540219873, + "timestamp": 8.112202054457356 + }, + { + "x": 8.225904877332317, + "y": 5.795920588198962, + "heading": -0.4121767719087599, + "angularVelocity": 0.06248628618665337, + "velocityX": -0.46613288633782896, + "velocityY": 0.14962033771067135, + "timestamp": 8.176519332089212 + }, + { + "x": 8.180932637466617, + "y": 5.810356062930585, + "heading": -0.4062099213750387, + "angularVelocity": 0.09277212521143607, + "velocityX": -0.6992248664987879, + "velocityY": 0.22444163159780386, + "timestamp": 8.240836609721068 + }, + { + "x": 8.120967221795235, + "y": 5.8296044401602, + "heading": -0.3983469021562322, + "angularVelocity": 0.12225360755804271, + "velocityX": -0.9323375907577511, + "velocityY": 0.29927226304244037, + "timestamp": 8.305153887352924 + }, + { + "x": 8.046007053762992, + "y": 5.853666398213901, + "heading": -0.38865015214844967, + "angularVelocity": 0.15076431038150498, + "velocityX": -1.16547482717328, + "velocityY": 0.3741134410481226, + "timestamp": 8.36947116498478 + }, + { + "x": 7.95605024235754, + "y": 5.882542674483832, + "heading": -0.37719620256520386, + "angularVelocity": 0.17808511188559348, + "velocityX": -1.398641464900824, + "velocityY": 0.44896608396916854, + "timestamp": 8.433788442616637 + }, + { + "x": 7.851094482525453, + "y": 5.916234036378714, + "heading": -0.3640809123745278, + "angularVelocity": 0.20391550565536098, + "velocityX": -1.6318439414186696, + "velocityY": 0.5238306585009168, + "timestamp": 8.498105720248493 + }, + { + "x": 7.731136916910522, + "y": 5.954741232958676, + "heading": -0.349427957292727, + "angularVelocity": 0.22782299906523523, + "velocityX": -1.8650908438872864, + "velocityY": 0.5987068793609814, + "timestamp": 8.562422997880349 + }, + { + "x": 7.596173948111591, + "y": 5.998064906716331, + "heading": -0.33340347371338613, + "angularVelocity": 0.24914741682729602, + "velocityX": -2.0983936784675863, + "velocityY": 0.6735930896459054, + "timestamp": 8.626740275512205 + }, + { + "x": 7.446201013798343, + "y": 6.046205411855825, + "heading": -0.31624349903105253, + "angularVelocity": 0.26680194364810067, + "velocityX": -2.331767447802665, + "velocityY": 0.7484848070691726, + "timestamp": 8.691057553144061 + }, + { + "x": 7.281212494788232, + "y": 6.099162371697227, + "heading": -0.2983118341454104, + "angularVelocity": 0.27880012254686504, + "velocityX": -2.56522858374831, + "velocityY": 0.82337066790236, + "timestamp": 8.755374830775917 + }, + { + "x": 7.101203008664871, + "y": 6.1569332933758485, + "heading": -0.28024668313688234, + "angularVelocity": 0.280875554340636, + "velocityX": -2.7987734050827355, + "velocityY": 0.8982177698703966, + "timestamp": 8.819692108407773 + }, + { + "x": 6.9061821193945345, + "y": 6.219506729950283, + "heading": -0.26348073873693967, + "angularVelocity": 0.26067559164908854, + "velocityX": -3.0321695266179045, + "velocityY": 0.9728868956891804, + "timestamp": 8.88400938603963 + }, + { + "x": 6.696607254404851, + "y": 6.286709539594328, + "heading": -0.25518312367209683, + "angularVelocity": 0.12901066976648753, + "velocityX": -3.258453602300528, + "velocityY": 1.0448640259419284, + "timestamp": 8.948326663671486 + }, + { + "x": 6.4817199239739445, + "y": 6.3438942094131185, + "heading": -0.2551831038227718, + "angularVelocity": 3.086157524298735e-7, + "velocityX": -3.3410513992973323, + "velocityY": 0.8891027718260752, + "timestamp": 9.012643941303342 + }, + { + "x": 6.263319492340088, + "y": 6.385702133178711, + "heading": -0.2551830834264048, + "angularVelocity": 3.1712111825091784e-7, + "velocityX": -3.395672821911929, + "velocityY": 0.6500263273718765, + "timestamp": 9.076961218935198 + }, + { + "x": 6.037162237236338, + "y": 6.412166410916887, + "heading": -0.25518303828533845, + "angularVelocity": 6.854074958240057e-7, + "velocityX": -3.4338993409830967, + "velocityY": 0.4018251187344374, + "timestamp": 9.142821406624625 + }, + { + "x": 5.8096806134973695, + "y": 6.422145044657871, + "heading": -0.2551829940595105, + "angularVelocity": 6.715108093540807e-7, + "velocityX": -3.4540081302483707, + "velocityY": 0.15151237934575548, + "timestamp": 9.208681594314053 + }, + { + "x": 5.5819802469231306, + "y": 6.422071707013465, + "heading": -0.25518294988305257, + "angularVelocity": 6.707611912611386e-7, + "velocityX": -3.457329451412868, + "velocityY": -0.0011135353083337968, + "timestamp": 9.27454178200348 + }, + { + "x": 5.357973256918571, + "y": 6.423240614393437, + "heading": -0.2430314751768064, + "angularVelocity": 0.18450410077098406, + "velocityX": -3.4012504042790286, + "velocityY": 0.017748315347736767, + "timestamp": 9.340401969692907 + }, + { + "x": 5.144457161392198, + "y": 6.423376601690841, + "heading": -0.1958139855508891, + "angularVelocity": 0.7169352424043682, + "velocityX": -3.241960021936682, + "velocityY": 0.0020647875776692963, + "timestamp": 9.406262157382335 + }, + { + "x": 4.947462840354243, + "y": 6.423510104020051, + "heading": -0.14701489396398695, + "angularVelocity": 0.7409497801163326, + "velocityX": -2.991098688739039, + "velocityY": 0.002027056616322834, + "timestamp": 9.472122345071762 + }, + { + "x": 4.766993522305038, + "y": 6.423644398305699, + "heading": -0.09964051366094051, + "angularVelocity": 0.719317420206062, + "velocityX": -2.74018833502617, + "velocityY": 0.002039081429309675, + "timestamp": 9.53798253276119 + }, + { + "x": 4.60301308935793, + "y": 6.423783110988961, + "heading": -0.05492917751371649, + "angularVelocity": 0.6788826105091881, + "velocityX": -2.4898263837385226, + "velocityY": 0.0021061689638151486, + "timestamp": 9.603842720450617 + }, + { + "x": 4.455491999512653, + "y": 6.4239249853365035, + "heading": -0.013574200580991685, + "angularVelocity": 0.6279207269760504, + "velocityX": -2.2399129887228693, + "velocityY": 0.0021541746618076994, + "timestamp": 9.669702908140044 + }, + { + "x": 4.324407720523135, + "y": 6.424066362703545, + "heading": 0.023974314912572615, + "angularVelocity": 0.570124635396258, + "velocityX": -1.9903417161162953, + "velocityY": 0.0021466286690262756, + "timestamp": 9.735563095829471 + }, + { + "x": 4.2097428344188135, + "y": 6.424202667515828, + "heading": 0.05739701354466356, + "angularVelocity": 0.5074795533486763, + "velocityX": -1.7410349123971218, + "velocityY": 0.002069608621913246, + "timestamp": 9.801423283518899 + }, + { + "x": 4.11148358029628, + "y": 6.424329194128387, + "heading": 0.08645394614427561, + "angularVelocity": 0.4411911599255325, + "velocityX": -1.4919370498287492, + "velocityY": 0.001921139568510551, + "timestamp": 9.867283471208326 + }, + { + "x": 4.029618879443776, + "y": 6.4244415432031925, + "heading": 0.11095800828046558, + "angularVelocity": 0.3720618327378889, + "velocityX": -1.2430074028720826, + "velocityY": 0.00170587237520769, + "timestamp": 9.933143658897754 + }, + { + "x": 3.964139686055167, + "y": 6.424535880555734, + "heading": 0.13075995126873372, + "angularVelocity": 0.3006663613175029, + "velocityX": -0.994215104539091, + "velocityY": 0.0014323881520982617, + "timestamp": 9.99900384658718 + }, + { + "x": 3.91503854489554, + "y": 6.424609101274365, + "heading": 0.14573933177166642, + "angularVelocity": 0.22744211683042614, + "velocityX": -0.7455360041056982, + "velocityY": 0.0011117599448221096, + "timestamp": 10.064864034276608 + }, + { + "x": 3.882309280386553, + "y": 6.424658939920723, + "heading": 0.15579869857714623, + "angularVelocity": 0.1527382043445731, + "velocityX": -0.496950671676277, + "velocityY": 0.000756734046866559, + "timestamp": 10.130724221966036 + }, + { + "x": 3.865946769714355, + "y": 6.424684047698975, + "heading": 0.1608595979864649, + "angularVelocity": 0.07684307602012863, + "velocityX": -0.2484431224119383, + "velocityY": 0.0003812284649235306, + "timestamp": 10.196584409655463 + }, + { + "x": 3.865946769714355, + "y": 6.424684047698975, + "heading": 0.1608595979864649, + "angularVelocity": 3.773099570003496e-26, + "velocityX": -4.36512163919617e-27, + "velocityY": 1.07464594312406e-25, + "timestamp": 10.26244459734489 } ], "trajectoryWaypoints": [ @@ -1126,91 +1531,102 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 22 }, { - "timestamp": 0.8089692619493356, + "timestamp": 1.4829493489002001, "isStopPoint": false, - "x": 2.486186981201172, - "y": 7.724460124969482, + "x": 2.8914051055908203, + "y": 7.769551753997803, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 21 + "controlIntervalCount": 15 }, { - "timestamp": 2.6756977278061993, - "isStopPoint": true, - "x": 8.360527038574219, - "y": 7.469053745269775, + "timestamp": 2.140884893249144, + "isStopPoint": false, + "x": 4.91845178604126, + "y": 7.730569362640381, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 16 }, { - "timestamp": 3.7558412704956616, + "timestamp": 2.8516907246857746, "isStopPoint": false, - "x": 5.0970048904418945, - "y": 6.844727993011475, - "heading": 0.2938692977225419, + "x": 7.296333312988281, + "y": 7.438207149505615, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, + "headingConstrained": true, "controlIntervalCount": 11 }, { - "timestamp": 4.615467015401092, + "timestamp": 3.6092579567107306, "isStopPoint": true, - "x": 2.897675037384033, - "y": 6.319726943969727, - "heading": 0.23441548450132177, + "x": 8.290366172790527, + "y": 7.457698345184326, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 24 + }, + { + "timestamp": 5.831963550719197, + "isStopPoint": true, + "x": 3.8659467697143555, + "y": 6.424684047698975, + "heading": 0.1608595979864649, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 }, { - "timestamp": 5.22662432989127, + "timestamp": 6.950936796652132, "isStopPoint": false, - "x": 4.501265048980713, - "y": 6.1953558921813965, - "heading": 0, + "x": 6.263319492340088, + "y": 6.385702133178711, + "heading": -0.1882215588380072, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 15 + "controlIntervalCount": 16 }, { - "timestamp": 6.516507023544398, + "timestamp": 8.0478847768255, "isStopPoint": true, - "x": 8.275391578674316, - "y": 5.78053617477417, - "heading": 0, + "x": 8.270874977111816, + "y": 5.7814860343933105, + "heading": -0.4182234468320189, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 16 }, { - "timestamp": 7.554313371451169, + "timestamp": 9.076961218935198, "isStopPoint": false, - "x": 5.139572620391846, - "y": 6.3622941970825195, - "heading": 0, + "x": 6.263319492340088, + "y": 6.385702133178711, + "heading": -0.2551830834264048, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 12 + "headingConstrained": true, + "controlIntervalCount": 18 }, { - "timestamp": 8.407024326587997, + "timestamp": 10.26244459734489, "isStopPoint": true, - "x": 2.897675037384033, - "y": 6.319726943969727, - "heading": 0.23441548450132177, + "x": 3.8659467697143555, + "y": 6.424684047698975, + "heading": 0.1608595979864649, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -1226,25 +1642,48 @@ }, { "scope": [ - 2 + 4 ], "type": "StopPoint" }, { "scope": [ - 4 + 5 ], "type": "StopPoint" }, { "scope": [ - 6 + 7 ], "type": "StopPoint" }, { "scope": [ - 8 + 1, + 2 + ], + "type": "StraightLine" + }, + { + "scope": [ + 0, + 0 + ], + "type": "MaxVelocity", + "velocity": 0 + }, + { + "scope": [ + 0, + 1 + ], + "type": "MaxVelocity", + "velocity": 2 + }, + { + "scope": [ + "last" ], "type": "StopPoint" } @@ -1254,7 +1693,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": true + "isTrajectoryStale": false }, "3PieceSourceSide": { "waypoints": [ @@ -1265,52 +1704,52 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 30 + "controlIntervalCount": 33 }, { - "x": 5.707507610321045, - "y": 1.6179983615875244, + "x": 5.813786029815674, + "y": 1.5033589601516724, "heading": 0.16076270850197952, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 14 }, { - "x": 7.384688854217529, - "y": 2.3971612453460693, - "heading": 0, + "x": 7.356464862823486, + "y": 2.2000527381896973, + "heading": 0.22471085620066228, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 10 }, { "x": 8.243088722229004, "y": 2.410367250442505, - "heading": 0, + "heading": 0.2561010030603239, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 19 }, { - "x": 5.667889595031738, - "y": 1.5915861129760742, + "x": 5.826227188110352, + "y": 1.4660362005233765, "heading": -0.04647804366485621, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 22 + "controlIntervalCount": 25 }, { - "x": 1.9701671600341797, - "y": 3.5328900814056396, - "heading": -0.765, + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 18 }, { "x": 3.9378836154937744, @@ -1319,7 +1758,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 9 + "controlIntervalCount": 13 }, { "x": 4.928345203399658, @@ -1328,34 +1767,34 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 10 + "controlIntervalCount": 18 }, { - "x": 6.658350467681885, - "y": 4.179991245269775, - "heading": 0, + "x": 7.406228542327881, + "y": 3.941786766052246, + "heading": 0.1911844788186837, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 10 }, { - "x": 8.282707214355469, - "y": 4.08754825592041, - "heading": 0, + "x": 8.289536476135254, + "y": 4.128400802612305, + "heading": 0.22015079613001914, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 20 }, { - "x": 4.783077239990234, - "y": 4.404496192932129, + "x": 4.957433700561523, + "y": 4.085783958435059, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 13 }, { "x": 3.924677610397339, @@ -1364,12 +1803,12 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 14 + "controlIntervalCount": 18 }, { - "x": 1.996579647064209, - "y": 3.5460963249206543, - "heading": -0.765, + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -1381,1495 +1820,1909 @@ "x": 0.6476043462753296, "y": 4.391018867492676, "heading": -1.0516500181249593, - "angularVelocity": -2.3506527398385264e-24, - "velocityX": -4.584841665957037e-25, - "velocityY": 1.794170584470112e-24, + "angularVelocity": 1.9413513581547193e-28, + "velocityX": -2.7445681219758215e-27, + "velocityY": -2.6836170825004164e-27, "timestamp": 0 }, { - "x": 0.692835797571452, - "y": 4.365429285724839, - "heading": -1.0363921320591865, - "angularVelocity": 0.19907376380794284, - "velocityX": 0.5901469714218079, - "velocityY": -0.33387419035865423, - "timestamp": 0.07664438434234237 - }, - { - "x": 0.7831334822061629, - "y": 4.314114094234643, - "heading": -1.005707809669664, - "angularVelocity": 0.4003466483919687, - "velocityX": 1.178138299492162, - "velocityY": -0.6695231742092023, - "timestamp": 0.15328876868468475 - }, - { - "x": 0.9181042106922528, - "y": 4.23677088276396, - "heading": -0.9591055566031375, - "angularVelocity": 0.608032192657082, - "velocityX": 1.7609995780411658, - "velocityY": -1.009117786441079, - "timestamp": 0.2299331530270271 - }, - { - "x": 1.0717422840106643, - "y": 4.1370476005177, - "heading": -0.8558256704459198, - "angularVelocity": 1.3475205919314872, - "velocityX": 2.004557471975592, - "velocityY": -1.3011166193315893, - "timestamp": 0.3065775373693695 - }, - { - "x": 1.203873817721518, - "y": 4.032707540283105, - "heading": -0.6892805911828493, - "angularVelocity": 2.172958667384875, - "velocityX": 1.7239558363554812, - "velocityY": -1.3613529696911204, - "timestamp": 0.3832219217117119 - }, - { - "x": 1.3594029637702276, - "y": 3.9301727362790966, - "heading": -0.5828610080840567, - "angularVelocity": 1.388485066609126, - "velocityX": 2.0292308090572955, - "velocityY": -1.3377993036779194, - "timestamp": 0.45986630605405426 - }, - { - "x": 1.5344193266927146, - "y": 3.825053132953821, - "heading": -0.5408815443461281, - "angularVelocity": 0.5477174107162478, - "velocityX": 2.2834857951334486, - "velocityY": -1.371523879110889, - "timestamp": 0.5365106903963967 - }, - { - "x": 1.7162004854447428, - "y": 3.7215786578067394, - "heading": -0.5174827165758584, - "angularVelocity": 0.30529083077705393, - "velocityX": 2.3717479148906513, - "velocityY": -1.3500594470809355, - "timestamp": 0.613155074738739 - }, - { - "x": 1.8995749541613056, - "y": 3.6205751589308552, - "heading": -0.4949062907307691, - "angularVelocity": 0.2945607305585351, - "velocityX": 2.3925362606801817, - "velocityY": -1.317819951749349, - "timestamp": 0.6897994590810813 - }, - { - "x": 2.0827414581868835, - "y": 3.51960952250048, - "heading": -0.4719606385920979, - "angularVelocity": 0.29937812581521006, - "velocityX": 2.389822889142672, - "velocityY": -1.3173259501883174, - "timestamp": 0.7664438434234236 - }, - { - "x": 2.265688686133792, - "y": 3.4186835470349224, - "heading": -0.4486127865936518, - "angularVelocity": 0.3046257361029851, - "velocityX": 2.3869619348724966, - "velocityY": -1.3168084828597342, - "timestamp": 0.843088227765766 - }, - { - "x": 2.4484040933032216, - "y": 3.3177997829423713, - "heading": -0.4248255225210326, - "angularVelocity": 0.3103588642107185, - "velocityX": 2.3839373065260236, - "velocityY": -1.3162577396660924, - "timestamp": 0.9197326121081083 - }, - { - "x": 2.6308732997732713, - "y": 3.216961383200288, - "heading": -0.4005559741960246, - "angularVelocity": 0.3166513572162684, - "velocityX": 2.380725059451553, - "velocityY": -1.3156658587232646, - "timestamp": 0.9963769964504506 - }, - { - "x": 2.813079628281676, - "y": 3.116172257902354, - "heading": -0.3757541558021446, - "angularVelocity": 0.32359602868097065, - "velocityX": 2.377295219628298, - "velocityY": -1.3150229617312381, - "timestamp": 1.073021380792793 - }, - { - "x": 2.9950035114027176, - "y": 3.015437295026463, - "heading": -0.3503610814650098, - "angularVelocity": 0.3313103047930201, - "velocityX": 2.3736100783125105, - "velocityY": -1.314316289970363, - "timestamp": 1.1496657651351354 - }, - { - "x": 3.176621688858468, - "y": 2.9147626672707037, - "heading": -0.3243062033321442, - "angularVelocity": 0.3399450378058761, - "velocityX": 2.3696214538631923, - "velocityY": -1.3135290813490341, - "timestamp": 1.2263101494774777 - }, - { - "x": 3.3579060875063353, - "y": 2.8141562687774018, - "heading": -0.2975038256949741, - "angularVelocity": 0.34969786589260987, - "velocityX": 2.3652665515341145, - "velocityY": -1.3126388757189247, - "timestamp": 1.30295453381982 - }, - { - "x": 3.538822210205962, - "y": 2.713628355793928, - "heading": -0.2698479287435623, - "angularVelocity": 0.3608339630974529, - "velocityX": 2.360461555689992, - "velocityY": -1.3116148540570647, - "timestamp": 1.3795989181621624 - }, - { - "x": 3.7193267349323875, - "y": 2.6131925205499895, - "heading": -0.24120442629231653, - "angularVelocity": 0.3737195190101045, - "velocityX": 2.3550913256759576, - "velocityY": -1.3104134909000977, - "timestamp": 1.4562433025045047 - }, - { - "x": 3.8993637811598734, - "y": 2.5128672365819, - "heading": -0.21139907897320392, - "angularVelocity": 0.38887842305553666, - "velocityX": 2.3489920073377766, - "velocityY": -1.3089710985213812, - "timestamp": 1.532887686846847 - }, - { - "x": 4.078858787892925, - "y": 2.4126784448405125, - "heading": -0.18019759590707388, - "angularVelocity": 0.4070941835316264, - "velocityX": 2.34191987153701, - "velocityY": -1.30719024754481, - "timestamp": 1.6095320711891894 - }, - { - "x": 4.257707770811751, - "y": 2.312664186467197, - "heading": -0.1472705722503286, - "angularVelocity": 0.42960777804245054, - "velocityX": 2.3334910242082882, - "velocityY": -1.3049130635140744, - "timestamp": 1.6861764555315317 - }, - { - "x": 4.435756686256742, - "y": 2.212883689420889, - "heading": -0.1121258503357887, - "angularVelocity": 0.4585426866704457, - "velocityX": 2.323052327613603, - "velocityY": -1.3018631162933536, - "timestamp": 1.762820839873874 - }, - { - "x": 4.612756399460092, - "y": 2.113437617489987, - "heading": -0.07396027426521577, - "angularVelocity": 0.4979565874011239, - "velocityX": 2.3093631023605954, - "velocityY": -1.2974997814153513, - "timestamp": 1.8394652242162164 - }, - { - "x": 4.788243072222853, - "y": 2.014522987095371, - "heading": -0.031264185221419126, - "angularVelocity": 0.5570674147904805, - "velocityX": 2.289622054747365, - "velocityY": -1.290565920039225, - "timestamp": 1.9161096085585587 - }, - { - "x": 4.961081026742291, - "y": 1.9166475782485084, - "heading": 0.019697120176888277, - "angularVelocity": 0.664905926710585, - "velocityX": 2.255063511860639, - "velocityY": -1.2770069155972317, - "timestamp": 1.992753992900901 - }, - { - "x": 5.123264328610966, - "y": 1.823526252461316, - "heading": 0.10546513381285838, - "angularVelocity": 1.1190384575714751, - "velocityX": 2.1160493787028227, - "velocityY": -1.214979108857517, - "timestamp": 2.0693983772432434 - }, - { - "x": 5.295119103926639, - "y": 1.7237778734191072, - "heading": 0.15590676979164073, - "angularVelocity": 0.6581256593239505, - "velocityX": 2.24223570702925, - "velocityY": -1.3014440640121712, - "timestamp": 2.1460427615855857 - }, - { - "x": 5.493802683128641, - "y": 1.6448426706793149, - "heading": 0.16075963684913044, - "angularVelocity": 0.06331666826122177, - "velocityX": 2.5922783633378295, - "velocityY": -1.029888926854938, - "timestamp": 2.222687145927928 - }, - { - "x": 5.707507610321045, - "y": 1.6179983615875244, + "x": 0.6597456116335061, + "y": 4.380178489721837, + "heading": -1.0354550463341647, + "angularVelocity": 0.23714267860510804, + "velocityX": 0.17778432873405597, + "velocityY": -0.15873545535467184, + "timestamp": 0.0682921011352938 + }, + { + "x": 0.6840676878744021, + "y": 4.358487813648757, + "heading": -1.0032553908058022, + "angularVelocity": 0.47149897269336843, + "velocityX": 0.35614772186773713, + "velocityY": -0.3176161768710194, + "timestamp": 0.1365842022705876 + }, + { + "x": 0.7206162348586238, + "y": 4.325939175146822, + "heading": -0.9552579550391015, + "angularVelocity": 0.7028255825898914, + "velocityX": 0.5351797115132725, + "velocityY": -0.4766091240545095, + "timestamp": 0.2048763034058814 + }, + { + "x": 0.7694577963308973, + "y": 4.282521644299476, + "heading": -0.8917843367572964, + "angularVelocity": 0.9294430428499669, + "velocityX": 0.7151861000075717, + "velocityY": -0.6357621178082019, + "timestamp": 0.2731684045411752 + }, + { + "x": 0.8306912777270616, + "y": 4.228207914210413, + "heading": -0.8133985275963236, + "angularVelocity": 1.1478019838001796, + "velocityX": 0.896640759007464, + "velocityY": -0.7953149659498937, + "timestamp": 0.341460505676469 + }, + { + "x": 0.9044507708380568, + "y": 4.162933772374405, + "heading": -0.7210416262705941, + "angularVelocity": 1.3523804333206972, + "velocityX": 1.0800589216733842, + "velocityY": -0.9558080766426159, + "timestamp": 0.40975260681176284 + }, + { + "x": 0.9908959110037516, + "y": 4.086578645763268, + "heading": -0.6161174549931975, + "angularVelocity": 1.5364027396013313, + "velocityX": 1.265814621729648, + "velocityY": -1.1180667360031697, + "timestamp": 0.47804470794705667 + }, + { + "x": 1.090196283285551, + "y": 3.998968512246339, + "heading": -0.5004646416711678, + "angularVelocity": 1.6935020507409695, + "velocityX": 1.4540535527684928, + "velocityY": -1.2828735982711192, + "timestamp": 0.5463368090823505 + }, + { + "x": 1.2025392306050893, + "y": 3.8999248788172114, + "heading": -0.3762963474938359, + "angularVelocity": 1.818194082670579, + "velocityX": 1.645035742815635, + "velocityY": -1.4502941303989414, + "timestamp": 0.6146289102176443 + }, + { + "x": 1.328190264179142, + "y": 3.789348006758151, + "heading": -0.2466808070464207, + "angularVelocity": 1.897958011141482, + "velocityX": 1.8399058088010056, + "velocityY": -1.619175134764066, + "timestamp": 0.6829210113529381 + }, + { + "x": 1.467420004237056, + "y": 3.6673010913969546, + "heading": -0.11832684408091969, + "angularVelocity": 1.8794847549238287, + "velocityX": 2.0387385619031573, + "velocityY": -1.7871307710888695, + "timestamp": 0.751213112488232 + }, + { + "x": 1.6176774224315258, + "y": 3.534549695823147, + "heading": -0.01051011565957327, + "angularVelocity": 1.5787584014694496, + "velocityX": 2.2002166531206018, + "velocityY": -1.9438762809598271, + "timestamp": 0.8195052136235258 + }, + { + "x": 1.7740513259099606, + "y": 3.391053821163867, + "heading": 0.062440796343179686, + "angularVelocity": 1.0682188831506234, + "velocityX": 2.2897802363503494, + "velocityY": -2.1012074936016356, + "timestamp": 0.8877973147588196 + }, + { + "x": 1.9367743639870942, + "y": 3.237147837801074, + "heading": 0.09987435620827004, + "angularVelocity": 0.5481389390982501, + "velocityX": 2.382750499281934, + "velocityY": -2.253642526796603, + "timestamp": 0.9560894158941134 + }, + { + "x": 2.1058806251727042, + "y": 3.0730806615870963, + "heading": 0.10139956904864746, + "angularVelocity": 0.022333663996599216, + "velocityX": 2.4762199196447727, + "velocityY": -2.4024326896743653, + "timestamp": 1.0243815170294073 + }, + { + "x": 2.282556811687828, + "y": 2.9164516007892924, + "heading": 0.1013996340867454, + "angularVelocity": 9.523516904308559e-7, + "velocityX": 2.587066199136408, + "velocityY": -2.293516500356401, + "timestamp": 1.092673618164701 + }, + { + "x": 2.4592332259385974, + "y": 2.759822796875614, + "heading": 0.10139969912475692, + "angularVelocity": 9.52350424751949e-7, + "velocityX": 2.5870695338653458, + "velocityY": -2.2935127388067964, + "timestamp": 1.1609657192999947 + }, + { + "x": 2.6359096401941113, + "y": 2.603193992967287, + "heading": 0.10139976416276904, + "angularVelocity": 9.523504335129237e-7, + "velocityX": 2.587069533934816, + "velocityY": -2.293512738728436, + "timestamp": 1.2292578204352884 + }, + { + "x": 2.812586054449808, + "y": 2.4465651890591653, + "heading": 0.1013998292007818, + "angularVelocity": 9.523504429819945e-7, + "velocityX": 2.587069533937483, + "velocityY": -2.2935127387254277, + "timestamp": 1.2975499215705821 + }, + { + "x": 2.989262477460467, + "y": 2.2899363950265936, + "heading": 0.10139989423879318, + "angularVelocity": 9.523504226466593e-7, + "velocityX": 2.5870696621362548, + "velocityY": -2.2935125941179324, + "timestamp": 1.3658420227058758 + }, + { + "x": 3.166357964338375, + "y": 2.1337815804962608, + "heading": 0.10139995920086388, + "angularVelocity": 9.512384243710626e-7, + "velocityX": 2.593206006754183, + "velocityY": -2.286572120851488, + "timestamp": 1.4341341238411696 + }, + { + "x": 3.3547011225730468, + "y": 1.9913948102576182, + "heading": 0.10140002311622047, + "angularVelocity": 9.359114091359504e-7, + "velocityX": 2.7579054547105564, + "velocityY": -2.0849668976586258, + "timestamp": 1.5024262249764633 + }, + { + "x": 3.553226828100459, + "y": 1.8635876061361314, + "heading": 0.1014000890794247, + "angularVelocity": 9.658980048505205e-7, + "velocityX": 2.907008310289235, + "velocityY": -1.8714785750739689, + "timestamp": 1.570718326111757 + }, + { + "x": 3.760808917151287, + "y": 1.7510851059584576, + "heading": 0.10140016079724051, + "angularVelocity": 0.0000010501626778013022, + "velocityX": 3.0396207701910694, + "velocityY": -1.647372072427445, + "timestamp": 1.6390104272470507 + }, + { + "x": 3.976269792438543, + "y": 1.654525560969519, + "heading": 0.10140024381940511, + "angularVelocity": 0.0000012156920527539028, + "velocityX": 3.1549896943484823, + "velocityY": -1.413919668361702, + "timestamp": 1.7073025283823444 + }, + { + "x": 4.1983871429917885, + "y": 1.5744567665619278, + "heading": 0.10140036224681147, + "angularVelocity": 0.0000017341303662335387, + "velocityX": 3.252460341104571, + "velocityY": -1.1724459062837547, + "timestamp": 1.7755946295176381 + }, + { + "x": 4.4257370453636415, + "y": 1.5113830168900522, + "heading": 0.10193866126599585, + "angularVelocity": 0.007882302788106277, + "velocityX": 3.3290805026111183, + "velocityY": -0.9235877740372923, + "timestamp": 1.8438867306529318 + }, + { + "x": 4.656186679939347, + "y": 1.4657767864094913, + "heading": 0.10576151989897133, + "angularVelocity": 0.05597804972206098, + "velocityX": 3.374469825128989, + "velocityY": -0.6678112068950683, + "timestamp": 1.9121788317882256 + }, + { + "x": 4.888314550102186, + "y": 1.4378182802801935, + "heading": 0.1133638582654562, + "angularVelocity": 0.11132090300492935, + "velocityX": 3.3990441984347375, + "velocityY": -0.4093958988596496, + "timestamp": 1.9804709329235193 + }, + { + "x": 5.121043949314908, + "y": 1.4275947415879202, + "heading": 0.12410092792765016, + "angularVelocity": 0.15722271659093798, + "velocityX": 3.4078523774171208, + "velocityY": -0.14970309189959885, + "timestamp": 2.048763034058813 + }, + { + "x": 5.353451540876787, + "y": 1.4351381309484952, + "heading": 0.13671420054587038, + "angularVelocity": 0.18469592249375974, + "velocityX": 3.403140153814491, + "velocityY": 0.11045771377908847, + "timestamp": 2.117055135194107 + }, + { + "x": 5.584673054960648, + "y": 1.4604251835156339, + "heading": 0.14959239179836323, + "angularVelocity": 0.18857512125713907, + "velocityX": 3.385772442786424, + "velocityY": 0.37027785273501185, + "timestamp": 2.185347236329401 + }, + { + "x": 5.813786029815674, + "y": 1.5033589601516724, "heading": 0.16076270850197952, - "angularVelocity": 0.000040076685010459537, - "velocityX": 2.7882659509385723, - "velocityY": -0.3502449569153927, - "timestamp": 2.2993315302702704 - }, - { - "x": 5.878932815955422, - "y": 1.6310716458031873, - "heading": 0.15207722794791115, - "angularVelocity": -0.13999820175733138, - "velocityX": 2.7631425083845143, - "velocityY": 0.21072366345901047, - "timestamp": 2.3613714739627154 - }, - { - "x": 6.041874898509352, - "y": 1.6781214038756682, - "heading": 0.13576601260266036, - "angularVelocity": -0.2629147348377915, - "velocityX": 2.6264060354679595, - "velocityY": 0.7583784779967588, - "timestamp": 2.4234114176551604 - }, - { - "x": 6.190063655455228, - "y": 1.7574804032751512, - "heading": 0.11619159565916007, - "angularVelocity": -0.3155131319998917, - "velocityX": 2.388602376567293, - "velocityY": 1.2791597586370276, - "timestamp": 2.4854513613476055 - }, - { - "x": 6.338186422130831, - "y": 1.840598011238034, - "heading": 0.10221625072518932, - "angularVelocity": -0.22526366244385518, - "velocityX": 2.387538702644562, - "velocityY": 1.3397434461727966, - "timestamp": 2.5474913050400505 - }, - { - "x": 6.486157900665009, - "y": 1.9236682945064507, - "heading": 0.08780760406345454, - "angularVelocity": -0.2322479003714719, - "velocityX": 2.3851001423812606, - "velocityY": 1.338980636091913, - "timestamp": 2.6095312487324955 - }, - { - "x": 6.633958405719426, - "y": 2.0066826604418155, - "heading": 0.07290105260244177, - "angularVelocity": -0.2402734524536335, - "velocityX": 2.382344281083177, - "velocityY": 1.3380793242962596, - "timestamp": 2.6715711924249406 - }, - { - "x": 6.781563534615612, - "y": 2.089630624589182, - "heading": 0.0574168439817537, - "angularVelocity": -0.24958450474179966, - "velocityX": 2.3791950816060985, - "velocityY": 1.33700901726427, - "timestamp": 2.7336111361173856 - }, - { - "x": 6.928941267710664, - "y": 2.172498140132396, - "heading": 0.04124990446967813, - "angularVelocity": -0.2605892035012319, - "velocityX": 2.3755297687834913, - "velocityY": 1.3357122945504047, - "timestamp": 2.7956510798098306 - }, - { - "x": 7.076047973733186, - "y": 2.255265656701969, - "heading": 0.024256449523087663, - "angularVelocity": -0.27391151466599145, - "velocityX": 2.3711611788653904, - "velocityY": 1.334100446317001, - "timestamp": 2.8576910235022757 - }, - { - "x": 7.222828783744423, - "y": 2.337909309225835, - "heading": 0.006256947367936294, - "angularVelocity": -0.2901276352599791, - "velocityX": 2.3659081758501124, - "velocityY": 1.3321039253929894, - "timestamp": 2.9197309671947207 - }, - { - "x": 7.384688854217529, - "y": 2.3971612453460693, - "heading": 6.712344331303172e-23, - "angularVelocity": -0.10085353073423609, - "velocityX": 2.608965463855139, - "velocityY": 0.9550610879656509, - "timestamp": 2.9817709108871657 - }, - { - "x": 7.581926675665867, - "y": 2.420671247090372, - "heading": -6.833387833600707e-9, - "angularVelocity": -9.667586454477017e-8, - "velocityX": 2.7904368043745778, - "velocityY": 0.33260950489354324, - "timestamp": 3.052454404288316 - }, - { - "x": 7.7805365921893905, - "y": 2.417576114646826, - "heading": -6.833380269760843e-9, - "angularVelocity": 1.0700998918582695e-13, - "velocityX": 2.8098486218890097, - "velocityY": -0.043788617322290876, - "timestamp": 3.1231378976894666 - }, - { - "x": 7.9655574312129644, - "y": 2.4146925691677037, - "heading": -4.100028695417927e-9, - "angularVelocity": 3.8670295465311964e-8, - "velocityX": 2.6175961334214803, - "velocityY": -0.0407951749463946, - "timestamp": 3.193821391090617 - }, - { - "x": 8.104323073360915, - "y": 2.412529909857503, - "heading": -2.050014465682545e-9, - "angularVelocity": 2.9002729365693992e-8, - "velocityX": 1.9631972822906998, - "velocityY": -0.03059638405147332, - "timestamp": 3.2645048844917675 - }, - { - "x": 8.196833505192958, - "y": 2.411088136925815, - "heading": -6.833381783003922e-10, - "angularVelocity": 1.933515480942414e-8, - "velocityX": 1.3087982410125008, - "velocityY": -0.020397590191323155, - "timestamp": 3.335188377892918 + "angularVelocity": 0.16356674517140343, + "velocityX": 3.3548971410489967, + "velocityY": 0.6286785136539, + "timestamp": 2.253639337464695 + }, + { + "x": 5.939295701159701, + "y": 1.5323683761963922, + "heading": 0.16590262686638915, + "angularVelocity": 0.1364467184876021, + "velocityX": 3.331839453313385, + "velocityY": 0.7700977610756782, + "timestamp": 2.291309121109046 + }, + { + "x": 6.063755261240901, + "y": 1.5666696664667372, + "heading": 0.1699280929818009, + "angularVelocity": 0.1068619388265435, + "velocityX": 3.3039627000847966, + "velocityY": 0.910578372156086, + "timestamp": 2.328978904753397 + }, + { + "x": 6.186965476091788, + "y": 1.6062187514342638, + "heading": 0.17281888262781223, + "angularVelocity": 0.07674027738794295, + "velocityX": 3.270796987159313, + "velocityY": 1.0498888271012834, + "timestamp": 2.366648688397748 + }, + { + "x": 6.308696366891361, + "y": 1.650958086998814, + "heading": 0.17466669111475505, + "angularVelocity": 0.049052803286273795, + "velocityX": 3.231526147027085, + "velocityY": 1.1876716889840546, + "timestamp": 2.4043184720420987 + }, + { + "x": 6.428664923898692, + "y": 1.7008051924773087, + "heading": 0.17574653518384492, + "angularVelocity": 0.028666054450562455, + "velocityX": 3.184742395655416, + "velocityY": 1.323264979409296, + "timestamp": 2.4419882556864496 + }, + { + "x": 6.546495426793477, + "y": 1.7556223876439374, + "heading": 0.17665953793741, + "angularVelocity": 0.02423700550512633, + "velocityX": 3.1279845938922866, + "velocityY": 1.4552033450515791, + "timestamp": 2.4796580393308005 + }, + { + "x": 6.661649606485285, + "y": 1.8151243678255207, + "heading": 0.1786491578806416, + "angularVelocity": 0.05281739767916041, + "velocityX": 3.0569376447447945, + "velocityY": 1.5795678770909583, + "timestamp": 2.5173278229751515 + }, + { + "x": 6.773137667082048, + "y": 1.8784298401932633, + "heading": 0.18358189083734322, + "angularVelocity": 0.13094667607525115, + "velocityX": 2.9596151028991167, + "velocityY": 1.6805371903758264, + "timestamp": 2.5549976066195024 + }, + { + "x": 6.87927675328215, + "y": 1.941846015988482, + "heading": 0.19021855833237603, + "angularVelocity": 0.17618013306609673, + "velocityX": 2.817618683510011, + "velocityY": 1.683475976234567, + "timestamp": 2.5926673902638533 + }, + { + "x": 6.980896999193432, + "y": 2.002302596889058, + "heading": 0.19752796800079392, + "angularVelocity": 0.1940390668931813, + "velocityX": 2.6976593991274846, + "velocityY": 1.6049091619787552, + "timestamp": 2.630337173908204 + }, + { + "x": 7.078947568278477, + "y": 2.0586931301517772, + "heading": 0.20493066809205301, + "angularVelocity": 0.19651559884574055, + "velocityX": 2.60289705963705, + "velocityY": 1.496969926748577, + "timestamp": 2.668006957552555 + }, + { + "x": 7.174005720052268, + "y": 2.1105770010329152, + "heading": 0.21206754751188253, + "angularVelocity": 0.18945899682383116, + "velocityX": 2.5234589258928724, + "velocityY": 1.3773339228859343, + "timestamp": 2.705676741196906 + }, + { + "x": 7.266431305502043, + "y": 2.15773800547381, + "heading": 0.21871161412654994, + "angularVelocity": 0.17637655361643606, + "velocityX": 2.4535735676739243, + "velocityY": 1.2519584632115888, + "timestamp": 2.743346524841257 + }, + { + "x": 7.356464862823486, + "y": 2.2000527381896973, + "heading": 0.22471085620066228, + "angularVelocity": 0.15925873455373588, + "velocityX": 2.390073650846282, + "velocityY": 1.123307028131367, + "timestamp": 2.781016308485608 + }, + { + "x": 7.523755330928558, + "y": 2.2638899288150194, + "heading": 0.23355968892559228, + "angularVelocity": 0.12033403723737161, + "velocityX": 2.274959652214718, + "velocityY": 0.868113017007844, + "timestamp": 2.854551884354925 + }, + { + "x": 7.678148398275777, + "y": 2.3116679387336183, + "heading": 0.24020478966195502, + "angularVelocity": 0.09036579448527025, + "velocityX": 2.0995697051668696, + "velocityY": 0.6497264671389397, + "timestamp": 2.9280874602242424 + }, + { + "x": 7.816541043927953, + "y": 2.346462739824989, + "heading": 0.2451990584982588, + "angularVelocity": 0.06791636262126016, + "velocityX": 1.8819822108705344, + "velocityY": 0.47316962817026026, + "timestamp": 3.0016230360935596 + }, + { + "x": 7.936995928235685, + "y": 2.3711072013144094, + "heading": 0.2489351660825785, + "angularVelocity": 0.05080680391976913, + "velocityX": 1.6380491059429139, + "velocityY": 0.3351365811456589, + "timestamp": 3.075158611962877 + }, + { + "x": 8.038363690822308, + "y": 2.387966753250885, + "heading": 0.2516849557707684, + "angularVelocity": 0.03739400495178875, + "velocityX": 1.3784860101832423, + "velocityY": 0.22927068615654433, + "timestamp": 3.148694187832194 + }, + { + "x": 8.119972690853793, + "y": 2.3989446235760092, + "heading": 0.2536437125866727, + "angularVelocity": 0.026636859679799414, + "velocityX": 1.1097893647629, + "velocityY": 0.14928652146049065, + "timestamp": 3.2222297637015114 + }, + { + "x": 8.18143147852008, + "y": 2.405561042236668, + "heading": 0.2549588543062311, + "angularVelocity": 0.0178844281017885, + "velocityX": 0.8357694481852939, + "velocityY": 0.08997575095375589, + "timestamp": 3.2957653395708286 + }, + { + "x": 8.222513130352727, + "y": 2.409037637931504, + "heading": 0.25574648281571827, + "angularVelocity": 0.010710849818962056, + "velocityX": 0.5586636311335063, + "velocityY": 0.04727773807081853, + "timestamp": 3.369300915440146 }, { "x": 8.243088722229004, "y": 2.410367250442505, - "heading": 1.830282426459157e-24, - "angularVelocity": 9.667577894349971e-9, - "velocityX": 0.6543991363518575, - "velocityY": -0.010198795342763814, - "timestamp": 3.4058718712940683 + "heading": 0.2561010030603239, + "angularVelocity": 0.004821071167453232, + "velocityX": 0.2798045929883303, + "velocityY": 0.018081214368450626, + "timestamp": 3.442836491309463 }, { "x": 8.243088722229004, "y": 2.410367250442505, - "heading": 1.851361909343925e-24, - "angularVelocity": 2.3995205522592844e-25, - "velocityX": -1.4269511777904615e-24, - "velocityY": -1.1273229249768833e-23, - "timestamp": 3.4765553646952188 - }, - { - "x": 8.183672767375366, - "y": 2.3891879079868126, - "heading": -0.016263075453039278, - "angularVelocity": -0.1934255260988222, - "velocityX": -0.7066659906617425, - "velocityY": -0.251897340619777, - "timestamp": 3.5606346265230004 - }, - { - "x": 8.064809267825755, - "y": 2.3467183743812834, - "heading": -0.04860613329336098, - "angularVelocity": -0.38467342763509904, - "velocityX": -1.413707696352949, - "velocityY": -0.5051130645333048, - "timestamp": 3.644713888350782 - }, - { - "x": 7.886434927132461, - "y": 2.2827439256274595, - "heading": -0.09665252446442359, - "angularVelocity": -0.5714416388368797, - "velocityX": -2.121502220828929, - "velocityY": -0.7608826167487317, - "timestamp": 3.728793150178564 - }, - { - "x": 7.663093479676549, - "y": 2.2056330475266677, - "heading": -0.09665343057468777, - "angularVelocity": -0.000010776857984756087, - "velocityX": -2.6563202697162027, - "velocityY": -0.9171212546886659, - "timestamp": 3.8128724120063455 - }, - { - "x": 7.439949308521917, - "y": 2.127952816871332, - "heading": -0.09665400996983407, - "angularVelocity": -0.000006891058909294993, - "velocityX": -2.653973956285386, - "velocityY": -0.9238928716387514, - "timestamp": 3.896951673834127 - }, - { - "x": 7.216805138002767, - "y": 2.0502725843903784, - "heading": -0.0966545893647738, - "angularVelocity": -0.0000068910564521426285, - "velocityX": -2.6539739487272445, - "velocityY": -0.9238928933518099, - "timestamp": 3.981030935661909 - }, - { - "x": 6.9936609674836125, - "y": 1.9725923519093191, - "heading": -0.09665516875951666, - "angularVelocity": -0.000006891054111144413, - "velocityX": -2.6539739487273097, - "velocityY": -0.923892893353071, - "timestamp": 4.06511019748969 - }, - { - "x": 6.77051679696445, - "y": 1.8949121194281597, - "heading": -0.09665574815406265, - "angularVelocity": -0.000006891051769457115, - "velocityX": -2.653973948727398, - "velocityY": -0.9238928933542627, - "timestamp": 4.149189459317472 - }, - { - "x": 6.547372626445282, - "y": 1.8172318869468993, - "heading": -0.09665632754841177, - "angularVelocity": -0.000006891049427961194, - "velocityX": -2.653973948727487, - "velocityY": -0.9238928933554545, - "timestamp": 4.2332687211452535 - }, - { - "x": 6.324228455926046, - "y": 1.7395516544657041, - "heading": -0.09665690694256396, - "angularVelocity": -0.000006891047085669869, - "velocityX": -2.653973948728258, - "velocityY": -0.9238928933546852, - "timestamp": 4.317347982973035 - }, - { - "x": 6.101084267512398, - "y": 1.6618714733923579, - "heading": -0.0966574863411208, - "angularVelocity": -0.000006891099472478973, - "velocityX": -2.65397416155617, - "velocityY": -0.9238922819334152, - "timestamp": 4.401427244800817 - }, - { - "x": 5.881706688567194, - "y": 1.6001619743925184, - "heading": -0.12280606064162562, - "angularVelocity": -0.3109990945694144, - "velocityX": -2.609175844033366, - "velocityY": -0.7339443479682077, - "timestamp": 4.485506506628599 - }, - { - "x": 5.667889595031738, - "y": 1.5915861129760742, - "heading": -0.19572183715761898, - "angularVelocity": -0.8672266493650448, - "velocityX": -2.5430419926070895, - "velocityY": -0.10199734429174848, - "timestamp": 4.56958576845638 - }, - { - "x": 5.472836165647801, - "y": 1.6405608510115604, - "heading": -0.2695567389374151, - "angularVelocity": -0.9372981580360532, - "velocityX": -2.4761083941768933, - "velocityY": 0.6217104735625232, - "timestamp": 4.648359956954205 - }, - { - "x": 5.3021481158717245, - "y": 1.712801388975876, - "heading": -0.4003704259742984, - "angularVelocity": -1.660616117175175, - "velocityX": -2.1668017536072774, - "velocityY": 0.9170584850431085, - "timestamp": 4.7271341454520295 - }, - { - "x": 5.1196004435735345, - "y": 1.8042197974618441, - "heading": -0.46428021863316127, - "angularVelocity": -0.8113037262278816, - "velocityX": -2.317353891918915, - "velocityY": 1.160512221442844, - "timestamp": 4.805908333949854 - }, - { - "x": 4.929096458765624, - "y": 1.904953408569058, - "heading": -0.48639976153317277, - "angularVelocity": -0.28079683614414136, - "velocityX": -2.4183554085507573, - "velocityY": 1.2787641869518616, - "timestamp": 4.884682522447679 - }, - { - "x": 4.737761672165951, - "y": 2.009314965020544, - "heading": -0.49933816069591924, - "angularVelocity": -0.1642466829487382, - "velocityX": -2.4289020331190754, - "velocityY": 1.3248191881325238, - "timestamp": 4.963456710945503 - }, - { - "x": 4.546911439579052, - "y": 2.114853649108329, - "heading": -0.5116829622012672, - "angularVelocity": -0.15671124946833295, - "velocityX": -2.4227508556583697, - "velocityY": 1.3397622508126334, - "timestamp": 5.042230899443328 - }, - { - "x": 4.356102309238819, - "y": 2.2203468351822107, - "heading": -0.524131682346679, - "angularVelocity": -0.15803044604839828, - "velocityX": -2.4222290826328545, - "velocityY": 1.3391846756605428, - "timestamp": 5.121005087941152 - }, - { - "x": 4.165335431709755, - "y": 2.3257928672028427, - "heading": -0.5366900663313149, - "angularVelocity": -0.15942257513681343, - "velocityX": -2.421692703750687, - "velocityY": 1.338586077894589, - "timestamp": 5.199779276438977 - }, - { - "x": 3.9746120593595244, - "y": 2.4311903935174577, - "heading": -0.5493635573249664, - "angularVelocity": -0.1608838026176728, - "velocityX": -2.421140426670299, - "velocityY": 1.3379703215543044, - "timestamp": 5.2785534649368016 - }, - { - "x": 3.783933515355731, - "y": 2.5365380161026203, - "heading": -0.5621578157899224, - "angularVelocity": -0.16241688691352676, - "velocityX": -2.420571352620895, - "velocityY": 1.337336817986659, - "timestamp": 5.357327653434626 - }, - { - "x": 3.593301216494676, - "y": 2.6418342523116722, - "heading": -0.5750788548667545, - "angularVelocity": -0.16402630510359092, - "velocityX": -2.4199842930317086, - "velocityY": 1.3366844929409736, - "timestamp": 5.436101841932451 - }, - { - "x": 3.4027166923911927, - "y": 2.7470775186047, - "heading": -0.5881331219292356, - "angularVelocity": -0.16571756956711148, - "velocityX": -2.41937781572635, - "velocityY": 1.3360120656264767, - "timestamp": 5.514876030430275 - }, - { - "x": 3.212181603664246, - "y": 2.8522661171153323, - "heading": -0.601327569493579, - "angularVelocity": -0.16749709284161973, - "velocityX": -2.4187502576711153, - "velocityY": 1.3353180847243658, - "timestamp": 5.5936502189281 - }, - { - "x": 3.0216977610811666, - "y": 2.9573982215901036, - "heading": -0.6146697280062193, - "angularVelocity": -0.1693722114701131, - "velocityX": -2.418099712805536, - "velocityY": 1.3346009204229918, - "timestamp": 5.6724244074259245 - }, - { - "x": 2.831267146879349, - "y": 3.0624718615565794, - "heading": -0.6281677862205469, - "angularVelocity": -0.17135128233914515, - "velocityX": -2.417424004400561, - "velocityY": 1.3338587419326686, - "timestamp": 5.751198595923749 - }, - { - "x": 2.640891939244419, - "y": 3.167484904027678, - "heading": -0.6418306831098634, - "angularVelocity": -0.17344382912549475, - "velocityX": -2.416720644988769, - "velocityY": 1.3330894862090363, - "timestamp": 5.829972784421574 - }, - { - "x": 2.4505746379296203, - "y": 3.272435032259823, - "heading": -0.655668504824747, - "angularVelocity": -0.17566441468661878, - "velocityX": -2.4159855524256404, - "velocityY": 1.3322908205527566, - "timestamp": 5.908746972919398 - }, - { - "x": 2.2618233887297388, - "y": 3.3772509809337916, - "heading": -0.674261275863946, - "angularVelocity": -0.2360261831159634, - "velocityX": -2.396105282697931, - "velocityY": 1.3305874763389332, - "timestamp": 5.987521161417223 - }, - { - "x": 2.116002537558778, - "y": 3.455078745186431, - "heading": -0.7196337030492912, - "angularVelocity": -0.5759808898139023, - "velocityX": -1.8511247649982117, - "velocityY": 0.9879856046347999, - "timestamp": 6.066295349915047 - }, - { - "x": 2.018778748901899, - "y": 3.5069532190081367, - "heading": -0.7498768931574519, - "angularVelocity": -0.3839225853655822, - "velocityX": -1.234208698443955, - "velocityY": 0.6585212086715196, - "timestamp": 6.145069538412872 - }, - { - "x": 1.9701671600341797, - "y": 3.5328900814056396, - "heading": -0.765, - "angularVelocity": -0.19198048410191668, - "velocityX": -0.617100471546741, - "velocityY": 0.32925585007098457, - "timestamp": 6.2238437269106965 - }, - { - "x": 1.9701671600341797, - "y": 3.5328900814056396, - "heading": -0.765, - "angularVelocity": -6.585493827087804e-24, - "velocityX": 2.2425722891132833e-25, - "velocityY": -1.917344763102387e-25, - "timestamp": 6.302617915408521 - }, - { - "x": 2.021866320736439, - "y": 3.5175409291347064, - "heading": -0.7649999986292967, - "angularVelocity": 1.7960508607271394e-8, - "velocityX": 0.6774210494904483, - "velocityY": -0.20112200466940705, - "timestamp": 6.37893553314589 - }, - { - "x": 2.1252646329115827, - "y": 3.486842627325834, - "heading": -0.7649999949328548, - "angularVelocity": 4.8434973827775214e-8, - "velocityX": 1.354841978047161, - "velocityY": -0.4022439735280295, - "timestamp": 6.45525315088326 - }, - { - "x": 2.264271046839328, - "y": 3.4455725064565113, - "heading": -0.7629134147129198, - "angularVelocity": 0.027340740995291143, - "velocityX": 1.8214197199669733, - "velocityY": -0.5407679392109075, - "timestamp": 6.531570768620629 - }, - { - "x": 2.403277460756995, - "y": 3.404302385553228, - "heading": -0.7557796152547765, - "angularVelocity": 0.09347513286765456, - "velocityX": 1.8214197198349207, - "velocityY": -0.5407679396558968, - "timestamp": 6.607888386357998 - }, - { - "x": 2.5422838746752925, - "y": 3.3630322646520687, - "heading": -0.7445970322006652, - "angularVelocity": 0.1465268883600871, - "velocityX": 1.8214197198431827, - "velocityY": -0.5407679396280685, - "timestamp": 6.684206004095367 - }, - { - "x": 2.6812902885942673, - "y": 3.321762143753191, - "heading": -0.7301562502874436, - "angularVelocity": 0.1892195058147188, - "velocityX": 1.821419719852059, - "velocityY": -0.540767939598171, - "timestamp": 6.7605236218327365 - }, - { - "x": 2.8202967025138705, - "y": 3.28049202285643, - "heading": -0.7130778427136069, - "angularVelocity": 0.2237806692631405, - "velocityX": 1.8214197198602944, - "velocityY": -0.5407679395704321, - "timestamp": 6.836841239570106 - }, - { - "x": 2.9593031164340085, - "y": 3.2392219019614696, - "heading": -0.6938417314556549, - "angularVelocity": 0.25205335056643224, - "velocityX": 1.8214197198673001, - "velocityY": -0.5407679395468353, - "timestamp": 6.913158857307475 - }, - { - "x": 3.0983095303545722, - "y": 3.1979517810679434, - "heading": -0.6728120786629418, - "angularVelocity": 0.27555436629432156, - "velocityX": 1.8214197198728768, - "velocityY": -0.5407679395280517, - "timestamp": 6.989476475044844 - }, - { - "x": 3.2373159442754544, - "y": 3.1566816601754897, - "heading": -0.6502538625008166, - "angularVelocity": 0.2955833375165695, - "velocityX": 1.8214197198770503, - "velocityY": -0.540767939513994, - "timestamp": 7.065794092782213 - }, - { - "x": 3.376322358196558, - "y": 3.1154115392837802, - "heading": -0.6263485003282439, - "angularVelocity": 0.31323517270727913, - "velocityX": 1.8214197198799456, - "velocityY": -0.5407679395042426, - "timestamp": 7.142111710519583 - }, - { - "x": 3.515328772117798, - "y": 3.074141418392532, - "heading": -0.6012000869662939, - "angularVelocity": 0.3295230394702948, - "velocityX": 1.821419719881741, - "velocityY": -0.5407679394981955, - "timestamp": 7.218429328256952 - }, - { - "x": 3.6543351862262687, - "y": 3.032871298131916, - "heading": -0.5748442396833396, - "angularVelocity": 0.3453442083799368, - "velocityX": 1.821419722335047, - "velocityY": -0.5407679312349462, - "timestamp": 7.294746945994321 - }, - { - "x": 3.7934386683270223, - "y": 2.9919295436941926, - "heading": -0.5472452836552583, - "angularVelocity": 0.3616328293036799, - "velocityX": 1.8226916172809426, - "velocityY": -0.5364653097351076, - "timestamp": 7.37106456373169 + "heading": 0.2561010030603239, + "angularVelocity": 1.4585674185817174e-27, + "velocityX": 1.7040232418526742e-26, + "velocityY": -1.4024664023967223e-26, + "timestamp": 3.5163720671787804 + }, + { + "x": 8.23076641369307, + "y": 2.401959089356935, + "heading": 0.2504586013872685, + "angularVelocity": -0.08952800568025769, + "velocityX": -0.19551810957860963, + "velocityY": -0.1334123192654238, + "timestamp": 3.5793959403253224 + }, + { + "x": 8.206023953099224, + "y": 2.3852645245784534, + "heading": 0.2393386250800309, + "angularVelocity": -0.17644070019913852, + "velocityX": -0.39258870263834844, + "velocityY": -0.26489271358590016, + "timestamp": 3.6424198134718644 + }, + { + "x": 8.16875053390707, + "y": 2.3604262651400836, + "heading": 0.22292161113767642, + "angularVelocity": -0.26048881356723236, + "velocityX": -0.5914174634346175, + "velocityY": -0.3941087432157136, + "timestamp": 3.7054436866184064 + }, + { + "x": 8.118819252308954, + "y": 2.32761409583082, + "heading": 0.20141017927873436, + "angularVelocity": -0.34132195920304775, + "velocityX": -0.7922598073592967, + "velocityY": -0.520630797046853, + "timestamp": 3.7684675597649484 + }, + { + "x": 8.05608308887793, + "y": 2.287033381738045, + "heading": 0.17503801825565893, + "angularVelocity": -0.418447164644346, + "velocityX": -0.9954349090090397, + "velocityY": -0.6438943223691957, + "timestamp": 3.8314914329114904 + }, + { + "x": 7.980369725106798, + "y": 2.23893720959337, + "heading": 0.1440830091327847, + "angularVelocity": -0.4911632303349274, + "velocityX": -1.2013441889723828, + "velocityY": -0.7631421196987141, + "timestamp": 3.8945153060580324 + }, + { + "x": 7.891474875927368, + "y": 2.1836442871211665, + "heading": 0.10888684951759207, + "angularVelocity": -0.558457578977337, + "velocityX": -1.4104948607765395, + "velocityY": -0.8773329805300566, + "timestamp": 3.9575391792045744 + }, + { + "x": 7.789153901265338, + "y": 2.121566339688964, + "heading": 0.06988488995899932, + "angularVelocity": -0.6188442190454735, + "velocityX": -1.6235272374345413, + "velocityY": -0.9849909936169672, + "timestamp": 4.020563052351116 + }, + { + "x": 7.6731119959595455, + "y": 2.0532519037884556, + "heading": 0.027652018184153897, + "angularVelocity": -0.6701091136789754, + "velocityX": -1.8412372885425963, + "velocityY": -1.083945376407843, + "timestamp": 4.0835869254976584 + }, + { + "x": 7.542995375924786, + "y": 1.9794598312116642, + "heading": -0.01702647841112277, + "angularVelocity": -0.7089138506513402, + "velocityX": -2.0645608328801752, + "velocityY": -1.1708590553489355, + "timestamp": 4.1466107986442005 + }, + { + "x": 7.398393469015792, + "y": 1.9012891169513035, + "heading": -0.06304666165055829, + "angularVelocity": -0.7302023969937583, + "velocityX": -2.294398926780781, + "velocityY": -1.2403349771696772, + "timestamp": 4.2096346717907425 + }, + { + "x": 7.238888974084811, + "y": 1.8204184823931229, + "heading": -0.10882385565793654, + "angularVelocity": -0.7263468860591595, + "velocityX": -2.530858339348697, + "velocityY": -1.2831746213080508, + "timestamp": 4.2726585449372845 + }, + { + "x": 7.064291128353646, + "y": 1.7395528396698954, + "heading": -0.1519933648459332, + "angularVelocity": -0.6849707425568674, + "velocityX": -2.770344585538159, + "velocityY": -1.2830954158466277, + "timestamp": 4.3356824180838265 + }, + { + "x": 6.875551934246269, + "y": 1.6630631067471717, + "heading": -0.18835706354611678, + "angularVelocity": -0.5769829254325797, + "velocityX": -2.994725406173053, + "velocityY": -1.213662840823371, + "timestamp": 4.3987062912303685 + }, + { + "x": 6.6765745235709275, + "y": 1.5964044626442309, + "heading": -0.21283448502566993, + "angularVelocity": -0.3883833261506904, + "velocityX": -3.157175221724038, + "velocityY": -1.0576729225756598, + "timestamp": 4.4617301643769105 + }, + { + "x": 6.470320359493221, + "y": 1.5423309052989251, + "heading": -0.22714102483187307, + "angularVelocity": -0.22700191359134367, + "velocityX": -3.272635491603752, + "velocityY": -0.8579853100994786, + "timestamp": 4.5247540375234525 + }, + { + "x": 6.258646009422052, + "y": 1.5021402605713223, + "heading": -0.23470258386507345, + "angularVelocity": -0.11997928174960579, + "velocityX": -3.35863760037386, + "velocityY": -0.6377050904845553, + "timestamp": 4.5877779106699945 + }, + { + "x": 6.043403030940588, + "y": 1.4765880663278637, + "heading": -0.23830057114107106, + "angularVelocity": -0.05708927579921407, + "velocityX": -3.415261038955589, + "velocityY": -0.4054367490878456, + "timestamp": 4.6508017838165365 + }, + { + "x": 5.826227188110352, + "y": 1.4660362005233765, + "heading": -0.2397939206210153, + "angularVelocity": -0.023694981050623704, + "velocityX": -3.4459298038580273, + "velocityY": -0.16742648900603332, + "timestamp": 4.7138256569630785 + }, + { + "x": 5.585950011247348, + "y": 1.472816838427013, + "heading": -0.2400341563641951, + "angularVelocity": -0.003454323639043374, + "velocityX": -3.454919409470775, + "velocityY": 0.09749805540300888, + "timestamp": 4.78337204817682 + }, + { + "x": 5.346828039308694, + "y": 1.498003817957295, + "heading": -0.24003425372372114, + "angularVelocity": -0.0000013999220426161217, + "velocityX": -3.438308843427179, + "velocityY": 0.3621608409971011, + "timestamp": 4.852918439390561 + }, + { + "x": 5.110339958561461, + "y": 1.5414445236933543, + "heading": -0.24003430995698885, + "angularVelocity": -8.085720445004714e-7, + "velocityX": -3.4004364082734204, + "velocityY": 0.6246291860428812, + "timestamp": 4.922464830604302 + }, + { + "x": 4.87787710863241, + "y": 1.6028833780076126, + "heading": -0.24003435241212726, + "angularVelocity": -6.104578199921794e-7, + "velocityX": -3.34255805185645, + "velocityY": 0.8834226081614251, + "timestamp": 4.992011221818044 + }, + { + "x": 4.6508071407642975, + "y": 1.6819589143395182, + "heading": -0.24003438676872058, + "angularVelocity": -4.940097212282082e-7, + "velocityX": -3.26501438687515, + "velocityY": 1.1370185418949712, + "timestamp": 5.061557613031785 + }, + { + "x": 4.430465976004345, + "y": 1.7782059025459391, + "heading": -0.24003441611574786, + "angularVelocity": -4.2197771504773454e-7, + "velocityX": -3.1682616583621654, + "velocityY": 1.3839249819680632, + "timestamp": 5.131104004245526 + }, + { + "x": 4.218149944366051, + "y": 1.8910580842509719, + "heading": -0.24003444232077203, + "angularVelocity": -3.767991943003991e-7, + "velocityX": -3.0528691414881703, + "velocityY": 1.6226892544028175, + "timestamp": 5.2006503954592675 + }, + { + "x": 4.015108153343392, + "y": 2.0198514989870335, + "heading": -0.24003446662822625, + "angularVelocity": -3.495142422988789e-7, + "velocityX": -2.9195158437284845, + "velocityY": 1.851906511442587, + "timestamp": 5.270196786673009 + }, + { + "x": 3.822535116912615, + "y": 2.1638283667067197, + "heading": -0.24003448995474508, + "angularVelocity": -3.3540947876441084e-7, + "velocityX": -2.768986759340679, + "velocityY": 2.0702277315467437, + "timestamp": 5.33974317788675 + }, + { + "x": 3.6415634821904335, + "y": 2.3221412856645247, + "heading": -0.24003451305454837, + "angularVelocity": -3.3214956098454113e-7, + "velocityX": -2.602171465173362, + "velocityY": 2.2763642540595908, + "timestamp": 5.409289569100491 + }, + { + "x": 3.464389353911089, + "y": 2.4846929425572983, + "heading": -0.24003453619468335, + "angularVelocity": -3.3272948572772484e-7, + "velocityX": -2.5475675327972547, + "velocityY": 2.3373126061019125, + "timestamp": 5.478835960314233 + }, + { + "x": 3.2927692258661447, + "y": 2.639311449150887, + "heading": -0.26906245693036734, + "angularVelocity": -0.4173893171030334, + "velocityX": -2.4677071671122786, + "velocityY": 2.2232427002342847, + "timestamp": 5.548382351527974 + }, + { + "x": 3.1316790096082485, + "y": 2.78734512036997, + "heading": -0.33414951484331207, + "angularVelocity": -0.9358797311697813, + "velocityX": -2.31629871006257, + "velocityY": 2.128560068115134, + "timestamp": 5.617928742741715 + }, + { + "x": 2.9841668130955736, + "y": 2.9229007119052977, + "heading": -0.399802866713189, + "angularVelocity": -0.9440224104238585, + "velocityX": -2.1210618399927625, + "velocityY": 1.9491391166324632, + "timestamp": 5.687475133955457 + }, + { + "x": 2.850184873056573, + "y": 3.0460054481110204, + "heading": -0.4624392764751436, + "angularVelocity": -0.9006421277769835, + "velocityX": -1.9265117528129565, + "velocityY": 1.7701096211790086, + "timestamp": 5.757021525169198 + }, + { + "x": 2.729677322997678, + "y": 3.156697524743402, + "heading": -0.5206194141968715, + "angularVelocity": -0.8365658764797604, + "velocityX": -1.7327649638718297, + "velocityY": 1.5916293383532172, + "timestamp": 5.826567916382939 + }, + { + "x": 2.6226093994758237, + "y": 3.255010853878184, + "heading": -0.5735505408260317, + "angularVelocity": -0.7610909165147557, + "velocityX": -1.5395180346999657, + "velocityY": 1.4136366735784966, + "timestamp": 5.89611430759668 + }, + { + "x": 2.528959064966524, + "y": 3.3409742896619217, + "heading": -0.6207186986875527, + "angularVelocity": -0.6782258150039155, + "velocityX": -1.346587980697349, + "velocityY": 1.2360588994408095, + "timestamp": 5.965660698810422 + }, + { + "x": 2.448711492549665, + "y": 3.414612123721043, + "heading": -0.6617567306008036, + "angularVelocity": -0.5900814003005007, + "velocityX": -1.153871121367444, + "velocityY": 1.0588304119592038, + "timestamp": 6.035207090024163 + }, + { + "x": 2.381856029019407, + "y": 3.475944657171963, + "heading": -0.6963860701683602, + "angularVelocity": -0.4979315096469609, + "velocityX": -0.9613074433263825, + "velocityY": 0.8818938320238976, + "timestamp": 6.104753481237904 + }, + { + "x": 2.3283844592549388, + "y": 3.524988689664759, + "heading": -0.7243871660827067, + "angularVelocity": -0.40262471460652677, + "velocityX": -0.7688618896145184, + "velocityY": 0.7051988124310513, + "timestamp": 6.174299872451646 + }, + { + "x": 2.288289941735103, + "y": 3.5617579108116493, + "heading": -0.7455827114186115, + "angularVelocity": -0.30476844256034935, + "velocityX": -0.5765147093917502, + "velocityY": 0.5287006342843157, + "timestamp": 6.243846263665387 + }, + { + "x": 2.261566307205076, + "y": 3.5862632051940473, + "heading": -0.7598272213603463, + "angularVelocity": -0.20482026016211682, + "velocityX": -0.384256236213544, + "velocityY": 0.35235896435063624, + "timestamp": 6.313392654879128 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -0.1031366044229252, + "velocityX": -0.1920838428798636, + "velocityY": 0.17613685695088044, + "timestamp": 6.382939046092869 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": 2.0585376302520875e-27, + "velocityX": 2.290307616331396e-26, + "velocityY": 1.9208049966092184e-26, + "timestamp": 6.452485437306611 + }, + { + "x": 2.262787388478086, + "y": 3.590396400582275, + "heading": -0.766999999504867, + "angularVelocity": 7.484697168821697e-9, + "velocityX": 0.2203964012318347, + "velocityY": -0.12269319419383633, + "timestamp": 6.518638147942005 + }, + { + "x": 2.29194702702894, + "y": 3.5741634259266726, + "heading": -0.7669999984635936, + "angularVelocity": 1.5740448734328548e-8, + "velocityX": 0.4407928000346015, + "velocityY": -0.24538638703819796, + "timestamp": 6.5847908585773975 + }, + { + "x": 2.335686484549911, + "y": 3.5498139641128845, + "heading": -0.7669999968047696, + "angularVelocity": 2.5075676414905366e-8, + "velocityX": 0.6611891954366733, + "velocityY": -0.36807957799329516, + "timestamp": 6.6509435692127905 + }, + { + "x": 2.394005760703551, + "y": 3.5173480153283805, + "heading": -0.7669999944212796, + "angularVelocity": 3.603011826808707e-8, + "velocityX": 0.881585585737703, + "velocityY": -0.49077276611449583, + "timestamp": 6.7170962798481835 + }, + { + "x": 2.466904854927447, + "y": 3.476765579885611, + "heading": -0.7669999911345978, + "angularVelocity": 4.968325032500711e-8, + "velocityX": 1.1019819675369964, + "velocityY": -0.6134659495125361, + "timestamp": 6.783248990483576 + }, + { + "x": 2.5543837660967745, + "y": 3.428066658409475, + "heading": -0.7669999865876731, + "angularVelocity": 6.873376258795686e-8, + "velocityX": 1.3223783323328206, + "velocityY": -0.7361591234642574, + "timestamp": 6.849401701118969 + }, + { + "x": 2.6564424908370574, + "y": 3.371251252774671, + "heading": -0.7669999797093515, + "angularVelocity": 1.0397641269909702e-7, + "velocityX": 1.5427746461182656, + "velocityY": -0.8588522690770366, + "timestamp": 6.915554411754362 + }, + { + "x": 2.7662623647276248, + "y": 3.3101152676874617, + "heading": -0.7638688738714546, + "angularVelocity": 0.047331482078705314, + "velocityX": 1.6600963563813693, + "velocityY": -0.9241644748945469, + "timestamp": 6.981707122389755 + }, + { + "x": 2.8760822385936273, + "y": 3.2489792825561232, + "heading": -0.7552335462540741, + "angularVelocity": 0.13053626275384136, + "velocityX": 1.6600963560100328, + "velocityY": -0.9241644755616281, + "timestamp": 7.047859833025148 + }, + { + "x": 2.9859021124597604, + "y": 3.187843297425019, + "heading": -0.7414908207894041, + "angularVelocity": 0.2077424391634433, + "velocityX": 1.660096356012001, + "velocityY": -0.9241644755580926, + "timestamp": 7.114012543660541 + }, + { + "x": 3.0957219863265877, + "y": 3.1267073122951614, + "heading": -0.722965226886389, + "angularVelocity": 0.2800428542546129, + "velocityX": 1.6600963560224962, + "velocityY": -0.9241644755392399, + "timestamp": 7.180165254295934 + }, + { + "x": 3.205541937606669, + "y": 3.065571466224591, + "heading": -0.6999120814365468, + "angularVelocity": 0.34848376171464274, + "velocityX": 1.6600975262429405, + "velocityY": -0.9241623734441646, + "timestamp": 7.246317964931327 + }, + { + "x": 3.3205554118020584, + "y": 3.0148771458100074, + "heading": -0.675189942683574, + "angularVelocity": 0.37371316330831955, + "velocityX": 1.738605615562701, + "velocityY": -0.7663226484246434, + "timestamp": 7.31247067556672 + }, + { + "x": 3.4412706707327407, + "y": 2.979865153460279, + "heading": -0.6504677784331367, + "angularVelocity": 0.3737135487417248, + "velocityX": 1.8247968642738608, + "velocityY": -0.5292601317986865, + "timestamp": 7.378623386202113 + }, + { + "x": 3.5655600724891614, + "y": 2.961152646598003, + "heading": -0.6257456072463011, + "angularVelocity": 0.37371365359605785, + "velocityX": 1.878825532054966, + "velocityY": -0.28286833120734667, + "timestamp": 7.444776096837506 + }, + { + "x": 3.691232955543935, + "y": 2.9590694551388426, + "heading": -0.6010234330400018, + "angularVelocity": 0.3737136992398969, + "velocityX": 1.8997389804241076, + "velocityY": -0.03149064398345659, + "timestamp": 7.510928807472899 + }, + { + "x": 3.816074269542962, + "y": 2.973652301113666, + "heading": -0.5763012572280835, + "angularVelocity": 0.3737137235113, + "velocityX": 1.8871685347423104, + "velocityY": 0.22044215323538027, + "timestamp": 7.577081518108292 }, { "x": 3.9378836154937744, "y": 3.0046441555023193, - "heading": -0.5196460913813736, - "angularVelocity": 0.361635924864182, - "velocityX": 1.8926815517726128, - "velocityY": 0.16660126698243774, - "timestamp": 7.4473821814690595 - }, - { - "x": 4.075926460541626, - "y": 3.0786892532968806, - "heading": -0.4898305952486719, - "angularVelocity": 0.3616359432061461, - "velocityX": 1.674339217752807, - "velocityY": 0.8981024049221815, - "timestamp": 7.529828345056582 - }, - { - "x": 4.180404466119937, - "y": 3.195406219293214, - "heading": -0.4598310277403017, - "angularVelocity": 0.3638685683236616, - "velocityX": 1.2672270125389096, - "velocityY": 1.4156749194573495, - "timestamp": 7.612274508644104 - }, - { - "x": 4.2848822354372995, - "y": 3.3121233967757027, - "heading": -0.4287713690600015, - "angularVelocity": 0.37672654892338603, - "velocityX": 1.2672241468998562, - "velocityY": 1.4156774845998201, - "timestamp": 7.694720672231626 - }, - { - "x": 4.389360004752337, - "y": 3.428840574260275, - "heading": -0.39697164385324546, - "angularVelocity": 0.38570290991161005, - "velocityX": 1.2672241468716323, - "velocityY": 1.415677484625084, - "timestamp": 7.777166835819148 - }, - { - "x": 4.493837774065624, - "y": 3.545557751746413, - "heading": -0.36470801458445545, - "angularVelocity": 0.3913296612587668, - "velocityX": 1.2672241468504064, - "velocityY": 1.415677484644084, - "timestamp": 7.859612999406671 - }, - { - "x": 4.598315543377444, - "y": 3.662274929233865, - "heading": -0.3322260766024941, - "angularVelocity": 0.3939775553956429, - "velocityX": 1.267224146832605, - "velocityY": 1.4156774846600186, - "timestamp": 7.942059162994193 - }, - { - "x": 4.7027933126881845, - "y": 3.778992106722283, - "heading": -0.29975459263588233, - "angularVelocity": 0.3938507573143906, - "velocityX": 1.2672241468195173, - "velocityY": 1.415677484671734, - "timestamp": 8.024505326581716 - }, - { - "x": 4.807271113642882, - "y": 3.8957092558850426, - "heading": -0.26751670065443317, - "angularVelocity": 0.3910174904284827, - "velocityX": 1.2672245306330898, - "velocityY": 1.4156771411062206, - "timestamp": 8.106951490169239 + "heading": -0.5515790804630568, + "angularVelocity": 0.37371373791899654, + "velocityX": 1.841335672882331, + "velocityY": 0.4684895613645789, + "timestamp": 7.643234228743685 + }, + { + "x": 4.041142175460698, + "y": 3.0442215809369055, + "heading": -0.5298282579971898, + "angularVelocity": 0.37371374601013674, + "velocityX": 1.7741463943907607, + "velocityY": 0.6800031557338396, + "timestamp": 7.701436054868853 + }, + { + "x": 4.139081405257562, + "y": 3.095569514841118, + "heading": -0.5080774349580934, + "angularVelocity": 0.3737137558591309, + "velocityX": 1.68275183644991, + "velocityY": 0.8822392237965997, + "timestamp": 7.7596378809940205 + }, + { + "x": 4.230365088756901, + "y": 3.1579873993126313, + "heading": -0.4863266111818347, + "angularVelocity": 0.3737137685247537, + "velocityX": 1.5683989588750435, + "velocityY": 1.0724385921719906, + "timestamp": 7.817839707119188 + }, + { + "x": 4.31374781521548, + "y": 3.2306236449287598, + "heading": -0.46457578637658614, + "angularVelocity": 0.3737137862044347, + "velocityX": 1.4326479426824048, + "velocityY": 1.2480062989762448, + "timestamp": 7.876041533244356 + }, + { + "x": 4.388091972544511, + "y": 3.312487248347592, + "heading": -0.44282495993350157, + "angularVelocity": 0.3737138143450645, + "velocityX": 1.277350940314963, + "velocityY": 1.4065469912022657, + "timestamp": 7.934243359369524 + }, + { + "x": 4.452383275670078, + "y": 3.40246130939932, + "heading": -0.4210741301794481, + "angularVelocity": 0.3737138712327796, + "velocityX": 1.1046269061610605, + "velocityY": 1.5458975609842933, + "timestamp": 7.992445185494692 + }, + { + "x": 4.505744665604898, + "y": 3.4993182420166447, + "heading": -0.39932328851517956, + "angularVelocity": 0.3737140758692305, + "velocityX": 0.9168336027818437, + "velocityY": 1.6641562484487396, + "timestamp": 8.050647011619859 + }, + { + "x": 4.553249848192767, + "y": 3.5991779484118642, + "heading": -0.376755167144662, + "angularVelocity": 0.3877562419086832, + "velocityX": 0.8162146405115538, + "velocityY": 1.7157486808139633, + "timestamp": 8.108848837745025 + }, + { + "x": 4.608641716419732, + "y": 3.694888158238082, + "heading": -0.35375349017349716, + "angularVelocity": 0.3952054171238199, + "velocityX": 0.9517204513109475, + "velocityY": 1.644453725908632, + "timestamp": 8.167050663870192 + }, + { + "x": 4.674815961157583, + "y": 3.7834865366072044, + "heading": -0.33075180592440745, + "angularVelocity": 0.3952055421701522, + "velocityX": 1.1369788397281038, + "velocityY": 1.5222611431226378, + "timestamp": 8.22525248999536 + }, + { + "x": 4.750869807438685, + "y": 3.8637642779379298, + "heading": -0.3077501195977143, + "angularVelocity": 0.3952055778666857, + "velocityX": 1.3067261174510774, + "velocityY": 1.37929935665733, + "timestamp": 8.283454316120526 + }, + { + "x": 4.835765641210989, + "y": 3.9346261199009778, + "heading": -0.2847484322977862, + "angularVelocity": 0.3952055945884119, + "velocityX": 1.4586455344155085, + "velocityY": 1.2175192202845004, + "timestamp": 8.341656142245693 }, { "x": 4.928345203399658, "y": 3.995105266571045, - "heading": -0.23561923052699993, - "angularVelocity": 0.386888470481369, - "velocityX": 1.4685230274935492, - "velocityY": 1.205586850387367, - "timestamp": 8.189397653756762 - }, - { - "x": 5.0976675546813945, - "y": 4.035971247104969, - "heading": -0.20015091281875375, - "angularVelocity": 0.38688845257830395, - "velocityX": 1.846968413139047, - "velocityY": 0.4457661652272064, - "timestamp": 8.281073477813743 - }, - { - "x": 5.270991671667468, - "y": 4.053258165584531, - "heading": -0.16565527924639437, - "angularVelocity": 0.37627841284435726, - "velocityX": 1.890619678295383, - "velocityY": 0.18856572774100455, - "timestamp": 8.372749301870725 - }, - { - "x": 5.444315791533781, - "y": 4.0705450551858755, - "heading": -0.13259854065392815, - "angularVelocity": 0.3605829446585594, - "velocityX": 1.8906197097130382, - "velocityY": 0.1885654127373893, - "timestamp": 8.464425125927706 - }, - { - "x": 5.617639911400379, - "y": 4.087831944784366, - "heading": -0.10138296753519387, - "angularVelocity": 0.34049950943808543, - "velocityX": 1.8906197097161441, - "velocityY": 0.18856541270624588, - "timestamp": 8.556100949984687 - }, - { - "x": 5.790964031267238, - "y": 4.105118834380232, - "heading": -0.07255368127548635, - "angularVelocity": 0.3144698894856807, - "velocityX": 1.8906197097189994, - "velocityY": 0.1885654126776188, - "timestamp": 8.647776774041668 - }, - { - "x": 5.9642881511343315, - "y": 4.122405723973758, - "heading": -0.046846868579348507, - "angularVelocity": 0.2804099440672579, - "velocityX": 1.890619709721544, - "velocityY": 0.18856541265210713, - "timestamp": 8.73945259809865 - }, - { - "x": 6.137612271001621, - "y": 4.139692613565316, - "heading": -0.02524481611998314, - "angularVelocity": 0.23563521442620056, - "velocityX": 1.890619709723685, - "velocityY": 0.18856541263063997, - "timestamp": 8.831128422155631 - }, - { - "x": 6.310936390869065, - "y": 4.156979503155326, - "heading": -0.009053820761373118, - "angularVelocity": 0.17661139700851253, - "velocityX": 1.8906197097253696, - "velocityY": 0.18856541261374912, - "timestamp": 8.922804246212612 - }, - { - "x": 6.484260512045991, - "y": 4.174266379616022, - "heading": -2.2378824393561327e-8, - "angularVelocity": 0.09875884373748682, - "velocityX": 1.890619724009207, - "velocityY": 0.188565269399175, - "timestamp": 9.014480070269594 - }, - { - "x": 6.658350467681885, - "y": 4.179991245269775, - "heading": 1.903926994866919e-23, - "angularVelocity": 2.4410824362649644e-7, - "velocityX": 1.8989734472164426, - "velocityY": 0.06244684149438171, - "timestamp": 9.106155894326575 - }, - { - "x": 6.892610562082988, - "y": 4.180747179242153, - "heading": 1.815110943876254e-8, - "angularVelocity": 2.0923543279309324e-7, - "velocityX": 2.7004141208851267, - "velocityY": 0.008713967176889024, - "timestamp": 9.192905586749424 - }, - { - "x": 7.135847606603004, - "y": 4.164439410363542, - "heading": 1.8151086995989633e-8, - "angularVelocity": -2.587072333894115e-13, - "velocityX": 2.803895180796635, - "velocityY": -0.18798647491591486, - "timestamp": 9.279655279172273 - }, - { - "x": 7.379084647879251, - "y": 4.14813159310281, - "heading": 1.8151064626797785e-8, - "angularVelocity": -2.5785903361398285e-13, - "velocityX": 2.803895143404351, - "velocityY": -0.18798703263687086, - "timestamp": 9.366404971595122 - }, - { - "x": 7.6223216891554895, - "y": 4.131823775841945, - "heading": 1.8151042187582598e-8, - "angularVelocity": -2.586662220352974e-13, - "velocityX": 2.8038951434042487, - "velocityY": -0.18798703263839134, - "timestamp": 9.45315466401797 - }, - { - "x": 7.865558730431713, - "y": 4.115515958581081, - "heading": 1.815101972750476e-8, - "angularVelocity": -2.589067144937878e-13, - "velocityX": 2.803895143404087, - "velocityY": -0.18798703263838049, - "timestamp": 9.53990435644082 - }, - { - "x": 8.07413296336415, - "y": 4.101532107856125, - "heading": 9.075510567318239e-9, - "angularVelocity": -1.0461719121664748e-7, - "velocityX": 2.404322448957763, - "velocityY": -0.16119769804825285, - "timestamp": 9.626654048863669 - }, - { - "x": 8.213182462520122, - "y": 4.092209539999878, - "heading": 3.025170279496359e-9, - "angularVelocity": -6.974480391619583e-8, - "velocityX": 1.6028817540723412, - "velocityY": -0.10746514017369478, - "timestamp": 9.713403741286518 - }, - { - "x": 8.282707214355469, - "y": 4.08754825592041, - "heading": -2.730279460976851e-23, - "angularVelocity": -3.487240352104816e-8, - "velocityX": 0.8014409030577104, - "velocityY": -0.05373257183146045, - "timestamp": 9.800153433709367 - }, - { - "x": 8.282707214355469, - "y": 4.08754825592041, - "heading": -1.3454200564607368e-23, - "angularVelocity": 4.663099853298001e-24, - "velocityX": 1.7036719365270032e-27, - "velocityY": 2.800838691430789e-26, - "timestamp": 9.886903126132216 - }, - { - "x": 8.214708333565108, - "y": 4.098060716631318, - "heading": 1.6847207046591105e-8, - "angularVelocity": 1.954345432230813e-7, - "velocityX": -0.7888150344560528, - "velocityY": 0.12194887565072522, - "timestamp": 9.973106960770398 - }, - { - "x": 8.078710576708064, - "y": 4.119085637322864, - "heading": 5.0541618776805506e-8, - "angularVelocity": 3.9086905903476216e-7, - "velocityX": -1.577630014115494, - "velocityY": 0.24389774283002824, - "timestamp": 10.05931079540858 - }, - { - "x": 7.8747139579553656, - "y": 4.15062301580424, - "heading": 1.0108322622636101e-7, - "angularVelocity": 5.863034708570761e-7, - "velocityX": -2.3664448293851432, - "velocityY": 0.3658465845950713, - "timestamp": 10.145514630046762 - }, - { - "x": 7.635308863013457, - "y": 4.1876344581296925, - "heading": 1.0108335901717796e-7, - "angularVelocity": 1.540428189705246e-12, - "velocityX": -2.7771977423829006, - "velocityY": 0.42934798064143237, - "timestamp": 10.231718464684944 - }, - { - "x": 7.395903768071533, - "y": 4.224645900455147, - "heading": 1.0108349186566566e-7, - "angularVelocity": 1.5410971948056658e-12, - "velocityX": -2.7771977423830965, - "velocityY": 0.42934798064146257, - "timestamp": 10.317922299323126 - }, - { - "x": 7.156498673129608, - "y": 4.261657342780601, - "heading": 1.0108362465293872e-7, - "angularVelocity": 1.5403870787070826e-12, - "velocityX": -2.7771977423830965, - "velocityY": 0.42934798064146257, - "timestamp": 10.404126133961308 - }, - { - "x": 6.917093578187684, - "y": 4.298668785106054, - "heading": 1.0108375751155647e-7, - "angularVelocity": 1.5412147075799493e-12, - "velocityX": -2.7771977423830965, - "velocityY": 0.4293479806414625, - "timestamp": 10.49032996859949 - }, - { - "x": 6.677688483245759, - "y": 4.335680227431509, - "heading": 1.0108389035423936e-7, - "angularVelocity": 1.5410298558530182e-12, - "velocityX": -2.7771977423830965, - "velocityY": 0.4293479806414627, - "timestamp": 10.576533803237671 - }, - { - "x": 6.438283388303835, - "y": 4.372691669756963, - "heading": 1.0108402320108348e-7, - "angularVelocity": 1.5410781280848912e-12, - "velocityX": -2.777197742383096, - "velocityY": 0.4293479806414625, - "timestamp": 10.662737637875853 - }, - { - "x": 6.1988782933619095, - "y": 4.409703112082417, - "heading": 1.0108415606074574e-7, - "angularVelocity": 1.5412268238689798e-12, - "velocityX": -2.7771977423830965, - "velocityY": 0.42934798064146257, - "timestamp": 10.748941472514035 - }, - { - "x": 5.959473198419985, - "y": 4.446714554407871, - "heading": 1.0108428894851187e-7, - "angularVelocity": 1.5415528401754331e-12, - "velocityX": -2.777197742383096, - "velocityY": 0.42934798064146257, - "timestamp": 10.835145307152217 - }, - { - "x": 5.72006810347805, - "y": 4.483725996733255, - "heading": 1.0108442171706879e-7, - "angularVelocity": 1.5401699643641334e-12, - "velocityX": -2.7771977423832217, - "velocityY": 0.4293479806406499, - "timestamp": 10.9213491417904 - }, - { - "x": 5.480663004845917, - "y": 4.520737415188933, - "heading": 1.0108455459674728e-7, - "angularVelocity": 1.5414590205195092e-12, - "velocityX": -2.7771977851910266, - "velocityY": 0.4293477037422273, - "timestamp": 11.007552976428581 - }, - { - "x": 5.240152409397331, - "y": 4.549708085687679, - "heading": 1.0108468832096651e-7, - "angularVelocity": 1.5512560408121339e-12, - "velocityX": -2.790022003755009, - "velocityY": 0.3360717144468437, - "timestamp": 11.093756811066763 - }, - { - "x": 5.001198008888612, - "y": 4.509890511709968, - "heading": 1.0108464169686405e-7, - "angularVelocity": -5.408587989447552e-13, - "velocityX": -2.7719695012602092, - "velocityY": -0.46190026400605494, - "timestamp": 11.179960645704945 - }, - { - "x": 4.783077239990234, - "y": 4.404496192932129, - "heading": 4.371418464571693e-25, - "angularVelocity": -0.0000011726234931559648, - "velocityX": -2.5302907905881877, - "velocityY": -1.2226175229930707, - "timestamp": 11.266164480343127 - }, - { - "x": 4.627041500035927, - "y": 4.275874365802291, - "heading": -0.00461711894400582, - "angularVelocity": -0.06368383973540494, - "velocityX": -2.152198194752189, - "velocityY": -1.774078581199584, - "timestamp": 11.338665117289471 - }, - { - "x": 4.5068214168723975, - "y": 4.114451023957695, - "heading": -0.01240880783073842, - "angularVelocity": -0.10747062667185843, - "velocityX": -1.6581934756311134, - "velocityY": -2.226509292105373, - "timestamp": 11.411165754235816 - }, - { - "x": 4.429140519397286, - "y": 3.930818027867112, - "heading": -0.027772737926692166, - "angularVelocity": -0.21191441541850167, - "velocityX": -1.0714512416297768, - "velocityY": -2.53284665935394, - "timestamp": 11.48366639118216 - }, - { - "x": 4.383091867034577, - "y": 3.7471675526312227, - "heading": -0.08613270246137088, - "angularVelocity": -0.8049579561331118, - "velocityX": -0.635148245618701, - "velocityY": -2.5330877488952703, - "timestamp": 11.556167028128504 - }, - { - "x": 4.346524489213487, - "y": 3.5742818254238546, - "heading": -0.20843940130739744, - "angularVelocity": -1.6869741287451163, - "velocityX": -0.5043731939645176, - "velocityY": -2.3846097701915894, - "timestamp": 11.628667665074849 - }, - { - "x": 4.289423143132127, - "y": 3.4062373324265396, - "heading": -0.32755846162196556, - "angularVelocity": -1.643007086995987, - "velocityX": -0.7875978541211613, - "velocityY": -2.317834712565055, - "timestamp": 11.701168302021193 - }, - { - "x": 4.203799594769533, - "y": 3.2403627849834162, - "heading": -0.40165980770192133, - "angularVelocity": -1.022078552700102, - "velocityX": -1.1810040845014969, - "velocityY": -2.28790469200817, - "timestamp": 11.773668938967537 - }, - { - "x": 4.079421660594405, - "y": 3.103563620218193, - "heading": -0.4716628621936354, - "angularVelocity": -0.9655508894841919, - "velocityX": -1.7155426409174284, - "velocityY": -1.8868684542242327, - "timestamp": 11.846169575913882 + "heading": -0.2617467444324387, + "angularVelocity": 0.39520560430321505, + "velocityX": 1.5906642171255918, + "velocityY": 1.0391279912764597, + "timestamp": 8.39985796837086 + }, + { + "x": 5.054848385978205, + "y": 4.053342311213538, + "heading": -0.23277931082663741, + "angularVelocity": 0.3952056123787909, + "velocityX": 1.7258956530000658, + "velocityY": 0.7945338618626627, + "timestamp": 8.473155089409811 + }, + { + "x": 5.188526339981156, + "y": 4.092391169041153, + "heading": -0.20381187629917086, + "angularVelocity": 0.39520562495316197, + "velocityX": 1.8237817817143471, + "velocityY": 0.5327474977750234, + "timestamp": 8.546452210448763 + }, + { + "x": 5.326486517103212, + "y": 4.111406906532098, + "heading": -0.17484444007241956, + "angularVelocity": 0.3952056481366752, + "velocityX": 1.8822045827521574, + "velocityY": 0.25943362060346054, + "timestamp": 8.619749331487714 + }, + { + "x": 5.465743715620603, + "y": 4.1099781194933795, + "heading": -0.1458769989760029, + "angularVelocity": 0.3952057145740096, + "velocityX": 1.8998999761994304, + "velocityY": -0.01949308538270602, + "timestamp": 8.693046452526666 + }, + { + "x": 5.604302908818607, + "y": 4.095979592520542, + "heading": -0.1165828656276628, + "angularVelocity": 0.3996628098499595, + "velocityX": 1.8903770193698237, + "velocityY": -0.19098331250144138, + "timestamp": 8.766343573565617 + }, + { + "x": 5.7428620917203625, + "y": 4.08198096363453, + "heading": -0.08691799128062003, + "angularVelocity": 0.4047208666119116, + "velocityX": 1.8903768788971027, + "velocityY": -0.19098470291312333, + "timestamp": 8.839640694604569 + }, + { + "x": 5.881421274622045, + "y": 4.067982334747774, + "heading": -0.057160358126426926, + "angularVelocity": 0.40598638435443013, + "velocityX": 1.8903768788960766, + "velocityY": -0.19098470292328013, + "timestamp": 8.91293781564352 + }, + { + "x": 6.019980457523705, + "y": 4.053983705860801, + "heading": -0.027589119251388173, + "angularVelocity": 0.4034433884425539, + "velocityX": 1.8903768788957764, + "velocityY": -0.19098470292625017, + "timestamp": 8.986234936682472 + }, + { + "x": 6.158539640425345, + "y": 4.039985076973634, + "heading": 0.0015129980186348265, + "angularVelocity": 0.39704311516624813, + "velocityX": 1.8903768788955113, + "velocityY": -0.19098470292887434, + "timestamp": 9.059532057721423 + }, + { + "x": 6.297098823326969, + "y": 4.025986448086315, + "heading": 0.029855200510632778, + "angularVelocity": 0.3866755213610124, + "velocityX": 1.8903768788953004, + "velocityY": -0.19098470293096353, + "timestamp": 9.132829178760375 + }, + { + "x": 6.435658006228585, + "y": 4.011987819198895, + "heading": 0.05713171335387273, + "angularVelocity": 0.3721362102168317, + "velocityX": 1.8903768788951598, + "velocityY": -0.19098470293235562, + "timestamp": 9.206126299799326 + }, + { + "x": 6.574217189130194, + "y": 3.9979891903114315, + "heading": 0.08301592008791062, + "angularVelocity": 0.35314083782748296, + "velocityX": 1.8903768788951019, + "velocityY": -0.19098470293292794, + "timestamp": 9.279423420838278 + }, + { + "x": 6.712776372031808, + "y": 3.983990561423991, + "heading": 0.10715451282177094, + "angularVelocity": 0.3293252503196199, + "velocityX": 1.8903768788951338, + "velocityY": -0.19098470293261177, + "timestamp": 9.352720541877229 + }, + { + "x": 6.851335554933429, + "y": 3.9699919325366397, + "heading": 0.12916191998569537, + "angularVelocity": 0.30024927107613325, + "velocityX": 1.8903768788952555, + "velocityY": -0.1909847029314087, + "timestamp": 9.42601766291618 + }, + { + "x": 6.989894737835075, + "y": 3.9559933036495325, + "heading": 0.14861220477274031, + "angularVelocity": 0.26536219310317194, + "velocityX": 1.8903768788955921, + "velocityY": -0.1909847029280752, + "timestamp": 9.499314783955132 + }, + { + "x": 7.128453922674329, + "y": 3.9419946939409853, + "heading": 0.165034903130564, + "angularVelocity": 0.2240565267099137, + "velocityX": 1.8903769053305712, + "velocityY": -0.19098444127304595, + "timestamp": 9.572611904994083 + }, + { + "x": 7.267333565421739, + "y": 3.9316480226780426, + "heading": 0.17810972743336856, + "angularVelocity": 0.17838114399958924, + "velocityX": 1.8947489448269792, + "velocityY": -0.1411606774765978, + "timestamp": 9.645909026033035 + }, + { + "x": 7.406228542327881, + "y": 3.941786766052246, + "heading": 0.1911844788186837, + "angularVelocity": 0.1783801491789407, + "velocityX": 1.8949581502980173, + "velocityY": 0.13832389636170525, + "timestamp": 9.719206147071986 + }, + { + "x": 7.565442985719265, + "y": 3.9581600818899694, + "heading": 0.2043161039154577, + "angularVelocity": 0.1783801287415228, + "velocityX": 2.162770616764113, + "velocityY": 0.22241528870453642, + "timestamp": 9.79282211085348 + }, + { + "x": 7.726607611629632, + "y": 3.9948081017418207, + "heading": 0.2081850028863188, + "angularVelocity": 0.052555162931029055, + "velocityX": 2.189261915917248, + "velocityY": 0.49782707403830934, + "timestamp": 9.866438074634974 + }, + { + "x": 7.867453146090771, + "y": 4.027711655278318, + "heading": 0.21132312519311341, + "angularVelocity": 0.04262828530112288, + "velocityX": 1.913247171214066, + "velocityY": 0.44696220556402627, + "timestamp": 9.940054038416468 + }, + { + "x": 7.988106192724927, + "y": 4.056232601254953, + "heading": 0.21391875024315468, + "angularVelocity": 0.03525899705321495, + "velocityX": 1.6389522114018413, + "velocityY": 0.38742881994034783, + "timestamp": 10.013670002197962 + }, + { + "x": 8.088612865120389, + "y": 4.0801619694506375, + "heading": 0.2160337271012685, + "angularVelocity": 0.02872986713033435, + "velocityX": 1.3652836590414905, + "velocityY": 0.3250567807101139, + "timestamp": 10.087285965979456 + }, + { + "x": 8.168996921740131, + "y": 4.099396001381588, + "heading": 0.2176988235230518, + "angularVelocity": 0.022618686712105623, + "velocityX": 1.0919378418835648, + "velocityY": 0.26127528518191084, + "timestamp": 10.16090192976095 + }, + { + "x": 8.229272838087912, + "y": 4.11387267730592, + "heading": 0.21893245113612445, + "angularVelocity": 0.016757610030540077, + "velocityX": 0.8187886601157901, + "velocityY": 0.19665131284977577, + "timestamp": 10.234517893542444 + }, + { + "x": 8.269450355435243, + "y": 4.123550749253992, + "heading": 0.21974686245033065, + "angularVelocity": 0.011062971567194144, + "velocityX": 0.5457718038791941, + "velocityY": 0.13146702767891838, + "timestamp": 10.308133857323938 + }, + { + "x": 8.289536476135254, + "y": 4.128400802612305, + "heading": 0.22015079613001912, + "angularVelocity": 0.005487039209150268, + "velocityX": 0.2728500676786763, + "velocityY": 0.06588317409942288, + "timestamp": 10.381749821105432 + }, + { + "x": 8.289536476135254, + "y": 4.128400802612305, + "heading": 0.22015079613001912, + "angularVelocity": 3.037718139065862e-28, + "velocityX": -8.872519474147407e-27, + "velocityY": 3.446506807522548e-26, + "timestamp": 10.455365784886926 + }, + { + "x": 8.269996806011259, + "y": 4.137231467736895, + "heading": 0.22023861459550317, + "angularVelocity": 0.0011710803997845823, + "velocityX": -0.2605662097867229, + "velocityY": 0.11775904745621692, + "timestamp": 10.530355055905561 + }, + { + "x": 8.230762965216833, + "y": 4.154542716289096, + "heading": 0.22035463158445837, + "angularVelocity": 0.0015471145055716861, + "velocityX": -0.5231927215918144, + "velocityY": 0.23084967112027163, + "timestamp": 10.605344326924197 + }, + { + "x": 8.171657212153168, + "y": 4.179909246382009, + "heading": 0.22042856436582195, + "angularVelocity": 0.0009859114558561591, + "velocityX": -0.7881894604493174, + "velocityY": 0.3382687916330005, + "timestamp": 10.680333597942832 + }, + { + "x": 8.092474127272867, + "y": 4.212804284514623, + "heading": 0.22037649675261314, + "angularVelocity": -0.0006943341694284194, + "velocityX": -1.0559255184734737, + "velocityY": 0.4386632605674932, + "timestamp": 10.755322868961468 + }, + { + "x": 7.992976017836279, + "y": 4.252560097776598, + "heading": 0.22009679360683257, + "angularVelocity": -0.003729908852042051, + "velocityX": -1.3268312664602737, + "velocityY": 0.5301533502318783, + "timestamp": 10.830312139980103 + }, + { + "x": 7.87288978503091, + "y": 4.298306284549337, + "heading": 0.21946436859290286, + "angularVelocity": -0.008433539962971137, + "velocityX": -1.6013788529231974, + "velocityY": 0.6100364245622665, + "timestamp": 10.905301410998739 + }, + { + "x": 7.731910356436966, + "y": 4.348869169420612, + "heading": 0.21832244216002894, + "angularVelocity": -0.015227864164596877, + "velocityX": -1.8799946536206542, + "velocityY": 0.6742682544374918, + "timestamp": 10.980290682017374 + }, + { + "x": 7.569726421911416, + "y": 4.402600574685328, + "heading": 0.21647017920240058, + "angularVelocity": -0.024700372899585307, + "velocityX": -2.162761850095162, + "velocityY": 0.7165212374362642, + "timestamp": 11.05527995303601 + }, + { + "x": 7.386118026863393, + "y": 4.4570773650237445, + "heading": 0.21364200538162792, + "angularVelocity": -0.0377143794352908, + "velocityX": -2.4484621940436835, + "velocityY": 0.7264611270174774, + "timestamp": 11.130269224054645 + }, + { + "x": 7.181283119008862, + "y": 4.5085907210335865, + "heading": 0.20946226202822887, + "angularVelocity": -0.055737884855026475, + "velocityX": -2.731522857498233, + "velocityY": 0.6869430161154807, + "timestamp": 11.20525849507328 + }, + { + "x": 6.956820012371809, + "y": 4.551523922612317, + "heading": 0.20331318020756176, + "angularVelocity": -0.08199948788859508, + "velocityX": -2.9932696182801153, + "velocityY": 0.5725245891250452, + "timestamp": 11.280247766091916 + }, + { + "x": 6.7176650220814205, + "y": 4.57889627508473, + "heading": 0.19410641329547768, + "angularVelocity": -0.12277445542571251, + "velocityX": -3.189189427257647, + "velocityY": 0.3650169164281033, + "timestamp": 11.355237037110552 + }, + { + "x": 6.472067123940792, + "y": 4.58587963769058, + "heading": 0.18079068250006503, + "angularVelocity": -0.1775684789908641, + "velocityX": -3.2751071560569964, + "velocityY": 0.09312482320456428, + "timestamp": 11.430226308129187 + }, + { + "x": 6.227490944705127, + "y": 4.571499129973418, + "heading": 0.16337266739917577, + "angularVelocity": -0.23227342877570947, + "velocityX": -3.261482288244776, + "velocityY": -0.1917675358330633, + "timestamp": 11.505215579147823 + }, + { + "x": 5.988790028800353, + "y": 4.536538809179358, + "heading": 0.14241529165637518, + "angularVelocity": -0.2794716558531782, + "velocityX": -3.1831342359023242, + "velocityY": -0.4662043025511265, + "timestamp": 11.580204850166458 + }, + { + "x": 5.758924851198093, + "y": 4.482073744796014, + "heading": 0.11847095232121185, + "angularVelocity": -0.31930353515789156, + "velocityX": -3.0653075363959554, + "velocityY": -0.7263047585808593, + "timestamp": 11.655194121185094 + }, + { + "x": 5.5397707564905145, + "y": 4.409059239841931, + "heading": 0.09196886032537212, + "angularVelocity": -0.35341178325702044, + "velocityX": -2.92247266483119, + "velocityY": -0.9736660186486978, + "timestamp": 11.73018339220373 + }, + { + "x": 5.33258476452675, + "y": 4.318274659916258, + "heading": 0.06323280289856088, + "angularVelocity": -0.3832022506215635, + "velocityX": -2.7628751306607398, + "velocityY": -1.21063425063984, + "timestamp": 11.805172663222365 + }, + { + "x": 5.138254335054246, + "y": 4.210347005162901, + "heading": 0.03251123556536211, + "angularVelocity": -0.4096795036927947, + "velocityX": -2.5914431068974344, + "velocityY": -1.4392412846170675, + "timestamp": 11.880161934241 + }, + { + "x": 4.957433700561523, + "y": 4.085783958435059, + "heading": -7.345594756701693e-29, + "angularVelocity": -0.4335451608441839, + "velocityX": -2.41128673524227, + "velocityY": -1.661078245405117, + "timestamp": 11.955151205259636 + }, + { + "x": 4.850763114224524, + "y": 4.00236715124268, + "heading": -0.02077718123853542, + "angularVelocity": -0.4471189141923733, + "velocityX": -2.2955200800195295, + "velocityY": -1.795105497182433, + "timestamp": 12.001620233026141 + }, + { + "x": 4.749732174637679, + "y": 3.9129578387828414, + "heading": -0.04223315292346125, + "angularVelocity": -0.4617262877273038, + "velocityX": -2.174156517637868, + "velocityY": -1.9240624725160098, + "timestamp": 12.048089260792647 + }, + { + "x": 4.654715925578021, + "y": 3.817934442549008, + "heading": -0.06443390851309351, + "angularVelocity": -0.47775382134493777, + "velocityX": -2.0447221219494662, + "velocityY": -2.0448759270647874, + "timestamp": 12.094558288559153 + }, + { + "x": 4.566286302759079, + "y": 3.717984210531405, + "heading": -0.08747291743607646, + "angularVelocity": -0.4957927899578095, + "velocityX": -1.9029798355858998, + "velocityY": -2.1509000041882853, + "timestamp": 12.141027316325658 + }, + { + "x": 4.485327184216237, + "y": 3.614602306633809, + "heading": -0.11148870772971044, + "angularVelocity": -0.5168128417557317, + "velocityX": -1.7422167502543655, + "velocityY": -2.2247485877488473, + "timestamp": 12.187496344092164 + }, + { + "x": 4.412567946745739, + "y": 3.511651220217229, + "heading": -0.13677526525666567, + "angularVelocity": -0.5441593840528177, + "velocityX": -1.5657576878107506, + "velocityY": -2.21547752050852, + "timestamp": 12.23396537185867 + }, + { + "x": 4.345051921407887, + "y": 3.4150129773556204, + "heading": -0.1637765662543946, + "angularVelocity": -0.581060166212278, + "velocityX": -1.4529252834189115, + "velocityY": -2.079626958136264, + "timestamp": 12.280434399625175 + }, + { + "x": 4.278849963154741, + "y": 3.3264929134982726, + "heading": -0.19187383355887797, + "angularVelocity": -0.6046450432676215, + "velocityX": -1.4246469408783942, + "velocityY": -1.9049261005015221, + "timestamp": 12.32690342739168 + }, + { + "x": 4.21205059305352, + "y": 3.246181936974994, + "heading": -0.22056728070035955, + "angularVelocity": -0.6174746604482066, + "velocityX": -1.437503070580043, + "velocityY": -1.728268921975704, + "timestamp": 12.373372455158187 + }, + { + "x": 4.143696181766486, + "y": 3.1739556218387466, + "heading": -0.24959647824597708, + "angularVelocity": -0.6246999117666328, + "velocityX": -1.4709671058860057, + "velocityY": -1.5542893537425588, + "timestamp": 12.419841482924692 + }, + { + "x": 4.073234187126779, + "y": 3.109688534157438, + "heading": -0.27881410074968266, + "angularVelocity": -0.6287547622152977, + "velocityX": -1.5163216883675306, + "velocityY": -1.3830090873480942, + "timestamp": 12.466310510691198 + }, + { + "x": 4.000309863583725, + "y": 3.0532784271020903, + "heading": -0.30812855213660195, + "angularVelocity": -0.6308384916985205, + "velocityX": -1.5693102922118025, + "velocityY": -1.2139291430583163, + "timestamp": 12.512779538457703 }, { "x": 3.924677610397339, "y": 3.0046441555023193, - "heading": -0.5385444180262977, - "angularVelocity": -0.9224961138225419, - "velocityX": -2.134381940831591, - "velocityY": -1.3643944230321734, - "timestamp": 11.918670212860226 - }, - { - "x": 3.7636444729485263, - "y": 2.9505820220343484, - "heading": -0.5968694156691691, - "angularVelocity": -0.8693980375808563, - "velocityX": -2.40037547092122, - "velocityY": -0.8058553732360577, - "timestamp": 11.98575685801204 - }, - { - "x": 3.5921162225925003, - "y": 2.93585850684271, - "heading": -0.6464965554196345, - "angularVelocity": -0.7397469293353471, - "velocityX": -2.556816635678569, - "velocityY": -0.21947013684048228, - "timestamp": 12.052843503163855 - }, - { - "x": 3.416479572667743, - "y": 2.9596548551854616, - "heading": -0.6807775343446694, - "angularVelocity": -0.5109955766525223, - "velocityX": -2.6180568357129337, - "velocityY": 0.3547106624410992, - "timestamp": 12.11993014831567 - }, - { - "x": 3.2390028785000764, - "y": 3.015476958858974, - "heading": -0.6885344713310354, - "angularVelocity": -0.115625650512295, - "velocityX": -2.645484712583867, - "velocityY": 0.8320896587866112, - "timestamp": 12.187016793467484 - }, - { - "x": 3.0657580475568027, - "y": 3.089829788896707, - "heading": -0.6885346535011025, - "angularVelocity": -0.00000271544458244769, - "velocityX": -2.5824041513959415, - "velocityY": 1.1083104523929412, - "timestamp": 12.254103438619298 - }, - { - "x": 2.8925132745209385, - "y": 3.1641827538610285, - "heading": -0.6885348356707467, - "angularVelocity": -0.000002715438277887047, - "velocityX": -2.582403288222506, - "velocityY": 1.1083124636216772, - "timestamp": 12.321190083771112 - }, - { - "x": 2.7192685014854168, - "y": 3.2385357188261574, - "heading": -0.6885350178403685, - "angularVelocity": -0.000002715437944094969, - "velocityX": -2.5824032882174053, - "velocityY": 1.108312463633708, - "timestamp": 12.388276728922927 - }, - { - "x": 2.5460237284499403, - "y": 3.312888683791401, - "heading": -0.6885352000099678, - "angularVelocity": -0.0000027154376105306044, - "velocityX": -2.5824032882167316, - "velocityY": 1.1083124636354227, - "timestamp": 12.455363374074741 - }, - { - "x": 2.3727789637362293, - "y": 3.3872416681385586, - "heading": -0.6885353821903477, - "angularVelocity": -0.0000027155983032171972, - "velocityX": -2.5824031641716974, - "velocityY": 1.108312752544088, - "timestamp": 12.522450019226556 - }, - { - "x": 2.2223047239417384, - "y": 3.4507934701253355, - "heading": -0.7191097564620745, - "angularVelocity": -0.45574457036177557, - "velocityX": -2.2429835245744076, - "velocityY": 0.9473092870117661, - "timestamp": 12.58953666437837 - }, - { - "x": 2.109444085282166, - "y": 3.4984470664225062, - "heading": -0.7420547518086502, - "angularVelocity": -0.34202031260695603, - "velocityX": -1.6823115599859222, - "velocityY": 0.7103291003646417, - "timestamp": 12.656623309530184 - }, - { - "x": 2.0342014899368133, - "y": 3.5302130152316624, - "heading": -0.7573535418351998, - "angularVelocity": -0.22804523898800838, - "velocityX": -1.1215733798445486, - "velocityY": 0.47350629528829474, - "timestamp": 12.723709954681999 - }, - { - "x": 1.996579647064209, - "y": 3.5460963249206543, - "heading": -0.765, - "angularVelocity": -0.11397884254752216, - "velocityX": -0.5607948167249468, - "velocityY": 0.23675814542593593, - "timestamp": 12.790796599833813 - }, - { - "x": 1.996579647064209, - "y": 3.5460963249206543, - "heading": -0.765, - "angularVelocity": 6.11903975445096e-25, - "velocityX": -4.7665538572669345e-25, - "velocityY": 2.8118183260637723e-25, - "timestamp": 12.857883244985628 + "heading": -0.3374787219433775, + "angularVelocity": -0.6316071417343185, + "velocityX": -1.627584152748309, + "velocityY": -1.0465954193004559, + "timestamp": 12.559248566224209 + }, + { + "x": 3.8052119909980813, + "y": 2.949187806356286, + "heading": -0.3813210096823129, + "angularVelocity": -0.6313779373848833, + "velocityX": -1.720438422690351, + "velocityY": -0.798633400410426, + "timestamp": 12.628687621887964 + }, + { + "x": 3.6796645340407665, + "y": 2.911082197769309, + "heading": -0.4250077410083283, + "angularVelocity": -0.6291377512038759, + "velocityX": -1.808023680005878, + "velocityY": -0.5487633468331752, + "timestamp": 12.69812667755172 + }, + { + "x": 3.548510692212512, + "y": 2.8904863098039106, + "heading": -0.4683618068061113, + "angularVelocity": -0.6243469958421788, + "velocityX": -1.8887618873065017, + "velocityY": -0.29660380269469544, + "timestamp": 12.767565733215475 + }, + { + "x": 3.4123915084504293, + "y": 2.887591555300515, + "heading": -0.5111475650763733, + "angularVelocity": -0.6161627323597821, + "velocityX": -1.960268359944496, + "velocityY": -0.04168769974944769, + "timestamp": 12.83700478887923 + }, + { + "x": 3.2722134837537085, + "y": 2.9026251488079913, + "heading": -0.5530315041603286, + "angularVelocity": -0.6031755282901653, + "velocityX": -2.0187202051754043, + "velocityY": 0.21650054661275753, + "timestamp": 12.906443844542986 + }, + { + "x": 3.129341493242039, + "y": 2.9358368291248627, + "heading": -0.5935016635404317, + "angularVelocity": -0.5828155206498206, + "velocityX": -2.0575163234289917, + "velocityY": 0.4782853107578603, + "timestamp": 12.975882900206742 + }, + { + "x": 2.9860054741751014, + "y": 2.987405262527824, + "heading": -0.6316842461722715, + "angularVelocity": -0.5498718590980219, + "velocityX": -2.0641988531787505, + "velocityY": 0.7426430689477005, + "timestamp": 13.045321955870497 + }, + { + "x": 2.846192204303753, + "y": 3.0569532479597985, + "heading": -0.6659032700236993, + "angularVelocity": -0.492792183366187, + "velocityX": -2.0134673280749626, + "velocityY": 1.0015687103918496, + "timestamp": 13.114761011534252 + }, + { + "x": 2.716907495900805, + "y": 3.1413547514524796, + "heading": -0.6931863548996687, + "angularVelocity": -0.39290691117809146, + "velocityX": -1.8618442772174881, + "velocityY": 1.2154759693360322, + "timestamp": 13.184200067198008 + }, + { + "x": 2.6049527292797388, + "y": 3.231193791663575, + "heading": -0.7132381462616506, + "angularVelocity": -0.288768203575217, + "velocityX": -1.6122737492742676, + "velocityY": 1.293782574551762, + "timestamp": 13.253639122861763 + }, + { + "x": 2.510838263874455, + "y": 3.3171226961552893, + "heading": -0.728589225015942, + "angularVelocity": -0.2210726889580136, + "velocityX": -1.3553534751540288, + "velocityY": 1.2374722505992761, + "timestamp": 13.323078178525519 + }, + { + "x": 2.4328922690741286, + "y": 3.3944440167178684, + "heading": -0.7405972040878053, + "angularVelocity": -0.17292831760284247, + "velocityX": -1.1225094301075331, + "velocityY": 1.1135134230078372, + "timestamp": 13.392517234189274 + }, + { + "x": 2.3697038520475897, + "y": 3.4608749704927635, + "heading": -0.7499497576912365, + "angularVelocity": -0.13468722340809483, + "velocityX": -0.9099838185086581, + "velocityY": 0.9566799712336846, + "timestamp": 13.46195628985303 + }, + { + "x": 2.3202711712076995, + "y": 3.5151563459749755, + "heading": -0.757044699270628, + "angularVelocity": -0.10217508737082179, + "velocityX": -0.711885845326848, + "velocityY": 0.781712466613299, + "timestamp": 13.531395345516785 + }, + { + "x": 2.2838770908763535, + "y": 3.556511565024103, + "heading": -0.7621393736392112, + "angularVelocity": -0.07336900422801312, + "velocityX": -0.5241154273119383, + "velocityY": 0.5955613689417452, + "timestamp": 13.60083440118054 + }, + { + "x": 2.2599910820649347, + "y": 3.584420486805288, + "heading": -0.7654136211145312, + "angularVelocity": -0.04715282263017491, + "velocityX": -0.34398521960151957, + "velocityY": 0.40191966198861, + "timestamp": 13.670273456844296 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -0.022845628735947136, + "velocityX": -0.16969575449988578, + "velocityY": 0.20294632487031472, + "timestamp": 13.739712512508051 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -4.7049839413389466e-29, + "velocityX": -4.475670243258915e-27, + "velocityY": -3.1749766992652438e-27, + "timestamp": 13.809151568171806 } ], "trajectoryWaypoints": [ @@ -2882,65 +3735,65 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 30 + "controlIntervalCount": 33 }, { - "timestamp": 2.2993315302702704, + "timestamp": 2.253639337464695, "isStopPoint": false, - "x": 5.707507610321045, - "y": 1.6179983615875244, + "x": 5.813786029815674, + "y": 1.5033589601516724, "heading": 0.16076270850197952, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 14 }, { - "timestamp": 2.9817709108871657, + "timestamp": 2.781016308485608, "isStopPoint": false, - "x": 7.384688854217529, - "y": 2.3971612453460693, - "heading": 0, + "x": 7.356464862823486, + "y": 2.2000527381896973, + "heading": 0.22471085620066228, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 10 }, { - "timestamp": 3.4765553646952188, + "timestamp": 3.5163720671787804, "isStopPoint": true, "x": 8.243088722229004, "y": 2.410367250442505, - "heading": 0, + "heading": 0.2561010030603239, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 19 }, { - "timestamp": 4.56958576845638, + "timestamp": 4.7138256569630785, "isStopPoint": false, - "x": 5.667889595031738, - "y": 1.5915861129760742, + "x": 5.826227188110352, + "y": 1.4660362005233765, "heading": -0.04647804366485621, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 22 + "controlIntervalCount": 25 }, { - "timestamp": 6.302617915408521, + "timestamp": 6.452485437306611, "isStopPoint": true, - "x": 1.9701671600341797, - "y": 3.5328900814056396, - "heading": -0.765, + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 18 }, { - "timestamp": 7.4473821814690595, + "timestamp": 7.643234228743685, "isStopPoint": false, "x": 3.9378836154937744, "y": 3.0046441555023193, @@ -2948,10 +3801,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 9 + "controlIntervalCount": 13 }, { - "timestamp": 8.189397653756762, + "timestamp": 8.39985796837086, "isStopPoint": false, "x": 4.928345203399658, "y": 3.995105266571045, @@ -2959,43 +3812,43 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 10 + "controlIntervalCount": 18 }, { - "timestamp": 9.106155894326575, + "timestamp": 9.719206147071986, "isStopPoint": false, - "x": 6.658350467681885, - "y": 4.179991245269775, - "heading": 0, + "x": 7.406228542327881, + "y": 3.941786766052246, + "heading": 0.1911844788186837, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 10 }, { - "timestamp": 9.886903126132216, + "timestamp": 10.455365784886926, "isStopPoint": true, - "x": 8.282707214355469, - "y": 4.08754825592041, - "heading": 0, + "x": 8.289536476135254, + "y": 4.128400802612305, + "heading": 0.22015079613001914, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 20 }, { - "timestamp": 11.266164480343127, + "timestamp": 11.955151205259636, "isStopPoint": false, - "x": 4.783077239990234, - "y": 4.404496192932129, + "x": 4.957433700561523, + "y": 4.085783958435059, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 13 }, { - "timestamp": 11.918670212860226, + "timestamp": 12.559248566224209, "isStopPoint": false, "x": 3.924677610397339, "y": 3.0046441555023193, @@ -3003,14 +3856,14 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 14 + "controlIntervalCount": 18 }, { - "timestamp": 12.857883244985628, + "timestamp": 13.809151568171806, "isStopPoint": true, - "x": 1.996579647064209, - "y": 3.5460963249206543, - "heading": -0.765, + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -3063,6 +3916,3572 @@ "circleObstacles": [], "eventMarkers": [], "isTrajectoryStale": false + }, + "6Piece": { + "waypoints": [ + { + "x": 1.3288218975067139, + "y": 5.58574914932251, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 2.672445297241211, + "y": 4.279448986053467, + "heading": -0.2516907201225374, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 1.8513420820236206, + "y": 4.9637017250061035, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 9 + }, + { + "x": 2.8590595722198486, + "y": 5.58574914932251, + "heading": 0.38687655192878945, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 1.838901162147522, + "y": 6.319766044616699, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 10 + }, + { + "x": 2.9088234901428223, + "y": 7.028900623321533, + "heading": 0.426627843154362, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 5.309927463531494, + "y": 7.402129173278809, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 8.258434295654297, + "y": 7.451892852783203, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 4.33372688293457, + "y": 6.3272294998168945, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 8.290366172790527, + "y": 5.761995315551758, + "heading": -0.3883190645755742, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 4.353217601776123, + "y": 6.307738780975342, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.328821897506714, + "y": 5.58574914932251, + "heading": 4.3539389552577884e-26, + "angularVelocity": 5.8461926746164104e-27, + "velocityX": -1.1704555712816255e-26, + "velocityY": -1.2477760568468226e-26, + "timestamp": 0 + }, + { + "x": 1.374949606535372, + "y": 5.540487462134801, + "heading": -0.013194729598639283, + "angularVelocity": -0.15905904586136208, + "velocityX": 0.5560575782186153, + "velocityY": -0.5456179093578979, + "timestamp": 0.08295491480654284 + }, + { + "x": 1.46721327852146, + "y": 5.449968559727431, + "heading": -0.03947645751575262, + "angularVelocity": -0.316819419059188, + "velocityX": 1.1122146554095553, + "velocityY": -1.0911819102999225, + "timestamp": 0.16590982961308567 + }, + { + "x": 1.605638421023178, + "y": 5.314200511590273, + "heading": -0.078646870972475, + "angularVelocity": -0.4721891831010949, + "velocityX": 1.6686792196042406, + "velocityY": -1.6366486356327254, + "timestamp": 0.24886474441962853 + }, + { + "x": 1.7902678160557024, + "y": 5.133199369169571, + "heading": -0.13043925037876178, + "angularVelocity": -0.6243437116061242, + "velocityX": 2.225659509904795, + "velocityY": -2.181921864940855, + "timestamp": 0.33181965922617135 + }, + { + "x": 1.9998217979277588, + "y": 4.932195127760977, + "heading": -0.13043932359342048, + "angularVelocity": -8.825837368571486e-7, + "velocityX": 2.5261189449805634, + "velocityY": -2.423054039382124, + "timestamp": 0.41477457403271417 + }, + { + "x": 2.2093757892592514, + "y": 4.731190896214115, + "heading": -0.13043939680794175, + "angularVelocity": -8.825820799374822e-7, + "velocityX": 2.5261190590116103, + "velocityY": -2.4230539205014976, + "timestamp": 0.497729488839257 + }, + { + "x": 2.3945844486540637, + "y": 4.550478243745755, + "heading": -0.17901854361361771, + "angularVelocity": -0.5856090253237706, + "velocityX": 2.232642391674238, + "velocityY": -2.1784441933283367, + "timestamp": 0.5806844036457999 + }, + { + "x": 2.533512254873414, + "y": 4.414958500153421, + "heading": -0.21536260429634638, + "angularVelocity": -0.4381182328676455, + "velocityX": 1.6747387004536223, + "velocityY": -1.6336553886936764, + "timestamp": 0.6636393184523427 + }, + { + "x": 2.6261356849469077, + "y": 4.324618150485375, + "heading": -0.23957423083578933, + "angularVelocity": -0.2918648834238011, + "velocityX": 1.1165514459209405, + "velocityY": -1.0890295032998, + "timestamp": 0.7465942332588855 + }, + { + "x": 2.672445297241211, + "y": 4.279448986053467, + "heading": -0.2516907201225374, + "angularVelocity": -0.14606113833043707, + "velocityX": 0.5582503749452395, + "velocityY": -0.5445025715142554, + "timestamp": 0.8295491480654283 + }, + { + "x": 2.672445297241211, + "y": 4.279448986053467, + "heading": -0.2516907201225374, + "angularVelocity": 1.3508666315673642e-26, + "velocityX": -7.696322556768917e-27, + "velocityY": -1.1161944739972978e-26, + "timestamp": 0.9125040628719712 + }, + { + "x": 2.625196176417341, + "y": 4.293300959249136, + "heading": -0.2476240648671981, + "angularVelocity": 0.05660704046380333, + "velocityX": -0.6576984589102508, + "velocityY": 0.19281673954566436, + "timestamp": 0.9843441594751433 + }, + { + "x": 2.5313898742849363, + "y": 4.322902155034929, + "heading": -0.238711790545806, + "angularVelocity": 0.12405710380125745, + "velocityX": -1.3057652560041622, + "velocityY": 0.4120428171095463, + "timestamp": 1.0561842560783155 + }, + { + "x": 2.3926976278493437, + "y": 4.3719573505978495, + "heading": -0.22331799219109705, + "angularVelocity": 0.21427864218697643, + "velocityX": -1.9305687630362727, + "velocityY": 0.6828386636767781, + "timestamp": 1.1280243526814877 + }, + { + "x": 2.215900012214516, + "y": 4.450150810574297, + "heading": -0.19663428050241719, + "angularVelocity": 0.3714320129060295, + "velocityX": -2.460987999660081, + "velocityY": 1.0884375672317064, + "timestamp": 1.19986444928466 + }, + { + "x": 2.052047257943267, + "y": 4.568466965487968, + "heading": -0.1386728664270459, + "angularVelocity": 0.8068114718099074, + "velocityX": -2.2807980782143584, + "velocityY": 1.6469375809336866, + "timestamp": 1.2717045458878322 + }, + { + "x": 1.935784479955621, + "y": 4.6986149037226514, + "heading": -0.07489980420839401, + "angularVelocity": 0.8877084696993005, + "velocityX": -1.6183549784162223, + "velocityY": 1.811633675182685, + "timestamp": 1.3435446424910045 + }, + { + "x": 1.8688424514197695, + "y": 4.831515653648781, + "heading": -0.009029725121146852, + "angularVelocity": 0.9168985316250061, + "velocityX": -0.9318198568916667, + "velocityY": 1.8499522719219348, + "timestamp": 1.4153847390941767 + }, + { + "x": 1.8513420820236208, + "y": 4.9637017250061035, + "heading": 0.057668349012968446, + "angularVelocity": 0.928424059652088, + "velocityX": -0.2436016963175404, + "velocityY": 1.84000408695839, + "timestamp": 1.487224835697349 + }, + { + "x": 1.8881504770913347, + "y": 5.100791187409367, + "heading": 0.12845802675955434, + "angularVelocity": 0.9309881682233778, + "velocityX": 0.48408442290141646, + "velocityY": 1.802927652000758, + "timestamp": 1.5632619757629151 + }, + { + "x": 1.9801594635213668, + "y": 5.23317628018901, + "heading": 0.19875878141335546, + "angularVelocity": 0.9245581119066463, + "velocityX": 1.2100532233418253, + "velocityY": 1.7410582863254496, + "timestamp": 1.6392991158284813 + }, + { + "x": 2.1268970903680344, + "y": 5.357201962866711, + "heading": 0.26712068314763787, + "angularVelocity": 0.8990593501456591, + "velocityX": 1.9298151761117934, + "velocityY": 1.6311197734522118, + "timestamp": 1.7153362558940475 + }, + { + "x": 2.3255858491677373, + "y": 5.4631354448246725, + "heading": 0.32902091884053974, + "angularVelocity": 0.8140789571980984, + "velocityX": 2.6130488157284057, + "velocityY": 1.393180778059456, + "timestamp": 1.7913733959596136 + }, + { + "x": 2.5347759563760905, + "y": 5.522688608468802, + "heading": 0.3564296694092298, + "angularVelocity": 0.3604653008392443, + "velocityX": 2.7511569612951026, + "velocityY": 0.7832115146989626, + "timestamp": 1.8674105360251798 + }, + { + "x": 2.696139551385315, + "y": 5.557119984012087, + "heading": 0.37271335067397476, + "angularVelocity": 0.21415430999513735, + "velocityX": 2.122168125603903, + "velocityY": 0.4528231271401756, + "timestamp": 1.943447676090746 + }, + { + "x": 2.8046075058969566, + "y": 5.576857728360174, + "heading": 0.38238828101753003, + "angularVelocity": 0.12723953498530663, + "velocityX": 1.4265128122666186, + "velocityY": 0.25958030945230076, + "timestamp": 2.019484816156312 + }, + { + "x": 2.8590595722198486, + "y": 5.58574914932251, + "heading": 0.38687655192878945, + "angularVelocity": 0.05902735041572054, + "velocityX": 0.7161245974787916, + "velocityY": 0.11693523657870776, + "timestamp": 2.0955219562218783 + }, + { + "x": 2.8590595722198486, + "y": 5.58574914932251, + "heading": 0.38687655192878945, + "angularVelocity": -4.595788384330154e-29, + "velocityX": 7.170077328456974e-29, + "velocityY": 4.9006221400778145e-30, + "timestamp": 2.1715590962874445 + }, + { + "x": 2.821439157373947, + "y": 5.594030750372382, + "heading": 0.387080960332372, + "angularVelocity": 0.0032239487618478326, + "velocityX": -0.5933527572101648, + "velocityY": 0.1306181985813967, + "timestamp": 2.2349622142282555 + }, + { + "x": 2.7464223945042074, + "y": 5.611552554364111, + "heading": 0.38751784243776727, + "angularVelocity": 0.006890546073824077, + "velocityX": -1.1831715112144825, + "velocityY": 0.27635555727852257, + "timestamp": 2.2983653321690665 + }, + { + "x": 2.6344378134858273, + "y": 5.639901380380037, + "heading": 0.38823547094203587, + "angularVelocity": 0.011318504950159194, + "velocityX": -1.7662314513131911, + "velocityY": 0.44712037730369514, + "timestamp": 2.3617684501098775 + }, + { + "x": 2.48653995660996, + "y": 5.682179259469134, + "heading": 0.3893349048666279, + "angularVelocity": 0.01734037631427531, + "velocityX": -2.332659050205305, + "velocityY": 0.6668107257527173, + "timestamp": 2.4251715680506885 + }, + { + "x": 2.307136094114684, + "y": 5.746612910577359, + "heading": 0.39112800963911476, + "angularVelocity": 0.02828101883192509, + "velocityX": -2.829574764174154, + "velocityY": 1.0162536670259112, + "timestamp": 2.4885746859914994 + }, + { + "x": 2.1381687976868347, + "y": 5.848112044286255, + "heading": 0.3940456425874033, + "angularVelocity": 0.046017184060447705, + "velocityX": -2.6649682525958127, + "velocityY": 1.600853980140992, + "timestamp": 2.5519778039323104 + }, + { + "x": 2.0057971851389707, + "y": 5.961635826733289, + "heading": 0.39722224472387, + "angularVelocity": 0.05010167070067704, + "velocityX": -2.087777649538265, + "velocityY": 1.7905078824832, + "timestamp": 2.6153809218731214 + }, + { + "x": 1.9116699025918782, + "y": 6.079772759952102, + "heading": 0.40050830041797525, + "angularVelocity": 0.05182798261077437, + "velocityX": -1.4845844432282262, + "velocityY": 1.8632669347444144, + "timestamp": 2.6787840398139324 + }, + { + "x": 1.8560250338172737, + "y": 6.199649277805345, + "heading": 0.40384210990951336, + "angularVelocity": 0.052581160040904545, + "velocityX": -0.8776361570506794, + "velocityY": 1.8907038288740154, + "timestamp": 2.7421871577547434 + }, + { + "x": 1.838901162147522, + "y": 6.319766044616699, + "heading": 0.4071898798527544, + "angularVelocity": 0.052801345611525094, + "velocityX": -0.27007933088933367, + "velocityY": 1.8944930582670674, + "timestamp": 2.8055902756955544 + }, + { + "x": 1.8674670617000426, + "y": 6.452466802830068, + "heading": 0.410902560889689, + "angularVelocity": 0.05267198382132873, + "velocityX": 0.4052657861269917, + "velocityY": 1.8826320171752862, + "timestamp": 2.8760771031810837 + }, + { + "x": 1.9435937805094023, + "y": 6.582998572823563, + "heading": 0.4145700789652226, + "angularVelocity": 0.052031254723253276, + "velocityX": 1.080013408533504, + "velocityY": 1.8518604773394354, + "timestamp": 2.946563930666613 + }, + { + "x": 2.067117864237862, + "y": 6.709034317074673, + "heading": 0.41812673034240355, + "angularVelocity": 0.05045838355983837, + "velocityX": 1.7524420964160776, + "velocityY": 1.7880751446358603, + "timestamp": 3.017050758152142 + }, + { + "x": 2.2372903513460827, + "y": 6.825552193890142, + "heading": 0.4214177842629497, + "angularVelocity": 0.04669033971236367, + "velocityX": 2.4142452310420213, + "velocityY": 1.6530447031310942, + "timestamp": 3.0875375856376714 + }, + { + "x": 2.447332984322481, + "y": 6.9160627740645415, + "heading": 0.423792969899213, + "angularVelocity": 0.03369687246529782, + "velocityX": 2.9798849014664373, + "velocityY": 1.2840779391437487, + "timestamp": 3.1580244131232007 + }, + { + "x": 2.6299484111085176, + "y": 6.9676741214205835, + "heading": 0.4250728750513537, + "angularVelocity": 0.018158075739803856, + "velocityX": 2.590773812646438, + "velocityY": 0.7322126586933964, + "timestamp": 3.22851124060873 + }, + { + "x": 2.768901501023009, + "y": 7.000310165629845, + "heading": 0.42589586451986794, + "angularVelocity": 0.01167579103603713, + "velocityX": 1.9713341466959602, + "velocityY": 0.46300912345588136, + "timestamp": 3.298998068094259 + }, + { + "x": 2.862081475468267, + "y": 7.0198533344035186, + "heading": 0.4263947957186992, + "angularVelocity": 0.007078360831798, + "velocityX": 1.3219487636095895, + "velocityY": 0.2772598721042683, + "timestamp": 3.3694848955797885 + }, + { + "x": 2.9088234901428223, + "y": 7.028900623321533, + "heading": 0.426627843154362, + "angularVelocity": 0.003306255139807895, + "velocityX": 0.6631312025520166, + "velocityY": 0.12835432152017248, + "timestamp": 3.4399717230653177 + }, + { + "x": 2.9088234901428223, + "y": 7.028900623321533, + "heading": 0.426627843154362, + "angularVelocity": 7.235947497328528e-24, + "velocityX": 3.9627330150979443e-25, + "velocityY": 3.746183212022811e-24, + "timestamp": 3.510458550550847 + }, + { + "x": 2.9499714553274323, + "y": 7.035529268833997, + "heading": 0.4162396153486674, + "angularVelocity": -0.15476273818242897, + "velocityX": 0.613018108739834, + "velocityY": 0.09875287191789282, + "timestamp": 3.577582122592131 + }, + { + "x": 3.032252734431755, + "y": 7.0488035270821205, + "heading": 0.3954328742907609, + "angularVelocity": -0.3099766657994496, + "velocityX": 1.2258179444579598, + "velocityY": 0.19775851976348513, + "timestamp": 3.6447056946334153 + }, + { + "x": 3.155648996542602, + "y": 7.068747158199284, + "heading": 0.3641717265377585, + "angularVelocity": -0.4657253301980919, + "velocityX": 1.8383446881365682, + "velocityY": 0.29711814360681277, + "timestamp": 3.7118292666746995 + }, + { + "x": 3.3201397118502305, + "y": 7.09538869210212, + "heading": 0.3224161177499544, + "angularVelocity": -0.6220707199867489, + "velocityX": 2.4505655808433304, + "velocityY": 0.3969028031829179, + "timestamp": 3.7789528387159836 + }, + { + "x": 3.525704706881839, + "y": 7.128759620860251, + "heading": 0.2701269686194379, + "angularVelocity": -0.7789983092430813, + "velocityX": 3.062485931248957, + "velocityY": 0.4971566283392252, + "timestamp": 3.846076410757268 + }, + { + "x": 3.736742521528462, + "y": 7.17085251865357, + "heading": 0.1968734668457863, + "angularVelocity": -1.0913230560584186, + "velocityX": 3.1440194290736665, + "velocityY": 0.6270956165358649, + "timestamp": 3.913199982798552 + }, + { + "x": 3.9603913131238833, + "y": 7.208349824687908, + "heading": 0.16611663819424638, + "angularVelocity": -0.45821203663927534, + "velocityX": 3.331896452975815, + "velocityY": 0.5586309681385255, + "timestamp": 3.980323554839836 + }, + { + "x": 4.186083835227695, + "y": 7.240507088585249, + "heading": 0.139868186298227, + "angularVelocity": -0.39104670829906596, + "velocityX": 3.3623437376813072, + "velocityY": 0.47907557538152645, + "timestamp": 4.04744712688112 + }, + { + "x": 4.411495404478292, + "y": 7.272714656791408, + "heading": 0.11312371036793661, + "angularVelocity": -0.398436422808984, + "velocityX": 3.3581581312740267, + "velocityY": 0.47982500374605697, + "timestamp": 4.114570698922404 + }, + { + "x": 4.636607862344283, + "y": 7.304977234634002, + "heading": 0.08583019595793309, + "angularVelocity": -0.40661594101721416, + "velocityX": 3.353702000953352, + "velocityY": 0.4806445315924413, + "timestamp": 4.181694270963688 + }, + { + "x": 4.861400835126187, + "y": 7.33729811854161, + "heading": 0.05792775603075696, + "angularVelocity": -0.4156876500853505, + "velocityX": 3.3489423453752654, + "velocityY": 0.48151316928916743, + "timestamp": 4.248817843004972 + }, + { + "x": 5.085850174369245, + "y": 7.369680784648805, + "heading": 0.02934541464474294, + "angularVelocity": -0.42581675135576136, + "velocityX": 3.343822928627991, + "velocityY": 0.48243359407748343, + "timestamp": 4.3159414150462565 + }, + { + "x": 5.309927463531494, + "y": 7.402129173278809, + "heading": 5.71560409253603e-24, + "angularVelocity": -0.4371849374567573, + "velocityX": 3.338280165191911, + "velocityY": 0.4834127213916103, + "timestamp": 4.383064987087541 + }, + { + "x": 5.5975844277501166, + "y": 7.40698430828562, + "heading": -5.977593030842461e-14, + "angularVelocity": -7.27279605332056e-13, + "velocityX": 3.49985424414606, + "velocityY": 0.05907127924278551, + "timestamp": 4.465256113960828 + }, + { + "x": 5.885241394648691, + "y": 7.411839284508277, + "heading": -6.292590842346059e-14, + "angularVelocity": -3.832503866135791e-14, + "velocityX": 3.4998542767523966, + "velocityY": 0.05906934735354094, + "timestamp": 4.547447240834116 + }, + { + "x": 6.1728983617447595, + "y": 7.416694249029442, + "heading": -6.528052611785862e-14, + "angularVelocity": -2.864807655005671e-14, + "velocityX": 3.499854279155253, + "velocityY": 0.05906920498425597, + "timestamp": 4.629638367707404 + }, + { + "x": 6.460555329029281, + "y": 7.421549202384699, + "heading": -6.686290101207671e-14, + "angularVelocity": -1.9252381058857556e-14, + "velocityX": 3.4998542814481235, + "velocityY": 0.059069069131291764, + "timestamp": 4.711829494580692 + }, + { + "x": 6.748212296487426, + "y": 7.426404145452759, + "heading": -6.776532099009745e-14, + "angularVelocity": -1.0979530423221106e-14, + "velocityX": 3.4998542835605484, + "velocityY": 0.05906894396940334, + "timestamp": 4.79402062145398 + }, + { + "x": 7.035869264100995, + "y": 7.431259079311885, + "heading": -6.813646923115834e-14, + "angularVelocity": -4.515672861335136e-15, + "velocityX": 3.499854285451553, + "velocityY": 0.05906883192648223, + "timestamp": 4.876211748327267 + }, + { + "x": 7.323526231850984, + "y": 7.436114005088022, + "heading": -6.829397797228803e-14, + "angularVelocity": -1.916371597823108e-15, + "velocityX": 3.4998542871113476, + "velocityY": 0.05906873358265542, + "timestamp": 4.958402875200555 + }, + { + "x": 7.611183199719373, + "y": 7.440968923848655, + "heading": -6.831359071366292e-14, + "angularVelocity": -2.386235853473579e-16, + "velocityX": 3.4998542885518997, + "velocityY": 0.059068648226685214, + "timestamp": 5.040594002073843 + }, + { + "x": 7.870083631568097, + "y": 7.445338496439803, + "heading": 1.5159787003061443e-11, + "angularVelocity": 1.852766980218185e-10, + "velocityX": 3.1499803165841067, + "velocityY": 0.05316355618148571, + "timestamp": 5.122785128947131 + }, + { + "x": 8.064258961919318, + "y": 7.448615675130285, + "heading": 1.666159387153368e-11, + "angularVelocity": 1.827212894632211e-11, + "velocityX": 2.3624853160923003, + "velocityY": 0.03987265797601595, + "timestamp": 5.2049762558204185 + }, + { + "x": 8.193709184032379, + "y": 7.450800460403948, + "heading": 8.958511488927219e-12, + "angularVelocity": -9.372158133909199e-11, + "velocityX": 1.5749902335884913, + "velocityY": 0.026581765657390878, + "timestamp": 5.287167382693706 + }, + { + "x": 8.258434295654297, + "y": 7.451892852783203, + "heading": -7.387785600293283e-27, + "angularVelocity": -1.0899609032904939e-10, + "velocityX": 0.7874951236731913, + "velocityY": 0.013290879694828065, + "timestamp": 5.369358509566994 + }, + { + "x": 8.258434295654297, + "y": 7.451892852783203, + "heading": -3.9312058218717824e-27, + "angularVelocity": 1.1753376253871266e-27, + "velocityX": 1.6594582279288362e-26, + "velocityY": 8.007277829857814e-26, + "timestamp": 5.451549636440282 + }, + { + "x": 8.173583810413728, + "y": 7.427578131824658, + "heading": 2.0826763233160825e-10, + "angularVelocity": 2.170043859254894e-9, + "velocityX": -0.884099331181325, + "velocityY": -0.2533471491219265, + "timestamp": 5.5475235640839005 + }, + { + "x": 8.003882842204604, + "y": 7.378948690082569, + "heading": 6.074862710776759e-10, + "angularVelocity": 4.159657195946921e-9, + "velocityX": -1.7681986386894193, + "velocityY": -0.5066942964204343, + "timestamp": 5.643497491727519 + }, + { + "x": 7.749331397599835, + "y": 7.306004528930369, + "heading": 1.1824464201448697e-9, + "angularVelocity": 5.990795241830689e-9, + "velocityX": -2.652297877711105, + "velocityY": -0.7600414294084665, + "timestamp": 5.739471419371138 + }, + { + "x": 7.426386753090784, + "y": 7.213461633012268, + "heading": 1.1824469628681777e-9, + "angularVelocity": 5.654903592256136e-15, + "velocityX": -3.3649205824757455, + "velocityY": -0.9642503770580384, + "timestamp": 5.8354453470147565 + }, + { + "x": 7.103442110110582, + "y": 7.120918731758949, + "heading": 1.1824474238942088e-9, + "angularVelocity": 4.803659101973182e-15, + "velocityX": -3.364920566545916, + "velocityY": -0.9642504326483329, + "timestamp": 5.931419274658375 + }, + { + "x": 6.7804974690937145, + "y": 7.0283758236542315, + "heading": 1.1824480406820372e-9, + "angularVelocity": 6.426618597592819e-15, + "velocityX": -3.364920546088964, + "velocityY": -0.9642505040364596, + "timestamp": 6.027393202301994 + }, + { + "x": 6.4575528306495285, + "y": 6.935832906571688, + "heading": 1.1824490782902537e-9, + "angularVelocity": 1.0811355148059625e-14, + "velocityX": -3.3649205192829115, + "velocityY": -0.964250597580873, + "timestamp": 6.1233671299456125 + }, + { + "x": 6.134608195619217, + "y": 6.843289977575833, + "heading": 1.18245090262753e-9, + "angularVelocity": 1.9008675803016132e-14, + "velocityX": -3.364920483712055, + "velocityY": -0.9642507217115844, + "timestamp": 6.219341057589231 + }, + { + "x": 5.811663565059857, + "y": 6.750747032977816, + "heading": 1.1824542039138914e-9, + "angularVelocity": 3.439774157926315e-14, + "velocityX": -3.36492043712699, + "velocityY": -0.9642508842782657, + "timestamp": 6.31531498523285 + }, + { + "x": 5.4887189399527605, + "y": 6.658204069353181, + "heading": 1.1824584939954223e-9, + "angularVelocity": 4.470048935450349e-14, + "velocityX": -3.36492038031715, + "velocityY": -0.964251082526029, + "timestamp": 6.4112889128764685 + }, + { + "x": 5.165774321115312, + "y": 6.565661083849535, + "heading": 1.1824622621223112e-9, + "angularVelocity": 3.926198480608096e-14, + "velocityX": -3.364920314990583, + "velocityY": -0.964251310494327, + "timestamp": 6.507262840520087 + }, + { + "x": 4.842829709322602, + "y": 6.473118073762103, + "heading": 1.1824633664401288e-9, + "angularVelocity": 1.1506435598787433e-14, + "velocityX": -3.3649202415879573, + "velocityY": -0.9642515666449902, + "timestamp": 6.603236768163706 + }, + { + "x": 4.588278299686279, + "y": 6.4001737905814675, + "heading": 6.155491002470294e-10, + "angularVelocity": -5.906961193650737e-9, + "velocityX": -2.6522975133575035, + "velocityY": -0.7600427008833064, + "timestamp": 6.6992106958073245 + }, + { + "x": 4.418577355669059, + "y": 6.351544264417561, + "heading": 2.0240396006129667e-10, + "angularVelocity": -4.304764328494195e-9, + "velocityX": -1.7681983866219761, + "velocityY": -0.5066951760532586, + "timestamp": 6.795184623450943 + }, + { + "x": 4.33372688293457, + "y": 6.3272294998168945, + "heading": 1.3559587351516463e-26, + "angularVelocity": -2.1089473467511516e-9, + "velocityX": -0.8840992008742675, + "velocityY": -0.2533476038508618, + "timestamp": 6.891158551094562 + }, + { + "x": 4.33372688293457, + "y": 6.3272294998168945, + "heading": 1.2207521358248593e-26, + "angularVelocity": -1.1102665151728888e-26, + "velocityX": 1.3383840268572876e-26, + "velocityY": 9.404006829398575e-27, + "timestamp": 6.9871324787381806 + }, + { + "x": 4.401618177694935, + "y": 6.325846819123709, + "heading": -0.01308630747679995, + "angularVelocity": -0.1537232950151799, + "velocityX": 0.7975109519558792, + "velocityY": -0.016242185390126448, + "timestamp": 7.072261459843534 + }, + { + "x": 4.537401642497592, + "y": 6.323108776290025, + "heading": -0.039258201685368914, + "angularVelocity": -0.3074381235243406, + "velocityX": 1.595032185744292, + "velocityY": -0.03216346299616805, + "timestamp": 7.1573904409488875 + }, + { + "x": 4.741079933125153, + "y": 6.319056529118257, + "heading": -0.07851462136724212, + "angularVelocity": -0.46114048555673864, + "velocityX": 2.392584616694674, + "velocityY": -0.04760126480020355, + "timestamp": 7.242519422054241 + }, + { + "x": 5.012658425331452, + "y": 6.313745941811267, + "heading": -0.1308573690249603, + "angularVelocity": -0.6148640213717588, + "velocityX": 3.1902001959849584, + "velocityY": -0.06238283646808974, + "timestamp": 7.3276484031595945 + }, + { + "x": 5.3105787380844705, + "y": 6.307711135194437, + "heading": -0.13085751022406972, + "angularVelocity": -0.0000016586491178053446, + "velocityX": 3.4996344239609702, + "velocityY": -0.07089015442768516, + "timestamp": 7.412777384264948 + }, + { + "x": 5.604210558319664, + "y": 6.269852166036089, + "heading": -0.1380733019285327, + "angularVelocity": -0.08476304556650165, + "velocityX": 3.4492580132235044, + "velocityY": -0.44472480072908593, + "timestamp": 7.4979063653703015 + }, + { + "x": 5.8916318870048965, + "y": 6.225423256640149, + "heading": -0.16478619158226124, + "angularVelocity": -0.31379313257219993, + "velocityX": 3.376304108814912, + "velocityY": -0.5219011060517268, + "timestamp": 7.583035346475655 + }, + { + "x": 6.17946353385963, + "y": 6.1717450490357715, + "heading": -0.18437285162886083, + "angularVelocity": -0.23008216229393866, + "velocityX": 3.381124067472625, + "velocityY": -0.6305515102776398, + "timestamp": 7.6681643275810085 + }, + { + "x": 6.469699084381167, + "y": 6.115046286529151, + "heading": -0.1929286980078038, + "angularVelocity": -0.10050450819274385, + "velocityX": 3.4093624374800013, + "velocityY": -0.666033608888738, + "timestamp": 7.753293308686362 + }, + { + "x": 6.760118549916772, + "y": 6.057863268678935, + "heading": -0.20041664509958976, + "angularVelocity": -0.08796002248069978, + "velocityX": 3.411522865241266, + "velocityY": -0.6717220987227442, + "timestamp": 7.8384222897917155 + }, + { + "x": 7.050519039292329, + "y": 6.000691583822382, + "heading": -0.20794097624295035, + "angularVelocity": -0.08838742160027335, + "velocityX": 3.4112999545497162, + "velocityY": -0.6715889713962131, + "timestamp": 7.923551270897069 + }, + { + "x": 7.340900045652317, + "y": 5.943531525847667, + "heading": -0.2155035684389204, + "angularVelocity": -0.08883686962740431, + "velocityX": 3.411071089886768, + "velocityY": -0.671452391800332, + "timestamp": 8.008680252002423 + }, + { + "x": 7.631260192862885, + "y": 5.886382859923546, + "heading": -0.22310904749616992, + "angularVelocity": -0.08934065647792855, + "velocityX": 3.4108260599433704, + "velocityY": -0.6713185707390947, + "timestamp": 8.093809233107777 + }, + { + "x": 7.894877666886783, + "y": 5.836591949181387, + "heading": -0.28923685715254654, + "angularVelocity": -0.7767955025156292, + "velocityX": 3.0966830637577116, + "velocityY": -0.584887897113894, + "timestamp": 8.178938214213131 + }, + { + "x": 8.092614293105157, + "y": 5.799284449530309, + "heading": -0.338793275351337, + "angularVelocity": -0.5821333411410221, + "velocityX": 2.322788592684559, + "velocityY": -0.4382467541213419, + "timestamp": 8.264067195318486 + }, + { + "x": 8.224448103747545, + "y": 5.774425131640709, + "heading": -0.3718133462112086, + "angularVelocity": -0.38788283885374675, + "velocityX": 1.5486360688286906, + "velocityY": -0.2920194458669074, + "timestamp": 8.34919617642384 + }, + { + "x": 8.290366172790527, + "y": 5.761995315551758, + "heading": -0.3883190645755742, + "angularVelocity": -0.19389070737189423, + "velocityX": 0.7743317045155746, + "velocityY": -0.14601156888707717, + "timestamp": 8.434325157529194 + }, + { + "x": 8.290366172790527, + "y": 5.761995315551758, + "heading": -0.3883190645755742, + "angularVelocity": -2.0367756371267684e-25, + "velocityX": -1.1743661571466787e-25, + "velocityY": 3.650643041091572e-26, + "timestamp": 8.519454138634549 + }, + { + "x": 8.217219935622422, + "y": 5.775759562142131, + "heading": -0.36996454764073433, + "angularVelocity": 0.2046665175088019, + "velocityX": -0.8156349569545082, + "velocityY": 0.15348158852586147, + "timestamp": 8.60913425416224 + }, + { + "x": 8.070960081985463, + "y": 5.803330463251493, + "heading": -0.3331909708699737, + "angularVelocity": 0.4100527363772757, + "velocityX": -1.630906168846257, + "velocityY": 0.3074360570024971, + "timestamp": 8.698814369689932 + }, + { + "x": 7.851622812213268, + "y": 5.844765942409557, + "heading": -0.27793235101810565, + "angularVelocity": 0.6161747175136558, + "velocityX": -2.4457737200892002, + "velocityY": 0.4620364159239815, + "timestamp": 8.788494485217624 + }, + { + "x": 7.562324711307271, + "y": 5.906544019791195, + "heading": -0.21038509942456823, + "angularVelocity": 0.7532021027858642, + "velocityX": -3.2258890301793066, + "velocityY": 0.6888715187098666, + "timestamp": 8.878174600745316 + }, + { + "x": 7.258647949682298, + "y": 5.966061500881446, + "heading": -0.19353755558831395, + "angularVelocity": 0.18786264644197387, + "velocityX": -3.3862217932937377, + "velocityY": 0.6636641884328596, + "timestamp": 8.967854716273008 + }, + { + "x": 6.951620794414783, + "y": 6.024601981076926, + "heading": -0.18840668187757148, + "angularVelocity": 0.0572130586646948, + "velocityX": -3.4235811747221354, + "velocityY": 0.6527699016779599, + "timestamp": 9.0575348318007 + }, + { + "x": 6.644049999727891, + "y": 6.0826472949292, + "heading": -0.18495304104679297, + "angularVelocity": 0.03851066438147053, + "velocityX": -3.4296431586544447, + "velocityY": 0.6472484286034442, + "timestamp": 9.147214947328392 + }, + { + "x": 6.336482277415872, + "y": 6.140693347402136, + "heading": -0.18149747756479873, + "angularVelocity": 0.038532103372761584, + "velocityX": -3.429608899389115, + "velocityY": 0.6472566647732668, + "timestamp": 9.236895062856084 + }, + { + "x": 6.028918781614655, + "y": 6.198740215406517, + "heading": -0.17803576282374894, + "angularVelocity": 0.03860069448707233, + "velocityX": -3.429561770649637, + "velocityY": 0.6472657585555538, + "timestamp": 9.326575178383775 + }, + { + "x": 5.72322506123202, + "y": 6.253439628104428, + "heading": -0.165421836270589, + "angularVelocity": 0.1406546644028885, + "velocityX": -3.4087123838309332, + "velocityY": 0.6099391417601291, + "timestamp": 9.416255293911467 + }, + { + "x": 5.423462479332292, + "y": 6.2899558743654325, + "heading": -0.12064161684333419, + "angularVelocity": 0.4993327580340479, + "velocityX": -3.3425757776501017, + "velocityY": 0.40718330976868544, + "timestamp": 9.505935409439159 + }, + { + "x": 5.109685533167775, + "y": 6.299162410823679, + "heading": -0.12064138771302875, + "angularVelocity": 0.0000025549733527998456, + "velocityX": -3.4988463643049696, + "velocityY": 0.1026597301316337, + "timestamp": 9.59561552496685 + }, + { + "x": 4.806186833344351, + "y": 6.30479303684294, + "heading": -0.0833583935793081, + "angularVelocity": 0.41573311892320053, + "velocityX": -3.3842362717486036, + "velocityY": 0.06278566866389057, + "timestamp": 9.685295640494543 + }, + { + "x": 4.579538960191469, + "y": 6.306389607556129, + "heading": -0.04245850388633555, + "angularVelocity": 0.45606419497020956, + "velocityX": -2.5272923860462164, + "velocityY": 0.017802951120149728, + "timestamp": 9.774975756022235 + }, + { + "x": 4.428627718565253, + "y": 6.307312197753628, + "heading": -0.014292556200369625, + "angularVelocity": 0.31407126897900167, + "velocityX": -1.682772605033218, + "velocityY": 0.01028756700490915, + "timestamp": 9.864655871549926 + }, + { + "x": 4.353217601776123, + "y": 6.307738780975342, + "heading": 3.5414503135026283e-25, + "angularVelocity": 0.1593726336799395, + "velocityX": -0.8408788987994145, + "velocityY": 0.004756720251796441, + "timestamp": 9.954335987077618 + }, + { + "x": 4.353217601776123, + "y": 6.307738780975342, + "heading": 1.7449109519298678e-25, + "angularVelocity": -6.34133546311643e-26, + "velocityX": 8.896875691666999e-28, + "velocityY": 1.43047293256897e-26, + "timestamp": 10.04401610260531 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.3288218975067139, + "y": 5.58574914932251, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 0.9125040628719712, + "isStopPoint": true, + "x": 2.672445297241211, + "y": 4.279448986053467, + "heading": -0.2516907201225374, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "timestamp": 1.487224835697349, + "isStopPoint": false, + "x": 1.8513420820236206, + "y": 4.9637017250061035, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 9 + }, + { + "timestamp": 2.1715590962874445, + "isStopPoint": true, + "x": 2.8590595722198486, + "y": 5.58574914932251, + "heading": 0.38687655192878945, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 2.8055902756955544, + "isStopPoint": false, + "x": 1.838901162147522, + "y": 6.319766044616699, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 10 + }, + { + "timestamp": 3.510458550550847, + "isStopPoint": true, + "x": 2.9088234901428223, + "y": 7.028900623321533, + "heading": 0.426627843154362, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 4.383064987087541, + "isStopPoint": false, + "x": 5.309927463531494, + "y": 7.402129173278809, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 5.451549636440282, + "isStopPoint": true, + "x": 8.258434295654297, + "y": 7.451892852783203, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 6.9871324787381806, + "isStopPoint": true, + "x": 4.33372688293457, + "y": 6.3272294998168945, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "timestamp": 8.519454138634549, + "isStopPoint": true, + "x": 8.290366172790527, + "y": 5.761995315551758, + "heading": -0.3883190645755742, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 10.04401610260531, + "isStopPoint": true, + "x": 4.353217601776123, + "y": 6.307738780975342, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 5 + ], + "type": "StopPoint" + }, + { + "scope": [ + 7 + ], + "type": "StopPoint" + }, + { + "scope": [ + 8 + ], + "type": "StopPoint" + }, + { + "scope": [ + 9 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [ + { + "x": 5.581140041351318, + "y": 5.294215679168701, + "radius": 0.3 + } + ], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "3PieceSourceSideLower": { + "waypoints": [ + { + "x": 0.6476043462753296, + "y": 4.391018867492676, + "heading": -1.0516500181249593, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 30 + }, + { + "x": 4.6845622062683105, + "y": 1.3570671081542969, + "heading": 0.16076270850197952, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 18 + }, + { + "x": 7.413279056549072, + "y": 0.72, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 8.270875930786133, + "y": 0.72, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 + }, + { + "x": 4.645580768585205, + "y": 1.29859459400177, + "heading": 0.16076270850197952, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 24 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 + }, + { + "x": 4.626090049743652, + "y": 1.3570671081542969, + "heading": 0.16076270850197952, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 20 + }, + { + "x": 7.432769298553467, + "y": 1.9417921304702759, + "heading": 0.5633166974965059, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 0.601472494720935, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 4.645580768585205, + "y": 1.3765579462051392, + "heading": 0.16076270850197952, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 24 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.6476043462753296, + "y": 4.391018867492676, + "heading": -1.0516500181249593, + "angularVelocity": 1.4837621687026506e-24, + "velocityX": 1.2605777337321822e-24, + "velocityY": -5.078976570788486e-24, + "timestamp": 0 + }, + { + "x": 0.6591914533996294, + "y": 4.3821264561987325, + "heading": -1.0407271677663157, + "angularVelocity": 0.1717852253768825, + "velocityX": 0.1822320862648084, + "velocityY": -0.1398522206307734, + "timestamp": 0.06358434105540607 + }, + { + "x": 0.682370568824486, + "y": 4.364325443623188, + "heading": -1.0189895299827134, + "angularVelocity": 0.341870929584073, + "velocityX": 0.36454125402760434, + "velocityY": -0.2799590635064334, + "timestamp": 0.12716868211081214 + }, + { + "x": 0.7171462629991276, + "y": 4.337595981630201, + "heading": -0.9865639713671387, + "angularVelocity": 0.509961384790003, + "velocityX": 0.5469223018972373, + "velocityY": -0.42037806084509965, + "timestamp": 0.19075302316621823 + }, + { + "x": 0.7635267836332045, + "y": 4.301915006000676, + "heading": -0.9436158522996869, + "angularVelocity": 0.6754511937149356, + "velocityX": 0.7294330626728007, + "velocityY": -0.5611597924469304, + "timestamp": 0.2543373642216243 + }, + { + "x": 0.821528668373285, + "y": 4.257255375739764, + "heading": -0.8903806097171314, + "angularVelocity": 0.8372382523578797, + "velocityX": 0.9122039133745026, + "velocityY": -0.7023683743454452, + "timestamp": 0.31792170527703034 + }, + { + "x": 0.891181254168102, + "y": 4.203582879820318, + "heading": -0.8272094806909008, + "angularVelocity": 0.9935013554860082, + "velocityX": 1.0954361504528793, + "velocityY": -0.8441149979470245, + "timestamp": 0.3815060463324364 + }, + { + "x": 0.9725299828215167, + "y": 4.140850923918169, + "heading": -0.7546270993304982, + "angularVelocity": 1.1415134631521273, + "velocityX": 1.279383057261364, + "velocityY": -0.986594417129931, + "timestamp": 0.44509038738784246 + }, + { + "x": 1.065637529662394, + "y": 4.068994254030079, + "heading": -0.673393688475822, + "angularVelocity": 1.2775694377943008, + "velocityX": 1.464315667905486, + "velocityY": -1.1301000953281266, + "timestamp": 0.5086747284432486 + }, + { + "x": 1.1705832794468332, + "y": 3.9879256971217893, + "heading": -0.584566561837581, + "angularVelocity": 1.3969968889170195, + "velocityX": 1.6504967739304255, + "velocityY": -1.2749767562684577, + "timestamp": 0.5722590694986547 + }, + { + "x": 1.2874653284560715, + "y": 3.897542804344524, + "heading": -0.4895895148245002, + "angularVelocity": 1.4937175637366416, + "velocityX": 1.8382206541605852, + "velocityY": -1.42146464486449, + "timestamp": 0.6358434105540608 + }, + { + "x": 1.4164104469481729, + "y": 3.7977514051305152, + "heading": -0.3905846134248042, + "angularVelocity": 1.557064204116311, + "velocityX": 2.0279382683189424, + "velocityY": -1.5694335674101274, + "timestamp": 0.6994277516094669 + }, + { + "x": 1.5575332696632753, + "y": 3.688506909682212, + "heading": -0.2916586370426824, + "angularVelocity": 1.5558229391088563, + "velocityX": 2.2194587593843385, + "velocityY": -1.718103760061147, + "timestamp": 0.763012092664873 + }, + { + "x": 1.7097613444585642, + "y": 3.5707774936908128, + "heading": -0.20742198100231668, + "angularVelocity": 1.324801903144104, + "velocityX": 2.394112642649561, + "velocityY": -1.8515473155381532, + "timestamp": 0.8265964337202791 + }, + { + "x": 1.8711613775944314, + "y": 3.4488084136916637, + "heading": -0.15383975328628857, + "angularVelocity": 0.8426953370380558, + "velocityX": 2.538361339550979, + "velocityY": -1.9182251160370993, + "timestamp": 0.8901807747756852 + }, + { + "x": 2.0417383028799354, + "y": 3.3225570909935143, + "heading": -0.13059320326371432, + "angularVelocity": 0.3656018075632435, + "velocityX": 2.682687631171131, + "velocityY": -1.9855725576858096, + "timestamp": 0.9537651158310914 + }, + { + "x": 2.2153893622715715, + "y": 3.187755682753254, + "heading": -0.13059307027328165, + "angularVelocity": 0.0000020915595014550256, + "velocityX": 2.731034976682693, + "velocityY": -2.120040972395975, + "timestamp": 1.0173494568864974 + }, + { + "x": 2.389039928413357, + "y": 3.0529536391086727, + "heading": -0.13059293728818866, + "angularVelocity": 0.000002091475523406473, + "velocityX": 2.7310272192719673, + "velocityY": -2.1200509654903494, + "timestamp": 1.0809337979419034 + }, + { + "x": 2.562690494523223, + "y": 2.918151595422971, + "heading": -0.13059280430309889, + "angularVelocity": 0.000002091475472778544, + "velocityX": 2.7310272187699582, + "velocityY": -2.1200509661370504, + "timestamp": 1.1445181389973094 + }, + { + "x": 2.7363410606330874, + "y": 2.7833495517372655, + "heading": -0.1305926713180123, + "angularVelocity": 0.0000020914754223957298, + "velocityX": 2.7310272187699196, + "velocityY": -2.1200509661371187, + "timestamp": 1.2081024800527154 + }, + { + "x": 2.9099916267429506, + "y": 2.648547508051558, + "heading": -0.13059253833292897, + "angularVelocity": 0.000002091475371640255, + "velocityX": 2.731027218769913, + "velocityY": -2.120050966137146, + "timestamp": 1.2716868211081214 + }, + { + "x": 3.0836421928528135, + "y": 2.513745464365849, + "heading": -0.1305924053478488, + "angularVelocity": 0.000002091475321590614, + "velocityX": 2.7310272187699067, + "velocityY": -2.120050966137172, + "timestamp": 1.3352711621635274 + }, + { + "x": 3.257292758962676, + "y": 2.3789434206801383, + "heading": -0.13059227236277188, + "angularVelocity": 0.000002091475271021181, + "velocityX": 2.7310272187699005, + "velocityY": -2.1200509661371987, + "timestamp": 1.3988555032189334 + }, + { + "x": 3.430943325072538, + "y": 2.244141376994426, + "heading": -0.1305921393776981, + "angularVelocity": 0.0000020914752212008574, + "velocityX": 2.7310272187698943, + "velocityY": -2.1200509661372253, + "timestamp": 1.4624398442743394 + }, + { + "x": 3.6045938911823994, + "y": 2.1093393333087116, + "heading": -0.13059200639262752, + "angularVelocity": 0.0000020914751708183746, + "velocityX": 2.731027218769888, + "velocityY": -2.120050966137252, + "timestamp": 1.5260241853297454 + }, + { + "x": 3.7782444572922627, + "y": 1.9745372896229973, + "heading": -0.13059187340756015, + "angularVelocity": 0.000002091475120575094, + "velocityX": 2.7310272187699067, + "velocityY": -2.1200509661372466, + "timestamp": 1.5896085263851514 + }, + { + "x": 3.951895023426423, + "y": 1.8397352459685825, + "heading": -0.13059174042249597, + "angularVelocity": 0.0000020914750700807474, + "velocityX": 2.731027219152042, + "velocityY": -2.1200509656450026, + "timestamp": 1.6531928674405574 + }, + { + "x": 4.125545965030257, + "y": 1.7049336859916362, + "heading": -0.13059160743738255, + "angularVelocity": 0.0000020914758451630756, + "velocityX": 2.731033124217136, + "velocityY": -2.1200433587804755, + "timestamp": 1.7167772084959634 + }, + { + "x": 4.303592186821449, + "y": 1.5759931847530775, + "heading": -0.13059147324861473, + "angularVelocity": 0.0000021104058890338206, + "velocityX": 2.800158322566331, + "velocityY": -2.0278656521139804, + "timestamp": 1.7803615495513694 + }, + { + "x": 4.4902367308457585, + "y": 1.459847055147331, + "heading": -0.13059133313690313, + "angularVelocity": 0.0000022035568702526466, + "velocityX": 2.9353853625953525, + "velocityY": -1.8266467447470887, + "timestamp": 1.8439458906067754 + }, + { + "x": 4.6845622062683105, + "y": 1.3570671081542969, + "heading": -0.13059118201554434, + "angularVelocity": 0.0000023767071623724323, + "velocityX": 3.0561844661285633, + "velocityY": -1.6164348845492313, + "timestamp": 1.9075302316621814 + }, + { + "x": 4.832067084470335, + "y": 1.2885914794569837, + "heading": -0.1305910326229948, + "angularVelocity": 0.0000031760307778025535, + "velocityX": 3.1358995782886923, + "velocityY": -1.4557667364794429, + "timestamp": 1.9545677322324386 + }, + { + "x": 4.982924581891149, + "y": 1.2278574954319752, + "heading": -0.13059088659891144, + "angularVelocity": 0.000003104418423448367, + "velocityX": 3.207175032514466, + "velocityY": -1.2911822118246548, + "timestamp": 2.001605232802696 + }, + { + "x": 5.136728649173284, + "y": 1.1750284268224231, + "heading": -0.13059074139155538, + "angularVelocity": 0.00000308705510077957, + "velocityX": 3.269818026415032, + "velocityY": -1.1231266110886224, + "timestamp": 2.048642733372953 + }, + { + "x": 5.293065104212638, + "y": 1.1302454562937592, + "heading": -0.1305905947227455, + "angularVelocity": 0.0000031181250722314943, + "velocityX": 3.3236556607816112, + "velocityY": -0.9520695186976181, + "timestamp": 2.0956802339432103 + }, + { + "x": 5.450482773711564, + "y": 1.0894253766985387, + "heading": -0.13059044674627163, + "angularVelocity": 0.0000031459255294965824, + "velocityX": 3.3466418834011025, + "velocityY": -0.8678199117797456, + "timestamp": 2.1427177345134676 + }, + { + "x": 5.607900655440282, + "y": 1.04860611555064, + "heading": -0.1305902987694863, + "angularVelocity": 0.0000031459321514570608, + "velocityX": 3.346646395328584, + "velocityY": -0.8678025118900428, + "timestamp": 2.189755235083725 + }, + { + "x": 5.765318537202331, + "y": 1.0077868545312796, + "heading": -0.13059015079270853, + "angularVelocity": 0.0000031459319896246793, + "velocityX": 3.346646396037204, + "velocityY": -0.867802509157372, + "timestamp": 2.236792735653982 + }, + { + "x": 5.922736418964388, + "y": 0.9669675935119395, + "heading": -0.1305900028159384, + "angularVelocity": 0.0000031459318275019883, + "velocityX": 3.3466463960373436, + "velocityY": -0.867802509156932, + "timestamp": 2.2838302362242393 + }, + { + "x": 6.080154300726486, + "y": 0.9261483324927575, + "heading": -0.13058985483917593, + "angularVelocity": 0.0000031459316651066595, + "velocityX": 3.3466463960382407, + "velocityY": -0.867802509153571, + "timestamp": 2.3308677367944965 + }, + { + "x": 6.237572182748789, + "y": 0.8853290724770384, + "heading": -0.1305897068624206, + "angularVelocity": 0.0000031459315124739283, + "velocityX": 3.346646401570085, + "velocityY": -0.86780248782032, + "timestamp": 2.377905237364754 + }, + { + "x": 6.394991721451892, + "y": 0.844516201922275, + "heading": -0.1305895588824519, + "angularVelocity": 0.0000031459998283836956, + "velocityX": 3.3466816219959377, + "velocityY": -0.8676666502252512, + "timestamp": 2.424942737935011 + }, + { + "x": 6.554152816050896, + "y": 0.8111344074726426, + "heading": -0.1305893710725833, + "angularVelocity": 0.000003992768882659048, + "velocityX": 3.3837064612154375, + "velocityY": -0.7096846993341356, + "timestamp": 2.4719802385052683 + }, + { + "x": 6.711882769951912, + "y": 0.7851880682804085, + "heading": -0.12042377120760904, + "angularVelocity": 0.21611692249230904, + "velocityX": 3.3532809351853774, + "velocityY": -0.5516096492729113, + "timestamp": 2.5190177390755255 + }, + { + "x": 6.866725553620017, + "y": 0.7651062041845728, + "heading": -0.09663025977821411, + "angularVelocity": 0.5058413210934933, + "velocityX": 3.29190075558594, + "velocityY": -0.4269330609061741, + "timestamp": 2.5660552396457827 + }, + { + "x": 7.014982029149098, + "y": 0.7495960675806145, + "heading": -0.06694522514508651, + "angularVelocity": 0.6310929422958753, + "velocityX": 3.151878261635918, + "velocityY": -0.3297398121907379, + "timestamp": 2.61309274021604 + }, + { + "x": 7.155468637798704, + "y": 0.7371442919133998, + "heading": -0.03984157562428539, + "angularVelocity": 0.5762136421410802, + "velocityX": 2.9866937432138485, + "velocityY": -0.26472018105247963, + "timestamp": 2.6601302407862972 + }, + { + "x": 7.28821756510369, + "y": 0.727344980547685, + "heading": -0.017222967030039466, + "angularVelocity": 0.48086331799160453, + "velocityX": 2.8221934774511723, + "velocityY": -0.20832976342095313, + "timestamp": 2.7071677413565545 + }, + { + "x": 7.413279056549072, + "y": 0.72, + "heading": -8.89059623238174e-24, + "angularVelocity": 0.3661539584637254, + "velocityX": 2.6587614122605316, + "velocityY": -0.15615159093571299, + "timestamp": 2.7542052419268117 + }, + { + "x": 7.584141987680618, + "y": 0.7142363916829146, + "heading": 0.012496138017563933, + "angularVelocity": 0.17655403361214328, + "velocityX": 2.4140690222585377, + "velocityY": -0.0814322229085219, + "timestamp": 2.824983222819707 + }, + { + "x": 7.7365481472096285, + "y": 0.7118516221881321, + "heading": 0.017397146839313638, + "angularVelocity": 0.0692448238833787, + "velocityX": 2.153299057225703, + "velocityY": -0.03369366383015727, + "timestamp": 2.8957612037126026 + }, + { + "x": 7.8700913671839885, + "y": 0.7115747244873029, + "heading": 0.01782819430589125, + "angularVelocity": 0.006090135111792666, + "velocityX": 1.8867904719752229, + "velocityY": -0.00391220118652886, + "timestamp": 2.966539184605498 + }, + { + "x": 7.98461628322269, + "y": 0.7125587164394606, + "heading": 0.01566183427732483, + "angularVelocity": -0.03060782465445932, + "velocityX": 1.6180867918795971, + "velocityY": 0.013902515157174911, + "timestamp": 3.0373171654983935 + }, + { + "x": 8.08006227999561, + "y": 0.7142078102454845, + "heading": 0.012151433911488669, + "angularVelocity": -0.04959735106244768, + "velocityX": 1.3485266967046623, + "velocityY": 0.023299531651227553, + "timestamp": 3.108095146391289 + }, + { + "x": 8.15640902937787, + "y": 0.716081694603084, + "heading": 0.008204279269867538, + "angularVelocity": -0.055768115900256314, + "velocityX": 1.0786793917982904, + "velocityY": 0.02647552719023059, + "timestamp": 3.1788731272841844 + }, + { + "x": 8.213654764882792, + "y": 0.7178420004122783, + "heading": 0.004513492953959301, + "angularVelocity": -0.05214596784688299, + "velocityX": 0.8088071287530345, + "velocityY": 0.02487081133125343, + "timestamp": 3.24965110817708 + }, + { + "x": 8.251806612817116, + "y": 0.7192206370589289, + "heading": 0.0016291300858943402, + "angularVelocity": -0.04075226266244733, + "velocityX": 0.5390355510714093, + "velocityY": 0.019478326864633543, + "timestamp": 3.3204290890699752 + }, + { + "x": 8.270875930786133, + "y": 0.72, + "heading": 5.577770790961069e-24, + "angularVelocity": -0.02301747048082103, + "velocityX": 0.2694244414498442, + "velocityY": 0.011011375730688399, + "timestamp": 3.3912070699628707 + }, + { + "x": 8.270875930786133, + "y": 0.72, + "heading": 2.68750083227073e-24, + "angularVelocity": -1.0022200251051524e-24, + "velocityX": 6.790628726650556e-26, + "velocityY": 3.41370039560792e-26, + "timestamp": 3.461985050855766 + }, + { + "x": 8.251549516655441, + "y": 0.7219586831033024, + "heading": -0.0006660755390764362, + "angularVelocity": -0.009331396835267377, + "velocityX": -0.2707537344882418, + "velocityY": 0.027440204960520992, + "timestamp": 3.5333650972555772 + }, + { + "x": 8.212896688696702, + "y": 0.7258760442658929, + "heading": -0.001998229790486319, + "angularVelocity": -0.018662838126333965, + "velocityX": -0.5415074647365562, + "velocityY": 0.054880339256839474, + "timestamp": 3.6047451436553883 + }, + { + "x": 8.154917446658368, + "y": 0.7317520758923975, + "heading": -0.003996392789209819, + "angularVelocity": -0.027993299241239875, + "velocityX": -0.812261198508941, + "velocityY": 0.08232036714563006, + "timestamp": 3.6761251900551994 + }, + { + "x": 8.077611789765818, + "y": 0.7395867678254814, + "heading": -0.006660425114458462, + "angularVelocity": -0.03732180713818774, + "velocityX": -1.0830149431334009, + "velocityY": 0.10976025273506382, + "timestamp": 3.7475052364550105 + }, + { + "x": 7.980979716767329, + "y": 0.749380107333022, + "heading": -0.00999012297038977, + "angularVelocity": -0.04664745995374023, + "velocityX": -1.353768705293865, + "velocityY": 0.13719995995360584, + "timestamp": 3.8188852828548217 + }, + { + "x": 7.86502122600115, + "y": 0.7611320790926304, + "heading": -0.013985224659649345, + "angularVelocity": -0.0559694465156602, + "velocityX": -1.6245224907346907, + "velocityY": 0.1646394525128738, + "timestamp": 3.8902653292546328 + }, + { + "x": 7.729736315496098, + "y": 0.7748426651724982, + "heading": -0.018645418435560536, + "angularVelocity": -0.06528706565709827, + "velocityX": -1.8952763037908287, + "velocityY": 0.1920786938561615, + "timestamp": 3.961645375654444 + }, + { + "x": 7.575124983133034, + "y": 0.790511845006039, + "heading": -0.023970351697693893, + "angularVelocity": -0.07459974503669481, + "velocityX": -2.1660301465350793, + "velocityY": 0.21951764707149618, + "timestamp": 4.033025422054255 + }, + { + "x": 7.401187226935479, + "y": 0.8081395953535565, + "heading": -0.029959641438283554, + "angularVelocity": -0.08390705866234215, + "velocityX": -2.4367840169688693, + "velocityY": 0.24695627470990408, + "timestamp": 4.1044054684540665 + }, + { + "x": 7.207923045694123, + "y": 0.8277258902299445, + "heading": -0.03661288565920585, + "angularVelocity": -0.09320874048829283, + "velocityX": -2.7075379043444587, + "velocityY": 0.2743945383095141, + "timestamp": 4.175785514853878 + }, + { + "x": 6.995332440744312, + "y": 0.8492707007132015, + "heading": -0.04392967465186433, + "angularVelocity": -0.10250468249454428, + "velocityX": -2.9782917730125558, + "velocityY": 0.3018323967258469, + "timestamp": 4.24716556125369 + }, + { + "x": 6.763415424631655, + "y": 0.872773994034365, + "heading": -0.05190959463777978, + "angularVelocity": -0.11179482766400335, + "velocityX": -3.249045465922667, + "velocityY": 0.32926979606482365, + "timestamp": 4.318545607653501 + }, + { + "x": 6.517888285888531, + "y": 0.8976525425650798, + "heading": -0.051909598739256456, + "angularVelocity": -5.745970870698805e-8, + "velocityX": -3.439716715339282, + "velocityY": 0.348536457812958, + "timestamp": 4.389925654053313 + }, + { + "x": 6.272361147029228, + "y": 0.9225310899511409, + "heading": -0.051909602840455864, + "angularVelocity": -5.7455824331798554e-8, + "velocityX": -3.439716716966896, + "velocityY": 0.3485364417769111, + "timestamp": 4.461305700453124 + }, + { + "x": 6.026834008173439, + "y": 0.94740963737188, + "heading": -0.051909606941655285, + "angularVelocity": -5.7455824691381894e-8, + "velocityX": -3.4397167169176694, + "velocityY": 0.34853644226273583, + "timestamp": 4.532685746852936 + }, + { + "x": 5.7813069462957465, + "y": 0.9722889444800497, + "heading": -0.05190961104285578, + "angularVelocity": -5.745583959865336e-8, + "velocityX": -3.4397156384916827, + "velocityY": 0.348547085116991, + "timestamp": 4.604065793252747 + }, + { + "x": 5.537158225945976, + "y": 1.0082600487060158, + "heading": -0.05190961517853181, + "angularVelocity": -5.793882502411163e-8, + "velocityX": -3.42040573891272, + "velocityY": 0.5039378095173286, + "timestamp": 4.675445839652559 + }, + { + "x": 5.296595540867912, + "y": 1.0633246680240616, + "heading": -0.051909619464878404, + "angularVelocity": -6.004964723504615e-8, + "velocityX": -3.3701671154797745, + "velocityY": 0.7714287408783704, + "timestamp": 4.74682588605237 + }, + { + "x": 5.061109922350585, + "y": 1.137142081975634, + "heading": -0.05190979783691868, + "angularVelocity": -0.0000024989061968370985, + "velocityX": -3.299039863302041, + "velocityY": 1.0341463430565587, + "timestamp": 4.818205932452182 + }, + { + "x": 4.843667300844079, + "y": 1.2172452033533754, + "heading": -0.059561850749452576, + "angularVelocity": -0.10720156820399783, + "velocityX": -3.0462661832492475, + "velocityY": 1.1222060704341816, + "timestamp": 4.8895859788519935 + }, + { + "x": 4.645580768585205, + "y": 1.29859459400177, + "heading": -0.06951316932259947, + "angularVelocity": -0.13941317041751372, + "velocityX": -2.7750967146947323, + "velocityY": 1.13966570143067, + "timestamp": 4.960966025251805 + }, + { + "x": 4.474092871614693, + "y": 1.385887514867849, + "heading": -0.08242444337490663, + "angularVelocity": -0.2012909785735536, + "velocityX": -2.6735523121009726, + "velocityY": 1.3609251412750347, + "timestamp": 5.025108363591072 + }, + { + "x": 4.3102265492062894, + "y": 1.4867635843508433, + "heading": -0.10022801616986421, + "angularVelocity": -0.27756351352189995, + "velocityX": -2.554729475898846, + "velocityY": 1.5726908637073918, + "timestamp": 5.0892507019303395 + }, + { + "x": 4.155003391680842, + "y": 1.6004909974861876, + "heading": -0.12448541502532746, + "angularVelocity": -0.37818076926286603, + "velocityX": -2.419979713000573, + "velocityY": 1.7730475077757013, + "timestamp": 5.153393040269607 + }, + { + "x": 4.00926290540325, + "y": 1.7261407641347544, + "heading": -0.1583340022250027, + "angularVelocity": -0.5277105274934034, + "velocityX": -2.2721417717378642, + "velocityY": 1.958920892218929, + "timestamp": 5.217535378608874 + }, + { + "x": 3.8730404280351673, + "y": 1.8620509915016963, + "heading": -0.21028223642363456, + "angularVelocity": -0.8098899345368795, + "velocityX": -2.1237529047906247, + "velocityY": 2.1188848253095043, + "timestamp": 5.281677716948141 + }, + { + "x": 3.741564544573194, + "y": 2.0025580918468195, + "heading": -0.2996033949603487, + "angularVelocity": -1.3925460288689253, + "velocityX": -2.049751955822999, + "velocityY": 2.1905515761202894, + "timestamp": 5.345820055287408 + }, + { + "x": 3.6100546633718698, + "y": 2.1430333719703096, + "heading": -0.3901784541639511, + "angularVelocity": -1.4120947497193557, + "velocityX": -2.050281991681201, + "velocityY": 2.1900554884743153, + "timestamp": 5.409962393626675 + }, + { + "x": 3.4785072023626884, + "y": 2.2834734612659138, + "heading": -0.4827580204171881, + "angularVelocity": -1.4433456691827582, + "velocityX": -2.0508678731571766, + "velocityY": 2.1895068519762364, + "timestamp": 5.474104731965943 + }, + { + "x": 3.346919123497923, + "y": 2.423875493587476, + "heading": -0.5782456610362502, + "angularVelocity": -1.488683498159982, + "velocityX": -2.051501118789228, + "velocityY": 2.188913531323639, + "timestamp": 5.53824707030521 + }, + { + "x": 3.2152861692168586, + "y": 2.5642354541414827, + "heading": -0.6777971832870476, + "angularVelocity": -1.552040739834588, + "velocityX": -2.052200741183146, + "velocityY": 2.1882576187291725, + "timestamp": 5.602389408644477 + }, + { + "x": 3.0838454699327147, + "y": 2.704775469009889, + "heading": -0.7323199293584344, + "angularVelocity": -0.8500274153243377, + "velocityX": -2.049203423001438, + "velocityY": 2.1910647242863015, + "timestamp": 5.666531746983744 + }, + { + "x": 2.9552905266702094, + "y": 2.842272453022611, + "heading": -0.7381340958895165, + "angularVelocity": -0.0906447548003214, + "velocityX": -2.0042135442980085, + "velocityY": 2.143622879562956, + "timestamp": 5.730674085323011 + }, + { + "x": 2.837445531451434, + "y": 2.968312162646033, + "heading": -0.7431823028036986, + "angularVelocity": -0.07870319425338904, + "velocityX": -1.837241957027799, + "velocityY": 1.965000230530428, + "timestamp": 5.794816423662279 + }, + { + "x": 2.7303126070394907, + "y": 3.082893932918914, + "heading": -0.747653667364265, + "angularVelocity": -0.06971003359615609, + "velocityX": -1.6702372751876633, + "velocityY": 1.7863672145350264, + "timestamp": 5.858958762001546 + }, + { + "x": 2.6338923756777755, + "y": 3.1860176221630003, + "heading": -0.751611677644213, + "angularVelocity": -0.061706672728596555, + "velocityX": -1.5032228923697837, + "velocityY": 1.6077319897293985, + "timestamp": 5.923101100340813 + }, + { + "x": 2.5481851332942993, + "y": 3.277683175661144, + "heading": -0.7550881872685589, + "angularVelocity": -0.05419992027664412, + "velocityX": -1.3362038959375901, + "velocityY": 1.4290959118655384, + "timestamp": 5.98724343868008 + }, + { + "x": 2.473191052958317, + "y": 3.3578905661578986, + "heading": -0.7581023488644949, + "angularVelocity": -0.04699176353679586, + "velocityX": -1.1691822012991828, + "velocityY": 1.2504594090803822, + "timestamp": 6.051385777019347 + }, + { + "x": 2.4089102482733002, + "y": 3.4266397779362934, + "heading": -0.7606669459018931, + "angularVelocity": -0.039982905266614695, + "velocityX": -1.0021587355455739, + "velocityY": 1.0718226612625237, + "timestamp": 6.1155281153586145 + }, + { + "x": 2.3553427994564915, + "y": 3.4839308009572485, + "heading": -0.7627911121209714, + "angularVelocity": -0.03311644498900487, + "velocityX": -0.8351340191789587, + "velocityY": 0.893185756932122, + "timestamp": 6.179670453697882 + }, + { + "x": 2.312488766021362, + "y": 3.5297636282468714, + "heading": -0.7644816934670975, + "angularVelocity": -0.026356715235171243, + "velocityX": -0.6681083749778789, + "velocityY": 0.714548743876462, + "timestamp": 6.243812792037149 + }, + { + "x": 2.280348193682581, + "y": 3.5641382545710476, + "heading": -0.7657440057178361, + "angularVelocity": -0.019679860189406772, + "velocityX": -0.501082018070189, + "velocityY": 0.5359116492192578, + "timestamp": 6.307955130376416 + }, + { + "x": 2.2589211184357674, + "y": 3.587054675697436, + "heading": -0.7665822895031111, + "angularVelocity": -0.013069117948915103, + "velocityX": -0.3340550999790392, + "velocityY": 0.35727448857846106, + "timestamp": 6.372097468715683 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -0.006512243047323657, + "velocityX": -0.1670277322411552, + "velocityY": 0.17863727070052132, + "timestamp": 6.4362398070549505 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": 1.4318885213105956e-25, + "velocityX": -3.1664598929952935e-25, + "velocityY": -8.517319099705152e-26, + "timestamp": 6.500382145394218 + }, + { + "x": 2.254662845986897, + "y": 3.5873395415687703, + "heading": -0.7560028105976356, + "angularVelocity": 0.18270874766778025, + "velocityX": 0.10724881682251722, + "velocityY": -0.18563544290641987, + "timestamp": 6.560571871079167 + }, + { + "x": 2.267640389134434, + "y": 3.5649741905137224, + "heading": -0.7343324319976283, + "angularVelocity": 0.3600345134223772, + "velocityX": 0.2156106046315132, + "velocityY": -0.3715808769774839, + "timestamp": 6.620761596764117 + }, + { + "x": 2.2872156815111926, + "y": 3.5313939684851823, + "heading": -0.7023698583511744, + "angularVelocity": 0.5310303923589071, + "velocityX": 0.3252264760138822, + "velocityY": -0.5579062148298977, + "timestamp": 6.680951322449067 + }, + { + "x": 2.313473905100035, + "y": 3.486574818355693, + "heading": -0.6605397935243593, + "angularVelocity": 0.6949701855390652, + "velocityX": 0.4362575720362232, + "velocityY": -0.7446312409544028, + "timestamp": 6.741141048134017 + }, + { + "x": 2.3465132166718874, + "y": 3.4304964860969096, + "heading": -0.6092924367494605, + "angularVelocity": 0.851430309603703, + "velocityX": 0.54891945752984, + "velocityY": -0.9316927701633545, + "timestamp": 6.801330773818966 + }, + { + "x": 2.3864518087256505, + "y": 3.3631500739334586, + "heading": -0.5490846408400017, + "angularVelocity": 1.0003002210809777, + "velocityX": 0.6635450087081837, + "velocityY": -1.118902128179834, + "timestamp": 6.861520499503916 + }, + { + "x": 2.4334407524616903, + "y": 3.284548274129898, + "heading": -0.48037214659109057, + "angularVelocity": 1.1415983951907667, + "velocityX": 0.7806804766313955, + "velocityY": -1.3059006152476056, + "timestamp": 6.921710225188866 + }, + { + "x": 2.4876845583004745, + "y": 3.194738200368583, + "heading": -0.4036330691658326, + "angularVelocity": 1.2749531012474145, + "velocityX": 0.9012137075139995, + "velocityY": -1.4921163494149543, + "timestamp": 6.9818999508738155 + }, + { + "x": 2.549471334765238, + "y": 3.093817881877023, + "heading": -0.3194506455237171, + "angularVelocity": 1.3986178319328149, + "velocityX": 1.0265336112042236, + "velocityY": -1.6767034131340948, + "timestamp": 7.042089676558765 + }, + { + "x": 2.619215453459605, + "y": 2.98196209624742, + "heading": -0.22869362474666396, + "angularVelocity": 1.5078490513829745, + "velocityX": 1.1587379390866084, + "velocityY": -1.858386699003879, + "timestamp": 7.102279402243715 + }, + { + "x": 2.6975187688111633, + "y": 2.859476881700614, + "heading": -0.13286180386406954, + "angularVelocity": 1.592162446198965, + "velocityX": 1.3009415553980122, + "velocityY": -2.0349854257174798, + "timestamp": 7.162469127928665 + }, + { + "x": 2.7852562883718943, + "y": 2.726945932671376, + "heading": -0.034753273861128454, + "angularVelocity": 1.6299879902505128, + "velocityX": 1.4576826619874292, + "velocityY": -2.201886576505485, + "timestamp": 7.2226588536136145 + }, + { + "x": 2.883617018333608, + "y": 2.5856822162797477, + "heading": 0.06025595074389153, + "angularVelocity": 1.5784957237108086, + "velocityX": 1.6341780734566165, + "velocityY": -2.3469739192871995, + "timestamp": 7.282848579298564 + }, + { + "x": 2.993618091797967, + "y": 2.439544726598588, + "heading": 0.14260766445208392, + "angularVelocity": 1.3682021768838868, + "velocityX": 1.8275722677344666, + "velocityY": -2.427947428205354, + "timestamp": 7.343038304983514 + }, + { + "x": 3.1145276346410644, + "y": 2.293581984129882, + "heading": 0.20384383077417081, + "angularVelocity": 1.017385702048459, + "velocityX": 2.008807009288801, + "velocityY": -2.425044155089132, + "timestamp": 7.403228030668464 + }, + { + "x": 3.2468175816396365, + "y": 2.1505705427004553, + "heading": 0.24757858696307516, + "angularVelocity": 0.7266149777426218, + "velocityX": 2.197882537146209, + "velocityY": -2.3760108523834984, + "timestamp": 7.463417756353413 + }, + { + "x": 3.390280853000983, + "y": 2.013955912336661, + "heading": 0.2801226137384155, + "angularVelocity": 0.5406907309344624, + "velocityX": 2.3835176141568395, + "velocityY": -2.269733394016016, + "timestamp": 7.523607482038363 + }, + { + "x": 3.544035674658465, + "y": 1.8860535950620219, + "heading": 0.3061151888137327, + "angularVelocity": 0.43184405277687576, + "velocityX": 2.55450278112711, + "velocityY": -2.1249858812136204, + "timestamp": 7.583797207723313 + }, + { + "x": 3.7070451600522367, + "y": 1.7682529006317282, + "heading": 0.3281829520976596, + "angularVelocity": 0.36663671470170595, + "velocityX": 2.7082609787426026, + "velocityY": -1.9571561938476998, + "timestamp": 7.643986933408263 + }, + { + "x": 3.8782577243031593, + "y": 1.6615213018872745, + "heading": 0.34797198576359334, + "angularVelocity": 0.328777601837151, + "velocityX": 2.844548007198066, + "velocityY": -1.7732527857514622, + "timestamp": 7.704176659093212 + }, + { + "x": 4.056668173911873, + "y": 1.5665668328609006, + "heading": 0.3664586508169338, + "angularVelocity": 0.30713987882425087, + "velocityX": 2.964134618964106, + "velocityY": -1.5775860073427241, + "timestamp": 7.764366384778162 + }, + { + "x": 4.241342350555325, + "y": 1.4839195959237403, + "heading": 0.38411092010286374, + "angularVelocity": 0.2932771180637541, + "velocityX": 3.0682010017804235, + "velocityY": -1.3731120385854438, + "timestamp": 7.824556110463112 + }, + { + "x": 4.431417064671585, + "y": 1.4139831463989663, + "heading": 0.40101516097721396, + "angularVelocity": 0.2808492758852529, + "velocityX": 3.1579262399560672, + "velocityY": -1.1619333487386434, + "timestamp": 7.884745836148062 + }, + { + "x": 4.626090049743652, + "y": 1.3570671081542969, + "heading": 0.41697492722531376, + "angularVelocity": 0.26515765052058465, + "velocityX": 3.2343225169531657, + "velocityY": -0.9456105273279376, + "timestamp": 7.944935561833011 + }, + { + "x": 4.771095590246596, + "y": 1.3224217071642599, + "heading": 0.4280403505516019, + "angularVelocity": 0.2504578977864778, + "velocityX": 3.2820961088293013, + "velocityY": -0.7841737314576779, + "timestamp": 7.989116334026994 + }, + { + "x": 4.917956708237317, + "y": 1.2949736641198544, + "heading": 0.43814957517205777, + "angularVelocity": 0.22881502786030425, + "velocityX": 3.3240957705742336, + "velocityY": -0.6212667113171019, + "timestamp": 8.033297106220976 + }, + { + "x": 5.066406183363406, + "y": 1.2747799857702726, + "heading": 0.4470279058701966, + "angularVelocity": 0.20095462929342126, + "velocityX": 3.3600470918502436, + "velocityY": -0.45706938441270734, + "timestamp": 8.077477878414959 + }, + { + "x": 5.21616079891639, + "y": 1.2618903010738838, + "heading": 0.45444388098508137, + "angularVelocity": 0.16785526251835975, + "velocityX": 3.389588006643782, + "velocityY": -0.2917487417330471, + "timestamp": 8.121658650608941 + }, + { + "x": 5.366915459547734, + "y": 1.25634723736205, + "heading": 0.46022561365529296, + "angularVelocity": 0.13086536027088905, + "velocityX": 3.4122233076740254, + "velocityY": -0.12546326006925063, + "timestamp": 8.165839422802923 + }, + { + "x": 5.518334246127743, + "y": 1.2581861921582334, + "heading": 0.4642859914379861, + "angularVelocity": 0.09190373053837578, + "velocityX": 3.427255320825579, + "velocityY": 0.041623419077170896, + "timestamp": 8.210020194996906 + }, + { + "x": 5.670036777042452, + "y": 1.2674335281135962, + "heading": 0.46666317405037006, + "angularVelocity": 0.05380581855714505, + "velocityX": 3.4336776697481812, + "velocityY": 0.20930679787037068, + "timestamp": 8.254200967190888 + }, + { + "x": 5.821577538212164, + "y": 1.284100828351401, + "heading": 0.4675873844825302, + "angularVelocity": 0.020918838360320994, + "velocityX": 3.4300161279288752, + "velocityY": 0.3772523523270378, + "timestamp": 8.29838173938487 + }, + { + "x": 5.972415606183977, + "y": 1.3081694677842932, + "heading": 0.467591351779606, + "angularVelocity": 0.00008979691569104718, + "velocityX": 3.414111172831806, + "velocityY": 0.544776341328194, + "timestamp": 8.342562511578853 + }, + { + "x": 6.121903298527153, + "y": 1.3395574928087577, + "heading": 0.46759157023044867, + "angularVelocity": 0.000004944477695120892, + "velocityX": 3.3835463917839124, + "velocityY": 0.7104453694618647, + "timestamp": 8.386743283772836 + }, + { + "x": 6.269684547080798, + "y": 1.3781901650520143, + "heading": 0.46759176450370754, + "angularVelocity": 0.0000043972354751028654, + "velocityX": 3.3449222640289906, + "velocityY": 0.8744227482859289, + "timestamp": 8.430924055966818 + }, + { + "x": 6.415408472766336, + "y": 1.423975753200933, + "heading": 0.4675919413300162, + "angularVelocity": 0.000004002336308335113, + "velocityX": 3.2983562407129146, + "velocityY": 1.036323854818335, + "timestamp": 8.4751048281608 + }, + { + "x": 6.5587290836787195, + "y": 1.476805540848842, + "heading": 0.4675921046539949, + "angularVelocity": 0.0000036967207824585226, + "velocityX": 3.243958939493236, + "velocityY": 1.1957642436839244, + "timestamp": 8.519285600354783 + }, + { + "x": 6.6993060573570675, + "y": 1.5365540678009835, + "heading": 0.46759241978399874, + "angularVelocity": 0.000007132740968196116, + "velocityX": 3.1818586841606873, + "velocityY": 1.352364931282918, + "timestamp": 8.563466372548765 + }, + { + "x": 6.836206251960683, + "y": 1.6027005176579716, + "heading": 0.47022361687585157, + "angularVelocity": 0.059555253590862826, + "velocityX": 3.0986374344598233, + "velocityY": 1.4971773142977605, + "timestamp": 8.607647144742748 + }, + { + "x": 6.968107168767895, + "y": 1.6730618727776314, + "heading": 0.48242555808722076, + "angularVelocity": 0.27618216263388745, + "velocityX": 2.9854823774487342, + "velocityY": 1.592578663196003, + "timestamp": 8.65182791693673 + }, + { + "x": 7.094292994493853, + "y": 1.7449092475770878, + "heading": 0.5050066138251464, + "angularVelocity": 0.5111059543907503, + "velocityX": 2.8561254016095394, + "velocityY": 1.6262136497750488, + "timestamp": 8.696008689130712 + }, + { + "x": 7.213566146886095, + "y": 1.8140035101921348, + "heading": 0.5272178489693323, + "angularVelocity": 0.5027353312582225, + "velocityX": 2.699662012890034, + "velocityY": 1.5638989357559931, + "timestamp": 8.740189461324695 + }, + { + "x": 7.326317415042033, + "y": 1.87970672983894, + "heading": 0.5469886414915726, + "angularVelocity": 0.44749766788668743, + "velocityX": 2.552043854301269, + "velocityY": 1.4871451172995578, + "timestamp": 8.784370233518677 + }, + { + "x": 7.432769298553467, + "y": 1.941792130470276, + "heading": 0.5633166974965059, + "angularVelocity": 0.3695738031296167, + "velocityX": 2.409461813932093, + "velocityY": 1.405258386130975, + "timestamp": 8.82855100571266 + }, + { + "x": 7.579701459580911, + "y": 2.027179132105106, + "heading": 0.5787605568170214, + "angularVelocity": 0.23108320735340232, + "velocityX": 2.198514913204677, + "velocityY": 1.2776276832472448, + "timestamp": 8.895383466181745 + }, + { + "x": 7.712165331896003, + "y": 2.1039708729412734, + "heading": 0.5887351557050889, + "angularVelocity": 0.14924781787259772, + "velocityX": 1.9820289629523165, + "velocityY": 1.1490186100763133, + "timestamp": 8.96221592665083 + }, + { + "x": 7.830026910442582, + "y": 2.172172632129026, + "heading": 0.5950957258261598, + "angularVelocity": 0.09517186822731284, + "velocityX": 1.7635379233284407, + "velocityY": 1.0204885277162767, + "timestamp": 9.029048387119914 + }, + { + "x": 7.933222174835435, + "y": 2.231800820984951, + "heading": 0.5989654042474855, + "angularVelocity": 0.05790118146429404, + "velocityX": 1.54408896019307, + "velocityY": 0.8922040044224955, + "timestamp": 9.095880847589 + }, + { + "x": 8.021715394731531, + "y": 2.28287256797719, + "heading": 0.6011062262493081, + "angularVelocity": 0.032032667760495764, + "velocityX": 1.3241053714763598, + "velocityY": 0.7641757707822818, + "timestamp": 9.162713308058084 + }, + { + "x": 8.095484569673351, + "y": 2.3254034695687893, + "heading": 0.6020727831826357, + "angularVelocity": 0.014462387386960431, + "velocityX": 1.103792594557322, + "velocityY": 0.6363809037267617, + "timestamp": 9.229545768527169 + }, + { + "x": 8.154515205691396, + "y": 2.359407222237499, + "heading": 0.6022883195935687, + "angularVelocity": 0.003225025824578962, + "velocityX": 0.8832629474318238, + "velocityY": 0.5087909741769772, + "timestamp": 9.296378228996254 + }, + { + "x": 8.198797270954351, + "y": 2.3848957424596233, + "heading": 0.6020871548762373, + "angularVelocity": -0.00300998520658142, + "velocityX": 0.6625831961317433, + "velocityY": 0.38137934834697124, + "timestamp": 9.363210689465339 + }, + { + "x": 8.22832355571237, + "y": 2.401879399345903, + "heading": 0.6017403922656039, + "angularVelocity": -0.005188535753427844, + "velocityX": 0.44179556686643673, + "velocityY": 0.2541228733324269, + "timestamp": 9.430043149934423 + }, + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 0.601472494720935, + "angularVelocity": -0.004008494417063559, + "velocityX": 0.220928070177263, + "velocityY": 0.12700192446944059, + "timestamp": 9.496875610403508 + }, + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 0.601472494720935, + "angularVelocity": 2.0480668095758719e-22, + "velocityX": 1.6959236187935907e-23, + "velocityY": -2.878465592599261e-23, + "timestamp": 9.563708070872593 + }, + { + "x": 8.225716112229817, + "y": 2.403169142964958, + "heading": 0.6014724848808553, + "angularVelocity": -1.401208853680834e-7, + "velocityX": -0.24738269796567666, + "velocityY": -0.10249969625322676, + "timestamp": 9.633933717783428 + }, + { + "x": 8.190970892472077, + "y": 2.3887729281095678, + "heading": 0.6014724652007141, + "angularVelocity": -2.802415057039817e-7, + "velocityX": -0.49476539250476237, + "velocityY": -0.20499939108669155, + "timestamp": 9.704159364694263 + }, + { + "x": 8.138853063244545, + "y": 2.3671786059959783, + "heading": 0.6014724356805342, + "angularVelocity": -4.203618073354203e-7, + "velocityX": -0.7421480829319401, + "velocityY": -0.30749908421644245, + "timestamp": 9.774385011605098 + }, + { + "x": 8.069362624900151, + "y": 2.3383861767704226, + "heading": 0.6014723963203429, + "angularVelocity": -5.604817189302354e-7, + "velocityX": -0.9895307683334543, + "velocityY": -0.40999877526387724, + "timestamp": 9.844610658515933 + }, + { + "x": 7.982499577880061, + "y": 2.3023956406156896, + "heading": 0.6014723471201746, + "angularVelocity": -7.006011425140131e-7, + "velocityX": -1.2369134474528922, + "velocityY": -0.5124984637084178, + "timestamp": 9.914836305426768 + }, + { + "x": 7.878263922751481, + "y": 2.259206997766796, + "heading": 0.601472288080073, + "angularVelocity": -8.407199383581404e-7, + "velocityX": -1.484296118495379, + "velocityY": -0.6149981488063821, + "timestamp": 9.985061952337603 + }, + { + "x": 7.756655660270691, + "y": 2.2088202485370934, + "heading": 0.6014722192000971, + "angularVelocity": -9.808378971745156e-7, + "velocityX": -1.7316787787686057, + "velocityY": -0.7174978294422485, + "timestamp": 10.055287599248437 + }, + { + "x": 7.617674791496479, + "y": 2.1512353933652784, + "heading": 0.6014721404803293, + "angularVelocity": -0.0000011209546846231955, + "velocityX": -1.979061423964885, + "velocityY": -0.8199975038311836, + "timestamp": 10.125513246159272 + }, + { + "x": 7.461321318017023, + "y": 2.0864524329093923, + "heading": 0.6014720519208926, + "angularVelocity": -0.0000012610697164822291, + "velocityX": -2.226444046545777, + "velocityY": -0.9224971688497368, + "timestamp": 10.195738893070107 + }, + { + "x": 7.287595242479287, + "y": 2.014471368266166, + "heading": 0.6014719535219923, + "angularVelocity": -0.0000014011818267188707, + "velocityX": -2.4738266314344535, + "velocityY": -1.0249968182510263, + "timestamp": 10.265964539980942 + }, + { + "x": 7.096496570177163, + "y": 1.935292201629055, + "heading": 0.6014718452840377, + "angularVelocity": -0.0000015412881098024927, + "velocityX": -2.7212091409390613, + "velocityY": -1.1274964364179387, + "timestamp": 10.336190186891777 + }, + { + "x": 6.8880253169921275, + "y": 1.8489149395783349, + "heading": 0.601471727208249, + "angularVelocity": -0.0000016813770192107404, + "velocityX": -2.9685914242944573, + "velocityY": -1.2299959608829605, + "timestamp": 10.406415833802612 + }, + { + "x": 6.663723389677425, + "y": 1.7559784504804536, + "heading": 0.6014717272081879, + "angularVelocity": -8.702352256817885e-13, + "velocityX": -3.1940172455726326, + "velocityY": -1.3233981200042444, + "timestamp": 10.476641480713447 + }, + { + "x": 6.439421272188231, + "y": 1.6630424203676835, + "heading": 0.6014717272081268, + "angularVelocity": -8.703596574297939e-13, + "velocityX": -3.1940199536216154, + "velocityY": -1.3233915841426174, + "timestamp": 10.546867127624282 + }, + { + "x": 6.212032631338727, + "y": 1.577936888330409, + "heading": 0.6014717272080651, + "angularVelocity": -8.787789669679053e-13, + "velocityX": -3.237971465584607, + "velocityY": -1.2118867647503735, + "timestamp": 10.617092774535116 + }, + { + "x": 5.978739653450143, + "y": 1.510684142319498, + "heading": 0.6014717272080008, + "angularVelocity": -9.15623540134239e-13, + "velocityX": -3.32204811419955, + "velocityY": -0.9576664504963734, + "timestamp": 10.687318421445951 + }, + { + "x": 5.7409415584912775, + "y": 1.4616883276374903, + "heading": 0.601471727201828, + "angularVelocity": -8.790086201060278e-11, + "velocityX": -3.386200133703241, + "velocityY": -0.6976911831686963, + "timestamp": 10.757544068356786 + }, + { + "x": 5.516819527735436, + "y": 1.4255992428677315, + "heading": 0.6014714661772929, + "angularVelocity": -0.000003716940268980297, + "velocityX": -3.1914555524195745, + "velocityY": -0.513901777445793, + "timestamp": 10.827769715267621 + }, + { + "x": 5.308884526984856, + "y": 1.3990809310343755, + "heading": 0.6014711092836408, + "angularVelocity": -0.0000050820984614807965, + "velocityX": -2.9609552905164764, + "velocityY": -0.3776157714435294, + "timestamp": 10.897995362178456 + }, + { + "x": 5.1176297142242815, + "y": 1.3812454338123161, + "heading": 0.6014706963446639, + "angularVelocity": -0.000005880173340714845, + "velocityX": -2.7234325516916758, + "velocityY": -0.253974124933375, + "timestamp": 10.96822100908929 + }, + { + "x": 4.943259966846924, + "y": 1.3716872939772524, + "heading": 0.6014702453730658, + "angularVelocity": -0.000006421750712363147, + "velocityX": -2.482992397332454, + "velocityY": -0.13610611301593797, + "timestamp": 11.038446656000126 + }, + { + "x": 4.785886935464025, + "y": 1.3701747617379714, + "heading": 0.6014697666441067, + "angularVelocity": -0.00000681701031230746, + "velocityX": -2.240962359274736, + "velocityY": -0.021538174524779245, + "timestamp": 11.10867230291096 + }, + { + "x": 4.645580768585205, + "y": 1.3765579462051392, + "heading": 0.6014692668007987, + "angularVelocity": -0.000007117674667444465, + "velocityX": -1.997933419637491, + "velocityY": 0.09089534590222227, + "timestamp": 11.178897949821796 + }, + { + "x": 4.4852918274531035, + "y": 1.4093136530981196, + "heading": 0.6014686169682122, + "angularVelocity": -0.00000794408595037517, + "velocityX": -1.9595033424539408, + "velocityY": 0.400432598081353, + "timestamp": 11.26069874988135 + }, + { + "x": 4.332045122157961, + "y": 1.4665930457792093, + "heading": 0.6014678756495806, + "angularVelocity": -0.000009062486322055742, + "velocityX": -1.8734132818208915, + "velocityY": 0.7002302256137938, + "timestamp": 11.342499549940904 + }, + { + "x": 4.189567990618789, + "y": 1.5470029255462814, + "heading": 0.6014669872102968, + "angularVelocity": -0.00001086100971182791, + "velocityX": -1.7417571886270768, + "velocityY": 0.9829962507522065, + "timestamp": 11.424300350000458 + }, + { + "x": 4.061325777655998, + "y": 1.6485874554715532, + "heading": 0.6014657443656565, + "angularVelocity": -0.00001519355115434508, + "velocityX": -1.567737881162854, + "velocityY": 1.2418525228520432, + "timestamp": 11.506101150060012 + }, + { + "x": 3.9499181722494128, + "y": 1.7683949151806115, + "heading": 0.5999046298375316, + "angularVelocity": -0.019084343025843265, + "velocityX": -1.3619378456625928, + "velocityY": 1.4646245467261332, + "timestamp": 11.587901950119566 + }, + { + "x": 3.8385139180743773, + "y": 1.888205491074706, + "heading": 0.5540381787699103, + "angularVelocity": -0.5607090766132972, + "velocityX": -1.361896877462441, + "velocityY": 1.4646626415251267, + "timestamp": 11.66970275017912 + }, + { + "x": 3.7271096639847774, + "y": 2.0080160670482403, + "heading": 0.4697368355238526, + "angularVelocity": -1.0305686886275442, + "velocityX": -1.361896876418011, + "velocityY": 1.4646626424962754, + "timestamp": 11.751503550238674 + }, + { + "x": 3.6157054099517287, + "y": 2.1278266430743598, + "heading": 0.3537615125967293, + "angularVelocity": -1.4177773670023999, + "velocityX": -1.3618968757266772, + "velocityY": 1.4646626431391025, + "timestamp": 11.833304350298228 + }, + { + "x": 3.504301155997762, + "y": 2.247637219174011, + "heading": 0.2137130812750941, + "angularVelocity": -1.7120667673137158, + "velocityX": -1.3618968747599156, + "velocityY": 1.4646626440380324, + "timestamp": 11.915105150357782 + }, + { + "x": 3.3928969021094617, + "y": 2.367447795334722, + "heading": 0.057794482937139194, + "angularVelocity": -1.9060766914802987, + "velocityX": -1.3618968739571615, + "velocityY": 1.4646626447844622, + "timestamp": 11.996905950417336 + }, + { + "x": 3.281492648234104, + "y": 2.487258371507466, + "heading": -0.105452505167902, + "angularVelocity": -1.995664932203507, + "velocityX": -1.3618968737989476, + "velocityY": 1.464662644931575, + "timestamp": 12.07870675047689 + }, + { + "x": 3.1700883943158846, + "y": 2.6070689476403572, + "heading": -0.26730664929832215, + "angularVelocity": -1.978637666289151, + "velocityX": -1.3618968743229085, + "velocityY": 1.4646626444443769, + "timestamp": 12.160507550536444 + }, + { + "x": 3.058684140329769, + "y": 2.7268795237101155, + "heading": -0.41916927419219197, + "angularVelocity": -1.8564931490072147, + "velocityX": -1.3618968751529306, + "velocityY": 1.4646626436725918, + "timestamp": 12.242308350595998 + }, + { + "x": 2.947279886280209, + "y": 2.846690099720882, + "heading": -0.5526625850121627, + "angularVelocity": -1.6319316036369287, + "velocityX": -1.3618968759285288, + "velocityY": 1.464662642951413, + "timestamp": 12.324109150655552 + }, + { + "x": 2.835875632186779, + "y": 2.966500675690855, + "heading": -0.65985673738226, + "angularVelocity": -1.3104291436276523, + "velocityX": -1.3618968764648283, + "velocityY": 1.464662642452743, + "timestamp": 12.405909950715106 + }, + { + "x": 2.7244713780717897, + "y": 3.086311251640782, + "heading": -0.7334716118848168, + "angularVelocity": -0.899928539194763, + "velocityX": -1.3618968767283959, + "velocityY": 1.4646626422076685, + "timestamp": 12.48771075077466 + }, + { + "x": 2.613067124142982, + "y": 3.2061218277638193, + "heading": -0.7669996227711985, + "angularVelocity": -0.4098738748517434, + "velocityX": -1.361896874452356, + "velocityY": 1.4646626443239155, + "timestamp": 12.569511550834214 + }, + { + "x": 2.5088215468762853, + "y": 3.3182335485748182, + "heading": -0.7669997571492978, + "angularVelocity": -0.0000016427479828891154, + "velocityX": -1.2743833457717078, + "velocityY": 1.3705455292537272, + "timestamp": 12.651312350893768 + }, + { + "x": 2.421950224540146, + "y": 3.411659991166572, + "heading": -0.7669998477403924, + "angularVelocity": -0.0000011074597615269072, + "velocityX": -1.06198621862983, + "velocityY": 1.1421213792986926, + "timestamp": 12.733113150953322 + }, + { + "x": 2.352453163850527, + "y": 3.4864011482861903, + "heading": -0.7669999126210906, + "angularVelocity": -7.931548110988282e-7, + "velocityX": -0.8495890093864996, + "velocityY": 0.9136971406783939, + "timestamp": 12.814913951012876 + }, + { + "x": 2.300330367046085, + "y": 3.5424570175160435, + "heading": -0.7669999578169298, + "angularVelocity": -5.525109684504957e-7, + "velocityX": -0.6371917727760005, + "velocityY": 0.6852728725029872, + "timestamp": 12.89671475107243 + }, + { + "x": 2.2655818352461483, + "y": 3.5798275976473137, + "heading": -0.7669999863406792, + "angularVelocity": -3.486976819494124e-7, + "velocityX": -0.4247945224819143, + "velocityY": 0.4568485895500231, + "timestamp": 12.978515551131984 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -1.6698272956598662e-7, + "velocityX": -0.21239726397767503, + "velocityY": 0.2284242977305235, + "timestamp": 13.060316351191538 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -2.381764007243456e-23, + "velocityX": 2.799365906238757e-23, + "velocityY": -7.174872988733629e-24, + "timestamp": 13.142117151251092 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.6476043462753296, + "y": 4.391018867492676, + "heading": -1.0516500181249593, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 30 + }, + { + "timestamp": 1.9075302316621814, + "isStopPoint": false, + "x": 4.6845622062683105, + "y": 1.3570671081542969, + "heading": 0.16076270850197952, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 18 + }, + { + "timestamp": 2.7542052419268117, + "isStopPoint": false, + "x": 7.413279056549072, + "y": 0.72, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 3.461985050855766, + "isStopPoint": true, + "x": 8.270875930786133, + "y": 0.72, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 + }, + { + "timestamp": 4.960966025251805, + "isStopPoint": false, + "x": 4.645580768585205, + "y": 1.29859459400177, + "heading": 0.16076270850197952, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 24 + }, + { + "timestamp": 6.500382145394218, + "isStopPoint": true, + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 + }, + { + "timestamp": 7.944935561833011, + "isStopPoint": false, + "x": 4.626090049743652, + "y": 1.3570671081542969, + "heading": 0.16076270850197952, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 20 + }, + { + "timestamp": 8.82855100571266, + "isStopPoint": false, + "x": 7.432769298553467, + "y": 1.9417921304702759, + "heading": 0.5633166974965059, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 9.563708070872593, + "isStopPoint": true, + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 0.601472494720935, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "timestamp": 11.178897949821796, + "isStopPoint": false, + "x": 4.645580768585205, + "y": 1.3765579462051392, + "heading": 0.16076270850197952, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 24 + }, + { + "timestamp": 13.142117151251092, + "isStopPoint": true, + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 8 + ], + "type": "StopPoint" + }, + { + "scope": [ + 10 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 5 + ], + "type": "StopPoint" + }, + { + "scope": [ + 9, + 10 + ], + "type": "MaxVelocity", + "velocity": 2 + }, + { + "scope": [ + 4, + 5 + ], + "type": "MaxVelocity", + "velocity": 3 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": true } }, "splitTrajectoriesAtStopPoints": true, diff --git a/README.md b/README.md index 8b13789..6ce9700 100644 --- a/README.md +++ b/README.md @@ -1 +1,30 @@ +# 2024 Robot Code - FRC Crescendo +## Features: + +### Fully AdvantageKit Replay compatible, with notable AdvantageKit implementations such as: +- AprilTag vision with PhotonVision +- Neural network game piece detection with Limelight +- High-Frequency odometry with Phoenix v6 + +### Full desktop simulation capability for the following: +- Drivetrain with CTRE sim API +- AprilTag vision with PhotonVision sim API +- Feeder rollers +- Shooter flywheels +- Double-jointed arm +- Intake sim with game piece pickup mocking + +### Other notable features: +- Cantilevered Double-Jointed Arm implementation +- Choreo Autonoumous routines using game piece detection + april tag localization for correction +- Extensive use of WPILIB's Command Based library +- CTRE CANDle-based LEDs implementation +- Use of MotionMagic & MotionMagicVelocity for motion profiling +- Both AdvantageScope 3D and Mechanism2d implementation of double-jointed Arm visualization +- AdvantageScope game piece visualization +- SysId routines for the drivetrain, shooter, and arm +- Code written to be switchable for two robots, with multiple vendor implementations for certain subsystems, e.g. Phoenix v6 or REVLib +- Driver Convenience Features (eg. Auto Drive to Chain & Auto Aim to Speaker) +*** +Thanks to 6328, for making AdvantageKit/Scope and inspiring many parts of this codebase diff --git a/build.gradle b/build.gradle index 6c233b5..4177486 100755 --- a/build.gradle +++ b/build.gradle @@ -29,6 +29,12 @@ deploy { // getTargetTypeClass is a shortcut to get the class type using a string frcJava(getArtifactTypeClass('FRCJavaArtifact')) { + jvmArgs.add("-Dcom.sun.management.jmxremote=true") + jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") + jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") + jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") + jvmArgs.add("-Djava.rmi.server.hostname=10.49.9.2") } // Static files artifact @@ -113,7 +119,11 @@ wpi.sim.addDriverstation() // in order to make them all available at runtime. Also adding the manifest so WPILib // knows where to look for our Robot Class. jar { - from { configurations.runtimeClasspath.collect { it.isDirectory() ? it : zipTree(it) } } + from { + configurations.runtimeClasspath.collect { + it.isDirectory() ? it : zipTree(it) + } + } from sourceSets.main.allSource manifest edu.wpi.first.gradlerio.GradleRIOPlugin.javaManifest(ROBOT_MAIN_CLASS) duplicatesStrategy = DuplicatesStrategy.INCLUDE @@ -145,8 +155,8 @@ project.compileJava.dependsOn(spotlessApply) spotless { java { target fileTree(".") { - include "**/*.java" - exclude "**/build/**", "**/build-*/**" + include "src/**/*.java" + exclude "**/build/**", "**/build-*/**", "src/main/java/org/littletonrobotics/frc2024/BuildConstants.java" } toggleOffOn() googleJavaFormat() diff --git a/src/main/deploy/choreo/3PieceAmpSide.1.traj b/src/main/deploy/choreo/3PieceAmpSide.1.traj new file mode 100644 index 0000000..6bc9e37 --- /dev/null +++ b/src/main/deploy/choreo/3PieceAmpSide.1.traj @@ -0,0 +1,590 @@ +{ + "samples": [ + { + "x": 0.660783052444458, + "y": 6.694385051727295, + "heading": 1.0498621745809171, + "angularVelocity": 5.036142496978594e-25, + "velocityX": 4.0909648611784826e-25, + "velocityY": -1.2650073502903171e-25, + "timestamp": 0 + }, + { + "x": 0.6757749919218088, + "y": 6.703069104575789, + "heading": 1.049862128706329, + "angularVelocity": -6.805633230382436e-7, + "velocityX": 0.2224099351379215, + "velocityY": 0.12883053814923912, + "timestamp": 0.06740678858637275 + }, + { + "x": 0.705758870441161, + "y": 6.720437210067111, + "heading": 1.0498620338020643, + "angularVelocity": -0.0000014079333376652512, + "velocityX": 0.4448198638173059, + "velocityY": 0.25766107324734056, + "timestamp": 0.1348135771727455 + }, + { + "x": 0.750734687393026, + "y": 6.746489367913325, + "heading": 1.049861885451002, + "angularVelocity": -0.000002200832664302106, + "velocityX": 0.6672297834547413, + "velocityY": 0.3864916040738485, + "timestamp": 0.20222036575911825 + }, + { + "x": 0.8107024418631711, + "y": 6.781225577682529, + "heading": 1.0498616770274682, + "angularVelocity": -0.000003092025867532283, + "velocityX": 0.8896396895292586, + "velocityY": 0.5153221284929648, + "timestamp": 0.269627154345491 + }, + { + "x": 0.8856621323278759, + "y": 6.82464583865489, + "heading": 1.0498613974886852, + "angularVelocity": -0.0000041470419969153815, + "velocityX": 1.1120495729989293, + "velocityY": 0.6441526422330909, + "timestamp": 0.33703394293186373 + }, + { + "x": 0.9756137557397047, + "y": 6.8767501493907295, + "heading": 1.0498610247491444, + "angularVelocity": -0.000005529703292486692, + "velocityX": 1.334459411258968, + "velocityY": 0.7729831346152221, + "timestamp": 0.40444073151823645 + }, + { + "x": 1.0805573029563829, + "y": 6.937538505571021, + "heading": 1.049860492552681, + "angularVelocity": -0.000007895294742633066, + "velocityX": 1.556869113890614, + "velocityY": 0.9018135629232547, + "timestamp": 0.4718475201046092 + }, + { + "x": 1.1972133032133725, + "y": 7.005111257740716, + "heading": 1.0334106168225663, + "angularVelocity": -0.2440388583271016, + "velocityX": 1.7306268805182825, + "velocityY": 1.0024621197182535, + "timestamp": 0.5392543086909819 + }, + { + "x": 1.3138692969010544, + "y": 7.072684021251592, + "heading": 0.9724418882550508, + "angularVelocity": -0.9044894415848371, + "velocityX": 1.7306267830606272, + "velocityY": 1.0024622879680827, + "timestamp": 0.6066610972773546 + }, + { + "x": 1.4305252906191748, + "y": 7.140256784709921, + "heading": 0.8706420088958982, + "angularVelocity": -1.5102318548926246, + "velocityX": 1.7306267835121878, + "velocityY": 1.0024622871885176, + "timestamp": 0.6740678858637273 + }, + { + "x": 1.5471812843420916, + "y": 7.207829548159968, + "heading": 0.7338773790646791, + "angularVelocity": -2.028944453509658, + "velocityX": 1.7306267835833422, + "velocityY": 1.0024622870656754, + "timestamp": 0.7414746744501001 + }, + { + "x": 1.6638372780224426, + "y": 7.2754023116835, + "heading": 0.5723600050127189, + "angularVelocity": -2.396158865289915, + "velocityX": 1.7306267829518702, + "velocityY": 1.002462288155831, + "timestamp": 0.8088814630364728 + }, + { + "x": 1.780493271638965, + "y": 7.3429750753172245, + "heading": 0.40418447555722015, + "angularVelocity": -2.4949346049916663, + "velocityX": 1.7306267820049508, + "velocityY": 1.0024622897905686, + "timestamp": 0.8762882516228455 + }, + { + "x": 1.8971492652070823, + "y": 7.410547839034513, + "heading": 0.25129496131963275, + "angularVelocity": -2.26816196771754, + "velocityX": 1.7306267812868497, + "velocityY": 1.0024622910302834, + "timestamp": 0.9436950402092182 + }, + { + "x": 2.013805258779043, + "y": 7.478120602745167, + "heading": 0.12816045902008172, + "angularVelocity": -1.8267374085292716, + "velocityX": 1.7306267813438676, + "velocityY": 1.0024622909318521, + "timestamp": 1.011101828795591 + }, + { + "x": 2.1304612523991975, + "y": 7.545693366372621, + "heading": 0.04280584220849794, + "angularVelocity": -1.266261434517286, + "velocityX": 1.7306267820588324, + "velocityY": 1.0024622896975566, + "timestamp": 1.0785086173819638 + }, + { + "x": 2.247117388567763, + "y": 7.613265883907332, + "heading": 0.000013466426965438275, + "angularVelocity": -0.6348377764162405, + "velocityX": 1.7306288968074217, + "velocityY": 1.002458638837633, + "timestamp": 1.1459154059683365 + }, + { + "x": 2.365737851453118, + "y": 7.677327455956403, + "heading": 0.000004459976492377945, + "angularVelocity": -0.00013361340396034107, + "velocityX": 1.759770274968055, + "velocityY": 0.9503727056657639, + "timestamp": 1.2133221945547092 + }, + { + "x": 2.491594313865089, + "y": 7.725647742212638, + "heading": 0.000001984433687850704, + "angularVelocity": -0.000036725422712508624, + "velocityX": 1.8671185061828453, + "velocityY": 0.7168459923634953, + "timestamp": 1.280728983141082 + }, + { + "x": 2.622608441209736, + "y": 7.7574282007435205, + "heading": 7.92136305089401e-7, + "angularVelocity": -0.00001768809058799076, + "velocityX": 1.9436340180599136, + "velocityY": 0.4714726691089775, + "timestamp": 1.3481357717274547 + }, + { + "x": 2.7566164536261604, + "y": 7.772143891629413, + "heading": 2.0780523669770732e-7, + "angularVelocity": -0.000008668727299520462, + "velocityX": 1.9880492043425424, + "velocityY": 0.2183117041251145, + "timestamp": 1.4155425603138274 + }, + { + "x": 2.8914051055908203, + "y": 7.769551753997803, + "heading": -8.09802518102299e-24, + "angularVelocity": -0.000003082853241575703, + "velocityX": 1.9996302270348674, + "velocityY": -0.03845514207057226, + "timestamp": 1.4829493489002001 + }, + { + "x": 2.9864482996127166, + "y": 7.767723966300088, + "heading": -1.0517201217515276e-7, + "angularVelocity": -0.0000023977731499342787, + "velocityX": 2.1668504195789944, + "velocityY": -0.04167097476524414, + "timestamp": 1.526811718523463 + }, + { + "x": 3.0888261669051587, + "y": 7.76575512457206, + "heading": -1.802948176200958e-7, + "angularVelocity": -0.0000017126937301878197, + "velocityX": 2.3340705979128114, + "velocityY": -0.04488680718663573, + "timestamp": 1.570674088146726 + }, + { + "x": 3.1985387066370827, + "y": 7.7636452288297, + "heading": -2.2536845598951497e-7, + "angularVelocity": -0.0000010276152145121185, + "velocityX": 2.5012907572995546, + "velocityY": -0.04810263924365375, + "timestamp": 1.614536457769989 + }, + { + "x": 3.315585917645001, + "y": 7.761394279095385, + "heading": -2.4039298339678027e-7, + "angularVelocity": -3.4253797814190234e-7, + "velocityX": 2.668510890160408, + "velocityY": -0.05131847079054902, + "timestamp": 1.658398827393252 + }, + { + "x": 3.4399677981836816, + "y": 7.759002275402676, + "heading": -2.2536848500409826e-7, + "angularVelocity": 3.4253731665043406e-7, + "velocityX": 2.835730983232458, + "velocityY": -0.05453430157226078, + "timestamp": 1.702261197016515 + }, + { + "x": 3.5716843453444107, + "y": 7.756469217807512, + "heading": -1.8029510454250157e-7, + "angularVelocity": 0.0000010276093345784838, + "velocityX": 3.0029510099899284, + "velocityY": -0.057750131078668464, + "timestamp": 1.7461235666397779 + }, + { + "x": 3.7107355533097754, + "y": 7.753795106421767, + "heading": -1.0517313285100412e-7, + "angularVelocity": 0.0000017126747217883014, + "velocityX": 3.1701709041186272, + "velocityY": -0.06096595803447545, + "timestamp": 1.7899859362630408 + }, + { + "x": 3.8571214046276947, + "y": 7.750979941581066, + "heading": -3.445312029122533e-12, + "angularVelocity": 0.000002397720151516781, + "velocityX": 3.337390400364549, + "velocityY": -0.06418177733854838, + "timestamp": 1.8338483058863038 + }, + { + "x": 4.008740030563312, + "y": 7.748064144589167, + "heading": -2.953693080850903e-12, + "angularVelocity": 1.1208216804044482e-11, + "velocityX": 3.456690261786607, + "velocityY": -0.06647604807818788, + "timestamp": 1.8777106755095667 + }, + { + "x": 4.160358656499413, + "y": 7.745148347597258, + "heading": -2.46204265095573e-12, + "angularVelocity": 1.1208934540541933e-11, + "velocityX": 3.4566902617976116, + "velocityY": -0.06647604807839952, + "timestamp": 1.9215730451328297 + }, + { + "x": 4.311977282435513, + "y": 7.742232550605349, + "heading": -1.970393895641841e-12, + "angularVelocity": 1.1208896362456744e-11, + "velocityX": 3.4566902617976116, + "velocityY": -0.06647604807839952, + "timestamp": 1.9654354147560926 + }, + { + "x": 4.463595908371613, + "y": 7.739316753613441, + "heading": -1.478772239262654e-12, + "angularVelocity": 1.1208278545067146e-11, + "velocityX": 3.456690261797612, + "velocityY": -0.06647604807839953, + "timestamp": 2.0092977843793554 + }, + { + "x": 4.615214534307714, + "y": 7.736400956621532, + "heading": -9.871132742331443e-13, + "angularVelocity": 1.1209129129534961e-11, + "velocityX": 3.4566902617976116, + "velocityY": -0.06647604807839952, + "timestamp": 2.0531601540026183 + }, + { + "x": 4.766833160243814, + "y": 7.733485159629623, + "heading": -4.955367585470695e-13, + "angularVelocity": 1.120724940109933e-11, + "velocityX": 3.456690261797612, + "velocityY": -0.06647604807839952, + "timestamp": 2.0970225236258813 + }, + { + "x": 4.91845178604126, + "y": 7.730569362640381, + "heading": 5.035858981265476e-24, + "angularVelocity": 1.1297537338117902e-11, + "velocityX": 3.4566902586364923, + "velocityY": -0.06647604801760763, + "timestamp": 2.140884893249144 + }, + { + "x": 5.07168754119456, + "y": 7.72009798657163, + "heading": -1.2237883220746723e-17, + "angularVelocity": -2.754707452373014e-16, + "velocityX": 3.4492852675356476, + "velocityY": -0.23570715051872318, + "timestamp": 2.1853102577139336 + }, + { + "x": 5.224226523742201, + "y": 7.702133987621307, + "heading": -3.220999671007562e-17, + "angularVelocity": -4.4956555179430785e-16, + "velocityX": 3.433601150724158, + "velocityY": -0.4043635694775183, + "timestamp": 2.229735622178723 + }, + { + "x": 5.375704105653677, + "y": 7.6767294867936675, + "heading": -8.086584933587513e-18, + "angularVelocity": 5.430098788639561e-16, + "velocityX": 3.4097093796840374, + "velocityY": -0.5718467621751168, + "timestamp": 2.2741609866435124 + }, + { + "x": 5.527091031475673, + "y": 7.65079022745198, + "heading": 1.237593714278334e-17, + "angularVelocity": 4.606044840123949e-16, + "velocityX": 3.4076687416257836, + "velocityY": -0.5838839963203132, + "timestamp": 2.318586351108302 + }, + { + "x": 5.678477940102667, + "y": 7.624850867756873, + "heading": -5.299489627912929e-18, + "angularVelocity": -3.9786790685233107e-16, + "velocityX": 3.4076683545720687, + "velocityY": -0.5838862552420002, + "timestamp": 2.363011715573091 + }, + { + "x": 5.82986484872645, + "y": 7.598911508043026, + "heading": -6.861733443062272e-18, + "angularVelocity": -3.51655824113109e-17, + "velocityX": 3.4076683544997888, + "velocityY": -0.5838862556638376, + "timestamp": 2.4074370800378806 + }, + { + "x": 5.981251757350232, + "y": 7.572972148329175, + "heading": 1.300275398298883e-18, + "angularVelocity": 1.8372407158846258e-16, + "velocityX": 3.4076683544997755, + "velocityY": -0.5838862556639163, + "timestamp": 2.45186244450267 + }, + { + "x": 6.13263866597402, + "y": 7.547032788615358, + "heading": 2.03304825021732e-17, + "angularVelocity": 4.2836355611375324e-16, + "velocityX": 3.407668354499907, + "velocityY": -0.5838862556631488, + "timestamp": 2.4962878089674594 + }, + { + "x": 6.284025574629089, + "y": 7.521093429084101, + "heading": 9.540525391081043e-18, + "angularVelocity": -2.428783025456181e-16, + "velocityX": 3.407668355204027, + "velocityY": -0.5838862515537734, + "timestamp": 2.540713173432249 + }, + { + "x": 6.435412650788291, + "y": 7.495155047156986, + "heading": 3.1121659748942844e-18, + "angularVelocity": -1.4470020659857382e-16, + "velocityX": 3.407672125665691, + "velocityY": -0.5838642460136346, + "timestamp": 2.585138537897038 + }, + { + "x": 6.587408261478287, + "y": 7.473060233149462, + "heading": 4.012412418719246e-18, + "angularVelocity": 2.0264244415020296e-17, + "velocityX": 3.4213700331139205, + "velocityY": -0.49734682593400187, + "timestamp": 2.6295639023618276 + }, + { + "x": 6.740303543651052, + "y": 7.458435646449454, + "heading": -2.1084051336842118e-18, + "angularVelocity": -1.377775427669848e-16, + "velocityX": 3.4416213353510248, + "velocityY": -0.3291945238085493, + "timestamp": 2.673989266826617 + }, + { + "x": 6.890285316295548, + "y": 7.450749433084636, + "heading": -1.4822341121773416e-18, + "angularVelocity": 1.40948989157144e-17, + "velocityX": 3.3760392165914315, + "velocityY": -0.17301407557185516, + "timestamp": 2.7184146312914064 + }, + { + "x": 7.032993253879116, + "y": 7.444993241304835, + "heading": -9.121457386741735e-19, + "angularVelocity": 1.2832497387232803e-17, + "velocityX": 3.212307637828718, + "velocityY": -0.1295699393611814, + "timestamp": 2.7628399957561958 + }, + { + "x": 7.168350422821424, + "y": 7.440849530732545, + "heading": -4.3340581409719474e-19, + "angularVelocity": 1.0776274550871163e-17, + "velocityX": 3.046844310069511, + "velocityY": -0.09327353016031785, + "timestamp": 2.807265360220985 + }, + { + "x": 7.296333312988281, + "y": 7.438207149505615, + "heading": -1.2424191874750213e-28, + "angularVelocity": 9.755818981171634e-18, + "velocityX": 2.880851777103443, + "velocityY": -0.05947911196145459, + "timestamp": 2.8516907246857746 + }, + { + "x": 7.476987619873736, + "y": 7.437582893575528, + "heading": 6.306246828045357e-19, + "angularVelocity": 9.156773444569897e-18, + "velocityX": 2.623130055966497, + "velocityY": -0.009064298112001053, + "timestamp": 2.9205604730516797 + }, + { + "x": 7.639661681076791, + "y": 7.438908313736393, + "heading": 7.717197233455447e-19, + "angularVelocity": 2.04872304504038e-18, + "velocityX": 2.362053950578807, + "velocityY": 0.019245317317299903, + "timestamp": 2.9894302214175847 + }, + { + "x": 7.78428303377631, + "y": 7.4413247257119215, + "heading": 8.357244621441239e-19, + "angularVelocity": 9.29359266725832e-19, + "velocityX": 2.0999256731873923, + "velocityY": 0.0350866967407844, + "timestamp": 3.05829996978349 + }, + { + "x": 7.91082689918953, + "y": 7.4442843222063875, + "heading": 7.220816869988264e-19, + "angularVelocity": -1.6501116644010886e-18, + "velocityX": 1.8374376038212434, + "velocityY": 0.042973824715339615, + "timestamp": 3.127169718149395 + }, + { + "x": 8.019285863587411, + "y": 7.447408189663725, + "heading": 5.712912024628398e-19, + "angularVelocity": -2.1895024749108468e-18, + "velocityX": 1.5748418858979918, + "velocityY": 0.04535906593908385, + "timestamp": 3.1960394665153 + }, + { + "x": 8.109659533807191, + "y": 7.450418969222921, + "heading": 4.341547519848611e-19, + "angularVelocity": -1.9912436692380873e-18, + "velocityX": 1.312240459187152, + "velocityY": 0.04371701117883307, + "timestamp": 3.264909214881205 + }, + { + "x": 8.181950471768632, + "y": 7.4531049914429115, + "heading": 2.769074151229353e-19, + "angularVelocity": -2.283257025771601e-18, + "velocityX": 1.0496762319699213, + "velocityY": 0.03900148154629361, + "timestamp": 3.33377896324711 + }, + { + "x": 8.236162441930201, + "y": 7.455299473800812, + "heading": 8.002361823653196e-20, + "angularVelocity": -2.858784902160653e-18, + "velocityX": 0.7871666653047966, + "velocityY": 0.03186424242820522, + "timestamp": 3.4026487116130153 + }, + { + "x": 8.27229961106123, + "y": 7.456867643223532, + "heading": 9.595707199020635e-21, + "angularVelocity": -1.0226247759898425e-18, + "velocityX": 0.5247176008111963, + "velocityY": 0.02277007626611394, + "timestamp": 3.4715184599789204 + }, + { + "x": 8.290366172790527, + "y": 7.457698345184326, + "heading": -2.737879233354451e-29, + "angularVelocity": -1.3933123630808857e-19, + "velocityX": 0.26232942849316815, + "velocityY": 0.012061928212380262, + "timestamp": 3.5403882083448255 + }, + { + "x": 8.290366172790527, + "y": 7.457698345184326, + "heading": -7.734690041039287e-29, + "angularVelocity": -1.1060203570884526e-28, + "velocityX": -8.232857746308476e-27, + "velocityY": 9.915594313558478e-27, + "timestamp": 3.6092579567107306 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3PieceAmpSide.2.traj b/src/main/deploy/choreo/3PieceAmpSide.2.traj new file mode 100644 index 0000000..e894036 --- /dev/null +++ b/src/main/deploy/choreo/3PieceAmpSide.2.traj @@ -0,0 +1,230 @@ +{ + "samples": [ + { + "x": 8.290366172790527, + "y": 7.457698345184326, + "heading": -7.734690041039287e-29, + "angularVelocity": -1.1060203570884526e-28, + "velocityX": -8.232857746308476e-27, + "velocityY": 9.915594313558478e-27, + "timestamp": 0 + }, + { + "x": 8.258529155042611, + "y": 7.450262882094732, + "heading": 0.0018141437015229199, + "angularVelocity": 0.019588491140668917, + "velocityX": -0.34376501683789845, + "velocityY": -0.0802855378738891, + "timestamp": 0.09261273308368612 + }, + { + "x": 8.19485511722891, + "y": 7.435392003543495, + "heading": 0.005442056461770318, + "angularVelocity": 0.03917293701902922, + "velocityX": -0.6875300587033178, + "velocityY": -0.16057056147775478, + "timestamp": 0.18522546616737223 + }, + { + "x": 8.099344042603773, + "y": 7.413085782435483, + "heading": 0.010882224859340402, + "angularVelocity": 0.05874104150078671, + "velocityX": -1.0312952813824383, + "velocityY": -0.24085479788027828, + "timestamp": 0.27783819925105835 + }, + { + "x": 7.9719959003584435, + "y": 7.383344317488874, + "heading": 0.018132064338159094, + "angularVelocity": 0.07828123884722897, + "velocityX": -1.3750608367237869, + "velocityY": -0.3211379683583422, + "timestamp": 0.37045093233474446 + }, + { + "x": 7.8128106462112585, + "y": 7.346167733877292, + "heading": 0.027188013699287764, + "angularVelocity": 0.09778298360923647, + "velocityX": -1.7188268702030736, + "velocityY": -0.4014197872552755, + "timestamp": 0.4630636654184306 + }, + { + "x": 7.621788223292644, + "y": 7.301556183975714, + "heading": 0.038045656619278445, + "angularVelocity": 0.11723704245051889, + "velocityX": -2.0625935177402037, + "velocityY": -0.481699960860308, + "timestamp": 0.5556763985021167 + }, + { + "x": 7.3989285635202835, + "y": 7.24950984823631, + "heading": 0.050699870016136385, + "angularVelocity": 0.1366357840387178, + "velocityX": -2.406360900406452, + "velocityY": -0.5619781860057411, + "timestamp": 0.6482891315858028 + }, + { + "x": 7.144231590225439, + "y": 7.190028936362276, + "heading": 0.06514499751032955, + "angularVelocity": 0.15597345001297352, + "velocityX": -2.750129110914985, + "velocityY": -0.6422541468491951, + "timestamp": 0.7409018646694889 + }, + { + "x": 6.857697227315765, + "y": 7.123113690113179, + "heading": 0.08137503414778206, + "angularVelocity": 0.17524627658690128, + "velocityX": -3.093898143042135, + "velocityY": -0.7225274972571053, + "timestamp": 0.833514597753175 + }, + { + "x": 6.545887160381392, + "y": 7.050327089705259, + "heading": 0.08137504044908753, + "angularVelocity": 6.803929929105723e-8, + "velocityX": -3.366816382069384, + "velocityY": -0.7859243322637903, + "timestamp": 0.9261273308368612 + }, + { + "x": 6.234077092172867, + "y": 6.977540494754869, + "heading": 0.0813750467500712, + "angularVelocity": 6.80358247192299e-8, + "velocityX": -3.3668163958272355, + "velocityY": -0.7859242733352659, + "timestamp": 1.0187400639205473 + }, + { + "x": 5.922267023964316, + "y": 6.904753899804592, + "heading": 0.08137505305105486, + "angularVelocity": 6.803582445105601e-8, + "velocityX": -3.3668163958275135, + "velocityY": -0.7859242733340743, + "timestamp": 1.1113527970042334 + }, + { + "x": 5.610456955755783, + "y": 6.831967304854232, + "heading": 0.08137505935203854, + "angularVelocity": 6.803582480789322e-8, + "velocityX": -3.366816395827309, + "velocityY": -0.7859242733349515, + "timestamp": 1.2039655300879195 + }, + { + "x": 5.298646888485436, + "y": 6.759180705885576, + "heading": 0.08137506565333762, + "angularVelocity": 6.803923039402274e-8, + "velocityX": -3.366816385697099, + "velocityY": -0.7859243167231056, + "timestamp": 1.2965782631716056 + }, + { + "x": 5.012107236392116, + "y": 6.692280236782266, + "heading": 0.09728577795318676, + "angularVelocity": 0.17179832373019394, + "velocityX": -3.0939552538029402, + "velocityY": -0.7223679387893466, + "timestamp": 1.3891909962552917 + }, + { + "x": 4.757405139637452, + "y": 6.632813653440834, + "heading": 0.1114212737800766, + "angularVelocity": 0.15263015523056464, + "velocityX": -2.75018443224769, + "velocityY": -0.642099432350162, + "timestamp": 1.4818037293389779 + }, + { + "x": 4.534540680564304, + "y": 6.580780772308004, + "heading": 0.12378499174610678, + "angularVelocity": 0.1334991156654254, + "velocityX": -2.4064127215829467, + "velocityY": -0.561832907855257, + "timestamp": 1.574416462422664 + }, + { + "x": 4.343513918608397, + "y": 6.536181432644826, + "heading": 0.13437950942576468, + "angularVelocity": 0.11439590785086255, + "velocityX": -2.0626403691519735, + "velocityY": -0.4815681189634867, + "timestamp": 1.66702919550635 + }, + { + "x": 4.184324899185163, + "y": 6.499015499114029, + "heading": 0.14320667584584268, + "angularVelocity": 0.0953126651828929, + "velocityX": -1.7188675263409952, + "velocityY": -0.4013047913963752, + "timestamp": 1.7596419285900362 + }, + { + "x": 4.056973656080411, + "y": 6.469282862636605, + "heading": 0.15026773537457858, + "angularVelocity": 0.07624285876927316, + "velocityX": -1.3750943187226343, + "velocityY": -0.32104264162636753, + "timestamp": 1.8522546616737223 + }, + { + "x": 3.9614602126260823, + "y": 6.446983440876931, + "heading": 0.1555634289688374, + "angularVelocity": 0.05718105294952869, + "velocityX": -1.0313208591740977, + "velocityY": -0.24078138088770026, + "timestamp": 1.9448673947574084 + }, + { + "x": 3.8977845824113757, + "y": 6.432117178562742, + "heading": 0.15909407056957425, + "angularVelocity": 0.03812263695475346, + "velocityX": -0.6875472528941416, + "velocityY": -0.1605207169596795, + "timestamp": 2.0374801278410946 + }, + { + "x": 3.865946769714355, + "y": 6.424684047698975, + "heading": 0.1608595979864649, + "angularVelocity": 0.019063549450541617, + "velocityX": -0.3437736004211391, + "velocityY": -0.08026035531260044, + "timestamp": 2.1300928609247807 + }, + { + "x": 3.865946769714355, + "y": 6.424684047698975, + "heading": 0.1608595979864649, + "angularVelocity": -9.999505736171943e-25, + "velocityX": -5.156111906167998e-25, + "velocityY": -7.69774068808886e-24, + "timestamp": 2.222705594008467 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3PieceAmpSide.3.traj b/src/main/deploy/choreo/3PieceAmpSide.3.traj new file mode 100644 index 0000000..c24e9ed --- /dev/null +++ b/src/main/deploy/choreo/3PieceAmpSide.3.traj @@ -0,0 +1,320 @@ +{ + "samples": [ + { + "x": 3.865946769714355, + "y": 6.424684047698975, + "heading": 0.1608595979864649, + "angularVelocity": -9.999505736171943e-25, + "velocityX": -5.156111906167998e-25, + "velocityY": -7.69774068808886e-24, + "timestamp": 0 + }, + { + "x": 3.8805908200661583, + "y": 6.424671235221358, + "heading": 0.15744018373663873, + "angularVelocity": -0.0550052976874839, + "velocityX": 0.23556676380826952, + "velocityY": -0.00020610376336075096, + "timestamp": 0.062165180329607495 + }, + { + "x": 3.9098813602559557, + "y": 6.424646814464556, + "heading": 0.1506499345999589, + "angularVelocity": -0.10922913921711586, + "velocityX": 0.47117276961950644, + "velocityY": -0.0003928365794473002, + "timestamp": 0.12433036065921499 + }, + { + "x": 3.9538212666067056, + "y": 6.424612381558881, + "heading": 0.14054735001329138, + "angularVelocity": -0.1625119485393969, + "velocityX": 0.7068250444666143, + "velocityY": -0.0005538937632522526, + "timestamp": 0.18649554098882248 + }, + { + "x": 4.012413923406757, + "y": 6.424569899192806, + "heading": 0.1272024405085879, + "angularVelocity": -0.21466855615872318, + "velocityX": 0.9425317595699895, + "velocityY": -0.0006833787957962723, + "timestamp": 0.24866072131842998 + }, + { + "x": 4.085663316044443, + "y": 6.4245216623134365, + "heading": 0.11069894640828985, + "angularVelocity": -0.2654781022558683, + "velocityX": 1.1783025843295045, + "velocityY": -0.0007759469065193845, + "timestamp": 0.3108259016480375 + }, + { + "x": 4.173574156900502, + "y": 6.4244702509624405, + "heading": 0.09113759111571623, + "angularVelocity": -0.3146673940115171, + "velocityX": 1.414149213272517, + "velocityY": -0.0008270120141812696, + "timestamp": 0.37299108197764497 + }, + { + "x": 4.276152059818016, + "y": 6.424418464281491, + "heading": 0.0686410579111913, + "angularVelocity": -0.36188318099047606, + "velocityX": 1.650086147480532, + "velocityY": -0.0008330496376791236, + "timestamp": 0.43515626230725246 + }, + { + "x": 4.39340378630487, + "y": 6.424369226491044, + "heading": 0.04336192483995491, + "angularVelocity": -0.40664457075815247, + "velocityX": 1.886131848490915, + "velocityY": -0.0007920477377600693, + "timestamp": 0.49732144263685996 + }, + { + "x": 4.525337595489836, + "y": 6.424325450771935, + "heading": 0.015495928724554285, + "angularVelocity": -0.4482573036489503, + "velocityX": 2.122310407296116, + "velocityY": -0.000704183899678652, + "timestamp": 0.5594866229664675 + }, + { + "x": 4.671963731284715, + "y": 6.424289841531667, + "heading": -0.014694474575960468, + "angularVelocity": -0.4856481255333208, + "velocityX": 2.3586537514642383, + "velocityY": -0.0005728164879388929, + "timestamp": 0.621651803296075 + }, + { + "x": 4.833295019214238, + "y": 6.424264622086766, + "heading": -0.04683458533303072, + "angularVelocity": -0.5170114618289433, + "velocityX": 2.595203409273861, + "velocityY": -0.00040568441636908034, + "timestamp": 0.6838169836256824 + }, + { + "x": 5.009347081398109, + "y": 6.424251287994793, + "heading": -0.08033775965322806, + "angularVelocity": -0.5389379415061509, + "velocityX": 2.832004367242584, + "velocityY": -0.00021449454344913397, + "timestamp": 0.7459821639552899 + }, + { + "x": 5.200134146051008, + "y": 6.424251504702917, + "heading": -0.1141370339338056, + "angularVelocity": -0.5437010574306965, + "velocityX": 3.069034202769489, + "velocityY": 0.0000034860049153566075, + "timestamp": 0.8081473442848974 + }, + { + "x": 5.405613054428805, + "y": 6.424281444650923, + "heading": -0.1455833954142087, + "angularVelocity": -0.5058516892200839, + "velocityX": 3.3053697791645313, + "velocityY": 0.0004816192577199728, + "timestamp": 0.8703125246145049 + }, + { + "x": 5.620478420069855, + "y": 6.42365674720977, + "heading": -0.14578823417625966, + "angularVelocity": -0.003295072273013325, + "velocityX": 3.456361977907994, + "velocityY": -0.0100489926650518, + "timestamp": 0.9324777049441124 + }, + { + "x": 5.8354039344878705, + "y": 6.423689815718493, + "heading": -0.1457882693852108, + "angularVelocity": -5.663773666898463e-7, + "velocityX": 3.4573295416896364, + "velocityY": 0.0005319458344337396, + "timestamp": 0.9946428852737199 + }, + { + "x": 6.050013195110809, + "y": 6.4120346435837465, + "heading": -0.14578830470722368, + "angularVelocity": -5.68196097923049e-7, + "velocityX": 3.4522422276434255, + "velocityY": -0.1874871442976483, + "timestamp": 1.0568080656033274 + }, + { + "x": 6.263319492340088, + "y": 6.385702133178711, + "heading": -0.14578834092456064, + "angularVelocity": -5.825984377491133e-7, + "velocityX": 3.4312825298390877, + "velocityY": -0.42358938340430125, + "timestamp": 1.118973245932935 + }, + { + "x": 6.49569885410855, + "y": 6.338969209681391, + "heading": -0.14578837085914265, + "angularVelocity": -4.366235413031805e-7, + "velocityX": 3.389467737301255, + "velocityY": -0.6816428759355991, + "timestamp": 1.1875324946937704 + }, + { + "x": 6.723883016534425, + "y": 6.274812198093867, + "heading": -0.14578840022622835, + "angularVelocity": -4.28346084046259e-7, + "velocityX": 3.328276878031172, + "velocityY": -0.9357893026414394, + "timestamp": 1.256091743454606 + }, + { + "x": 6.944379070760873, + "y": 6.204658250666653, + "heading": -0.16522808025100505, + "angularVelocity": -0.28354569771601223, + "velocityX": 3.2161387152247607, + "velocityY": -1.023260153738583, + "timestamp": 1.3246509922154415 + }, + { + "x": 7.148638930902927, + "y": 6.139545447418234, + "heading": -0.19715028206546042, + "angularVelocity": -0.465614813339248, + "velocityX": 2.979318820347672, + "velocityY": -0.949730407279711, + "timestamp": 1.393210240976277 + }, + { + "x": 7.3358170328874195, + "y": 6.0798421831912135, + "heading": -0.22994658912328278, + "angularVelocity": -0.4783644460899831, + "velocityX": 2.7301655920625914, + "velocityY": -0.870827281601222, + "timestamp": 1.4617694897371125 + }, + { + "x": 7.5059237430887356, + "y": 6.025569482278596, + "heading": -0.26160545834967963, + "angularVelocity": -0.4617738641920779, + "velocityX": 2.481163566927677, + "velocityY": -0.7916174971803536, + "timestamp": 1.530328738497948 + }, + { + "x": 7.658979831941821, + "y": 5.976731413281962, + "heading": -0.291240609478123, + "angularVelocity": -0.43225606557947815, + "velocityX": 2.232464497780769, + "velocityY": -0.7123483684455512, + "timestamp": 1.5988879872587836 + }, + { + "x": 7.795001086268662, + "y": 5.933327606851567, + "heading": -0.3183467386103757, + "angularVelocity": -0.39536794265075353, + "velocityX": 1.9839956940214167, + "velocityY": -0.633084627018105, + "timestamp": 1.667447236019619 + }, + { + "x": 7.91399908186447, + "y": 5.895356201186841, + "heading": -0.34259459104051004, + "angularVelocity": -0.3536773355659347, + "velocityX": 1.7356957339326504, + "velocityY": -0.5538480416724806, + "timestamp": 1.7360064847804546 + }, + { + "x": 8.015982502141213, + "y": 5.862814866579216, + "heading": -0.36375111540905714, + "angularVelocity": -0.3085874590363489, + "velocityX": 1.4875224294318823, + "velocityY": -0.4746454372792861, + "timestamp": 1.8045657335412901 + }, + { + "x": 8.100958056756967, + "y": 5.835701248475415, + "heading": -0.3816420491863885, + "angularVelocity": -0.26095580247302225, + "velocityX": 1.2394469914947026, + "velocityY": -0.39547717622147305, + "timestamp": 1.8731249823021257 + }, + { + "x": 8.168931078031942, + "y": 5.814013189562108, + "heading": -0.3961322011943633, + "angularVelocity": -0.211352257643937, + "velocityX": 0.9914493303754669, + "velocityY": -0.31634038157220884, + "timestamp": 1.9416842310629612 + }, + { + "x": 8.219905912133822, + "y": 5.797748853681938, + "heading": -0.4071141507461263, + "angularVelocity": -0.16018188282769613, + "velocityX": 0.7435150621282579, + "velocityY": -0.237230368974835, + "timestamp": 2.0102434798237967 + }, + { + "x": 8.253886183057102, + "y": 5.786906800693844, + "heading": -0.4145013188137657, + "angularVelocity": -0.10774867288013994, + "velocityX": 0.49563365318977964, + "velocityY": -0.1581413621656758, + "timestamp": 2.078802728584632 + }, + { + "x": 8.270874977111816, + "y": 5.7814860343933105, + "heading": -0.4182234468320189, + "angularVelocity": -0.05429067683103692, + "velocityX": 0.2477972609352767, + "velocityY": -0.07906688591998655, + "timestamp": 2.1473619773454677 + }, + { + "x": 8.270874977111816, + "y": 5.7814860343933105, + "heading": -0.4182234468320189, + "angularVelocity": 1.2149869731922202e-24, + "velocityX": 1.6200325032236621e-24, + "velocityY": -9.81678651755837e-25, + "timestamp": 2.2159212261063024 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3PieceAmpSide.4.traj b/src/main/deploy/choreo/3PieceAmpSide.4.traj new file mode 100644 index 0000000..db1d01f --- /dev/null +++ b/src/main/deploy/choreo/3PieceAmpSide.4.traj @@ -0,0 +1,320 @@ +{ + "samples": [ + { + "x": 8.270874977111816, + "y": 5.7814860343933105, + "heading": -0.4182234468320189, + "angularVelocity": 1.2149869731922202e-24, + "velocityX": 1.6200325032236621e-24, + "velocityY": -9.81678651755837e-25, + "timestamp": 0 + }, + { + "x": 8.255885275596246, + "y": 5.786297415399053, + "heading": -0.4161957197256105, + "angularVelocity": 0.03152694238731417, + "velocityX": -0.233058706268173, + "velocityY": 0.07480697540219873, + "timestamp": 0.06431727763185613 + }, + { + "x": 8.225904877332317, + "y": 5.795920588198962, + "heading": -0.4121767719087599, + "angularVelocity": 0.06248628618665337, + "velocityX": -0.46613288633782896, + "velocityY": 0.14962033771067135, + "timestamp": 0.12863455526371226 + }, + { + "x": 8.180932637466617, + "y": 5.810356062930585, + "heading": -0.4062099213750387, + "angularVelocity": 0.09277212521143607, + "velocityX": -0.6992248664987879, + "velocityY": 0.22444163159780386, + "timestamp": 0.1929518328955684 + }, + { + "x": 8.120967221795235, + "y": 5.8296044401602, + "heading": -0.3983469021562322, + "angularVelocity": 0.12225360755804271, + "velocityX": -0.9323375907577511, + "velocityY": 0.29927226304244037, + "timestamp": 0.2572691105274245 + }, + { + "x": 8.046007053762992, + "y": 5.853666398213901, + "heading": -0.38865015214844967, + "angularVelocity": 0.15076431038150498, + "velocityX": -1.16547482717328, + "velocityY": 0.3741134410481226, + "timestamp": 0.32158638815928065 + }, + { + "x": 7.95605024235754, + "y": 5.882542674483832, + "heading": -0.37719620256520386, + "angularVelocity": 0.17808511188559348, + "velocityX": -1.398641464900824, + "velocityY": 0.44896608396916854, + "timestamp": 0.3859036657911368 + }, + { + "x": 7.851094482525453, + "y": 5.916234036378714, + "heading": -0.3640809123745278, + "angularVelocity": 0.20391550565536098, + "velocityX": -1.6318439414186696, + "velocityY": 0.5238306585009168, + "timestamp": 0.4502209434229929 + }, + { + "x": 7.731136916910522, + "y": 5.954741232958676, + "heading": -0.349427957292727, + "angularVelocity": 0.22782299906523523, + "velocityX": -1.8650908438872864, + "velocityY": 0.5987068793609814, + "timestamp": 0.514538221054849 + }, + { + "x": 7.596173948111591, + "y": 5.998064906716331, + "heading": -0.33340347371338613, + "angularVelocity": 0.24914741682729602, + "velocityX": -2.0983936784675863, + "velocityY": 0.6735930896459054, + "timestamp": 0.5788554986867052 + }, + { + "x": 7.446201013798343, + "y": 6.046205411855825, + "heading": -0.31624349903105253, + "angularVelocity": 0.26680194364810067, + "velocityX": -2.331767447802665, + "velocityY": 0.7484848070691726, + "timestamp": 0.6431727763185613 + }, + { + "x": 7.281212494788232, + "y": 6.099162371697227, + "heading": -0.2983118341454104, + "angularVelocity": 0.27880012254686504, + "velocityX": -2.56522858374831, + "velocityY": 0.82337066790236, + "timestamp": 0.7074900539504174 + }, + { + "x": 7.101203008664871, + "y": 6.1569332933758485, + "heading": -0.28024668313688234, + "angularVelocity": 0.280875554340636, + "velocityX": -2.7987734050827355, + "velocityY": 0.8982177698703966, + "timestamp": 0.7718073315822735 + }, + { + "x": 6.9061821193945345, + "y": 6.219506729950283, + "heading": -0.26348073873693967, + "angularVelocity": 0.26067559164908854, + "velocityX": -3.0321695266179045, + "velocityY": 0.9728868956891804, + "timestamp": 0.8361246092141297 + }, + { + "x": 6.696607254404851, + "y": 6.286709539594328, + "heading": -0.25518312367209683, + "angularVelocity": 0.12901066976648753, + "velocityX": -3.258453602300528, + "velocityY": 1.0448640259419284, + "timestamp": 0.9004418868459858 + }, + { + "x": 6.4817199239739445, + "y": 6.3438942094131185, + "heading": -0.2551831038227718, + "angularVelocity": 3.086157524298735e-7, + "velocityX": -3.3410513992973323, + "velocityY": 0.8891027718260752, + "timestamp": 0.9647591644778419 + }, + { + "x": 6.263319492340088, + "y": 6.385702133178711, + "heading": -0.2551830834264048, + "angularVelocity": 3.1712111825091784e-7, + "velocityX": -3.395672821911929, + "velocityY": 0.6500263273718765, + "timestamp": 1.029076442109698 + }, + { + "x": 6.037162237236338, + "y": 6.412166410916887, + "heading": -0.25518303828533845, + "angularVelocity": 6.854074958240057e-7, + "velocityX": -3.4338993409830967, + "velocityY": 0.4018251187344374, + "timestamp": 1.0949366297991254 + }, + { + "x": 5.8096806134973695, + "y": 6.422145044657871, + "heading": -0.2551829940595105, + "angularVelocity": 6.715108093540807e-7, + "velocityX": -3.4540081302483707, + "velocityY": 0.15151237934575548, + "timestamp": 1.1607968174885528 + }, + { + "x": 5.5819802469231306, + "y": 6.422071707013465, + "heading": -0.25518294988305257, + "angularVelocity": 6.707611912611386e-7, + "velocityX": -3.457329451412868, + "velocityY": -0.0011135353083337968, + "timestamp": 1.2266570051779802 + }, + { + "x": 5.357973256918571, + "y": 6.423240614393437, + "heading": -0.2430314751768064, + "angularVelocity": 0.18450410077098406, + "velocityX": -3.4012504042790286, + "velocityY": 0.017748315347736767, + "timestamp": 1.2925171928674075 + }, + { + "x": 5.144457161392198, + "y": 6.423376601690841, + "heading": -0.1958139855508891, + "angularVelocity": 0.7169352424043682, + "velocityX": -3.241960021936682, + "velocityY": 0.0020647875776692963, + "timestamp": 1.3583773805568349 + }, + { + "x": 4.947462840354243, + "y": 6.423510104020051, + "heading": -0.14701489396398695, + "angularVelocity": 0.7409497801163326, + "velocityX": -2.991098688739039, + "velocityY": 0.002027056616322834, + "timestamp": 1.4242375682462622 + }, + { + "x": 4.766993522305038, + "y": 6.423644398305699, + "heading": -0.09964051366094051, + "angularVelocity": 0.719317420206062, + "velocityX": -2.74018833502617, + "velocityY": 0.002039081429309675, + "timestamp": 1.4900977559356896 + }, + { + "x": 4.60301308935793, + "y": 6.423783110988961, + "heading": -0.05492917751371649, + "angularVelocity": 0.6788826105091881, + "velocityX": -2.4898263837385226, + "velocityY": 0.0021061689638151486, + "timestamp": 1.555957943625117 + }, + { + "x": 4.455491999512653, + "y": 6.4239249853365035, + "heading": -0.013574200580991685, + "angularVelocity": 0.6279207269760504, + "velocityX": -2.2399129887228693, + "velocityY": 0.0021541746618076994, + "timestamp": 1.6218181313145443 + }, + { + "x": 4.324407720523135, + "y": 6.424066362703545, + "heading": 0.023974314912572615, + "angularVelocity": 0.570124635396258, + "velocityX": -1.9903417161162953, + "velocityY": 0.0021466286690262756, + "timestamp": 1.6876783190039717 + }, + { + "x": 4.2097428344188135, + "y": 6.424202667515828, + "heading": 0.05739701354466356, + "angularVelocity": 0.5074795533486763, + "velocityX": -1.7410349123971218, + "velocityY": 0.002069608621913246, + "timestamp": 1.753538506693399 + }, + { + "x": 4.11148358029628, + "y": 6.424329194128387, + "heading": 0.08645394614427561, + "angularVelocity": 0.4411911599255325, + "velocityX": -1.4919370498287492, + "velocityY": 0.001921139568510551, + "timestamp": 1.8193986943828264 + }, + { + "x": 4.029618879443776, + "y": 6.4244415432031925, + "heading": 0.11095800828046558, + "angularVelocity": 0.3720618327378889, + "velocityX": -1.2430074028720826, + "velocityY": 0.00170587237520769, + "timestamp": 1.8852588820722538 + }, + { + "x": 3.964139686055167, + "y": 6.424535880555734, + "heading": 0.13075995126873372, + "angularVelocity": 0.3006663613175029, + "velocityX": -0.994215104539091, + "velocityY": 0.0014323881520982617, + "timestamp": 1.9511190697616811 + }, + { + "x": 3.91503854489554, + "y": 6.424609101274365, + "heading": 0.14573933177166642, + "angularVelocity": 0.22744211683042614, + "velocityX": -0.7455360041056982, + "velocityY": 0.0011117599448221096, + "timestamp": 2.0169792574511085 + }, + { + "x": 3.882309280386553, + "y": 6.424658939920723, + "heading": 0.15579869857714623, + "angularVelocity": 0.1527382043445731, + "velocityX": -0.496950671676277, + "velocityY": 0.000756734046866559, + "timestamp": 2.082839445140536 + }, + { + "x": 3.865946769714355, + "y": 6.424684047698975, + "heading": 0.1608595979864649, + "angularVelocity": 0.07684307602012863, + "velocityX": -0.2484431224119383, + "velocityY": 0.0003812284649235306, + "timestamp": 2.1486996328299632 + }, + { + "x": 3.865946769714355, + "y": 6.424684047698975, + "heading": 0.1608595979864649, + "angularVelocity": 3.773099570003496e-26, + "velocityX": -4.36512163919617e-27, + "velocityY": 1.07464594312406e-25, + "timestamp": 2.2145598205193906 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3PieceAmpSide.traj b/src/main/deploy/choreo/3PieceAmpSide.traj new file mode 100644 index 0000000..8ba9362 --- /dev/null +++ b/src/main/deploy/choreo/3PieceAmpSide.traj @@ -0,0 +1,1418 @@ +{ + "samples": [ + { + "x": 0.660783052444458, + "y": 6.694385051727295, + "heading": 1.0498621745809171, + "angularVelocity": 5.036142496978594e-25, + "velocityX": 4.0909648611784826e-25, + "velocityY": -1.2650073502903171e-25, + "timestamp": 0 + }, + { + "x": 0.6757749919218088, + "y": 6.703069104575789, + "heading": 1.049862128706329, + "angularVelocity": -6.805633230382436e-7, + "velocityX": 0.2224099351379215, + "velocityY": 0.12883053814923912, + "timestamp": 0.06740678858637275 + }, + { + "x": 0.705758870441161, + "y": 6.720437210067111, + "heading": 1.0498620338020643, + "angularVelocity": -0.0000014079333376652512, + "velocityX": 0.4448198638173059, + "velocityY": 0.25766107324734056, + "timestamp": 0.1348135771727455 + }, + { + "x": 0.750734687393026, + "y": 6.746489367913325, + "heading": 1.049861885451002, + "angularVelocity": -0.000002200832664302106, + "velocityX": 0.6672297834547413, + "velocityY": 0.3864916040738485, + "timestamp": 0.20222036575911825 + }, + { + "x": 0.8107024418631711, + "y": 6.781225577682529, + "heading": 1.0498616770274682, + "angularVelocity": -0.000003092025867532283, + "velocityX": 0.8896396895292586, + "velocityY": 0.5153221284929648, + "timestamp": 0.269627154345491 + }, + { + "x": 0.8856621323278759, + "y": 6.82464583865489, + "heading": 1.0498613974886852, + "angularVelocity": -0.0000041470419969153815, + "velocityX": 1.1120495729989293, + "velocityY": 0.6441526422330909, + "timestamp": 0.33703394293186373 + }, + { + "x": 0.9756137557397047, + "y": 6.8767501493907295, + "heading": 1.0498610247491444, + "angularVelocity": -0.000005529703292486692, + "velocityX": 1.334459411258968, + "velocityY": 0.7729831346152221, + "timestamp": 0.40444073151823645 + }, + { + "x": 1.0805573029563829, + "y": 6.937538505571021, + "heading": 1.049860492552681, + "angularVelocity": -0.000007895294742633066, + "velocityX": 1.556869113890614, + "velocityY": 0.9018135629232547, + "timestamp": 0.4718475201046092 + }, + { + "x": 1.1972133032133725, + "y": 7.005111257740716, + "heading": 1.0334106168225663, + "angularVelocity": -0.2440388583271016, + "velocityX": 1.7306268805182825, + "velocityY": 1.0024621197182535, + "timestamp": 0.5392543086909819 + }, + { + "x": 1.3138692969010544, + "y": 7.072684021251592, + "heading": 0.9724418882550508, + "angularVelocity": -0.9044894415848371, + "velocityX": 1.7306267830606272, + "velocityY": 1.0024622879680827, + "timestamp": 0.6066610972773546 + }, + { + "x": 1.4305252906191748, + "y": 7.140256784709921, + "heading": 0.8706420088958982, + "angularVelocity": -1.5102318548926246, + "velocityX": 1.7306267835121878, + "velocityY": 1.0024622871885176, + "timestamp": 0.6740678858637273 + }, + { + "x": 1.5471812843420916, + "y": 7.207829548159968, + "heading": 0.7338773790646791, + "angularVelocity": -2.028944453509658, + "velocityX": 1.7306267835833422, + "velocityY": 1.0024622870656754, + "timestamp": 0.7414746744501001 + }, + { + "x": 1.6638372780224426, + "y": 7.2754023116835, + "heading": 0.5723600050127189, + "angularVelocity": -2.396158865289915, + "velocityX": 1.7306267829518702, + "velocityY": 1.002462288155831, + "timestamp": 0.8088814630364728 + }, + { + "x": 1.780493271638965, + "y": 7.3429750753172245, + "heading": 0.40418447555722015, + "angularVelocity": -2.4949346049916663, + "velocityX": 1.7306267820049508, + "velocityY": 1.0024622897905686, + "timestamp": 0.8762882516228455 + }, + { + "x": 1.8971492652070823, + "y": 7.410547839034513, + "heading": 0.25129496131963275, + "angularVelocity": -2.26816196771754, + "velocityX": 1.7306267812868497, + "velocityY": 1.0024622910302834, + "timestamp": 0.9436950402092182 + }, + { + "x": 2.013805258779043, + "y": 7.478120602745167, + "heading": 0.12816045902008172, + "angularVelocity": -1.8267374085292716, + "velocityX": 1.7306267813438676, + "velocityY": 1.0024622909318521, + "timestamp": 1.011101828795591 + }, + { + "x": 2.1304612523991975, + "y": 7.545693366372621, + "heading": 0.04280584220849794, + "angularVelocity": -1.266261434517286, + "velocityX": 1.7306267820588324, + "velocityY": 1.0024622896975566, + "timestamp": 1.0785086173819638 + }, + { + "x": 2.247117388567763, + "y": 7.613265883907332, + "heading": 0.000013466426965438275, + "angularVelocity": -0.6348377764162405, + "velocityX": 1.7306288968074217, + "velocityY": 1.002458638837633, + "timestamp": 1.1459154059683365 + }, + { + "x": 2.365737851453118, + "y": 7.677327455956403, + "heading": 0.000004459976492377945, + "angularVelocity": -0.00013361340396034107, + "velocityX": 1.759770274968055, + "velocityY": 0.9503727056657639, + "timestamp": 1.2133221945547092 + }, + { + "x": 2.491594313865089, + "y": 7.725647742212638, + "heading": 0.000001984433687850704, + "angularVelocity": -0.000036725422712508624, + "velocityX": 1.8671185061828453, + "velocityY": 0.7168459923634953, + "timestamp": 1.280728983141082 + }, + { + "x": 2.622608441209736, + "y": 7.7574282007435205, + "heading": 7.92136305089401e-7, + "angularVelocity": -0.00001768809058799076, + "velocityX": 1.9436340180599136, + "velocityY": 0.4714726691089775, + "timestamp": 1.3481357717274547 + }, + { + "x": 2.7566164536261604, + "y": 7.772143891629413, + "heading": 2.0780523669770732e-7, + "angularVelocity": -0.000008668727299520462, + "velocityX": 1.9880492043425424, + "velocityY": 0.2183117041251145, + "timestamp": 1.4155425603138274 + }, + { + "x": 2.8914051055908203, + "y": 7.769551753997803, + "heading": -8.09802518102299e-24, + "angularVelocity": -0.000003082853241575703, + "velocityX": 1.9996302270348674, + "velocityY": -0.03845514207057226, + "timestamp": 1.4829493489002001 + }, + { + "x": 2.9864482996127166, + "y": 7.767723966300088, + "heading": -1.0517201217515276e-7, + "angularVelocity": -0.0000023977731499342787, + "velocityX": 2.1668504195789944, + "velocityY": -0.04167097476524414, + "timestamp": 1.526811718523463 + }, + { + "x": 3.0888261669051587, + "y": 7.76575512457206, + "heading": -1.802948176200958e-7, + "angularVelocity": -0.0000017126937301878197, + "velocityX": 2.3340705979128114, + "velocityY": -0.04488680718663573, + "timestamp": 1.570674088146726 + }, + { + "x": 3.1985387066370827, + "y": 7.7636452288297, + "heading": -2.2536845598951497e-7, + "angularVelocity": -0.0000010276152145121185, + "velocityX": 2.5012907572995546, + "velocityY": -0.04810263924365375, + "timestamp": 1.614536457769989 + }, + { + "x": 3.315585917645001, + "y": 7.761394279095385, + "heading": -2.4039298339678027e-7, + "angularVelocity": -3.4253797814190234e-7, + "velocityX": 2.668510890160408, + "velocityY": -0.05131847079054902, + "timestamp": 1.658398827393252 + }, + { + "x": 3.4399677981836816, + "y": 7.759002275402676, + "heading": -2.2536848500409826e-7, + "angularVelocity": 3.4253731665043406e-7, + "velocityX": 2.835730983232458, + "velocityY": -0.05453430157226078, + "timestamp": 1.702261197016515 + }, + { + "x": 3.5716843453444107, + "y": 7.756469217807512, + "heading": -1.8029510454250157e-7, + "angularVelocity": 0.0000010276093345784838, + "velocityX": 3.0029510099899284, + "velocityY": -0.057750131078668464, + "timestamp": 1.7461235666397779 + }, + { + "x": 3.7107355533097754, + "y": 7.753795106421767, + "heading": -1.0517313285100412e-7, + "angularVelocity": 0.0000017126747217883014, + "velocityX": 3.1701709041186272, + "velocityY": -0.06096595803447545, + "timestamp": 1.7899859362630408 + }, + { + "x": 3.8571214046276947, + "y": 7.750979941581066, + "heading": -3.445312029122533e-12, + "angularVelocity": 0.000002397720151516781, + "velocityX": 3.337390400364549, + "velocityY": -0.06418177733854838, + "timestamp": 1.8338483058863038 + }, + { + "x": 4.008740030563312, + "y": 7.748064144589167, + "heading": -2.953693080850903e-12, + "angularVelocity": 1.1208216804044482e-11, + "velocityX": 3.456690261786607, + "velocityY": -0.06647604807818788, + "timestamp": 1.8777106755095667 + }, + { + "x": 4.160358656499413, + "y": 7.745148347597258, + "heading": -2.46204265095573e-12, + "angularVelocity": 1.1208934540541933e-11, + "velocityX": 3.4566902617976116, + "velocityY": -0.06647604807839952, + "timestamp": 1.9215730451328297 + }, + { + "x": 4.311977282435513, + "y": 7.742232550605349, + "heading": -1.970393895641841e-12, + "angularVelocity": 1.1208896362456744e-11, + "velocityX": 3.4566902617976116, + "velocityY": -0.06647604807839952, + "timestamp": 1.9654354147560926 + }, + { + "x": 4.463595908371613, + "y": 7.739316753613441, + "heading": -1.478772239262654e-12, + "angularVelocity": 1.1208278545067146e-11, + "velocityX": 3.456690261797612, + "velocityY": -0.06647604807839953, + "timestamp": 2.0092977843793554 + }, + { + "x": 4.615214534307714, + "y": 7.736400956621532, + "heading": -9.871132742331443e-13, + "angularVelocity": 1.1209129129534961e-11, + "velocityX": 3.4566902617976116, + "velocityY": -0.06647604807839952, + "timestamp": 2.0531601540026183 + }, + { + "x": 4.766833160243814, + "y": 7.733485159629623, + "heading": -4.955367585470695e-13, + "angularVelocity": 1.120724940109933e-11, + "velocityX": 3.456690261797612, + "velocityY": -0.06647604807839952, + "timestamp": 2.0970225236258813 + }, + { + "x": 4.91845178604126, + "y": 7.730569362640381, + "heading": 5.035858981265476e-24, + "angularVelocity": 1.1297537338117902e-11, + "velocityX": 3.4566902586364923, + "velocityY": -0.06647604801760763, + "timestamp": 2.140884893249144 + }, + { + "x": 5.07168754119456, + "y": 7.72009798657163, + "heading": -1.2237883220746723e-17, + "angularVelocity": -2.754707452373014e-16, + "velocityX": 3.4492852675356476, + "velocityY": -0.23570715051872318, + "timestamp": 2.1853102577139336 + }, + { + "x": 5.224226523742201, + "y": 7.702133987621307, + "heading": -3.220999671007562e-17, + "angularVelocity": -4.4956555179430785e-16, + "velocityX": 3.433601150724158, + "velocityY": -0.4043635694775183, + "timestamp": 2.229735622178723 + }, + { + "x": 5.375704105653677, + "y": 7.6767294867936675, + "heading": -8.086584933587513e-18, + "angularVelocity": 5.430098788639561e-16, + "velocityX": 3.4097093796840374, + "velocityY": -0.5718467621751168, + "timestamp": 2.2741609866435124 + }, + { + "x": 5.527091031475673, + "y": 7.65079022745198, + "heading": 1.237593714278334e-17, + "angularVelocity": 4.606044840123949e-16, + "velocityX": 3.4076687416257836, + "velocityY": -0.5838839963203132, + "timestamp": 2.318586351108302 + }, + { + "x": 5.678477940102667, + "y": 7.624850867756873, + "heading": -5.299489627912929e-18, + "angularVelocity": -3.9786790685233107e-16, + "velocityX": 3.4076683545720687, + "velocityY": -0.5838862552420002, + "timestamp": 2.363011715573091 + }, + { + "x": 5.82986484872645, + "y": 7.598911508043026, + "heading": -6.861733443062272e-18, + "angularVelocity": -3.51655824113109e-17, + "velocityX": 3.4076683544997888, + "velocityY": -0.5838862556638376, + "timestamp": 2.4074370800378806 + }, + { + "x": 5.981251757350232, + "y": 7.572972148329175, + "heading": 1.300275398298883e-18, + "angularVelocity": 1.8372407158846258e-16, + "velocityX": 3.4076683544997755, + "velocityY": -0.5838862556639163, + "timestamp": 2.45186244450267 + }, + { + "x": 6.13263866597402, + "y": 7.547032788615358, + "heading": 2.03304825021732e-17, + "angularVelocity": 4.2836355611375324e-16, + "velocityX": 3.407668354499907, + "velocityY": -0.5838862556631488, + "timestamp": 2.4962878089674594 + }, + { + "x": 6.284025574629089, + "y": 7.521093429084101, + "heading": 9.540525391081043e-18, + "angularVelocity": -2.428783025456181e-16, + "velocityX": 3.407668355204027, + "velocityY": -0.5838862515537734, + "timestamp": 2.540713173432249 + }, + { + "x": 6.435412650788291, + "y": 7.495155047156986, + "heading": 3.1121659748942844e-18, + "angularVelocity": -1.4470020659857382e-16, + "velocityX": 3.407672125665691, + "velocityY": -0.5838642460136346, + "timestamp": 2.585138537897038 + }, + { + "x": 6.587408261478287, + "y": 7.473060233149462, + "heading": 4.012412418719246e-18, + "angularVelocity": 2.0264244415020296e-17, + "velocityX": 3.4213700331139205, + "velocityY": -0.49734682593400187, + "timestamp": 2.6295639023618276 + }, + { + "x": 6.740303543651052, + "y": 7.458435646449454, + "heading": -2.1084051336842118e-18, + "angularVelocity": -1.377775427669848e-16, + "velocityX": 3.4416213353510248, + "velocityY": -0.3291945238085493, + "timestamp": 2.673989266826617 + }, + { + "x": 6.890285316295548, + "y": 7.450749433084636, + "heading": -1.4822341121773416e-18, + "angularVelocity": 1.40948989157144e-17, + "velocityX": 3.3760392165914315, + "velocityY": -0.17301407557185516, + "timestamp": 2.7184146312914064 + }, + { + "x": 7.032993253879116, + "y": 7.444993241304835, + "heading": -9.121457386741735e-19, + "angularVelocity": 1.2832497387232803e-17, + "velocityX": 3.212307637828718, + "velocityY": -0.1295699393611814, + "timestamp": 2.7628399957561958 + }, + { + "x": 7.168350422821424, + "y": 7.440849530732545, + "heading": -4.3340581409719474e-19, + "angularVelocity": 1.0776274550871163e-17, + "velocityX": 3.046844310069511, + "velocityY": -0.09327353016031785, + "timestamp": 2.807265360220985 + }, + { + "x": 7.296333312988281, + "y": 7.438207149505615, + "heading": -1.2424191874750213e-28, + "angularVelocity": 9.755818981171634e-18, + "velocityX": 2.880851777103443, + "velocityY": -0.05947911196145459, + "timestamp": 2.8516907246857746 + }, + { + "x": 7.476987619873736, + "y": 7.437582893575528, + "heading": 6.306246828045357e-19, + "angularVelocity": 9.156773444569897e-18, + "velocityX": 2.623130055966497, + "velocityY": -0.009064298112001053, + "timestamp": 2.9205604730516797 + }, + { + "x": 7.639661681076791, + "y": 7.438908313736393, + "heading": 7.717197233455447e-19, + "angularVelocity": 2.04872304504038e-18, + "velocityX": 2.362053950578807, + "velocityY": 0.019245317317299903, + "timestamp": 2.9894302214175847 + }, + { + "x": 7.78428303377631, + "y": 7.4413247257119215, + "heading": 8.357244621441239e-19, + "angularVelocity": 9.29359266725832e-19, + "velocityX": 2.0999256731873923, + "velocityY": 0.0350866967407844, + "timestamp": 3.05829996978349 + }, + { + "x": 7.91082689918953, + "y": 7.4442843222063875, + "heading": 7.220816869988264e-19, + "angularVelocity": -1.6501116644010886e-18, + "velocityX": 1.8374376038212434, + "velocityY": 0.042973824715339615, + "timestamp": 3.127169718149395 + }, + { + "x": 8.019285863587411, + "y": 7.447408189663725, + "heading": 5.712912024628398e-19, + "angularVelocity": -2.1895024749108468e-18, + "velocityX": 1.5748418858979918, + "velocityY": 0.04535906593908385, + "timestamp": 3.1960394665153 + }, + { + "x": 8.109659533807191, + "y": 7.450418969222921, + "heading": 4.341547519848611e-19, + "angularVelocity": -1.9912436692380873e-18, + "velocityX": 1.312240459187152, + "velocityY": 0.04371701117883307, + "timestamp": 3.264909214881205 + }, + { + "x": 8.181950471768632, + "y": 7.4531049914429115, + "heading": 2.769074151229353e-19, + "angularVelocity": -2.283257025771601e-18, + "velocityX": 1.0496762319699213, + "velocityY": 0.03900148154629361, + "timestamp": 3.33377896324711 + }, + { + "x": 8.236162441930201, + "y": 7.455299473800812, + "heading": 8.002361823653196e-20, + "angularVelocity": -2.858784902160653e-18, + "velocityX": 0.7871666653047966, + "velocityY": 0.03186424242820522, + "timestamp": 3.4026487116130153 + }, + { + "x": 8.27229961106123, + "y": 7.456867643223532, + "heading": 9.595707199020635e-21, + "angularVelocity": -1.0226247759898425e-18, + "velocityX": 0.5247176008111963, + "velocityY": 0.02277007626611394, + "timestamp": 3.4715184599789204 + }, + { + "x": 8.290366172790527, + "y": 7.457698345184326, + "heading": -2.737879233354451e-29, + "angularVelocity": -1.3933123630808857e-19, + "velocityX": 0.26232942849316815, + "velocityY": 0.012061928212380262, + "timestamp": 3.5403882083448255 + }, + { + "x": 8.290366172790527, + "y": 7.457698345184326, + "heading": -7.734690041039287e-29, + "angularVelocity": -1.1060203570884526e-28, + "velocityX": -8.232857746308476e-27, + "velocityY": 9.915594313558478e-27, + "timestamp": 3.6092579567107306 + }, + { + "x": 8.258529155042611, + "y": 7.450262882094732, + "heading": 0.0018141437015229199, + "angularVelocity": 0.019588491140668917, + "velocityX": -0.34376501683789845, + "velocityY": -0.0802855378738891, + "timestamp": 3.7018706897944167 + }, + { + "x": 8.19485511722891, + "y": 7.435392003543495, + "heading": 0.005442056461770318, + "angularVelocity": 0.03917293701902922, + "velocityX": -0.6875300587033178, + "velocityY": -0.16057056147775478, + "timestamp": 3.794483422878103 + }, + { + "x": 8.099344042603773, + "y": 7.413085782435483, + "heading": 0.010882224859340402, + "angularVelocity": 0.05874104150078671, + "velocityX": -1.0312952813824383, + "velocityY": -0.24085479788027828, + "timestamp": 3.887096155961789 + }, + { + "x": 7.9719959003584435, + "y": 7.383344317488874, + "heading": 0.018132064338159094, + "angularVelocity": 0.07828123884722897, + "velocityX": -1.3750608367237869, + "velocityY": -0.3211379683583422, + "timestamp": 3.979708889045475 + }, + { + "x": 7.8128106462112585, + "y": 7.346167733877292, + "heading": 0.027188013699287764, + "angularVelocity": 0.09778298360923647, + "velocityX": -1.7188268702030736, + "velocityY": -0.4014197872552755, + "timestamp": 4.072321622129161 + }, + { + "x": 7.621788223292644, + "y": 7.301556183975714, + "heading": 0.038045656619278445, + "angularVelocity": 0.11723704245051889, + "velocityX": -2.0625935177402037, + "velocityY": -0.481699960860308, + "timestamp": 4.164934355212847 + }, + { + "x": 7.3989285635202835, + "y": 7.24950984823631, + "heading": 0.050699870016136385, + "angularVelocity": 0.1366357840387178, + "velocityX": -2.406360900406452, + "velocityY": -0.5619781860057411, + "timestamp": 4.257547088296533 + }, + { + "x": 7.144231590225439, + "y": 7.190028936362276, + "heading": 0.06514499751032955, + "angularVelocity": 0.15597345001297352, + "velocityX": -2.750129110914985, + "velocityY": -0.6422541468491951, + "timestamp": 4.3501598213802195 + }, + { + "x": 6.857697227315765, + "y": 7.123113690113179, + "heading": 0.08137503414778206, + "angularVelocity": 0.17524627658690128, + "velocityX": -3.093898143042135, + "velocityY": -0.7225274972571053, + "timestamp": 4.442772554463906 + }, + { + "x": 6.545887160381392, + "y": 7.050327089705259, + "heading": 0.08137504044908753, + "angularVelocity": 6.803929929105723e-8, + "velocityX": -3.366816382069384, + "velocityY": -0.7859243322637903, + "timestamp": 4.535385287547592 + }, + { + "x": 6.234077092172867, + "y": 6.977540494754869, + "heading": 0.0813750467500712, + "angularVelocity": 6.80358247192299e-8, + "velocityX": -3.3668163958272355, + "velocityY": -0.7859242733352659, + "timestamp": 4.627998020631278 + }, + { + "x": 5.922267023964316, + "y": 6.904753899804592, + "heading": 0.08137505305105486, + "angularVelocity": 6.803582445105601e-8, + "velocityX": -3.3668163958275135, + "velocityY": -0.7859242733340743, + "timestamp": 4.720610753714964 + }, + { + "x": 5.610456955755783, + "y": 6.831967304854232, + "heading": 0.08137505935203854, + "angularVelocity": 6.803582480789322e-8, + "velocityX": -3.366816395827309, + "velocityY": -0.7859242733349515, + "timestamp": 4.81322348679865 + }, + { + "x": 5.298646888485436, + "y": 6.759180705885576, + "heading": 0.08137506565333762, + "angularVelocity": 6.803923039402274e-8, + "velocityX": -3.366816385697099, + "velocityY": -0.7859243167231056, + "timestamp": 4.905836219882336 + }, + { + "x": 5.012107236392116, + "y": 6.692280236782266, + "heading": 0.09728577795318676, + "angularVelocity": 0.17179832373019394, + "velocityX": -3.0939552538029402, + "velocityY": -0.7223679387893466, + "timestamp": 4.998448952966022 + }, + { + "x": 4.757405139637452, + "y": 6.632813653440834, + "heading": 0.1114212737800766, + "angularVelocity": 0.15263015523056464, + "velocityX": -2.75018443224769, + "velocityY": -0.642099432350162, + "timestamp": 5.091061686049708 + }, + { + "x": 4.534540680564304, + "y": 6.580780772308004, + "heading": 0.12378499174610678, + "angularVelocity": 0.1334991156654254, + "velocityX": -2.4064127215829467, + "velocityY": -0.561832907855257, + "timestamp": 5.1836744191333946 + }, + { + "x": 4.343513918608397, + "y": 6.536181432644826, + "heading": 0.13437950942576468, + "angularVelocity": 0.11439590785086255, + "velocityX": -2.0626403691519735, + "velocityY": -0.4815681189634867, + "timestamp": 5.276287152217081 + }, + { + "x": 4.184324899185163, + "y": 6.499015499114029, + "heading": 0.14320667584584268, + "angularVelocity": 0.0953126651828929, + "velocityX": -1.7188675263409952, + "velocityY": -0.4013047913963752, + "timestamp": 5.368899885300767 + }, + { + "x": 4.056973656080411, + "y": 6.469282862636605, + "heading": 0.15026773537457858, + "angularVelocity": 0.07624285876927316, + "velocityX": -1.3750943187226343, + "velocityY": -0.32104264162636753, + "timestamp": 5.461512618384453 + }, + { + "x": 3.9614602126260823, + "y": 6.446983440876931, + "heading": 0.1555634289688374, + "angularVelocity": 0.05718105294952869, + "velocityX": -1.0313208591740977, + "velocityY": -0.24078138088770026, + "timestamp": 5.554125351468139 + }, + { + "x": 3.8977845824113757, + "y": 6.432117178562742, + "heading": 0.15909407056957425, + "angularVelocity": 0.03812263695475346, + "velocityX": -0.6875472528941416, + "velocityY": -0.1605207169596795, + "timestamp": 5.646738084551825 + }, + { + "x": 3.865946769714355, + "y": 6.424684047698975, + "heading": 0.1608595979864649, + "angularVelocity": 0.019063549450541617, + "velocityX": -0.3437736004211391, + "velocityY": -0.08026035531260044, + "timestamp": 5.739350817635511 + }, + { + "x": 3.865946769714355, + "y": 6.424684047698975, + "heading": 0.1608595979864649, + "angularVelocity": -9.999505736171943e-25, + "velocityX": -5.156111906167998e-25, + "velocityY": -7.69774068808886e-24, + "timestamp": 5.831963550719197 + }, + { + "x": 3.8805908200661583, + "y": 6.424671235221358, + "heading": 0.15744018373663873, + "angularVelocity": -0.0550052976874839, + "velocityX": 0.23556676380826952, + "velocityY": -0.00020610376336075096, + "timestamp": 5.894128731048805 + }, + { + "x": 3.9098813602559557, + "y": 6.424646814464556, + "heading": 0.1506499345999589, + "angularVelocity": -0.10922913921711586, + "velocityX": 0.47117276961950644, + "velocityY": -0.0003928365794473002, + "timestamp": 5.956293911378412 + }, + { + "x": 3.9538212666067056, + "y": 6.424612381558881, + "heading": 0.14054735001329138, + "angularVelocity": -0.1625119485393969, + "velocityX": 0.7068250444666143, + "velocityY": -0.0005538937632522526, + "timestamp": 6.01845909170802 + }, + { + "x": 4.012413923406757, + "y": 6.424569899192806, + "heading": 0.1272024405085879, + "angularVelocity": -0.21466855615872318, + "velocityX": 0.9425317595699895, + "velocityY": -0.0006833787957962723, + "timestamp": 6.080624272037627 + }, + { + "x": 4.085663316044443, + "y": 6.4245216623134365, + "heading": 0.11069894640828985, + "angularVelocity": -0.2654781022558683, + "velocityX": 1.1783025843295045, + "velocityY": -0.0007759469065193845, + "timestamp": 6.142789452367235 + }, + { + "x": 4.173574156900502, + "y": 6.4244702509624405, + "heading": 0.09113759111571623, + "angularVelocity": -0.3146673940115171, + "velocityX": 1.414149213272517, + "velocityY": -0.0008270120141812696, + "timestamp": 6.204954632696842 + }, + { + "x": 4.276152059818016, + "y": 6.424418464281491, + "heading": 0.0686410579111913, + "angularVelocity": -0.36188318099047606, + "velocityX": 1.650086147480532, + "velocityY": -0.0008330496376791236, + "timestamp": 6.26711981302645 + }, + { + "x": 4.39340378630487, + "y": 6.424369226491044, + "heading": 0.04336192483995491, + "angularVelocity": -0.40664457075815247, + "velocityX": 1.886131848490915, + "velocityY": -0.0007920477377600693, + "timestamp": 6.329284993356057 + }, + { + "x": 4.525337595489836, + "y": 6.424325450771935, + "heading": 0.015495928724554285, + "angularVelocity": -0.4482573036489503, + "velocityX": 2.122310407296116, + "velocityY": -0.000704183899678652, + "timestamp": 6.391450173685665 + }, + { + "x": 4.671963731284715, + "y": 6.424289841531667, + "heading": -0.014694474575960468, + "angularVelocity": -0.4856481255333208, + "velocityX": 2.3586537514642383, + "velocityY": -0.0005728164879388929, + "timestamp": 6.453615354015272 + }, + { + "x": 4.833295019214238, + "y": 6.424264622086766, + "heading": -0.04683458533303072, + "angularVelocity": -0.5170114618289433, + "velocityX": 2.595203409273861, + "velocityY": -0.00040568441636908034, + "timestamp": 6.51578053434488 + }, + { + "x": 5.009347081398109, + "y": 6.424251287994793, + "heading": -0.08033775965322806, + "angularVelocity": -0.5389379415061509, + "velocityX": 2.832004367242584, + "velocityY": -0.00021449454344913397, + "timestamp": 6.577945714674487 + }, + { + "x": 5.200134146051008, + "y": 6.424251504702917, + "heading": -0.1141370339338056, + "angularVelocity": -0.5437010574306965, + "velocityX": 3.069034202769489, + "velocityY": 0.0000034860049153566075, + "timestamp": 6.640110895004095 + }, + { + "x": 5.405613054428805, + "y": 6.424281444650923, + "heading": -0.1455833954142087, + "angularVelocity": -0.5058516892200839, + "velocityX": 3.3053697791645313, + "velocityY": 0.0004816192577199728, + "timestamp": 6.702276075333702 + }, + { + "x": 5.620478420069855, + "y": 6.42365674720977, + "heading": -0.14578823417625966, + "angularVelocity": -0.003295072273013325, + "velocityX": 3.456361977907994, + "velocityY": -0.0100489926650518, + "timestamp": 6.76444125566331 + }, + { + "x": 5.8354039344878705, + "y": 6.423689815718493, + "heading": -0.1457882693852108, + "angularVelocity": -5.663773666898463e-7, + "velocityX": 3.4573295416896364, + "velocityY": 0.0005319458344337396, + "timestamp": 6.826606435992917 + }, + { + "x": 6.050013195110809, + "y": 6.4120346435837465, + "heading": -0.14578830470722368, + "angularVelocity": -5.68196097923049e-7, + "velocityX": 3.4522422276434255, + "velocityY": -0.1874871442976483, + "timestamp": 6.888771616322525 + }, + { + "x": 6.263319492340088, + "y": 6.385702133178711, + "heading": -0.14578834092456064, + "angularVelocity": -5.825984377491133e-7, + "velocityX": 3.4312825298390877, + "velocityY": -0.42358938340430125, + "timestamp": 6.950936796652132 + }, + { + "x": 6.49569885410855, + "y": 6.338969209681391, + "heading": -0.14578837085914265, + "angularVelocity": -4.366235413031805e-7, + "velocityX": 3.389467737301255, + "velocityY": -0.6816428759355991, + "timestamp": 7.019496045412968 + }, + { + "x": 6.723883016534425, + "y": 6.274812198093867, + "heading": -0.14578840022622835, + "angularVelocity": -4.28346084046259e-7, + "velocityX": 3.328276878031172, + "velocityY": -0.9357893026414394, + "timestamp": 7.088055294173803 + }, + { + "x": 6.944379070760873, + "y": 6.204658250666653, + "heading": -0.16522808025100505, + "angularVelocity": -0.28354569771601223, + "velocityX": 3.2161387152247607, + "velocityY": -1.023260153738583, + "timestamp": 7.156614542934639 + }, + { + "x": 7.148638930902927, + "y": 6.139545447418234, + "heading": -0.19715028206546042, + "angularVelocity": -0.465614813339248, + "velocityX": 2.979318820347672, + "velocityY": -0.949730407279711, + "timestamp": 7.225173791695474 + }, + { + "x": 7.3358170328874195, + "y": 6.0798421831912135, + "heading": -0.22994658912328278, + "angularVelocity": -0.4783644460899831, + "velocityX": 2.7301655920625914, + "velocityY": -0.870827281601222, + "timestamp": 7.29373304045631 + }, + { + "x": 7.5059237430887356, + "y": 6.025569482278596, + "heading": -0.26160545834967963, + "angularVelocity": -0.4617738641920779, + "velocityX": 2.481163566927677, + "velocityY": -0.7916174971803536, + "timestamp": 7.362292289217145 + }, + { + "x": 7.658979831941821, + "y": 5.976731413281962, + "heading": -0.291240609478123, + "angularVelocity": -0.43225606557947815, + "velocityX": 2.232464497780769, + "velocityY": -0.7123483684455512, + "timestamp": 7.430851537977981 + }, + { + "x": 7.795001086268662, + "y": 5.933327606851567, + "heading": -0.3183467386103757, + "angularVelocity": -0.39536794265075353, + "velocityX": 1.9839956940214167, + "velocityY": -0.633084627018105, + "timestamp": 7.4994107867388164 + }, + { + "x": 7.91399908186447, + "y": 5.895356201186841, + "heading": -0.34259459104051004, + "angularVelocity": -0.3536773355659347, + "velocityX": 1.7356957339326504, + "velocityY": -0.5538480416724806, + "timestamp": 7.567970035499652 + }, + { + "x": 8.015982502141213, + "y": 5.862814866579216, + "heading": -0.36375111540905714, + "angularVelocity": -0.3085874590363489, + "velocityX": 1.4875224294318823, + "velocityY": -0.4746454372792861, + "timestamp": 7.6365292842604875 + }, + { + "x": 8.100958056756967, + "y": 5.835701248475415, + "heading": -0.3816420491863885, + "angularVelocity": -0.26095580247302225, + "velocityX": 1.2394469914947026, + "velocityY": -0.39547717622147305, + "timestamp": 7.705088533021323 + }, + { + "x": 8.168931078031942, + "y": 5.814013189562108, + "heading": -0.3961322011943633, + "angularVelocity": -0.211352257643937, + "velocityX": 0.9914493303754669, + "velocityY": -0.31634038157220884, + "timestamp": 7.7736477817821585 + }, + { + "x": 8.219905912133822, + "y": 5.797748853681938, + "heading": -0.4071141507461263, + "angularVelocity": -0.16018188282769613, + "velocityX": 0.7435150621282579, + "velocityY": -0.237230368974835, + "timestamp": 7.842207030542994 + }, + { + "x": 8.253886183057102, + "y": 5.786906800693844, + "heading": -0.4145013188137657, + "angularVelocity": -0.10774867288013994, + "velocityX": 0.49563365318977964, + "velocityY": -0.1581413621656758, + "timestamp": 7.91076627930383 + }, + { + "x": 8.270874977111816, + "y": 5.7814860343933105, + "heading": -0.4182234468320189, + "angularVelocity": -0.05429067683103692, + "velocityX": 0.2477972609352767, + "velocityY": -0.07906688591998655, + "timestamp": 7.979325528064665 + }, + { + "x": 8.270874977111816, + "y": 5.7814860343933105, + "heading": -0.4182234468320189, + "angularVelocity": 1.2149869731922202e-24, + "velocityX": 1.6200325032236621e-24, + "velocityY": -9.81678651755837e-25, + "timestamp": 8.0478847768255 + }, + { + "x": 8.255885275596246, + "y": 5.786297415399053, + "heading": -0.4161957197256105, + "angularVelocity": 0.03152694238731417, + "velocityX": -0.233058706268173, + "velocityY": 0.07480697540219873, + "timestamp": 8.112202054457356 + }, + { + "x": 8.225904877332317, + "y": 5.795920588198962, + "heading": -0.4121767719087599, + "angularVelocity": 0.06248628618665337, + "velocityX": -0.46613288633782896, + "velocityY": 0.14962033771067135, + "timestamp": 8.176519332089212 + }, + { + "x": 8.180932637466617, + "y": 5.810356062930585, + "heading": -0.4062099213750387, + "angularVelocity": 0.09277212521143607, + "velocityX": -0.6992248664987879, + "velocityY": 0.22444163159780386, + "timestamp": 8.240836609721068 + }, + { + "x": 8.120967221795235, + "y": 5.8296044401602, + "heading": -0.3983469021562322, + "angularVelocity": 0.12225360755804271, + "velocityX": -0.9323375907577511, + "velocityY": 0.29927226304244037, + "timestamp": 8.305153887352924 + }, + { + "x": 8.046007053762992, + "y": 5.853666398213901, + "heading": -0.38865015214844967, + "angularVelocity": 0.15076431038150498, + "velocityX": -1.16547482717328, + "velocityY": 0.3741134410481226, + "timestamp": 8.36947116498478 + }, + { + "x": 7.95605024235754, + "y": 5.882542674483832, + "heading": -0.37719620256520386, + "angularVelocity": 0.17808511188559348, + "velocityX": -1.398641464900824, + "velocityY": 0.44896608396916854, + "timestamp": 8.433788442616637 + }, + { + "x": 7.851094482525453, + "y": 5.916234036378714, + "heading": -0.3640809123745278, + "angularVelocity": 0.20391550565536098, + "velocityX": -1.6318439414186696, + "velocityY": 0.5238306585009168, + "timestamp": 8.498105720248493 + }, + { + "x": 7.731136916910522, + "y": 5.954741232958676, + "heading": -0.349427957292727, + "angularVelocity": 0.22782299906523523, + "velocityX": -1.8650908438872864, + "velocityY": 0.5987068793609814, + "timestamp": 8.562422997880349 + }, + { + "x": 7.596173948111591, + "y": 5.998064906716331, + "heading": -0.33340347371338613, + "angularVelocity": 0.24914741682729602, + "velocityX": -2.0983936784675863, + "velocityY": 0.6735930896459054, + "timestamp": 8.626740275512205 + }, + { + "x": 7.446201013798343, + "y": 6.046205411855825, + "heading": -0.31624349903105253, + "angularVelocity": 0.26680194364810067, + "velocityX": -2.331767447802665, + "velocityY": 0.7484848070691726, + "timestamp": 8.691057553144061 + }, + { + "x": 7.281212494788232, + "y": 6.099162371697227, + "heading": -0.2983118341454104, + "angularVelocity": 0.27880012254686504, + "velocityX": -2.56522858374831, + "velocityY": 0.82337066790236, + "timestamp": 8.755374830775917 + }, + { + "x": 7.101203008664871, + "y": 6.1569332933758485, + "heading": -0.28024668313688234, + "angularVelocity": 0.280875554340636, + "velocityX": -2.7987734050827355, + "velocityY": 0.8982177698703966, + "timestamp": 8.819692108407773 + }, + { + "x": 6.9061821193945345, + "y": 6.219506729950283, + "heading": -0.26348073873693967, + "angularVelocity": 0.26067559164908854, + "velocityX": -3.0321695266179045, + "velocityY": 0.9728868956891804, + "timestamp": 8.88400938603963 + }, + { + "x": 6.696607254404851, + "y": 6.286709539594328, + "heading": -0.25518312367209683, + "angularVelocity": 0.12901066976648753, + "velocityX": -3.258453602300528, + "velocityY": 1.0448640259419284, + "timestamp": 8.948326663671486 + }, + { + "x": 6.4817199239739445, + "y": 6.3438942094131185, + "heading": -0.2551831038227718, + "angularVelocity": 3.086157524298735e-7, + "velocityX": -3.3410513992973323, + "velocityY": 0.8891027718260752, + "timestamp": 9.012643941303342 + }, + { + "x": 6.263319492340088, + "y": 6.385702133178711, + "heading": -0.2551830834264048, + "angularVelocity": 3.1712111825091784e-7, + "velocityX": -3.395672821911929, + "velocityY": 0.6500263273718765, + "timestamp": 9.076961218935198 + }, + { + "x": 6.037162237236338, + "y": 6.412166410916887, + "heading": -0.25518303828533845, + "angularVelocity": 6.854074958240057e-7, + "velocityX": -3.4338993409830967, + "velocityY": 0.4018251187344374, + "timestamp": 9.142821406624625 + }, + { + "x": 5.8096806134973695, + "y": 6.422145044657871, + "heading": -0.2551829940595105, + "angularVelocity": 6.715108093540807e-7, + "velocityX": -3.4540081302483707, + "velocityY": 0.15151237934575548, + "timestamp": 9.208681594314053 + }, + { + "x": 5.5819802469231306, + "y": 6.422071707013465, + "heading": -0.25518294988305257, + "angularVelocity": 6.707611912611386e-7, + "velocityX": -3.457329451412868, + "velocityY": -0.0011135353083337968, + "timestamp": 9.27454178200348 + }, + { + "x": 5.357973256918571, + "y": 6.423240614393437, + "heading": -0.2430314751768064, + "angularVelocity": 0.18450410077098406, + "velocityX": -3.4012504042790286, + "velocityY": 0.017748315347736767, + "timestamp": 9.340401969692907 + }, + { + "x": 5.144457161392198, + "y": 6.423376601690841, + "heading": -0.1958139855508891, + "angularVelocity": 0.7169352424043682, + "velocityX": -3.241960021936682, + "velocityY": 0.0020647875776692963, + "timestamp": 9.406262157382335 + }, + { + "x": 4.947462840354243, + "y": 6.423510104020051, + "heading": -0.14701489396398695, + "angularVelocity": 0.7409497801163326, + "velocityX": -2.991098688739039, + "velocityY": 0.002027056616322834, + "timestamp": 9.472122345071762 + }, + { + "x": 4.766993522305038, + "y": 6.423644398305699, + "heading": -0.09964051366094051, + "angularVelocity": 0.719317420206062, + "velocityX": -2.74018833502617, + "velocityY": 0.002039081429309675, + "timestamp": 9.53798253276119 + }, + { + "x": 4.60301308935793, + "y": 6.423783110988961, + "heading": -0.05492917751371649, + "angularVelocity": 0.6788826105091881, + "velocityX": -2.4898263837385226, + "velocityY": 0.0021061689638151486, + "timestamp": 9.603842720450617 + }, + { + "x": 4.455491999512653, + "y": 6.4239249853365035, + "heading": -0.013574200580991685, + "angularVelocity": 0.6279207269760504, + "velocityX": -2.2399129887228693, + "velocityY": 0.0021541746618076994, + "timestamp": 9.669702908140044 + }, + { + "x": 4.324407720523135, + "y": 6.424066362703545, + "heading": 0.023974314912572615, + "angularVelocity": 0.570124635396258, + "velocityX": -1.9903417161162953, + "velocityY": 0.0021466286690262756, + "timestamp": 9.735563095829471 + }, + { + "x": 4.2097428344188135, + "y": 6.424202667515828, + "heading": 0.05739701354466356, + "angularVelocity": 0.5074795533486763, + "velocityX": -1.7410349123971218, + "velocityY": 0.002069608621913246, + "timestamp": 9.801423283518899 + }, + { + "x": 4.11148358029628, + "y": 6.424329194128387, + "heading": 0.08645394614427561, + "angularVelocity": 0.4411911599255325, + "velocityX": -1.4919370498287492, + "velocityY": 0.001921139568510551, + "timestamp": 9.867283471208326 + }, + { + "x": 4.029618879443776, + "y": 6.4244415432031925, + "heading": 0.11095800828046558, + "angularVelocity": 0.3720618327378889, + "velocityX": -1.2430074028720826, + "velocityY": 0.00170587237520769, + "timestamp": 9.933143658897754 + }, + { + "x": 3.964139686055167, + "y": 6.424535880555734, + "heading": 0.13075995126873372, + "angularVelocity": 0.3006663613175029, + "velocityX": -0.994215104539091, + "velocityY": 0.0014323881520982617, + "timestamp": 9.99900384658718 + }, + { + "x": 3.91503854489554, + "y": 6.424609101274365, + "heading": 0.14573933177166642, + "angularVelocity": 0.22744211683042614, + "velocityX": -0.7455360041056982, + "velocityY": 0.0011117599448221096, + "timestamp": 10.064864034276608 + }, + { + "x": 3.882309280386553, + "y": 6.424658939920723, + "heading": 0.15579869857714623, + "angularVelocity": 0.1527382043445731, + "velocityX": -0.496950671676277, + "velocityY": 0.000756734046866559, + "timestamp": 10.130724221966036 + }, + { + "x": 3.865946769714355, + "y": 6.424684047698975, + "heading": 0.1608595979864649, + "angularVelocity": 0.07684307602012863, + "velocityX": -0.2484431224119383, + "velocityY": 0.0003812284649235306, + "timestamp": 10.196584409655463 + }, + { + "x": 3.865946769714355, + "y": 6.424684047698975, + "heading": 0.1608595979864649, + "angularVelocity": 3.773099570003496e-26, + "velocityX": -4.36512163919617e-27, + "velocityY": 1.07464594312406e-25, + "timestamp": 10.26244459734489 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3PieceSourceSide.1.traj b/src/main/deploy/choreo/3PieceSourceSide.1.traj index 836a57b..c6af840 100644 --- a/src/main/deploy/choreo/3PieceSourceSide.1.traj +++ b/src/main/deploy/choreo/3PieceSourceSide.1.traj @@ -4,442 +4,523 @@ "x": 0.6476043462753296, "y": 4.391018867492676, "heading": -1.0516500181249593, - "angularVelocity": -2.3506527398385264e-24, - "velocityX": -4.584841665957037e-25, - "velocityY": 1.794170584470112e-24, + "angularVelocity": 1.9413513581547193e-28, + "velocityX": -2.7445681219758215e-27, + "velocityY": -2.6836170825004164e-27, "timestamp": 0 }, { - "x": 0.692835797571452, - "y": 4.365429285724839, - "heading": -1.0363921320591865, - "angularVelocity": 0.19907376380794284, - "velocityX": 0.5901469714218079, - "velocityY": -0.33387419035865423, - "timestamp": 0.07664438434234237 - }, - { - "x": 0.7831334822061629, - "y": 4.314114094234643, - "heading": -1.005707809669664, - "angularVelocity": 0.4003466483919687, - "velocityX": 1.178138299492162, - "velocityY": -0.6695231742092023, - "timestamp": 0.15328876868468475 - }, - { - "x": 0.9181042106922528, - "y": 4.23677088276396, - "heading": -0.9591055566031375, - "angularVelocity": 0.608032192657082, - "velocityX": 1.7609995780411658, - "velocityY": -1.009117786441079, - "timestamp": 0.2299331530270271 - }, - { - "x": 1.0717422840106643, - "y": 4.1370476005177, - "heading": -0.8558256704459198, - "angularVelocity": 1.3475205919314872, - "velocityX": 2.004557471975592, - "velocityY": -1.3011166193315893, - "timestamp": 0.3065775373693695 - }, - { - "x": 1.203873817721518, - "y": 4.032707540283105, - "heading": -0.6892805911828493, - "angularVelocity": 2.172958667384875, - "velocityX": 1.7239558363554812, - "velocityY": -1.3613529696911204, - "timestamp": 0.3832219217117119 - }, - { - "x": 1.3594029637702276, - "y": 3.9301727362790966, - "heading": -0.5828610080840567, - "angularVelocity": 1.388485066609126, - "velocityX": 2.0292308090572955, - "velocityY": -1.3377993036779194, - "timestamp": 0.45986630605405426 - }, - { - "x": 1.5344193266927146, - "y": 3.825053132953821, - "heading": -0.5408815443461281, - "angularVelocity": 0.5477174107162478, - "velocityX": 2.2834857951334486, - "velocityY": -1.371523879110889, - "timestamp": 0.5365106903963967 - }, - { - "x": 1.7162004854447428, - "y": 3.7215786578067394, - "heading": -0.5174827165758584, - "angularVelocity": 0.30529083077705393, - "velocityX": 2.3717479148906513, - "velocityY": -1.3500594470809355, - "timestamp": 0.613155074738739 - }, - { - "x": 1.8995749541613056, - "y": 3.6205751589308552, - "heading": -0.4949062907307691, - "angularVelocity": 0.2945607305585351, - "velocityX": 2.3925362606801817, - "velocityY": -1.317819951749349, - "timestamp": 0.6897994590810813 - }, - { - "x": 2.0827414581868835, - "y": 3.51960952250048, - "heading": -0.4719606385920979, - "angularVelocity": 0.29937812581521006, - "velocityX": 2.389822889142672, - "velocityY": -1.3173259501883174, - "timestamp": 0.7664438434234236 - }, - { - "x": 2.265688686133792, - "y": 3.4186835470349224, - "heading": -0.4486127865936518, - "angularVelocity": 0.3046257361029851, - "velocityX": 2.3869619348724966, - "velocityY": -1.3168084828597342, - "timestamp": 0.843088227765766 - }, - { - "x": 2.4484040933032216, - "y": 3.3177997829423713, - "heading": -0.4248255225210326, - "angularVelocity": 0.3103588642107185, - "velocityX": 2.3839373065260236, - "velocityY": -1.3162577396660924, - "timestamp": 0.9197326121081083 - }, - { - "x": 2.6308732997732713, - "y": 3.216961383200288, - "heading": -0.4005559741960246, - "angularVelocity": 0.3166513572162684, - "velocityX": 2.380725059451553, - "velocityY": -1.3156658587232646, - "timestamp": 0.9963769964504506 - }, - { - "x": 2.813079628281676, - "y": 3.116172257902354, - "heading": -0.3757541558021446, - "angularVelocity": 0.32359602868097065, - "velocityX": 2.377295219628298, - "velocityY": -1.3150229617312381, - "timestamp": 1.073021380792793 - }, - { - "x": 2.9950035114027176, - "y": 3.015437295026463, - "heading": -0.3503610814650098, - "angularVelocity": 0.3313103047930201, - "velocityX": 2.3736100783125105, - "velocityY": -1.314316289970363, - "timestamp": 1.1496657651351354 - }, - { - "x": 3.176621688858468, - "y": 2.9147626672707037, - "heading": -0.3243062033321442, - "angularVelocity": 0.3399450378058761, - "velocityX": 2.3696214538631923, - "velocityY": -1.3135290813490341, - "timestamp": 1.2263101494774777 - }, - { - "x": 3.3579060875063353, - "y": 2.8141562687774018, - "heading": -0.2975038256949741, - "angularVelocity": 0.34969786589260987, - "velocityX": 2.3652665515341145, - "velocityY": -1.3126388757189247, - "timestamp": 1.30295453381982 - }, - { - "x": 3.538822210205962, - "y": 2.713628355793928, - "heading": -0.2698479287435623, - "angularVelocity": 0.3608339630974529, - "velocityX": 2.360461555689992, - "velocityY": -1.3116148540570647, - "timestamp": 1.3795989181621624 - }, - { - "x": 3.7193267349323875, - "y": 2.6131925205499895, - "heading": -0.24120442629231653, - "angularVelocity": 0.3737195190101045, - "velocityX": 2.3550913256759576, - "velocityY": -1.3104134909000977, - "timestamp": 1.4562433025045047 - }, - { - "x": 3.8993637811598734, - "y": 2.5128672365819, - "heading": -0.21139907897320392, - "angularVelocity": 0.38887842305553666, - "velocityX": 2.3489920073377766, - "velocityY": -1.3089710985213812, - "timestamp": 1.532887686846847 - }, - { - "x": 4.078858787892925, - "y": 2.4126784448405125, - "heading": -0.18019759590707388, - "angularVelocity": 0.4070941835316264, - "velocityX": 2.34191987153701, - "velocityY": -1.30719024754481, - "timestamp": 1.6095320711891894 - }, - { - "x": 4.257707770811751, - "y": 2.312664186467197, - "heading": -0.1472705722503286, - "angularVelocity": 0.42960777804245054, - "velocityX": 2.3334910242082882, - "velocityY": -1.3049130635140744, - "timestamp": 1.6861764555315317 - }, - { - "x": 4.435756686256742, - "y": 2.212883689420889, - "heading": -0.1121258503357887, - "angularVelocity": 0.4585426866704457, - "velocityX": 2.323052327613603, - "velocityY": -1.3018631162933536, - "timestamp": 1.762820839873874 - }, - { - "x": 4.612756399460092, - "y": 2.113437617489987, - "heading": -0.07396027426521577, - "angularVelocity": 0.4979565874011239, - "velocityX": 2.3093631023605954, - "velocityY": -1.2974997814153513, - "timestamp": 1.8394652242162164 - }, - { - "x": 4.788243072222853, - "y": 2.014522987095371, - "heading": -0.031264185221419126, - "angularVelocity": 0.5570674147904805, - "velocityX": 2.289622054747365, - "velocityY": -1.290565920039225, - "timestamp": 1.9161096085585587 - }, - { - "x": 4.961081026742291, - "y": 1.9166475782485084, - "heading": 0.019697120176888277, - "angularVelocity": 0.664905926710585, - "velocityX": 2.255063511860639, - "velocityY": -1.2770069155972317, - "timestamp": 1.992753992900901 - }, - { - "x": 5.123264328610966, - "y": 1.823526252461316, - "heading": 0.10546513381285838, - "angularVelocity": 1.1190384575714751, - "velocityX": 2.1160493787028227, - "velocityY": -1.214979108857517, - "timestamp": 2.0693983772432434 - }, - { - "x": 5.295119103926639, - "y": 1.7237778734191072, - "heading": 0.15590676979164073, - "angularVelocity": 0.6581256593239505, - "velocityX": 2.24223570702925, - "velocityY": -1.3014440640121712, - "timestamp": 2.1460427615855857 - }, - { - "x": 5.493802683128641, - "y": 1.6448426706793149, - "heading": 0.16075963684913044, - "angularVelocity": 0.06331666826122177, - "velocityX": 2.5922783633378295, - "velocityY": -1.029888926854938, - "timestamp": 2.222687145927928 - }, - { - "x": 5.707507610321045, - "y": 1.6179983615875244, + "x": 0.6597456116335061, + "y": 4.380178489721837, + "heading": -1.0354550463341647, + "angularVelocity": 0.23714267860510804, + "velocityX": 0.17778432873405597, + "velocityY": -0.15873545535467184, + "timestamp": 0.0682921011352938 + }, + { + "x": 0.6840676878744021, + "y": 4.358487813648757, + "heading": -1.0032553908058022, + "angularVelocity": 0.47149897269336843, + "velocityX": 0.35614772186773713, + "velocityY": -0.3176161768710194, + "timestamp": 0.1365842022705876 + }, + { + "x": 0.7206162348586238, + "y": 4.325939175146822, + "heading": -0.9552579550391015, + "angularVelocity": 0.7028255825898914, + "velocityX": 0.5351797115132725, + "velocityY": -0.4766091240545095, + "timestamp": 0.2048763034058814 + }, + { + "x": 0.7694577963308973, + "y": 4.282521644299476, + "heading": -0.8917843367572964, + "angularVelocity": 0.9294430428499669, + "velocityX": 0.7151861000075717, + "velocityY": -0.6357621178082019, + "timestamp": 0.2731684045411752 + }, + { + "x": 0.8306912777270616, + "y": 4.228207914210413, + "heading": -0.8133985275963236, + "angularVelocity": 1.1478019838001796, + "velocityX": 0.896640759007464, + "velocityY": -0.7953149659498937, + "timestamp": 0.341460505676469 + }, + { + "x": 0.9044507708380568, + "y": 4.162933772374405, + "heading": -0.7210416262705941, + "angularVelocity": 1.3523804333206972, + "velocityX": 1.0800589216733842, + "velocityY": -0.9558080766426159, + "timestamp": 0.40975260681176284 + }, + { + "x": 0.9908959110037516, + "y": 4.086578645763268, + "heading": -0.6161174549931975, + "angularVelocity": 1.5364027396013313, + "velocityX": 1.265814621729648, + "velocityY": -1.1180667360031697, + "timestamp": 0.47804470794705667 + }, + { + "x": 1.090196283285551, + "y": 3.998968512246339, + "heading": -0.5004646416711678, + "angularVelocity": 1.6935020507409695, + "velocityX": 1.4540535527684928, + "velocityY": -1.2828735982711192, + "timestamp": 0.5463368090823505 + }, + { + "x": 1.2025392306050893, + "y": 3.8999248788172114, + "heading": -0.3762963474938359, + "angularVelocity": 1.818194082670579, + "velocityX": 1.645035742815635, + "velocityY": -1.4502941303989414, + "timestamp": 0.6146289102176443 + }, + { + "x": 1.328190264179142, + "y": 3.789348006758151, + "heading": -0.2466808070464207, + "angularVelocity": 1.897958011141482, + "velocityX": 1.8399058088010056, + "velocityY": -1.619175134764066, + "timestamp": 0.6829210113529381 + }, + { + "x": 1.467420004237056, + "y": 3.6673010913969546, + "heading": -0.11832684408091969, + "angularVelocity": 1.8794847549238287, + "velocityX": 2.0387385619031573, + "velocityY": -1.7871307710888695, + "timestamp": 0.751213112488232 + }, + { + "x": 1.6176774224315258, + "y": 3.534549695823147, + "heading": -0.01051011565957327, + "angularVelocity": 1.5787584014694496, + "velocityX": 2.2002166531206018, + "velocityY": -1.9438762809598271, + "timestamp": 0.8195052136235258 + }, + { + "x": 1.7740513259099606, + "y": 3.391053821163867, + "heading": 0.062440796343179686, + "angularVelocity": 1.0682188831506234, + "velocityX": 2.2897802363503494, + "velocityY": -2.1012074936016356, + "timestamp": 0.8877973147588196 + }, + { + "x": 1.9367743639870942, + "y": 3.237147837801074, + "heading": 0.09987435620827004, + "angularVelocity": 0.5481389390982501, + "velocityX": 2.382750499281934, + "velocityY": -2.253642526796603, + "timestamp": 0.9560894158941134 + }, + { + "x": 2.1058806251727042, + "y": 3.0730806615870963, + "heading": 0.10139956904864746, + "angularVelocity": 0.022333663996599216, + "velocityX": 2.4762199196447727, + "velocityY": -2.4024326896743653, + "timestamp": 1.0243815170294073 + }, + { + "x": 2.282556811687828, + "y": 2.9164516007892924, + "heading": 0.1013996340867454, + "angularVelocity": 9.523516904308559e-7, + "velocityX": 2.587066199136408, + "velocityY": -2.293516500356401, + "timestamp": 1.092673618164701 + }, + { + "x": 2.4592332259385974, + "y": 2.759822796875614, + "heading": 0.10139969912475692, + "angularVelocity": 9.52350424751949e-7, + "velocityX": 2.5870695338653458, + "velocityY": -2.2935127388067964, + "timestamp": 1.1609657192999947 + }, + { + "x": 2.6359096401941113, + "y": 2.603193992967287, + "heading": 0.10139976416276904, + "angularVelocity": 9.523504335129237e-7, + "velocityX": 2.587069533934816, + "velocityY": -2.293512738728436, + "timestamp": 1.2292578204352884 + }, + { + "x": 2.812586054449808, + "y": 2.4465651890591653, + "heading": 0.1013998292007818, + "angularVelocity": 9.523504429819945e-7, + "velocityX": 2.587069533937483, + "velocityY": -2.2935127387254277, + "timestamp": 1.2975499215705821 + }, + { + "x": 2.989262477460467, + "y": 2.2899363950265936, + "heading": 0.10139989423879318, + "angularVelocity": 9.523504226466593e-7, + "velocityX": 2.5870696621362548, + "velocityY": -2.2935125941179324, + "timestamp": 1.3658420227058758 + }, + { + "x": 3.166357964338375, + "y": 2.1337815804962608, + "heading": 0.10139995920086388, + "angularVelocity": 9.512384243710626e-7, + "velocityX": 2.593206006754183, + "velocityY": -2.286572120851488, + "timestamp": 1.4341341238411696 + }, + { + "x": 3.3547011225730468, + "y": 1.9913948102576182, + "heading": 0.10140002311622047, + "angularVelocity": 9.359114091359504e-7, + "velocityX": 2.7579054547105564, + "velocityY": -2.0849668976586258, + "timestamp": 1.5024262249764633 + }, + { + "x": 3.553226828100459, + "y": 1.8635876061361314, + "heading": 0.1014000890794247, + "angularVelocity": 9.658980048505205e-7, + "velocityX": 2.907008310289235, + "velocityY": -1.8714785750739689, + "timestamp": 1.570718326111757 + }, + { + "x": 3.760808917151287, + "y": 1.7510851059584576, + "heading": 0.10140016079724051, + "angularVelocity": 0.0000010501626778013022, + "velocityX": 3.0396207701910694, + "velocityY": -1.647372072427445, + "timestamp": 1.6390104272470507 + }, + { + "x": 3.976269792438543, + "y": 1.654525560969519, + "heading": 0.10140024381940511, + "angularVelocity": 0.0000012156920527539028, + "velocityX": 3.1549896943484823, + "velocityY": -1.413919668361702, + "timestamp": 1.7073025283823444 + }, + { + "x": 4.1983871429917885, + "y": 1.5744567665619278, + "heading": 0.10140036224681147, + "angularVelocity": 0.0000017341303662335387, + "velocityX": 3.252460341104571, + "velocityY": -1.1724459062837547, + "timestamp": 1.7755946295176381 + }, + { + "x": 4.4257370453636415, + "y": 1.5113830168900522, + "heading": 0.10193866126599585, + "angularVelocity": 0.007882302788106277, + "velocityX": 3.3290805026111183, + "velocityY": -0.9235877740372923, + "timestamp": 1.8438867306529318 + }, + { + "x": 4.656186679939347, + "y": 1.4657767864094913, + "heading": 0.10576151989897133, + "angularVelocity": 0.05597804972206098, + "velocityX": 3.374469825128989, + "velocityY": -0.6678112068950683, + "timestamp": 1.9121788317882256 + }, + { + "x": 4.888314550102186, + "y": 1.4378182802801935, + "heading": 0.1133638582654562, + "angularVelocity": 0.11132090300492935, + "velocityX": 3.3990441984347375, + "velocityY": -0.4093958988596496, + "timestamp": 1.9804709329235193 + }, + { + "x": 5.121043949314908, + "y": 1.4275947415879202, + "heading": 0.12410092792765016, + "angularVelocity": 0.15722271659093798, + "velocityX": 3.4078523774171208, + "velocityY": -0.14970309189959885, + "timestamp": 2.048763034058813 + }, + { + "x": 5.353451540876787, + "y": 1.4351381309484952, + "heading": 0.13671420054587038, + "angularVelocity": 0.18469592249375974, + "velocityX": 3.403140153814491, + "velocityY": 0.11045771377908847, + "timestamp": 2.117055135194107 + }, + { + "x": 5.584673054960648, + "y": 1.4604251835156339, + "heading": 0.14959239179836323, + "angularVelocity": 0.18857512125713907, + "velocityX": 3.385772442786424, + "velocityY": 0.37027785273501185, + "timestamp": 2.185347236329401 + }, + { + "x": 5.813786029815674, + "y": 1.5033589601516724, "heading": 0.16076270850197952, - "angularVelocity": 0.000040076685010459537, - "velocityX": 2.7882659509385723, - "velocityY": -0.3502449569153927, - "timestamp": 2.2993315302702704 - }, - { - "x": 5.878932815955422, - "y": 1.6310716458031873, - "heading": 0.15207722794791115, - "angularVelocity": -0.13999820175733138, - "velocityX": 2.7631425083845143, - "velocityY": 0.21072366345901047, - "timestamp": 2.3613714739627154 - }, - { - "x": 6.041874898509352, - "y": 1.6781214038756682, - "heading": 0.13576601260266036, - "angularVelocity": -0.2629147348377915, - "velocityX": 2.6264060354679595, - "velocityY": 0.7583784779967588, - "timestamp": 2.4234114176551604 - }, - { - "x": 6.190063655455228, - "y": 1.7574804032751512, - "heading": 0.11619159565916007, - "angularVelocity": -0.3155131319998917, - "velocityX": 2.388602376567293, - "velocityY": 1.2791597586370276, - "timestamp": 2.4854513613476055 - }, - { - "x": 6.338186422130831, - "y": 1.840598011238034, - "heading": 0.10221625072518932, - "angularVelocity": -0.22526366244385518, - "velocityX": 2.387538702644562, - "velocityY": 1.3397434461727966, - "timestamp": 2.5474913050400505 - }, - { - "x": 6.486157900665009, - "y": 1.9236682945064507, - "heading": 0.08780760406345454, - "angularVelocity": -0.2322479003714719, - "velocityX": 2.3851001423812606, - "velocityY": 1.338980636091913, - "timestamp": 2.6095312487324955 - }, - { - "x": 6.633958405719426, - "y": 2.0066826604418155, - "heading": 0.07290105260244177, - "angularVelocity": -0.2402734524536335, - "velocityX": 2.382344281083177, - "velocityY": 1.3380793242962596, - "timestamp": 2.6715711924249406 - }, - { - "x": 6.781563534615612, - "y": 2.089630624589182, - "heading": 0.0574168439817537, - "angularVelocity": -0.24958450474179966, - "velocityX": 2.3791950816060985, - "velocityY": 1.33700901726427, - "timestamp": 2.7336111361173856 - }, - { - "x": 6.928941267710664, - "y": 2.172498140132396, - "heading": 0.04124990446967813, - "angularVelocity": -0.2605892035012319, - "velocityX": 2.3755297687834913, - "velocityY": 1.3357122945504047, - "timestamp": 2.7956510798098306 - }, - { - "x": 7.076047973733186, - "y": 2.255265656701969, - "heading": 0.024256449523087663, - "angularVelocity": -0.27391151466599145, - "velocityX": 2.3711611788653904, - "velocityY": 1.334100446317001, - "timestamp": 2.8576910235022757 - }, - { - "x": 7.222828783744423, - "y": 2.337909309225835, - "heading": 0.006256947367936294, - "angularVelocity": -0.2901276352599791, - "velocityX": 2.3659081758501124, - "velocityY": 1.3321039253929894, - "timestamp": 2.9197309671947207 - }, - { - "x": 7.384688854217529, - "y": 2.3971612453460693, - "heading": 6.712344331303172e-23, - "angularVelocity": -0.10085353073423609, - "velocityX": 2.608965463855139, - "velocityY": 0.9550610879656509, - "timestamp": 2.9817709108871657 - }, - { - "x": 7.581926675665867, - "y": 2.420671247090372, - "heading": -6.833387833600707e-9, - "angularVelocity": -9.667586454477017e-8, - "velocityX": 2.7904368043745778, - "velocityY": 0.33260950489354324, - "timestamp": 3.052454404288316 - }, - { - "x": 7.7805365921893905, - "y": 2.417576114646826, - "heading": -6.833380269760843e-9, - "angularVelocity": 1.0700998918582695e-13, - "velocityX": 2.8098486218890097, - "velocityY": -0.043788617322290876, - "timestamp": 3.1231378976894666 - }, - { - "x": 7.9655574312129644, - "y": 2.4146925691677037, - "heading": -4.100028695417927e-9, - "angularVelocity": 3.8670295465311964e-8, - "velocityX": 2.6175961334214803, - "velocityY": -0.0407951749463946, - "timestamp": 3.193821391090617 - }, - { - "x": 8.104323073360915, - "y": 2.412529909857503, - "heading": -2.050014465682545e-9, - "angularVelocity": 2.9002729365693992e-8, - "velocityX": 1.9631972822906998, - "velocityY": -0.03059638405147332, - "timestamp": 3.2645048844917675 - }, - { - "x": 8.196833505192958, - "y": 2.411088136925815, - "heading": -6.833381783003922e-10, - "angularVelocity": 1.933515480942414e-8, - "velocityX": 1.3087982410125008, - "velocityY": -0.020397590191323155, - "timestamp": 3.335188377892918 + "angularVelocity": 0.16356674517140343, + "velocityX": 3.3548971410489967, + "velocityY": 0.6286785136539, + "timestamp": 2.253639337464695 + }, + { + "x": 5.939295701159701, + "y": 1.5323683761963922, + "heading": 0.16590262686638915, + "angularVelocity": 0.1364467184876021, + "velocityX": 3.331839453313385, + "velocityY": 0.7700977610756782, + "timestamp": 2.291309121109046 + }, + { + "x": 6.063755261240901, + "y": 1.5666696664667372, + "heading": 0.1699280929818009, + "angularVelocity": 0.1068619388265435, + "velocityX": 3.3039627000847966, + "velocityY": 0.910578372156086, + "timestamp": 2.328978904753397 + }, + { + "x": 6.186965476091788, + "y": 1.6062187514342638, + "heading": 0.17281888262781223, + "angularVelocity": 0.07674027738794295, + "velocityX": 3.270796987159313, + "velocityY": 1.0498888271012834, + "timestamp": 2.366648688397748 + }, + { + "x": 6.308696366891361, + "y": 1.650958086998814, + "heading": 0.17466669111475505, + "angularVelocity": 0.049052803286273795, + "velocityX": 3.231526147027085, + "velocityY": 1.1876716889840546, + "timestamp": 2.4043184720420987 + }, + { + "x": 6.428664923898692, + "y": 1.7008051924773087, + "heading": 0.17574653518384492, + "angularVelocity": 0.028666054450562455, + "velocityX": 3.184742395655416, + "velocityY": 1.323264979409296, + "timestamp": 2.4419882556864496 + }, + { + "x": 6.546495426793477, + "y": 1.7556223876439374, + "heading": 0.17665953793741, + "angularVelocity": 0.02423700550512633, + "velocityX": 3.1279845938922866, + "velocityY": 1.4552033450515791, + "timestamp": 2.4796580393308005 + }, + { + "x": 6.661649606485285, + "y": 1.8151243678255207, + "heading": 0.1786491578806416, + "angularVelocity": 0.05281739767916041, + "velocityX": 3.0569376447447945, + "velocityY": 1.5795678770909583, + "timestamp": 2.5173278229751515 + }, + { + "x": 6.773137667082048, + "y": 1.8784298401932633, + "heading": 0.18358189083734322, + "angularVelocity": 0.13094667607525115, + "velocityX": 2.9596151028991167, + "velocityY": 1.6805371903758264, + "timestamp": 2.5549976066195024 + }, + { + "x": 6.87927675328215, + "y": 1.941846015988482, + "heading": 0.19021855833237603, + "angularVelocity": 0.17618013306609673, + "velocityX": 2.817618683510011, + "velocityY": 1.683475976234567, + "timestamp": 2.5926673902638533 + }, + { + "x": 6.980896999193432, + "y": 2.002302596889058, + "heading": 0.19752796800079392, + "angularVelocity": 0.1940390668931813, + "velocityX": 2.6976593991274846, + "velocityY": 1.6049091619787552, + "timestamp": 2.630337173908204 + }, + { + "x": 7.078947568278477, + "y": 2.0586931301517772, + "heading": 0.20493066809205301, + "angularVelocity": 0.19651559884574055, + "velocityX": 2.60289705963705, + "velocityY": 1.496969926748577, + "timestamp": 2.668006957552555 + }, + { + "x": 7.174005720052268, + "y": 2.1105770010329152, + "heading": 0.21206754751188253, + "angularVelocity": 0.18945899682383116, + "velocityX": 2.5234589258928724, + "velocityY": 1.3773339228859343, + "timestamp": 2.705676741196906 + }, + { + "x": 7.266431305502043, + "y": 2.15773800547381, + "heading": 0.21871161412654994, + "angularVelocity": 0.17637655361643606, + "velocityX": 2.4535735676739243, + "velocityY": 1.2519584632115888, + "timestamp": 2.743346524841257 + }, + { + "x": 7.356464862823486, + "y": 2.2000527381896973, + "heading": 0.22471085620066228, + "angularVelocity": 0.15925873455373588, + "velocityX": 2.390073650846282, + "velocityY": 1.123307028131367, + "timestamp": 2.781016308485608 + }, + { + "x": 7.523755330928558, + "y": 2.2638899288150194, + "heading": 0.23355968892559228, + "angularVelocity": 0.12033403723737161, + "velocityX": 2.274959652214718, + "velocityY": 0.868113017007844, + "timestamp": 2.854551884354925 + }, + { + "x": 7.678148398275777, + "y": 2.3116679387336183, + "heading": 0.24020478966195502, + "angularVelocity": 0.09036579448527025, + "velocityX": 2.0995697051668696, + "velocityY": 0.6497264671389397, + "timestamp": 2.9280874602242424 + }, + { + "x": 7.816541043927953, + "y": 2.346462739824989, + "heading": 0.2451990584982588, + "angularVelocity": 0.06791636262126016, + "velocityX": 1.8819822108705344, + "velocityY": 0.47316962817026026, + "timestamp": 3.0016230360935596 + }, + { + "x": 7.936995928235685, + "y": 2.3711072013144094, + "heading": 0.2489351660825785, + "angularVelocity": 0.05080680391976913, + "velocityX": 1.6380491059429139, + "velocityY": 0.3351365811456589, + "timestamp": 3.075158611962877 + }, + { + "x": 8.038363690822308, + "y": 2.387966753250885, + "heading": 0.2516849557707684, + "angularVelocity": 0.03739400495178875, + "velocityX": 1.3784860101832423, + "velocityY": 0.22927068615654433, + "timestamp": 3.148694187832194 + }, + { + "x": 8.119972690853793, + "y": 2.3989446235760092, + "heading": 0.2536437125866727, + "angularVelocity": 0.026636859679799414, + "velocityX": 1.1097893647629, + "velocityY": 0.14928652146049065, + "timestamp": 3.2222297637015114 + }, + { + "x": 8.18143147852008, + "y": 2.405561042236668, + "heading": 0.2549588543062311, + "angularVelocity": 0.0178844281017885, + "velocityX": 0.8357694481852939, + "velocityY": 0.08997575095375589, + "timestamp": 3.2957653395708286 + }, + { + "x": 8.222513130352727, + "y": 2.409037637931504, + "heading": 0.25574648281571827, + "angularVelocity": 0.010710849818962056, + "velocityX": 0.5586636311335063, + "velocityY": 0.04727773807081853, + "timestamp": 3.369300915440146 }, { "x": 8.243088722229004, "y": 2.410367250442505, - "heading": 1.830282426459157e-24, - "angularVelocity": 9.667577894349971e-9, - "velocityX": 0.6543991363518575, - "velocityY": -0.010198795342763814, - "timestamp": 3.4058718712940683 + "heading": 0.2561010030603239, + "angularVelocity": 0.004821071167453232, + "velocityX": 0.2798045929883303, + "velocityY": 0.018081214368450626, + "timestamp": 3.442836491309463 }, { "x": 8.243088722229004, "y": 2.410367250442505, - "heading": 1.851361909343925e-24, - "angularVelocity": 2.3995205522592844e-25, - "velocityX": -1.4269511777904615e-24, - "velocityY": -1.1273229249768833e-23, - "timestamp": 3.4765553646952188 + "heading": 0.2561010030603239, + "angularVelocity": 1.4585674185817174e-27, + "velocityX": 1.7040232418526742e-26, + "velocityY": -1.4024664023967223e-26, + "timestamp": 3.5163720671787804 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/3PieceSourceSide.2.traj b/src/main/deploy/choreo/3PieceSourceSide.2.traj index 208c091..d164089 100644 --- a/src/main/deploy/choreo/3PieceSourceSide.2.traj +++ b/src/main/deploy/choreo/3PieceSourceSide.2.traj @@ -3,326 +3,407 @@ { "x": 8.243088722229004, "y": 2.410367250442505, - "heading": 1.851361909343925e-24, - "angularVelocity": 2.3995205522592844e-25, - "velocityX": -1.4269511777904615e-24, - "velocityY": -1.1273229249768833e-23, + "heading": 0.2561010030603239, + "angularVelocity": 1.4585674185817174e-27, + "velocityX": 1.7040232418526742e-26, + "velocityY": -1.4024664023967223e-26, "timestamp": 0 }, { - "x": 8.183672767375366, - "y": 2.3891879079868126, - "heading": -0.016263075453039278, - "angularVelocity": -0.1934255260988222, - "velocityX": -0.7066659906617425, - "velocityY": -0.251897340619777, - "timestamp": 0.08407926182778169 - }, - { - "x": 8.064809267825755, - "y": 2.3467183743812834, - "heading": -0.04860613329336098, - "angularVelocity": -0.38467342763509904, - "velocityX": -1.413707696352949, - "velocityY": -0.5051130645333048, - "timestamp": 0.16815852365556339 - }, - { - "x": 7.886434927132461, - "y": 2.2827439256274595, - "heading": -0.09665252446442359, - "angularVelocity": -0.5714416388368797, - "velocityX": -2.121502220828929, - "velocityY": -0.7608826167487317, - "timestamp": 0.2522377854833451 - }, - { - "x": 7.663093479676549, - "y": 2.2056330475266677, - "heading": -0.09665343057468777, - "angularVelocity": -0.000010776857984756087, - "velocityX": -2.6563202697162027, - "velocityY": -0.9171212546886659, - "timestamp": 0.33631704731112677 - }, - { - "x": 7.439949308521917, - "y": 2.127952816871332, - "heading": -0.09665400996983407, - "angularVelocity": -0.000006891058909294993, - "velocityX": -2.653973956285386, - "velocityY": -0.9238928716387514, - "timestamp": 0.42039630913890846 - }, - { - "x": 7.216805138002767, - "y": 2.0502725843903784, - "heading": -0.0966545893647738, - "angularVelocity": -0.0000068910564521426285, - "velocityX": -2.6539739487272445, - "velocityY": -0.9238928933518099, - "timestamp": 0.5044755709666902 - }, - { - "x": 6.9936609674836125, - "y": 1.9725923519093191, - "heading": -0.09665516875951666, - "angularVelocity": -0.000006891054111144413, - "velocityX": -2.6539739487273097, - "velocityY": -0.923892893353071, - "timestamp": 0.5885548327944714 - }, - { - "x": 6.77051679696445, - "y": 1.8949121194281597, - "heading": -0.09665574815406265, - "angularVelocity": -0.000006891051769457115, - "velocityX": -2.653973948727398, - "velocityY": -0.9238928933542627, - "timestamp": 0.6726340946222531 - }, - { - "x": 6.547372626445282, - "y": 1.8172318869468993, - "heading": -0.09665632754841177, - "angularVelocity": -0.000006891049427961194, - "velocityX": -2.653973948727487, - "velocityY": -0.9238928933554545, - "timestamp": 0.7567133564500348 - }, - { - "x": 6.324228455926046, - "y": 1.7395516544657041, - "heading": -0.09665690694256396, - "angularVelocity": -0.000006891047085669869, - "velocityX": -2.653973948728258, - "velocityY": -0.9238928933546852, - "timestamp": 0.8407926182778165 - }, - { - "x": 6.101084267512398, - "y": 1.6618714733923579, - "heading": -0.0966574863411208, - "angularVelocity": -0.000006891099472478973, - "velocityX": -2.65397416155617, - "velocityY": -0.9238922819334152, - "timestamp": 0.9248718801055982 - }, - { - "x": 5.881706688567194, - "y": 1.6001619743925184, - "heading": -0.12280606064162562, - "angularVelocity": -0.3109990945694144, - "velocityX": -2.609175844033366, - "velocityY": -0.7339443479682077, - "timestamp": 1.0089511419333799 - }, - { - "x": 5.667889595031738, - "y": 1.5915861129760742, - "heading": -0.19572183715761898, - "angularVelocity": -0.8672266493650448, - "velocityX": -2.5430419926070895, - "velocityY": -0.10199734429174848, - "timestamp": 1.0930304037611616 - }, - { - "x": 5.472836165647801, - "y": 1.6405608510115604, - "heading": -0.2695567389374151, - "angularVelocity": -0.9372981580360532, - "velocityX": -2.4761083941768933, - "velocityY": 0.6217104735625232, - "timestamp": 1.1718045922589861 - }, - { - "x": 5.3021481158717245, - "y": 1.712801388975876, - "heading": -0.4003704259742984, - "angularVelocity": -1.660616117175175, - "velocityX": -2.1668017536072774, - "velocityY": 0.9170584850431085, - "timestamp": 1.2505787807568107 - }, - { - "x": 5.1196004435735345, - "y": 1.8042197974618441, - "heading": -0.46428021863316127, - "angularVelocity": -0.8113037262278816, - "velocityX": -2.317353891918915, - "velocityY": 1.160512221442844, - "timestamp": 1.3293529692546353 - }, - { - "x": 4.929096458765624, - "y": 1.904953408569058, - "heading": -0.48639976153317277, - "angularVelocity": -0.28079683614414136, - "velocityX": -2.4183554085507573, - "velocityY": 1.2787641869518616, - "timestamp": 1.40812715775246 - }, - { - "x": 4.737761672165951, - "y": 2.009314965020544, - "heading": -0.49933816069591924, - "angularVelocity": -0.1642466829487382, - "velocityX": -2.4289020331190754, - "velocityY": 1.3248191881325238, - "timestamp": 1.4869013462502845 - }, - { - "x": 4.546911439579052, - "y": 2.114853649108329, - "heading": -0.5116829622012672, - "angularVelocity": -0.15671124946833295, - "velocityX": -2.4227508556583697, - "velocityY": 1.3397622508126334, - "timestamp": 1.565675534748109 - }, - { - "x": 4.356102309238819, - "y": 2.2203468351822107, - "heading": -0.524131682346679, - "angularVelocity": -0.15803044604839828, - "velocityX": -2.4222290826328545, - "velocityY": 1.3391846756605428, - "timestamp": 1.6444497232459336 - }, - { - "x": 4.165335431709755, - "y": 2.3257928672028427, - "heading": -0.5366900663313149, - "angularVelocity": -0.15942257513681343, - "velocityX": -2.421692703750687, - "velocityY": 1.338586077894589, - "timestamp": 1.7232239117437582 - }, - { - "x": 3.9746120593595244, - "y": 2.4311903935174577, - "heading": -0.5493635573249664, - "angularVelocity": -0.1608838026176728, - "velocityX": -2.421140426670299, - "velocityY": 1.3379703215543044, - "timestamp": 1.8019981002415828 - }, - { - "x": 3.783933515355731, - "y": 2.5365380161026203, - "heading": -0.5621578157899224, - "angularVelocity": -0.16241688691352676, - "velocityX": -2.420571352620895, - "velocityY": 1.337336817986659, - "timestamp": 1.8807722887394074 - }, - { - "x": 3.593301216494676, - "y": 2.6418342523116722, - "heading": -0.5750788548667545, - "angularVelocity": -0.16402630510359092, - "velocityX": -2.4199842930317086, - "velocityY": 1.3366844929409736, - "timestamp": 1.959546477237232 - }, - { - "x": 3.4027166923911927, - "y": 2.7470775186047, - "heading": -0.5881331219292356, - "angularVelocity": -0.16571756956711148, - "velocityX": -2.41937781572635, - "velocityY": 1.3360120656264767, - "timestamp": 2.0383206657350565 - }, - { - "x": 3.212181603664246, - "y": 2.8522661171153323, - "heading": -0.601327569493579, - "angularVelocity": -0.16749709284161973, - "velocityX": -2.4187502576711153, - "velocityY": 1.3353180847243658, - "timestamp": 2.117094854232881 - }, - { - "x": 3.0216977610811666, - "y": 2.9573982215901036, - "heading": -0.6146697280062193, - "angularVelocity": -0.1693722114701131, - "velocityX": -2.418099712805536, - "velocityY": 1.3346009204229918, - "timestamp": 2.1958690427307057 - }, - { - "x": 2.831267146879349, - "y": 3.0624718615565794, - "heading": -0.6281677862205469, - "angularVelocity": -0.17135128233914515, - "velocityX": -2.417424004400561, - "velocityY": 1.3338587419326686, - "timestamp": 2.2746432312285303 - }, - { - "x": 2.640891939244419, - "y": 3.167484904027678, - "heading": -0.6418306831098634, - "angularVelocity": -0.17344382912549475, - "velocityX": -2.416720644988769, - "velocityY": 1.3330894862090363, - "timestamp": 2.353417419726355 - }, - { - "x": 2.4505746379296203, - "y": 3.272435032259823, - "heading": -0.655668504824747, - "angularVelocity": -0.17566441468661878, - "velocityX": -2.4159855524256404, - "velocityY": 1.3322908205527566, - "timestamp": 2.4321916082241795 - }, - { - "x": 2.2618233887297388, - "y": 3.3772509809337916, - "heading": -0.674261275863946, - "angularVelocity": -0.2360261831159634, - "velocityX": -2.396105282697931, - "velocityY": 1.3305874763389332, - "timestamp": 2.510965796722004 - }, - { - "x": 2.116002537558778, - "y": 3.455078745186431, - "heading": -0.7196337030492912, - "angularVelocity": -0.5759808898139023, - "velocityX": -1.8511247649982117, - "velocityY": 0.9879856046347999, - "timestamp": 2.5897399852198286 - }, - { - "x": 2.018778748901899, - "y": 3.5069532190081367, - "heading": -0.7498768931574519, - "angularVelocity": -0.3839225853655822, - "velocityX": -1.234208698443955, - "velocityY": 0.6585212086715196, - "timestamp": 2.668514173717653 - }, - { - "x": 1.9701671600341797, - "y": 3.5328900814056396, - "heading": -0.765, - "angularVelocity": -0.19198048410191668, - "velocityX": -0.617100471546741, - "velocityY": 0.32925585007098457, - "timestamp": 2.747288362215478 - }, - { - "x": 1.9701671600341797, - "y": 3.5328900814056396, - "heading": -0.765, - "angularVelocity": -6.585493827087804e-24, - "velocityX": 2.2425722891132833e-25, - "velocityY": -1.917344763102387e-25, - "timestamp": 2.8260625507133024 + "x": 8.23076641369307, + "y": 2.401959089356935, + "heading": 0.2504586013872685, + "angularVelocity": -0.08952800568025769, + "velocityX": -0.19551810957860963, + "velocityY": -0.1334123192654238, + "timestamp": 0.06302387314654201 + }, + { + "x": 8.206023953099224, + "y": 2.3852645245784534, + "heading": 0.2393386250800309, + "angularVelocity": -0.17644070019913852, + "velocityX": -0.39258870263834844, + "velocityY": -0.26489271358590016, + "timestamp": 0.12604774629308402 + }, + { + "x": 8.16875053390707, + "y": 2.3604262651400836, + "heading": 0.22292161113767642, + "angularVelocity": -0.26048881356723236, + "velocityX": -0.5914174634346175, + "velocityY": -0.3941087432157136, + "timestamp": 0.18907161943962603 + }, + { + "x": 8.118819252308954, + "y": 2.32761409583082, + "heading": 0.20141017927873436, + "angularVelocity": -0.34132195920304775, + "velocityX": -0.7922598073592967, + "velocityY": -0.520630797046853, + "timestamp": 0.25209549258616804 + }, + { + "x": 8.05608308887793, + "y": 2.287033381738045, + "heading": 0.17503801825565893, + "angularVelocity": -0.418447164644346, + "velocityX": -0.9954349090090397, + "velocityY": -0.6438943223691957, + "timestamp": 0.31511936573271004 + }, + { + "x": 7.980369725106798, + "y": 2.23893720959337, + "heading": 0.1440830091327847, + "angularVelocity": -0.4911632303349274, + "velocityX": -1.2013441889723828, + "velocityY": -0.7631421196987141, + "timestamp": 0.37814323887925205 + }, + { + "x": 7.891474875927368, + "y": 2.1836442871211665, + "heading": 0.10888684951759207, + "angularVelocity": -0.558457578977337, + "velocityX": -1.4104948607765395, + "velocityY": -0.8773329805300566, + "timestamp": 0.44116711202579406 + }, + { + "x": 7.789153901265338, + "y": 2.121566339688964, + "heading": 0.06988488995899932, + "angularVelocity": -0.6188442190454735, + "velocityX": -1.6235272374345413, + "velocityY": -0.9849909936169672, + "timestamp": 0.5041909851723361 + }, + { + "x": 7.6731119959595455, + "y": 2.0532519037884556, + "heading": 0.027652018184153897, + "angularVelocity": -0.6701091136789754, + "velocityX": -1.8412372885425963, + "velocityY": -1.083945376407843, + "timestamp": 0.5672148583188781 + }, + { + "x": 7.542995375924786, + "y": 1.9794598312116642, + "heading": -0.01702647841112277, + "angularVelocity": -0.7089138506513402, + "velocityX": -2.0645608328801752, + "velocityY": -1.1708590553489355, + "timestamp": 0.6302387314654201 + }, + { + "x": 7.398393469015792, + "y": 1.9012891169513035, + "heading": -0.06304666165055829, + "angularVelocity": -0.7302023969937583, + "velocityX": -2.294398926780781, + "velocityY": -1.2403349771696772, + "timestamp": 0.6932626046119621 + }, + { + "x": 7.238888974084811, + "y": 1.8204184823931229, + "heading": -0.10882385565793654, + "angularVelocity": -0.7263468860591595, + "velocityX": -2.530858339348697, + "velocityY": -1.2831746213080508, + "timestamp": 0.7562864777585041 + }, + { + "x": 7.064291128353646, + "y": 1.7395528396698954, + "heading": -0.1519933648459332, + "angularVelocity": -0.6849707425568674, + "velocityX": -2.770344585538159, + "velocityY": -1.2830954158466277, + "timestamp": 0.8193103509050461 + }, + { + "x": 6.875551934246269, + "y": 1.6630631067471717, + "heading": -0.18835706354611678, + "angularVelocity": -0.5769829254325797, + "velocityX": -2.994725406173053, + "velocityY": -1.213662840823371, + "timestamp": 0.8823342240515881 + }, + { + "x": 6.6765745235709275, + "y": 1.5964044626442309, + "heading": -0.21283448502566993, + "angularVelocity": -0.3883833261506904, + "velocityX": -3.157175221724038, + "velocityY": -1.0576729225756598, + "timestamp": 0.9453580971981301 + }, + { + "x": 6.470320359493221, + "y": 1.5423309052989251, + "heading": -0.22714102483187307, + "angularVelocity": -0.22700191359134367, + "velocityX": -3.272635491603752, + "velocityY": -0.8579853100994786, + "timestamp": 1.0083819703446721 + }, + { + "x": 6.258646009422052, + "y": 1.5021402605713223, + "heading": -0.23470258386507345, + "angularVelocity": -0.11997928174960579, + "velocityX": -3.35863760037386, + "velocityY": -0.6377050904845553, + "timestamp": 1.0714058434912141 + }, + { + "x": 6.043403030940588, + "y": 1.4765880663278637, + "heading": -0.23830057114107106, + "angularVelocity": -0.05708927579921407, + "velocityX": -3.415261038955589, + "velocityY": -0.4054367490878456, + "timestamp": 1.1344297166377562 + }, + { + "x": 5.826227188110352, + "y": 1.4660362005233765, + "heading": -0.2397939206210153, + "angularVelocity": -0.023694981050623704, + "velocityX": -3.4459298038580273, + "velocityY": -0.16742648900603332, + "timestamp": 1.1974535897842982 + }, + { + "x": 5.585950011247348, + "y": 1.472816838427013, + "heading": -0.2400341563641951, + "angularVelocity": -0.003454323639043374, + "velocityX": -3.454919409470775, + "velocityY": 0.09749805540300888, + "timestamp": 1.2669999809980395 + }, + { + "x": 5.346828039308694, + "y": 1.498003817957295, + "heading": -0.24003425372372114, + "angularVelocity": -0.0000013999220426161217, + "velocityX": -3.438308843427179, + "velocityY": 0.3621608409971011, + "timestamp": 1.3365463722117807 + }, + { + "x": 5.110339958561461, + "y": 1.5414445236933543, + "heading": -0.24003430995698885, + "angularVelocity": -8.085720445004714e-7, + "velocityX": -3.4004364082734204, + "velocityY": 0.6246291860428812, + "timestamp": 1.406092763425522 + }, + { + "x": 4.87787710863241, + "y": 1.6028833780076126, + "heading": -0.24003435241212726, + "angularVelocity": -6.104578199921794e-7, + "velocityX": -3.34255805185645, + "velocityY": 0.8834226081614251, + "timestamp": 1.4756391546392633 + }, + { + "x": 4.6508071407642975, + "y": 1.6819589143395182, + "heading": -0.24003438676872058, + "angularVelocity": -4.940097212282082e-7, + "velocityX": -3.26501438687515, + "velocityY": 1.1370185418949712, + "timestamp": 1.5451855458530046 + }, + { + "x": 4.430465976004345, + "y": 1.7782059025459391, + "heading": -0.24003441611574786, + "angularVelocity": -4.2197771504773454e-7, + "velocityX": -3.1682616583621654, + "velocityY": 1.3839249819680632, + "timestamp": 1.614731937066746 + }, + { + "x": 4.218149944366051, + "y": 1.8910580842509719, + "heading": -0.24003444232077203, + "angularVelocity": -3.767991943003991e-7, + "velocityX": -3.0528691414881703, + "velocityY": 1.6226892544028175, + "timestamp": 1.6842783282804872 + }, + { + "x": 4.015108153343392, + "y": 2.0198514989870335, + "heading": -0.24003446662822625, + "angularVelocity": -3.495142422988789e-7, + "velocityX": -2.9195158437284845, + "velocityY": 1.851906511442587, + "timestamp": 1.7538247194942285 + }, + { + "x": 3.822535116912615, + "y": 2.1638283667067197, + "heading": -0.24003448995474508, + "angularVelocity": -3.3540947876441084e-7, + "velocityX": -2.768986759340679, + "velocityY": 2.0702277315467437, + "timestamp": 1.8233711107079698 + }, + { + "x": 3.6415634821904335, + "y": 2.3221412856645247, + "heading": -0.24003451305454837, + "angularVelocity": -3.3214956098454113e-7, + "velocityX": -2.602171465173362, + "velocityY": 2.2763642540595908, + "timestamp": 1.892917501921711 + }, + { + "x": 3.464389353911089, + "y": 2.4846929425572983, + "heading": -0.24003453619468335, + "angularVelocity": -3.3272948572772484e-7, + "velocityX": -2.5475675327972547, + "velocityY": 2.3373126061019125, + "timestamp": 1.9624638931354523 + }, + { + "x": 3.2927692258661447, + "y": 2.639311449150887, + "heading": -0.26906245693036734, + "angularVelocity": -0.4173893171030334, + "velocityX": -2.4677071671122786, + "velocityY": 2.2232427002342847, + "timestamp": 2.0320102843491936 + }, + { + "x": 3.1316790096082485, + "y": 2.78734512036997, + "heading": -0.33414951484331207, + "angularVelocity": -0.9358797311697813, + "velocityX": -2.31629871006257, + "velocityY": 2.128560068115134, + "timestamp": 2.101556675562935 + }, + { + "x": 2.9841668130955736, + "y": 2.9229007119052977, + "heading": -0.399802866713189, + "angularVelocity": -0.9440224104238585, + "velocityX": -2.1210618399927625, + "velocityY": 1.9491391166324632, + "timestamp": 2.171103066776676 + }, + { + "x": 2.850184873056573, + "y": 3.0460054481110204, + "heading": -0.4624392764751436, + "angularVelocity": -0.9006421277769835, + "velocityX": -1.9265117528129565, + "velocityY": 1.7701096211790086, + "timestamp": 2.2406494579904175 + }, + { + "x": 2.729677322997678, + "y": 3.156697524743402, + "heading": -0.5206194141968715, + "angularVelocity": -0.8365658764797604, + "velocityX": -1.7327649638718297, + "velocityY": 1.5916293383532172, + "timestamp": 2.3101958492041588 + }, + { + "x": 2.6226093994758237, + "y": 3.255010853878184, + "heading": -0.5735505408260317, + "angularVelocity": -0.7610909165147557, + "velocityX": -1.5395180346999657, + "velocityY": 1.4136366735784966, + "timestamp": 2.3797422404179 + }, + { + "x": 2.528959064966524, + "y": 3.3409742896619217, + "heading": -0.6207186986875527, + "angularVelocity": -0.6782258150039155, + "velocityX": -1.346587980697349, + "velocityY": 1.2360588994408095, + "timestamp": 2.4492886316316413 + }, + { + "x": 2.448711492549665, + "y": 3.414612123721043, + "heading": -0.6617567306008036, + "angularVelocity": -0.5900814003005007, + "velocityX": -1.153871121367444, + "velocityY": 1.0588304119592038, + "timestamp": 2.5188350228453826 + }, + { + "x": 2.381856029019407, + "y": 3.475944657171963, + "heading": -0.6963860701683602, + "angularVelocity": -0.4979315096469609, + "velocityX": -0.9613074433263825, + "velocityY": 0.8818938320238976, + "timestamp": 2.588381414059124 + }, + { + "x": 2.3283844592549388, + "y": 3.524988689664759, + "heading": -0.7243871660827067, + "angularVelocity": -0.40262471460652677, + "velocityX": -0.7688618896145184, + "velocityY": 0.7051988124310513, + "timestamp": 2.657927805272865 + }, + { + "x": 2.288289941735103, + "y": 3.5617579108116493, + "heading": -0.7455827114186115, + "angularVelocity": -0.30476844256034935, + "velocityX": -0.5765147093917502, + "velocityY": 0.5287006342843157, + "timestamp": 2.7274741964866065 + }, + { + "x": 2.261566307205076, + "y": 3.5862632051940473, + "heading": -0.7598272213603463, + "angularVelocity": -0.20482026016211682, + "velocityX": -0.384256236213544, + "velocityY": 0.35235896435063624, + "timestamp": 2.797020587700348 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -0.1031366044229252, + "velocityX": -0.1920838428798636, + "velocityY": 0.17613685695088044, + "timestamp": 2.866566978914089 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": 2.0585376302520875e-27, + "velocityX": 2.290307616331396e-26, + "velocityY": 1.9208049966092184e-26, + "timestamp": 2.9361133701278304 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/3PieceSourceSide.3.traj b/src/main/deploy/choreo/3PieceSourceSide.3.traj index c6431f7..118469f 100644 --- a/src/main/deploy/choreo/3PieceSourceSide.3.traj +++ b/src/main/deploy/choreo/3PieceSourceSide.3.traj @@ -1,400 +1,544 @@ { "samples": [ { - "x": 1.9701671600341797, - "y": 3.5328900814056396, - "heading": -0.765, - "angularVelocity": -6.585493827087804e-24, - "velocityX": 2.2425722891132833e-25, - "velocityY": -1.917344763102387e-25, + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": 2.0585376302520875e-27, + "velocityX": 2.290307616331396e-26, + "velocityY": 1.9208049966092184e-26, "timestamp": 0 }, { - "x": 2.021866320736439, - "y": 3.5175409291347064, - "heading": -0.7649999986292967, - "angularVelocity": 1.7960508607271394e-8, - "velocityX": 0.6774210494904483, - "velocityY": -0.20112200466940705, - "timestamp": 0.07631761773736923 - }, - { - "x": 2.1252646329115827, - "y": 3.486842627325834, - "heading": -0.7649999949328548, - "angularVelocity": 4.8434973827775214e-8, - "velocityX": 1.354841978047161, - "velocityY": -0.4022439735280295, - "timestamp": 0.15263523547473845 - }, - { - "x": 2.264271046839328, - "y": 3.4455725064565113, - "heading": -0.7629134147129198, - "angularVelocity": 0.027340740995291143, - "velocityX": 1.8214197199669733, - "velocityY": -0.5407679392109075, - "timestamp": 0.22895285321210768 - }, - { - "x": 2.403277460756995, - "y": 3.404302385553228, - "heading": -0.7557796152547765, - "angularVelocity": 0.09347513286765456, - "velocityX": 1.8214197198349207, - "velocityY": -0.5407679396558968, - "timestamp": 0.3052704709494769 - }, - { - "x": 2.5422838746752925, - "y": 3.3630322646520687, - "heading": -0.7445970322006652, - "angularVelocity": 0.1465268883600871, - "velocityX": 1.8214197198431827, - "velocityY": -0.5407679396280685, - "timestamp": 0.3815880886868461 - }, - { - "x": 2.6812902885942673, - "y": 3.321762143753191, - "heading": -0.7301562502874436, - "angularVelocity": 0.1892195058147188, - "velocityX": 1.821419719852059, - "velocityY": -0.540767939598171, - "timestamp": 0.45790570642421535 - }, - { - "x": 2.8202967025138705, - "y": 3.28049202285643, - "heading": -0.7130778427136069, - "angularVelocity": 0.2237806692631405, - "velocityX": 1.8214197198602944, - "velocityY": -0.5407679395704321, - "timestamp": 0.5342233241615846 - }, - { - "x": 2.9593031164340085, - "y": 3.2392219019614696, - "heading": -0.6938417314556549, - "angularVelocity": 0.25205335056643224, - "velocityX": 1.8214197198673001, - "velocityY": -0.5407679395468353, - "timestamp": 0.6105409418989538 - }, - { - "x": 3.0983095303545722, - "y": 3.1979517810679434, - "heading": -0.6728120786629418, - "angularVelocity": 0.27555436629432156, - "velocityX": 1.8214197198728768, - "velocityY": -0.5407679395280517, - "timestamp": 0.686858559636323 - }, - { - "x": 3.2373159442754544, - "y": 3.1566816601754897, - "heading": -0.6502538625008166, - "angularVelocity": 0.2955833375165695, - "velocityX": 1.8214197198770503, - "velocityY": -0.540767939513994, - "timestamp": 0.7631761773736923 - }, - { - "x": 3.376322358196558, - "y": 3.1154115392837802, - "heading": -0.6263485003282439, - "angularVelocity": 0.31323517270727913, - "velocityX": 1.8214197198799456, - "velocityY": -0.5407679395042426, - "timestamp": 0.8394937951110615 - }, - { - "x": 3.515328772117798, - "y": 3.074141418392532, - "heading": -0.6012000869662939, - "angularVelocity": 0.3295230394702948, - "velocityX": 1.821419719881741, - "velocityY": -0.5407679394981955, - "timestamp": 0.9158114128484307 - }, - { - "x": 3.6543351862262687, - "y": 3.032871298131916, - "heading": -0.5748442396833396, - "angularVelocity": 0.3453442083799368, - "velocityX": 1.821419722335047, - "velocityY": -0.5407679312349462, - "timestamp": 0.9921290305857999 - }, - { - "x": 3.7934386683270223, - "y": 2.9919295436941926, - "heading": -0.5472452836552583, - "angularVelocity": 0.3616328293036799, - "velocityX": 1.8226916172809426, - "velocityY": -0.5364653097351076, - "timestamp": 1.0684466483231692 + "x": 2.262787388478086, + "y": 3.590396400582275, + "heading": -0.766999999504867, + "angularVelocity": 7.484697168821697e-9, + "velocityX": 0.2203964012318347, + "velocityY": -0.12269319419383633, + "timestamp": 0.06615271063539385 + }, + { + "x": 2.29194702702894, + "y": 3.5741634259266726, + "heading": -0.7669999984635936, + "angularVelocity": 1.5740448734328548e-8, + "velocityX": 0.4407928000346015, + "velocityY": -0.24538638703819796, + "timestamp": 0.13230542127078682 + }, + { + "x": 2.335686484549911, + "y": 3.5498139641128845, + "heading": -0.7669999968047696, + "angularVelocity": 2.5075676414905366e-8, + "velocityX": 0.6611891954366733, + "velocityY": -0.36807957799329516, + "timestamp": 0.19845813190617978 + }, + { + "x": 2.394005760703551, + "y": 3.5173480153283805, + "heading": -0.7669999944212796, + "angularVelocity": 3.603011826808707e-8, + "velocityX": 0.881585585737703, + "velocityY": -0.49077276611449583, + "timestamp": 0.26461084254157274 + }, + { + "x": 2.466904854927447, + "y": 3.476765579885611, + "heading": -0.7669999911345978, + "angularVelocity": 4.968325032500711e-8, + "velocityX": 1.1019819675369964, + "velocityY": -0.6134659495125361, + "timestamp": 0.3307635531769657 + }, + { + "x": 2.5543837660967745, + "y": 3.428066658409475, + "heading": -0.7669999865876731, + "angularVelocity": 6.873376258795686e-8, + "velocityX": 1.3223783323328206, + "velocityY": -0.7361591234642574, + "timestamp": 0.39691626381235867 + }, + { + "x": 2.6564424908370574, + "y": 3.371251252774671, + "heading": -0.7669999797093515, + "angularVelocity": 1.0397641269909702e-7, + "velocityX": 1.5427746461182656, + "velocityY": -0.8588522690770366, + "timestamp": 0.46306897444775164 + }, + { + "x": 2.7662623647276248, + "y": 3.3101152676874617, + "heading": -0.7638688738714546, + "angularVelocity": 0.047331482078705314, + "velocityX": 1.6600963563813693, + "velocityY": -0.9241644748945469, + "timestamp": 0.5292216850831446 + }, + { + "x": 2.8760822385936273, + "y": 3.2489792825561232, + "heading": -0.7552335462540741, + "angularVelocity": 0.13053626275384136, + "velocityX": 1.6600963560100328, + "velocityY": -0.9241644755616281, + "timestamp": 0.5953743957185376 + }, + { + "x": 2.9859021124597604, + "y": 3.187843297425019, + "heading": -0.7414908207894041, + "angularVelocity": 0.2077424391634433, + "velocityX": 1.660096356012001, + "velocityY": -0.9241644755580926, + "timestamp": 0.6615271063539305 + }, + { + "x": 3.0957219863265877, + "y": 3.1267073122951614, + "heading": -0.722965226886389, + "angularVelocity": 0.2800428542546129, + "velocityX": 1.6600963560224962, + "velocityY": -0.9241644755392399, + "timestamp": 0.7276798169893235 + }, + { + "x": 3.205541937606669, + "y": 3.065571466224591, + "heading": -0.6999120814365468, + "angularVelocity": 0.34848376171464274, + "velocityX": 1.6600975262429405, + "velocityY": -0.9241623734441646, + "timestamp": 0.7938325276247165 + }, + { + "x": 3.3205554118020584, + "y": 3.0148771458100074, + "heading": -0.675189942683574, + "angularVelocity": 0.37371316330831955, + "velocityX": 1.738605615562701, + "velocityY": -0.7663226484246434, + "timestamp": 0.8599852382601094 + }, + { + "x": 3.4412706707327407, + "y": 2.979865153460279, + "heading": -0.6504677784331367, + "angularVelocity": 0.3737135487417248, + "velocityX": 1.8247968642738608, + "velocityY": -0.5292601317986865, + "timestamp": 0.9261379488955024 + }, + { + "x": 3.5655600724891614, + "y": 2.961152646598003, + "heading": -0.6257456072463011, + "angularVelocity": 0.37371365359605785, + "velocityX": 1.878825532054966, + "velocityY": -0.28286833120734667, + "timestamp": 0.9922906595308953 + }, + { + "x": 3.691232955543935, + "y": 2.9590694551388426, + "heading": -0.6010234330400018, + "angularVelocity": 0.3737136992398969, + "velocityX": 1.8997389804241076, + "velocityY": -0.03149064398345659, + "timestamp": 1.0584433701662883 + }, + { + "x": 3.816074269542962, + "y": 2.973652301113666, + "heading": -0.5763012572280835, + "angularVelocity": 0.3737137235113, + "velocityX": 1.8871685347423104, + "velocityY": 0.22044215323538027, + "timestamp": 1.1245960808016813 }, { "x": 3.9378836154937744, "y": 3.0046441555023193, - "heading": -0.5196460913813736, - "angularVelocity": 0.361635924864182, - "velocityX": 1.8926815517726128, - "velocityY": 0.16660126698243774, - "timestamp": 1.1447642660605384 - }, - { - "x": 4.075926460541626, - "y": 3.0786892532968806, - "heading": -0.4898305952486719, - "angularVelocity": 0.3616359432061461, - "velocityX": 1.674339217752807, - "velocityY": 0.8981024049221815, - "timestamp": 1.2272104296480606 - }, - { - "x": 4.180404466119937, - "y": 3.195406219293214, - "heading": -0.4598310277403017, - "angularVelocity": 0.3638685683236616, - "velocityX": 1.2672270125389096, - "velocityY": 1.4156749194573495, - "timestamp": 1.3096565932355828 - }, - { - "x": 4.2848822354372995, - "y": 3.3121233967757027, - "heading": -0.4287713690600015, - "angularVelocity": 0.37672654892338603, - "velocityX": 1.2672241468998562, - "velocityY": 1.4156774845998201, - "timestamp": 1.392102756823105 - }, - { - "x": 4.389360004752337, - "y": 3.428840574260275, - "heading": -0.39697164385324546, - "angularVelocity": 0.38570290991161005, - "velocityX": 1.2672241468716323, - "velocityY": 1.415677484625084, - "timestamp": 1.4745489204106272 - }, - { - "x": 4.493837774065624, - "y": 3.545557751746413, - "heading": -0.36470801458445545, - "angularVelocity": 0.3913296612587668, - "velocityX": 1.2672241468504064, - "velocityY": 1.415677484644084, - "timestamp": 1.5569950839981495 - }, - { - "x": 4.598315543377444, - "y": 3.662274929233865, - "heading": -0.3322260766024941, - "angularVelocity": 0.3939775553956429, - "velocityX": 1.267224146832605, - "velocityY": 1.4156774846600186, - "timestamp": 1.6394412475856717 - }, - { - "x": 4.7027933126881845, - "y": 3.778992106722283, - "heading": -0.29975459263588233, - "angularVelocity": 0.3938507573143906, - "velocityX": 1.2672241468195173, - "velocityY": 1.415677484671734, - "timestamp": 1.7218874111731948 - }, - { - "x": 4.807271113642882, - "y": 3.8957092558850426, - "heading": -0.26751670065443317, - "angularVelocity": 0.3910174904284827, - "velocityX": 1.2672245306330898, - "velocityY": 1.4156771411062206, - "timestamp": 1.8043335747607179 + "heading": -0.5515790804630568, + "angularVelocity": 0.37371373791899654, + "velocityX": 1.841335672882331, + "velocityY": 0.4684895613645789, + "timestamp": 1.1907487914370742 + }, + { + "x": 4.041142175460698, + "y": 3.0442215809369055, + "heading": -0.5298282579971898, + "angularVelocity": 0.37371374601013674, + "velocityX": 1.7741463943907607, + "velocityY": 0.6800031557338396, + "timestamp": 1.248950617562242 + }, + { + "x": 4.139081405257562, + "y": 3.095569514841118, + "heading": -0.5080774349580934, + "angularVelocity": 0.3737137558591309, + "velocityX": 1.68275183644991, + "velocityY": 0.8822392237965997, + "timestamp": 1.3071524436874098 + }, + { + "x": 4.230365088756901, + "y": 3.1579873993126313, + "heading": -0.4863266111818347, + "angularVelocity": 0.3737137685247537, + "velocityX": 1.5683989588750435, + "velocityY": 1.0724385921719906, + "timestamp": 1.3653542698125776 + }, + { + "x": 4.31374781521548, + "y": 3.2306236449287598, + "heading": -0.46457578637658614, + "angularVelocity": 0.3737137862044347, + "velocityX": 1.4326479426824048, + "velocityY": 1.2480062989762448, + "timestamp": 1.4235560959377453 + }, + { + "x": 4.388091972544511, + "y": 3.312487248347592, + "heading": -0.44282495993350157, + "angularVelocity": 0.3737138143450645, + "velocityX": 1.277350940314963, + "velocityY": 1.4065469912022657, + "timestamp": 1.4817579220629131 + }, + { + "x": 4.452383275670078, + "y": 3.40246130939932, + "heading": -0.4210741301794481, + "angularVelocity": 0.3737138712327796, + "velocityX": 1.1046269061610605, + "velocityY": 1.5458975609842933, + "timestamp": 1.539959748188081 + }, + { + "x": 4.505744665604898, + "y": 3.4993182420166447, + "heading": -0.39932328851517956, + "angularVelocity": 0.3737140758692305, + "velocityX": 0.9168336027818437, + "velocityY": 1.6641562484487396, + "timestamp": 1.5981615743132478 + }, + { + "x": 4.553249848192767, + "y": 3.5991779484118642, + "heading": -0.376755167144662, + "angularVelocity": 0.3877562419086832, + "velocityX": 0.8162146405115538, + "velocityY": 1.7157486808139633, + "timestamp": 1.6563634004384147 + }, + { + "x": 4.608641716419732, + "y": 3.694888158238082, + "heading": -0.35375349017349716, + "angularVelocity": 0.3952054171238199, + "velocityX": 0.9517204513109475, + "velocityY": 1.644453725908632, + "timestamp": 1.7145652265635816 + }, + { + "x": 4.674815961157583, + "y": 3.7834865366072044, + "heading": -0.33075180592440745, + "angularVelocity": 0.3952055421701522, + "velocityX": 1.1369788397281038, + "velocityY": 1.5222611431226378, + "timestamp": 1.7727670526887485 + }, + { + "x": 4.750869807438685, + "y": 3.8637642779379298, + "heading": -0.3077501195977143, + "angularVelocity": 0.3952055778666857, + "velocityX": 1.3067261174510774, + "velocityY": 1.37929935665733, + "timestamp": 1.8309688788139153 + }, + { + "x": 4.835765641210989, + "y": 3.9346261199009778, + "heading": -0.2847484322977862, + "angularVelocity": 0.3952055945884119, + "velocityX": 1.4586455344155085, + "velocityY": 1.2175192202845004, + "timestamp": 1.8891707049390822 }, { "x": 4.928345203399658, "y": 3.995105266571045, - "heading": -0.23561923052699993, - "angularVelocity": 0.386888470481369, - "velocityX": 1.4685230274935492, - "velocityY": 1.205586850387367, - "timestamp": 1.886779738348241 - }, - { - "x": 5.0976675546813945, - "y": 4.035971247104969, - "heading": -0.20015091281875375, - "angularVelocity": 0.38688845257830395, - "velocityX": 1.846968413139047, - "velocityY": 0.4457661652272064, - "timestamp": 1.9784555624052222 - }, - { - "x": 5.270991671667468, - "y": 4.053258165584531, - "heading": -0.16565527924639437, - "angularVelocity": 0.37627841284435726, - "velocityX": 1.890619678295383, - "velocityY": 0.18856572774100455, - "timestamp": 2.0701313864622035 - }, - { - "x": 5.444315791533781, - "y": 4.0705450551858755, - "heading": -0.13259854065392815, - "angularVelocity": 0.3605829446585594, - "velocityX": 1.8906197097130382, - "velocityY": 0.1885654127373893, - "timestamp": 2.161807210519185 - }, - { - "x": 5.617639911400379, - "y": 4.087831944784366, - "heading": -0.10138296753519387, - "angularVelocity": 0.34049950943808543, - "velocityX": 1.8906197097161441, - "velocityY": 0.18856541270624588, - "timestamp": 2.253483034576166 - }, - { - "x": 5.790964031267238, - "y": 4.105118834380232, - "heading": -0.07255368127548635, - "angularVelocity": 0.3144698894856807, - "velocityX": 1.8906197097189994, - "velocityY": 0.1885654126776188, - "timestamp": 2.3451588586331473 - }, - { - "x": 5.9642881511343315, - "y": 4.122405723973758, - "heading": -0.046846868579348507, - "angularVelocity": 0.2804099440672579, - "velocityX": 1.890619709721544, - "velocityY": 0.18856541265210713, - "timestamp": 2.4368346826901286 - }, - { - "x": 6.137612271001621, - "y": 4.139692613565316, - "heading": -0.02524481611998314, - "angularVelocity": 0.23563521442620056, - "velocityX": 1.890619709723685, - "velocityY": 0.18856541263063997, - "timestamp": 2.52851050674711 - }, - { - "x": 6.310936390869065, - "y": 4.156979503155326, - "heading": -0.009053820761373118, - "angularVelocity": 0.17661139700851253, - "velocityX": 1.8906197097253696, - "velocityY": 0.18856541261374912, - "timestamp": 2.620186330804091 - }, - { - "x": 6.484260512045991, - "y": 4.174266379616022, - "heading": -2.2378824393561327e-8, - "angularVelocity": 0.09875884373748682, - "velocityX": 1.890619724009207, - "velocityY": 0.188565269399175, - "timestamp": 2.7118621548610724 - }, - { - "x": 6.658350467681885, - "y": 4.179991245269775, - "heading": 1.903926994866919e-23, - "angularVelocity": 2.4410824362649644e-7, - "velocityX": 1.8989734472164426, - "velocityY": 0.06244684149438171, - "timestamp": 2.8035379789180537 - }, - { - "x": 6.892610562082988, - "y": 4.180747179242153, - "heading": 1.815110943876254e-8, - "angularVelocity": 2.0923543279309324e-7, - "velocityX": 2.7004141208851267, - "velocityY": 0.008713967176889024, - "timestamp": 2.8902876713409027 - }, - { - "x": 7.135847606603004, - "y": 4.164439410363542, - "heading": 1.8151086995989633e-8, - "angularVelocity": -2.587072333894115e-13, - "velocityX": 2.803895180796635, - "velocityY": -0.18798647491591486, - "timestamp": 2.9770373637637517 - }, - { - "x": 7.379084647879251, - "y": 4.14813159310281, - "heading": 1.8151064626797785e-8, - "angularVelocity": -2.5785903361398285e-13, - "velocityX": 2.803895143404351, - "velocityY": -0.18798703263687086, - "timestamp": 3.0637870561866007 - }, - { - "x": 7.6223216891554895, - "y": 4.131823775841945, - "heading": 1.8151042187582598e-8, - "angularVelocity": -2.586662220352974e-13, - "velocityX": 2.8038951434042487, - "velocityY": -0.18798703263839134, - "timestamp": 3.1505367486094498 - }, - { - "x": 7.865558730431713, - "y": 4.115515958581081, - "heading": 1.815101972750476e-8, - "angularVelocity": -2.589067144937878e-13, - "velocityX": 2.803895143404087, - "velocityY": -0.18798703263838049, - "timestamp": 3.237286441032299 - }, - { - "x": 8.07413296336415, - "y": 4.101532107856125, - "heading": 9.075510567318239e-9, - "angularVelocity": -1.0461719121664748e-7, - "velocityX": 2.404322448957763, - "velocityY": -0.16119769804825285, - "timestamp": 3.324036133455148 - }, - { - "x": 8.213182462520122, - "y": 4.092209539999878, - "heading": 3.025170279496359e-9, - "angularVelocity": -6.974480391619583e-8, - "velocityX": 1.6028817540723412, - "velocityY": -0.10746514017369478, - "timestamp": 3.410785825877997 - }, - { - "x": 8.282707214355469, - "y": 4.08754825592041, - "heading": -2.730279460976851e-23, - "angularVelocity": -3.487240352104816e-8, - "velocityX": 0.8014409030577104, - "velocityY": -0.05373257183146045, - "timestamp": 3.497535518300846 - }, - { - "x": 8.282707214355469, - "y": 4.08754825592041, - "heading": -1.3454200564607368e-23, - "angularVelocity": 4.663099853298001e-24, - "velocityX": 1.7036719365270032e-27, - "velocityY": 2.800838691430789e-26, - "timestamp": 3.584285210723695 + "heading": -0.2617467444324387, + "angularVelocity": 0.39520560430321505, + "velocityX": 1.5906642171255918, + "velocityY": 1.0391279912764597, + "timestamp": 1.9473725310642491 + }, + { + "x": 5.054848385978205, + "y": 4.053342311213538, + "heading": -0.23277931082663741, + "angularVelocity": 0.3952056123787909, + "velocityX": 1.7258956530000658, + "velocityY": 0.7945338618626627, + "timestamp": 2.0206696521032006 + }, + { + "x": 5.188526339981156, + "y": 4.092391169041153, + "heading": -0.20381187629917086, + "angularVelocity": 0.39520562495316197, + "velocityX": 1.8237817817143471, + "velocityY": 0.5327474977750234, + "timestamp": 2.093966773142152 + }, + { + "x": 5.326486517103212, + "y": 4.111406906532098, + "heading": -0.17484444007241956, + "angularVelocity": 0.3952056481366752, + "velocityX": 1.8822045827521574, + "velocityY": 0.25943362060346054, + "timestamp": 2.1672638941811035 + }, + { + "x": 5.465743715620603, + "y": 4.1099781194933795, + "heading": -0.1458769989760029, + "angularVelocity": 0.3952057145740096, + "velocityX": 1.8998999761994304, + "velocityY": -0.01949308538270602, + "timestamp": 2.240561015220055 + }, + { + "x": 5.604302908818607, + "y": 4.095979592520542, + "heading": -0.1165828656276628, + "angularVelocity": 0.3996628098499595, + "velocityX": 1.8903770193698237, + "velocityY": -0.19098331250144138, + "timestamp": 2.3138581362590065 + }, + { + "x": 5.7428620917203625, + "y": 4.08198096363453, + "heading": -0.08691799128062003, + "angularVelocity": 0.4047208666119116, + "velocityX": 1.8903768788971027, + "velocityY": -0.19098470291312333, + "timestamp": 2.387155257297958 + }, + { + "x": 5.881421274622045, + "y": 4.067982334747774, + "heading": -0.057160358126426926, + "angularVelocity": 0.40598638435443013, + "velocityX": 1.8903768788960766, + "velocityY": -0.19098470292328013, + "timestamp": 2.4604523783369094 + }, + { + "x": 6.019980457523705, + "y": 4.053983705860801, + "heading": -0.027589119251388173, + "angularVelocity": 0.4034433884425539, + "velocityX": 1.8903768788957764, + "velocityY": -0.19098470292625017, + "timestamp": 2.533749499375861 + }, + { + "x": 6.158539640425345, + "y": 4.039985076973634, + "heading": 0.0015129980186348265, + "angularVelocity": 0.39704311516624813, + "velocityX": 1.8903768788955113, + "velocityY": -0.19098470292887434, + "timestamp": 2.6070466204148124 + }, + { + "x": 6.297098823326969, + "y": 4.025986448086315, + "heading": 0.029855200510632778, + "angularVelocity": 0.3866755213610124, + "velocityX": 1.8903768788953004, + "velocityY": -0.19098470293096353, + "timestamp": 2.680343741453764 + }, + { + "x": 6.435658006228585, + "y": 4.011987819198895, + "heading": 0.05713171335387273, + "angularVelocity": 0.3721362102168317, + "velocityX": 1.8903768788951598, + "velocityY": -0.19098470293235562, + "timestamp": 2.7536408624927153 + }, + { + "x": 6.574217189130194, + "y": 3.9979891903114315, + "heading": 0.08301592008791062, + "angularVelocity": 0.35314083782748296, + "velocityX": 1.8903768788951019, + "velocityY": -0.19098470293292794, + "timestamp": 2.826937983531667 + }, + { + "x": 6.712776372031808, + "y": 3.983990561423991, + "heading": 0.10715451282177094, + "angularVelocity": 0.3293252503196199, + "velocityX": 1.8903768788951338, + "velocityY": -0.19098470293261177, + "timestamp": 2.9002351045706183 + }, + { + "x": 6.851335554933429, + "y": 3.9699919325366397, + "heading": 0.12916191998569537, + "angularVelocity": 0.30024927107613325, + "velocityX": 1.8903768788952555, + "velocityY": -0.1909847029314087, + "timestamp": 2.9735322256095698 + }, + { + "x": 6.989894737835075, + "y": 3.9559933036495325, + "heading": 0.14861220477274031, + "angularVelocity": 0.26536219310317194, + "velocityX": 1.8903768788955921, + "velocityY": -0.1909847029280752, + "timestamp": 3.0468293466485212 + }, + { + "x": 7.128453922674329, + "y": 3.9419946939409853, + "heading": 0.165034903130564, + "angularVelocity": 0.2240565267099137, + "velocityX": 1.8903769053305712, + "velocityY": -0.19098444127304595, + "timestamp": 3.1201264676874727 + }, + { + "x": 7.267333565421739, + "y": 3.9316480226780426, + "heading": 0.17810972743336856, + "angularVelocity": 0.17838114399958924, + "velocityX": 1.8947489448269792, + "velocityY": -0.1411606774765978, + "timestamp": 3.193423588726424 + }, + { + "x": 7.406228542327881, + "y": 3.941786766052246, + "heading": 0.1911844788186837, + "angularVelocity": 0.1783801491789407, + "velocityX": 1.8949581502980173, + "velocityY": 0.13832389636170525, + "timestamp": 3.2667207097653757 + }, + { + "x": 7.565442985719265, + "y": 3.9581600818899694, + "heading": 0.2043161039154577, + "angularVelocity": 0.1783801287415228, + "velocityX": 2.162770616764113, + "velocityY": 0.22241528870453642, + "timestamp": 3.3403366735468696 + }, + { + "x": 7.726607611629632, + "y": 3.9948081017418207, + "heading": 0.2081850028863188, + "angularVelocity": 0.052555162931029055, + "velocityX": 2.189261915917248, + "velocityY": 0.49782707403830934, + "timestamp": 3.4139526373283635 + }, + { + "x": 7.867453146090771, + "y": 4.027711655278318, + "heading": 0.21132312519311341, + "angularVelocity": 0.04262828530112288, + "velocityX": 1.913247171214066, + "velocityY": 0.44696220556402627, + "timestamp": 3.4875686011098574 + }, + { + "x": 7.988106192724927, + "y": 4.056232601254953, + "heading": 0.21391875024315468, + "angularVelocity": 0.03525899705321495, + "velocityX": 1.6389522114018413, + "velocityY": 0.38742881994034783, + "timestamp": 3.5611845648913514 + }, + { + "x": 8.088612865120389, + "y": 4.0801619694506375, + "heading": 0.2160337271012685, + "angularVelocity": 0.02872986713033435, + "velocityX": 1.3652836590414905, + "velocityY": 0.3250567807101139, + "timestamp": 3.6348005286728453 + }, + { + "x": 8.168996921740131, + "y": 4.099396001381588, + "heading": 0.2176988235230518, + "angularVelocity": 0.022618686712105623, + "velocityX": 1.0919378418835648, + "velocityY": 0.26127528518191084, + "timestamp": 3.7084164924543392 + }, + { + "x": 8.229272838087912, + "y": 4.11387267730592, + "heading": 0.21893245113612445, + "angularVelocity": 0.016757610030540077, + "velocityX": 0.8187886601157901, + "velocityY": 0.19665131284977577, + "timestamp": 3.782032456235833 + }, + { + "x": 8.269450355435243, + "y": 4.123550749253992, + "heading": 0.21974686245033065, + "angularVelocity": 0.011062971567194144, + "velocityX": 0.5457718038791941, + "velocityY": 0.13146702767891838, + "timestamp": 3.855648420017327 + }, + { + "x": 8.289536476135254, + "y": 4.128400802612305, + "heading": 0.22015079613001912, + "angularVelocity": 0.005487039209150268, + "velocityX": 0.2728500676786763, + "velocityY": 0.06588317409942288, + "timestamp": 3.929264383798821 + }, + { + "x": 8.289536476135254, + "y": 4.128400802612305, + "heading": 0.22015079613001912, + "angularVelocity": 3.037718139065862e-28, + "velocityX": -8.872519474147407e-27, + "velocityY": 3.446506807522548e-26, + "timestamp": 4.002880347580315 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/3PieceSourceSide.4.traj b/src/main/deploy/choreo/3PieceSourceSide.4.traj index 726d6c5..79477bd 100644 --- a/src/main/deploy/choreo/3PieceSourceSide.4.traj +++ b/src/main/deploy/choreo/3PieceSourceSide.4.traj @@ -1,364 +1,472 @@ { "samples": [ { - "x": 8.282707214355469, - "y": 4.08754825592041, - "heading": -1.3454200564607368e-23, - "angularVelocity": 4.663099853298001e-24, - "velocityX": 1.7036719365270032e-27, - "velocityY": 2.800838691430789e-26, + "x": 8.289536476135254, + "y": 4.128400802612305, + "heading": 0.22015079613001912, + "angularVelocity": 3.037718139065862e-28, + "velocityX": -8.872519474147407e-27, + "velocityY": 3.446506807522548e-26, "timestamp": 0 }, { - "x": 8.214708333565108, - "y": 4.098060716631318, - "heading": 1.6847207046591105e-8, - "angularVelocity": 1.954345432230813e-7, - "velocityX": -0.7888150344560528, - "velocityY": 0.12194887565072522, - "timestamp": 0.08620383463818193 - }, - { - "x": 8.078710576708064, - "y": 4.119085637322864, - "heading": 5.0541618776805506e-8, - "angularVelocity": 3.9086905903476216e-7, - "velocityX": -1.577630014115494, - "velocityY": 0.24389774283002824, - "timestamp": 0.17240766927636386 - }, - { - "x": 7.8747139579553656, - "y": 4.15062301580424, - "heading": 1.0108322622636101e-7, - "angularVelocity": 5.863034708570761e-7, - "velocityX": -2.3664448293851432, - "velocityY": 0.3658465845950713, - "timestamp": 0.2586115039145458 - }, - { - "x": 7.635308863013457, - "y": 4.1876344581296925, - "heading": 1.0108335901717796e-7, - "angularVelocity": 1.540428189705246e-12, - "velocityX": -2.7771977423829006, - "velocityY": 0.42934798064143237, - "timestamp": 0.3448153385527277 - }, - { - "x": 7.395903768071533, - "y": 4.224645900455147, - "heading": 1.0108349186566566e-7, - "angularVelocity": 1.5410971948056658e-12, - "velocityX": -2.7771977423830965, - "velocityY": 0.42934798064146257, - "timestamp": 0.43101917319090965 - }, - { - "x": 7.156498673129608, - "y": 4.261657342780601, - "heading": 1.0108362465293872e-7, - "angularVelocity": 1.5403870787070826e-12, - "velocityX": -2.7771977423830965, - "velocityY": 0.42934798064146257, - "timestamp": 0.5172230078290916 - }, - { - "x": 6.917093578187684, - "y": 4.298668785106054, - "heading": 1.0108375751155647e-7, - "angularVelocity": 1.5412147075799493e-12, - "velocityX": -2.7771977423830965, - "velocityY": 0.4293479806414625, - "timestamp": 0.6034268424672735 - }, - { - "x": 6.677688483245759, - "y": 4.335680227431509, - "heading": 1.0108389035423936e-7, - "angularVelocity": 1.5410298558530182e-12, - "velocityX": -2.7771977423830965, - "velocityY": 0.4293479806414627, - "timestamp": 0.6896306771054554 - }, - { - "x": 6.438283388303835, - "y": 4.372691669756963, - "heading": 1.0108402320108348e-7, - "angularVelocity": 1.5410781280848912e-12, - "velocityX": -2.777197742383096, - "velocityY": 0.4293479806414625, - "timestamp": 0.7758345117436374 - }, - { - "x": 6.1988782933619095, - "y": 4.409703112082417, - "heading": 1.0108415606074574e-7, - "angularVelocity": 1.5412268238689798e-12, - "velocityX": -2.7771977423830965, - "velocityY": 0.42934798064146257, - "timestamp": 0.8620383463818193 - }, - { - "x": 5.959473198419985, - "y": 4.446714554407871, - "heading": 1.0108428894851187e-7, - "angularVelocity": 1.5415528401754331e-12, - "velocityX": -2.777197742383096, - "velocityY": 0.42934798064146257, - "timestamp": 0.9482421810200012 - }, - { - "x": 5.72006810347805, - "y": 4.483725996733255, - "heading": 1.0108442171706879e-7, - "angularVelocity": 1.5401699643641334e-12, - "velocityX": -2.7771977423832217, - "velocityY": 0.4293479806406499, - "timestamp": 1.0344460156581832 - }, - { - "x": 5.480663004845917, - "y": 4.520737415188933, - "heading": 1.0108455459674728e-7, - "angularVelocity": 1.5414590205195092e-12, - "velocityX": -2.7771977851910266, - "velocityY": 0.4293477037422273, - "timestamp": 1.120649850296365 - }, - { - "x": 5.240152409397331, - "y": 4.549708085687679, - "heading": 1.0108468832096651e-7, - "angularVelocity": 1.5512560408121339e-12, - "velocityX": -2.790022003755009, - "velocityY": 0.3360717144468437, - "timestamp": 1.206853684934547 - }, - { - "x": 5.001198008888612, - "y": 4.509890511709968, - "heading": 1.0108464169686405e-7, - "angularVelocity": -5.408587989447552e-13, - "velocityX": -2.7719695012602092, - "velocityY": -0.46190026400605494, - "timestamp": 1.293057519572729 - }, - { - "x": 4.783077239990234, - "y": 4.404496192932129, - "heading": 4.371418464571693e-25, - "angularVelocity": -0.0000011726234931559648, - "velocityX": -2.5302907905881877, - "velocityY": -1.2226175229930707, - "timestamp": 1.379261354210911 - }, - { - "x": 4.627041500035927, - "y": 4.275874365802291, - "heading": -0.00461711894400582, - "angularVelocity": -0.06368383973540494, - "velocityX": -2.152198194752189, - "velocityY": -1.774078581199584, - "timestamp": 1.4517619911572552 - }, - { - "x": 4.5068214168723975, - "y": 4.114451023957695, - "heading": -0.01240880783073842, - "angularVelocity": -0.10747062667185843, - "velocityX": -1.6581934756311134, - "velocityY": -2.226509292105373, - "timestamp": 1.5242626281035996 - }, - { - "x": 4.429140519397286, - "y": 3.930818027867112, - "heading": -0.027772737926692166, - "angularVelocity": -0.21191441541850167, - "velocityX": -1.0714512416297768, - "velocityY": -2.53284665935394, - "timestamp": 1.596763265049944 - }, - { - "x": 4.383091867034577, - "y": 3.7471675526312227, - "heading": -0.08613270246137088, - "angularVelocity": -0.8049579561331118, - "velocityX": -0.635148245618701, - "velocityY": -2.5330877488952703, - "timestamp": 1.6692639019962883 - }, - { - "x": 4.346524489213487, - "y": 3.5742818254238546, - "heading": -0.20843940130739744, - "angularVelocity": -1.6869741287451163, - "velocityX": -0.5043731939645176, - "velocityY": -2.3846097701915894, - "timestamp": 1.7417645389426326 - }, - { - "x": 4.289423143132127, - "y": 3.4062373324265396, - "heading": -0.32755846162196556, - "angularVelocity": -1.643007086995987, - "velocityX": -0.7875978541211613, - "velocityY": -2.317834712565055, - "timestamp": 1.814265175888977 - }, - { - "x": 4.203799594769533, - "y": 3.2403627849834162, - "heading": -0.40165980770192133, - "angularVelocity": -1.022078552700102, - "velocityX": -1.1810040845014969, - "velocityY": -2.28790469200817, - "timestamp": 1.8867658128353213 - }, - { - "x": 4.079421660594405, - "y": 3.103563620218193, - "heading": -0.4716628621936354, - "angularVelocity": -0.9655508894841919, - "velocityX": -1.7155426409174284, - "velocityY": -1.8868684542242327, - "timestamp": 1.9592664497816656 + "x": 8.269996806011259, + "y": 4.137231467736895, + "heading": 0.22023861459550317, + "angularVelocity": 0.0011710803997845823, + "velocityX": -0.2605662097867229, + "velocityY": 0.11775904745621692, + "timestamp": 0.0749892710186355 + }, + { + "x": 8.230762965216833, + "y": 4.154542716289096, + "heading": 0.22035463158445837, + "angularVelocity": 0.0015471145055716861, + "velocityX": -0.5231927215918144, + "velocityY": 0.23084967112027163, + "timestamp": 0.149978542037271 + }, + { + "x": 8.171657212153168, + "y": 4.179909246382009, + "heading": 0.22042856436582195, + "angularVelocity": 0.0009859114558561591, + "velocityX": -0.7881894604493174, + "velocityY": 0.3382687916330005, + "timestamp": 0.22496781305590652 + }, + { + "x": 8.092474127272867, + "y": 4.212804284514623, + "heading": 0.22037649675261314, + "angularVelocity": -0.0006943341694284194, + "velocityX": -1.0559255184734737, + "velocityY": 0.4386632605674932, + "timestamp": 0.299957084074542 + }, + { + "x": 7.992976017836279, + "y": 4.252560097776598, + "heading": 0.22009679360683257, + "angularVelocity": -0.003729908852042051, + "velocityX": -1.3268312664602737, + "velocityY": 0.5301533502318783, + "timestamp": 0.37494635509317753 + }, + { + "x": 7.87288978503091, + "y": 4.298306284549337, + "heading": 0.21946436859290286, + "angularVelocity": -0.008433539962971137, + "velocityX": -1.6013788529231974, + "velocityY": 0.6100364245622665, + "timestamp": 0.44993562611181304 + }, + { + "x": 7.731910356436966, + "y": 4.348869169420612, + "heading": 0.21832244216002894, + "angularVelocity": -0.015227864164596877, + "velocityX": -1.8799946536206542, + "velocityY": 0.6742682544374918, + "timestamp": 0.5249248971304485 + }, + { + "x": 7.569726421911416, + "y": 4.402600574685328, + "heading": 0.21647017920240058, + "angularVelocity": -0.024700372899585307, + "velocityX": -2.162761850095162, + "velocityY": 0.7165212374362642, + "timestamp": 0.599914168149084 + }, + { + "x": 7.386118026863393, + "y": 4.4570773650237445, + "heading": 0.21364200538162792, + "angularVelocity": -0.0377143794352908, + "velocityX": -2.4484621940436835, + "velocityY": 0.7264611270174774, + "timestamp": 0.6749034391677196 + }, + { + "x": 7.181283119008862, + "y": 4.5085907210335865, + "heading": 0.20946226202822887, + "angularVelocity": -0.055737884855026475, + "velocityX": -2.731522857498233, + "velocityY": 0.6869430161154807, + "timestamp": 0.7498927101863551 + }, + { + "x": 6.956820012371809, + "y": 4.551523922612317, + "heading": 0.20331318020756176, + "angularVelocity": -0.08199948788859508, + "velocityX": -2.9932696182801153, + "velocityY": 0.5725245891250452, + "timestamp": 0.8248819812049906 + }, + { + "x": 6.7176650220814205, + "y": 4.57889627508473, + "heading": 0.19410641329547768, + "angularVelocity": -0.12277445542571251, + "velocityX": -3.189189427257647, + "velocityY": 0.3650169164281033, + "timestamp": 0.8998712522236261 + }, + { + "x": 6.472067123940792, + "y": 4.58587963769058, + "heading": 0.18079068250006503, + "angularVelocity": -0.1775684789908641, + "velocityX": -3.2751071560569964, + "velocityY": 0.09312482320456428, + "timestamp": 0.9748605232422616 + }, + { + "x": 6.227490944705127, + "y": 4.571499129973418, + "heading": 0.16337266739917577, + "angularVelocity": -0.23227342877570947, + "velocityX": -3.261482288244776, + "velocityY": -0.1917675358330633, + "timestamp": 1.049849794260897 + }, + { + "x": 5.988790028800353, + "y": 4.536538809179358, + "heading": 0.14241529165637518, + "angularVelocity": -0.2794716558531782, + "velocityX": -3.1831342359023242, + "velocityY": -0.4662043025511265, + "timestamp": 1.1248390652795326 + }, + { + "x": 5.758924851198093, + "y": 4.482073744796014, + "heading": 0.11847095232121185, + "angularVelocity": -0.31930353515789156, + "velocityX": -3.0653075363959554, + "velocityY": -0.7263047585808593, + "timestamp": 1.199828336298168 + }, + { + "x": 5.5397707564905145, + "y": 4.409059239841931, + "heading": 0.09196886032537212, + "angularVelocity": -0.35341178325702044, + "velocityX": -2.92247266483119, + "velocityY": -0.9736660186486978, + "timestamp": 1.2748176073168036 + }, + { + "x": 5.33258476452675, + "y": 4.318274659916258, + "heading": 0.06323280289856088, + "angularVelocity": -0.3832022506215635, + "velocityX": -2.7628751306607398, + "velocityY": -1.21063425063984, + "timestamp": 1.349806878335439 + }, + { + "x": 5.138254335054246, + "y": 4.210347005162901, + "heading": 0.03251123556536211, + "angularVelocity": -0.4096795036927947, + "velocityX": -2.5914431068974344, + "velocityY": -1.4392412846170675, + "timestamp": 1.4247961493540746 + }, + { + "x": 4.957433700561523, + "y": 4.085783958435059, + "heading": -7.345594756701693e-29, + "angularVelocity": -0.4335451608441839, + "velocityX": -2.41128673524227, + "velocityY": -1.661078245405117, + "timestamp": 1.4997854203727101 + }, + { + "x": 4.850763114224524, + "y": 4.00236715124268, + "heading": -0.02077718123853542, + "angularVelocity": -0.4471189141923733, + "velocityX": -2.2955200800195295, + "velocityY": -1.795105497182433, + "timestamp": 1.5462544481392158 + }, + { + "x": 4.749732174637679, + "y": 3.9129578387828414, + "heading": -0.04223315292346125, + "angularVelocity": -0.4617262877273038, + "velocityX": -2.174156517637868, + "velocityY": -1.9240624725160098, + "timestamp": 1.5927234759057214 + }, + { + "x": 4.654715925578021, + "y": 3.817934442549008, + "heading": -0.06443390851309351, + "angularVelocity": -0.47775382134493777, + "velocityX": -2.0447221219494662, + "velocityY": -2.0448759270647874, + "timestamp": 1.639192503672227 + }, + { + "x": 4.566286302759079, + "y": 3.717984210531405, + "heading": -0.08747291743607646, + "angularVelocity": -0.4957927899578095, + "velocityX": -1.9029798355858998, + "velocityY": -2.1509000041882853, + "timestamp": 1.6856615314387327 + }, + { + "x": 4.485327184216237, + "y": 3.614602306633809, + "heading": -0.11148870772971044, + "angularVelocity": -0.5168128417557317, + "velocityX": -1.7422167502543655, + "velocityY": -2.2247485877488473, + "timestamp": 1.7321305592052383 + }, + { + "x": 4.412567946745739, + "y": 3.511651220217229, + "heading": -0.13677526525666567, + "angularVelocity": -0.5441593840528177, + "velocityX": -1.5657576878107506, + "velocityY": -2.21547752050852, + "timestamp": 1.778599586971744 + }, + { + "x": 4.345051921407887, + "y": 3.4150129773556204, + "heading": -0.1637765662543946, + "angularVelocity": -0.581060166212278, + "velocityX": -1.4529252834189115, + "velocityY": -2.079626958136264, + "timestamp": 1.8250686147382496 + }, + { + "x": 4.278849963154741, + "y": 3.3264929134982726, + "heading": -0.19187383355887797, + "angularVelocity": -0.6046450432676215, + "velocityX": -1.4246469408783942, + "velocityY": -1.9049261005015221, + "timestamp": 1.8715376425047552 + }, + { + "x": 4.21205059305352, + "y": 3.246181936974994, + "heading": -0.22056728070035955, + "angularVelocity": -0.6174746604482066, + "velocityX": -1.437503070580043, + "velocityY": -1.728268921975704, + "timestamp": 1.9180066702712608 + }, + { + "x": 4.143696181766486, + "y": 3.1739556218387466, + "heading": -0.24959647824597708, + "angularVelocity": -0.6246999117666328, + "velocityX": -1.4709671058860057, + "velocityY": -1.5542893537425588, + "timestamp": 1.9644756980377664 + }, + { + "x": 4.073234187126779, + "y": 3.109688534157438, + "heading": -0.27881410074968266, + "angularVelocity": -0.6287547622152977, + "velocityX": -1.5163216883675306, + "velocityY": -1.3830090873480942, + "timestamp": 2.010944725804272 + }, + { + "x": 4.000309863583725, + "y": 3.0532784271020903, + "heading": -0.30812855213660195, + "angularVelocity": -0.6308384916985205, + "velocityX": -1.5693102922118025, + "velocityY": -1.2139291430583163, + "timestamp": 2.0574137535707777 }, { "x": 3.924677610397339, "y": 3.0046441555023193, - "heading": -0.5385444180262977, - "angularVelocity": -0.9224961138225419, - "velocityX": -2.134381940831591, - "velocityY": -1.3643944230321734, - "timestamp": 2.03176708672801 - }, - { - "x": 3.7636444729485263, - "y": 2.9505820220343484, - "heading": -0.5968694156691691, - "angularVelocity": -0.8693980375808563, - "velocityX": -2.40037547092122, - "velocityY": -0.8058553732360577, - "timestamp": 2.0988537318798244 - }, - { - "x": 3.5921162225925003, - "y": 2.93585850684271, - "heading": -0.6464965554196345, - "angularVelocity": -0.7397469293353471, - "velocityX": -2.556816635678569, - "velocityY": -0.21947013684048228, - "timestamp": 2.1659403770316388 - }, - { - "x": 3.416479572667743, - "y": 2.9596548551854616, - "heading": -0.6807775343446694, - "angularVelocity": -0.5109955766525223, - "velocityX": -2.6180568357129337, - "velocityY": 0.3547106624410992, - "timestamp": 2.233027022183453 - }, - { - "x": 3.2390028785000764, - "y": 3.015476958858974, - "heading": -0.6885344713310354, - "angularVelocity": -0.115625650512295, - "velocityX": -2.645484712583867, - "velocityY": 0.8320896587866112, - "timestamp": 2.3001136673352676 - }, - { - "x": 3.0657580475568027, - "y": 3.089829788896707, - "heading": -0.6885346535011025, - "angularVelocity": -0.00000271544458244769, - "velocityX": -2.5824041513959415, - "velocityY": 1.1083104523929412, - "timestamp": 2.367200312487082 - }, - { - "x": 2.8925132745209385, - "y": 3.1641827538610285, - "heading": -0.6885348356707467, - "angularVelocity": -0.000002715438277887047, - "velocityX": -2.582403288222506, - "velocityY": 1.1083124636216772, - "timestamp": 2.4342869576388964 - }, - { - "x": 2.7192685014854168, - "y": 3.2385357188261574, - "heading": -0.6885350178403685, - "angularVelocity": -0.000002715437944094969, - "velocityX": -2.5824032882174053, - "velocityY": 1.108312463633708, - "timestamp": 2.5013736027907107 - }, - { - "x": 2.5460237284499403, - "y": 3.312888683791401, - "heading": -0.6885352000099678, - "angularVelocity": -0.0000027154376105306044, - "velocityX": -2.5824032882167316, - "velocityY": 1.1083124636354227, - "timestamp": 2.568460247942525 - }, - { - "x": 2.3727789637362293, - "y": 3.3872416681385586, - "heading": -0.6885353821903477, - "angularVelocity": -0.0000027155983032171972, - "velocityX": -2.5824031641716974, - "velocityY": 1.108312752544088, - "timestamp": 2.6355468930943395 - }, - { - "x": 2.2223047239417384, - "y": 3.4507934701253355, - "heading": -0.7191097564620745, - "angularVelocity": -0.45574457036177557, - "velocityX": -2.2429835245744076, - "velocityY": 0.9473092870117661, - "timestamp": 2.702633538246154 - }, - { - "x": 2.109444085282166, - "y": 3.4984470664225062, - "heading": -0.7420547518086502, - "angularVelocity": -0.34202031260695603, - "velocityX": -1.6823115599859222, - "velocityY": 0.7103291003646417, - "timestamp": 2.7697201833979683 - }, - { - "x": 2.0342014899368133, - "y": 3.5302130152316624, - "heading": -0.7573535418351998, - "angularVelocity": -0.22804523898800838, - "velocityX": -1.1215733798445486, - "velocityY": 0.47350629528829474, - "timestamp": 2.8368068285497827 - }, - { - "x": 1.996579647064209, - "y": 3.5460963249206543, - "heading": -0.765, - "angularVelocity": -0.11397884254752216, - "velocityX": -0.5607948167249468, - "velocityY": 0.23675814542593593, - "timestamp": 2.903893473701597 - }, - { - "x": 1.996579647064209, - "y": 3.5460963249206543, - "heading": -0.765, - "angularVelocity": 6.11903975445096e-25, - "velocityX": -4.7665538572669345e-25, - "velocityY": 2.8118183260637723e-25, - "timestamp": 2.9709801188534115 + "heading": -0.3374787219433775, + "angularVelocity": -0.6316071417343185, + "velocityX": -1.627584152748309, + "velocityY": -1.0465954193004559, + "timestamp": 2.1038827813372833 + }, + { + "x": 3.8052119909980813, + "y": 2.949187806356286, + "heading": -0.3813210096823129, + "angularVelocity": -0.6313779373848833, + "velocityX": -1.720438422690351, + "velocityY": -0.798633400410426, + "timestamp": 2.1733218370010388 + }, + { + "x": 3.6796645340407665, + "y": 2.911082197769309, + "heading": -0.4250077410083283, + "angularVelocity": -0.6291377512038759, + "velocityX": -1.808023680005878, + "velocityY": -0.5487633468331752, + "timestamp": 2.242760892664794 + }, + { + "x": 3.548510692212512, + "y": 2.8904863098039106, + "heading": -0.4683618068061113, + "angularVelocity": -0.6243469958421788, + "velocityX": -1.8887618873065017, + "velocityY": -0.29660380269469544, + "timestamp": 2.3121999483285496 + }, + { + "x": 3.4123915084504293, + "y": 2.887591555300515, + "heading": -0.5111475650763733, + "angularVelocity": -0.6161627323597821, + "velocityX": -1.960268359944496, + "velocityY": -0.04168769974944769, + "timestamp": 2.381639003992305 + }, + { + "x": 3.2722134837537085, + "y": 2.9026251488079913, + "heading": -0.5530315041603286, + "angularVelocity": -0.6031755282901653, + "velocityX": -2.0187202051754043, + "velocityY": 0.21650054661275753, + "timestamp": 2.4510780596560604 + }, + { + "x": 3.129341493242039, + "y": 2.9358368291248627, + "heading": -0.5935016635404317, + "angularVelocity": -0.5828155206498206, + "velocityX": -2.0575163234289917, + "velocityY": 0.4782853107578603, + "timestamp": 2.520517115319816 + }, + { + "x": 2.9860054741751014, + "y": 2.987405262527824, + "heading": -0.6316842461722715, + "angularVelocity": -0.5498718590980219, + "velocityX": -2.0641988531787505, + "velocityY": 0.7426430689477005, + "timestamp": 2.5899561709835712 + }, + { + "x": 2.846192204303753, + "y": 3.0569532479597985, + "heading": -0.6659032700236993, + "angularVelocity": -0.492792183366187, + "velocityX": -2.0134673280749626, + "velocityY": 1.0015687103918496, + "timestamp": 2.6593952266473266 + }, + { + "x": 2.716907495900805, + "y": 3.1413547514524796, + "heading": -0.6931863548996687, + "angularVelocity": -0.39290691117809146, + "velocityX": -1.8618442772174881, + "velocityY": 1.2154759693360322, + "timestamp": 2.728834282311082 + }, + { + "x": 2.6049527292797388, + "y": 3.231193791663575, + "heading": -0.7132381462616506, + "angularVelocity": -0.288768203575217, + "velocityX": -1.6122737492742676, + "velocityY": 1.293782574551762, + "timestamp": 2.7982733379748375 + }, + { + "x": 2.510838263874455, + "y": 3.3171226961552893, + "heading": -0.728589225015942, + "angularVelocity": -0.2210726889580136, + "velocityX": -1.3553534751540288, + "velocityY": 1.2374722505992761, + "timestamp": 2.867712393638593 + }, + { + "x": 2.4328922690741286, + "y": 3.3944440167178684, + "heading": -0.7405972040878053, + "angularVelocity": -0.17292831760284247, + "velocityX": -1.1225094301075331, + "velocityY": 1.1135134230078372, + "timestamp": 2.9371514493023483 + }, + { + "x": 2.3697038520475897, + "y": 3.4608749704927635, + "heading": -0.7499497576912365, + "angularVelocity": -0.13468722340809483, + "velocityX": -0.9099838185086581, + "velocityY": 0.9566799712336846, + "timestamp": 3.0065905049661037 + }, + { + "x": 2.3202711712076995, + "y": 3.5151563459749755, + "heading": -0.757044699270628, + "angularVelocity": -0.10217508737082179, + "velocityX": -0.711885845326848, + "velocityY": 0.781712466613299, + "timestamp": 3.076029560629859 + }, + { + "x": 2.2838770908763535, + "y": 3.556511565024103, + "heading": -0.7621393736392112, + "angularVelocity": -0.07336900422801312, + "velocityX": -0.5241154273119383, + "velocityY": 0.5955613689417452, + "timestamp": 3.1454686162936145 + }, + { + "x": 2.2599910820649347, + "y": 3.584420486805288, + "heading": -0.7654136211145312, + "angularVelocity": -0.04715282263017491, + "velocityX": -0.34398521960151957, + "velocityY": 0.40191966198861, + "timestamp": 3.21490767195737 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -0.022845628735947136, + "velocityX": -0.16969575449988578, + "velocityY": 0.20294632487031472, + "timestamp": 3.2843467276211253 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -4.7049839413389466e-29, + "velocityX": -4.475670243258915e-27, + "velocityY": -3.1749766992652438e-27, + "timestamp": 3.3537857832848808 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/3PieceSourceSide.traj b/src/main/deploy/choreo/3PieceSourceSide.traj index 60ce6a0..4a09e4b 100644 --- a/src/main/deploy/choreo/3PieceSourceSide.traj +++ b/src/main/deploy/choreo/3PieceSourceSide.traj @@ -4,1495 +4,1909 @@ "x": 0.6476043462753296, "y": 4.391018867492676, "heading": -1.0516500181249593, - "angularVelocity": -2.3506527398385264e-24, - "velocityX": -4.584841665957037e-25, - "velocityY": 1.794170584470112e-24, + "angularVelocity": 1.9413513581547193e-28, + "velocityX": -2.7445681219758215e-27, + "velocityY": -2.6836170825004164e-27, "timestamp": 0 }, { - "x": 0.692835797571452, - "y": 4.365429285724839, - "heading": -1.0363921320591865, - "angularVelocity": 0.19907376380794284, - "velocityX": 0.5901469714218079, - "velocityY": -0.33387419035865423, - "timestamp": 0.07664438434234237 - }, - { - "x": 0.7831334822061629, - "y": 4.314114094234643, - "heading": -1.005707809669664, - "angularVelocity": 0.4003466483919687, - "velocityX": 1.178138299492162, - "velocityY": -0.6695231742092023, - "timestamp": 0.15328876868468475 - }, - { - "x": 0.9181042106922528, - "y": 4.23677088276396, - "heading": -0.9591055566031375, - "angularVelocity": 0.608032192657082, - "velocityX": 1.7609995780411658, - "velocityY": -1.009117786441079, - "timestamp": 0.2299331530270271 - }, - { - "x": 1.0717422840106643, - "y": 4.1370476005177, - "heading": -0.8558256704459198, - "angularVelocity": 1.3475205919314872, - "velocityX": 2.004557471975592, - "velocityY": -1.3011166193315893, - "timestamp": 0.3065775373693695 - }, - { - "x": 1.203873817721518, - "y": 4.032707540283105, - "heading": -0.6892805911828493, - "angularVelocity": 2.172958667384875, - "velocityX": 1.7239558363554812, - "velocityY": -1.3613529696911204, - "timestamp": 0.3832219217117119 - }, - { - "x": 1.3594029637702276, - "y": 3.9301727362790966, - "heading": -0.5828610080840567, - "angularVelocity": 1.388485066609126, - "velocityX": 2.0292308090572955, - "velocityY": -1.3377993036779194, - "timestamp": 0.45986630605405426 - }, - { - "x": 1.5344193266927146, - "y": 3.825053132953821, - "heading": -0.5408815443461281, - "angularVelocity": 0.5477174107162478, - "velocityX": 2.2834857951334486, - "velocityY": -1.371523879110889, - "timestamp": 0.5365106903963967 - }, - { - "x": 1.7162004854447428, - "y": 3.7215786578067394, - "heading": -0.5174827165758584, - "angularVelocity": 0.30529083077705393, - "velocityX": 2.3717479148906513, - "velocityY": -1.3500594470809355, - "timestamp": 0.613155074738739 - }, - { - "x": 1.8995749541613056, - "y": 3.6205751589308552, - "heading": -0.4949062907307691, - "angularVelocity": 0.2945607305585351, - "velocityX": 2.3925362606801817, - "velocityY": -1.317819951749349, - "timestamp": 0.6897994590810813 - }, - { - "x": 2.0827414581868835, - "y": 3.51960952250048, - "heading": -0.4719606385920979, - "angularVelocity": 0.29937812581521006, - "velocityX": 2.389822889142672, - "velocityY": -1.3173259501883174, - "timestamp": 0.7664438434234236 - }, - { - "x": 2.265688686133792, - "y": 3.4186835470349224, - "heading": -0.4486127865936518, - "angularVelocity": 0.3046257361029851, - "velocityX": 2.3869619348724966, - "velocityY": -1.3168084828597342, - "timestamp": 0.843088227765766 - }, - { - "x": 2.4484040933032216, - "y": 3.3177997829423713, - "heading": -0.4248255225210326, - "angularVelocity": 0.3103588642107185, - "velocityX": 2.3839373065260236, - "velocityY": -1.3162577396660924, - "timestamp": 0.9197326121081083 - }, - { - "x": 2.6308732997732713, - "y": 3.216961383200288, - "heading": -0.4005559741960246, - "angularVelocity": 0.3166513572162684, - "velocityX": 2.380725059451553, - "velocityY": -1.3156658587232646, - "timestamp": 0.9963769964504506 - }, - { - "x": 2.813079628281676, - "y": 3.116172257902354, - "heading": -0.3757541558021446, - "angularVelocity": 0.32359602868097065, - "velocityX": 2.377295219628298, - "velocityY": -1.3150229617312381, - "timestamp": 1.073021380792793 - }, - { - "x": 2.9950035114027176, - "y": 3.015437295026463, - "heading": -0.3503610814650098, - "angularVelocity": 0.3313103047930201, - "velocityX": 2.3736100783125105, - "velocityY": -1.314316289970363, - "timestamp": 1.1496657651351354 - }, - { - "x": 3.176621688858468, - "y": 2.9147626672707037, - "heading": -0.3243062033321442, - "angularVelocity": 0.3399450378058761, - "velocityX": 2.3696214538631923, - "velocityY": -1.3135290813490341, - "timestamp": 1.2263101494774777 - }, - { - "x": 3.3579060875063353, - "y": 2.8141562687774018, - "heading": -0.2975038256949741, - "angularVelocity": 0.34969786589260987, - "velocityX": 2.3652665515341145, - "velocityY": -1.3126388757189247, - "timestamp": 1.30295453381982 - }, - { - "x": 3.538822210205962, - "y": 2.713628355793928, - "heading": -0.2698479287435623, - "angularVelocity": 0.3608339630974529, - "velocityX": 2.360461555689992, - "velocityY": -1.3116148540570647, - "timestamp": 1.3795989181621624 - }, - { - "x": 3.7193267349323875, - "y": 2.6131925205499895, - "heading": -0.24120442629231653, - "angularVelocity": 0.3737195190101045, - "velocityX": 2.3550913256759576, - "velocityY": -1.3104134909000977, - "timestamp": 1.4562433025045047 - }, - { - "x": 3.8993637811598734, - "y": 2.5128672365819, - "heading": -0.21139907897320392, - "angularVelocity": 0.38887842305553666, - "velocityX": 2.3489920073377766, - "velocityY": -1.3089710985213812, - "timestamp": 1.532887686846847 - }, - { - "x": 4.078858787892925, - "y": 2.4126784448405125, - "heading": -0.18019759590707388, - "angularVelocity": 0.4070941835316264, - "velocityX": 2.34191987153701, - "velocityY": -1.30719024754481, - "timestamp": 1.6095320711891894 - }, - { - "x": 4.257707770811751, - "y": 2.312664186467197, - "heading": -0.1472705722503286, - "angularVelocity": 0.42960777804245054, - "velocityX": 2.3334910242082882, - "velocityY": -1.3049130635140744, - "timestamp": 1.6861764555315317 - }, - { - "x": 4.435756686256742, - "y": 2.212883689420889, - "heading": -0.1121258503357887, - "angularVelocity": 0.4585426866704457, - "velocityX": 2.323052327613603, - "velocityY": -1.3018631162933536, - "timestamp": 1.762820839873874 - }, - { - "x": 4.612756399460092, - "y": 2.113437617489987, - "heading": -0.07396027426521577, - "angularVelocity": 0.4979565874011239, - "velocityX": 2.3093631023605954, - "velocityY": -1.2974997814153513, - "timestamp": 1.8394652242162164 - }, - { - "x": 4.788243072222853, - "y": 2.014522987095371, - "heading": -0.031264185221419126, - "angularVelocity": 0.5570674147904805, - "velocityX": 2.289622054747365, - "velocityY": -1.290565920039225, - "timestamp": 1.9161096085585587 - }, - { - "x": 4.961081026742291, - "y": 1.9166475782485084, - "heading": 0.019697120176888277, - "angularVelocity": 0.664905926710585, - "velocityX": 2.255063511860639, - "velocityY": -1.2770069155972317, - "timestamp": 1.992753992900901 - }, - { - "x": 5.123264328610966, - "y": 1.823526252461316, - "heading": 0.10546513381285838, - "angularVelocity": 1.1190384575714751, - "velocityX": 2.1160493787028227, - "velocityY": -1.214979108857517, - "timestamp": 2.0693983772432434 - }, - { - "x": 5.295119103926639, - "y": 1.7237778734191072, - "heading": 0.15590676979164073, - "angularVelocity": 0.6581256593239505, - "velocityX": 2.24223570702925, - "velocityY": -1.3014440640121712, - "timestamp": 2.1460427615855857 - }, - { - "x": 5.493802683128641, - "y": 1.6448426706793149, - "heading": 0.16075963684913044, - "angularVelocity": 0.06331666826122177, - "velocityX": 2.5922783633378295, - "velocityY": -1.029888926854938, - "timestamp": 2.222687145927928 - }, - { - "x": 5.707507610321045, - "y": 1.6179983615875244, + "x": 0.6597456116335061, + "y": 4.380178489721837, + "heading": -1.0354550463341647, + "angularVelocity": 0.23714267860510804, + "velocityX": 0.17778432873405597, + "velocityY": -0.15873545535467184, + "timestamp": 0.0682921011352938 + }, + { + "x": 0.6840676878744021, + "y": 4.358487813648757, + "heading": -1.0032553908058022, + "angularVelocity": 0.47149897269336843, + "velocityX": 0.35614772186773713, + "velocityY": -0.3176161768710194, + "timestamp": 0.1365842022705876 + }, + { + "x": 0.7206162348586238, + "y": 4.325939175146822, + "heading": -0.9552579550391015, + "angularVelocity": 0.7028255825898914, + "velocityX": 0.5351797115132725, + "velocityY": -0.4766091240545095, + "timestamp": 0.2048763034058814 + }, + { + "x": 0.7694577963308973, + "y": 4.282521644299476, + "heading": -0.8917843367572964, + "angularVelocity": 0.9294430428499669, + "velocityX": 0.7151861000075717, + "velocityY": -0.6357621178082019, + "timestamp": 0.2731684045411752 + }, + { + "x": 0.8306912777270616, + "y": 4.228207914210413, + "heading": -0.8133985275963236, + "angularVelocity": 1.1478019838001796, + "velocityX": 0.896640759007464, + "velocityY": -0.7953149659498937, + "timestamp": 0.341460505676469 + }, + { + "x": 0.9044507708380568, + "y": 4.162933772374405, + "heading": -0.7210416262705941, + "angularVelocity": 1.3523804333206972, + "velocityX": 1.0800589216733842, + "velocityY": -0.9558080766426159, + "timestamp": 0.40975260681176284 + }, + { + "x": 0.9908959110037516, + "y": 4.086578645763268, + "heading": -0.6161174549931975, + "angularVelocity": 1.5364027396013313, + "velocityX": 1.265814621729648, + "velocityY": -1.1180667360031697, + "timestamp": 0.47804470794705667 + }, + { + "x": 1.090196283285551, + "y": 3.998968512246339, + "heading": -0.5004646416711678, + "angularVelocity": 1.6935020507409695, + "velocityX": 1.4540535527684928, + "velocityY": -1.2828735982711192, + "timestamp": 0.5463368090823505 + }, + { + "x": 1.2025392306050893, + "y": 3.8999248788172114, + "heading": -0.3762963474938359, + "angularVelocity": 1.818194082670579, + "velocityX": 1.645035742815635, + "velocityY": -1.4502941303989414, + "timestamp": 0.6146289102176443 + }, + { + "x": 1.328190264179142, + "y": 3.789348006758151, + "heading": -0.2466808070464207, + "angularVelocity": 1.897958011141482, + "velocityX": 1.8399058088010056, + "velocityY": -1.619175134764066, + "timestamp": 0.6829210113529381 + }, + { + "x": 1.467420004237056, + "y": 3.6673010913969546, + "heading": -0.11832684408091969, + "angularVelocity": 1.8794847549238287, + "velocityX": 2.0387385619031573, + "velocityY": -1.7871307710888695, + "timestamp": 0.751213112488232 + }, + { + "x": 1.6176774224315258, + "y": 3.534549695823147, + "heading": -0.01051011565957327, + "angularVelocity": 1.5787584014694496, + "velocityX": 2.2002166531206018, + "velocityY": -1.9438762809598271, + "timestamp": 0.8195052136235258 + }, + { + "x": 1.7740513259099606, + "y": 3.391053821163867, + "heading": 0.062440796343179686, + "angularVelocity": 1.0682188831506234, + "velocityX": 2.2897802363503494, + "velocityY": -2.1012074936016356, + "timestamp": 0.8877973147588196 + }, + { + "x": 1.9367743639870942, + "y": 3.237147837801074, + "heading": 0.09987435620827004, + "angularVelocity": 0.5481389390982501, + "velocityX": 2.382750499281934, + "velocityY": -2.253642526796603, + "timestamp": 0.9560894158941134 + }, + { + "x": 2.1058806251727042, + "y": 3.0730806615870963, + "heading": 0.10139956904864746, + "angularVelocity": 0.022333663996599216, + "velocityX": 2.4762199196447727, + "velocityY": -2.4024326896743653, + "timestamp": 1.0243815170294073 + }, + { + "x": 2.282556811687828, + "y": 2.9164516007892924, + "heading": 0.1013996340867454, + "angularVelocity": 9.523516904308559e-7, + "velocityX": 2.587066199136408, + "velocityY": -2.293516500356401, + "timestamp": 1.092673618164701 + }, + { + "x": 2.4592332259385974, + "y": 2.759822796875614, + "heading": 0.10139969912475692, + "angularVelocity": 9.52350424751949e-7, + "velocityX": 2.5870695338653458, + "velocityY": -2.2935127388067964, + "timestamp": 1.1609657192999947 + }, + { + "x": 2.6359096401941113, + "y": 2.603193992967287, + "heading": 0.10139976416276904, + "angularVelocity": 9.523504335129237e-7, + "velocityX": 2.587069533934816, + "velocityY": -2.293512738728436, + "timestamp": 1.2292578204352884 + }, + { + "x": 2.812586054449808, + "y": 2.4465651890591653, + "heading": 0.1013998292007818, + "angularVelocity": 9.523504429819945e-7, + "velocityX": 2.587069533937483, + "velocityY": -2.2935127387254277, + "timestamp": 1.2975499215705821 + }, + { + "x": 2.989262477460467, + "y": 2.2899363950265936, + "heading": 0.10139989423879318, + "angularVelocity": 9.523504226466593e-7, + "velocityX": 2.5870696621362548, + "velocityY": -2.2935125941179324, + "timestamp": 1.3658420227058758 + }, + { + "x": 3.166357964338375, + "y": 2.1337815804962608, + "heading": 0.10139995920086388, + "angularVelocity": 9.512384243710626e-7, + "velocityX": 2.593206006754183, + "velocityY": -2.286572120851488, + "timestamp": 1.4341341238411696 + }, + { + "x": 3.3547011225730468, + "y": 1.9913948102576182, + "heading": 0.10140002311622047, + "angularVelocity": 9.359114091359504e-7, + "velocityX": 2.7579054547105564, + "velocityY": -2.0849668976586258, + "timestamp": 1.5024262249764633 + }, + { + "x": 3.553226828100459, + "y": 1.8635876061361314, + "heading": 0.1014000890794247, + "angularVelocity": 9.658980048505205e-7, + "velocityX": 2.907008310289235, + "velocityY": -1.8714785750739689, + "timestamp": 1.570718326111757 + }, + { + "x": 3.760808917151287, + "y": 1.7510851059584576, + "heading": 0.10140016079724051, + "angularVelocity": 0.0000010501626778013022, + "velocityX": 3.0396207701910694, + "velocityY": -1.647372072427445, + "timestamp": 1.6390104272470507 + }, + { + "x": 3.976269792438543, + "y": 1.654525560969519, + "heading": 0.10140024381940511, + "angularVelocity": 0.0000012156920527539028, + "velocityX": 3.1549896943484823, + "velocityY": -1.413919668361702, + "timestamp": 1.7073025283823444 + }, + { + "x": 4.1983871429917885, + "y": 1.5744567665619278, + "heading": 0.10140036224681147, + "angularVelocity": 0.0000017341303662335387, + "velocityX": 3.252460341104571, + "velocityY": -1.1724459062837547, + "timestamp": 1.7755946295176381 + }, + { + "x": 4.4257370453636415, + "y": 1.5113830168900522, + "heading": 0.10193866126599585, + "angularVelocity": 0.007882302788106277, + "velocityX": 3.3290805026111183, + "velocityY": -0.9235877740372923, + "timestamp": 1.8438867306529318 + }, + { + "x": 4.656186679939347, + "y": 1.4657767864094913, + "heading": 0.10576151989897133, + "angularVelocity": 0.05597804972206098, + "velocityX": 3.374469825128989, + "velocityY": -0.6678112068950683, + "timestamp": 1.9121788317882256 + }, + { + "x": 4.888314550102186, + "y": 1.4378182802801935, + "heading": 0.1133638582654562, + "angularVelocity": 0.11132090300492935, + "velocityX": 3.3990441984347375, + "velocityY": -0.4093958988596496, + "timestamp": 1.9804709329235193 + }, + { + "x": 5.121043949314908, + "y": 1.4275947415879202, + "heading": 0.12410092792765016, + "angularVelocity": 0.15722271659093798, + "velocityX": 3.4078523774171208, + "velocityY": -0.14970309189959885, + "timestamp": 2.048763034058813 + }, + { + "x": 5.353451540876787, + "y": 1.4351381309484952, + "heading": 0.13671420054587038, + "angularVelocity": 0.18469592249375974, + "velocityX": 3.403140153814491, + "velocityY": 0.11045771377908847, + "timestamp": 2.117055135194107 + }, + { + "x": 5.584673054960648, + "y": 1.4604251835156339, + "heading": 0.14959239179836323, + "angularVelocity": 0.18857512125713907, + "velocityX": 3.385772442786424, + "velocityY": 0.37027785273501185, + "timestamp": 2.185347236329401 + }, + { + "x": 5.813786029815674, + "y": 1.5033589601516724, "heading": 0.16076270850197952, - "angularVelocity": 0.000040076685010459537, - "velocityX": 2.7882659509385723, - "velocityY": -0.3502449569153927, - "timestamp": 2.2993315302702704 - }, - { - "x": 5.878932815955422, - "y": 1.6310716458031873, - "heading": 0.15207722794791115, - "angularVelocity": -0.13999820175733138, - "velocityX": 2.7631425083845143, - "velocityY": 0.21072366345901047, - "timestamp": 2.3613714739627154 - }, - { - "x": 6.041874898509352, - "y": 1.6781214038756682, - "heading": 0.13576601260266036, - "angularVelocity": -0.2629147348377915, - "velocityX": 2.6264060354679595, - "velocityY": 0.7583784779967588, - "timestamp": 2.4234114176551604 - }, - { - "x": 6.190063655455228, - "y": 1.7574804032751512, - "heading": 0.11619159565916007, - "angularVelocity": -0.3155131319998917, - "velocityX": 2.388602376567293, - "velocityY": 1.2791597586370276, - "timestamp": 2.4854513613476055 - }, - { - "x": 6.338186422130831, - "y": 1.840598011238034, - "heading": 0.10221625072518932, - "angularVelocity": -0.22526366244385518, - "velocityX": 2.387538702644562, - "velocityY": 1.3397434461727966, - "timestamp": 2.5474913050400505 - }, - { - "x": 6.486157900665009, - "y": 1.9236682945064507, - "heading": 0.08780760406345454, - "angularVelocity": -0.2322479003714719, - "velocityX": 2.3851001423812606, - "velocityY": 1.338980636091913, - "timestamp": 2.6095312487324955 - }, - { - "x": 6.633958405719426, - "y": 2.0066826604418155, - "heading": 0.07290105260244177, - "angularVelocity": -0.2402734524536335, - "velocityX": 2.382344281083177, - "velocityY": 1.3380793242962596, - "timestamp": 2.6715711924249406 - }, - { - "x": 6.781563534615612, - "y": 2.089630624589182, - "heading": 0.0574168439817537, - "angularVelocity": -0.24958450474179966, - "velocityX": 2.3791950816060985, - "velocityY": 1.33700901726427, - "timestamp": 2.7336111361173856 - }, - { - "x": 6.928941267710664, - "y": 2.172498140132396, - "heading": 0.04124990446967813, - "angularVelocity": -0.2605892035012319, - "velocityX": 2.3755297687834913, - "velocityY": 1.3357122945504047, - "timestamp": 2.7956510798098306 - }, - { - "x": 7.076047973733186, - "y": 2.255265656701969, - "heading": 0.024256449523087663, - "angularVelocity": -0.27391151466599145, - "velocityX": 2.3711611788653904, - "velocityY": 1.334100446317001, - "timestamp": 2.8576910235022757 - }, - { - "x": 7.222828783744423, - "y": 2.337909309225835, - "heading": 0.006256947367936294, - "angularVelocity": -0.2901276352599791, - "velocityX": 2.3659081758501124, - "velocityY": 1.3321039253929894, - "timestamp": 2.9197309671947207 - }, - { - "x": 7.384688854217529, - "y": 2.3971612453460693, - "heading": 6.712344331303172e-23, - "angularVelocity": -0.10085353073423609, - "velocityX": 2.608965463855139, - "velocityY": 0.9550610879656509, - "timestamp": 2.9817709108871657 - }, - { - "x": 7.581926675665867, - "y": 2.420671247090372, - "heading": -6.833387833600707e-9, - "angularVelocity": -9.667586454477017e-8, - "velocityX": 2.7904368043745778, - "velocityY": 0.33260950489354324, - "timestamp": 3.052454404288316 - }, - { - "x": 7.7805365921893905, - "y": 2.417576114646826, - "heading": -6.833380269760843e-9, - "angularVelocity": 1.0700998918582695e-13, - "velocityX": 2.8098486218890097, - "velocityY": -0.043788617322290876, - "timestamp": 3.1231378976894666 - }, - { - "x": 7.9655574312129644, - "y": 2.4146925691677037, - "heading": -4.100028695417927e-9, - "angularVelocity": 3.8670295465311964e-8, - "velocityX": 2.6175961334214803, - "velocityY": -0.0407951749463946, - "timestamp": 3.193821391090617 - }, - { - "x": 8.104323073360915, - "y": 2.412529909857503, - "heading": -2.050014465682545e-9, - "angularVelocity": 2.9002729365693992e-8, - "velocityX": 1.9631972822906998, - "velocityY": -0.03059638405147332, - "timestamp": 3.2645048844917675 - }, - { - "x": 8.196833505192958, - "y": 2.411088136925815, - "heading": -6.833381783003922e-10, - "angularVelocity": 1.933515480942414e-8, - "velocityX": 1.3087982410125008, - "velocityY": -0.020397590191323155, - "timestamp": 3.335188377892918 + "angularVelocity": 0.16356674517140343, + "velocityX": 3.3548971410489967, + "velocityY": 0.6286785136539, + "timestamp": 2.253639337464695 + }, + { + "x": 5.939295701159701, + "y": 1.5323683761963922, + "heading": 0.16590262686638915, + "angularVelocity": 0.1364467184876021, + "velocityX": 3.331839453313385, + "velocityY": 0.7700977610756782, + "timestamp": 2.291309121109046 + }, + { + "x": 6.063755261240901, + "y": 1.5666696664667372, + "heading": 0.1699280929818009, + "angularVelocity": 0.1068619388265435, + "velocityX": 3.3039627000847966, + "velocityY": 0.910578372156086, + "timestamp": 2.328978904753397 + }, + { + "x": 6.186965476091788, + "y": 1.6062187514342638, + "heading": 0.17281888262781223, + "angularVelocity": 0.07674027738794295, + "velocityX": 3.270796987159313, + "velocityY": 1.0498888271012834, + "timestamp": 2.366648688397748 + }, + { + "x": 6.308696366891361, + "y": 1.650958086998814, + "heading": 0.17466669111475505, + "angularVelocity": 0.049052803286273795, + "velocityX": 3.231526147027085, + "velocityY": 1.1876716889840546, + "timestamp": 2.4043184720420987 + }, + { + "x": 6.428664923898692, + "y": 1.7008051924773087, + "heading": 0.17574653518384492, + "angularVelocity": 0.028666054450562455, + "velocityX": 3.184742395655416, + "velocityY": 1.323264979409296, + "timestamp": 2.4419882556864496 + }, + { + "x": 6.546495426793477, + "y": 1.7556223876439374, + "heading": 0.17665953793741, + "angularVelocity": 0.02423700550512633, + "velocityX": 3.1279845938922866, + "velocityY": 1.4552033450515791, + "timestamp": 2.4796580393308005 + }, + { + "x": 6.661649606485285, + "y": 1.8151243678255207, + "heading": 0.1786491578806416, + "angularVelocity": 0.05281739767916041, + "velocityX": 3.0569376447447945, + "velocityY": 1.5795678770909583, + "timestamp": 2.5173278229751515 + }, + { + "x": 6.773137667082048, + "y": 1.8784298401932633, + "heading": 0.18358189083734322, + "angularVelocity": 0.13094667607525115, + "velocityX": 2.9596151028991167, + "velocityY": 1.6805371903758264, + "timestamp": 2.5549976066195024 + }, + { + "x": 6.87927675328215, + "y": 1.941846015988482, + "heading": 0.19021855833237603, + "angularVelocity": 0.17618013306609673, + "velocityX": 2.817618683510011, + "velocityY": 1.683475976234567, + "timestamp": 2.5926673902638533 + }, + { + "x": 6.980896999193432, + "y": 2.002302596889058, + "heading": 0.19752796800079392, + "angularVelocity": 0.1940390668931813, + "velocityX": 2.6976593991274846, + "velocityY": 1.6049091619787552, + "timestamp": 2.630337173908204 + }, + { + "x": 7.078947568278477, + "y": 2.0586931301517772, + "heading": 0.20493066809205301, + "angularVelocity": 0.19651559884574055, + "velocityX": 2.60289705963705, + "velocityY": 1.496969926748577, + "timestamp": 2.668006957552555 + }, + { + "x": 7.174005720052268, + "y": 2.1105770010329152, + "heading": 0.21206754751188253, + "angularVelocity": 0.18945899682383116, + "velocityX": 2.5234589258928724, + "velocityY": 1.3773339228859343, + "timestamp": 2.705676741196906 + }, + { + "x": 7.266431305502043, + "y": 2.15773800547381, + "heading": 0.21871161412654994, + "angularVelocity": 0.17637655361643606, + "velocityX": 2.4535735676739243, + "velocityY": 1.2519584632115888, + "timestamp": 2.743346524841257 + }, + { + "x": 7.356464862823486, + "y": 2.2000527381896973, + "heading": 0.22471085620066228, + "angularVelocity": 0.15925873455373588, + "velocityX": 2.390073650846282, + "velocityY": 1.123307028131367, + "timestamp": 2.781016308485608 + }, + { + "x": 7.523755330928558, + "y": 2.2638899288150194, + "heading": 0.23355968892559228, + "angularVelocity": 0.12033403723737161, + "velocityX": 2.274959652214718, + "velocityY": 0.868113017007844, + "timestamp": 2.854551884354925 + }, + { + "x": 7.678148398275777, + "y": 2.3116679387336183, + "heading": 0.24020478966195502, + "angularVelocity": 0.09036579448527025, + "velocityX": 2.0995697051668696, + "velocityY": 0.6497264671389397, + "timestamp": 2.9280874602242424 + }, + { + "x": 7.816541043927953, + "y": 2.346462739824989, + "heading": 0.2451990584982588, + "angularVelocity": 0.06791636262126016, + "velocityX": 1.8819822108705344, + "velocityY": 0.47316962817026026, + "timestamp": 3.0016230360935596 + }, + { + "x": 7.936995928235685, + "y": 2.3711072013144094, + "heading": 0.2489351660825785, + "angularVelocity": 0.05080680391976913, + "velocityX": 1.6380491059429139, + "velocityY": 0.3351365811456589, + "timestamp": 3.075158611962877 + }, + { + "x": 8.038363690822308, + "y": 2.387966753250885, + "heading": 0.2516849557707684, + "angularVelocity": 0.03739400495178875, + "velocityX": 1.3784860101832423, + "velocityY": 0.22927068615654433, + "timestamp": 3.148694187832194 + }, + { + "x": 8.119972690853793, + "y": 2.3989446235760092, + "heading": 0.2536437125866727, + "angularVelocity": 0.026636859679799414, + "velocityX": 1.1097893647629, + "velocityY": 0.14928652146049065, + "timestamp": 3.2222297637015114 + }, + { + "x": 8.18143147852008, + "y": 2.405561042236668, + "heading": 0.2549588543062311, + "angularVelocity": 0.0178844281017885, + "velocityX": 0.8357694481852939, + "velocityY": 0.08997575095375589, + "timestamp": 3.2957653395708286 + }, + { + "x": 8.222513130352727, + "y": 2.409037637931504, + "heading": 0.25574648281571827, + "angularVelocity": 0.010710849818962056, + "velocityX": 0.5586636311335063, + "velocityY": 0.04727773807081853, + "timestamp": 3.369300915440146 }, { "x": 8.243088722229004, "y": 2.410367250442505, - "heading": 1.830282426459157e-24, - "angularVelocity": 9.667577894349971e-9, - "velocityX": 0.6543991363518575, - "velocityY": -0.010198795342763814, - "timestamp": 3.4058718712940683 + "heading": 0.2561010030603239, + "angularVelocity": 0.004821071167453232, + "velocityX": 0.2798045929883303, + "velocityY": 0.018081214368450626, + "timestamp": 3.442836491309463 }, { "x": 8.243088722229004, "y": 2.410367250442505, - "heading": 1.851361909343925e-24, - "angularVelocity": 2.3995205522592844e-25, - "velocityX": -1.4269511777904615e-24, - "velocityY": -1.1273229249768833e-23, - "timestamp": 3.4765553646952188 - }, - { - "x": 8.183672767375366, - "y": 2.3891879079868126, - "heading": -0.016263075453039278, - "angularVelocity": -0.1934255260988222, - "velocityX": -0.7066659906617425, - "velocityY": -0.251897340619777, - "timestamp": 3.5606346265230004 - }, - { - "x": 8.064809267825755, - "y": 2.3467183743812834, - "heading": -0.04860613329336098, - "angularVelocity": -0.38467342763509904, - "velocityX": -1.413707696352949, - "velocityY": -0.5051130645333048, - "timestamp": 3.644713888350782 - }, - { - "x": 7.886434927132461, - "y": 2.2827439256274595, - "heading": -0.09665252446442359, - "angularVelocity": -0.5714416388368797, - "velocityX": -2.121502220828929, - "velocityY": -0.7608826167487317, - "timestamp": 3.728793150178564 - }, - { - "x": 7.663093479676549, - "y": 2.2056330475266677, - "heading": -0.09665343057468777, - "angularVelocity": -0.000010776857984756087, - "velocityX": -2.6563202697162027, - "velocityY": -0.9171212546886659, - "timestamp": 3.8128724120063455 - }, - { - "x": 7.439949308521917, - "y": 2.127952816871332, - "heading": -0.09665400996983407, - "angularVelocity": -0.000006891058909294993, - "velocityX": -2.653973956285386, - "velocityY": -0.9238928716387514, - "timestamp": 3.896951673834127 - }, - { - "x": 7.216805138002767, - "y": 2.0502725843903784, - "heading": -0.0966545893647738, - "angularVelocity": -0.0000068910564521426285, - "velocityX": -2.6539739487272445, - "velocityY": -0.9238928933518099, - "timestamp": 3.981030935661909 - }, - { - "x": 6.9936609674836125, - "y": 1.9725923519093191, - "heading": -0.09665516875951666, - "angularVelocity": -0.000006891054111144413, - "velocityX": -2.6539739487273097, - "velocityY": -0.923892893353071, - "timestamp": 4.06511019748969 - }, - { - "x": 6.77051679696445, - "y": 1.8949121194281597, - "heading": -0.09665574815406265, - "angularVelocity": -0.000006891051769457115, - "velocityX": -2.653973948727398, - "velocityY": -0.9238928933542627, - "timestamp": 4.149189459317472 - }, - { - "x": 6.547372626445282, - "y": 1.8172318869468993, - "heading": -0.09665632754841177, - "angularVelocity": -0.000006891049427961194, - "velocityX": -2.653973948727487, - "velocityY": -0.9238928933554545, - "timestamp": 4.2332687211452535 - }, - { - "x": 6.324228455926046, - "y": 1.7395516544657041, - "heading": -0.09665690694256396, - "angularVelocity": -0.000006891047085669869, - "velocityX": -2.653973948728258, - "velocityY": -0.9238928933546852, - "timestamp": 4.317347982973035 - }, - { - "x": 6.101084267512398, - "y": 1.6618714733923579, - "heading": -0.0966574863411208, - "angularVelocity": -0.000006891099472478973, - "velocityX": -2.65397416155617, - "velocityY": -0.9238922819334152, - "timestamp": 4.401427244800817 - }, - { - "x": 5.881706688567194, - "y": 1.6001619743925184, - "heading": -0.12280606064162562, - "angularVelocity": -0.3109990945694144, - "velocityX": -2.609175844033366, - "velocityY": -0.7339443479682077, - "timestamp": 4.485506506628599 - }, - { - "x": 5.667889595031738, - "y": 1.5915861129760742, - "heading": -0.19572183715761898, - "angularVelocity": -0.8672266493650448, - "velocityX": -2.5430419926070895, - "velocityY": -0.10199734429174848, - "timestamp": 4.56958576845638 - }, - { - "x": 5.472836165647801, - "y": 1.6405608510115604, - "heading": -0.2695567389374151, - "angularVelocity": -0.9372981580360532, - "velocityX": -2.4761083941768933, - "velocityY": 0.6217104735625232, - "timestamp": 4.648359956954205 - }, - { - "x": 5.3021481158717245, - "y": 1.712801388975876, - "heading": -0.4003704259742984, - "angularVelocity": -1.660616117175175, - "velocityX": -2.1668017536072774, - "velocityY": 0.9170584850431085, - "timestamp": 4.7271341454520295 - }, - { - "x": 5.1196004435735345, - "y": 1.8042197974618441, - "heading": -0.46428021863316127, - "angularVelocity": -0.8113037262278816, - "velocityX": -2.317353891918915, - "velocityY": 1.160512221442844, - "timestamp": 4.805908333949854 - }, - { - "x": 4.929096458765624, - "y": 1.904953408569058, - "heading": -0.48639976153317277, - "angularVelocity": -0.28079683614414136, - "velocityX": -2.4183554085507573, - "velocityY": 1.2787641869518616, - "timestamp": 4.884682522447679 - }, - { - "x": 4.737761672165951, - "y": 2.009314965020544, - "heading": -0.49933816069591924, - "angularVelocity": -0.1642466829487382, - "velocityX": -2.4289020331190754, - "velocityY": 1.3248191881325238, - "timestamp": 4.963456710945503 - }, - { - "x": 4.546911439579052, - "y": 2.114853649108329, - "heading": -0.5116829622012672, - "angularVelocity": -0.15671124946833295, - "velocityX": -2.4227508556583697, - "velocityY": 1.3397622508126334, - "timestamp": 5.042230899443328 - }, - { - "x": 4.356102309238819, - "y": 2.2203468351822107, - "heading": -0.524131682346679, - "angularVelocity": -0.15803044604839828, - "velocityX": -2.4222290826328545, - "velocityY": 1.3391846756605428, - "timestamp": 5.121005087941152 - }, - { - "x": 4.165335431709755, - "y": 2.3257928672028427, - "heading": -0.5366900663313149, - "angularVelocity": -0.15942257513681343, - "velocityX": -2.421692703750687, - "velocityY": 1.338586077894589, - "timestamp": 5.199779276438977 - }, - { - "x": 3.9746120593595244, - "y": 2.4311903935174577, - "heading": -0.5493635573249664, - "angularVelocity": -0.1608838026176728, - "velocityX": -2.421140426670299, - "velocityY": 1.3379703215543044, - "timestamp": 5.2785534649368016 - }, - { - "x": 3.783933515355731, - "y": 2.5365380161026203, - "heading": -0.5621578157899224, - "angularVelocity": -0.16241688691352676, - "velocityX": -2.420571352620895, - "velocityY": 1.337336817986659, - "timestamp": 5.357327653434626 - }, - { - "x": 3.593301216494676, - "y": 2.6418342523116722, - "heading": -0.5750788548667545, - "angularVelocity": -0.16402630510359092, - "velocityX": -2.4199842930317086, - "velocityY": 1.3366844929409736, - "timestamp": 5.436101841932451 - }, - { - "x": 3.4027166923911927, - "y": 2.7470775186047, - "heading": -0.5881331219292356, - "angularVelocity": -0.16571756956711148, - "velocityX": -2.41937781572635, - "velocityY": 1.3360120656264767, - "timestamp": 5.514876030430275 - }, - { - "x": 3.212181603664246, - "y": 2.8522661171153323, - "heading": -0.601327569493579, - "angularVelocity": -0.16749709284161973, - "velocityX": -2.4187502576711153, - "velocityY": 1.3353180847243658, - "timestamp": 5.5936502189281 - }, - { - "x": 3.0216977610811666, - "y": 2.9573982215901036, - "heading": -0.6146697280062193, - "angularVelocity": -0.1693722114701131, - "velocityX": -2.418099712805536, - "velocityY": 1.3346009204229918, - "timestamp": 5.6724244074259245 - }, - { - "x": 2.831267146879349, - "y": 3.0624718615565794, - "heading": -0.6281677862205469, - "angularVelocity": -0.17135128233914515, - "velocityX": -2.417424004400561, - "velocityY": 1.3338587419326686, - "timestamp": 5.751198595923749 - }, - { - "x": 2.640891939244419, - "y": 3.167484904027678, - "heading": -0.6418306831098634, - "angularVelocity": -0.17344382912549475, - "velocityX": -2.416720644988769, - "velocityY": 1.3330894862090363, - "timestamp": 5.829972784421574 - }, - { - "x": 2.4505746379296203, - "y": 3.272435032259823, - "heading": -0.655668504824747, - "angularVelocity": -0.17566441468661878, - "velocityX": -2.4159855524256404, - "velocityY": 1.3322908205527566, - "timestamp": 5.908746972919398 - }, - { - "x": 2.2618233887297388, - "y": 3.3772509809337916, - "heading": -0.674261275863946, - "angularVelocity": -0.2360261831159634, - "velocityX": -2.396105282697931, - "velocityY": 1.3305874763389332, - "timestamp": 5.987521161417223 - }, - { - "x": 2.116002537558778, - "y": 3.455078745186431, - "heading": -0.7196337030492912, - "angularVelocity": -0.5759808898139023, - "velocityX": -1.8511247649982117, - "velocityY": 0.9879856046347999, - "timestamp": 6.066295349915047 - }, - { - "x": 2.018778748901899, - "y": 3.5069532190081367, - "heading": -0.7498768931574519, - "angularVelocity": -0.3839225853655822, - "velocityX": -1.234208698443955, - "velocityY": 0.6585212086715196, - "timestamp": 6.145069538412872 - }, - { - "x": 1.9701671600341797, - "y": 3.5328900814056396, - "heading": -0.765, - "angularVelocity": -0.19198048410191668, - "velocityX": -0.617100471546741, - "velocityY": 0.32925585007098457, - "timestamp": 6.2238437269106965 - }, - { - "x": 1.9701671600341797, - "y": 3.5328900814056396, - "heading": -0.765, - "angularVelocity": -6.585493827087804e-24, - "velocityX": 2.2425722891132833e-25, - "velocityY": -1.917344763102387e-25, - "timestamp": 6.302617915408521 - }, - { - "x": 2.021866320736439, - "y": 3.5175409291347064, - "heading": -0.7649999986292967, - "angularVelocity": 1.7960508607271394e-8, - "velocityX": 0.6774210494904483, - "velocityY": -0.20112200466940705, - "timestamp": 6.37893553314589 - }, - { - "x": 2.1252646329115827, - "y": 3.486842627325834, - "heading": -0.7649999949328548, - "angularVelocity": 4.8434973827775214e-8, - "velocityX": 1.354841978047161, - "velocityY": -0.4022439735280295, - "timestamp": 6.45525315088326 - }, - { - "x": 2.264271046839328, - "y": 3.4455725064565113, - "heading": -0.7629134147129198, - "angularVelocity": 0.027340740995291143, - "velocityX": 1.8214197199669733, - "velocityY": -0.5407679392109075, - "timestamp": 6.531570768620629 - }, - { - "x": 2.403277460756995, - "y": 3.404302385553228, - "heading": -0.7557796152547765, - "angularVelocity": 0.09347513286765456, - "velocityX": 1.8214197198349207, - "velocityY": -0.5407679396558968, - "timestamp": 6.607888386357998 - }, - { - "x": 2.5422838746752925, - "y": 3.3630322646520687, - "heading": -0.7445970322006652, - "angularVelocity": 0.1465268883600871, - "velocityX": 1.8214197198431827, - "velocityY": -0.5407679396280685, - "timestamp": 6.684206004095367 - }, - { - "x": 2.6812902885942673, - "y": 3.321762143753191, - "heading": -0.7301562502874436, - "angularVelocity": 0.1892195058147188, - "velocityX": 1.821419719852059, - "velocityY": -0.540767939598171, - "timestamp": 6.7605236218327365 - }, - { - "x": 2.8202967025138705, - "y": 3.28049202285643, - "heading": -0.7130778427136069, - "angularVelocity": 0.2237806692631405, - "velocityX": 1.8214197198602944, - "velocityY": -0.5407679395704321, - "timestamp": 6.836841239570106 - }, - { - "x": 2.9593031164340085, - "y": 3.2392219019614696, - "heading": -0.6938417314556549, - "angularVelocity": 0.25205335056643224, - "velocityX": 1.8214197198673001, - "velocityY": -0.5407679395468353, - "timestamp": 6.913158857307475 - }, - { - "x": 3.0983095303545722, - "y": 3.1979517810679434, - "heading": -0.6728120786629418, - "angularVelocity": 0.27555436629432156, - "velocityX": 1.8214197198728768, - "velocityY": -0.5407679395280517, - "timestamp": 6.989476475044844 - }, - { - "x": 3.2373159442754544, - "y": 3.1566816601754897, - "heading": -0.6502538625008166, - "angularVelocity": 0.2955833375165695, - "velocityX": 1.8214197198770503, - "velocityY": -0.540767939513994, - "timestamp": 7.065794092782213 - }, - { - "x": 3.376322358196558, - "y": 3.1154115392837802, - "heading": -0.6263485003282439, - "angularVelocity": 0.31323517270727913, - "velocityX": 1.8214197198799456, - "velocityY": -0.5407679395042426, - "timestamp": 7.142111710519583 - }, - { - "x": 3.515328772117798, - "y": 3.074141418392532, - "heading": -0.6012000869662939, - "angularVelocity": 0.3295230394702948, - "velocityX": 1.821419719881741, - "velocityY": -0.5407679394981955, - "timestamp": 7.218429328256952 - }, - { - "x": 3.6543351862262687, - "y": 3.032871298131916, - "heading": -0.5748442396833396, - "angularVelocity": 0.3453442083799368, - "velocityX": 1.821419722335047, - "velocityY": -0.5407679312349462, - "timestamp": 7.294746945994321 - }, - { - "x": 3.7934386683270223, - "y": 2.9919295436941926, - "heading": -0.5472452836552583, - "angularVelocity": 0.3616328293036799, - "velocityX": 1.8226916172809426, - "velocityY": -0.5364653097351076, - "timestamp": 7.37106456373169 + "heading": 0.2561010030603239, + "angularVelocity": 1.4585674185817174e-27, + "velocityX": 1.7040232418526742e-26, + "velocityY": -1.4024664023967223e-26, + "timestamp": 3.5163720671787804 + }, + { + "x": 8.23076641369307, + "y": 2.401959089356935, + "heading": 0.2504586013872685, + "angularVelocity": -0.08952800568025769, + "velocityX": -0.19551810957860963, + "velocityY": -0.1334123192654238, + "timestamp": 3.5793959403253224 + }, + { + "x": 8.206023953099224, + "y": 2.3852645245784534, + "heading": 0.2393386250800309, + "angularVelocity": -0.17644070019913852, + "velocityX": -0.39258870263834844, + "velocityY": -0.26489271358590016, + "timestamp": 3.6424198134718644 + }, + { + "x": 8.16875053390707, + "y": 2.3604262651400836, + "heading": 0.22292161113767642, + "angularVelocity": -0.26048881356723236, + "velocityX": -0.5914174634346175, + "velocityY": -0.3941087432157136, + "timestamp": 3.7054436866184064 + }, + { + "x": 8.118819252308954, + "y": 2.32761409583082, + "heading": 0.20141017927873436, + "angularVelocity": -0.34132195920304775, + "velocityX": -0.7922598073592967, + "velocityY": -0.520630797046853, + "timestamp": 3.7684675597649484 + }, + { + "x": 8.05608308887793, + "y": 2.287033381738045, + "heading": 0.17503801825565893, + "angularVelocity": -0.418447164644346, + "velocityX": -0.9954349090090397, + "velocityY": -0.6438943223691957, + "timestamp": 3.8314914329114904 + }, + { + "x": 7.980369725106798, + "y": 2.23893720959337, + "heading": 0.1440830091327847, + "angularVelocity": -0.4911632303349274, + "velocityX": -1.2013441889723828, + "velocityY": -0.7631421196987141, + "timestamp": 3.8945153060580324 + }, + { + "x": 7.891474875927368, + "y": 2.1836442871211665, + "heading": 0.10888684951759207, + "angularVelocity": -0.558457578977337, + "velocityX": -1.4104948607765395, + "velocityY": -0.8773329805300566, + "timestamp": 3.9575391792045744 + }, + { + "x": 7.789153901265338, + "y": 2.121566339688964, + "heading": 0.06988488995899932, + "angularVelocity": -0.6188442190454735, + "velocityX": -1.6235272374345413, + "velocityY": -0.9849909936169672, + "timestamp": 4.020563052351116 + }, + { + "x": 7.6731119959595455, + "y": 2.0532519037884556, + "heading": 0.027652018184153897, + "angularVelocity": -0.6701091136789754, + "velocityX": -1.8412372885425963, + "velocityY": -1.083945376407843, + "timestamp": 4.0835869254976584 + }, + { + "x": 7.542995375924786, + "y": 1.9794598312116642, + "heading": -0.01702647841112277, + "angularVelocity": -0.7089138506513402, + "velocityX": -2.0645608328801752, + "velocityY": -1.1708590553489355, + "timestamp": 4.1466107986442005 + }, + { + "x": 7.398393469015792, + "y": 1.9012891169513035, + "heading": -0.06304666165055829, + "angularVelocity": -0.7302023969937583, + "velocityX": -2.294398926780781, + "velocityY": -1.2403349771696772, + "timestamp": 4.2096346717907425 + }, + { + "x": 7.238888974084811, + "y": 1.8204184823931229, + "heading": -0.10882385565793654, + "angularVelocity": -0.7263468860591595, + "velocityX": -2.530858339348697, + "velocityY": -1.2831746213080508, + "timestamp": 4.2726585449372845 + }, + { + "x": 7.064291128353646, + "y": 1.7395528396698954, + "heading": -0.1519933648459332, + "angularVelocity": -0.6849707425568674, + "velocityX": -2.770344585538159, + "velocityY": -1.2830954158466277, + "timestamp": 4.3356824180838265 + }, + { + "x": 6.875551934246269, + "y": 1.6630631067471717, + "heading": -0.18835706354611678, + "angularVelocity": -0.5769829254325797, + "velocityX": -2.994725406173053, + "velocityY": -1.213662840823371, + "timestamp": 4.3987062912303685 + }, + { + "x": 6.6765745235709275, + "y": 1.5964044626442309, + "heading": -0.21283448502566993, + "angularVelocity": -0.3883833261506904, + "velocityX": -3.157175221724038, + "velocityY": -1.0576729225756598, + "timestamp": 4.4617301643769105 + }, + { + "x": 6.470320359493221, + "y": 1.5423309052989251, + "heading": -0.22714102483187307, + "angularVelocity": -0.22700191359134367, + "velocityX": -3.272635491603752, + "velocityY": -0.8579853100994786, + "timestamp": 4.5247540375234525 + }, + { + "x": 6.258646009422052, + "y": 1.5021402605713223, + "heading": -0.23470258386507345, + "angularVelocity": -0.11997928174960579, + "velocityX": -3.35863760037386, + "velocityY": -0.6377050904845553, + "timestamp": 4.5877779106699945 + }, + { + "x": 6.043403030940588, + "y": 1.4765880663278637, + "heading": -0.23830057114107106, + "angularVelocity": -0.05708927579921407, + "velocityX": -3.415261038955589, + "velocityY": -0.4054367490878456, + "timestamp": 4.6508017838165365 + }, + { + "x": 5.826227188110352, + "y": 1.4660362005233765, + "heading": -0.2397939206210153, + "angularVelocity": -0.023694981050623704, + "velocityX": -3.4459298038580273, + "velocityY": -0.16742648900603332, + "timestamp": 4.7138256569630785 + }, + { + "x": 5.585950011247348, + "y": 1.472816838427013, + "heading": -0.2400341563641951, + "angularVelocity": -0.003454323639043374, + "velocityX": -3.454919409470775, + "velocityY": 0.09749805540300888, + "timestamp": 4.78337204817682 + }, + { + "x": 5.346828039308694, + "y": 1.498003817957295, + "heading": -0.24003425372372114, + "angularVelocity": -0.0000013999220426161217, + "velocityX": -3.438308843427179, + "velocityY": 0.3621608409971011, + "timestamp": 4.852918439390561 + }, + { + "x": 5.110339958561461, + "y": 1.5414445236933543, + "heading": -0.24003430995698885, + "angularVelocity": -8.085720445004714e-7, + "velocityX": -3.4004364082734204, + "velocityY": 0.6246291860428812, + "timestamp": 4.922464830604302 + }, + { + "x": 4.87787710863241, + "y": 1.6028833780076126, + "heading": -0.24003435241212726, + "angularVelocity": -6.104578199921794e-7, + "velocityX": -3.34255805185645, + "velocityY": 0.8834226081614251, + "timestamp": 4.992011221818044 + }, + { + "x": 4.6508071407642975, + "y": 1.6819589143395182, + "heading": -0.24003438676872058, + "angularVelocity": -4.940097212282082e-7, + "velocityX": -3.26501438687515, + "velocityY": 1.1370185418949712, + "timestamp": 5.061557613031785 + }, + { + "x": 4.430465976004345, + "y": 1.7782059025459391, + "heading": -0.24003441611574786, + "angularVelocity": -4.2197771504773454e-7, + "velocityX": -3.1682616583621654, + "velocityY": 1.3839249819680632, + "timestamp": 5.131104004245526 + }, + { + "x": 4.218149944366051, + "y": 1.8910580842509719, + "heading": -0.24003444232077203, + "angularVelocity": -3.767991943003991e-7, + "velocityX": -3.0528691414881703, + "velocityY": 1.6226892544028175, + "timestamp": 5.2006503954592675 + }, + { + "x": 4.015108153343392, + "y": 2.0198514989870335, + "heading": -0.24003446662822625, + "angularVelocity": -3.495142422988789e-7, + "velocityX": -2.9195158437284845, + "velocityY": 1.851906511442587, + "timestamp": 5.270196786673009 + }, + { + "x": 3.822535116912615, + "y": 2.1638283667067197, + "heading": -0.24003448995474508, + "angularVelocity": -3.3540947876441084e-7, + "velocityX": -2.768986759340679, + "velocityY": 2.0702277315467437, + "timestamp": 5.33974317788675 + }, + { + "x": 3.6415634821904335, + "y": 2.3221412856645247, + "heading": -0.24003451305454837, + "angularVelocity": -3.3214956098454113e-7, + "velocityX": -2.602171465173362, + "velocityY": 2.2763642540595908, + "timestamp": 5.409289569100491 + }, + { + "x": 3.464389353911089, + "y": 2.4846929425572983, + "heading": -0.24003453619468335, + "angularVelocity": -3.3272948572772484e-7, + "velocityX": -2.5475675327972547, + "velocityY": 2.3373126061019125, + "timestamp": 5.478835960314233 + }, + { + "x": 3.2927692258661447, + "y": 2.639311449150887, + "heading": -0.26906245693036734, + "angularVelocity": -0.4173893171030334, + "velocityX": -2.4677071671122786, + "velocityY": 2.2232427002342847, + "timestamp": 5.548382351527974 + }, + { + "x": 3.1316790096082485, + "y": 2.78734512036997, + "heading": -0.33414951484331207, + "angularVelocity": -0.9358797311697813, + "velocityX": -2.31629871006257, + "velocityY": 2.128560068115134, + "timestamp": 5.617928742741715 + }, + { + "x": 2.9841668130955736, + "y": 2.9229007119052977, + "heading": -0.399802866713189, + "angularVelocity": -0.9440224104238585, + "velocityX": -2.1210618399927625, + "velocityY": 1.9491391166324632, + "timestamp": 5.687475133955457 + }, + { + "x": 2.850184873056573, + "y": 3.0460054481110204, + "heading": -0.4624392764751436, + "angularVelocity": -0.9006421277769835, + "velocityX": -1.9265117528129565, + "velocityY": 1.7701096211790086, + "timestamp": 5.757021525169198 + }, + { + "x": 2.729677322997678, + "y": 3.156697524743402, + "heading": -0.5206194141968715, + "angularVelocity": -0.8365658764797604, + "velocityX": -1.7327649638718297, + "velocityY": 1.5916293383532172, + "timestamp": 5.826567916382939 + }, + { + "x": 2.6226093994758237, + "y": 3.255010853878184, + "heading": -0.5735505408260317, + "angularVelocity": -0.7610909165147557, + "velocityX": -1.5395180346999657, + "velocityY": 1.4136366735784966, + "timestamp": 5.89611430759668 + }, + { + "x": 2.528959064966524, + "y": 3.3409742896619217, + "heading": -0.6207186986875527, + "angularVelocity": -0.6782258150039155, + "velocityX": -1.346587980697349, + "velocityY": 1.2360588994408095, + "timestamp": 5.965660698810422 + }, + { + "x": 2.448711492549665, + "y": 3.414612123721043, + "heading": -0.6617567306008036, + "angularVelocity": -0.5900814003005007, + "velocityX": -1.153871121367444, + "velocityY": 1.0588304119592038, + "timestamp": 6.035207090024163 + }, + { + "x": 2.381856029019407, + "y": 3.475944657171963, + "heading": -0.6963860701683602, + "angularVelocity": -0.4979315096469609, + "velocityX": -0.9613074433263825, + "velocityY": 0.8818938320238976, + "timestamp": 6.104753481237904 + }, + { + "x": 2.3283844592549388, + "y": 3.524988689664759, + "heading": -0.7243871660827067, + "angularVelocity": -0.40262471460652677, + "velocityX": -0.7688618896145184, + "velocityY": 0.7051988124310513, + "timestamp": 6.174299872451646 + }, + { + "x": 2.288289941735103, + "y": 3.5617579108116493, + "heading": -0.7455827114186115, + "angularVelocity": -0.30476844256034935, + "velocityX": -0.5765147093917502, + "velocityY": 0.5287006342843157, + "timestamp": 6.243846263665387 + }, + { + "x": 2.261566307205076, + "y": 3.5862632051940473, + "heading": -0.7598272213603463, + "angularVelocity": -0.20482026016211682, + "velocityX": -0.384256236213544, + "velocityY": 0.35235896435063624, + "timestamp": 6.313392654879128 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -0.1031366044229252, + "velocityX": -0.1920838428798636, + "velocityY": 0.17613685695088044, + "timestamp": 6.382939046092869 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": 2.0585376302520875e-27, + "velocityX": 2.290307616331396e-26, + "velocityY": 1.9208049966092184e-26, + "timestamp": 6.452485437306611 + }, + { + "x": 2.262787388478086, + "y": 3.590396400582275, + "heading": -0.766999999504867, + "angularVelocity": 7.484697168821697e-9, + "velocityX": 0.2203964012318347, + "velocityY": -0.12269319419383633, + "timestamp": 6.518638147942005 + }, + { + "x": 2.29194702702894, + "y": 3.5741634259266726, + "heading": -0.7669999984635936, + "angularVelocity": 1.5740448734328548e-8, + "velocityX": 0.4407928000346015, + "velocityY": -0.24538638703819796, + "timestamp": 6.5847908585773975 + }, + { + "x": 2.335686484549911, + "y": 3.5498139641128845, + "heading": -0.7669999968047696, + "angularVelocity": 2.5075676414905366e-8, + "velocityX": 0.6611891954366733, + "velocityY": -0.36807957799329516, + "timestamp": 6.6509435692127905 + }, + { + "x": 2.394005760703551, + "y": 3.5173480153283805, + "heading": -0.7669999944212796, + "angularVelocity": 3.603011826808707e-8, + "velocityX": 0.881585585737703, + "velocityY": -0.49077276611449583, + "timestamp": 6.7170962798481835 + }, + { + "x": 2.466904854927447, + "y": 3.476765579885611, + "heading": -0.7669999911345978, + "angularVelocity": 4.968325032500711e-8, + "velocityX": 1.1019819675369964, + "velocityY": -0.6134659495125361, + "timestamp": 6.783248990483576 + }, + { + "x": 2.5543837660967745, + "y": 3.428066658409475, + "heading": -0.7669999865876731, + "angularVelocity": 6.873376258795686e-8, + "velocityX": 1.3223783323328206, + "velocityY": -0.7361591234642574, + "timestamp": 6.849401701118969 + }, + { + "x": 2.6564424908370574, + "y": 3.371251252774671, + "heading": -0.7669999797093515, + "angularVelocity": 1.0397641269909702e-7, + "velocityX": 1.5427746461182656, + "velocityY": -0.8588522690770366, + "timestamp": 6.915554411754362 + }, + { + "x": 2.7662623647276248, + "y": 3.3101152676874617, + "heading": -0.7638688738714546, + "angularVelocity": 0.047331482078705314, + "velocityX": 1.6600963563813693, + "velocityY": -0.9241644748945469, + "timestamp": 6.981707122389755 + }, + { + "x": 2.8760822385936273, + "y": 3.2489792825561232, + "heading": -0.7552335462540741, + "angularVelocity": 0.13053626275384136, + "velocityX": 1.6600963560100328, + "velocityY": -0.9241644755616281, + "timestamp": 7.047859833025148 + }, + { + "x": 2.9859021124597604, + "y": 3.187843297425019, + "heading": -0.7414908207894041, + "angularVelocity": 0.2077424391634433, + "velocityX": 1.660096356012001, + "velocityY": -0.9241644755580926, + "timestamp": 7.114012543660541 + }, + { + "x": 3.0957219863265877, + "y": 3.1267073122951614, + "heading": -0.722965226886389, + "angularVelocity": 0.2800428542546129, + "velocityX": 1.6600963560224962, + "velocityY": -0.9241644755392399, + "timestamp": 7.180165254295934 + }, + { + "x": 3.205541937606669, + "y": 3.065571466224591, + "heading": -0.6999120814365468, + "angularVelocity": 0.34848376171464274, + "velocityX": 1.6600975262429405, + "velocityY": -0.9241623734441646, + "timestamp": 7.246317964931327 + }, + { + "x": 3.3205554118020584, + "y": 3.0148771458100074, + "heading": -0.675189942683574, + "angularVelocity": 0.37371316330831955, + "velocityX": 1.738605615562701, + "velocityY": -0.7663226484246434, + "timestamp": 7.31247067556672 + }, + { + "x": 3.4412706707327407, + "y": 2.979865153460279, + "heading": -0.6504677784331367, + "angularVelocity": 0.3737135487417248, + "velocityX": 1.8247968642738608, + "velocityY": -0.5292601317986865, + "timestamp": 7.378623386202113 + }, + { + "x": 3.5655600724891614, + "y": 2.961152646598003, + "heading": -0.6257456072463011, + "angularVelocity": 0.37371365359605785, + "velocityX": 1.878825532054966, + "velocityY": -0.28286833120734667, + "timestamp": 7.444776096837506 + }, + { + "x": 3.691232955543935, + "y": 2.9590694551388426, + "heading": -0.6010234330400018, + "angularVelocity": 0.3737136992398969, + "velocityX": 1.8997389804241076, + "velocityY": -0.03149064398345659, + "timestamp": 7.510928807472899 + }, + { + "x": 3.816074269542962, + "y": 2.973652301113666, + "heading": -0.5763012572280835, + "angularVelocity": 0.3737137235113, + "velocityX": 1.8871685347423104, + "velocityY": 0.22044215323538027, + "timestamp": 7.577081518108292 }, { "x": 3.9378836154937744, "y": 3.0046441555023193, - "heading": -0.5196460913813736, - "angularVelocity": 0.361635924864182, - "velocityX": 1.8926815517726128, - "velocityY": 0.16660126698243774, - "timestamp": 7.4473821814690595 - }, - { - "x": 4.075926460541626, - "y": 3.0786892532968806, - "heading": -0.4898305952486719, - "angularVelocity": 0.3616359432061461, - "velocityX": 1.674339217752807, - "velocityY": 0.8981024049221815, - "timestamp": 7.529828345056582 - }, - { - "x": 4.180404466119937, - "y": 3.195406219293214, - "heading": -0.4598310277403017, - "angularVelocity": 0.3638685683236616, - "velocityX": 1.2672270125389096, - "velocityY": 1.4156749194573495, - "timestamp": 7.612274508644104 - }, - { - "x": 4.2848822354372995, - "y": 3.3121233967757027, - "heading": -0.4287713690600015, - "angularVelocity": 0.37672654892338603, - "velocityX": 1.2672241468998562, - "velocityY": 1.4156774845998201, - "timestamp": 7.694720672231626 - }, - { - "x": 4.389360004752337, - "y": 3.428840574260275, - "heading": -0.39697164385324546, - "angularVelocity": 0.38570290991161005, - "velocityX": 1.2672241468716323, - "velocityY": 1.415677484625084, - "timestamp": 7.777166835819148 - }, - { - "x": 4.493837774065624, - "y": 3.545557751746413, - "heading": -0.36470801458445545, - "angularVelocity": 0.3913296612587668, - "velocityX": 1.2672241468504064, - "velocityY": 1.415677484644084, - "timestamp": 7.859612999406671 - }, - { - "x": 4.598315543377444, - "y": 3.662274929233865, - "heading": -0.3322260766024941, - "angularVelocity": 0.3939775553956429, - "velocityX": 1.267224146832605, - "velocityY": 1.4156774846600186, - "timestamp": 7.942059162994193 - }, - { - "x": 4.7027933126881845, - "y": 3.778992106722283, - "heading": -0.29975459263588233, - "angularVelocity": 0.3938507573143906, - "velocityX": 1.2672241468195173, - "velocityY": 1.415677484671734, - "timestamp": 8.024505326581716 - }, - { - "x": 4.807271113642882, - "y": 3.8957092558850426, - "heading": -0.26751670065443317, - "angularVelocity": 0.3910174904284827, - "velocityX": 1.2672245306330898, - "velocityY": 1.4156771411062206, - "timestamp": 8.106951490169239 + "heading": -0.5515790804630568, + "angularVelocity": 0.37371373791899654, + "velocityX": 1.841335672882331, + "velocityY": 0.4684895613645789, + "timestamp": 7.643234228743685 + }, + { + "x": 4.041142175460698, + "y": 3.0442215809369055, + "heading": -0.5298282579971898, + "angularVelocity": 0.37371374601013674, + "velocityX": 1.7741463943907607, + "velocityY": 0.6800031557338396, + "timestamp": 7.701436054868853 + }, + { + "x": 4.139081405257562, + "y": 3.095569514841118, + "heading": -0.5080774349580934, + "angularVelocity": 0.3737137558591309, + "velocityX": 1.68275183644991, + "velocityY": 0.8822392237965997, + "timestamp": 7.7596378809940205 + }, + { + "x": 4.230365088756901, + "y": 3.1579873993126313, + "heading": -0.4863266111818347, + "angularVelocity": 0.3737137685247537, + "velocityX": 1.5683989588750435, + "velocityY": 1.0724385921719906, + "timestamp": 7.817839707119188 + }, + { + "x": 4.31374781521548, + "y": 3.2306236449287598, + "heading": -0.46457578637658614, + "angularVelocity": 0.3737137862044347, + "velocityX": 1.4326479426824048, + "velocityY": 1.2480062989762448, + "timestamp": 7.876041533244356 + }, + { + "x": 4.388091972544511, + "y": 3.312487248347592, + "heading": -0.44282495993350157, + "angularVelocity": 0.3737138143450645, + "velocityX": 1.277350940314963, + "velocityY": 1.4065469912022657, + "timestamp": 7.934243359369524 + }, + { + "x": 4.452383275670078, + "y": 3.40246130939932, + "heading": -0.4210741301794481, + "angularVelocity": 0.3737138712327796, + "velocityX": 1.1046269061610605, + "velocityY": 1.5458975609842933, + "timestamp": 7.992445185494692 + }, + { + "x": 4.505744665604898, + "y": 3.4993182420166447, + "heading": -0.39932328851517956, + "angularVelocity": 0.3737140758692305, + "velocityX": 0.9168336027818437, + "velocityY": 1.6641562484487396, + "timestamp": 8.050647011619859 + }, + { + "x": 4.553249848192767, + "y": 3.5991779484118642, + "heading": -0.376755167144662, + "angularVelocity": 0.3877562419086832, + "velocityX": 0.8162146405115538, + "velocityY": 1.7157486808139633, + "timestamp": 8.108848837745025 + }, + { + "x": 4.608641716419732, + "y": 3.694888158238082, + "heading": -0.35375349017349716, + "angularVelocity": 0.3952054171238199, + "velocityX": 0.9517204513109475, + "velocityY": 1.644453725908632, + "timestamp": 8.167050663870192 + }, + { + "x": 4.674815961157583, + "y": 3.7834865366072044, + "heading": -0.33075180592440745, + "angularVelocity": 0.3952055421701522, + "velocityX": 1.1369788397281038, + "velocityY": 1.5222611431226378, + "timestamp": 8.22525248999536 + }, + { + "x": 4.750869807438685, + "y": 3.8637642779379298, + "heading": -0.3077501195977143, + "angularVelocity": 0.3952055778666857, + "velocityX": 1.3067261174510774, + "velocityY": 1.37929935665733, + "timestamp": 8.283454316120526 + }, + { + "x": 4.835765641210989, + "y": 3.9346261199009778, + "heading": -0.2847484322977862, + "angularVelocity": 0.3952055945884119, + "velocityX": 1.4586455344155085, + "velocityY": 1.2175192202845004, + "timestamp": 8.341656142245693 }, { "x": 4.928345203399658, "y": 3.995105266571045, - "heading": -0.23561923052699993, - "angularVelocity": 0.386888470481369, - "velocityX": 1.4685230274935492, - "velocityY": 1.205586850387367, - "timestamp": 8.189397653756762 - }, - { - "x": 5.0976675546813945, - "y": 4.035971247104969, - "heading": -0.20015091281875375, - "angularVelocity": 0.38688845257830395, - "velocityX": 1.846968413139047, - "velocityY": 0.4457661652272064, - "timestamp": 8.281073477813743 - }, - { - "x": 5.270991671667468, - "y": 4.053258165584531, - "heading": -0.16565527924639437, - "angularVelocity": 0.37627841284435726, - "velocityX": 1.890619678295383, - "velocityY": 0.18856572774100455, - "timestamp": 8.372749301870725 - }, - { - "x": 5.444315791533781, - "y": 4.0705450551858755, - "heading": -0.13259854065392815, - "angularVelocity": 0.3605829446585594, - "velocityX": 1.8906197097130382, - "velocityY": 0.1885654127373893, - "timestamp": 8.464425125927706 - }, - { - "x": 5.617639911400379, - "y": 4.087831944784366, - "heading": -0.10138296753519387, - "angularVelocity": 0.34049950943808543, - "velocityX": 1.8906197097161441, - "velocityY": 0.18856541270624588, - "timestamp": 8.556100949984687 - }, - { - "x": 5.790964031267238, - "y": 4.105118834380232, - "heading": -0.07255368127548635, - "angularVelocity": 0.3144698894856807, - "velocityX": 1.8906197097189994, - "velocityY": 0.1885654126776188, - "timestamp": 8.647776774041668 - }, - { - "x": 5.9642881511343315, - "y": 4.122405723973758, - "heading": -0.046846868579348507, - "angularVelocity": 0.2804099440672579, - "velocityX": 1.890619709721544, - "velocityY": 0.18856541265210713, - "timestamp": 8.73945259809865 - }, - { - "x": 6.137612271001621, - "y": 4.139692613565316, - "heading": -0.02524481611998314, - "angularVelocity": 0.23563521442620056, - "velocityX": 1.890619709723685, - "velocityY": 0.18856541263063997, - "timestamp": 8.831128422155631 - }, - { - "x": 6.310936390869065, - "y": 4.156979503155326, - "heading": -0.009053820761373118, - "angularVelocity": 0.17661139700851253, - "velocityX": 1.8906197097253696, - "velocityY": 0.18856541261374912, - "timestamp": 8.922804246212612 - }, - { - "x": 6.484260512045991, - "y": 4.174266379616022, - "heading": -2.2378824393561327e-8, - "angularVelocity": 0.09875884373748682, - "velocityX": 1.890619724009207, - "velocityY": 0.188565269399175, - "timestamp": 9.014480070269594 - }, - { - "x": 6.658350467681885, - "y": 4.179991245269775, - "heading": 1.903926994866919e-23, - "angularVelocity": 2.4410824362649644e-7, - "velocityX": 1.8989734472164426, - "velocityY": 0.06244684149438171, - "timestamp": 9.106155894326575 - }, - { - "x": 6.892610562082988, - "y": 4.180747179242153, - "heading": 1.815110943876254e-8, - "angularVelocity": 2.0923543279309324e-7, - "velocityX": 2.7004141208851267, - "velocityY": 0.008713967176889024, - "timestamp": 9.192905586749424 - }, - { - "x": 7.135847606603004, - "y": 4.164439410363542, - "heading": 1.8151086995989633e-8, - "angularVelocity": -2.587072333894115e-13, - "velocityX": 2.803895180796635, - "velocityY": -0.18798647491591486, - "timestamp": 9.279655279172273 - }, - { - "x": 7.379084647879251, - "y": 4.14813159310281, - "heading": 1.8151064626797785e-8, - "angularVelocity": -2.5785903361398285e-13, - "velocityX": 2.803895143404351, - "velocityY": -0.18798703263687086, - "timestamp": 9.366404971595122 - }, - { - "x": 7.6223216891554895, - "y": 4.131823775841945, - "heading": 1.8151042187582598e-8, - "angularVelocity": -2.586662220352974e-13, - "velocityX": 2.8038951434042487, - "velocityY": -0.18798703263839134, - "timestamp": 9.45315466401797 - }, - { - "x": 7.865558730431713, - "y": 4.115515958581081, - "heading": 1.815101972750476e-8, - "angularVelocity": -2.589067144937878e-13, - "velocityX": 2.803895143404087, - "velocityY": -0.18798703263838049, - "timestamp": 9.53990435644082 - }, - { - "x": 8.07413296336415, - "y": 4.101532107856125, - "heading": 9.075510567318239e-9, - "angularVelocity": -1.0461719121664748e-7, - "velocityX": 2.404322448957763, - "velocityY": -0.16119769804825285, - "timestamp": 9.626654048863669 - }, - { - "x": 8.213182462520122, - "y": 4.092209539999878, - "heading": 3.025170279496359e-9, - "angularVelocity": -6.974480391619583e-8, - "velocityX": 1.6028817540723412, - "velocityY": -0.10746514017369478, - "timestamp": 9.713403741286518 - }, - { - "x": 8.282707214355469, - "y": 4.08754825592041, - "heading": -2.730279460976851e-23, - "angularVelocity": -3.487240352104816e-8, - "velocityX": 0.8014409030577104, - "velocityY": -0.05373257183146045, - "timestamp": 9.800153433709367 - }, - { - "x": 8.282707214355469, - "y": 4.08754825592041, - "heading": -1.3454200564607368e-23, - "angularVelocity": 4.663099853298001e-24, - "velocityX": 1.7036719365270032e-27, - "velocityY": 2.800838691430789e-26, - "timestamp": 9.886903126132216 - }, - { - "x": 8.214708333565108, - "y": 4.098060716631318, - "heading": 1.6847207046591105e-8, - "angularVelocity": 1.954345432230813e-7, - "velocityX": -0.7888150344560528, - "velocityY": 0.12194887565072522, - "timestamp": 9.973106960770398 - }, - { - "x": 8.078710576708064, - "y": 4.119085637322864, - "heading": 5.0541618776805506e-8, - "angularVelocity": 3.9086905903476216e-7, - "velocityX": -1.577630014115494, - "velocityY": 0.24389774283002824, - "timestamp": 10.05931079540858 - }, - { - "x": 7.8747139579553656, - "y": 4.15062301580424, - "heading": 1.0108322622636101e-7, - "angularVelocity": 5.863034708570761e-7, - "velocityX": -2.3664448293851432, - "velocityY": 0.3658465845950713, - "timestamp": 10.145514630046762 - }, - { - "x": 7.635308863013457, - "y": 4.1876344581296925, - "heading": 1.0108335901717796e-7, - "angularVelocity": 1.540428189705246e-12, - "velocityX": -2.7771977423829006, - "velocityY": 0.42934798064143237, - "timestamp": 10.231718464684944 - }, - { - "x": 7.395903768071533, - "y": 4.224645900455147, - "heading": 1.0108349186566566e-7, - "angularVelocity": 1.5410971948056658e-12, - "velocityX": -2.7771977423830965, - "velocityY": 0.42934798064146257, - "timestamp": 10.317922299323126 - }, - { - "x": 7.156498673129608, - "y": 4.261657342780601, - "heading": 1.0108362465293872e-7, - "angularVelocity": 1.5403870787070826e-12, - "velocityX": -2.7771977423830965, - "velocityY": 0.42934798064146257, - "timestamp": 10.404126133961308 - }, - { - "x": 6.917093578187684, - "y": 4.298668785106054, - "heading": 1.0108375751155647e-7, - "angularVelocity": 1.5412147075799493e-12, - "velocityX": -2.7771977423830965, - "velocityY": 0.4293479806414625, - "timestamp": 10.49032996859949 - }, - { - "x": 6.677688483245759, - "y": 4.335680227431509, - "heading": 1.0108389035423936e-7, - "angularVelocity": 1.5410298558530182e-12, - "velocityX": -2.7771977423830965, - "velocityY": 0.4293479806414627, - "timestamp": 10.576533803237671 - }, - { - "x": 6.438283388303835, - "y": 4.372691669756963, - "heading": 1.0108402320108348e-7, - "angularVelocity": 1.5410781280848912e-12, - "velocityX": -2.777197742383096, - "velocityY": 0.4293479806414625, - "timestamp": 10.662737637875853 - }, - { - "x": 6.1988782933619095, - "y": 4.409703112082417, - "heading": 1.0108415606074574e-7, - "angularVelocity": 1.5412268238689798e-12, - "velocityX": -2.7771977423830965, - "velocityY": 0.42934798064146257, - "timestamp": 10.748941472514035 - }, - { - "x": 5.959473198419985, - "y": 4.446714554407871, - "heading": 1.0108428894851187e-7, - "angularVelocity": 1.5415528401754331e-12, - "velocityX": -2.777197742383096, - "velocityY": 0.42934798064146257, - "timestamp": 10.835145307152217 - }, - { - "x": 5.72006810347805, - "y": 4.483725996733255, - "heading": 1.0108442171706879e-7, - "angularVelocity": 1.5401699643641334e-12, - "velocityX": -2.7771977423832217, - "velocityY": 0.4293479806406499, - "timestamp": 10.9213491417904 - }, - { - "x": 5.480663004845917, - "y": 4.520737415188933, - "heading": 1.0108455459674728e-7, - "angularVelocity": 1.5414590205195092e-12, - "velocityX": -2.7771977851910266, - "velocityY": 0.4293477037422273, - "timestamp": 11.007552976428581 - }, - { - "x": 5.240152409397331, - "y": 4.549708085687679, - "heading": 1.0108468832096651e-7, - "angularVelocity": 1.5512560408121339e-12, - "velocityX": -2.790022003755009, - "velocityY": 0.3360717144468437, - "timestamp": 11.093756811066763 - }, - { - "x": 5.001198008888612, - "y": 4.509890511709968, - "heading": 1.0108464169686405e-7, - "angularVelocity": -5.408587989447552e-13, - "velocityX": -2.7719695012602092, - "velocityY": -0.46190026400605494, - "timestamp": 11.179960645704945 - }, - { - "x": 4.783077239990234, - "y": 4.404496192932129, - "heading": 4.371418464571693e-25, - "angularVelocity": -0.0000011726234931559648, - "velocityX": -2.5302907905881877, - "velocityY": -1.2226175229930707, - "timestamp": 11.266164480343127 - }, - { - "x": 4.627041500035927, - "y": 4.275874365802291, - "heading": -0.00461711894400582, - "angularVelocity": -0.06368383973540494, - "velocityX": -2.152198194752189, - "velocityY": -1.774078581199584, - "timestamp": 11.338665117289471 - }, - { - "x": 4.5068214168723975, - "y": 4.114451023957695, - "heading": -0.01240880783073842, - "angularVelocity": -0.10747062667185843, - "velocityX": -1.6581934756311134, - "velocityY": -2.226509292105373, - "timestamp": 11.411165754235816 - }, - { - "x": 4.429140519397286, - "y": 3.930818027867112, - "heading": -0.027772737926692166, - "angularVelocity": -0.21191441541850167, - "velocityX": -1.0714512416297768, - "velocityY": -2.53284665935394, - "timestamp": 11.48366639118216 - }, - { - "x": 4.383091867034577, - "y": 3.7471675526312227, - "heading": -0.08613270246137088, - "angularVelocity": -0.8049579561331118, - "velocityX": -0.635148245618701, - "velocityY": -2.5330877488952703, - "timestamp": 11.556167028128504 - }, - { - "x": 4.346524489213487, - "y": 3.5742818254238546, - "heading": -0.20843940130739744, - "angularVelocity": -1.6869741287451163, - "velocityX": -0.5043731939645176, - "velocityY": -2.3846097701915894, - "timestamp": 11.628667665074849 - }, - { - "x": 4.289423143132127, - "y": 3.4062373324265396, - "heading": -0.32755846162196556, - "angularVelocity": -1.643007086995987, - "velocityX": -0.7875978541211613, - "velocityY": -2.317834712565055, - "timestamp": 11.701168302021193 - }, - { - "x": 4.203799594769533, - "y": 3.2403627849834162, - "heading": -0.40165980770192133, - "angularVelocity": -1.022078552700102, - "velocityX": -1.1810040845014969, - "velocityY": -2.28790469200817, - "timestamp": 11.773668938967537 - }, - { - "x": 4.079421660594405, - "y": 3.103563620218193, - "heading": -0.4716628621936354, - "angularVelocity": -0.9655508894841919, - "velocityX": -1.7155426409174284, - "velocityY": -1.8868684542242327, - "timestamp": 11.846169575913882 + "heading": -0.2617467444324387, + "angularVelocity": 0.39520560430321505, + "velocityX": 1.5906642171255918, + "velocityY": 1.0391279912764597, + "timestamp": 8.39985796837086 + }, + { + "x": 5.054848385978205, + "y": 4.053342311213538, + "heading": -0.23277931082663741, + "angularVelocity": 0.3952056123787909, + "velocityX": 1.7258956530000658, + "velocityY": 0.7945338618626627, + "timestamp": 8.473155089409811 + }, + { + "x": 5.188526339981156, + "y": 4.092391169041153, + "heading": -0.20381187629917086, + "angularVelocity": 0.39520562495316197, + "velocityX": 1.8237817817143471, + "velocityY": 0.5327474977750234, + "timestamp": 8.546452210448763 + }, + { + "x": 5.326486517103212, + "y": 4.111406906532098, + "heading": -0.17484444007241956, + "angularVelocity": 0.3952056481366752, + "velocityX": 1.8822045827521574, + "velocityY": 0.25943362060346054, + "timestamp": 8.619749331487714 + }, + { + "x": 5.465743715620603, + "y": 4.1099781194933795, + "heading": -0.1458769989760029, + "angularVelocity": 0.3952057145740096, + "velocityX": 1.8998999761994304, + "velocityY": -0.01949308538270602, + "timestamp": 8.693046452526666 + }, + { + "x": 5.604302908818607, + "y": 4.095979592520542, + "heading": -0.1165828656276628, + "angularVelocity": 0.3996628098499595, + "velocityX": 1.8903770193698237, + "velocityY": -0.19098331250144138, + "timestamp": 8.766343573565617 + }, + { + "x": 5.7428620917203625, + "y": 4.08198096363453, + "heading": -0.08691799128062003, + "angularVelocity": 0.4047208666119116, + "velocityX": 1.8903768788971027, + "velocityY": -0.19098470291312333, + "timestamp": 8.839640694604569 + }, + { + "x": 5.881421274622045, + "y": 4.067982334747774, + "heading": -0.057160358126426926, + "angularVelocity": 0.40598638435443013, + "velocityX": 1.8903768788960766, + "velocityY": -0.19098470292328013, + "timestamp": 8.91293781564352 + }, + { + "x": 6.019980457523705, + "y": 4.053983705860801, + "heading": -0.027589119251388173, + "angularVelocity": 0.4034433884425539, + "velocityX": 1.8903768788957764, + "velocityY": -0.19098470292625017, + "timestamp": 8.986234936682472 + }, + { + "x": 6.158539640425345, + "y": 4.039985076973634, + "heading": 0.0015129980186348265, + "angularVelocity": 0.39704311516624813, + "velocityX": 1.8903768788955113, + "velocityY": -0.19098470292887434, + "timestamp": 9.059532057721423 + }, + { + "x": 6.297098823326969, + "y": 4.025986448086315, + "heading": 0.029855200510632778, + "angularVelocity": 0.3866755213610124, + "velocityX": 1.8903768788953004, + "velocityY": -0.19098470293096353, + "timestamp": 9.132829178760375 + }, + { + "x": 6.435658006228585, + "y": 4.011987819198895, + "heading": 0.05713171335387273, + "angularVelocity": 0.3721362102168317, + "velocityX": 1.8903768788951598, + "velocityY": -0.19098470293235562, + "timestamp": 9.206126299799326 + }, + { + "x": 6.574217189130194, + "y": 3.9979891903114315, + "heading": 0.08301592008791062, + "angularVelocity": 0.35314083782748296, + "velocityX": 1.8903768788951019, + "velocityY": -0.19098470293292794, + "timestamp": 9.279423420838278 + }, + { + "x": 6.712776372031808, + "y": 3.983990561423991, + "heading": 0.10715451282177094, + "angularVelocity": 0.3293252503196199, + "velocityX": 1.8903768788951338, + "velocityY": -0.19098470293261177, + "timestamp": 9.352720541877229 + }, + { + "x": 6.851335554933429, + "y": 3.9699919325366397, + "heading": 0.12916191998569537, + "angularVelocity": 0.30024927107613325, + "velocityX": 1.8903768788952555, + "velocityY": -0.1909847029314087, + "timestamp": 9.42601766291618 + }, + { + "x": 6.989894737835075, + "y": 3.9559933036495325, + "heading": 0.14861220477274031, + "angularVelocity": 0.26536219310317194, + "velocityX": 1.8903768788955921, + "velocityY": -0.1909847029280752, + "timestamp": 9.499314783955132 + }, + { + "x": 7.128453922674329, + "y": 3.9419946939409853, + "heading": 0.165034903130564, + "angularVelocity": 0.2240565267099137, + "velocityX": 1.8903769053305712, + "velocityY": -0.19098444127304595, + "timestamp": 9.572611904994083 + }, + { + "x": 7.267333565421739, + "y": 3.9316480226780426, + "heading": 0.17810972743336856, + "angularVelocity": 0.17838114399958924, + "velocityX": 1.8947489448269792, + "velocityY": -0.1411606774765978, + "timestamp": 9.645909026033035 + }, + { + "x": 7.406228542327881, + "y": 3.941786766052246, + "heading": 0.1911844788186837, + "angularVelocity": 0.1783801491789407, + "velocityX": 1.8949581502980173, + "velocityY": 0.13832389636170525, + "timestamp": 9.719206147071986 + }, + { + "x": 7.565442985719265, + "y": 3.9581600818899694, + "heading": 0.2043161039154577, + "angularVelocity": 0.1783801287415228, + "velocityX": 2.162770616764113, + "velocityY": 0.22241528870453642, + "timestamp": 9.79282211085348 + }, + { + "x": 7.726607611629632, + "y": 3.9948081017418207, + "heading": 0.2081850028863188, + "angularVelocity": 0.052555162931029055, + "velocityX": 2.189261915917248, + "velocityY": 0.49782707403830934, + "timestamp": 9.866438074634974 + }, + { + "x": 7.867453146090771, + "y": 4.027711655278318, + "heading": 0.21132312519311341, + "angularVelocity": 0.04262828530112288, + "velocityX": 1.913247171214066, + "velocityY": 0.44696220556402627, + "timestamp": 9.940054038416468 + }, + { + "x": 7.988106192724927, + "y": 4.056232601254953, + "heading": 0.21391875024315468, + "angularVelocity": 0.03525899705321495, + "velocityX": 1.6389522114018413, + "velocityY": 0.38742881994034783, + "timestamp": 10.013670002197962 + }, + { + "x": 8.088612865120389, + "y": 4.0801619694506375, + "heading": 0.2160337271012685, + "angularVelocity": 0.02872986713033435, + "velocityX": 1.3652836590414905, + "velocityY": 0.3250567807101139, + "timestamp": 10.087285965979456 + }, + { + "x": 8.168996921740131, + "y": 4.099396001381588, + "heading": 0.2176988235230518, + "angularVelocity": 0.022618686712105623, + "velocityX": 1.0919378418835648, + "velocityY": 0.26127528518191084, + "timestamp": 10.16090192976095 + }, + { + "x": 8.229272838087912, + "y": 4.11387267730592, + "heading": 0.21893245113612445, + "angularVelocity": 0.016757610030540077, + "velocityX": 0.8187886601157901, + "velocityY": 0.19665131284977577, + "timestamp": 10.234517893542444 + }, + { + "x": 8.269450355435243, + "y": 4.123550749253992, + "heading": 0.21974686245033065, + "angularVelocity": 0.011062971567194144, + "velocityX": 0.5457718038791941, + "velocityY": 0.13146702767891838, + "timestamp": 10.308133857323938 + }, + { + "x": 8.289536476135254, + "y": 4.128400802612305, + "heading": 0.22015079613001912, + "angularVelocity": 0.005487039209150268, + "velocityX": 0.2728500676786763, + "velocityY": 0.06588317409942288, + "timestamp": 10.381749821105432 + }, + { + "x": 8.289536476135254, + "y": 4.128400802612305, + "heading": 0.22015079613001912, + "angularVelocity": 3.037718139065862e-28, + "velocityX": -8.872519474147407e-27, + "velocityY": 3.446506807522548e-26, + "timestamp": 10.455365784886926 + }, + { + "x": 8.269996806011259, + "y": 4.137231467736895, + "heading": 0.22023861459550317, + "angularVelocity": 0.0011710803997845823, + "velocityX": -0.2605662097867229, + "velocityY": 0.11775904745621692, + "timestamp": 10.530355055905561 + }, + { + "x": 8.230762965216833, + "y": 4.154542716289096, + "heading": 0.22035463158445837, + "angularVelocity": 0.0015471145055716861, + "velocityX": -0.5231927215918144, + "velocityY": 0.23084967112027163, + "timestamp": 10.605344326924197 + }, + { + "x": 8.171657212153168, + "y": 4.179909246382009, + "heading": 0.22042856436582195, + "angularVelocity": 0.0009859114558561591, + "velocityX": -0.7881894604493174, + "velocityY": 0.3382687916330005, + "timestamp": 10.680333597942832 + }, + { + "x": 8.092474127272867, + "y": 4.212804284514623, + "heading": 0.22037649675261314, + "angularVelocity": -0.0006943341694284194, + "velocityX": -1.0559255184734737, + "velocityY": 0.4386632605674932, + "timestamp": 10.755322868961468 + }, + { + "x": 7.992976017836279, + "y": 4.252560097776598, + "heading": 0.22009679360683257, + "angularVelocity": -0.003729908852042051, + "velocityX": -1.3268312664602737, + "velocityY": 0.5301533502318783, + "timestamp": 10.830312139980103 + }, + { + "x": 7.87288978503091, + "y": 4.298306284549337, + "heading": 0.21946436859290286, + "angularVelocity": -0.008433539962971137, + "velocityX": -1.6013788529231974, + "velocityY": 0.6100364245622665, + "timestamp": 10.905301410998739 + }, + { + "x": 7.731910356436966, + "y": 4.348869169420612, + "heading": 0.21832244216002894, + "angularVelocity": -0.015227864164596877, + "velocityX": -1.8799946536206542, + "velocityY": 0.6742682544374918, + "timestamp": 10.980290682017374 + }, + { + "x": 7.569726421911416, + "y": 4.402600574685328, + "heading": 0.21647017920240058, + "angularVelocity": -0.024700372899585307, + "velocityX": -2.162761850095162, + "velocityY": 0.7165212374362642, + "timestamp": 11.05527995303601 + }, + { + "x": 7.386118026863393, + "y": 4.4570773650237445, + "heading": 0.21364200538162792, + "angularVelocity": -0.0377143794352908, + "velocityX": -2.4484621940436835, + "velocityY": 0.7264611270174774, + "timestamp": 11.130269224054645 + }, + { + "x": 7.181283119008862, + "y": 4.5085907210335865, + "heading": 0.20946226202822887, + "angularVelocity": -0.055737884855026475, + "velocityX": -2.731522857498233, + "velocityY": 0.6869430161154807, + "timestamp": 11.20525849507328 + }, + { + "x": 6.956820012371809, + "y": 4.551523922612317, + "heading": 0.20331318020756176, + "angularVelocity": -0.08199948788859508, + "velocityX": -2.9932696182801153, + "velocityY": 0.5725245891250452, + "timestamp": 11.280247766091916 + }, + { + "x": 6.7176650220814205, + "y": 4.57889627508473, + "heading": 0.19410641329547768, + "angularVelocity": -0.12277445542571251, + "velocityX": -3.189189427257647, + "velocityY": 0.3650169164281033, + "timestamp": 11.355237037110552 + }, + { + "x": 6.472067123940792, + "y": 4.58587963769058, + "heading": 0.18079068250006503, + "angularVelocity": -0.1775684789908641, + "velocityX": -3.2751071560569964, + "velocityY": 0.09312482320456428, + "timestamp": 11.430226308129187 + }, + { + "x": 6.227490944705127, + "y": 4.571499129973418, + "heading": 0.16337266739917577, + "angularVelocity": -0.23227342877570947, + "velocityX": -3.261482288244776, + "velocityY": -0.1917675358330633, + "timestamp": 11.505215579147823 + }, + { + "x": 5.988790028800353, + "y": 4.536538809179358, + "heading": 0.14241529165637518, + "angularVelocity": -0.2794716558531782, + "velocityX": -3.1831342359023242, + "velocityY": -0.4662043025511265, + "timestamp": 11.580204850166458 + }, + { + "x": 5.758924851198093, + "y": 4.482073744796014, + "heading": 0.11847095232121185, + "angularVelocity": -0.31930353515789156, + "velocityX": -3.0653075363959554, + "velocityY": -0.7263047585808593, + "timestamp": 11.655194121185094 + }, + { + "x": 5.5397707564905145, + "y": 4.409059239841931, + "heading": 0.09196886032537212, + "angularVelocity": -0.35341178325702044, + "velocityX": -2.92247266483119, + "velocityY": -0.9736660186486978, + "timestamp": 11.73018339220373 + }, + { + "x": 5.33258476452675, + "y": 4.318274659916258, + "heading": 0.06323280289856088, + "angularVelocity": -0.3832022506215635, + "velocityX": -2.7628751306607398, + "velocityY": -1.21063425063984, + "timestamp": 11.805172663222365 + }, + { + "x": 5.138254335054246, + "y": 4.210347005162901, + "heading": 0.03251123556536211, + "angularVelocity": -0.4096795036927947, + "velocityX": -2.5914431068974344, + "velocityY": -1.4392412846170675, + "timestamp": 11.880161934241 + }, + { + "x": 4.957433700561523, + "y": 4.085783958435059, + "heading": -7.345594756701693e-29, + "angularVelocity": -0.4335451608441839, + "velocityX": -2.41128673524227, + "velocityY": -1.661078245405117, + "timestamp": 11.955151205259636 + }, + { + "x": 4.850763114224524, + "y": 4.00236715124268, + "heading": -0.02077718123853542, + "angularVelocity": -0.4471189141923733, + "velocityX": -2.2955200800195295, + "velocityY": -1.795105497182433, + "timestamp": 12.001620233026141 + }, + { + "x": 4.749732174637679, + "y": 3.9129578387828414, + "heading": -0.04223315292346125, + "angularVelocity": -0.4617262877273038, + "velocityX": -2.174156517637868, + "velocityY": -1.9240624725160098, + "timestamp": 12.048089260792647 + }, + { + "x": 4.654715925578021, + "y": 3.817934442549008, + "heading": -0.06443390851309351, + "angularVelocity": -0.47775382134493777, + "velocityX": -2.0447221219494662, + "velocityY": -2.0448759270647874, + "timestamp": 12.094558288559153 + }, + { + "x": 4.566286302759079, + "y": 3.717984210531405, + "heading": -0.08747291743607646, + "angularVelocity": -0.4957927899578095, + "velocityX": -1.9029798355858998, + "velocityY": -2.1509000041882853, + "timestamp": 12.141027316325658 + }, + { + "x": 4.485327184216237, + "y": 3.614602306633809, + "heading": -0.11148870772971044, + "angularVelocity": -0.5168128417557317, + "velocityX": -1.7422167502543655, + "velocityY": -2.2247485877488473, + "timestamp": 12.187496344092164 + }, + { + "x": 4.412567946745739, + "y": 3.511651220217229, + "heading": -0.13677526525666567, + "angularVelocity": -0.5441593840528177, + "velocityX": -1.5657576878107506, + "velocityY": -2.21547752050852, + "timestamp": 12.23396537185867 + }, + { + "x": 4.345051921407887, + "y": 3.4150129773556204, + "heading": -0.1637765662543946, + "angularVelocity": -0.581060166212278, + "velocityX": -1.4529252834189115, + "velocityY": -2.079626958136264, + "timestamp": 12.280434399625175 + }, + { + "x": 4.278849963154741, + "y": 3.3264929134982726, + "heading": -0.19187383355887797, + "angularVelocity": -0.6046450432676215, + "velocityX": -1.4246469408783942, + "velocityY": -1.9049261005015221, + "timestamp": 12.32690342739168 + }, + { + "x": 4.21205059305352, + "y": 3.246181936974994, + "heading": -0.22056728070035955, + "angularVelocity": -0.6174746604482066, + "velocityX": -1.437503070580043, + "velocityY": -1.728268921975704, + "timestamp": 12.373372455158187 + }, + { + "x": 4.143696181766486, + "y": 3.1739556218387466, + "heading": -0.24959647824597708, + "angularVelocity": -0.6246999117666328, + "velocityX": -1.4709671058860057, + "velocityY": -1.5542893537425588, + "timestamp": 12.419841482924692 + }, + { + "x": 4.073234187126779, + "y": 3.109688534157438, + "heading": -0.27881410074968266, + "angularVelocity": -0.6287547622152977, + "velocityX": -1.5163216883675306, + "velocityY": -1.3830090873480942, + "timestamp": 12.466310510691198 + }, + { + "x": 4.000309863583725, + "y": 3.0532784271020903, + "heading": -0.30812855213660195, + "angularVelocity": -0.6308384916985205, + "velocityX": -1.5693102922118025, + "velocityY": -1.2139291430583163, + "timestamp": 12.512779538457703 }, { "x": 3.924677610397339, "y": 3.0046441555023193, - "heading": -0.5385444180262977, - "angularVelocity": -0.9224961138225419, - "velocityX": -2.134381940831591, - "velocityY": -1.3643944230321734, - "timestamp": 11.918670212860226 - }, - { - "x": 3.7636444729485263, - "y": 2.9505820220343484, - "heading": -0.5968694156691691, - "angularVelocity": -0.8693980375808563, - "velocityX": -2.40037547092122, - "velocityY": -0.8058553732360577, - "timestamp": 11.98575685801204 - }, - { - "x": 3.5921162225925003, - "y": 2.93585850684271, - "heading": -0.6464965554196345, - "angularVelocity": -0.7397469293353471, - "velocityX": -2.556816635678569, - "velocityY": -0.21947013684048228, - "timestamp": 12.052843503163855 - }, - { - "x": 3.416479572667743, - "y": 2.9596548551854616, - "heading": -0.6807775343446694, - "angularVelocity": -0.5109955766525223, - "velocityX": -2.6180568357129337, - "velocityY": 0.3547106624410992, - "timestamp": 12.11993014831567 - }, - { - "x": 3.2390028785000764, - "y": 3.015476958858974, - "heading": -0.6885344713310354, - "angularVelocity": -0.115625650512295, - "velocityX": -2.645484712583867, - "velocityY": 0.8320896587866112, - "timestamp": 12.187016793467484 - }, - { - "x": 3.0657580475568027, - "y": 3.089829788896707, - "heading": -0.6885346535011025, - "angularVelocity": -0.00000271544458244769, - "velocityX": -2.5824041513959415, - "velocityY": 1.1083104523929412, - "timestamp": 12.254103438619298 - }, - { - "x": 2.8925132745209385, - "y": 3.1641827538610285, - "heading": -0.6885348356707467, - "angularVelocity": -0.000002715438277887047, - "velocityX": -2.582403288222506, - "velocityY": 1.1083124636216772, - "timestamp": 12.321190083771112 - }, - { - "x": 2.7192685014854168, - "y": 3.2385357188261574, - "heading": -0.6885350178403685, - "angularVelocity": -0.000002715437944094969, - "velocityX": -2.5824032882174053, - "velocityY": 1.108312463633708, - "timestamp": 12.388276728922927 - }, - { - "x": 2.5460237284499403, - "y": 3.312888683791401, - "heading": -0.6885352000099678, - "angularVelocity": -0.0000027154376105306044, - "velocityX": -2.5824032882167316, - "velocityY": 1.1083124636354227, - "timestamp": 12.455363374074741 - }, - { - "x": 2.3727789637362293, - "y": 3.3872416681385586, - "heading": -0.6885353821903477, - "angularVelocity": -0.0000027155983032171972, - "velocityX": -2.5824031641716974, - "velocityY": 1.108312752544088, - "timestamp": 12.522450019226556 - }, - { - "x": 2.2223047239417384, - "y": 3.4507934701253355, - "heading": -0.7191097564620745, - "angularVelocity": -0.45574457036177557, - "velocityX": -2.2429835245744076, - "velocityY": 0.9473092870117661, - "timestamp": 12.58953666437837 - }, - { - "x": 2.109444085282166, - "y": 3.4984470664225062, - "heading": -0.7420547518086502, - "angularVelocity": -0.34202031260695603, - "velocityX": -1.6823115599859222, - "velocityY": 0.7103291003646417, - "timestamp": 12.656623309530184 - }, - { - "x": 2.0342014899368133, - "y": 3.5302130152316624, - "heading": -0.7573535418351998, - "angularVelocity": -0.22804523898800838, - "velocityX": -1.1215733798445486, - "velocityY": 0.47350629528829474, - "timestamp": 12.723709954681999 - }, - { - "x": 1.996579647064209, - "y": 3.5460963249206543, - "heading": -0.765, - "angularVelocity": -0.11397884254752216, - "velocityX": -0.5607948167249468, - "velocityY": 0.23675814542593593, - "timestamp": 12.790796599833813 - }, - { - "x": 1.996579647064209, - "y": 3.5460963249206543, - "heading": -0.765, - "angularVelocity": 6.11903975445096e-25, - "velocityX": -4.7665538572669345e-25, - "velocityY": 2.8118183260637723e-25, - "timestamp": 12.857883244985628 + "heading": -0.3374787219433775, + "angularVelocity": -0.6316071417343185, + "velocityX": -1.627584152748309, + "velocityY": -1.0465954193004559, + "timestamp": 12.559248566224209 + }, + { + "x": 3.8052119909980813, + "y": 2.949187806356286, + "heading": -0.3813210096823129, + "angularVelocity": -0.6313779373848833, + "velocityX": -1.720438422690351, + "velocityY": -0.798633400410426, + "timestamp": 12.628687621887964 + }, + { + "x": 3.6796645340407665, + "y": 2.911082197769309, + "heading": -0.4250077410083283, + "angularVelocity": -0.6291377512038759, + "velocityX": -1.808023680005878, + "velocityY": -0.5487633468331752, + "timestamp": 12.69812667755172 + }, + { + "x": 3.548510692212512, + "y": 2.8904863098039106, + "heading": -0.4683618068061113, + "angularVelocity": -0.6243469958421788, + "velocityX": -1.8887618873065017, + "velocityY": -0.29660380269469544, + "timestamp": 12.767565733215475 + }, + { + "x": 3.4123915084504293, + "y": 2.887591555300515, + "heading": -0.5111475650763733, + "angularVelocity": -0.6161627323597821, + "velocityX": -1.960268359944496, + "velocityY": -0.04168769974944769, + "timestamp": 12.83700478887923 + }, + { + "x": 3.2722134837537085, + "y": 2.9026251488079913, + "heading": -0.5530315041603286, + "angularVelocity": -0.6031755282901653, + "velocityX": -2.0187202051754043, + "velocityY": 0.21650054661275753, + "timestamp": 12.906443844542986 + }, + { + "x": 3.129341493242039, + "y": 2.9358368291248627, + "heading": -0.5935016635404317, + "angularVelocity": -0.5828155206498206, + "velocityX": -2.0575163234289917, + "velocityY": 0.4782853107578603, + "timestamp": 12.975882900206742 + }, + { + "x": 2.9860054741751014, + "y": 2.987405262527824, + "heading": -0.6316842461722715, + "angularVelocity": -0.5498718590980219, + "velocityX": -2.0641988531787505, + "velocityY": 0.7426430689477005, + "timestamp": 13.045321955870497 + }, + { + "x": 2.846192204303753, + "y": 3.0569532479597985, + "heading": -0.6659032700236993, + "angularVelocity": -0.492792183366187, + "velocityX": -2.0134673280749626, + "velocityY": 1.0015687103918496, + "timestamp": 13.114761011534252 + }, + { + "x": 2.716907495900805, + "y": 3.1413547514524796, + "heading": -0.6931863548996687, + "angularVelocity": -0.39290691117809146, + "velocityX": -1.8618442772174881, + "velocityY": 1.2154759693360322, + "timestamp": 13.184200067198008 + }, + { + "x": 2.6049527292797388, + "y": 3.231193791663575, + "heading": -0.7132381462616506, + "angularVelocity": -0.288768203575217, + "velocityX": -1.6122737492742676, + "velocityY": 1.293782574551762, + "timestamp": 13.253639122861763 + }, + { + "x": 2.510838263874455, + "y": 3.3171226961552893, + "heading": -0.728589225015942, + "angularVelocity": -0.2210726889580136, + "velocityX": -1.3553534751540288, + "velocityY": 1.2374722505992761, + "timestamp": 13.323078178525519 + }, + { + "x": 2.4328922690741286, + "y": 3.3944440167178684, + "heading": -0.7405972040878053, + "angularVelocity": -0.17292831760284247, + "velocityX": -1.1225094301075331, + "velocityY": 1.1135134230078372, + "timestamp": 13.392517234189274 + }, + { + "x": 2.3697038520475897, + "y": 3.4608749704927635, + "heading": -0.7499497576912365, + "angularVelocity": -0.13468722340809483, + "velocityX": -0.9099838185086581, + "velocityY": 0.9566799712336846, + "timestamp": 13.46195628985303 + }, + { + "x": 2.3202711712076995, + "y": 3.5151563459749755, + "heading": -0.757044699270628, + "angularVelocity": -0.10217508737082179, + "velocityX": -0.711885845326848, + "velocityY": 0.781712466613299, + "timestamp": 13.531395345516785 + }, + { + "x": 2.2838770908763535, + "y": 3.556511565024103, + "heading": -0.7621393736392112, + "angularVelocity": -0.07336900422801312, + "velocityX": -0.5241154273119383, + "velocityY": 0.5955613689417452, + "timestamp": 13.60083440118054 + }, + { + "x": 2.2599910820649347, + "y": 3.584420486805288, + "heading": -0.7654136211145312, + "angularVelocity": -0.04715282263017491, + "velocityX": -0.34398521960151957, + "velocityY": 0.40191966198861, + "timestamp": 13.670273456844296 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -0.022845628735947136, + "velocityX": -0.16969575449988578, + "velocityY": 0.20294632487031472, + "timestamp": 13.739712512508051 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -4.7049839413389466e-29, + "velocityX": -4.475670243258915e-27, + "velocityY": -3.1749766992652438e-27, + "timestamp": 13.809151568171806 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/3PieceSourceSideLower.1.traj b/src/main/deploy/choreo/3PieceSourceSideLower.1.traj new file mode 100644 index 0000000..c3bf183 --- /dev/null +++ b/src/main/deploy/choreo/3PieceSourceSideLower.1.traj @@ -0,0 +1,536 @@ +{ + "samples": [ + { + "x": 0.6476043462753296, + "y": 4.391018867492676, + "heading": -1.0516500181249593, + "angularVelocity": 1.4837621687026506e-24, + "velocityX": 1.2605777337321822e-24, + "velocityY": -5.078976570788486e-24, + "timestamp": 0 + }, + { + "x": 0.6591914533996294, + "y": 4.3821264561987325, + "heading": -1.0407271677663157, + "angularVelocity": 0.1717852253768825, + "velocityX": 0.1822320862648084, + "velocityY": -0.1398522206307734, + "timestamp": 0.06358434105540607 + }, + { + "x": 0.682370568824486, + "y": 4.364325443623188, + "heading": -1.0189895299827134, + "angularVelocity": 0.341870929584073, + "velocityX": 0.36454125402760434, + "velocityY": -0.2799590635064334, + "timestamp": 0.12716868211081214 + }, + { + "x": 0.7171462629991276, + "y": 4.337595981630201, + "heading": -0.9865639713671387, + "angularVelocity": 0.509961384790003, + "velocityX": 0.5469223018972373, + "velocityY": -0.42037806084509965, + "timestamp": 0.19075302316621823 + }, + { + "x": 0.7635267836332045, + "y": 4.301915006000676, + "heading": -0.9436158522996869, + "angularVelocity": 0.6754511937149356, + "velocityX": 0.7294330626728007, + "velocityY": -0.5611597924469304, + "timestamp": 0.2543373642216243 + }, + { + "x": 0.821528668373285, + "y": 4.257255375739764, + "heading": -0.8903806097171314, + "angularVelocity": 0.8372382523578797, + "velocityX": 0.9122039133745026, + "velocityY": -0.7023683743454452, + "timestamp": 0.31792170527703034 + }, + { + "x": 0.891181254168102, + "y": 4.203582879820318, + "heading": -0.8272094806909008, + "angularVelocity": 0.9935013554860082, + "velocityX": 1.0954361504528793, + "velocityY": -0.8441149979470245, + "timestamp": 0.3815060463324364 + }, + { + "x": 0.9725299828215167, + "y": 4.140850923918169, + "heading": -0.7546270993304982, + "angularVelocity": 1.1415134631521273, + "velocityX": 1.279383057261364, + "velocityY": -0.986594417129931, + "timestamp": 0.44509038738784246 + }, + { + "x": 1.065637529662394, + "y": 4.068994254030079, + "heading": -0.673393688475822, + "angularVelocity": 1.2775694377943008, + "velocityX": 1.464315667905486, + "velocityY": -1.1301000953281266, + "timestamp": 0.5086747284432486 + }, + { + "x": 1.1705832794468332, + "y": 3.9879256971217893, + "heading": -0.584566561837581, + "angularVelocity": 1.3969968889170195, + "velocityX": 1.6504967739304255, + "velocityY": -1.2749767562684577, + "timestamp": 0.5722590694986547 + }, + { + "x": 1.2874653284560715, + "y": 3.897542804344524, + "heading": -0.4895895148245002, + "angularVelocity": 1.4937175637366416, + "velocityX": 1.8382206541605852, + "velocityY": -1.42146464486449, + "timestamp": 0.6358434105540608 + }, + { + "x": 1.4164104469481729, + "y": 3.7977514051305152, + "heading": -0.3905846134248042, + "angularVelocity": 1.557064204116311, + "velocityX": 2.0279382683189424, + "velocityY": -1.5694335674101274, + "timestamp": 0.6994277516094669 + }, + { + "x": 1.5575332696632753, + "y": 3.688506909682212, + "heading": -0.2916586370426824, + "angularVelocity": 1.5558229391088563, + "velocityX": 2.2194587593843385, + "velocityY": -1.718103760061147, + "timestamp": 0.763012092664873 + }, + { + "x": 1.7097613444585642, + "y": 3.5707774936908128, + "heading": -0.20742198100231668, + "angularVelocity": 1.324801903144104, + "velocityX": 2.394112642649561, + "velocityY": -1.8515473155381532, + "timestamp": 0.8265964337202791 + }, + { + "x": 1.8711613775944314, + "y": 3.4488084136916637, + "heading": -0.15383975328628857, + "angularVelocity": 0.8426953370380558, + "velocityX": 2.538361339550979, + "velocityY": -1.9182251160370993, + "timestamp": 0.8901807747756852 + }, + { + "x": 2.0417383028799354, + "y": 3.3225570909935143, + "heading": -0.13059320326371432, + "angularVelocity": 0.3656018075632435, + "velocityX": 2.682687631171131, + "velocityY": -1.9855725576858096, + "timestamp": 0.9537651158310914 + }, + { + "x": 2.2153893622715715, + "y": 3.187755682753254, + "heading": -0.13059307027328165, + "angularVelocity": 0.0000020915595014550256, + "velocityX": 2.731034976682693, + "velocityY": -2.120040972395975, + "timestamp": 1.0173494568864974 + }, + { + "x": 2.389039928413357, + "y": 3.0529536391086727, + "heading": -0.13059293728818866, + "angularVelocity": 0.000002091475523406473, + "velocityX": 2.7310272192719673, + "velocityY": -2.1200509654903494, + "timestamp": 1.0809337979419034 + }, + { + "x": 2.562690494523223, + "y": 2.918151595422971, + "heading": -0.13059280430309889, + "angularVelocity": 0.000002091475472778544, + "velocityX": 2.7310272187699582, + "velocityY": -2.1200509661370504, + "timestamp": 1.1445181389973094 + }, + { + "x": 2.7363410606330874, + "y": 2.7833495517372655, + "heading": -0.1305926713180123, + "angularVelocity": 0.0000020914754223957298, + "velocityX": 2.7310272187699196, + "velocityY": -2.1200509661371187, + "timestamp": 1.2081024800527154 + }, + { + "x": 2.9099916267429506, + "y": 2.648547508051558, + "heading": -0.13059253833292897, + "angularVelocity": 0.000002091475371640255, + "velocityX": 2.731027218769913, + "velocityY": -2.120050966137146, + "timestamp": 1.2716868211081214 + }, + { + "x": 3.0836421928528135, + "y": 2.513745464365849, + "heading": -0.1305924053478488, + "angularVelocity": 0.000002091475321590614, + "velocityX": 2.7310272187699067, + "velocityY": -2.120050966137172, + "timestamp": 1.3352711621635274 + }, + { + "x": 3.257292758962676, + "y": 2.3789434206801383, + "heading": -0.13059227236277188, + "angularVelocity": 0.000002091475271021181, + "velocityX": 2.7310272187699005, + "velocityY": -2.1200509661371987, + "timestamp": 1.3988555032189334 + }, + { + "x": 3.430943325072538, + "y": 2.244141376994426, + "heading": -0.1305921393776981, + "angularVelocity": 0.0000020914752212008574, + "velocityX": 2.7310272187698943, + "velocityY": -2.1200509661372253, + "timestamp": 1.4624398442743394 + }, + { + "x": 3.6045938911823994, + "y": 2.1093393333087116, + "heading": -0.13059200639262752, + "angularVelocity": 0.0000020914751708183746, + "velocityX": 2.731027218769888, + "velocityY": -2.120050966137252, + "timestamp": 1.5260241853297454 + }, + { + "x": 3.7782444572922627, + "y": 1.9745372896229973, + "heading": -0.13059187340756015, + "angularVelocity": 0.000002091475120575094, + "velocityX": 2.7310272187699067, + "velocityY": -2.1200509661372466, + "timestamp": 1.5896085263851514 + }, + { + "x": 3.951895023426423, + "y": 1.8397352459685825, + "heading": -0.13059174042249597, + "angularVelocity": 0.0000020914750700807474, + "velocityX": 2.731027219152042, + "velocityY": -2.1200509656450026, + "timestamp": 1.6531928674405574 + }, + { + "x": 4.125545965030257, + "y": 1.7049336859916362, + "heading": -0.13059160743738255, + "angularVelocity": 0.0000020914758451630756, + "velocityX": 2.731033124217136, + "velocityY": -2.1200433587804755, + "timestamp": 1.7167772084959634 + }, + { + "x": 4.303592186821449, + "y": 1.5759931847530775, + "heading": -0.13059147324861473, + "angularVelocity": 0.0000021104058890338206, + "velocityX": 2.800158322566331, + "velocityY": -2.0278656521139804, + "timestamp": 1.7803615495513694 + }, + { + "x": 4.4902367308457585, + "y": 1.459847055147331, + "heading": -0.13059133313690313, + "angularVelocity": 0.0000022035568702526466, + "velocityX": 2.9353853625953525, + "velocityY": -1.8266467447470887, + "timestamp": 1.8439458906067754 + }, + { + "x": 4.6845622062683105, + "y": 1.3570671081542969, + "heading": -0.13059118201554434, + "angularVelocity": 0.0000023767071623724323, + "velocityX": 3.0561844661285633, + "velocityY": -1.6164348845492313, + "timestamp": 1.9075302316621814 + }, + { + "x": 4.832067084470335, + "y": 1.2885914794569837, + "heading": -0.1305910326229948, + "angularVelocity": 0.0000031760307778025535, + "velocityX": 3.1358995782886923, + "velocityY": -1.4557667364794429, + "timestamp": 1.9545677322324386 + }, + { + "x": 4.982924581891149, + "y": 1.2278574954319752, + "heading": -0.13059088659891144, + "angularVelocity": 0.000003104418423448367, + "velocityX": 3.207175032514466, + "velocityY": -1.2911822118246548, + "timestamp": 2.001605232802696 + }, + { + "x": 5.136728649173284, + "y": 1.1750284268224231, + "heading": -0.13059074139155538, + "angularVelocity": 0.00000308705510077957, + "velocityX": 3.269818026415032, + "velocityY": -1.1231266110886224, + "timestamp": 2.048642733372953 + }, + { + "x": 5.293065104212638, + "y": 1.1302454562937592, + "heading": -0.1305905947227455, + "angularVelocity": 0.0000031181250722314943, + "velocityX": 3.3236556607816112, + "velocityY": -0.9520695186976181, + "timestamp": 2.0956802339432103 + }, + { + "x": 5.450482773711564, + "y": 1.0894253766985387, + "heading": -0.13059044674627163, + "angularVelocity": 0.0000031459255294965824, + "velocityX": 3.3466418834011025, + "velocityY": -0.8678199117797456, + "timestamp": 2.1427177345134676 + }, + { + "x": 5.607900655440282, + "y": 1.04860611555064, + "heading": -0.1305902987694863, + "angularVelocity": 0.0000031459321514570608, + "velocityX": 3.346646395328584, + "velocityY": -0.8678025118900428, + "timestamp": 2.189755235083725 + }, + { + "x": 5.765318537202331, + "y": 1.0077868545312796, + "heading": -0.13059015079270853, + "angularVelocity": 0.0000031459319896246793, + "velocityX": 3.346646396037204, + "velocityY": -0.867802509157372, + "timestamp": 2.236792735653982 + }, + { + "x": 5.922736418964388, + "y": 0.9669675935119395, + "heading": -0.1305900028159384, + "angularVelocity": 0.0000031459318275019883, + "velocityX": 3.3466463960373436, + "velocityY": -0.867802509156932, + "timestamp": 2.2838302362242393 + }, + { + "x": 6.080154300726486, + "y": 0.9261483324927575, + "heading": -0.13058985483917593, + "angularVelocity": 0.0000031459316651066595, + "velocityX": 3.3466463960382407, + "velocityY": -0.867802509153571, + "timestamp": 2.3308677367944965 + }, + { + "x": 6.237572182748789, + "y": 0.8853290724770384, + "heading": -0.1305897068624206, + "angularVelocity": 0.0000031459315124739283, + "velocityX": 3.346646401570085, + "velocityY": -0.86780248782032, + "timestamp": 2.377905237364754 + }, + { + "x": 6.394991721451892, + "y": 0.844516201922275, + "heading": -0.1305895588824519, + "angularVelocity": 0.0000031459998283836956, + "velocityX": 3.3466816219959377, + "velocityY": -0.8676666502252512, + "timestamp": 2.424942737935011 + }, + { + "x": 6.554152816050896, + "y": 0.8111344074726426, + "heading": -0.1305893710725833, + "angularVelocity": 0.000003992768882659048, + "velocityX": 3.3837064612154375, + "velocityY": -0.7096846993341356, + "timestamp": 2.4719802385052683 + }, + { + "x": 6.711882769951912, + "y": 0.7851880682804085, + "heading": -0.12042377120760904, + "angularVelocity": 0.21611692249230904, + "velocityX": 3.3532809351853774, + "velocityY": -0.5516096492729113, + "timestamp": 2.5190177390755255 + }, + { + "x": 6.866725553620017, + "y": 0.7651062041845728, + "heading": -0.09663025977821411, + "angularVelocity": 0.5058413210934933, + "velocityX": 3.29190075558594, + "velocityY": -0.4269330609061741, + "timestamp": 2.5660552396457827 + }, + { + "x": 7.014982029149098, + "y": 0.7495960675806145, + "heading": -0.06694522514508651, + "angularVelocity": 0.6310929422958753, + "velocityX": 3.151878261635918, + "velocityY": -0.3297398121907379, + "timestamp": 2.61309274021604 + }, + { + "x": 7.155468637798704, + "y": 0.7371442919133998, + "heading": -0.03984157562428539, + "angularVelocity": 0.5762136421410802, + "velocityX": 2.9866937432138485, + "velocityY": -0.26472018105247963, + "timestamp": 2.6601302407862972 + }, + { + "x": 7.28821756510369, + "y": 0.727344980547685, + "heading": -0.017222967030039466, + "angularVelocity": 0.48086331799160453, + "velocityX": 2.8221934774511723, + "velocityY": -0.20832976342095313, + "timestamp": 2.7071677413565545 + }, + { + "x": 7.413279056549072, + "y": 0.72, + "heading": -8.89059623238174e-24, + "angularVelocity": 0.3661539584637254, + "velocityX": 2.6587614122605316, + "velocityY": -0.15615159093571299, + "timestamp": 2.7542052419268117 + }, + { + "x": 7.584141987680618, + "y": 0.7142363916829146, + "heading": 0.012496138017563933, + "angularVelocity": 0.17655403361214328, + "velocityX": 2.4140690222585377, + "velocityY": -0.0814322229085219, + "timestamp": 2.824983222819707 + }, + { + "x": 7.7365481472096285, + "y": 0.7118516221881321, + "heading": 0.017397146839313638, + "angularVelocity": 0.0692448238833787, + "velocityX": 2.153299057225703, + "velocityY": -0.03369366383015727, + "timestamp": 2.8957612037126026 + }, + { + "x": 7.8700913671839885, + "y": 0.7115747244873029, + "heading": 0.01782819430589125, + "angularVelocity": 0.006090135111792666, + "velocityX": 1.8867904719752229, + "velocityY": -0.00391220118652886, + "timestamp": 2.966539184605498 + }, + { + "x": 7.98461628322269, + "y": 0.7125587164394606, + "heading": 0.01566183427732483, + "angularVelocity": -0.03060782465445932, + "velocityX": 1.6180867918795971, + "velocityY": 0.013902515157174911, + "timestamp": 3.0373171654983935 + }, + { + "x": 8.08006227999561, + "y": 0.7142078102454845, + "heading": 0.012151433911488669, + "angularVelocity": -0.04959735106244768, + "velocityX": 1.3485266967046623, + "velocityY": 0.023299531651227553, + "timestamp": 3.108095146391289 + }, + { + "x": 8.15640902937787, + "y": 0.716081694603084, + "heading": 0.008204279269867538, + "angularVelocity": -0.055768115900256314, + "velocityX": 1.0786793917982904, + "velocityY": 0.02647552719023059, + "timestamp": 3.1788731272841844 + }, + { + "x": 8.213654764882792, + "y": 0.7178420004122783, + "heading": 0.004513492953959301, + "angularVelocity": -0.05214596784688299, + "velocityX": 0.8088071287530345, + "velocityY": 0.02487081133125343, + "timestamp": 3.24965110817708 + }, + { + "x": 8.251806612817116, + "y": 0.7192206370589289, + "heading": 0.0016291300858943402, + "angularVelocity": -0.04075226266244733, + "velocityX": 0.5390355510714093, + "velocityY": 0.019478326864633543, + "timestamp": 3.3204290890699752 + }, + { + "x": 8.270875930786133, + "y": 0.72, + "heading": 5.577770790961069e-24, + "angularVelocity": -0.02301747048082103, + "velocityX": 0.2694244414498442, + "velocityY": 0.011011375730688399, + "timestamp": 3.3912070699628707 + }, + { + "x": 8.270875930786133, + "y": 0.72, + "heading": 2.68750083227073e-24, + "angularVelocity": -1.0022200251051524e-24, + "velocityX": 6.790628726650556e-26, + "velocityY": 3.41370039560792e-26, + "timestamp": 3.461985050855766 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3PieceSourceSideLower.2.traj b/src/main/deploy/choreo/3PieceSourceSideLower.2.traj new file mode 100644 index 0000000..367001e --- /dev/null +++ b/src/main/deploy/choreo/3PieceSourceSideLower.2.traj @@ -0,0 +1,419 @@ +{ + "samples": [ + { + "x": 8.270875930786133, + "y": 0.72, + "heading": 2.68750083227073e-24, + "angularVelocity": -1.0022200251051524e-24, + "velocityX": 6.790628726650556e-26, + "velocityY": 3.41370039560792e-26, + "timestamp": 0 + }, + { + "x": 8.251549516655441, + "y": 0.7219586831033024, + "heading": -0.0006660755390764362, + "angularVelocity": -0.009331396835267377, + "velocityX": -0.2707537344882418, + "velocityY": 0.027440204960520992, + "timestamp": 0.0713800463998111 + }, + { + "x": 8.212896688696702, + "y": 0.7258760442658929, + "heading": -0.001998229790486319, + "angularVelocity": -0.018662838126333965, + "velocityX": -0.5415074647365562, + "velocityY": 0.054880339256839474, + "timestamp": 0.1427600927996222 + }, + { + "x": 8.154917446658368, + "y": 0.7317520758923975, + "heading": -0.003996392789209819, + "angularVelocity": -0.027993299241239875, + "velocityX": -0.812261198508941, + "velocityY": 0.08232036714563006, + "timestamp": 0.2141401391994333 + }, + { + "x": 8.077611789765818, + "y": 0.7395867678254814, + "heading": -0.006660425114458462, + "angularVelocity": -0.03732180713818774, + "velocityX": -1.0830149431334009, + "velocityY": 0.10976025273506382, + "timestamp": 0.2855201855992444 + }, + { + "x": 7.980979716767329, + "y": 0.749380107333022, + "heading": -0.00999012297038977, + "angularVelocity": -0.04664745995374023, + "velocityX": -1.353768705293865, + "velocityY": 0.13719995995360584, + "timestamp": 0.3569002319990555 + }, + { + "x": 7.86502122600115, + "y": 0.7611320790926304, + "heading": -0.013985224659649345, + "angularVelocity": -0.0559694465156602, + "velocityX": -1.6245224907346907, + "velocityY": 0.1646394525128738, + "timestamp": 0.4282802783988666 + }, + { + "x": 7.729736315496098, + "y": 0.7748426651724982, + "heading": -0.018645418435560536, + "angularVelocity": -0.06528706565709827, + "velocityX": -1.8952763037908287, + "velocityY": 0.1920786938561615, + "timestamp": 0.4996603247986777 + }, + { + "x": 7.575124983133034, + "y": 0.790511845006039, + "heading": -0.023970351697693893, + "angularVelocity": -0.07459974503669481, + "velocityX": -2.1660301465350793, + "velocityY": 0.21951764707149618, + "timestamp": 0.5710403711984888 + }, + { + "x": 7.401187226935479, + "y": 0.8081395953535565, + "heading": -0.029959641438283554, + "angularVelocity": -0.08390705866234215, + "velocityX": -2.4367840169688693, + "velocityY": 0.24695627470990408, + "timestamp": 0.6424204175983004 + }, + { + "x": 7.207923045694123, + "y": 0.8277258902299445, + "heading": -0.03661288565920585, + "angularVelocity": -0.09320874048829283, + "velocityX": -2.7075379043444587, + "velocityY": 0.2743945383095141, + "timestamp": 0.7138004639981119 + }, + { + "x": 6.995332440744312, + "y": 0.8492707007132015, + "heading": -0.04392967465186433, + "angularVelocity": -0.10250468249454428, + "velocityX": -2.9782917730125558, + "velocityY": 0.3018323967258469, + "timestamp": 0.7851805103979235 + }, + { + "x": 6.763415424631655, + "y": 0.872773994034365, + "heading": -0.05190959463777978, + "angularVelocity": -0.11179482766400335, + "velocityX": -3.249045465922667, + "velocityY": 0.32926979606482365, + "timestamp": 0.856560556797735 + }, + { + "x": 6.517888285888531, + "y": 0.8976525425650798, + "heading": -0.051909598739256456, + "angularVelocity": -5.745970870698805e-8, + "velocityX": -3.439716715339282, + "velocityY": 0.348536457812958, + "timestamp": 0.9279406031975466 + }, + { + "x": 6.272361147029228, + "y": 0.9225310899511409, + "heading": -0.051909602840455864, + "angularVelocity": -5.7455824331798554e-8, + "velocityX": -3.439716716966896, + "velocityY": 0.3485364417769111, + "timestamp": 0.9993206495973581 + }, + { + "x": 6.026834008173439, + "y": 0.94740963737188, + "heading": -0.051909606941655285, + "angularVelocity": -5.7455824691381894e-8, + "velocityX": -3.4397167169176694, + "velocityY": 0.34853644226273583, + "timestamp": 1.0707006959971697 + }, + { + "x": 5.7813069462957465, + "y": 0.9722889444800497, + "heading": -0.05190961104285578, + "angularVelocity": -5.745583959865336e-8, + "velocityX": -3.4397156384916827, + "velocityY": 0.348547085116991, + "timestamp": 1.1420807423969812 + }, + { + "x": 5.537158225945976, + "y": 1.0082600487060158, + "heading": -0.05190961517853181, + "angularVelocity": -5.793882502411163e-8, + "velocityX": -3.42040573891272, + "velocityY": 0.5039378095173286, + "timestamp": 1.2134607887967928 + }, + { + "x": 5.296595540867912, + "y": 1.0633246680240616, + "heading": -0.051909619464878404, + "angularVelocity": -6.004964723504615e-8, + "velocityX": -3.3701671154797745, + "velocityY": 0.7714287408783704, + "timestamp": 1.2848408351966043 + }, + { + "x": 5.061109922350585, + "y": 1.137142081975634, + "heading": -0.05190979783691868, + "angularVelocity": -0.0000024989061968370985, + "velocityX": -3.299039863302041, + "velocityY": 1.0341463430565587, + "timestamp": 1.3562208815964159 + }, + { + "x": 4.843667300844079, + "y": 1.2172452033533754, + "heading": -0.059561850749452576, + "angularVelocity": -0.10720156820399783, + "velocityX": -3.0462661832492475, + "velocityY": 1.1222060704341816, + "timestamp": 1.4276009279962274 + }, + { + "x": 4.645580768585205, + "y": 1.29859459400177, + "heading": -0.06951316932259947, + "angularVelocity": -0.13941317041751372, + "velocityX": -2.7750967146947323, + "velocityY": 1.13966570143067, + "timestamp": 1.498980974396039 + }, + { + "x": 4.474092871614693, + "y": 1.385887514867849, + "heading": -0.08242444337490663, + "angularVelocity": -0.2012909785735536, + "velocityX": -2.6735523121009726, + "velocityY": 1.3609251412750347, + "timestamp": 1.5631233127353061 + }, + { + "x": 4.3102265492062894, + "y": 1.4867635843508433, + "heading": -0.10022801616986421, + "angularVelocity": -0.27756351352189995, + "velocityX": -2.554729475898846, + "velocityY": 1.5726908637073918, + "timestamp": 1.6272656510745733 + }, + { + "x": 4.155003391680842, + "y": 1.6004909974861876, + "heading": -0.12448541502532746, + "angularVelocity": -0.37818076926286603, + "velocityX": -2.419979713000573, + "velocityY": 1.7730475077757013, + "timestamp": 1.6914079894138405 + }, + { + "x": 4.00926290540325, + "y": 1.7261407641347544, + "heading": -0.1583340022250027, + "angularVelocity": -0.5277105274934034, + "velocityX": -2.2721417717378642, + "velocityY": 1.958920892218929, + "timestamp": 1.7555503277531077 + }, + { + "x": 3.8730404280351673, + "y": 1.8620509915016963, + "heading": -0.21028223642363456, + "angularVelocity": -0.8098899345368795, + "velocityX": -2.1237529047906247, + "velocityY": 2.1188848253095043, + "timestamp": 1.819692666092375 + }, + { + "x": 3.741564544573194, + "y": 2.0025580918468195, + "heading": -0.2996033949603487, + "angularVelocity": -1.3925460288689253, + "velocityX": -2.049751955822999, + "velocityY": 2.1905515761202894, + "timestamp": 1.883835004431642 + }, + { + "x": 3.6100546633718698, + "y": 2.1430333719703096, + "heading": -0.3901784541639511, + "angularVelocity": -1.4120947497193557, + "velocityX": -2.050281991681201, + "velocityY": 2.1900554884743153, + "timestamp": 1.9479773427709093 + }, + { + "x": 3.4785072023626884, + "y": 2.2834734612659138, + "heading": -0.4827580204171881, + "angularVelocity": -1.4433456691827582, + "velocityX": -2.0508678731571766, + "velocityY": 2.1895068519762364, + "timestamp": 2.0121196811101765 + }, + { + "x": 3.346919123497923, + "y": 2.423875493587476, + "heading": -0.5782456610362502, + "angularVelocity": -1.488683498159982, + "velocityX": -2.051501118789228, + "velocityY": 2.188913531323639, + "timestamp": 2.0762620194494437 + }, + { + "x": 3.2152861692168586, + "y": 2.5642354541414827, + "heading": -0.6777971832870476, + "angularVelocity": -1.552040739834588, + "velocityX": -2.052200741183146, + "velocityY": 2.1882576187291725, + "timestamp": 2.140404357788711 + }, + { + "x": 3.0838454699327147, + "y": 2.704775469009889, + "heading": -0.7323199293584344, + "angularVelocity": -0.8500274153243377, + "velocityX": -2.049203423001438, + "velocityY": 2.1910647242863015, + "timestamp": 2.204546696127978 + }, + { + "x": 2.9552905266702094, + "y": 2.842272453022611, + "heading": -0.7381340958895165, + "angularVelocity": -0.0906447548003214, + "velocityX": -2.0042135442980085, + "velocityY": 2.143622879562956, + "timestamp": 2.2686890344672452 + }, + { + "x": 2.837445531451434, + "y": 2.968312162646033, + "heading": -0.7431823028036986, + "angularVelocity": -0.07870319425338904, + "velocityX": -1.837241957027799, + "velocityY": 1.965000230530428, + "timestamp": 2.3328313728065124 + }, + { + "x": 2.7303126070394907, + "y": 3.082893932918914, + "heading": -0.747653667364265, + "angularVelocity": -0.06971003359615609, + "velocityX": -1.6702372751876633, + "velocityY": 1.7863672145350264, + "timestamp": 2.3969737111457796 + }, + { + "x": 2.6338923756777755, + "y": 3.1860176221630003, + "heading": -0.751611677644213, + "angularVelocity": -0.061706672728596555, + "velocityX": -1.5032228923697837, + "velocityY": 1.6077319897293985, + "timestamp": 2.461116049485047 + }, + { + "x": 2.5481851332942993, + "y": 3.277683175661144, + "heading": -0.7550881872685589, + "angularVelocity": -0.05419992027664412, + "velocityX": -1.3362038959375901, + "velocityY": 1.4290959118655384, + "timestamp": 2.525258387824314 + }, + { + "x": 2.473191052958317, + "y": 3.3578905661578986, + "heading": -0.7581023488644949, + "angularVelocity": -0.04699176353679586, + "velocityX": -1.1691822012991828, + "velocityY": 1.2504594090803822, + "timestamp": 2.589400726163581 + }, + { + "x": 2.4089102482733002, + "y": 3.4266397779362934, + "heading": -0.7606669459018931, + "angularVelocity": -0.039982905266614695, + "velocityX": -1.0021587355455739, + "velocityY": 1.0718226612625237, + "timestamp": 2.6535430645028484 + }, + { + "x": 2.3553427994564915, + "y": 3.4839308009572485, + "heading": -0.7627911121209714, + "angularVelocity": -0.03311644498900487, + "velocityX": -0.8351340191789587, + "velocityY": 0.893185756932122, + "timestamp": 2.7176854028421156 + }, + { + "x": 2.312488766021362, + "y": 3.5297636282468714, + "heading": -0.7644816934670975, + "angularVelocity": -0.026356715235171243, + "velocityX": -0.6681083749778789, + "velocityY": 0.714548743876462, + "timestamp": 2.7818277411813828 + }, + { + "x": 2.280348193682581, + "y": 3.5641382545710476, + "heading": -0.7657440057178361, + "angularVelocity": -0.019679860189406772, + "velocityX": -0.501082018070189, + "velocityY": 0.5359116492192578, + "timestamp": 2.84597007952065 + }, + { + "x": 2.2589211184357674, + "y": 3.587054675697436, + "heading": -0.7665822895031111, + "angularVelocity": -0.013069117948915103, + "velocityX": -0.3340550999790392, + "velocityY": 0.35727448857846106, + "timestamp": 2.910112417859917 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -0.006512243047323657, + "velocityX": -0.1670277322411552, + "velocityY": 0.17863727070052132, + "timestamp": 2.9742547561991843 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": 1.4318885213105956e-25, + "velocityX": -3.1664598929952935e-25, + "velocityY": -8.517319099705152e-26, + "timestamp": 3.0383970945384515 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3PieceSourceSideLower.3.traj b/src/main/deploy/choreo/3PieceSourceSideLower.3.traj new file mode 100644 index 0000000..6bd0e7b --- /dev/null +++ b/src/main/deploy/choreo/3PieceSourceSideLower.3.traj @@ -0,0 +1,509 @@ +{ + "samples": [ + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": 1.4318885213105956e-25, + "velocityX": -3.1664598929952935e-25, + "velocityY": -8.517319099705152e-26, + "timestamp": 0 + }, + { + "x": 2.254662845986897, + "y": 3.5873395415687703, + "heading": -0.7560028105976356, + "angularVelocity": 0.18270874766778025, + "velocityX": 0.10724881682251722, + "velocityY": -0.18563544290641987, + "timestamp": 0.060189725684949735 + }, + { + "x": 2.267640389134434, + "y": 3.5649741905137224, + "heading": -0.7343324319976283, + "angularVelocity": 0.3600345134223772, + "velocityX": 0.2156106046315132, + "velocityY": -0.3715808769774839, + "timestamp": 0.12037945136989947 + }, + { + "x": 2.2872156815111926, + "y": 3.5313939684851823, + "heading": -0.7023698583511744, + "angularVelocity": 0.5310303923589071, + "velocityX": 0.3252264760138822, + "velocityY": -0.5579062148298977, + "timestamp": 0.1805691770548492 + }, + { + "x": 2.313473905100035, + "y": 3.486574818355693, + "heading": -0.6605397935243593, + "angularVelocity": 0.6949701855390652, + "velocityX": 0.4362575720362232, + "velocityY": -0.7446312409544028, + "timestamp": 0.24075890273979894 + }, + { + "x": 2.3465132166718874, + "y": 3.4304964860969096, + "heading": -0.6092924367494605, + "angularVelocity": 0.851430309603703, + "velocityX": 0.54891945752984, + "velocityY": -0.9316927701633545, + "timestamp": 0.3009486284247487 + }, + { + "x": 2.3864518087256505, + "y": 3.3631500739334586, + "heading": -0.5490846408400017, + "angularVelocity": 1.0003002210809777, + "velocityX": 0.6635450087081837, + "velocityY": -1.118902128179834, + "timestamp": 0.3611383541096984 + }, + { + "x": 2.4334407524616903, + "y": 3.284548274129898, + "heading": -0.48037214659109057, + "angularVelocity": 1.1415983951907667, + "velocityX": 0.7806804766313955, + "velocityY": -1.3059006152476056, + "timestamp": 0.42132807979464815 + }, + { + "x": 2.4876845583004745, + "y": 3.194738200368583, + "heading": -0.4036330691658326, + "angularVelocity": 1.2749531012474145, + "velocityX": 0.9012137075139995, + "velocityY": -1.4921163494149543, + "timestamp": 0.4815178054795979 + }, + { + "x": 2.549471334765238, + "y": 3.093817881877023, + "heading": -0.3194506455237171, + "angularVelocity": 1.3986178319328149, + "velocityX": 1.0265336112042236, + "velocityY": -1.6767034131340948, + "timestamp": 0.5417075311645476 + }, + { + "x": 2.619215453459605, + "y": 2.98196209624742, + "heading": -0.22869362474666396, + "angularVelocity": 1.5078490513829745, + "velocityX": 1.1587379390866084, + "velocityY": -1.858386699003879, + "timestamp": 0.6018972568494974 + }, + { + "x": 2.6975187688111633, + "y": 2.859476881700614, + "heading": -0.13286180386406954, + "angularVelocity": 1.592162446198965, + "velocityX": 1.3009415553980122, + "velocityY": -2.0349854257174798, + "timestamp": 0.6620869825344471 + }, + { + "x": 2.7852562883718943, + "y": 2.726945932671376, + "heading": -0.034753273861128454, + "angularVelocity": 1.6299879902505128, + "velocityX": 1.4576826619874292, + "velocityY": -2.201886576505485, + "timestamp": 0.7222767082193968 + }, + { + "x": 2.883617018333608, + "y": 2.5856822162797477, + "heading": 0.06025595074389153, + "angularVelocity": 1.5784957237108086, + "velocityX": 1.6341780734566165, + "velocityY": -2.3469739192871995, + "timestamp": 0.7824664339043466 + }, + { + "x": 2.993618091797967, + "y": 2.439544726598588, + "heading": 0.14260766445208392, + "angularVelocity": 1.3682021768838868, + "velocityX": 1.8275722677344666, + "velocityY": -2.427947428205354, + "timestamp": 0.8426561595892963 + }, + { + "x": 3.1145276346410644, + "y": 2.293581984129882, + "heading": 0.20384383077417081, + "angularVelocity": 1.017385702048459, + "velocityX": 2.008807009288801, + "velocityY": -2.425044155089132, + "timestamp": 0.902845885274246 + }, + { + "x": 3.2468175816396365, + "y": 2.1505705427004553, + "heading": 0.24757858696307516, + "angularVelocity": 0.7266149777426218, + "velocityX": 2.197882537146209, + "velocityY": -2.3760108523834984, + "timestamp": 0.9630356109591958 + }, + { + "x": 3.390280853000983, + "y": 2.013955912336661, + "heading": 0.2801226137384155, + "angularVelocity": 0.5406907309344624, + "velocityX": 2.3835176141568395, + "velocityY": -2.269733394016016, + "timestamp": 1.0232253366441455 + }, + { + "x": 3.544035674658465, + "y": 1.8860535950620219, + "heading": 0.3061151888137327, + "angularVelocity": 0.43184405277687576, + "velocityX": 2.55450278112711, + "velocityY": -2.1249858812136204, + "timestamp": 1.0834150623290952 + }, + { + "x": 3.7070451600522367, + "y": 1.7682529006317282, + "heading": 0.3281829520976596, + "angularVelocity": 0.36663671470170595, + "velocityX": 2.7082609787426026, + "velocityY": -1.9571561938476998, + "timestamp": 1.143604788014045 + }, + { + "x": 3.8782577243031593, + "y": 1.6615213018872745, + "heading": 0.34797198576359334, + "angularVelocity": 0.328777601837151, + "velocityX": 2.844548007198066, + "velocityY": -1.7732527857514622, + "timestamp": 1.2037945136989947 + }, + { + "x": 4.056668173911873, + "y": 1.5665668328609006, + "heading": 0.3664586508169338, + "angularVelocity": 0.30713987882425087, + "velocityX": 2.964134618964106, + "velocityY": -1.5775860073427241, + "timestamp": 1.2639842393839444 + }, + { + "x": 4.241342350555325, + "y": 1.4839195959237403, + "heading": 0.38411092010286374, + "angularVelocity": 0.2932771180637541, + "velocityX": 3.0682010017804235, + "velocityY": -1.3731120385854438, + "timestamp": 1.3241739650688942 + }, + { + "x": 4.431417064671585, + "y": 1.4139831463989663, + "heading": 0.40101516097721396, + "angularVelocity": 0.2808492758852529, + "velocityX": 3.1579262399560672, + "velocityY": -1.1619333487386434, + "timestamp": 1.384363690753844 + }, + { + "x": 4.626090049743652, + "y": 1.3570671081542969, + "heading": 0.41697492722531376, + "angularVelocity": 0.26515765052058465, + "velocityX": 3.2343225169531657, + "velocityY": -0.9456105273279376, + "timestamp": 1.4445534164387936 + }, + { + "x": 4.771095590246596, + "y": 1.3224217071642599, + "heading": 0.4280403505516019, + "angularVelocity": 0.2504578977864778, + "velocityX": 3.2820961088293013, + "velocityY": -0.7841737314576779, + "timestamp": 1.488734188632776 + }, + { + "x": 4.917956708237317, + "y": 1.2949736641198544, + "heading": 0.43814957517205777, + "angularVelocity": 0.22881502786030425, + "velocityX": 3.3240957705742336, + "velocityY": -0.6212667113171019, + "timestamp": 1.5329149608267585 + }, + { + "x": 5.066406183363406, + "y": 1.2747799857702726, + "heading": 0.4470279058701966, + "angularVelocity": 0.20095462929342126, + "velocityX": 3.3600470918502436, + "velocityY": -0.45706938441270734, + "timestamp": 1.577095733020741 + }, + { + "x": 5.21616079891639, + "y": 1.2618903010738838, + "heading": 0.45444388098508137, + "angularVelocity": 0.16785526251835975, + "velocityX": 3.389588006643782, + "velocityY": -0.2917487417330471, + "timestamp": 1.6212765052147233 + }, + { + "x": 5.366915459547734, + "y": 1.25634723736205, + "heading": 0.46022561365529296, + "angularVelocity": 0.13086536027088905, + "velocityX": 3.4122233076740254, + "velocityY": -0.12546326006925063, + "timestamp": 1.6654572774087057 + }, + { + "x": 5.518334246127743, + "y": 1.2581861921582334, + "heading": 0.4642859914379861, + "angularVelocity": 0.09190373053837578, + "velocityX": 3.427255320825579, + "velocityY": 0.041623419077170896, + "timestamp": 1.7096380496026882 + }, + { + "x": 5.670036777042452, + "y": 1.2674335281135962, + "heading": 0.46666317405037006, + "angularVelocity": 0.05380581855714505, + "velocityX": 3.4336776697481812, + "velocityY": 0.20930679787037068, + "timestamp": 1.7538188217966706 + }, + { + "x": 5.821577538212164, + "y": 1.284100828351401, + "heading": 0.4675873844825302, + "angularVelocity": 0.020918838360320994, + "velocityX": 3.4300161279288752, + "velocityY": 0.3772523523270378, + "timestamp": 1.797999593990653 + }, + { + "x": 5.972415606183977, + "y": 1.3081694677842932, + "heading": 0.467591351779606, + "angularVelocity": 0.00008979691569104718, + "velocityX": 3.414111172831806, + "velocityY": 0.544776341328194, + "timestamp": 1.8421803661846354 + }, + { + "x": 6.121903298527153, + "y": 1.3395574928087577, + "heading": 0.46759157023044867, + "angularVelocity": 0.000004944477695120892, + "velocityX": 3.3835463917839124, + "velocityY": 0.7104453694618647, + "timestamp": 1.8863611383786179 + }, + { + "x": 6.269684547080798, + "y": 1.3781901650520143, + "heading": 0.46759176450370754, + "angularVelocity": 0.0000043972354751028654, + "velocityX": 3.3449222640289906, + "velocityY": 0.8744227482859289, + "timestamp": 1.9305419105726003 + }, + { + "x": 6.415408472766336, + "y": 1.423975753200933, + "heading": 0.4675919413300162, + "angularVelocity": 0.000004002336308335113, + "velocityX": 3.2983562407129146, + "velocityY": 1.036323854818335, + "timestamp": 1.9747226827665827 + }, + { + "x": 6.5587290836787195, + "y": 1.476805540848842, + "heading": 0.4675921046539949, + "angularVelocity": 0.0000036967207824585226, + "velocityX": 3.243958939493236, + "velocityY": 1.1957642436839244, + "timestamp": 2.018903454960565 + }, + { + "x": 6.6993060573570675, + "y": 1.5365540678009835, + "heading": 0.46759241978399874, + "angularVelocity": 0.000007132740968196116, + "velocityX": 3.1818586841606873, + "velocityY": 1.352364931282918, + "timestamp": 2.0630842271545475 + }, + { + "x": 6.836206251960683, + "y": 1.6027005176579716, + "heading": 0.47022361687585157, + "angularVelocity": 0.059555253590862826, + "velocityX": 3.0986374344598233, + "velocityY": 1.4971773142977605, + "timestamp": 2.10726499934853 + }, + { + "x": 6.968107168767895, + "y": 1.6730618727776314, + "heading": 0.48242555808722076, + "angularVelocity": 0.27618216263388745, + "velocityX": 2.9854823774487342, + "velocityY": 1.592578663196003, + "timestamp": 2.1514457715425124 + }, + { + "x": 7.094292994493853, + "y": 1.7449092475770878, + "heading": 0.5050066138251464, + "angularVelocity": 0.5111059543907503, + "velocityX": 2.8561254016095394, + "velocityY": 1.6262136497750488, + "timestamp": 2.195626543736495 + }, + { + "x": 7.213566146886095, + "y": 1.8140035101921348, + "heading": 0.5272178489693323, + "angularVelocity": 0.5027353312582225, + "velocityX": 2.699662012890034, + "velocityY": 1.5638989357559931, + "timestamp": 2.239807315930477 + }, + { + "x": 7.326317415042033, + "y": 1.87970672983894, + "heading": 0.5469886414915726, + "angularVelocity": 0.44749766788668743, + "velocityX": 2.552043854301269, + "velocityY": 1.4871451172995578, + "timestamp": 2.2839880881244596 + }, + { + "x": 7.432769298553467, + "y": 1.941792130470276, + "heading": 0.5633166974965059, + "angularVelocity": 0.3695738031296167, + "velocityX": 2.409461813932093, + "velocityY": 1.405258386130975, + "timestamp": 2.328168860318442 + }, + { + "x": 7.579701459580911, + "y": 2.027179132105106, + "heading": 0.5787605568170214, + "angularVelocity": 0.23108320735340232, + "velocityX": 2.198514913204677, + "velocityY": 1.2776276832472448, + "timestamp": 2.395001320787527 + }, + { + "x": 7.712165331896003, + "y": 2.1039708729412734, + "heading": 0.5887351557050889, + "angularVelocity": 0.14924781787259772, + "velocityX": 1.9820289629523165, + "velocityY": 1.1490186100763133, + "timestamp": 2.461833781256612 + }, + { + "x": 7.830026910442582, + "y": 2.172172632129026, + "heading": 0.5950957258261598, + "angularVelocity": 0.09517186822731284, + "velocityX": 1.7635379233284407, + "velocityY": 1.0204885277162767, + "timestamp": 2.5286662417256967 + }, + { + "x": 7.933222174835435, + "y": 2.231800820984951, + "heading": 0.5989654042474855, + "angularVelocity": 0.05790118146429404, + "velocityX": 1.54408896019307, + "velocityY": 0.8922040044224955, + "timestamp": 2.5954987021947815 + }, + { + "x": 8.021715394731531, + "y": 2.28287256797719, + "heading": 0.6011062262493081, + "angularVelocity": 0.032032667760495764, + "velocityX": 1.3241053714763598, + "velocityY": 0.7641757707822818, + "timestamp": 2.6623311626638664 + }, + { + "x": 8.095484569673351, + "y": 2.3254034695687893, + "heading": 0.6020727831826357, + "angularVelocity": 0.014462387386960431, + "velocityX": 1.103792594557322, + "velocityY": 0.6363809037267617, + "timestamp": 2.7291636231329512 + }, + { + "x": 8.154515205691396, + "y": 2.359407222237499, + "heading": 0.6022883195935687, + "angularVelocity": 0.003225025824578962, + "velocityX": 0.8832629474318238, + "velocityY": 0.5087909741769772, + "timestamp": 2.795996083602036 + }, + { + "x": 8.198797270954351, + "y": 2.3848957424596233, + "heading": 0.6020871548762373, + "angularVelocity": -0.00300998520658142, + "velocityX": 0.6625831961317433, + "velocityY": 0.38137934834697124, + "timestamp": 2.862828544071121 + }, + { + "x": 8.22832355571237, + "y": 2.401879399345903, + "heading": 0.6017403922656039, + "angularVelocity": -0.005188535753427844, + "velocityX": 0.44179556686643673, + "velocityY": 0.2541228733324269, + "timestamp": 2.929661004540206 + }, + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 0.601472494720935, + "angularVelocity": -0.004008494417063559, + "velocityX": 0.220928070177263, + "velocityY": 0.12700192446944059, + "timestamp": 2.9964934650092907 + }, + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 0.601472494720935, + "angularVelocity": 2.0480668095758719e-22, + "velocityX": 1.6959236187935907e-23, + "velocityY": -2.878465592599261e-23, + "timestamp": 3.0633259254783756 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3PieceSourceSideLower.4.traj b/src/main/deploy/choreo/3PieceSourceSideLower.4.traj new file mode 100644 index 0000000..cbec533 --- /dev/null +++ b/src/main/deploy/choreo/3PieceSourceSideLower.4.traj @@ -0,0 +1,437 @@ +{ + "samples": [ + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 0.601472494720935, + "angularVelocity": 2.0480668095758719e-22, + "velocityX": 1.6959236187935907e-23, + "velocityY": -2.878465592599261e-23, + "timestamp": 0 + }, + { + "x": 8.225716112229817, + "y": 2.403169142964958, + "heading": 0.6014724848808553, + "angularVelocity": -1.401208853680834e-7, + "velocityX": -0.24738269796567666, + "velocityY": -0.10249969625322676, + "timestamp": 0.07022564691083488 + }, + { + "x": 8.190970892472077, + "y": 2.3887729281095678, + "heading": 0.6014724652007141, + "angularVelocity": -2.802415057039817e-7, + "velocityX": -0.49476539250476237, + "velocityY": -0.20499939108669155, + "timestamp": 0.14045129382166976 + }, + { + "x": 8.138853063244545, + "y": 2.3671786059959783, + "heading": 0.6014724356805342, + "angularVelocity": -4.203618073354203e-7, + "velocityX": -0.7421480829319401, + "velocityY": -0.30749908421644245, + "timestamp": 0.21067694073250465 + }, + { + "x": 8.069362624900151, + "y": 2.3383861767704226, + "heading": 0.6014723963203429, + "angularVelocity": -5.604817189302354e-7, + "velocityX": -0.9895307683334543, + "velocityY": -0.40999877526387724, + "timestamp": 0.28090258764333953 + }, + { + "x": 7.982499577880061, + "y": 2.3023956406156896, + "heading": 0.6014723471201746, + "angularVelocity": -7.006011425140131e-7, + "velocityX": -1.2369134474528922, + "velocityY": -0.5124984637084178, + "timestamp": 0.3511282345541744 + }, + { + "x": 7.878263922751481, + "y": 2.259206997766796, + "heading": 0.601472288080073, + "angularVelocity": -8.407199383581404e-7, + "velocityX": -1.484296118495379, + "velocityY": -0.6149981488063821, + "timestamp": 0.4213538814650093 + }, + { + "x": 7.756655660270691, + "y": 2.2088202485370934, + "heading": 0.6014722192000971, + "angularVelocity": -9.808378971745156e-7, + "velocityX": -1.7316787787686057, + "velocityY": -0.7174978294422485, + "timestamp": 0.4915795283758442 + }, + { + "x": 7.617674791496479, + "y": 2.1512353933652784, + "heading": 0.6014721404803293, + "angularVelocity": -0.0000011209546846231955, + "velocityX": -1.979061423964885, + "velocityY": -0.8199975038311836, + "timestamp": 0.5618051752866791 + }, + { + "x": 7.461321318017023, + "y": 2.0864524329093923, + "heading": 0.6014720519208926, + "angularVelocity": -0.0000012610697164822291, + "velocityX": -2.226444046545777, + "velocityY": -0.9224971688497368, + "timestamp": 0.6320308221975139 + }, + { + "x": 7.287595242479287, + "y": 2.014471368266166, + "heading": 0.6014719535219923, + "angularVelocity": -0.0000014011818267188707, + "velocityX": -2.4738266314344535, + "velocityY": -1.0249968182510263, + "timestamp": 0.7022564691083488 + }, + { + "x": 7.096496570177163, + "y": 1.935292201629055, + "heading": 0.6014718452840377, + "angularVelocity": -0.0000015412881098024927, + "velocityX": -2.7212091409390613, + "velocityY": -1.1274964364179387, + "timestamp": 0.7724821160191837 + }, + { + "x": 6.8880253169921275, + "y": 1.8489149395783349, + "heading": 0.601471727208249, + "angularVelocity": -0.0000016813770192107404, + "velocityX": -2.9685914242944573, + "velocityY": -1.2299959608829605, + "timestamp": 0.8427077629300186 + }, + { + "x": 6.663723389677425, + "y": 1.7559784504804536, + "heading": 0.6014717272081879, + "angularVelocity": -8.702352256817885e-13, + "velocityX": -3.1940172455726326, + "velocityY": -1.3233981200042444, + "timestamp": 0.9129334098408535 + }, + { + "x": 6.439421272188231, + "y": 1.6630424203676835, + "heading": 0.6014717272081268, + "angularVelocity": -8.703596574297939e-13, + "velocityX": -3.1940199536216154, + "velocityY": -1.3233915841426174, + "timestamp": 0.9831590567516884 + }, + { + "x": 6.212032631338727, + "y": 1.577936888330409, + "heading": 0.6014717272080651, + "angularVelocity": -8.787789669679053e-13, + "velocityX": -3.237971465584607, + "velocityY": -1.2118867647503735, + "timestamp": 1.0533847036625232 + }, + { + "x": 5.978739653450143, + "y": 1.510684142319498, + "heading": 0.6014717272080008, + "angularVelocity": -9.15623540134239e-13, + "velocityX": -3.32204811419955, + "velocityY": -0.9576664504963734, + "timestamp": 1.1236103505733581 + }, + { + "x": 5.7409415584912775, + "y": 1.4616883276374903, + "heading": 0.601471727201828, + "angularVelocity": -8.790086201060278e-11, + "velocityX": -3.386200133703241, + "velocityY": -0.6976911831686963, + "timestamp": 1.193835997484193 + }, + { + "x": 5.516819527735436, + "y": 1.4255992428677315, + "heading": 0.6014714661772929, + "angularVelocity": -0.000003716940268980297, + "velocityX": -3.1914555524195745, + "velocityY": -0.513901777445793, + "timestamp": 1.2640616443950279 + }, + { + "x": 5.308884526984856, + "y": 1.3990809310343755, + "heading": 0.6014711092836408, + "angularVelocity": -0.0000050820984614807965, + "velocityX": -2.9609552905164764, + "velocityY": -0.3776157714435294, + "timestamp": 1.3342872913058628 + }, + { + "x": 5.1176297142242815, + "y": 1.3812454338123161, + "heading": 0.6014706963446639, + "angularVelocity": -0.000005880173340714845, + "velocityX": -2.7234325516916758, + "velocityY": -0.253974124933375, + "timestamp": 1.4045129382166976 + }, + { + "x": 4.943259966846924, + "y": 1.3716872939772524, + "heading": 0.6014702453730658, + "angularVelocity": -0.000006421750712363147, + "velocityX": -2.482992397332454, + "velocityY": -0.13610611301593797, + "timestamp": 1.4747385851275325 + }, + { + "x": 4.785886935464025, + "y": 1.3701747617379714, + "heading": 0.6014697666441067, + "angularVelocity": -0.00000681701031230746, + "velocityX": -2.240962359274736, + "velocityY": -0.021538174524779245, + "timestamp": 1.5449642320383674 + }, + { + "x": 4.645580768585205, + "y": 1.3765579462051392, + "heading": 0.6014692668007987, + "angularVelocity": -0.000007117674667444465, + "velocityX": -1.997933419637491, + "velocityY": 0.09089534590222227, + "timestamp": 1.6151898789492023 + }, + { + "x": 4.4852918274531035, + "y": 1.4093136530981196, + "heading": 0.6014686169682122, + "angularVelocity": -0.00000794408595037517, + "velocityX": -1.9595033424539408, + "velocityY": 0.400432598081353, + "timestamp": 1.6969906790087563 + }, + { + "x": 4.332045122157961, + "y": 1.4665930457792093, + "heading": 0.6014678756495806, + "angularVelocity": -0.000009062486322055742, + "velocityX": -1.8734132818208915, + "velocityY": 0.7002302256137938, + "timestamp": 1.7787914790683104 + }, + { + "x": 4.189567990618789, + "y": 1.5470029255462814, + "heading": 0.6014669872102968, + "angularVelocity": -0.00001086100971182791, + "velocityX": -1.7417571886270768, + "velocityY": 0.9829962507522065, + "timestamp": 1.8605922791278644 + }, + { + "x": 4.061325777655998, + "y": 1.6485874554715532, + "heading": 0.6014657443656565, + "angularVelocity": -0.00001519355115434508, + "velocityX": -1.567737881162854, + "velocityY": 1.2418525228520432, + "timestamp": 1.9423930791874184 + }, + { + "x": 3.9499181722494128, + "y": 1.7683949151806115, + "heading": 0.5999046298375316, + "angularVelocity": -0.019084343025843265, + "velocityX": -1.3619378456625928, + "velocityY": 1.4646245467261332, + "timestamp": 2.0241938792469725 + }, + { + "x": 3.8385139180743773, + "y": 1.888205491074706, + "heading": 0.5540381787699103, + "angularVelocity": -0.5607090766132972, + "velocityX": -1.361896877462441, + "velocityY": 1.4646626415251267, + "timestamp": 2.1059946793065265 + }, + { + "x": 3.7271096639847774, + "y": 2.0080160670482403, + "heading": 0.4697368355238526, + "angularVelocity": -1.0305686886275442, + "velocityX": -1.361896876418011, + "velocityY": 1.4646626424962754, + "timestamp": 2.1877954793660805 + }, + { + "x": 3.6157054099517287, + "y": 2.1278266430743598, + "heading": 0.3537615125967293, + "angularVelocity": -1.4177773670023999, + "velocityX": -1.3618968757266772, + "velocityY": 1.4646626431391025, + "timestamp": 2.2695962794256346 + }, + { + "x": 3.504301155997762, + "y": 2.247637219174011, + "heading": 0.2137130812750941, + "angularVelocity": -1.7120667673137158, + "velocityX": -1.3618968747599156, + "velocityY": 1.4646626440380324, + "timestamp": 2.3513970794851886 + }, + { + "x": 3.3928969021094617, + "y": 2.367447795334722, + "heading": 0.057794482937139194, + "angularVelocity": -1.9060766914802987, + "velocityX": -1.3618968739571615, + "velocityY": 1.4646626447844622, + "timestamp": 2.4331978795447426 + }, + { + "x": 3.281492648234104, + "y": 2.487258371507466, + "heading": -0.105452505167902, + "angularVelocity": -1.995664932203507, + "velocityX": -1.3618968737989476, + "velocityY": 1.464662644931575, + "timestamp": 2.5149986796042967 + }, + { + "x": 3.1700883943158846, + "y": 2.6070689476403572, + "heading": -0.26730664929832215, + "angularVelocity": -1.978637666289151, + "velocityX": -1.3618968743229085, + "velocityY": 1.4646626444443769, + "timestamp": 2.5967994796638507 + }, + { + "x": 3.058684140329769, + "y": 2.7268795237101155, + "heading": -0.41916927419219197, + "angularVelocity": -1.8564931490072147, + "velocityX": -1.3618968751529306, + "velocityY": 1.4646626436725918, + "timestamp": 2.6786002797234048 + }, + { + "x": 2.947279886280209, + "y": 2.846690099720882, + "heading": -0.5526625850121627, + "angularVelocity": -1.6319316036369287, + "velocityX": -1.3618968759285288, + "velocityY": 1.464662642951413, + "timestamp": 2.760401079782959 + }, + { + "x": 2.835875632186779, + "y": 2.966500675690855, + "heading": -0.65985673738226, + "angularVelocity": -1.3104291436276523, + "velocityX": -1.3618968764648283, + "velocityY": 1.464662642452743, + "timestamp": 2.842201879842513 + }, + { + "x": 2.7244713780717897, + "y": 3.086311251640782, + "heading": -0.7334716118848168, + "angularVelocity": -0.899928539194763, + "velocityX": -1.3618968767283959, + "velocityY": 1.4646626422076685, + "timestamp": 2.924002679902067 + }, + { + "x": 2.613067124142982, + "y": 3.2061218277638193, + "heading": -0.7669996227711985, + "angularVelocity": -0.4098738748517434, + "velocityX": -1.361896874452356, + "velocityY": 1.4646626443239155, + "timestamp": 3.005803479961621 + }, + { + "x": 2.5088215468762853, + "y": 3.3182335485748182, + "heading": -0.7669997571492978, + "angularVelocity": -0.0000016427479828891154, + "velocityX": -1.2743833457717078, + "velocityY": 1.3705455292537272, + "timestamp": 3.087604280021175 + }, + { + "x": 2.421950224540146, + "y": 3.411659991166572, + "heading": -0.7669998477403924, + "angularVelocity": -0.0000011074597615269072, + "velocityX": -1.06198621862983, + "velocityY": 1.1421213792986926, + "timestamp": 3.169405080080729 + }, + { + "x": 2.352453163850527, + "y": 3.4864011482861903, + "heading": -0.7669999126210906, + "angularVelocity": -7.931548110988282e-7, + "velocityX": -0.8495890093864996, + "velocityY": 0.9136971406783939, + "timestamp": 3.251205880140283 + }, + { + "x": 2.300330367046085, + "y": 3.5424570175160435, + "heading": -0.7669999578169298, + "angularVelocity": -5.525109684504957e-7, + "velocityX": -0.6371917727760005, + "velocityY": 0.6852728725029872, + "timestamp": 3.333006680199837 + }, + { + "x": 2.2655818352461483, + "y": 3.5798275976473137, + "heading": -0.7669999863406792, + "angularVelocity": -3.486976819494124e-7, + "velocityX": -0.4247945224819143, + "velocityY": 0.4568485895500231, + "timestamp": 3.414807480259391 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -1.6698272956598662e-7, + "velocityX": -0.21239726397767503, + "velocityY": 0.2284242977305235, + "timestamp": 3.496608280318945 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -2.381764007243456e-23, + "velocityX": 2.799365906238757e-23, + "velocityY": -7.174872988733629e-24, + "timestamp": 3.578409080378499 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3PieceSourceSideLower.traj b/src/main/deploy/choreo/3PieceSourceSideLower.traj new file mode 100644 index 0000000..adae522 --- /dev/null +++ b/src/main/deploy/choreo/3PieceSourceSideLower.traj @@ -0,0 +1,1859 @@ +{ + "samples": [ + { + "x": 0.6476043462753296, + "y": 4.391018867492676, + "heading": -1.0516500181249593, + "angularVelocity": 1.4837621687026506e-24, + "velocityX": 1.2605777337321822e-24, + "velocityY": -5.078976570788486e-24, + "timestamp": 0 + }, + { + "x": 0.6591914533996294, + "y": 4.3821264561987325, + "heading": -1.0407271677663157, + "angularVelocity": 0.1717852253768825, + "velocityX": 0.1822320862648084, + "velocityY": -0.1398522206307734, + "timestamp": 0.06358434105540607 + }, + { + "x": 0.682370568824486, + "y": 4.364325443623188, + "heading": -1.0189895299827134, + "angularVelocity": 0.341870929584073, + "velocityX": 0.36454125402760434, + "velocityY": -0.2799590635064334, + "timestamp": 0.12716868211081214 + }, + { + "x": 0.7171462629991276, + "y": 4.337595981630201, + "heading": -0.9865639713671387, + "angularVelocity": 0.509961384790003, + "velocityX": 0.5469223018972373, + "velocityY": -0.42037806084509965, + "timestamp": 0.19075302316621823 + }, + { + "x": 0.7635267836332045, + "y": 4.301915006000676, + "heading": -0.9436158522996869, + "angularVelocity": 0.6754511937149356, + "velocityX": 0.7294330626728007, + "velocityY": -0.5611597924469304, + "timestamp": 0.2543373642216243 + }, + { + "x": 0.821528668373285, + "y": 4.257255375739764, + "heading": -0.8903806097171314, + "angularVelocity": 0.8372382523578797, + "velocityX": 0.9122039133745026, + "velocityY": -0.7023683743454452, + "timestamp": 0.31792170527703034 + }, + { + "x": 0.891181254168102, + "y": 4.203582879820318, + "heading": -0.8272094806909008, + "angularVelocity": 0.9935013554860082, + "velocityX": 1.0954361504528793, + "velocityY": -0.8441149979470245, + "timestamp": 0.3815060463324364 + }, + { + "x": 0.9725299828215167, + "y": 4.140850923918169, + "heading": -0.7546270993304982, + "angularVelocity": 1.1415134631521273, + "velocityX": 1.279383057261364, + "velocityY": -0.986594417129931, + "timestamp": 0.44509038738784246 + }, + { + "x": 1.065637529662394, + "y": 4.068994254030079, + "heading": -0.673393688475822, + "angularVelocity": 1.2775694377943008, + "velocityX": 1.464315667905486, + "velocityY": -1.1301000953281266, + "timestamp": 0.5086747284432486 + }, + { + "x": 1.1705832794468332, + "y": 3.9879256971217893, + "heading": -0.584566561837581, + "angularVelocity": 1.3969968889170195, + "velocityX": 1.6504967739304255, + "velocityY": -1.2749767562684577, + "timestamp": 0.5722590694986547 + }, + { + "x": 1.2874653284560715, + "y": 3.897542804344524, + "heading": -0.4895895148245002, + "angularVelocity": 1.4937175637366416, + "velocityX": 1.8382206541605852, + "velocityY": -1.42146464486449, + "timestamp": 0.6358434105540608 + }, + { + "x": 1.4164104469481729, + "y": 3.7977514051305152, + "heading": -0.3905846134248042, + "angularVelocity": 1.557064204116311, + "velocityX": 2.0279382683189424, + "velocityY": -1.5694335674101274, + "timestamp": 0.6994277516094669 + }, + { + "x": 1.5575332696632753, + "y": 3.688506909682212, + "heading": -0.2916586370426824, + "angularVelocity": 1.5558229391088563, + "velocityX": 2.2194587593843385, + "velocityY": -1.718103760061147, + "timestamp": 0.763012092664873 + }, + { + "x": 1.7097613444585642, + "y": 3.5707774936908128, + "heading": -0.20742198100231668, + "angularVelocity": 1.324801903144104, + "velocityX": 2.394112642649561, + "velocityY": -1.8515473155381532, + "timestamp": 0.8265964337202791 + }, + { + "x": 1.8711613775944314, + "y": 3.4488084136916637, + "heading": -0.15383975328628857, + "angularVelocity": 0.8426953370380558, + "velocityX": 2.538361339550979, + "velocityY": -1.9182251160370993, + "timestamp": 0.8901807747756852 + }, + { + "x": 2.0417383028799354, + "y": 3.3225570909935143, + "heading": -0.13059320326371432, + "angularVelocity": 0.3656018075632435, + "velocityX": 2.682687631171131, + "velocityY": -1.9855725576858096, + "timestamp": 0.9537651158310914 + }, + { + "x": 2.2153893622715715, + "y": 3.187755682753254, + "heading": -0.13059307027328165, + "angularVelocity": 0.0000020915595014550256, + "velocityX": 2.731034976682693, + "velocityY": -2.120040972395975, + "timestamp": 1.0173494568864974 + }, + { + "x": 2.389039928413357, + "y": 3.0529536391086727, + "heading": -0.13059293728818866, + "angularVelocity": 0.000002091475523406473, + "velocityX": 2.7310272192719673, + "velocityY": -2.1200509654903494, + "timestamp": 1.0809337979419034 + }, + { + "x": 2.562690494523223, + "y": 2.918151595422971, + "heading": -0.13059280430309889, + "angularVelocity": 0.000002091475472778544, + "velocityX": 2.7310272187699582, + "velocityY": -2.1200509661370504, + "timestamp": 1.1445181389973094 + }, + { + "x": 2.7363410606330874, + "y": 2.7833495517372655, + "heading": -0.1305926713180123, + "angularVelocity": 0.0000020914754223957298, + "velocityX": 2.7310272187699196, + "velocityY": -2.1200509661371187, + "timestamp": 1.2081024800527154 + }, + { + "x": 2.9099916267429506, + "y": 2.648547508051558, + "heading": -0.13059253833292897, + "angularVelocity": 0.000002091475371640255, + "velocityX": 2.731027218769913, + "velocityY": -2.120050966137146, + "timestamp": 1.2716868211081214 + }, + { + "x": 3.0836421928528135, + "y": 2.513745464365849, + "heading": -0.1305924053478488, + "angularVelocity": 0.000002091475321590614, + "velocityX": 2.7310272187699067, + "velocityY": -2.120050966137172, + "timestamp": 1.3352711621635274 + }, + { + "x": 3.257292758962676, + "y": 2.3789434206801383, + "heading": -0.13059227236277188, + "angularVelocity": 0.000002091475271021181, + "velocityX": 2.7310272187699005, + "velocityY": -2.1200509661371987, + "timestamp": 1.3988555032189334 + }, + { + "x": 3.430943325072538, + "y": 2.244141376994426, + "heading": -0.1305921393776981, + "angularVelocity": 0.0000020914752212008574, + "velocityX": 2.7310272187698943, + "velocityY": -2.1200509661372253, + "timestamp": 1.4624398442743394 + }, + { + "x": 3.6045938911823994, + "y": 2.1093393333087116, + "heading": -0.13059200639262752, + "angularVelocity": 0.0000020914751708183746, + "velocityX": 2.731027218769888, + "velocityY": -2.120050966137252, + "timestamp": 1.5260241853297454 + }, + { + "x": 3.7782444572922627, + "y": 1.9745372896229973, + "heading": -0.13059187340756015, + "angularVelocity": 0.000002091475120575094, + "velocityX": 2.7310272187699067, + "velocityY": -2.1200509661372466, + "timestamp": 1.5896085263851514 + }, + { + "x": 3.951895023426423, + "y": 1.8397352459685825, + "heading": -0.13059174042249597, + "angularVelocity": 0.0000020914750700807474, + "velocityX": 2.731027219152042, + "velocityY": -2.1200509656450026, + "timestamp": 1.6531928674405574 + }, + { + "x": 4.125545965030257, + "y": 1.7049336859916362, + "heading": -0.13059160743738255, + "angularVelocity": 0.0000020914758451630756, + "velocityX": 2.731033124217136, + "velocityY": -2.1200433587804755, + "timestamp": 1.7167772084959634 + }, + { + "x": 4.303592186821449, + "y": 1.5759931847530775, + "heading": -0.13059147324861473, + "angularVelocity": 0.0000021104058890338206, + "velocityX": 2.800158322566331, + "velocityY": -2.0278656521139804, + "timestamp": 1.7803615495513694 + }, + { + "x": 4.4902367308457585, + "y": 1.459847055147331, + "heading": -0.13059133313690313, + "angularVelocity": 0.0000022035568702526466, + "velocityX": 2.9353853625953525, + "velocityY": -1.8266467447470887, + "timestamp": 1.8439458906067754 + }, + { + "x": 4.6845622062683105, + "y": 1.3570671081542969, + "heading": -0.13059118201554434, + "angularVelocity": 0.0000023767071623724323, + "velocityX": 3.0561844661285633, + "velocityY": -1.6164348845492313, + "timestamp": 1.9075302316621814 + }, + { + "x": 4.832067084470335, + "y": 1.2885914794569837, + "heading": -0.1305910326229948, + "angularVelocity": 0.0000031760307778025535, + "velocityX": 3.1358995782886923, + "velocityY": -1.4557667364794429, + "timestamp": 1.9545677322324386 + }, + { + "x": 4.982924581891149, + "y": 1.2278574954319752, + "heading": -0.13059088659891144, + "angularVelocity": 0.000003104418423448367, + "velocityX": 3.207175032514466, + "velocityY": -1.2911822118246548, + "timestamp": 2.001605232802696 + }, + { + "x": 5.136728649173284, + "y": 1.1750284268224231, + "heading": -0.13059074139155538, + "angularVelocity": 0.00000308705510077957, + "velocityX": 3.269818026415032, + "velocityY": -1.1231266110886224, + "timestamp": 2.048642733372953 + }, + { + "x": 5.293065104212638, + "y": 1.1302454562937592, + "heading": -0.1305905947227455, + "angularVelocity": 0.0000031181250722314943, + "velocityX": 3.3236556607816112, + "velocityY": -0.9520695186976181, + "timestamp": 2.0956802339432103 + }, + { + "x": 5.450482773711564, + "y": 1.0894253766985387, + "heading": -0.13059044674627163, + "angularVelocity": 0.0000031459255294965824, + "velocityX": 3.3466418834011025, + "velocityY": -0.8678199117797456, + "timestamp": 2.1427177345134676 + }, + { + "x": 5.607900655440282, + "y": 1.04860611555064, + "heading": -0.1305902987694863, + "angularVelocity": 0.0000031459321514570608, + "velocityX": 3.346646395328584, + "velocityY": -0.8678025118900428, + "timestamp": 2.189755235083725 + }, + { + "x": 5.765318537202331, + "y": 1.0077868545312796, + "heading": -0.13059015079270853, + "angularVelocity": 0.0000031459319896246793, + "velocityX": 3.346646396037204, + "velocityY": -0.867802509157372, + "timestamp": 2.236792735653982 + }, + { + "x": 5.922736418964388, + "y": 0.9669675935119395, + "heading": -0.1305900028159384, + "angularVelocity": 0.0000031459318275019883, + "velocityX": 3.3466463960373436, + "velocityY": -0.867802509156932, + "timestamp": 2.2838302362242393 + }, + { + "x": 6.080154300726486, + "y": 0.9261483324927575, + "heading": -0.13058985483917593, + "angularVelocity": 0.0000031459316651066595, + "velocityX": 3.3466463960382407, + "velocityY": -0.867802509153571, + "timestamp": 2.3308677367944965 + }, + { + "x": 6.237572182748789, + "y": 0.8853290724770384, + "heading": -0.1305897068624206, + "angularVelocity": 0.0000031459315124739283, + "velocityX": 3.346646401570085, + "velocityY": -0.86780248782032, + "timestamp": 2.377905237364754 + }, + { + "x": 6.394991721451892, + "y": 0.844516201922275, + "heading": -0.1305895588824519, + "angularVelocity": 0.0000031459998283836956, + "velocityX": 3.3466816219959377, + "velocityY": -0.8676666502252512, + "timestamp": 2.424942737935011 + }, + { + "x": 6.554152816050896, + "y": 0.8111344074726426, + "heading": -0.1305893710725833, + "angularVelocity": 0.000003992768882659048, + "velocityX": 3.3837064612154375, + "velocityY": -0.7096846993341356, + "timestamp": 2.4719802385052683 + }, + { + "x": 6.711882769951912, + "y": 0.7851880682804085, + "heading": -0.12042377120760904, + "angularVelocity": 0.21611692249230904, + "velocityX": 3.3532809351853774, + "velocityY": -0.5516096492729113, + "timestamp": 2.5190177390755255 + }, + { + "x": 6.866725553620017, + "y": 0.7651062041845728, + "heading": -0.09663025977821411, + "angularVelocity": 0.5058413210934933, + "velocityX": 3.29190075558594, + "velocityY": -0.4269330609061741, + "timestamp": 2.5660552396457827 + }, + { + "x": 7.014982029149098, + "y": 0.7495960675806145, + "heading": -0.06694522514508651, + "angularVelocity": 0.6310929422958753, + "velocityX": 3.151878261635918, + "velocityY": -0.3297398121907379, + "timestamp": 2.61309274021604 + }, + { + "x": 7.155468637798704, + "y": 0.7371442919133998, + "heading": -0.03984157562428539, + "angularVelocity": 0.5762136421410802, + "velocityX": 2.9866937432138485, + "velocityY": -0.26472018105247963, + "timestamp": 2.6601302407862972 + }, + { + "x": 7.28821756510369, + "y": 0.727344980547685, + "heading": -0.017222967030039466, + "angularVelocity": 0.48086331799160453, + "velocityX": 2.8221934774511723, + "velocityY": -0.20832976342095313, + "timestamp": 2.7071677413565545 + }, + { + "x": 7.413279056549072, + "y": 0.72, + "heading": -8.89059623238174e-24, + "angularVelocity": 0.3661539584637254, + "velocityX": 2.6587614122605316, + "velocityY": -0.15615159093571299, + "timestamp": 2.7542052419268117 + }, + { + "x": 7.584141987680618, + "y": 0.7142363916829146, + "heading": 0.012496138017563933, + "angularVelocity": 0.17655403361214328, + "velocityX": 2.4140690222585377, + "velocityY": -0.0814322229085219, + "timestamp": 2.824983222819707 + }, + { + "x": 7.7365481472096285, + "y": 0.7118516221881321, + "heading": 0.017397146839313638, + "angularVelocity": 0.0692448238833787, + "velocityX": 2.153299057225703, + "velocityY": -0.03369366383015727, + "timestamp": 2.8957612037126026 + }, + { + "x": 7.8700913671839885, + "y": 0.7115747244873029, + "heading": 0.01782819430589125, + "angularVelocity": 0.006090135111792666, + "velocityX": 1.8867904719752229, + "velocityY": -0.00391220118652886, + "timestamp": 2.966539184605498 + }, + { + "x": 7.98461628322269, + "y": 0.7125587164394606, + "heading": 0.01566183427732483, + "angularVelocity": -0.03060782465445932, + "velocityX": 1.6180867918795971, + "velocityY": 0.013902515157174911, + "timestamp": 3.0373171654983935 + }, + { + "x": 8.08006227999561, + "y": 0.7142078102454845, + "heading": 0.012151433911488669, + "angularVelocity": -0.04959735106244768, + "velocityX": 1.3485266967046623, + "velocityY": 0.023299531651227553, + "timestamp": 3.108095146391289 + }, + { + "x": 8.15640902937787, + "y": 0.716081694603084, + "heading": 0.008204279269867538, + "angularVelocity": -0.055768115900256314, + "velocityX": 1.0786793917982904, + "velocityY": 0.02647552719023059, + "timestamp": 3.1788731272841844 + }, + { + "x": 8.213654764882792, + "y": 0.7178420004122783, + "heading": 0.004513492953959301, + "angularVelocity": -0.05214596784688299, + "velocityX": 0.8088071287530345, + "velocityY": 0.02487081133125343, + "timestamp": 3.24965110817708 + }, + { + "x": 8.251806612817116, + "y": 0.7192206370589289, + "heading": 0.0016291300858943402, + "angularVelocity": -0.04075226266244733, + "velocityX": 0.5390355510714093, + "velocityY": 0.019478326864633543, + "timestamp": 3.3204290890699752 + }, + { + "x": 8.270875930786133, + "y": 0.72, + "heading": 5.577770790961069e-24, + "angularVelocity": -0.02301747048082103, + "velocityX": 0.2694244414498442, + "velocityY": 0.011011375730688399, + "timestamp": 3.3912070699628707 + }, + { + "x": 8.270875930786133, + "y": 0.72, + "heading": 2.68750083227073e-24, + "angularVelocity": -1.0022200251051524e-24, + "velocityX": 6.790628726650556e-26, + "velocityY": 3.41370039560792e-26, + "timestamp": 3.461985050855766 + }, + { + "x": 8.251549516655441, + "y": 0.7219586831033024, + "heading": -0.0006660755390764362, + "angularVelocity": -0.009331396835267377, + "velocityX": -0.2707537344882418, + "velocityY": 0.027440204960520992, + "timestamp": 3.5333650972555772 + }, + { + "x": 8.212896688696702, + "y": 0.7258760442658929, + "heading": -0.001998229790486319, + "angularVelocity": -0.018662838126333965, + "velocityX": -0.5415074647365562, + "velocityY": 0.054880339256839474, + "timestamp": 3.6047451436553883 + }, + { + "x": 8.154917446658368, + "y": 0.7317520758923975, + "heading": -0.003996392789209819, + "angularVelocity": -0.027993299241239875, + "velocityX": -0.812261198508941, + "velocityY": 0.08232036714563006, + "timestamp": 3.6761251900551994 + }, + { + "x": 8.077611789765818, + "y": 0.7395867678254814, + "heading": -0.006660425114458462, + "angularVelocity": -0.03732180713818774, + "velocityX": -1.0830149431334009, + "velocityY": 0.10976025273506382, + "timestamp": 3.7475052364550105 + }, + { + "x": 7.980979716767329, + "y": 0.749380107333022, + "heading": -0.00999012297038977, + "angularVelocity": -0.04664745995374023, + "velocityX": -1.353768705293865, + "velocityY": 0.13719995995360584, + "timestamp": 3.8188852828548217 + }, + { + "x": 7.86502122600115, + "y": 0.7611320790926304, + "heading": -0.013985224659649345, + "angularVelocity": -0.0559694465156602, + "velocityX": -1.6245224907346907, + "velocityY": 0.1646394525128738, + "timestamp": 3.8902653292546328 + }, + { + "x": 7.729736315496098, + "y": 0.7748426651724982, + "heading": -0.018645418435560536, + "angularVelocity": -0.06528706565709827, + "velocityX": -1.8952763037908287, + "velocityY": 0.1920786938561615, + "timestamp": 3.961645375654444 + }, + { + "x": 7.575124983133034, + "y": 0.790511845006039, + "heading": -0.023970351697693893, + "angularVelocity": -0.07459974503669481, + "velocityX": -2.1660301465350793, + "velocityY": 0.21951764707149618, + "timestamp": 4.033025422054255 + }, + { + "x": 7.401187226935479, + "y": 0.8081395953535565, + "heading": -0.029959641438283554, + "angularVelocity": -0.08390705866234215, + "velocityX": -2.4367840169688693, + "velocityY": 0.24695627470990408, + "timestamp": 4.1044054684540665 + }, + { + "x": 7.207923045694123, + "y": 0.8277258902299445, + "heading": -0.03661288565920585, + "angularVelocity": -0.09320874048829283, + "velocityX": -2.7075379043444587, + "velocityY": 0.2743945383095141, + "timestamp": 4.175785514853878 + }, + { + "x": 6.995332440744312, + "y": 0.8492707007132015, + "heading": -0.04392967465186433, + "angularVelocity": -0.10250468249454428, + "velocityX": -2.9782917730125558, + "velocityY": 0.3018323967258469, + "timestamp": 4.24716556125369 + }, + { + "x": 6.763415424631655, + "y": 0.872773994034365, + "heading": -0.05190959463777978, + "angularVelocity": -0.11179482766400335, + "velocityX": -3.249045465922667, + "velocityY": 0.32926979606482365, + "timestamp": 4.318545607653501 + }, + { + "x": 6.517888285888531, + "y": 0.8976525425650798, + "heading": -0.051909598739256456, + "angularVelocity": -5.745970870698805e-8, + "velocityX": -3.439716715339282, + "velocityY": 0.348536457812958, + "timestamp": 4.389925654053313 + }, + { + "x": 6.272361147029228, + "y": 0.9225310899511409, + "heading": -0.051909602840455864, + "angularVelocity": -5.7455824331798554e-8, + "velocityX": -3.439716716966896, + "velocityY": 0.3485364417769111, + "timestamp": 4.461305700453124 + }, + { + "x": 6.026834008173439, + "y": 0.94740963737188, + "heading": -0.051909606941655285, + "angularVelocity": -5.7455824691381894e-8, + "velocityX": -3.4397167169176694, + "velocityY": 0.34853644226273583, + "timestamp": 4.532685746852936 + }, + { + "x": 5.7813069462957465, + "y": 0.9722889444800497, + "heading": -0.05190961104285578, + "angularVelocity": -5.745583959865336e-8, + "velocityX": -3.4397156384916827, + "velocityY": 0.348547085116991, + "timestamp": 4.604065793252747 + }, + { + "x": 5.537158225945976, + "y": 1.0082600487060158, + "heading": -0.05190961517853181, + "angularVelocity": -5.793882502411163e-8, + "velocityX": -3.42040573891272, + "velocityY": 0.5039378095173286, + "timestamp": 4.675445839652559 + }, + { + "x": 5.296595540867912, + "y": 1.0633246680240616, + "heading": -0.051909619464878404, + "angularVelocity": -6.004964723504615e-8, + "velocityX": -3.3701671154797745, + "velocityY": 0.7714287408783704, + "timestamp": 4.74682588605237 + }, + { + "x": 5.061109922350585, + "y": 1.137142081975634, + "heading": -0.05190979783691868, + "angularVelocity": -0.0000024989061968370985, + "velocityX": -3.299039863302041, + "velocityY": 1.0341463430565587, + "timestamp": 4.818205932452182 + }, + { + "x": 4.843667300844079, + "y": 1.2172452033533754, + "heading": -0.059561850749452576, + "angularVelocity": -0.10720156820399783, + "velocityX": -3.0462661832492475, + "velocityY": 1.1222060704341816, + "timestamp": 4.8895859788519935 + }, + { + "x": 4.645580768585205, + "y": 1.29859459400177, + "heading": -0.06951316932259947, + "angularVelocity": -0.13941317041751372, + "velocityX": -2.7750967146947323, + "velocityY": 1.13966570143067, + "timestamp": 4.960966025251805 + }, + { + "x": 4.474092871614693, + "y": 1.385887514867849, + "heading": -0.08242444337490663, + "angularVelocity": -0.2012909785735536, + "velocityX": -2.6735523121009726, + "velocityY": 1.3609251412750347, + "timestamp": 5.025108363591072 + }, + { + "x": 4.3102265492062894, + "y": 1.4867635843508433, + "heading": -0.10022801616986421, + "angularVelocity": -0.27756351352189995, + "velocityX": -2.554729475898846, + "velocityY": 1.5726908637073918, + "timestamp": 5.0892507019303395 + }, + { + "x": 4.155003391680842, + "y": 1.6004909974861876, + "heading": -0.12448541502532746, + "angularVelocity": -0.37818076926286603, + "velocityX": -2.419979713000573, + "velocityY": 1.7730475077757013, + "timestamp": 5.153393040269607 + }, + { + "x": 4.00926290540325, + "y": 1.7261407641347544, + "heading": -0.1583340022250027, + "angularVelocity": -0.5277105274934034, + "velocityX": -2.2721417717378642, + "velocityY": 1.958920892218929, + "timestamp": 5.217535378608874 + }, + { + "x": 3.8730404280351673, + "y": 1.8620509915016963, + "heading": -0.21028223642363456, + "angularVelocity": -0.8098899345368795, + "velocityX": -2.1237529047906247, + "velocityY": 2.1188848253095043, + "timestamp": 5.281677716948141 + }, + { + "x": 3.741564544573194, + "y": 2.0025580918468195, + "heading": -0.2996033949603487, + "angularVelocity": -1.3925460288689253, + "velocityX": -2.049751955822999, + "velocityY": 2.1905515761202894, + "timestamp": 5.345820055287408 + }, + { + "x": 3.6100546633718698, + "y": 2.1430333719703096, + "heading": -0.3901784541639511, + "angularVelocity": -1.4120947497193557, + "velocityX": -2.050281991681201, + "velocityY": 2.1900554884743153, + "timestamp": 5.409962393626675 + }, + { + "x": 3.4785072023626884, + "y": 2.2834734612659138, + "heading": -0.4827580204171881, + "angularVelocity": -1.4433456691827582, + "velocityX": -2.0508678731571766, + "velocityY": 2.1895068519762364, + "timestamp": 5.474104731965943 + }, + { + "x": 3.346919123497923, + "y": 2.423875493587476, + "heading": -0.5782456610362502, + "angularVelocity": -1.488683498159982, + "velocityX": -2.051501118789228, + "velocityY": 2.188913531323639, + "timestamp": 5.53824707030521 + }, + { + "x": 3.2152861692168586, + "y": 2.5642354541414827, + "heading": -0.6777971832870476, + "angularVelocity": -1.552040739834588, + "velocityX": -2.052200741183146, + "velocityY": 2.1882576187291725, + "timestamp": 5.602389408644477 + }, + { + "x": 3.0838454699327147, + "y": 2.704775469009889, + "heading": -0.7323199293584344, + "angularVelocity": -0.8500274153243377, + "velocityX": -2.049203423001438, + "velocityY": 2.1910647242863015, + "timestamp": 5.666531746983744 + }, + { + "x": 2.9552905266702094, + "y": 2.842272453022611, + "heading": -0.7381340958895165, + "angularVelocity": -0.0906447548003214, + "velocityX": -2.0042135442980085, + "velocityY": 2.143622879562956, + "timestamp": 5.730674085323011 + }, + { + "x": 2.837445531451434, + "y": 2.968312162646033, + "heading": -0.7431823028036986, + "angularVelocity": -0.07870319425338904, + "velocityX": -1.837241957027799, + "velocityY": 1.965000230530428, + "timestamp": 5.794816423662279 + }, + { + "x": 2.7303126070394907, + "y": 3.082893932918914, + "heading": -0.747653667364265, + "angularVelocity": -0.06971003359615609, + "velocityX": -1.6702372751876633, + "velocityY": 1.7863672145350264, + "timestamp": 5.858958762001546 + }, + { + "x": 2.6338923756777755, + "y": 3.1860176221630003, + "heading": -0.751611677644213, + "angularVelocity": -0.061706672728596555, + "velocityX": -1.5032228923697837, + "velocityY": 1.6077319897293985, + "timestamp": 5.923101100340813 + }, + { + "x": 2.5481851332942993, + "y": 3.277683175661144, + "heading": -0.7550881872685589, + "angularVelocity": -0.05419992027664412, + "velocityX": -1.3362038959375901, + "velocityY": 1.4290959118655384, + "timestamp": 5.98724343868008 + }, + { + "x": 2.473191052958317, + "y": 3.3578905661578986, + "heading": -0.7581023488644949, + "angularVelocity": -0.04699176353679586, + "velocityX": -1.1691822012991828, + "velocityY": 1.2504594090803822, + "timestamp": 6.051385777019347 + }, + { + "x": 2.4089102482733002, + "y": 3.4266397779362934, + "heading": -0.7606669459018931, + "angularVelocity": -0.039982905266614695, + "velocityX": -1.0021587355455739, + "velocityY": 1.0718226612625237, + "timestamp": 6.1155281153586145 + }, + { + "x": 2.3553427994564915, + "y": 3.4839308009572485, + "heading": -0.7627911121209714, + "angularVelocity": -0.03311644498900487, + "velocityX": -0.8351340191789587, + "velocityY": 0.893185756932122, + "timestamp": 6.179670453697882 + }, + { + "x": 2.312488766021362, + "y": 3.5297636282468714, + "heading": -0.7644816934670975, + "angularVelocity": -0.026356715235171243, + "velocityX": -0.6681083749778789, + "velocityY": 0.714548743876462, + "timestamp": 6.243812792037149 + }, + { + "x": 2.280348193682581, + "y": 3.5641382545710476, + "heading": -0.7657440057178361, + "angularVelocity": -0.019679860189406772, + "velocityX": -0.501082018070189, + "velocityY": 0.5359116492192578, + "timestamp": 6.307955130376416 + }, + { + "x": 2.2589211184357674, + "y": 3.587054675697436, + "heading": -0.7665822895031111, + "angularVelocity": -0.013069117948915103, + "velocityX": -0.3340550999790392, + "velocityY": 0.35727448857846106, + "timestamp": 6.372097468715683 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -0.006512243047323657, + "velocityX": -0.1670277322411552, + "velocityY": 0.17863727070052132, + "timestamp": 6.4362398070549505 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": 1.4318885213105956e-25, + "velocityX": -3.1664598929952935e-25, + "velocityY": -8.517319099705152e-26, + "timestamp": 6.500382145394218 + }, + { + "x": 2.254662845986897, + "y": 3.5873395415687703, + "heading": -0.7560028105976356, + "angularVelocity": 0.18270874766778025, + "velocityX": 0.10724881682251722, + "velocityY": -0.18563544290641987, + "timestamp": 6.560571871079167 + }, + { + "x": 2.267640389134434, + "y": 3.5649741905137224, + "heading": -0.7343324319976283, + "angularVelocity": 0.3600345134223772, + "velocityX": 0.2156106046315132, + "velocityY": -0.3715808769774839, + "timestamp": 6.620761596764117 + }, + { + "x": 2.2872156815111926, + "y": 3.5313939684851823, + "heading": -0.7023698583511744, + "angularVelocity": 0.5310303923589071, + "velocityX": 0.3252264760138822, + "velocityY": -0.5579062148298977, + "timestamp": 6.680951322449067 + }, + { + "x": 2.313473905100035, + "y": 3.486574818355693, + "heading": -0.6605397935243593, + "angularVelocity": 0.6949701855390652, + "velocityX": 0.4362575720362232, + "velocityY": -0.7446312409544028, + "timestamp": 6.741141048134017 + }, + { + "x": 2.3465132166718874, + "y": 3.4304964860969096, + "heading": -0.6092924367494605, + "angularVelocity": 0.851430309603703, + "velocityX": 0.54891945752984, + "velocityY": -0.9316927701633545, + "timestamp": 6.801330773818966 + }, + { + "x": 2.3864518087256505, + "y": 3.3631500739334586, + "heading": -0.5490846408400017, + "angularVelocity": 1.0003002210809777, + "velocityX": 0.6635450087081837, + "velocityY": -1.118902128179834, + "timestamp": 6.861520499503916 + }, + { + "x": 2.4334407524616903, + "y": 3.284548274129898, + "heading": -0.48037214659109057, + "angularVelocity": 1.1415983951907667, + "velocityX": 0.7806804766313955, + "velocityY": -1.3059006152476056, + "timestamp": 6.921710225188866 + }, + { + "x": 2.4876845583004745, + "y": 3.194738200368583, + "heading": -0.4036330691658326, + "angularVelocity": 1.2749531012474145, + "velocityX": 0.9012137075139995, + "velocityY": -1.4921163494149543, + "timestamp": 6.9818999508738155 + }, + { + "x": 2.549471334765238, + "y": 3.093817881877023, + "heading": -0.3194506455237171, + "angularVelocity": 1.3986178319328149, + "velocityX": 1.0265336112042236, + "velocityY": -1.6767034131340948, + "timestamp": 7.042089676558765 + }, + { + "x": 2.619215453459605, + "y": 2.98196209624742, + "heading": -0.22869362474666396, + "angularVelocity": 1.5078490513829745, + "velocityX": 1.1587379390866084, + "velocityY": -1.858386699003879, + "timestamp": 7.102279402243715 + }, + { + "x": 2.6975187688111633, + "y": 2.859476881700614, + "heading": -0.13286180386406954, + "angularVelocity": 1.592162446198965, + "velocityX": 1.3009415553980122, + "velocityY": -2.0349854257174798, + "timestamp": 7.162469127928665 + }, + { + "x": 2.7852562883718943, + "y": 2.726945932671376, + "heading": -0.034753273861128454, + "angularVelocity": 1.6299879902505128, + "velocityX": 1.4576826619874292, + "velocityY": -2.201886576505485, + "timestamp": 7.2226588536136145 + }, + { + "x": 2.883617018333608, + "y": 2.5856822162797477, + "heading": 0.06025595074389153, + "angularVelocity": 1.5784957237108086, + "velocityX": 1.6341780734566165, + "velocityY": -2.3469739192871995, + "timestamp": 7.282848579298564 + }, + { + "x": 2.993618091797967, + "y": 2.439544726598588, + "heading": 0.14260766445208392, + "angularVelocity": 1.3682021768838868, + "velocityX": 1.8275722677344666, + "velocityY": -2.427947428205354, + "timestamp": 7.343038304983514 + }, + { + "x": 3.1145276346410644, + "y": 2.293581984129882, + "heading": 0.20384383077417081, + "angularVelocity": 1.017385702048459, + "velocityX": 2.008807009288801, + "velocityY": -2.425044155089132, + "timestamp": 7.403228030668464 + }, + { + "x": 3.2468175816396365, + "y": 2.1505705427004553, + "heading": 0.24757858696307516, + "angularVelocity": 0.7266149777426218, + "velocityX": 2.197882537146209, + "velocityY": -2.3760108523834984, + "timestamp": 7.463417756353413 + }, + { + "x": 3.390280853000983, + "y": 2.013955912336661, + "heading": 0.2801226137384155, + "angularVelocity": 0.5406907309344624, + "velocityX": 2.3835176141568395, + "velocityY": -2.269733394016016, + "timestamp": 7.523607482038363 + }, + { + "x": 3.544035674658465, + "y": 1.8860535950620219, + "heading": 0.3061151888137327, + "angularVelocity": 0.43184405277687576, + "velocityX": 2.55450278112711, + "velocityY": -2.1249858812136204, + "timestamp": 7.583797207723313 + }, + { + "x": 3.7070451600522367, + "y": 1.7682529006317282, + "heading": 0.3281829520976596, + "angularVelocity": 0.36663671470170595, + "velocityX": 2.7082609787426026, + "velocityY": -1.9571561938476998, + "timestamp": 7.643986933408263 + }, + { + "x": 3.8782577243031593, + "y": 1.6615213018872745, + "heading": 0.34797198576359334, + "angularVelocity": 0.328777601837151, + "velocityX": 2.844548007198066, + "velocityY": -1.7732527857514622, + "timestamp": 7.704176659093212 + }, + { + "x": 4.056668173911873, + "y": 1.5665668328609006, + "heading": 0.3664586508169338, + "angularVelocity": 0.30713987882425087, + "velocityX": 2.964134618964106, + "velocityY": -1.5775860073427241, + "timestamp": 7.764366384778162 + }, + { + "x": 4.241342350555325, + "y": 1.4839195959237403, + "heading": 0.38411092010286374, + "angularVelocity": 0.2932771180637541, + "velocityX": 3.0682010017804235, + "velocityY": -1.3731120385854438, + "timestamp": 7.824556110463112 + }, + { + "x": 4.431417064671585, + "y": 1.4139831463989663, + "heading": 0.40101516097721396, + "angularVelocity": 0.2808492758852529, + "velocityX": 3.1579262399560672, + "velocityY": -1.1619333487386434, + "timestamp": 7.884745836148062 + }, + { + "x": 4.626090049743652, + "y": 1.3570671081542969, + "heading": 0.41697492722531376, + "angularVelocity": 0.26515765052058465, + "velocityX": 3.2343225169531657, + "velocityY": -0.9456105273279376, + "timestamp": 7.944935561833011 + }, + { + "x": 4.771095590246596, + "y": 1.3224217071642599, + "heading": 0.4280403505516019, + "angularVelocity": 0.2504578977864778, + "velocityX": 3.2820961088293013, + "velocityY": -0.7841737314576779, + "timestamp": 7.989116334026994 + }, + { + "x": 4.917956708237317, + "y": 1.2949736641198544, + "heading": 0.43814957517205777, + "angularVelocity": 0.22881502786030425, + "velocityX": 3.3240957705742336, + "velocityY": -0.6212667113171019, + "timestamp": 8.033297106220976 + }, + { + "x": 5.066406183363406, + "y": 1.2747799857702726, + "heading": 0.4470279058701966, + "angularVelocity": 0.20095462929342126, + "velocityX": 3.3600470918502436, + "velocityY": -0.45706938441270734, + "timestamp": 8.077477878414959 + }, + { + "x": 5.21616079891639, + "y": 1.2618903010738838, + "heading": 0.45444388098508137, + "angularVelocity": 0.16785526251835975, + "velocityX": 3.389588006643782, + "velocityY": -0.2917487417330471, + "timestamp": 8.121658650608941 + }, + { + "x": 5.366915459547734, + "y": 1.25634723736205, + "heading": 0.46022561365529296, + "angularVelocity": 0.13086536027088905, + "velocityX": 3.4122233076740254, + "velocityY": -0.12546326006925063, + "timestamp": 8.165839422802923 + }, + { + "x": 5.518334246127743, + "y": 1.2581861921582334, + "heading": 0.4642859914379861, + "angularVelocity": 0.09190373053837578, + "velocityX": 3.427255320825579, + "velocityY": 0.041623419077170896, + "timestamp": 8.210020194996906 + }, + { + "x": 5.670036777042452, + "y": 1.2674335281135962, + "heading": 0.46666317405037006, + "angularVelocity": 0.05380581855714505, + "velocityX": 3.4336776697481812, + "velocityY": 0.20930679787037068, + "timestamp": 8.254200967190888 + }, + { + "x": 5.821577538212164, + "y": 1.284100828351401, + "heading": 0.4675873844825302, + "angularVelocity": 0.020918838360320994, + "velocityX": 3.4300161279288752, + "velocityY": 0.3772523523270378, + "timestamp": 8.29838173938487 + }, + { + "x": 5.972415606183977, + "y": 1.3081694677842932, + "heading": 0.467591351779606, + "angularVelocity": 0.00008979691569104718, + "velocityX": 3.414111172831806, + "velocityY": 0.544776341328194, + "timestamp": 8.342562511578853 + }, + { + "x": 6.121903298527153, + "y": 1.3395574928087577, + "heading": 0.46759157023044867, + "angularVelocity": 0.000004944477695120892, + "velocityX": 3.3835463917839124, + "velocityY": 0.7104453694618647, + "timestamp": 8.386743283772836 + }, + { + "x": 6.269684547080798, + "y": 1.3781901650520143, + "heading": 0.46759176450370754, + "angularVelocity": 0.0000043972354751028654, + "velocityX": 3.3449222640289906, + "velocityY": 0.8744227482859289, + "timestamp": 8.430924055966818 + }, + { + "x": 6.415408472766336, + "y": 1.423975753200933, + "heading": 0.4675919413300162, + "angularVelocity": 0.000004002336308335113, + "velocityX": 3.2983562407129146, + "velocityY": 1.036323854818335, + "timestamp": 8.4751048281608 + }, + { + "x": 6.5587290836787195, + "y": 1.476805540848842, + "heading": 0.4675921046539949, + "angularVelocity": 0.0000036967207824585226, + "velocityX": 3.243958939493236, + "velocityY": 1.1957642436839244, + "timestamp": 8.519285600354783 + }, + { + "x": 6.6993060573570675, + "y": 1.5365540678009835, + "heading": 0.46759241978399874, + "angularVelocity": 0.000007132740968196116, + "velocityX": 3.1818586841606873, + "velocityY": 1.352364931282918, + "timestamp": 8.563466372548765 + }, + { + "x": 6.836206251960683, + "y": 1.6027005176579716, + "heading": 0.47022361687585157, + "angularVelocity": 0.059555253590862826, + "velocityX": 3.0986374344598233, + "velocityY": 1.4971773142977605, + "timestamp": 8.607647144742748 + }, + { + "x": 6.968107168767895, + "y": 1.6730618727776314, + "heading": 0.48242555808722076, + "angularVelocity": 0.27618216263388745, + "velocityX": 2.9854823774487342, + "velocityY": 1.592578663196003, + "timestamp": 8.65182791693673 + }, + { + "x": 7.094292994493853, + "y": 1.7449092475770878, + "heading": 0.5050066138251464, + "angularVelocity": 0.5111059543907503, + "velocityX": 2.8561254016095394, + "velocityY": 1.6262136497750488, + "timestamp": 8.696008689130712 + }, + { + "x": 7.213566146886095, + "y": 1.8140035101921348, + "heading": 0.5272178489693323, + "angularVelocity": 0.5027353312582225, + "velocityX": 2.699662012890034, + "velocityY": 1.5638989357559931, + "timestamp": 8.740189461324695 + }, + { + "x": 7.326317415042033, + "y": 1.87970672983894, + "heading": 0.5469886414915726, + "angularVelocity": 0.44749766788668743, + "velocityX": 2.552043854301269, + "velocityY": 1.4871451172995578, + "timestamp": 8.784370233518677 + }, + { + "x": 7.432769298553467, + "y": 1.941792130470276, + "heading": 0.5633166974965059, + "angularVelocity": 0.3695738031296167, + "velocityX": 2.409461813932093, + "velocityY": 1.405258386130975, + "timestamp": 8.82855100571266 + }, + { + "x": 7.579701459580911, + "y": 2.027179132105106, + "heading": 0.5787605568170214, + "angularVelocity": 0.23108320735340232, + "velocityX": 2.198514913204677, + "velocityY": 1.2776276832472448, + "timestamp": 8.895383466181745 + }, + { + "x": 7.712165331896003, + "y": 2.1039708729412734, + "heading": 0.5887351557050889, + "angularVelocity": 0.14924781787259772, + "velocityX": 1.9820289629523165, + "velocityY": 1.1490186100763133, + "timestamp": 8.96221592665083 + }, + { + "x": 7.830026910442582, + "y": 2.172172632129026, + "heading": 0.5950957258261598, + "angularVelocity": 0.09517186822731284, + "velocityX": 1.7635379233284407, + "velocityY": 1.0204885277162767, + "timestamp": 9.029048387119914 + }, + { + "x": 7.933222174835435, + "y": 2.231800820984951, + "heading": 0.5989654042474855, + "angularVelocity": 0.05790118146429404, + "velocityX": 1.54408896019307, + "velocityY": 0.8922040044224955, + "timestamp": 9.095880847589 + }, + { + "x": 8.021715394731531, + "y": 2.28287256797719, + "heading": 0.6011062262493081, + "angularVelocity": 0.032032667760495764, + "velocityX": 1.3241053714763598, + "velocityY": 0.7641757707822818, + "timestamp": 9.162713308058084 + }, + { + "x": 8.095484569673351, + "y": 2.3254034695687893, + "heading": 0.6020727831826357, + "angularVelocity": 0.014462387386960431, + "velocityX": 1.103792594557322, + "velocityY": 0.6363809037267617, + "timestamp": 9.229545768527169 + }, + { + "x": 8.154515205691396, + "y": 2.359407222237499, + "heading": 0.6022883195935687, + "angularVelocity": 0.003225025824578962, + "velocityX": 0.8832629474318238, + "velocityY": 0.5087909741769772, + "timestamp": 9.296378228996254 + }, + { + "x": 8.198797270954351, + "y": 2.3848957424596233, + "heading": 0.6020871548762373, + "angularVelocity": -0.00300998520658142, + "velocityX": 0.6625831961317433, + "velocityY": 0.38137934834697124, + "timestamp": 9.363210689465339 + }, + { + "x": 8.22832355571237, + "y": 2.401879399345903, + "heading": 0.6017403922656039, + "angularVelocity": -0.005188535753427844, + "velocityX": 0.44179556686643673, + "velocityY": 0.2541228733324269, + "timestamp": 9.430043149934423 + }, + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 0.601472494720935, + "angularVelocity": -0.004008494417063559, + "velocityX": 0.220928070177263, + "velocityY": 0.12700192446944059, + "timestamp": 9.496875610403508 + }, + { + "x": 8.243088722229004, + "y": 2.410367250442505, + "heading": 0.601472494720935, + "angularVelocity": 2.0480668095758719e-22, + "velocityX": 1.6959236187935907e-23, + "velocityY": -2.878465592599261e-23, + "timestamp": 9.563708070872593 + }, + { + "x": 8.225716112229817, + "y": 2.403169142964958, + "heading": 0.6014724848808553, + "angularVelocity": -1.401208853680834e-7, + "velocityX": -0.24738269796567666, + "velocityY": -0.10249969625322676, + "timestamp": 9.633933717783428 + }, + { + "x": 8.190970892472077, + "y": 2.3887729281095678, + "heading": 0.6014724652007141, + "angularVelocity": -2.802415057039817e-7, + "velocityX": -0.49476539250476237, + "velocityY": -0.20499939108669155, + "timestamp": 9.704159364694263 + }, + { + "x": 8.138853063244545, + "y": 2.3671786059959783, + "heading": 0.6014724356805342, + "angularVelocity": -4.203618073354203e-7, + "velocityX": -0.7421480829319401, + "velocityY": -0.30749908421644245, + "timestamp": 9.774385011605098 + }, + { + "x": 8.069362624900151, + "y": 2.3383861767704226, + "heading": 0.6014723963203429, + "angularVelocity": -5.604817189302354e-7, + "velocityX": -0.9895307683334543, + "velocityY": -0.40999877526387724, + "timestamp": 9.844610658515933 + }, + { + "x": 7.982499577880061, + "y": 2.3023956406156896, + "heading": 0.6014723471201746, + "angularVelocity": -7.006011425140131e-7, + "velocityX": -1.2369134474528922, + "velocityY": -0.5124984637084178, + "timestamp": 9.914836305426768 + }, + { + "x": 7.878263922751481, + "y": 2.259206997766796, + "heading": 0.601472288080073, + "angularVelocity": -8.407199383581404e-7, + "velocityX": -1.484296118495379, + "velocityY": -0.6149981488063821, + "timestamp": 9.985061952337603 + }, + { + "x": 7.756655660270691, + "y": 2.2088202485370934, + "heading": 0.6014722192000971, + "angularVelocity": -9.808378971745156e-7, + "velocityX": -1.7316787787686057, + "velocityY": -0.7174978294422485, + "timestamp": 10.055287599248437 + }, + { + "x": 7.617674791496479, + "y": 2.1512353933652784, + "heading": 0.6014721404803293, + "angularVelocity": -0.0000011209546846231955, + "velocityX": -1.979061423964885, + "velocityY": -0.8199975038311836, + "timestamp": 10.125513246159272 + }, + { + "x": 7.461321318017023, + "y": 2.0864524329093923, + "heading": 0.6014720519208926, + "angularVelocity": -0.0000012610697164822291, + "velocityX": -2.226444046545777, + "velocityY": -0.9224971688497368, + "timestamp": 10.195738893070107 + }, + { + "x": 7.287595242479287, + "y": 2.014471368266166, + "heading": 0.6014719535219923, + "angularVelocity": -0.0000014011818267188707, + "velocityX": -2.4738266314344535, + "velocityY": -1.0249968182510263, + "timestamp": 10.265964539980942 + }, + { + "x": 7.096496570177163, + "y": 1.935292201629055, + "heading": 0.6014718452840377, + "angularVelocity": -0.0000015412881098024927, + "velocityX": -2.7212091409390613, + "velocityY": -1.1274964364179387, + "timestamp": 10.336190186891777 + }, + { + "x": 6.8880253169921275, + "y": 1.8489149395783349, + "heading": 0.601471727208249, + "angularVelocity": -0.0000016813770192107404, + "velocityX": -2.9685914242944573, + "velocityY": -1.2299959608829605, + "timestamp": 10.406415833802612 + }, + { + "x": 6.663723389677425, + "y": 1.7559784504804536, + "heading": 0.6014717272081879, + "angularVelocity": -8.702352256817885e-13, + "velocityX": -3.1940172455726326, + "velocityY": -1.3233981200042444, + "timestamp": 10.476641480713447 + }, + { + "x": 6.439421272188231, + "y": 1.6630424203676835, + "heading": 0.6014717272081268, + "angularVelocity": -8.703596574297939e-13, + "velocityX": -3.1940199536216154, + "velocityY": -1.3233915841426174, + "timestamp": 10.546867127624282 + }, + { + "x": 6.212032631338727, + "y": 1.577936888330409, + "heading": 0.6014717272080651, + "angularVelocity": -8.787789669679053e-13, + "velocityX": -3.237971465584607, + "velocityY": -1.2118867647503735, + "timestamp": 10.617092774535116 + }, + { + "x": 5.978739653450143, + "y": 1.510684142319498, + "heading": 0.6014717272080008, + "angularVelocity": -9.15623540134239e-13, + "velocityX": -3.32204811419955, + "velocityY": -0.9576664504963734, + "timestamp": 10.687318421445951 + }, + { + "x": 5.7409415584912775, + "y": 1.4616883276374903, + "heading": 0.601471727201828, + "angularVelocity": -8.790086201060278e-11, + "velocityX": -3.386200133703241, + "velocityY": -0.6976911831686963, + "timestamp": 10.757544068356786 + }, + { + "x": 5.516819527735436, + "y": 1.4255992428677315, + "heading": 0.6014714661772929, + "angularVelocity": -0.000003716940268980297, + "velocityX": -3.1914555524195745, + "velocityY": -0.513901777445793, + "timestamp": 10.827769715267621 + }, + { + "x": 5.308884526984856, + "y": 1.3990809310343755, + "heading": 0.6014711092836408, + "angularVelocity": -0.0000050820984614807965, + "velocityX": -2.9609552905164764, + "velocityY": -0.3776157714435294, + "timestamp": 10.897995362178456 + }, + { + "x": 5.1176297142242815, + "y": 1.3812454338123161, + "heading": 0.6014706963446639, + "angularVelocity": -0.000005880173340714845, + "velocityX": -2.7234325516916758, + "velocityY": -0.253974124933375, + "timestamp": 10.96822100908929 + }, + { + "x": 4.943259966846924, + "y": 1.3716872939772524, + "heading": 0.6014702453730658, + "angularVelocity": -0.000006421750712363147, + "velocityX": -2.482992397332454, + "velocityY": -0.13610611301593797, + "timestamp": 11.038446656000126 + }, + { + "x": 4.785886935464025, + "y": 1.3701747617379714, + "heading": 0.6014697666441067, + "angularVelocity": -0.00000681701031230746, + "velocityX": -2.240962359274736, + "velocityY": -0.021538174524779245, + "timestamp": 11.10867230291096 + }, + { + "x": 4.645580768585205, + "y": 1.3765579462051392, + "heading": 0.6014692668007987, + "angularVelocity": -0.000007117674667444465, + "velocityX": -1.997933419637491, + "velocityY": 0.09089534590222227, + "timestamp": 11.178897949821796 + }, + { + "x": 4.4852918274531035, + "y": 1.4093136530981196, + "heading": 0.6014686169682122, + "angularVelocity": -0.00000794408595037517, + "velocityX": -1.9595033424539408, + "velocityY": 0.400432598081353, + "timestamp": 11.26069874988135 + }, + { + "x": 4.332045122157961, + "y": 1.4665930457792093, + "heading": 0.6014678756495806, + "angularVelocity": -0.000009062486322055742, + "velocityX": -1.8734132818208915, + "velocityY": 0.7002302256137938, + "timestamp": 11.342499549940904 + }, + { + "x": 4.189567990618789, + "y": 1.5470029255462814, + "heading": 0.6014669872102968, + "angularVelocity": -0.00001086100971182791, + "velocityX": -1.7417571886270768, + "velocityY": 0.9829962507522065, + "timestamp": 11.424300350000458 + }, + { + "x": 4.061325777655998, + "y": 1.6485874554715532, + "heading": 0.6014657443656565, + "angularVelocity": -0.00001519355115434508, + "velocityX": -1.567737881162854, + "velocityY": 1.2418525228520432, + "timestamp": 11.506101150060012 + }, + { + "x": 3.9499181722494128, + "y": 1.7683949151806115, + "heading": 0.5999046298375316, + "angularVelocity": -0.019084343025843265, + "velocityX": -1.3619378456625928, + "velocityY": 1.4646245467261332, + "timestamp": 11.587901950119566 + }, + { + "x": 3.8385139180743773, + "y": 1.888205491074706, + "heading": 0.5540381787699103, + "angularVelocity": -0.5607090766132972, + "velocityX": -1.361896877462441, + "velocityY": 1.4646626415251267, + "timestamp": 11.66970275017912 + }, + { + "x": 3.7271096639847774, + "y": 2.0080160670482403, + "heading": 0.4697368355238526, + "angularVelocity": -1.0305686886275442, + "velocityX": -1.361896876418011, + "velocityY": 1.4646626424962754, + "timestamp": 11.751503550238674 + }, + { + "x": 3.6157054099517287, + "y": 2.1278266430743598, + "heading": 0.3537615125967293, + "angularVelocity": -1.4177773670023999, + "velocityX": -1.3618968757266772, + "velocityY": 1.4646626431391025, + "timestamp": 11.833304350298228 + }, + { + "x": 3.504301155997762, + "y": 2.247637219174011, + "heading": 0.2137130812750941, + "angularVelocity": -1.7120667673137158, + "velocityX": -1.3618968747599156, + "velocityY": 1.4646626440380324, + "timestamp": 11.915105150357782 + }, + { + "x": 3.3928969021094617, + "y": 2.367447795334722, + "heading": 0.057794482937139194, + "angularVelocity": -1.9060766914802987, + "velocityX": -1.3618968739571615, + "velocityY": 1.4646626447844622, + "timestamp": 11.996905950417336 + }, + { + "x": 3.281492648234104, + "y": 2.487258371507466, + "heading": -0.105452505167902, + "angularVelocity": -1.995664932203507, + "velocityX": -1.3618968737989476, + "velocityY": 1.464662644931575, + "timestamp": 12.07870675047689 + }, + { + "x": 3.1700883943158846, + "y": 2.6070689476403572, + "heading": -0.26730664929832215, + "angularVelocity": -1.978637666289151, + "velocityX": -1.3618968743229085, + "velocityY": 1.4646626444443769, + "timestamp": 12.160507550536444 + }, + { + "x": 3.058684140329769, + "y": 2.7268795237101155, + "heading": -0.41916927419219197, + "angularVelocity": -1.8564931490072147, + "velocityX": -1.3618968751529306, + "velocityY": 1.4646626436725918, + "timestamp": 12.242308350595998 + }, + { + "x": 2.947279886280209, + "y": 2.846690099720882, + "heading": -0.5526625850121627, + "angularVelocity": -1.6319316036369287, + "velocityX": -1.3618968759285288, + "velocityY": 1.464662642951413, + "timestamp": 12.324109150655552 + }, + { + "x": 2.835875632186779, + "y": 2.966500675690855, + "heading": -0.65985673738226, + "angularVelocity": -1.3104291436276523, + "velocityX": -1.3618968764648283, + "velocityY": 1.464662642452743, + "timestamp": 12.405909950715106 + }, + { + "x": 2.7244713780717897, + "y": 3.086311251640782, + "heading": -0.7334716118848168, + "angularVelocity": -0.899928539194763, + "velocityX": -1.3618968767283959, + "velocityY": 1.4646626422076685, + "timestamp": 12.48771075077466 + }, + { + "x": 2.613067124142982, + "y": 3.2061218277638193, + "heading": -0.7669996227711985, + "angularVelocity": -0.4098738748517434, + "velocityX": -1.361896874452356, + "velocityY": 1.4646626443239155, + "timestamp": 12.569511550834214 + }, + { + "x": 2.5088215468762853, + "y": 3.3182335485748182, + "heading": -0.7669997571492978, + "angularVelocity": -0.0000016427479828891154, + "velocityX": -1.2743833457717078, + "velocityY": 1.3705455292537272, + "timestamp": 12.651312350893768 + }, + { + "x": 2.421950224540146, + "y": 3.411659991166572, + "heading": -0.7669998477403924, + "angularVelocity": -0.0000011074597615269072, + "velocityX": -1.06198621862983, + "velocityY": 1.1421213792986926, + "timestamp": 12.733113150953322 + }, + { + "x": 2.352453163850527, + "y": 3.4864011482861903, + "heading": -0.7669999126210906, + "angularVelocity": -7.931548110988282e-7, + "velocityX": -0.8495890093864996, + "velocityY": 0.9136971406783939, + "timestamp": 12.814913951012876 + }, + { + "x": 2.300330367046085, + "y": 3.5424570175160435, + "heading": -0.7669999578169298, + "angularVelocity": -5.525109684504957e-7, + "velocityX": -0.6371917727760005, + "velocityY": 0.6852728725029872, + "timestamp": 12.89671475107243 + }, + { + "x": 2.2655818352461483, + "y": 3.5798275976473137, + "heading": -0.7669999863406792, + "angularVelocity": -3.486976819494124e-7, + "velocityX": -0.4247945224819143, + "velocityY": 0.4568485895500231, + "timestamp": 12.978515551131984 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -1.6698272956598662e-7, + "velocityX": -0.21239726397767503, + "velocityY": 0.2284242977305235, + "timestamp": 13.060316351191538 + }, + { + "x": 2.2482075691223145, + "y": 3.598512887954712, + "heading": -0.767, + "angularVelocity": -2.381764007243456e-23, + "velocityX": 2.799365906238757e-23, + "velocityY": -7.174872988733629e-24, + "timestamp": 13.142117151251092 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/6Piece.1.traj b/src/main/deploy/choreo/6Piece.1.traj new file mode 100644 index 0000000..507160d --- /dev/null +++ b/src/main/deploy/choreo/6Piece.1.traj @@ -0,0 +1,113 @@ +{ + "samples": [ + { + "x": 1.328821897506714, + "y": 5.58574914932251, + "heading": 4.3539389552577884e-26, + "angularVelocity": 5.8461926746164104e-27, + "velocityX": -1.1704555712816255e-26, + "velocityY": -1.2477760568468226e-26, + "timestamp": 0 + }, + { + "x": 1.374949606535372, + "y": 5.540487462134801, + "heading": -0.013194729598639283, + "angularVelocity": -0.15905904586136208, + "velocityX": 0.5560575782186153, + "velocityY": -0.5456179093578979, + "timestamp": 0.08295491480654284 + }, + { + "x": 1.46721327852146, + "y": 5.449968559727431, + "heading": -0.03947645751575262, + "angularVelocity": -0.316819419059188, + "velocityX": 1.1122146554095553, + "velocityY": -1.0911819102999225, + "timestamp": 0.16590982961308567 + }, + { + "x": 1.605638421023178, + "y": 5.314200511590273, + "heading": -0.078646870972475, + "angularVelocity": -0.4721891831010949, + "velocityX": 1.6686792196042406, + "velocityY": -1.6366486356327254, + "timestamp": 0.24886474441962853 + }, + { + "x": 1.7902678160557024, + "y": 5.133199369169571, + "heading": -0.13043925037876178, + "angularVelocity": -0.6243437116061242, + "velocityX": 2.225659509904795, + "velocityY": -2.181921864940855, + "timestamp": 0.33181965922617135 + }, + { + "x": 1.9998217979277588, + "y": 4.932195127760977, + "heading": -0.13043932359342048, + "angularVelocity": -8.825837368571486e-7, + "velocityX": 2.5261189449805634, + "velocityY": -2.423054039382124, + "timestamp": 0.41477457403271417 + }, + { + "x": 2.2093757892592514, + "y": 4.731190896214115, + "heading": -0.13043939680794175, + "angularVelocity": -8.825820799374822e-7, + "velocityX": 2.5261190590116103, + "velocityY": -2.4230539205014976, + "timestamp": 0.497729488839257 + }, + { + "x": 2.3945844486540637, + "y": 4.550478243745755, + "heading": -0.17901854361361771, + "angularVelocity": -0.5856090253237706, + "velocityX": 2.232642391674238, + "velocityY": -2.1784441933283367, + "timestamp": 0.5806844036457999 + }, + { + "x": 2.533512254873414, + "y": 4.414958500153421, + "heading": -0.21536260429634638, + "angularVelocity": -0.4381182328676455, + "velocityX": 1.6747387004536223, + "velocityY": -1.6336553886936764, + "timestamp": 0.6636393184523427 + }, + { + "x": 2.6261356849469077, + "y": 4.324618150485375, + "heading": -0.23957423083578933, + "angularVelocity": -0.2918648834238011, + "velocityX": 1.1165514459209405, + "velocityY": -1.0890295032998, + "timestamp": 0.7465942332588855 + }, + { + "x": 2.672445297241211, + "y": 4.279448986053467, + "heading": -0.2516907201225374, + "angularVelocity": -0.14606113833043707, + "velocityX": 0.5582503749452395, + "velocityY": -0.5445025715142554, + "timestamp": 0.8295491480654283 + }, + { + "x": 2.672445297241211, + "y": 4.279448986053467, + "heading": -0.2516907201225374, + "angularVelocity": 1.3508666315673642e-26, + "velocityX": -7.696322556768917e-27, + "velocityY": -1.1161944739972978e-26, + "timestamp": 0.9125040628719712 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/6Piece.2.traj b/src/main/deploy/choreo/6Piece.2.traj new file mode 100644 index 0000000..0f8eae6 --- /dev/null +++ b/src/main/deploy/choreo/6Piece.2.traj @@ -0,0 +1,167 @@ +{ + "samples": [ + { + "x": 2.672445297241211, + "y": 4.279448986053467, + "heading": -0.2516907201225374, + "angularVelocity": 1.3508666315673642e-26, + "velocityX": -7.696322556768917e-27, + "velocityY": -1.1161944739972978e-26, + "timestamp": 0 + }, + { + "x": 2.625196176417341, + "y": 4.293300959249136, + "heading": -0.2476240648671981, + "angularVelocity": 0.05660704046380333, + "velocityX": -0.6576984589102508, + "velocityY": 0.19281673954566436, + "timestamp": 0.07184009660317214 + }, + { + "x": 2.5313898742849363, + "y": 4.322902155034929, + "heading": -0.238711790545806, + "angularVelocity": 0.12405710380125745, + "velocityX": -1.3057652560041622, + "velocityY": 0.4120428171095463, + "timestamp": 0.1436801932063443 + }, + { + "x": 2.3926976278493437, + "y": 4.3719573505978495, + "heading": -0.22331799219109705, + "angularVelocity": 0.21427864218697643, + "velocityX": -1.9305687630362727, + "velocityY": 0.6828386636767781, + "timestamp": 0.21552028980951654 + }, + { + "x": 2.215900012214516, + "y": 4.450150810574297, + "heading": -0.19663428050241719, + "angularVelocity": 0.3714320129060295, + "velocityX": -2.460987999660081, + "velocityY": 1.0884375672317064, + "timestamp": 0.2873603864126888 + }, + { + "x": 2.052047257943267, + "y": 4.568466965487968, + "heading": -0.1386728664270459, + "angularVelocity": 0.8068114718099074, + "velocityX": -2.2807980782143584, + "velocityY": 1.6469375809336866, + "timestamp": 0.35920048301586105 + }, + { + "x": 1.935784479955621, + "y": 4.6986149037226514, + "heading": -0.07489980420839401, + "angularVelocity": 0.8877084696993005, + "velocityX": -1.6183549784162223, + "velocityY": 1.811633675182685, + "timestamp": 0.4310405796190333 + }, + { + "x": 1.8688424514197695, + "y": 4.831515653648781, + "heading": -0.009029725121146852, + "angularVelocity": 0.9168985316250061, + "velocityX": -0.9318198568916667, + "velocityY": 1.8499522719219348, + "timestamp": 0.5028806762222056 + }, + { + "x": 1.8513420820236208, + "y": 4.9637017250061035, + "heading": 0.057668349012968446, + "angularVelocity": 0.928424059652088, + "velocityX": -0.2436016963175404, + "velocityY": 1.84000408695839, + "timestamp": 0.5747207728253778 + }, + { + "x": 1.8881504770913347, + "y": 5.100791187409367, + "heading": 0.12845802675955434, + "angularVelocity": 0.9309881682233778, + "velocityX": 0.48408442290141646, + "velocityY": 1.802927652000758, + "timestamp": 0.650757912890944 + }, + { + "x": 1.9801594635213668, + "y": 5.23317628018901, + "heading": 0.19875878141335546, + "angularVelocity": 0.9245581119066463, + "velocityX": 1.2100532233418253, + "velocityY": 1.7410582863254496, + "timestamp": 0.7267950529565101 + }, + { + "x": 2.1268970903680344, + "y": 5.357201962866711, + "heading": 0.26712068314763787, + "angularVelocity": 0.8990593501456591, + "velocityX": 1.9298151761117934, + "velocityY": 1.6311197734522118, + "timestamp": 0.8028321930220763 + }, + { + "x": 2.3255858491677373, + "y": 5.4631354448246725, + "heading": 0.32902091884053974, + "angularVelocity": 0.8140789571980984, + "velocityX": 2.6130488157284057, + "velocityY": 1.393180778059456, + "timestamp": 0.8788693330876425 + }, + { + "x": 2.5347759563760905, + "y": 5.522688608468802, + "heading": 0.3564296694092298, + "angularVelocity": 0.3604653008392443, + "velocityX": 2.7511569612951026, + "velocityY": 0.7832115146989626, + "timestamp": 0.9549064731532086 + }, + { + "x": 2.696139551385315, + "y": 5.557119984012087, + "heading": 0.37271335067397476, + "angularVelocity": 0.21415430999513735, + "velocityX": 2.122168125603903, + "velocityY": 0.4528231271401756, + "timestamp": 1.0309436132187748 + }, + { + "x": 2.8046075058969566, + "y": 5.576857728360174, + "heading": 0.38238828101753003, + "angularVelocity": 0.12723953498530663, + "velocityX": 1.4265128122666186, + "velocityY": 0.25958030945230076, + "timestamp": 1.106980753284341 + }, + { + "x": 2.8590595722198486, + "y": 5.58574914932251, + "heading": 0.38687655192878945, + "angularVelocity": 0.05902735041572054, + "velocityX": 0.7161245974787916, + "velocityY": 0.11693523657870776, + "timestamp": 1.1830178933499071 + }, + { + "x": 2.8590595722198486, + "y": 5.58574914932251, + "heading": 0.38687655192878945, + "angularVelocity": -4.595788384330154e-29, + "velocityX": 7.170077328456974e-29, + "velocityY": 4.9006221400778145e-30, + "timestamp": 1.2590550334154733 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/6Piece.3.traj b/src/main/deploy/choreo/6Piece.3.traj new file mode 100644 index 0000000..32d1c88 --- /dev/null +++ b/src/main/deploy/choreo/6Piece.3.traj @@ -0,0 +1,194 @@ +{ + "samples": [ + { + "x": 2.8590595722198486, + "y": 5.58574914932251, + "heading": 0.38687655192878945, + "angularVelocity": -4.595788384330154e-29, + "velocityX": 7.170077328456974e-29, + "velocityY": 4.9006221400778145e-30, + "timestamp": 0 + }, + { + "x": 2.821439157373947, + "y": 5.594030750372382, + "heading": 0.387080960332372, + "angularVelocity": 0.0032239487618478326, + "velocityX": -0.5933527572101648, + "velocityY": 0.1306181985813967, + "timestamp": 0.063403117940811 + }, + { + "x": 2.7464223945042074, + "y": 5.611552554364111, + "heading": 0.38751784243776727, + "angularVelocity": 0.006890546073824077, + "velocityX": -1.1831715112144825, + "velocityY": 0.27635555727852257, + "timestamp": 0.126806235881622 + }, + { + "x": 2.6344378134858273, + "y": 5.639901380380037, + "heading": 0.38823547094203587, + "angularVelocity": 0.011318504950159194, + "velocityX": -1.7662314513131911, + "velocityY": 0.44712037730369514, + "timestamp": 0.19020935382243298 + }, + { + "x": 2.48653995660996, + "y": 5.682179259469134, + "heading": 0.3893349048666279, + "angularVelocity": 0.01734037631427531, + "velocityX": -2.332659050205305, + "velocityY": 0.6668107257527173, + "timestamp": 0.253612471763244 + }, + { + "x": 2.307136094114684, + "y": 5.746612910577359, + "heading": 0.39112800963911476, + "angularVelocity": 0.02828101883192509, + "velocityX": -2.829574764174154, + "velocityY": 1.0162536670259112, + "timestamp": 0.31701558970405497 + }, + { + "x": 2.1381687976868347, + "y": 5.848112044286255, + "heading": 0.3940456425874033, + "angularVelocity": 0.046017184060447705, + "velocityX": -2.6649682525958127, + "velocityY": 1.600853980140992, + "timestamp": 0.38041870764486596 + }, + { + "x": 2.0057971851389707, + "y": 5.961635826733289, + "heading": 0.39722224472387, + "angularVelocity": 0.05010167070067704, + "velocityX": -2.087777649538265, + "velocityY": 1.7905078824832, + "timestamp": 0.44382182558567695 + }, + { + "x": 1.9116699025918782, + "y": 6.079772759952102, + "heading": 0.40050830041797525, + "angularVelocity": 0.05182798261077437, + "velocityX": -1.4845844432282262, + "velocityY": 1.8632669347444144, + "timestamp": 0.507224943526488 + }, + { + "x": 1.8560250338172737, + "y": 6.199649277805345, + "heading": 0.40384210990951336, + "angularVelocity": 0.052581160040904545, + "velocityX": -0.8776361570506794, + "velocityY": 1.8907038288740154, + "timestamp": 0.5706280614672989 + }, + { + "x": 1.838901162147522, + "y": 6.319766044616699, + "heading": 0.4071898798527544, + "angularVelocity": 0.052801345611525094, + "velocityX": -0.27007933088933367, + "velocityY": 1.8944930582670674, + "timestamp": 0.6340311794081099 + }, + { + "x": 1.8674670617000426, + "y": 6.452466802830068, + "heading": 0.410902560889689, + "angularVelocity": 0.05267198382132873, + "velocityX": 0.4052657861269917, + "velocityY": 1.8826320171752862, + "timestamp": 0.7045180068936392 + }, + { + "x": 1.9435937805094023, + "y": 6.582998572823563, + "heading": 0.4145700789652226, + "angularVelocity": 0.052031254723253276, + "velocityX": 1.080013408533504, + "velocityY": 1.8518604773394354, + "timestamp": 0.7750048343791685 + }, + { + "x": 2.067117864237862, + "y": 6.709034317074673, + "heading": 0.41812673034240355, + "angularVelocity": 0.05045838355983837, + "velocityX": 1.7524420964160776, + "velocityY": 1.7880751446358603, + "timestamp": 0.8454916618646977 + }, + { + "x": 2.2372903513460827, + "y": 6.825552193890142, + "heading": 0.4214177842629497, + "angularVelocity": 0.04669033971236367, + "velocityX": 2.4142452310420213, + "velocityY": 1.6530447031310942, + "timestamp": 0.915978489350227 + }, + { + "x": 2.447332984322481, + "y": 6.9160627740645415, + "heading": 0.423792969899213, + "angularVelocity": 0.03369687246529782, + "velocityX": 2.9798849014664373, + "velocityY": 1.2840779391437487, + "timestamp": 0.9864653168357562 + }, + { + "x": 2.6299484111085176, + "y": 6.9676741214205835, + "heading": 0.4250728750513537, + "angularVelocity": 0.018158075739803856, + "velocityX": 2.590773812646438, + "velocityY": 0.7322126586933964, + "timestamp": 1.0569521443212855 + }, + { + "x": 2.768901501023009, + "y": 7.000310165629845, + "heading": 0.42589586451986794, + "angularVelocity": 0.01167579103603713, + "velocityX": 1.9713341466959602, + "velocityY": 0.46300912345588136, + "timestamp": 1.1274389718068147 + }, + { + "x": 2.862081475468267, + "y": 7.0198533344035186, + "heading": 0.4263947957186992, + "angularVelocity": 0.007078360831798, + "velocityX": 1.3219487636095895, + "velocityY": 0.2772598721042683, + "timestamp": 1.197925799292344 + }, + { + "x": 2.9088234901428223, + "y": 7.028900623321533, + "heading": 0.426627843154362, + "angularVelocity": 0.003306255139807895, + "velocityX": 0.6631312025520166, + "velocityY": 0.12835432152017248, + "timestamp": 1.2684126267778733 + }, + { + "x": 2.9088234901428223, + "y": 7.028900623321533, + "heading": 0.426627843154362, + "angularVelocity": 7.235947497328528e-24, + "velocityX": 3.9627330150979443e-25, + "velocityY": 3.746183212022811e-24, + "timestamp": 1.3388994542634025 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/6Piece.4.traj b/src/main/deploy/choreo/6Piece.4.traj new file mode 100644 index 0000000..ce5dabd --- /dev/null +++ b/src/main/deploy/choreo/6Piece.4.traj @@ -0,0 +1,248 @@ +{ + "samples": [ + { + "x": 2.9088234901428223, + "y": 7.028900623321533, + "heading": 0.426627843154362, + "angularVelocity": 7.235947497328528e-24, + "velocityX": 3.9627330150979443e-25, + "velocityY": 3.746183212022811e-24, + "timestamp": 0 + }, + { + "x": 2.9499714553274323, + "y": 7.035529268833997, + "heading": 0.4162396153486674, + "angularVelocity": -0.15476273818242897, + "velocityX": 0.613018108739834, + "velocityY": 0.09875287191789282, + "timestamp": 0.06712357204128416 + }, + { + "x": 3.032252734431755, + "y": 7.0488035270821205, + "heading": 0.3954328742907609, + "angularVelocity": -0.3099766657994496, + "velocityX": 1.2258179444579598, + "velocityY": 0.19775851976348513, + "timestamp": 0.13424714408256833 + }, + { + "x": 3.155648996542602, + "y": 7.068747158199284, + "heading": 0.3641717265377585, + "angularVelocity": -0.4657253301980919, + "velocityX": 1.8383446881365682, + "velocityY": 0.29711814360681277, + "timestamp": 0.2013707161238525 + }, + { + "x": 3.3201397118502305, + "y": 7.09538869210212, + "heading": 0.3224161177499544, + "angularVelocity": -0.6220707199867489, + "velocityX": 2.4505655808433304, + "velocityY": 0.3969028031829179, + "timestamp": 0.26849428816513665 + }, + { + "x": 3.525704706881839, + "y": 7.128759620860251, + "heading": 0.2701269686194379, + "angularVelocity": -0.7789983092430813, + "velocityX": 3.062485931248957, + "velocityY": 0.4971566283392252, + "timestamp": 0.3356178602064208 + }, + { + "x": 3.736742521528462, + "y": 7.17085251865357, + "heading": 0.1968734668457863, + "angularVelocity": -1.0913230560584186, + "velocityX": 3.1440194290736665, + "velocityY": 0.6270956165358649, + "timestamp": 0.402741432247705 + }, + { + "x": 3.9603913131238833, + "y": 7.208349824687908, + "heading": 0.16611663819424638, + "angularVelocity": -0.45821203663927534, + "velocityX": 3.331896452975815, + "velocityY": 0.5586309681385255, + "timestamp": 0.46986500428898914 + }, + { + "x": 4.186083835227695, + "y": 7.240507088585249, + "heading": 0.139868186298227, + "angularVelocity": -0.39104670829906596, + "velocityX": 3.3623437376813072, + "velocityY": 0.47907557538152645, + "timestamp": 0.5369885763302729 + }, + { + "x": 4.411495404478292, + "y": 7.272714656791408, + "heading": 0.11312371036793661, + "angularVelocity": -0.398436422808984, + "velocityX": 3.3581581312740267, + "velocityY": 0.47982500374605697, + "timestamp": 0.604112148371557 + }, + { + "x": 4.636607862344283, + "y": 7.304977234634002, + "heading": 0.08583019595793309, + "angularVelocity": -0.40661594101721416, + "velocityX": 3.353702000953352, + "velocityY": 0.4806445315924413, + "timestamp": 0.6712357204128412 + }, + { + "x": 4.861400835126187, + "y": 7.33729811854161, + "heading": 0.05792775603075696, + "angularVelocity": -0.4156876500853505, + "velocityX": 3.3489423453752654, + "velocityY": 0.48151316928916743, + "timestamp": 0.7383592924541253 + }, + { + "x": 5.085850174369245, + "y": 7.369680784648805, + "heading": 0.02934541464474294, + "angularVelocity": -0.42581675135576136, + "velocityX": 3.343822928627991, + "velocityY": 0.48243359407748343, + "timestamp": 0.8054828644954095 + }, + { + "x": 5.309927463531494, + "y": 7.402129173278809, + "heading": 5.71560409253603e-24, + "angularVelocity": -0.4371849374567573, + "velocityX": 3.338280165191911, + "velocityY": 0.4834127213916103, + "timestamp": 0.8726064365366937 + }, + { + "x": 5.5975844277501166, + "y": 7.40698430828562, + "heading": -5.977593030842461e-14, + "angularVelocity": -7.27279605332056e-13, + "velocityX": 3.49985424414606, + "velocityY": 0.05907127924278551, + "timestamp": 0.9547975634099815 + }, + { + "x": 5.885241394648691, + "y": 7.411839284508277, + "heading": -6.292590842346059e-14, + "angularVelocity": -3.832503866135791e-14, + "velocityX": 3.4998542767523966, + "velocityY": 0.05906934735354094, + "timestamp": 1.0369886902832692 + }, + { + "x": 6.1728983617447595, + "y": 7.416694249029442, + "heading": -6.528052611785862e-14, + "angularVelocity": -2.864807655005671e-14, + "velocityX": 3.499854279155253, + "velocityY": 0.05906920498425597, + "timestamp": 1.119179817156557 + }, + { + "x": 6.460555329029281, + "y": 7.421549202384699, + "heading": -6.686290101207671e-14, + "angularVelocity": -1.9252381058857556e-14, + "velocityX": 3.4998542814481235, + "velocityY": 0.059069069131291764, + "timestamp": 1.2013709440298448 + }, + { + "x": 6.748212296487426, + "y": 7.426404145452759, + "heading": -6.776532099009745e-14, + "angularVelocity": -1.0979530423221106e-14, + "velocityX": 3.4998542835605484, + "velocityY": 0.05906894396940334, + "timestamp": 1.2835620709031326 + }, + { + "x": 7.035869264100995, + "y": 7.431259079311885, + "heading": -6.813646923115834e-14, + "angularVelocity": -4.515672861335136e-15, + "velocityX": 3.499854285451553, + "velocityY": 0.05906883192648223, + "timestamp": 1.3657531977764203 + }, + { + "x": 7.323526231850984, + "y": 7.436114005088022, + "heading": -6.829397797228803e-14, + "angularVelocity": -1.916371597823108e-15, + "velocityX": 3.4998542871113476, + "velocityY": 0.05906873358265542, + "timestamp": 1.4479443246497081 + }, + { + "x": 7.611183199719373, + "y": 7.440968923848655, + "heading": -6.831359071366292e-14, + "angularVelocity": -2.386235853473579e-16, + "velocityX": 3.4998542885518997, + "velocityY": 0.059068648226685214, + "timestamp": 1.530135451522996 + }, + { + "x": 7.870083631568097, + "y": 7.445338496439803, + "heading": 1.5159787003061443e-11, + "angularVelocity": 1.852766980218185e-10, + "velocityX": 3.1499803165841067, + "velocityY": 0.05316355618148571, + "timestamp": 1.6123265783962837 + }, + { + "x": 8.064258961919318, + "y": 7.448615675130285, + "heading": 1.666159387153368e-11, + "angularVelocity": 1.827212894632211e-11, + "velocityX": 2.3624853160923003, + "velocityY": 0.03987265797601595, + "timestamp": 1.6945177052695715 + }, + { + "x": 8.193709184032379, + "y": 7.450800460403948, + "heading": 8.958511488927219e-12, + "angularVelocity": -9.372158133909199e-11, + "velocityX": 1.5749902335884913, + "velocityY": 0.026581765657390878, + "timestamp": 1.7767088321428592 + }, + { + "x": 8.258434295654297, + "y": 7.451892852783203, + "heading": -7.387785600293283e-27, + "angularVelocity": -1.0899609032904939e-10, + "velocityX": 0.7874951236731913, + "velocityY": 0.013290879694828065, + "timestamp": 1.858899959016147 + }, + { + "x": 8.258434295654297, + "y": 7.451892852783203, + "heading": -3.9312058218717824e-27, + "angularVelocity": 1.1753376253871266e-27, + "velocityX": 1.6594582279288362e-26, + "velocityY": 8.007277829857814e-26, + "timestamp": 1.9410910858894348 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/6Piece.5.traj b/src/main/deploy/choreo/6Piece.5.traj new file mode 100644 index 0000000..5312e5b --- /dev/null +++ b/src/main/deploy/choreo/6Piece.5.traj @@ -0,0 +1,158 @@ +{ + "samples": [ + { + "x": 8.258434295654297, + "y": 7.451892852783203, + "heading": -3.9312058218717824e-27, + "angularVelocity": 1.1753376253871266e-27, + "velocityX": 1.6594582279288362e-26, + "velocityY": 8.007277829857814e-26, + "timestamp": 0 + }, + { + "x": 8.173583810413728, + "y": 7.427578131824658, + "heading": 2.0826763233160825e-10, + "angularVelocity": 2.170043859254894e-9, + "velocityX": -0.884099331181325, + "velocityY": -0.2533471491219265, + "timestamp": 0.09597392764361867 + }, + { + "x": 8.003882842204604, + "y": 7.378948690082569, + "heading": 6.074862710776759e-10, + "angularVelocity": 4.159657195946921e-9, + "velocityX": -1.7681986386894193, + "velocityY": -0.5066942964204343, + "timestamp": 0.19194785528723735 + }, + { + "x": 7.749331397599835, + "y": 7.306004528930369, + "heading": 1.1824464201448697e-9, + "angularVelocity": 5.990795241830689e-9, + "velocityX": -2.652297877711105, + "velocityY": -0.7600414294084665, + "timestamp": 0.287921782930856 + }, + { + "x": 7.426386753090784, + "y": 7.213461633012268, + "heading": 1.1824469628681777e-9, + "angularVelocity": 5.654903592256136e-15, + "velocityX": -3.3649205824757455, + "velocityY": -0.9642503770580384, + "timestamp": 0.3838957105744747 + }, + { + "x": 7.103442110110582, + "y": 7.120918731758949, + "heading": 1.1824474238942088e-9, + "angularVelocity": 4.803659101973182e-15, + "velocityX": -3.364920566545916, + "velocityY": -0.9642504326483329, + "timestamp": 0.47986963821809336 + }, + { + "x": 6.7804974690937145, + "y": 7.0283758236542315, + "heading": 1.1824480406820372e-9, + "angularVelocity": 6.426618597592819e-15, + "velocityX": -3.364920546088964, + "velocityY": -0.9642505040364596, + "timestamp": 0.575843565861712 + }, + { + "x": 6.4575528306495285, + "y": 6.935832906571688, + "heading": 1.1824490782902537e-9, + "angularVelocity": 1.0811355148059625e-14, + "velocityX": -3.3649205192829115, + "velocityY": -0.964250597580873, + "timestamp": 0.6718174935053307 + }, + { + "x": 6.134608195619217, + "y": 6.843289977575833, + "heading": 1.18245090262753e-9, + "angularVelocity": 1.9008675803016132e-14, + "velocityX": -3.364920483712055, + "velocityY": -0.9642507217115844, + "timestamp": 0.7677914211489494 + }, + { + "x": 5.811663565059857, + "y": 6.750747032977816, + "heading": 1.1824542039138914e-9, + "angularVelocity": 3.439774157926315e-14, + "velocityX": -3.36492043712699, + "velocityY": -0.9642508842782657, + "timestamp": 0.863765348792568 + }, + { + "x": 5.4887189399527605, + "y": 6.658204069353181, + "heading": 1.1824584939954223e-9, + "angularVelocity": 4.470048935450349e-14, + "velocityX": -3.36492038031715, + "velocityY": -0.964251082526029, + "timestamp": 0.9597392764361867 + }, + { + "x": 5.165774321115312, + "y": 6.565661083849535, + "heading": 1.1824622621223112e-9, + "angularVelocity": 3.926198480608096e-14, + "velocityX": -3.364920314990583, + "velocityY": -0.964251310494327, + "timestamp": 1.0557132040798054 + }, + { + "x": 4.842829709322602, + "y": 6.473118073762103, + "heading": 1.1824633664401288e-9, + "angularVelocity": 1.1506435598787433e-14, + "velocityX": -3.3649202415879573, + "velocityY": -0.9642515666449902, + "timestamp": 1.151687131723424 + }, + { + "x": 4.588278299686279, + "y": 6.4001737905814675, + "heading": 6.155491002470294e-10, + "angularVelocity": -5.906961193650737e-9, + "velocityX": -2.6522975133575035, + "velocityY": -0.7600427008833064, + "timestamp": 1.2476610593670427 + }, + { + "x": 4.418577355669059, + "y": 6.351544264417561, + "heading": 2.0240396006129667e-10, + "angularVelocity": -4.304764328494195e-9, + "velocityX": -1.7681983866219761, + "velocityY": -0.5066951760532586, + "timestamp": 1.3436349870106614 + }, + { + "x": 4.33372688293457, + "y": 6.3272294998168945, + "heading": 1.3559587351516463e-26, + "angularVelocity": -2.1089473467511516e-9, + "velocityX": -0.8840992008742675, + "velocityY": -0.2533476038508618, + "timestamp": 1.43960891465428 + }, + { + "x": 4.33372688293457, + "y": 6.3272294998168945, + "heading": 1.2207521358248593e-26, + "angularVelocity": -1.1102665151728888e-26, + "velocityX": 1.3383840268572876e-26, + "velocityY": 9.404006829398575e-27, + "timestamp": 1.5355828422978988 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/6Piece.6.traj b/src/main/deploy/choreo/6Piece.6.traj new file mode 100644 index 0000000..3c9d68b --- /dev/null +++ b/src/main/deploy/choreo/6Piece.6.traj @@ -0,0 +1,176 @@ +{ + "samples": [ + { + "x": 4.33372688293457, + "y": 6.3272294998168945, + "heading": 1.2207521358248593e-26, + "angularVelocity": -1.1102665151728888e-26, + "velocityX": 1.3383840268572876e-26, + "velocityY": 9.404006829398575e-27, + "timestamp": 0 + }, + { + "x": 4.401618177694935, + "y": 6.325846819123709, + "heading": -0.01308630747679995, + "angularVelocity": -0.1537232950151799, + "velocityX": 0.7975109519558792, + "velocityY": -0.016242185390126448, + "timestamp": 0.0851289811053535 + }, + { + "x": 4.537401642497592, + "y": 6.323108776290025, + "heading": -0.039258201685368914, + "angularVelocity": -0.3074381235243406, + "velocityX": 1.595032185744292, + "velocityY": -0.03216346299616805, + "timestamp": 0.170257962210707 + }, + { + "x": 4.741079933125153, + "y": 6.319056529118257, + "heading": -0.07851462136724212, + "angularVelocity": -0.46114048555673864, + "velocityX": 2.392584616694674, + "velocityY": -0.04760126480020355, + "timestamp": 0.2553869433160605 + }, + { + "x": 5.012658425331452, + "y": 6.313745941811267, + "heading": -0.1308573690249603, + "angularVelocity": -0.6148640213717588, + "velocityX": 3.1902001959849584, + "velocityY": -0.06238283646808974, + "timestamp": 0.340515924421414 + }, + { + "x": 5.3105787380844705, + "y": 6.307711135194437, + "heading": -0.13085751022406972, + "angularVelocity": -0.0000016586491178053446, + "velocityX": 3.4996344239609702, + "velocityY": -0.07089015442768516, + "timestamp": 0.4256449055267675 + }, + { + "x": 5.604210558319664, + "y": 6.269852166036089, + "heading": -0.1380733019285327, + "angularVelocity": -0.08476304556650165, + "velocityX": 3.4492580132235044, + "velocityY": -0.44472480072908593, + "timestamp": 0.510773886632121 + }, + { + "x": 5.8916318870048965, + "y": 6.225423256640149, + "heading": -0.16478619158226124, + "angularVelocity": -0.31379313257219993, + "velocityX": 3.376304108814912, + "velocityY": -0.5219011060517268, + "timestamp": 0.5959028677374745 + }, + { + "x": 6.17946353385963, + "y": 6.1717450490357715, + "heading": -0.18437285162886083, + "angularVelocity": -0.23008216229393866, + "velocityX": 3.381124067472625, + "velocityY": -0.6305515102776398, + "timestamp": 0.681031848842828 + }, + { + "x": 6.469699084381167, + "y": 6.115046286529151, + "heading": -0.1929286980078038, + "angularVelocity": -0.10050450819274385, + "velocityX": 3.4093624374800013, + "velocityY": -0.666033608888738, + "timestamp": 0.7661608299481815 + }, + { + "x": 6.760118549916772, + "y": 6.057863268678935, + "heading": -0.20041664509958976, + "angularVelocity": -0.08796002248069978, + "velocityX": 3.411522865241266, + "velocityY": -0.6717220987227442, + "timestamp": 0.851289811053535 + }, + { + "x": 7.050519039292329, + "y": 6.000691583822382, + "heading": -0.20794097624295035, + "angularVelocity": -0.08838742160027335, + "velocityX": 3.4112999545497162, + "velocityY": -0.6715889713962131, + "timestamp": 0.9364187921588885 + }, + { + "x": 7.340900045652317, + "y": 5.943531525847667, + "heading": -0.2155035684389204, + "angularVelocity": -0.08883686962740431, + "velocityX": 3.411071089886768, + "velocityY": -0.671452391800332, + "timestamp": 1.021547773264242 + }, + { + "x": 7.631260192862885, + "y": 5.886382859923546, + "heading": -0.22310904749616992, + "angularVelocity": -0.08934065647792855, + "velocityX": 3.4108260599433704, + "velocityY": -0.6713185707390947, + "timestamp": 1.1066767543695963 + }, + { + "x": 7.894877666886783, + "y": 5.836591949181387, + "heading": -0.28923685715254654, + "angularVelocity": -0.7767955025156292, + "velocityX": 3.0966830637577116, + "velocityY": -0.584887897113894, + "timestamp": 1.1918057354749507 + }, + { + "x": 8.092614293105157, + "y": 5.799284449530309, + "heading": -0.338793275351337, + "angularVelocity": -0.5821333411410221, + "velocityX": 2.322788592684559, + "velocityY": -0.4382467541213419, + "timestamp": 1.276934716580305 + }, + { + "x": 8.224448103747545, + "y": 5.774425131640709, + "heading": -0.3718133462112086, + "angularVelocity": -0.38788283885374675, + "velocityX": 1.5486360688286906, + "velocityY": -0.2920194458669074, + "timestamp": 1.3620636976856595 + }, + { + "x": 8.290366172790527, + "y": 5.761995315551758, + "heading": -0.3883190645755742, + "angularVelocity": -0.19389070737189423, + "velocityX": 0.7743317045155746, + "velocityY": -0.14601156888707717, + "timestamp": 1.4471926787910139 + }, + { + "x": 8.290366172790527, + "y": 5.761995315551758, + "heading": -0.3883190645755742, + "angularVelocity": -2.0367756371267684e-25, + "velocityX": -1.1743661571466787e-25, + "velocityY": 3.650643041091572e-26, + "timestamp": 1.5323216598963683 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/6Piece.7.traj b/src/main/deploy/choreo/6Piece.7.traj new file mode 100644 index 0000000..d023e3d --- /dev/null +++ b/src/main/deploy/choreo/6Piece.7.traj @@ -0,0 +1,167 @@ +{ + "samples": [ + { + "x": 8.290366172790527, + "y": 5.761995315551758, + "heading": -0.3883190645755742, + "angularVelocity": -2.0367756371267684e-25, + "velocityX": -1.1743661571466787e-25, + "velocityY": 3.650643041091572e-26, + "timestamp": 0 + }, + { + "x": 8.217219935622422, + "y": 5.775759562142131, + "heading": -0.36996454764073433, + "angularVelocity": 0.2046665175088019, + "velocityX": -0.8156349569545082, + "velocityY": 0.15348158852586147, + "timestamp": 0.08968011552769184 + }, + { + "x": 8.070960081985463, + "y": 5.803330463251493, + "heading": -0.3331909708699737, + "angularVelocity": 0.4100527363772757, + "velocityX": -1.630906168846257, + "velocityY": 0.3074360570024971, + "timestamp": 0.17936023105538368 + }, + { + "x": 7.851622812213268, + "y": 5.844765942409557, + "heading": -0.27793235101810565, + "angularVelocity": 0.6161747175136558, + "velocityX": -2.4457737200892002, + "velocityY": 0.4620364159239815, + "timestamp": 0.2690403465830755 + }, + { + "x": 7.562324711307271, + "y": 5.906544019791195, + "heading": -0.21038509942456823, + "angularVelocity": 0.7532021027858642, + "velocityX": -3.2258890301793066, + "velocityY": 0.6888715187098666, + "timestamp": 0.35872046211076736 + }, + { + "x": 7.258647949682298, + "y": 5.966061500881446, + "heading": -0.19353755558831395, + "angularVelocity": 0.18786264644197387, + "velocityX": -3.3862217932937377, + "velocityY": 0.6636641884328596, + "timestamp": 0.4484005776384592 + }, + { + "x": 6.951620794414783, + "y": 6.024601981076926, + "heading": -0.18840668187757148, + "angularVelocity": 0.0572130586646948, + "velocityX": -3.4235811747221354, + "velocityY": 0.6527699016779599, + "timestamp": 0.538080693166151 + }, + { + "x": 6.644049999727891, + "y": 6.0826472949292, + "heading": -0.18495304104679297, + "angularVelocity": 0.03851066438147053, + "velocityX": -3.4296431586544447, + "velocityY": 0.6472484286034442, + "timestamp": 0.6277608086938429 + }, + { + "x": 6.336482277415872, + "y": 6.140693347402136, + "heading": -0.18149747756479873, + "angularVelocity": 0.038532103372761584, + "velocityX": -3.429608899389115, + "velocityY": 0.6472566647732668, + "timestamp": 0.7174409242215347 + }, + { + "x": 6.028918781614655, + "y": 6.198740215406517, + "heading": -0.17803576282374894, + "angularVelocity": 0.03860069448707233, + "velocityX": -3.429561770649637, + "velocityY": 0.6472657585555538, + "timestamp": 0.8071210397492266 + }, + { + "x": 5.72322506123202, + "y": 6.253439628104428, + "heading": -0.165421836270589, + "angularVelocity": 0.1406546644028885, + "velocityX": -3.4087123838309332, + "velocityY": 0.6099391417601291, + "timestamp": 0.8968011552769184 + }, + { + "x": 5.423462479332292, + "y": 6.2899558743654325, + "heading": -0.12064161684333419, + "angularVelocity": 0.4993327580340479, + "velocityX": -3.3425757776501017, + "velocityY": 0.40718330976868544, + "timestamp": 0.9864812708046102 + }, + { + "x": 5.109685533167775, + "y": 6.299162410823679, + "heading": -0.12064138771302875, + "angularVelocity": 0.0000025549733527998456, + "velocityX": -3.4988463643049696, + "velocityY": 0.1026597301316337, + "timestamp": 1.076161386332302 + }, + { + "x": 4.806186833344351, + "y": 6.30479303684294, + "heading": -0.0833583935793081, + "angularVelocity": 0.41573311892320053, + "velocityX": -3.3842362717486036, + "velocityY": 0.06278566866389057, + "timestamp": 1.165841501859994 + }, + { + "x": 4.579538960191469, + "y": 6.306389607556129, + "heading": -0.04245850388633555, + "angularVelocity": 0.45606419497020956, + "velocityX": -2.5272923860462164, + "velocityY": 0.017802951120149728, + "timestamp": 1.2555216173876858 + }, + { + "x": 4.428627718565253, + "y": 6.307312197753628, + "heading": -0.014292556200369625, + "angularVelocity": 0.31407126897900167, + "velocityX": -1.682772605033218, + "velocityY": 0.01028756700490915, + "timestamp": 1.3452017329153776 + }, + { + "x": 4.353217601776123, + "y": 6.307738780975342, + "heading": 3.5414503135026283e-25, + "angularVelocity": 0.1593726336799395, + "velocityX": -0.8408788987994145, + "velocityY": 0.004756720251796441, + "timestamp": 1.4348818484430694 + }, + { + "x": 4.353217601776123, + "y": 6.307738780975342, + "heading": 1.7449109519298678e-25, + "angularVelocity": -6.34133546311643e-26, + "velocityX": 8.896875691666999e-28, + "velocityY": 1.43047293256897e-26, + "timestamp": 1.5245619639707613 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/6Piece.traj b/src/main/deploy/choreo/6Piece.traj new file mode 100644 index 0000000..d37bc0d --- /dev/null +++ b/src/main/deploy/choreo/6Piece.traj @@ -0,0 +1,1139 @@ +{ + "samples": [ + { + "x": 1.328821897506714, + "y": 5.58574914932251, + "heading": 4.3539389552577884e-26, + "angularVelocity": 5.8461926746164104e-27, + "velocityX": -1.1704555712816255e-26, + "velocityY": -1.2477760568468226e-26, + "timestamp": 0 + }, + { + "x": 1.374949606535372, + "y": 5.540487462134801, + "heading": -0.013194729598639283, + "angularVelocity": -0.15905904586136208, + "velocityX": 0.5560575782186153, + "velocityY": -0.5456179093578979, + "timestamp": 0.08295491480654284 + }, + { + "x": 1.46721327852146, + "y": 5.449968559727431, + "heading": -0.03947645751575262, + "angularVelocity": -0.316819419059188, + "velocityX": 1.1122146554095553, + "velocityY": -1.0911819102999225, + "timestamp": 0.16590982961308567 + }, + { + "x": 1.605638421023178, + "y": 5.314200511590273, + "heading": -0.078646870972475, + "angularVelocity": -0.4721891831010949, + "velocityX": 1.6686792196042406, + "velocityY": -1.6366486356327254, + "timestamp": 0.24886474441962853 + }, + { + "x": 1.7902678160557024, + "y": 5.133199369169571, + "heading": -0.13043925037876178, + "angularVelocity": -0.6243437116061242, + "velocityX": 2.225659509904795, + "velocityY": -2.181921864940855, + "timestamp": 0.33181965922617135 + }, + { + "x": 1.9998217979277588, + "y": 4.932195127760977, + "heading": -0.13043932359342048, + "angularVelocity": -8.825837368571486e-7, + "velocityX": 2.5261189449805634, + "velocityY": -2.423054039382124, + "timestamp": 0.41477457403271417 + }, + { + "x": 2.2093757892592514, + "y": 4.731190896214115, + "heading": -0.13043939680794175, + "angularVelocity": -8.825820799374822e-7, + "velocityX": 2.5261190590116103, + "velocityY": -2.4230539205014976, + "timestamp": 0.497729488839257 + }, + { + "x": 2.3945844486540637, + "y": 4.550478243745755, + "heading": -0.17901854361361771, + "angularVelocity": -0.5856090253237706, + "velocityX": 2.232642391674238, + "velocityY": -2.1784441933283367, + "timestamp": 0.5806844036457999 + }, + { + "x": 2.533512254873414, + "y": 4.414958500153421, + "heading": -0.21536260429634638, + "angularVelocity": -0.4381182328676455, + "velocityX": 1.6747387004536223, + "velocityY": -1.6336553886936764, + "timestamp": 0.6636393184523427 + }, + { + "x": 2.6261356849469077, + "y": 4.324618150485375, + "heading": -0.23957423083578933, + "angularVelocity": -0.2918648834238011, + "velocityX": 1.1165514459209405, + "velocityY": -1.0890295032998, + "timestamp": 0.7465942332588855 + }, + { + "x": 2.672445297241211, + "y": 4.279448986053467, + "heading": -0.2516907201225374, + "angularVelocity": -0.14606113833043707, + "velocityX": 0.5582503749452395, + "velocityY": -0.5445025715142554, + "timestamp": 0.8295491480654283 + }, + { + "x": 2.672445297241211, + "y": 4.279448986053467, + "heading": -0.2516907201225374, + "angularVelocity": 1.3508666315673642e-26, + "velocityX": -7.696322556768917e-27, + "velocityY": -1.1161944739972978e-26, + "timestamp": 0.9125040628719712 + }, + { + "x": 2.625196176417341, + "y": 4.293300959249136, + "heading": -0.2476240648671981, + "angularVelocity": 0.05660704046380333, + "velocityX": -0.6576984589102508, + "velocityY": 0.19281673954566436, + "timestamp": 0.9843441594751433 + }, + { + "x": 2.5313898742849363, + "y": 4.322902155034929, + "heading": -0.238711790545806, + "angularVelocity": 0.12405710380125745, + "velocityX": -1.3057652560041622, + "velocityY": 0.4120428171095463, + "timestamp": 1.0561842560783155 + }, + { + "x": 2.3926976278493437, + "y": 4.3719573505978495, + "heading": -0.22331799219109705, + "angularVelocity": 0.21427864218697643, + "velocityX": -1.9305687630362727, + "velocityY": 0.6828386636767781, + "timestamp": 1.1280243526814877 + }, + { + "x": 2.215900012214516, + "y": 4.450150810574297, + "heading": -0.19663428050241719, + "angularVelocity": 0.3714320129060295, + "velocityX": -2.460987999660081, + "velocityY": 1.0884375672317064, + "timestamp": 1.19986444928466 + }, + { + "x": 2.052047257943267, + "y": 4.568466965487968, + "heading": -0.1386728664270459, + "angularVelocity": 0.8068114718099074, + "velocityX": -2.2807980782143584, + "velocityY": 1.6469375809336866, + "timestamp": 1.2717045458878322 + }, + { + "x": 1.935784479955621, + "y": 4.6986149037226514, + "heading": -0.07489980420839401, + "angularVelocity": 0.8877084696993005, + "velocityX": -1.6183549784162223, + "velocityY": 1.811633675182685, + "timestamp": 1.3435446424910045 + }, + { + "x": 1.8688424514197695, + "y": 4.831515653648781, + "heading": -0.009029725121146852, + "angularVelocity": 0.9168985316250061, + "velocityX": -0.9318198568916667, + "velocityY": 1.8499522719219348, + "timestamp": 1.4153847390941767 + }, + { + "x": 1.8513420820236208, + "y": 4.9637017250061035, + "heading": 0.057668349012968446, + "angularVelocity": 0.928424059652088, + "velocityX": -0.2436016963175404, + "velocityY": 1.84000408695839, + "timestamp": 1.487224835697349 + }, + { + "x": 1.8881504770913347, + "y": 5.100791187409367, + "heading": 0.12845802675955434, + "angularVelocity": 0.9309881682233778, + "velocityX": 0.48408442290141646, + "velocityY": 1.802927652000758, + "timestamp": 1.5632619757629151 + }, + { + "x": 1.9801594635213668, + "y": 5.23317628018901, + "heading": 0.19875878141335546, + "angularVelocity": 0.9245581119066463, + "velocityX": 1.2100532233418253, + "velocityY": 1.7410582863254496, + "timestamp": 1.6392991158284813 + }, + { + "x": 2.1268970903680344, + "y": 5.357201962866711, + "heading": 0.26712068314763787, + "angularVelocity": 0.8990593501456591, + "velocityX": 1.9298151761117934, + "velocityY": 1.6311197734522118, + "timestamp": 1.7153362558940475 + }, + { + "x": 2.3255858491677373, + "y": 5.4631354448246725, + "heading": 0.32902091884053974, + "angularVelocity": 0.8140789571980984, + "velocityX": 2.6130488157284057, + "velocityY": 1.393180778059456, + "timestamp": 1.7913733959596136 + }, + { + "x": 2.5347759563760905, + "y": 5.522688608468802, + "heading": 0.3564296694092298, + "angularVelocity": 0.3604653008392443, + "velocityX": 2.7511569612951026, + "velocityY": 0.7832115146989626, + "timestamp": 1.8674105360251798 + }, + { + "x": 2.696139551385315, + "y": 5.557119984012087, + "heading": 0.37271335067397476, + "angularVelocity": 0.21415430999513735, + "velocityX": 2.122168125603903, + "velocityY": 0.4528231271401756, + "timestamp": 1.943447676090746 + }, + { + "x": 2.8046075058969566, + "y": 5.576857728360174, + "heading": 0.38238828101753003, + "angularVelocity": 0.12723953498530663, + "velocityX": 1.4265128122666186, + "velocityY": 0.25958030945230076, + "timestamp": 2.019484816156312 + }, + { + "x": 2.8590595722198486, + "y": 5.58574914932251, + "heading": 0.38687655192878945, + "angularVelocity": 0.05902735041572054, + "velocityX": 0.7161245974787916, + "velocityY": 0.11693523657870776, + "timestamp": 2.0955219562218783 + }, + { + "x": 2.8590595722198486, + "y": 5.58574914932251, + "heading": 0.38687655192878945, + "angularVelocity": -4.595788384330154e-29, + "velocityX": 7.170077328456974e-29, + "velocityY": 4.9006221400778145e-30, + "timestamp": 2.1715590962874445 + }, + { + "x": 2.821439157373947, + "y": 5.594030750372382, + "heading": 0.387080960332372, + "angularVelocity": 0.0032239487618478326, + "velocityX": -0.5933527572101648, + "velocityY": 0.1306181985813967, + "timestamp": 2.2349622142282555 + }, + { + "x": 2.7464223945042074, + "y": 5.611552554364111, + "heading": 0.38751784243776727, + "angularVelocity": 0.006890546073824077, + "velocityX": -1.1831715112144825, + "velocityY": 0.27635555727852257, + "timestamp": 2.2983653321690665 + }, + { + "x": 2.6344378134858273, + "y": 5.639901380380037, + "heading": 0.38823547094203587, + "angularVelocity": 0.011318504950159194, + "velocityX": -1.7662314513131911, + "velocityY": 0.44712037730369514, + "timestamp": 2.3617684501098775 + }, + { + "x": 2.48653995660996, + "y": 5.682179259469134, + "heading": 0.3893349048666279, + "angularVelocity": 0.01734037631427531, + "velocityX": -2.332659050205305, + "velocityY": 0.6668107257527173, + "timestamp": 2.4251715680506885 + }, + { + "x": 2.307136094114684, + "y": 5.746612910577359, + "heading": 0.39112800963911476, + "angularVelocity": 0.02828101883192509, + "velocityX": -2.829574764174154, + "velocityY": 1.0162536670259112, + "timestamp": 2.4885746859914994 + }, + { + "x": 2.1381687976868347, + "y": 5.848112044286255, + "heading": 0.3940456425874033, + "angularVelocity": 0.046017184060447705, + "velocityX": -2.6649682525958127, + "velocityY": 1.600853980140992, + "timestamp": 2.5519778039323104 + }, + { + "x": 2.0057971851389707, + "y": 5.961635826733289, + "heading": 0.39722224472387, + "angularVelocity": 0.05010167070067704, + "velocityX": -2.087777649538265, + "velocityY": 1.7905078824832, + "timestamp": 2.6153809218731214 + }, + { + "x": 1.9116699025918782, + "y": 6.079772759952102, + "heading": 0.40050830041797525, + "angularVelocity": 0.05182798261077437, + "velocityX": -1.4845844432282262, + "velocityY": 1.8632669347444144, + "timestamp": 2.6787840398139324 + }, + { + "x": 1.8560250338172737, + "y": 6.199649277805345, + "heading": 0.40384210990951336, + "angularVelocity": 0.052581160040904545, + "velocityX": -0.8776361570506794, + "velocityY": 1.8907038288740154, + "timestamp": 2.7421871577547434 + }, + { + "x": 1.838901162147522, + "y": 6.319766044616699, + "heading": 0.4071898798527544, + "angularVelocity": 0.052801345611525094, + "velocityX": -0.27007933088933367, + "velocityY": 1.8944930582670674, + "timestamp": 2.8055902756955544 + }, + { + "x": 1.8674670617000426, + "y": 6.452466802830068, + "heading": 0.410902560889689, + "angularVelocity": 0.05267198382132873, + "velocityX": 0.4052657861269917, + "velocityY": 1.8826320171752862, + "timestamp": 2.8760771031810837 + }, + { + "x": 1.9435937805094023, + "y": 6.582998572823563, + "heading": 0.4145700789652226, + "angularVelocity": 0.052031254723253276, + "velocityX": 1.080013408533504, + "velocityY": 1.8518604773394354, + "timestamp": 2.946563930666613 + }, + { + "x": 2.067117864237862, + "y": 6.709034317074673, + "heading": 0.41812673034240355, + "angularVelocity": 0.05045838355983837, + "velocityX": 1.7524420964160776, + "velocityY": 1.7880751446358603, + "timestamp": 3.017050758152142 + }, + { + "x": 2.2372903513460827, + "y": 6.825552193890142, + "heading": 0.4214177842629497, + "angularVelocity": 0.04669033971236367, + "velocityX": 2.4142452310420213, + "velocityY": 1.6530447031310942, + "timestamp": 3.0875375856376714 + }, + { + "x": 2.447332984322481, + "y": 6.9160627740645415, + "heading": 0.423792969899213, + "angularVelocity": 0.03369687246529782, + "velocityX": 2.9798849014664373, + "velocityY": 1.2840779391437487, + "timestamp": 3.1580244131232007 + }, + { + "x": 2.6299484111085176, + "y": 6.9676741214205835, + "heading": 0.4250728750513537, + "angularVelocity": 0.018158075739803856, + "velocityX": 2.590773812646438, + "velocityY": 0.7322126586933964, + "timestamp": 3.22851124060873 + }, + { + "x": 2.768901501023009, + "y": 7.000310165629845, + "heading": 0.42589586451986794, + "angularVelocity": 0.01167579103603713, + "velocityX": 1.9713341466959602, + "velocityY": 0.46300912345588136, + "timestamp": 3.298998068094259 + }, + { + "x": 2.862081475468267, + "y": 7.0198533344035186, + "heading": 0.4263947957186992, + "angularVelocity": 0.007078360831798, + "velocityX": 1.3219487636095895, + "velocityY": 0.2772598721042683, + "timestamp": 3.3694848955797885 + }, + { + "x": 2.9088234901428223, + "y": 7.028900623321533, + "heading": 0.426627843154362, + "angularVelocity": 0.003306255139807895, + "velocityX": 0.6631312025520166, + "velocityY": 0.12835432152017248, + "timestamp": 3.4399717230653177 + }, + { + "x": 2.9088234901428223, + "y": 7.028900623321533, + "heading": 0.426627843154362, + "angularVelocity": 7.235947497328528e-24, + "velocityX": 3.9627330150979443e-25, + "velocityY": 3.746183212022811e-24, + "timestamp": 3.510458550550847 + }, + { + "x": 2.9499714553274323, + "y": 7.035529268833997, + "heading": 0.4162396153486674, + "angularVelocity": -0.15476273818242897, + "velocityX": 0.613018108739834, + "velocityY": 0.09875287191789282, + "timestamp": 3.577582122592131 + }, + { + "x": 3.032252734431755, + "y": 7.0488035270821205, + "heading": 0.3954328742907609, + "angularVelocity": -0.3099766657994496, + "velocityX": 1.2258179444579598, + "velocityY": 0.19775851976348513, + "timestamp": 3.6447056946334153 + }, + { + "x": 3.155648996542602, + "y": 7.068747158199284, + "heading": 0.3641717265377585, + "angularVelocity": -0.4657253301980919, + "velocityX": 1.8383446881365682, + "velocityY": 0.29711814360681277, + "timestamp": 3.7118292666746995 + }, + { + "x": 3.3201397118502305, + "y": 7.09538869210212, + "heading": 0.3224161177499544, + "angularVelocity": -0.6220707199867489, + "velocityX": 2.4505655808433304, + "velocityY": 0.3969028031829179, + "timestamp": 3.7789528387159836 + }, + { + "x": 3.525704706881839, + "y": 7.128759620860251, + "heading": 0.2701269686194379, + "angularVelocity": -0.7789983092430813, + "velocityX": 3.062485931248957, + "velocityY": 0.4971566283392252, + "timestamp": 3.846076410757268 + }, + { + "x": 3.736742521528462, + "y": 7.17085251865357, + "heading": 0.1968734668457863, + "angularVelocity": -1.0913230560584186, + "velocityX": 3.1440194290736665, + "velocityY": 0.6270956165358649, + "timestamp": 3.913199982798552 + }, + { + "x": 3.9603913131238833, + "y": 7.208349824687908, + "heading": 0.16611663819424638, + "angularVelocity": -0.45821203663927534, + "velocityX": 3.331896452975815, + "velocityY": 0.5586309681385255, + "timestamp": 3.980323554839836 + }, + { + "x": 4.186083835227695, + "y": 7.240507088585249, + "heading": 0.139868186298227, + "angularVelocity": -0.39104670829906596, + "velocityX": 3.3623437376813072, + "velocityY": 0.47907557538152645, + "timestamp": 4.04744712688112 + }, + { + "x": 4.411495404478292, + "y": 7.272714656791408, + "heading": 0.11312371036793661, + "angularVelocity": -0.398436422808984, + "velocityX": 3.3581581312740267, + "velocityY": 0.47982500374605697, + "timestamp": 4.114570698922404 + }, + { + "x": 4.636607862344283, + "y": 7.304977234634002, + "heading": 0.08583019595793309, + "angularVelocity": -0.40661594101721416, + "velocityX": 3.353702000953352, + "velocityY": 0.4806445315924413, + "timestamp": 4.181694270963688 + }, + { + "x": 4.861400835126187, + "y": 7.33729811854161, + "heading": 0.05792775603075696, + "angularVelocity": -0.4156876500853505, + "velocityX": 3.3489423453752654, + "velocityY": 0.48151316928916743, + "timestamp": 4.248817843004972 + }, + { + "x": 5.085850174369245, + "y": 7.369680784648805, + "heading": 0.02934541464474294, + "angularVelocity": -0.42581675135576136, + "velocityX": 3.343822928627991, + "velocityY": 0.48243359407748343, + "timestamp": 4.3159414150462565 + }, + { + "x": 5.309927463531494, + "y": 7.402129173278809, + "heading": 5.71560409253603e-24, + "angularVelocity": -0.4371849374567573, + "velocityX": 3.338280165191911, + "velocityY": 0.4834127213916103, + "timestamp": 4.383064987087541 + }, + { + "x": 5.5975844277501166, + "y": 7.40698430828562, + "heading": -5.977593030842461e-14, + "angularVelocity": -7.27279605332056e-13, + "velocityX": 3.49985424414606, + "velocityY": 0.05907127924278551, + "timestamp": 4.465256113960828 + }, + { + "x": 5.885241394648691, + "y": 7.411839284508277, + "heading": -6.292590842346059e-14, + "angularVelocity": -3.832503866135791e-14, + "velocityX": 3.4998542767523966, + "velocityY": 0.05906934735354094, + "timestamp": 4.547447240834116 + }, + { + "x": 6.1728983617447595, + "y": 7.416694249029442, + "heading": -6.528052611785862e-14, + "angularVelocity": -2.864807655005671e-14, + "velocityX": 3.499854279155253, + "velocityY": 0.05906920498425597, + "timestamp": 4.629638367707404 + }, + { + "x": 6.460555329029281, + "y": 7.421549202384699, + "heading": -6.686290101207671e-14, + "angularVelocity": -1.9252381058857556e-14, + "velocityX": 3.4998542814481235, + "velocityY": 0.059069069131291764, + "timestamp": 4.711829494580692 + }, + { + "x": 6.748212296487426, + "y": 7.426404145452759, + "heading": -6.776532099009745e-14, + "angularVelocity": -1.0979530423221106e-14, + "velocityX": 3.4998542835605484, + "velocityY": 0.05906894396940334, + "timestamp": 4.79402062145398 + }, + { + "x": 7.035869264100995, + "y": 7.431259079311885, + "heading": -6.813646923115834e-14, + "angularVelocity": -4.515672861335136e-15, + "velocityX": 3.499854285451553, + "velocityY": 0.05906883192648223, + "timestamp": 4.876211748327267 + }, + { + "x": 7.323526231850984, + "y": 7.436114005088022, + "heading": -6.829397797228803e-14, + "angularVelocity": -1.916371597823108e-15, + "velocityX": 3.4998542871113476, + "velocityY": 0.05906873358265542, + "timestamp": 4.958402875200555 + }, + { + "x": 7.611183199719373, + "y": 7.440968923848655, + "heading": -6.831359071366292e-14, + "angularVelocity": -2.386235853473579e-16, + "velocityX": 3.4998542885518997, + "velocityY": 0.059068648226685214, + "timestamp": 5.040594002073843 + }, + { + "x": 7.870083631568097, + "y": 7.445338496439803, + "heading": 1.5159787003061443e-11, + "angularVelocity": 1.852766980218185e-10, + "velocityX": 3.1499803165841067, + "velocityY": 0.05316355618148571, + "timestamp": 5.122785128947131 + }, + { + "x": 8.064258961919318, + "y": 7.448615675130285, + "heading": 1.666159387153368e-11, + "angularVelocity": 1.827212894632211e-11, + "velocityX": 2.3624853160923003, + "velocityY": 0.03987265797601595, + "timestamp": 5.2049762558204185 + }, + { + "x": 8.193709184032379, + "y": 7.450800460403948, + "heading": 8.958511488927219e-12, + "angularVelocity": -9.372158133909199e-11, + "velocityX": 1.5749902335884913, + "velocityY": 0.026581765657390878, + "timestamp": 5.287167382693706 + }, + { + "x": 8.258434295654297, + "y": 7.451892852783203, + "heading": -7.387785600293283e-27, + "angularVelocity": -1.0899609032904939e-10, + "velocityX": 0.7874951236731913, + "velocityY": 0.013290879694828065, + "timestamp": 5.369358509566994 + }, + { + "x": 8.258434295654297, + "y": 7.451892852783203, + "heading": -3.9312058218717824e-27, + "angularVelocity": 1.1753376253871266e-27, + "velocityX": 1.6594582279288362e-26, + "velocityY": 8.007277829857814e-26, + "timestamp": 5.451549636440282 + }, + { + "x": 8.173583810413728, + "y": 7.427578131824658, + "heading": 2.0826763233160825e-10, + "angularVelocity": 2.170043859254894e-9, + "velocityX": -0.884099331181325, + "velocityY": -0.2533471491219265, + "timestamp": 5.5475235640839005 + }, + { + "x": 8.003882842204604, + "y": 7.378948690082569, + "heading": 6.074862710776759e-10, + "angularVelocity": 4.159657195946921e-9, + "velocityX": -1.7681986386894193, + "velocityY": -0.5066942964204343, + "timestamp": 5.643497491727519 + }, + { + "x": 7.749331397599835, + "y": 7.306004528930369, + "heading": 1.1824464201448697e-9, + "angularVelocity": 5.990795241830689e-9, + "velocityX": -2.652297877711105, + "velocityY": -0.7600414294084665, + "timestamp": 5.739471419371138 + }, + { + "x": 7.426386753090784, + "y": 7.213461633012268, + "heading": 1.1824469628681777e-9, + "angularVelocity": 5.654903592256136e-15, + "velocityX": -3.3649205824757455, + "velocityY": -0.9642503770580384, + "timestamp": 5.8354453470147565 + }, + { + "x": 7.103442110110582, + "y": 7.120918731758949, + "heading": 1.1824474238942088e-9, + "angularVelocity": 4.803659101973182e-15, + "velocityX": -3.364920566545916, + "velocityY": -0.9642504326483329, + "timestamp": 5.931419274658375 + }, + { + "x": 6.7804974690937145, + "y": 7.0283758236542315, + "heading": 1.1824480406820372e-9, + "angularVelocity": 6.426618597592819e-15, + "velocityX": -3.364920546088964, + "velocityY": -0.9642505040364596, + "timestamp": 6.027393202301994 + }, + { + "x": 6.4575528306495285, + "y": 6.935832906571688, + "heading": 1.1824490782902537e-9, + "angularVelocity": 1.0811355148059625e-14, + "velocityX": -3.3649205192829115, + "velocityY": -0.964250597580873, + "timestamp": 6.1233671299456125 + }, + { + "x": 6.134608195619217, + "y": 6.843289977575833, + "heading": 1.18245090262753e-9, + "angularVelocity": 1.9008675803016132e-14, + "velocityX": -3.364920483712055, + "velocityY": -0.9642507217115844, + "timestamp": 6.219341057589231 + }, + { + "x": 5.811663565059857, + "y": 6.750747032977816, + "heading": 1.1824542039138914e-9, + "angularVelocity": 3.439774157926315e-14, + "velocityX": -3.36492043712699, + "velocityY": -0.9642508842782657, + "timestamp": 6.31531498523285 + }, + { + "x": 5.4887189399527605, + "y": 6.658204069353181, + "heading": 1.1824584939954223e-9, + "angularVelocity": 4.470048935450349e-14, + "velocityX": -3.36492038031715, + "velocityY": -0.964251082526029, + "timestamp": 6.4112889128764685 + }, + { + "x": 5.165774321115312, + "y": 6.565661083849535, + "heading": 1.1824622621223112e-9, + "angularVelocity": 3.926198480608096e-14, + "velocityX": -3.364920314990583, + "velocityY": -0.964251310494327, + "timestamp": 6.507262840520087 + }, + { + "x": 4.842829709322602, + "y": 6.473118073762103, + "heading": 1.1824633664401288e-9, + "angularVelocity": 1.1506435598787433e-14, + "velocityX": -3.3649202415879573, + "velocityY": -0.9642515666449902, + "timestamp": 6.603236768163706 + }, + { + "x": 4.588278299686279, + "y": 6.4001737905814675, + "heading": 6.155491002470294e-10, + "angularVelocity": -5.906961193650737e-9, + "velocityX": -2.6522975133575035, + "velocityY": -0.7600427008833064, + "timestamp": 6.6992106958073245 + }, + { + "x": 4.418577355669059, + "y": 6.351544264417561, + "heading": 2.0240396006129667e-10, + "angularVelocity": -4.304764328494195e-9, + "velocityX": -1.7681983866219761, + "velocityY": -0.5066951760532586, + "timestamp": 6.795184623450943 + }, + { + "x": 4.33372688293457, + "y": 6.3272294998168945, + "heading": 1.3559587351516463e-26, + "angularVelocity": -2.1089473467511516e-9, + "velocityX": -0.8840992008742675, + "velocityY": -0.2533476038508618, + "timestamp": 6.891158551094562 + }, + { + "x": 4.33372688293457, + "y": 6.3272294998168945, + "heading": 1.2207521358248593e-26, + "angularVelocity": -1.1102665151728888e-26, + "velocityX": 1.3383840268572876e-26, + "velocityY": 9.404006829398575e-27, + "timestamp": 6.9871324787381806 + }, + { + "x": 4.401618177694935, + "y": 6.325846819123709, + "heading": -0.01308630747679995, + "angularVelocity": -0.1537232950151799, + "velocityX": 0.7975109519558792, + "velocityY": -0.016242185390126448, + "timestamp": 7.072261459843534 + }, + { + "x": 4.537401642497592, + "y": 6.323108776290025, + "heading": -0.039258201685368914, + "angularVelocity": -0.3074381235243406, + "velocityX": 1.595032185744292, + "velocityY": -0.03216346299616805, + "timestamp": 7.1573904409488875 + }, + { + "x": 4.741079933125153, + "y": 6.319056529118257, + "heading": -0.07851462136724212, + "angularVelocity": -0.46114048555673864, + "velocityX": 2.392584616694674, + "velocityY": -0.04760126480020355, + "timestamp": 7.242519422054241 + }, + { + "x": 5.012658425331452, + "y": 6.313745941811267, + "heading": -0.1308573690249603, + "angularVelocity": -0.6148640213717588, + "velocityX": 3.1902001959849584, + "velocityY": -0.06238283646808974, + "timestamp": 7.3276484031595945 + }, + { + "x": 5.3105787380844705, + "y": 6.307711135194437, + "heading": -0.13085751022406972, + "angularVelocity": -0.0000016586491178053446, + "velocityX": 3.4996344239609702, + "velocityY": -0.07089015442768516, + "timestamp": 7.412777384264948 + }, + { + "x": 5.604210558319664, + "y": 6.269852166036089, + "heading": -0.1380733019285327, + "angularVelocity": -0.08476304556650165, + "velocityX": 3.4492580132235044, + "velocityY": -0.44472480072908593, + "timestamp": 7.4979063653703015 + }, + { + "x": 5.8916318870048965, + "y": 6.225423256640149, + "heading": -0.16478619158226124, + "angularVelocity": -0.31379313257219993, + "velocityX": 3.376304108814912, + "velocityY": -0.5219011060517268, + "timestamp": 7.583035346475655 + }, + { + "x": 6.17946353385963, + "y": 6.1717450490357715, + "heading": -0.18437285162886083, + "angularVelocity": -0.23008216229393866, + "velocityX": 3.381124067472625, + "velocityY": -0.6305515102776398, + "timestamp": 7.6681643275810085 + }, + { + "x": 6.469699084381167, + "y": 6.115046286529151, + "heading": -0.1929286980078038, + "angularVelocity": -0.10050450819274385, + "velocityX": 3.4093624374800013, + "velocityY": -0.666033608888738, + "timestamp": 7.753293308686362 + }, + { + "x": 6.760118549916772, + "y": 6.057863268678935, + "heading": -0.20041664509958976, + "angularVelocity": -0.08796002248069978, + "velocityX": 3.411522865241266, + "velocityY": -0.6717220987227442, + "timestamp": 7.8384222897917155 + }, + { + "x": 7.050519039292329, + "y": 6.000691583822382, + "heading": -0.20794097624295035, + "angularVelocity": -0.08838742160027335, + "velocityX": 3.4112999545497162, + "velocityY": -0.6715889713962131, + "timestamp": 7.923551270897069 + }, + { + "x": 7.340900045652317, + "y": 5.943531525847667, + "heading": -0.2155035684389204, + "angularVelocity": -0.08883686962740431, + "velocityX": 3.411071089886768, + "velocityY": -0.671452391800332, + "timestamp": 8.008680252002423 + }, + { + "x": 7.631260192862885, + "y": 5.886382859923546, + "heading": -0.22310904749616992, + "angularVelocity": -0.08934065647792855, + "velocityX": 3.4108260599433704, + "velocityY": -0.6713185707390947, + "timestamp": 8.093809233107777 + }, + { + "x": 7.894877666886783, + "y": 5.836591949181387, + "heading": -0.28923685715254654, + "angularVelocity": -0.7767955025156292, + "velocityX": 3.0966830637577116, + "velocityY": -0.584887897113894, + "timestamp": 8.178938214213131 + }, + { + "x": 8.092614293105157, + "y": 5.799284449530309, + "heading": -0.338793275351337, + "angularVelocity": -0.5821333411410221, + "velocityX": 2.322788592684559, + "velocityY": -0.4382467541213419, + "timestamp": 8.264067195318486 + }, + { + "x": 8.224448103747545, + "y": 5.774425131640709, + "heading": -0.3718133462112086, + "angularVelocity": -0.38788283885374675, + "velocityX": 1.5486360688286906, + "velocityY": -0.2920194458669074, + "timestamp": 8.34919617642384 + }, + { + "x": 8.290366172790527, + "y": 5.761995315551758, + "heading": -0.3883190645755742, + "angularVelocity": -0.19389070737189423, + "velocityX": 0.7743317045155746, + "velocityY": -0.14601156888707717, + "timestamp": 8.434325157529194 + }, + { + "x": 8.290366172790527, + "y": 5.761995315551758, + "heading": -0.3883190645755742, + "angularVelocity": -2.0367756371267684e-25, + "velocityX": -1.1743661571466787e-25, + "velocityY": 3.650643041091572e-26, + "timestamp": 8.519454138634549 + }, + { + "x": 8.217219935622422, + "y": 5.775759562142131, + "heading": -0.36996454764073433, + "angularVelocity": 0.2046665175088019, + "velocityX": -0.8156349569545082, + "velocityY": 0.15348158852586147, + "timestamp": 8.60913425416224 + }, + { + "x": 8.070960081985463, + "y": 5.803330463251493, + "heading": -0.3331909708699737, + "angularVelocity": 0.4100527363772757, + "velocityX": -1.630906168846257, + "velocityY": 0.3074360570024971, + "timestamp": 8.698814369689932 + }, + { + "x": 7.851622812213268, + "y": 5.844765942409557, + "heading": -0.27793235101810565, + "angularVelocity": 0.6161747175136558, + "velocityX": -2.4457737200892002, + "velocityY": 0.4620364159239815, + "timestamp": 8.788494485217624 + }, + { + "x": 7.562324711307271, + "y": 5.906544019791195, + "heading": -0.21038509942456823, + "angularVelocity": 0.7532021027858642, + "velocityX": -3.2258890301793066, + "velocityY": 0.6888715187098666, + "timestamp": 8.878174600745316 + }, + { + "x": 7.258647949682298, + "y": 5.966061500881446, + "heading": -0.19353755558831395, + "angularVelocity": 0.18786264644197387, + "velocityX": -3.3862217932937377, + "velocityY": 0.6636641884328596, + "timestamp": 8.967854716273008 + }, + { + "x": 6.951620794414783, + "y": 6.024601981076926, + "heading": -0.18840668187757148, + "angularVelocity": 0.0572130586646948, + "velocityX": -3.4235811747221354, + "velocityY": 0.6527699016779599, + "timestamp": 9.0575348318007 + }, + { + "x": 6.644049999727891, + "y": 6.0826472949292, + "heading": -0.18495304104679297, + "angularVelocity": 0.03851066438147053, + "velocityX": -3.4296431586544447, + "velocityY": 0.6472484286034442, + "timestamp": 9.147214947328392 + }, + { + "x": 6.336482277415872, + "y": 6.140693347402136, + "heading": -0.18149747756479873, + "angularVelocity": 0.038532103372761584, + "velocityX": -3.429608899389115, + "velocityY": 0.6472566647732668, + "timestamp": 9.236895062856084 + }, + { + "x": 6.028918781614655, + "y": 6.198740215406517, + "heading": -0.17803576282374894, + "angularVelocity": 0.03860069448707233, + "velocityX": -3.429561770649637, + "velocityY": 0.6472657585555538, + "timestamp": 9.326575178383775 + }, + { + "x": 5.72322506123202, + "y": 6.253439628104428, + "heading": -0.165421836270589, + "angularVelocity": 0.1406546644028885, + "velocityX": -3.4087123838309332, + "velocityY": 0.6099391417601291, + "timestamp": 9.416255293911467 + }, + { + "x": 5.423462479332292, + "y": 6.2899558743654325, + "heading": -0.12064161684333419, + "angularVelocity": 0.4993327580340479, + "velocityX": -3.3425757776501017, + "velocityY": 0.40718330976868544, + "timestamp": 9.505935409439159 + }, + { + "x": 5.109685533167775, + "y": 6.299162410823679, + "heading": -0.12064138771302875, + "angularVelocity": 0.0000025549733527998456, + "velocityX": -3.4988463643049696, + "velocityY": 0.1026597301316337, + "timestamp": 9.59561552496685 + }, + { + "x": 4.806186833344351, + "y": 6.30479303684294, + "heading": -0.0833583935793081, + "angularVelocity": 0.41573311892320053, + "velocityX": -3.3842362717486036, + "velocityY": 0.06278566866389057, + "timestamp": 9.685295640494543 + }, + { + "x": 4.579538960191469, + "y": 6.306389607556129, + "heading": -0.04245850388633555, + "angularVelocity": 0.45606419497020956, + "velocityX": -2.5272923860462164, + "velocityY": 0.017802951120149728, + "timestamp": 9.774975756022235 + }, + { + "x": 4.428627718565253, + "y": 6.307312197753628, + "heading": -0.014292556200369625, + "angularVelocity": 0.31407126897900167, + "velocityX": -1.682772605033218, + "velocityY": 0.01028756700490915, + "timestamp": 9.864655871549926 + }, + { + "x": 4.353217601776123, + "y": 6.307738780975342, + "heading": 3.5414503135026283e-25, + "angularVelocity": 0.1593726336799395, + "velocityX": -0.8408788987994145, + "velocityY": 0.004756720251796441, + "timestamp": 9.954335987077618 + }, + { + "x": 4.353217601776123, + "y": 6.307738780975342, + "heading": 1.7449109519298678e-25, + "angularVelocity": -6.34133546311643e-26, + "velocityX": 8.896875691666999e-28, + "velocityY": 1.43047293256897e-26, + "timestamp": 10.04401610260531 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Centerline Auto.1.traj b/src/main/deploy/choreo/Centerline Auto.1.traj deleted file mode 100644 index d4736c5..0000000 --- a/src/main/deploy/choreo/Centerline Auto.1.traj +++ /dev/null @@ -1,338 +0,0 @@ -{ - "samples": [ - { - "x": 0.660783052444458, - "y": 6.694385051727295, - "heading": 1.0498621745809171, - "angularVelocity": 5.825284855617402e-28, - "velocityX": -1.9685293298057674e-27, - "velocityY": 1.4418564963898336e-27, - "timestamp": 0 - }, - { - "x": 0.6784036347143663, - "y": 6.707198268292706, - "heading": 1.0175475056665193, - "angularVelocity": -0.5991822637957332, - "velocityX": 0.32672283914932904, - "velocityY": 0.23758411786628814, - "timestamp": 0.05393128412995569 - }, - { - "x": 0.7135162843776108, - "y": 6.732573985913541, - "heading": 0.9516130808872657, - "angularVelocity": -1.2225635981589924, - "velocityX": 0.6510627408506594, - "velocityY": 0.4705194402508243, - "timestamp": 0.10786256825991138 - }, - { - "x": 0.7660994692609764, - "y": 6.770017684654193, - "heading": 0.8504346226500515, - "angularVelocity": -1.8760624722639487, - "velocityX": 0.9750033905489506, - "velocityY": 0.6942853177837462, - "timestamp": 0.16179385238986707 - }, - { - "x": 0.836479083356807, - "y": 6.818804043384565, - "heading": 0.7130270962325688, - "angularVelocity": -2.5478259721459318, - "velocityX": 1.3049868036933845, - "velocityY": 0.9046022084846815, - "timestamp": 0.21572513651982275 - }, - { - "x": 0.9255037273969096, - "y": 6.878132431612943, - "heading": 0.5402736783143448, - "angularVelocity": -3.2032135096569965, - "velocityX": 1.6507050680563082, - "velocityY": 1.1000737176110378, - "timestamp": 0.26965642064977846 - }, - { - "x": 1.035016950774323, - "y": 6.947674803955847, - "heading": 0.3381434888609651, - "angularVelocity": -3.747920946334524, - "velocityX": 2.0306066347970617, - "velocityY": 1.289462571952319, - "timestamp": 0.32358770477973414 - }, - { - "x": 1.162461605941772, - "y": 7.018689321892079, - "heading": 0.18269313635392612, - "angularVelocity": -2.882378104190095, - "velocityX": 2.3630932810787844, - "velocityY": 1.3167592628633178, - "timestamp": 0.3775189889096898 - }, - { - "x": 1.3050246421848908, - "y": 7.094859887834467, - "heading": 0.07841153426675496, - "angularVelocity": -1.9336013182235503, - "velocityX": 2.6434200212921195, - "velocityY": 1.4123632910138557, - "timestamp": 0.4314502730396455 - }, - { - "x": 1.4591047302314346, - "y": 7.181935811218979, - "heading": 0.022297438159640273, - "angularVelocity": -1.0404739477721165, - "velocityX": 2.8569705048235896, - "velocityY": 1.6145716681748061, - "timestamp": 0.4853815571696012 - }, - { - "x": 1.6255168678692307, - "y": 7.279018436792272, - "heading": 0.014123453422294747, - "angularVelocity": -0.15156295402959544, - "velocityX": 3.0856327699670643, - "velocityY": 1.800117077489892, - "timestamp": 0.5393128412995569 - }, - { - "x": 1.793858101376193, - "y": 7.378107838800647, - "heading": 0.014120593150970575, - "angularVelocity": -0.00005303547598235602, - "velocityX": 3.121402284828195, - "velocityY": 1.8373269542331496, - "timestamp": 0.5932441254295127 - }, - { - "x": 1.9621993268656661, - "y": 7.477197254469766, - "heading": 0.014117732942591941, - "angularVelocity": -0.000053034308839039934, - "velocityX": 3.121402136167001, - "velocityY": 1.8373272075322298, - "timestamp": 0.6471754095594684 - }, - { - "x": 2.130540737170093, - "y": 7.576286356115685, - "heading": 0.014114872666943619, - "angularVelocity": -0.00005303555616123241, - "velocityX": 3.121405563026878, - "velocityY": 1.8373213848783807, - "timestamp": 0.7011066936894241 - }, - { - "x": 2.3040566274888983, - "y": 7.663067241706454, - "heading": 0.010006762068371105, - "angularVelocity": -0.0761730536338314, - "velocityX": 3.217351359568822, - "velocityY": 1.609101043870144, - "timestamp": 0.7550379778193799 - }, - { - "x": 2.486186981201172, - "y": 7.724460124969482, - "heading": -5.2714780410694486e-27, - "angularVelocity": -0.18554651960925458, - "velocityX": 3.377081718903682, - "velocityY": 1.1383538191876352, - "timestamp": 0.8089692619493356 - }, - { - "x": 2.8066356107472474, - "y": 7.755704422012626, - "heading": -3.618373256811644e-9, - "angularVelocity": -4.070535151890212e-8, - "velocityX": 3.6049277350997357, - "velocityY": 0.3514867051672969, - "timestamp": 0.8978610936568053 - }, - { - "x": 3.128175858643877, - "y": 7.739109235929212, - "heading": -3.6183723834730783e-9, - "angularVelocity": 9.824733603484871e-15, - "velocityX": 3.6172080349831512, - "velocityY": -0.18668966275806864, - "timestamp": 0.9867529253642751 - }, - { - "x": 3.449716088164231, - "y": 7.722513693799808, - "heading": -3.6183716236893314e-9, - "angularVelocity": 8.547284180686901e-15, - "velocityX": 3.6172078282568996, - "velocityY": -0.18669366814284583, - "timestamp": 1.0756447570717447 - }, - { - "x": 3.771256317684507, - "y": 7.7059181516688815, - "heading": -3.6183708487107216e-9, - "angularVelocity": 8.718220728132132e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.18669366815997415, - "timestamp": 1.1645365887792143 - }, - { - "x": 4.092796547204784, - "y": 7.689322609537955, - "heading": -3.618370111441706e-9, - "angularVelocity": 8.29400184228923e-15, - "velocityX": 3.6172078282560163, - "velocityY": -0.18669366815997418, - "timestamp": 1.253428420486684 - }, - { - "x": 4.414336776725059, - "y": 7.6727270674070285, - "heading": -3.6183692900088547e-9, - "angularVelocity": 9.240813644117634e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.1866936681599742, - "timestamp": 1.3423202521941535 - }, - { - "x": 4.735877006245335, - "y": 7.656131525276102, - "heading": -3.6183684638446085e-9, - "angularVelocity": 9.294040074640308e-15, - "velocityX": 3.6172078282560163, - "velocityY": -0.1866936681599742, - "timestamp": 1.4312120839016231 - }, - { - "x": 5.0574172357656115, - "y": 7.6395359831451755, - "heading": -3.618367662939708e-9, - "angularVelocity": 9.009881839671523e-15, - "velocityX": 3.6172078282560163, - "velocityY": -0.1866936681599742, - "timestamp": 1.5201039156090927 - }, - { - "x": 5.378957465285888, - "y": 7.62294044101425, - "heading": -3.6183667762068362e-9, - "angularVelocity": 9.975414552450845e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.18669366815997418, - "timestamp": 1.6089957473165624 - }, - { - "x": 5.700497694806164, - "y": 7.606344898883323, - "heading": -3.6183660187139743e-9, - "angularVelocity": 8.521512579724434e-15, - "velocityX": 3.6172078282560163, - "velocityY": -0.18669366815997426, - "timestamp": 1.697887579024032 - }, - { - "x": 6.02203792432644, - "y": 7.589749356752397, - "heading": -3.618365173816459e-9, - "angularVelocity": 9.504782370286536e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.1866936681599742, - "timestamp": 1.7867794107315016 - }, - { - "x": 6.343578153846717, - "y": 7.57315381462147, - "heading": -3.6183643942346165e-9, - "angularVelocity": 8.770005387165316e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.1866936681599742, - "timestamp": 1.8756712424389712 - }, - { - "x": 6.665118383366993, - "y": 7.556558272490544, - "heading": -3.6183635769900644e-9, - "angularVelocity": 9.193696838193051e-15, - "velocityX": 3.6172078282560163, - "velocityY": -0.18669366815997424, - "timestamp": 1.9645630741464408 - }, - { - "x": 6.986658612887269, - "y": 7.539962730359617, - "heading": -3.6183627012139706e-9, - "angularVelocity": 9.85215488559423e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.1866936681599742, - "timestamp": 2.0534549058539104 - }, - { - "x": 7.3081988424075455, - "y": 7.523367188228691, - "heading": -3.618361818393418e-9, - "angularVelocity": 9.931402417986347e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.1866936681599742, - "timestamp": 2.1423467375613803 - }, - { - "x": 7.629739071927805, - "y": 7.506771646097765, - "heading": -3.6183609804060354e-9, - "angularVelocity": 9.427045955569345e-15, - "velocityX": 3.617207828255831, - "velocityY": -0.1866936681599647, - "timestamp": 2.23123856926885 - }, - { - "x": 7.922054241421621, - "y": 7.491684486652487, - "heading": -2.171017261999522e-9, - "angularVelocity": 1.6282077786061535e-8, - "velocityX": 3.288436787485536, - "velocityY": -0.16972492472567544, - "timestamp": 2.32013040097632 - }, - { - "x": 8.14129063555876, - "y": 7.480369116190248, - "heading": -1.0855088008297502e-9, - "angularVelocity": 1.221156590340072e-8, - "velocityX": 2.466327782046558, - "velocityY": -0.12729370342458965, - "timestamp": 2.40902223268379 - }, - { - "x": 8.287448236582586, - "y": 7.472825535627514, - "heading": -3.6183629850910975e-10, - "angularVelocity": 8.141046127861815e-9, - "velocityX": 1.6442185768520245, - "velocityY": -0.08486247181359102, - "timestamp": 2.4979140643912596 - }, - { - "x": 8.360527038574219, - "y": 7.469053745269775, - "heading": 6.2013561613280124e-27, - "angularVelocity": 4.070523596587155e-9, - "velocityX": 0.8221093050723073, - "velocityY": -0.042431236765954845, - "timestamp": 2.5868058960987295 - }, - { - "x": 8.360527038574219, - "y": 7.469053745269775, - "heading": 4.2684027559613635e-27, - "angularVelocity": -4.179791912699714e-28, - "velocityX": 2.3088842991913657e-27, - "velocityY": -4.6090179027433225e-27, - "timestamp": 2.6756977278061993 - } - ], - "eventMarkers": [] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/Centerline Auto.2.traj b/src/main/deploy/choreo/Centerline Auto.2.traj deleted file mode 100644 index 7a0d96d..0000000 --- a/src/main/deploy/choreo/Centerline Auto.2.traj +++ /dev/null @@ -1,248 +0,0 @@ -{ - "samples": [ - { - "x": 8.360527038574219, - "y": 7.469053745269775, - "heading": 4.2684027559613635e-27, - "angularVelocity": -4.179791912699714e-28, - "velocityX": 2.3088842991913657e-27, - "velocityY": -4.6090179027433225e-27, - "timestamp": 0 - }, - { - "x": 8.313753662903038, - "y": 7.460040831603572, - "heading": 0.010953449074032317, - "angularVelocity": 0.1521110200792272, - "velocityX": -0.6495438868437788, - "velocityY": -0.12516272111059268, - "timestamp": 0.07200956951263082 - }, - { - "x": 8.220205864520521, - "y": 7.442024067957108, - "heading": 0.03284727412772724, - "angularVelocity": 0.30404049353266294, - "velocityX": -1.299102313979371, - "velocityY": -0.2501995744232807, - "timestamp": 0.14401913902526164 - }, - { - "x": 8.079881048506913, - "y": 7.415017178316717, - "heading": 0.06565325181712828, - "angularVelocity": 0.45557802819035925, - "velocityX": -1.9486967768776025, - "velocityY": -0.37504584214539577, - "timestamp": 0.21602870853789247 - }, - { - "x": 7.892775351387043, - "y": 7.379038650911545, - "heading": 0.10933893425739379, - "angularVelocity": 0.6066649576706988, - "velocityX": -2.598344892022307, - "velocityY": -0.4996353630313118, - "timestamp": 0.2880382780505233 - }, - { - "x": 7.65888403905923, - "y": 7.3341118085455905, - "heading": 0.1638822878842525, - "angularVelocity": 0.7574459060929595, - "velocityX": -3.248058749841399, - "velocityY": -0.623900999131414, - "timestamp": 0.3600478475631541 - }, - { - "x": 7.402696123218692, - "y": 7.2851734321166655, - "heading": 0.1638823285777561, - "angularVelocity": 5.651124407641853e-7, - "velocityX": -3.557692645219899, - "velocityY": -0.6796093458153674, - "timestamp": 0.43205741707578493 - }, - { - "x": 7.146508206567032, - "y": 7.2362350599332865, - "heading": 0.16388236927081118, - "angularVelocity": 5.651062127144245e-7, - "velocityX": -3.557692656483975, - "velocityY": -0.6796092868572866, - "timestamp": 0.5040669865884158 - }, - { - "x": 6.8903202899153655, - "y": 7.187296687749944, - "heading": 0.16388240996386633, - "angularVelocity": 5.651062132417643e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567837, - "timestamp": 0.5760765561010466 - }, - { - "x": 6.634132373263698, - "y": 7.138358315566601, - "heading": 0.16388245065692145, - "angularVelocity": 5.651062127692066e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567831, - "timestamp": 0.6480861256136774 - }, - { - "x": 6.377944456612031, - "y": 7.089419943383258, - "heading": 0.1638824913499765, - "angularVelocity": 5.651062124923317e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567826, - "timestamp": 0.7200956951263082 - }, - { - "x": 6.121756539960365, - "y": 7.040481571199916, - "heading": 0.1638825320430316, - "angularVelocity": 5.65106212535781e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567821, - "timestamp": 0.792105264638939 - }, - { - "x": 5.865568623308698, - "y": 6.991543199016573, - "heading": 0.16388257273608667, - "angularVelocity": 5.651062121582895e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567813, - "timestamp": 0.8641148341515699 - }, - { - "x": 5.6093807066570305, - "y": 6.94260482683323, - "heading": 0.16388261342914182, - "angularVelocity": 5.651062135141695e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567811, - "timestamp": 0.9361244036642007 - }, - { - "x": 5.35319279000551, - "y": 6.893666454649124, - "heading": 0.16388265412219696, - "angularVelocity": 5.651062132389692e-7, - "velocityX": -3.5576926564820495, - "velocityY": -0.6796092868673679, - "timestamp": 1.0081339731768315 - }, - { - "x": 5.0970048904418945, - "y": 6.844727993011475, - "heading": 0.16388269481525294, - "angularVelocity": 5.651062246743821e-7, - "velocityX": -3.5576924191815573, - "velocityY": -0.6796105291126032, - "timestamp": 1.0801435426894623 - }, - { - "x": 4.821698218246571, - "y": 6.778961174770714, - "heading": 0.16388272951436206, - "angularVelocity": 4.4401904271117674e-7, - "velocityX": -3.522897507544664, - "velocityY": -0.8415697237266404, - "timestamp": 1.1582913376808648 - }, - { - "x": 4.546391565680663, - "y": 6.7131942743592505, - "heading": 0.163882764213471, - "angularVelocity": 4.440190404819063e-7, - "velocityX": -3.5228972563614294, - "velocityY": -0.8415707752048409, - "timestamp": 1.2364391326722672 - }, - { - "x": 4.271084913114876, - "y": 6.647427373947279, - "heading": 0.16388279891257995, - "angularVelocity": 4.440190403154282e-7, - "velocityX": -3.522897256359876, - "velocityY": -0.8415707752113447, - "timestamp": 1.3145869276636697 - }, - { - "x": 3.995778260549088, - "y": 6.5816604735353135, - "heading": 0.1638828336116889, - "angularVelocity": 4.440190406386135e-7, - "velocityX": -3.5228972563598964, - "velocityY": -0.8415707752112599, - "timestamp": 1.3927347226550721 - }, - { - "x": 3.7204716077277675, - "y": 6.515893574193041, - "heading": 0.16388286831080823, - "angularVelocity": 4.440191733622333e-7, - "velocityX": -3.522897259629757, - "velocityY": -0.8415707615231752, - "timestamp": 1.470882517646475 - }, - { - "x": 3.446523890940015, - "y": 6.450633718978252, - "heading": 0.1690942590558449, - "angularVelocity": 0.06668634406908087, - "velocityX": -3.50550795218074, - "velocityY": -0.8350824898126525, - "timestamp": 1.549030312637878 - }, - { - "x": 3.2268656747177786, - "y": 6.398260340243826, - "heading": 0.190922516728536, - "angularVelocity": 0.27932019931071, - "velocityX": -2.8108050425018627, - "velocityY": -0.670183704353886, - "timestamp": 1.6271781076292808 - }, - { - "x": 3.062219316413325, - "y": 6.3589866212900406, - "heading": 0.21130201229843995, - "angularVelocity": 0.2607814535540765, - "velocityX": -2.1068586557377182, - "velocityY": -0.5025569686016857, - "timestamp": 1.7053259026206837 - }, - { - "x": 2.9525091880643317, - "y": 6.3328115268917164, - "heading": 0.22637932021512078, - "angularVelocity": 0.1929332480633594, - "velocityX": -1.4038800245235723, - "velocityY": -0.334943479866623, - "timestamp": 1.7834736976120866 - }, - { - "x": 2.897675037384033, - "y": 6.319726943969727, - "heading": 0.23441548450132177, - "angularVelocity": 0.10283289870283706, - "velocityX": -0.7016723976195487, - "velocityY": -0.1674338082530608, - "timestamp": 1.8616214926034895 - }, - { - "x": 2.897675037384033, - "y": 6.319726943969727, - "heading": 0.23441548450132177, - "angularVelocity": 2.348167343500178e-28, - "velocityX": 2.328514438851991e-27, - "velocityY": -3.011007529624761e-27, - "timestamp": 1.9397692875948924 - } - ], - "eventMarkers": [] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/Centerline Auto.3.traj b/src/main/deploy/choreo/Centerline Auto.3.traj deleted file mode 100644 index fdb2b6d..0000000 --- a/src/main/deploy/choreo/Centerline Auto.3.traj +++ /dev/null @@ -1,239 +0,0 @@ -{ - "samples": [ - { - "x": 2.897675037384033, - "y": 6.319726943969727, - "heading": 0.23441548450132177, - "angularVelocity": 2.348167343500178e-28, - "velocityX": 2.328514438851991e-27, - "velocityY": -3.011007529624761e-27, - "timestamp": 0 - }, - { - "x": 2.931995611661183, - "y": 6.317030660120305, - "heading": 0.22827399101803314, - "angularVelocity": -0.1004895685231517, - "velocityX": 0.5615669396966905, - "velocityY": -0.04411767290505802, - "timestamp": 0.06111573144901783 - }, - { - "x": 3.0006366597167062, - "y": 6.311640001770152, - "heading": 0.21599961790492203, - "angularVelocity": -0.20083819373658737, - "velocityX": 1.1231322349922719, - "velocityY": -0.08820410428451562, - "timestamp": 0.12223146289803566 - }, - { - "x": 3.1035985534895527, - "y": 6.303558029226905, - "heading": 0.1976138856084477, - "angularVelocity": -0.30083469281246417, - "velocityX": 1.684703616101463, - "velocityY": -0.13224046168846443, - "timestamp": 0.1833471943470535 - }, - { - "x": 3.2408820865427965, - "y": 6.29278900601246, - "heading": 0.173149008298291, - "angularVelocity": -0.4003040907816843, - "velocityX": 2.2462879818065202, - "velocityY": -0.17620705764487352, - "timestamp": 0.24446292579607132 - }, - { - "x": 3.412488399373154, - "y": 6.279338451783206, - "heading": 0.14264484283477577, - "angularVelocity": -0.4991213348884074, - "velocityX": 2.807891008774219, - "velocityY": -0.22008333877955472, - "timestamp": 0.30557865724508915 - }, - { - "x": 3.618418876396286, - "y": 6.263213190116882, - "heading": 0.10614503413984216, - "angularVelocity": -0.597224443355985, - "velocityX": 3.3695166881037326, - "velocityY": -0.26384796981077935, - "timestamp": 0.366694388694107 - }, - { - "x": 3.839130420023726, - "y": 6.246248871896369, - "heading": 0.10614500290272542, - "angularVelocity": -5.111141764182067e-7, - "velocityX": 3.611370401605275, - "velocityY": -0.2775769481653736, - "timestamp": 0.4278101201431248 - }, - { - "x": 4.059841964366672, - "y": 6.229284562983422, - "heading": 0.10614497166605225, - "angularVelocity": -5.111069183870487e-7, - "velocityX": 3.6113704133126814, - "velocityY": -0.2775767958712516, - "timestamp": 0.48892585159214264 - }, - { - "x": 4.280553508709572, - "y": 6.212320254069855, - "heading": 0.10614494042937911, - "angularVelocity": -5.111069180282712e-7, - "velocityX": 3.6113704133119002, - "velocityY": -0.2775767958814193, - "timestamp": 0.5500415830411605 - }, - { - "x": 4.501265048980713, - "y": 6.1953558921813965, - "heading": 0.1061449091927048, - "angularVelocity": -5.111069369418327e-7, - "velocityX": 3.611370346688179, - "velocityY": -0.27757766267774153, - "timestamp": 0.6111573144901783 - }, - { - "x": 4.810869637193169, - "y": 6.16135848513122, - "heading": 0.10614488355477533, - "angularVelocity": -2.98142570668873e-7, - "velocityX": 3.6003807524808256, - "velocityY": -0.39535463826432365, - "timestamp": 0.6971494940670535 - }, - { - "x": 5.120474220145008, - "y": 6.12736103017416, - "heading": 0.1061448579168455, - "angularVelocity": -2.9814257488756455e-7, - "velocityX": 3.600380691305304, - "velocityY": -0.39535519537177727, - "timestamp": 0.7831416736439287 - }, - { - "x": 5.430078803096823, - "y": 6.093363575216878, - "heading": 0.1061448322789156, - "angularVelocity": -2.981425755600158e-7, - "velocityX": 3.6003806913050207, - "velocityY": -0.39535519537435887, - "timestamp": 0.8691338532208039 - }, - { - "x": 5.739683386048638, - "y": 6.059366120259595, - "heading": 0.10614480664098574, - "angularVelocity": -2.981425750224502e-7, - "velocityX": 3.60038069130502, - "velocityY": -0.39535519537435865, - "timestamp": 0.9551260327976792 - }, - { - "x": 6.049287969000453, - "y": 6.025368665302312, - "heading": 0.10614478100305595, - "angularVelocity": -2.981425744595309e-7, - "velocityX": 3.60038069130502, - "velocityY": -0.3953551953743586, - "timestamp": 1.0411182123745544 - }, - { - "x": 6.358892551952268, - "y": 5.991371210345029, - "heading": 0.1061447553651262, - "angularVelocity": -2.98142573863633e-7, - "velocityX": 3.6003806913050203, - "velocityY": -0.3953551953743583, - "timestamp": 1.1271103919514296 - }, - { - "x": 6.668497134904083, - "y": 5.9573737553877475, - "heading": 0.10614472972719659, - "angularVelocity": -2.981425722498942e-7, - "velocityX": 3.6003806913050203, - "velocityY": -0.3953551953743583, - "timestamp": 1.2131025715283048 - }, - { - "x": 6.978101717855898, - "y": 5.923376300430465, - "heading": 0.10614470408926703, - "angularVelocity": -2.981425716615336e-7, - "velocityX": 3.6003806913050207, - "velocityY": -0.39535519537435826, - "timestamp": 1.29909475110518 - }, - { - "x": 7.287706300807711, - "y": 5.889378845473171, - "heading": 0.10614467845133739, - "angularVelocity": -2.9814257255193557e-7, - "velocityX": 3.600380691305006, - "velocityY": -0.39535519537448677, - "timestamp": 1.3850869306820552 - }, - { - "x": 7.597310883497341, - "y": 5.855381388128685, - "heading": 0.10614465281325862, - "angularVelocity": -2.981443068468836e-7, - "velocityX": 3.600380688256082, - "velocityY": -0.39535522313506416, - "timestamp": 1.4710791102589305 - }, - { - "x": 7.868541995298217, - "y": 5.825436456932061, - "heading": 0.06366050775847867, - "angularVelocity": -0.4940466128876299, - "velocityX": 3.154136959145237, - "velocityY": -0.34822854059484143, - "timestamp": 1.5570712898358057 - }, - { - "x": 8.071966668862895, - "y": 5.802984598737895, - "heading": 0.03182759417957115, - "angularVelocity": -0.3701838206164972, - "velocityX": 2.365618298845671, - "velocityY": -0.2610918609650363, - "timestamp": 1.6430634694126809 - }, - { - "x": 8.207583377724607, - "y": 5.788018984348283, - "heading": 0.01061131016646268, - "angularVelocity": -0.24672341272780052, - "velocityX": 1.577081887318283, - "velocityY": -0.17403459783495048, - "timestamp": 1.729055648989556 - }, - { - "x": 8.275391578674316, - "y": 5.78053617477417, - "heading": 6.762629528326585e-25, - "angularVelocity": -0.12339854878287405, - "velocityX": 0.7885391588323608, - "velocityY": -0.08701732658634945, - "timestamp": 1.8150478285664313 - }, - { - "x": 8.275391578674316, - "y": 5.78053617477417, - "heading": 6.890807484355009e-25, - "angularVelocity": 1.714109587590018e-25, - "velocityX": -1.5632941987910777e-25, - "velocityY": 4.01842027987716e-26, - "timestamp": 1.9010400081433065 - } - ], - "eventMarkers": [] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/Centerline Auto.4.traj b/src/main/deploy/choreo/Centerline Auto.4.traj deleted file mode 100644 index 3ab4d9f..0000000 --- a/src/main/deploy/choreo/Centerline Auto.4.traj +++ /dev/null @@ -1,239 +0,0 @@ -{ - "samples": [ - { - "x": 8.275391578674316, - "y": 5.78053617477417, - "heading": 6.890807484355009e-25, - "angularVelocity": 1.714109587590018e-25, - "velocityX": -1.5632941987910777e-25, - "velocityY": 4.01842027987716e-26, - "timestamp": 0 - }, - { - "x": 8.217699478068795, - "y": 5.791359796793642, - "heading": 0.01104341509549353, - "angularVelocity": 0.13833447495381113, - "velocityX": -0.7226755833441465, - "velocityY": 0.1355812541876774, - "timestamp": 0.07983125753129006 - }, - { - "x": 8.10231615629365, - "y": 5.8130147597317485, - "heading": 0.03315104134696604, - "angularVelocity": 0.276929450131827, - "velocityX": -1.4453401505033163, - "velocityY": 0.2712591985616573, - "timestamp": 0.1596625150625801 - }, - { - "x": 7.929243964127431, - "y": 5.845512314072997, - "heading": 0.06637865596420629, - "angularVelocity": 0.4162231141632274, - "velocityX": -2.167975270818996, - "velocityY": 0.40707807124934986, - "timestamp": 0.23949377259387017 - }, - { - "x": 7.698485965371198, - "y": 5.888866692718938, - "heading": 0.11081243304487726, - "angularVelocity": 0.5565962312851562, - "velocityX": -2.8905720126704284, - "velocityY": 0.5430752312645023, - "timestamp": 0.3193250301251602 - }, - { - "x": 7.41422585651022, - "y": 5.941821981236076, - "heading": 0.11081246572396981, - "angularVelocity": 4.0935209556931575e-7, - "velocityX": -3.5607620079085063, - "velocityY": 0.6633402774142934, - "timestamp": 0.3991562876564503 - }, - { - "x": 7.129965619426801, - "y": 5.994776581882574, - "heading": 0.11081249814230922, - "angularVelocity": 4.060857915201533e-7, - "velocityX": -3.5607636140768695, - "velocityY": 0.6633316608565432, - "timestamp": 0.47898754518774034 - }, - { - "x": 6.8457053823426035, - "y": 6.047731182524891, - "heading": 0.11081253056064869, - "angularVelocity": 4.060857924168534e-7, - "velocityX": -3.560763614086623, - "velocityY": 0.6633316608041847, - "timestamp": 0.5588188027190304 - }, - { - "x": 6.561445145258405, - "y": 6.100685783167208, - "heading": 0.11081256297898824, - "angularVelocity": 4.060857934452354e-7, - "velocityX": -3.5607636140866235, - "velocityY": 0.6633316608041844, - "timestamp": 0.6386500602503205 - }, - { - "x": 6.277184908174208, - "y": 6.153640383809526, - "heading": 0.11081259539732799, - "angularVelocity": 4.060857956958186e-7, - "velocityX": -3.560763614086623, - "velocityY": 0.6633316608041847, - "timestamp": 0.7184813177816105 - }, - { - "x": 5.99292467109001, - "y": 6.206594984451843, - "heading": 0.11081262781566778, - "angularVelocity": 4.060857964387038e-7, - "velocityX": -3.560763614086623, - "velocityY": 0.6633316608041848, - "timestamp": 0.7983125753129006 - }, - { - "x": 5.708664434005791, - "y": 6.259549585094043, - "heading": 0.11081266023400761, - "angularVelocity": 4.060857968841719e-7, - "velocityX": -3.5607636140868966, - "velocityY": 0.6633316608027189, - "timestamp": 0.8781438328441906 - }, - { - "x": 5.424404193332451, - "y": 6.312504166469845, - "heading": 0.11081269265234713, - "angularVelocity": 4.060857929641026e-7, - "velocityX": -3.5607636590457346, - "velocityY": 0.6633314194636794, - "timestamp": 0.9579750903754807 - }, - { - "x": 5.139572620391846, - "y": 6.3622941970825195, - "heading": 0.11081272507571231, - "angularVelocity": 4.061487466795785e-7, - "velocityX": -3.567920407980088, - "velocityY": 0.6236909219820286, - "timestamp": 1.0378063479067707 - }, - { - "x": 4.8822045552292, - "y": 6.360012293211402, - "heading": 0.1108127534765096, - "angularVelocity": 3.996777164840354e-7, - "velocityX": -3.6218800325558864, - "velocityY": -0.03211269456367951, - "timestamp": 1.108865594168173 - }, - { - "x": 4.624881625896748, - "y": 6.3546795533312395, - "heading": 0.11081278178743512, - "angularVelocity": 3.98412972255652e-7, - "velocityX": -3.6212448466713205, - "velocityY": -0.07504638960770253, - "timestamp": 1.1799248404295755 - }, - { - "x": 4.367558697083316, - "y": 6.349346788406661, - "heading": 0.11081281009836048, - "angularVelocity": 3.9841297029584103e-7, - "velocityX": -3.62124483936728, - "velocityY": -0.07504674205185301, - "timestamp": 1.2509840866909778 - }, - { - "x": 4.110235768269888, - "y": 6.344014023481877, - "heading": 0.11081283840928584, - "angularVelocity": 3.9841296996573985e-7, - "velocityX": -3.6212448393672196, - "velocityY": -0.075046742054734, - "timestamp": 1.3220433329523802 - }, - { - "x": 3.852912839456459, - "y": 6.338681258557136, - "heading": 0.11081286672021123, - "angularVelocity": 3.984129707844367e-7, - "velocityX": -3.621244839367232, - "velocityY": -0.07504674205415067, - "timestamp": 1.3931025792137826 - }, - { - "x": 3.5955899105380604, - "y": 6.333348498701343, - "heading": 0.110812895031426, - "angularVelocity": 3.98417042898143e-7, - "velocityX": -3.6212448408444415, - "velocityY": -0.07504667072002474, - "timestamp": 1.464161825475185 - }, - { - "x": 3.3629508998707167, - "y": 6.328797181710379, - "heading": 0.15195588309543634, - "angularVelocity": 0.5789955597426332, - "velocityX": -3.2738738856241922, - "velocityY": -0.06404960973301323, - "timestamp": 1.5352210717365873 - }, - { - "x": 3.176840519639808, - "y": 6.325164863882251, - "heading": 0.18492044940341654, - "angularVelocity": 0.4639025607830811, - "velocityX": -2.6190874519872214, - "velocityY": -0.05111675143253586, - "timestamp": 1.6062803179979896 - }, - { - "x": 3.0372579905429093, - "y": 6.322444909479725, - "heading": 0.2096671002374919, - "angularVelocity": 0.3482537760538718, - "velocityX": -1.9643119852893276, - "velocityY": -0.03827727629589979, - "timestamp": 1.677339564259392 - }, - { - "x": 2.944202841875484, - "y": 6.320632970128358, - "heading": 0.22616886731909797, - "angularVelocity": 0.2322254731059452, - "velocityX": -1.3095431427052717, - "velocityY": -0.025498994806420823, - "timestamp": 1.7483988105207944 - }, - { - "x": 2.897675037384033, - "y": 6.319726943969727, - "heading": 0.23441548450132177, - "angularVelocity": 0.11605269709570704, - "velocityX": -0.6547748103081646, - "velocityY": -0.012750292274397987, - "timestamp": 1.8194580567821967 - }, - { - "x": 2.897675037384033, - "y": 6.319726943969727, - "heading": 0.23441548450132177, - "angularVelocity": 6.477597733926451e-26, - "velocityX": -1.5355730051117952e-25, - "velocityY": -1.2183012914286086e-25, - "timestamp": 1.890517303043599 - } - ], - "eventMarkers": [] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/Centerline Auto.traj b/src/main/deploy/choreo/Centerline Auto.traj deleted file mode 100644 index 92fc558..0000000 --- a/src/main/deploy/choreo/Centerline Auto.traj +++ /dev/null @@ -1,1022 +0,0 @@ -{ - "samples": [ - { - "x": 0.660783052444458, - "y": 6.694385051727295, - "heading": 1.0498621745809171, - "angularVelocity": 5.825284855617402e-28, - "velocityX": -1.9685293298057674e-27, - "velocityY": 1.4418564963898336e-27, - "timestamp": 0 - }, - { - "x": 0.6784036347143663, - "y": 6.707198268292706, - "heading": 1.0175475056665193, - "angularVelocity": -0.5991822637957332, - "velocityX": 0.32672283914932904, - "velocityY": 0.23758411786628814, - "timestamp": 0.05393128412995569 - }, - { - "x": 0.7135162843776108, - "y": 6.732573985913541, - "heading": 0.9516130808872657, - "angularVelocity": -1.2225635981589924, - "velocityX": 0.6510627408506594, - "velocityY": 0.4705194402508243, - "timestamp": 0.10786256825991138 - }, - { - "x": 0.7660994692609764, - "y": 6.770017684654193, - "heading": 0.8504346226500515, - "angularVelocity": -1.8760624722639487, - "velocityX": 0.9750033905489506, - "velocityY": 0.6942853177837462, - "timestamp": 0.16179385238986707 - }, - { - "x": 0.836479083356807, - "y": 6.818804043384565, - "heading": 0.7130270962325688, - "angularVelocity": -2.5478259721459318, - "velocityX": 1.3049868036933845, - "velocityY": 0.9046022084846815, - "timestamp": 0.21572513651982275 - }, - { - "x": 0.9255037273969096, - "y": 6.878132431612943, - "heading": 0.5402736783143448, - "angularVelocity": -3.2032135096569965, - "velocityX": 1.6507050680563082, - "velocityY": 1.1000737176110378, - "timestamp": 0.26965642064977846 - }, - { - "x": 1.035016950774323, - "y": 6.947674803955847, - "heading": 0.3381434888609651, - "angularVelocity": -3.747920946334524, - "velocityX": 2.0306066347970617, - "velocityY": 1.289462571952319, - "timestamp": 0.32358770477973414 - }, - { - "x": 1.162461605941772, - "y": 7.018689321892079, - "heading": 0.18269313635392612, - "angularVelocity": -2.882378104190095, - "velocityX": 2.3630932810787844, - "velocityY": 1.3167592628633178, - "timestamp": 0.3775189889096898 - }, - { - "x": 1.3050246421848908, - "y": 7.094859887834467, - "heading": 0.07841153426675496, - "angularVelocity": -1.9336013182235503, - "velocityX": 2.6434200212921195, - "velocityY": 1.4123632910138557, - "timestamp": 0.4314502730396455 - }, - { - "x": 1.4591047302314346, - "y": 7.181935811218979, - "heading": 0.022297438159640273, - "angularVelocity": -1.0404739477721165, - "velocityX": 2.8569705048235896, - "velocityY": 1.6145716681748061, - "timestamp": 0.4853815571696012 - }, - { - "x": 1.6255168678692307, - "y": 7.279018436792272, - "heading": 0.014123453422294747, - "angularVelocity": -0.15156295402959544, - "velocityX": 3.0856327699670643, - "velocityY": 1.800117077489892, - "timestamp": 0.5393128412995569 - }, - { - "x": 1.793858101376193, - "y": 7.378107838800647, - "heading": 0.014120593150970575, - "angularVelocity": -0.00005303547598235602, - "velocityX": 3.121402284828195, - "velocityY": 1.8373269542331496, - "timestamp": 0.5932441254295127 - }, - { - "x": 1.9621993268656661, - "y": 7.477197254469766, - "heading": 0.014117732942591941, - "angularVelocity": -0.000053034308839039934, - "velocityX": 3.121402136167001, - "velocityY": 1.8373272075322298, - "timestamp": 0.6471754095594684 - }, - { - "x": 2.130540737170093, - "y": 7.576286356115685, - "heading": 0.014114872666943619, - "angularVelocity": -0.00005303555616123241, - "velocityX": 3.121405563026878, - "velocityY": 1.8373213848783807, - "timestamp": 0.7011066936894241 - }, - { - "x": 2.3040566274888983, - "y": 7.663067241706454, - "heading": 0.010006762068371105, - "angularVelocity": -0.0761730536338314, - "velocityX": 3.217351359568822, - "velocityY": 1.609101043870144, - "timestamp": 0.7550379778193799 - }, - { - "x": 2.486186981201172, - "y": 7.724460124969482, - "heading": -5.2714780410694486e-27, - "angularVelocity": -0.18554651960925458, - "velocityX": 3.377081718903682, - "velocityY": 1.1383538191876352, - "timestamp": 0.8089692619493356 - }, - { - "x": 2.8066356107472474, - "y": 7.755704422012626, - "heading": -3.618373256811644e-9, - "angularVelocity": -4.070535151890212e-8, - "velocityX": 3.6049277350997357, - "velocityY": 0.3514867051672969, - "timestamp": 0.8978610936568053 - }, - { - "x": 3.128175858643877, - "y": 7.739109235929212, - "heading": -3.6183723834730783e-9, - "angularVelocity": 9.824733603484871e-15, - "velocityX": 3.6172080349831512, - "velocityY": -0.18668966275806864, - "timestamp": 0.9867529253642751 - }, - { - "x": 3.449716088164231, - "y": 7.722513693799808, - "heading": -3.6183716236893314e-9, - "angularVelocity": 8.547284180686901e-15, - "velocityX": 3.6172078282568996, - "velocityY": -0.18669366814284583, - "timestamp": 1.0756447570717447 - }, - { - "x": 3.771256317684507, - "y": 7.7059181516688815, - "heading": -3.6183708487107216e-9, - "angularVelocity": 8.718220728132132e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.18669366815997415, - "timestamp": 1.1645365887792143 - }, - { - "x": 4.092796547204784, - "y": 7.689322609537955, - "heading": -3.618370111441706e-9, - "angularVelocity": 8.29400184228923e-15, - "velocityX": 3.6172078282560163, - "velocityY": -0.18669366815997418, - "timestamp": 1.253428420486684 - }, - { - "x": 4.414336776725059, - "y": 7.6727270674070285, - "heading": -3.6183692900088547e-9, - "angularVelocity": 9.240813644117634e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.1866936681599742, - "timestamp": 1.3423202521941535 - }, - { - "x": 4.735877006245335, - "y": 7.656131525276102, - "heading": -3.6183684638446085e-9, - "angularVelocity": 9.294040074640308e-15, - "velocityX": 3.6172078282560163, - "velocityY": -0.1866936681599742, - "timestamp": 1.4312120839016231 - }, - { - "x": 5.0574172357656115, - "y": 7.6395359831451755, - "heading": -3.618367662939708e-9, - "angularVelocity": 9.009881839671523e-15, - "velocityX": 3.6172078282560163, - "velocityY": -0.1866936681599742, - "timestamp": 1.5201039156090927 - }, - { - "x": 5.378957465285888, - "y": 7.62294044101425, - "heading": -3.6183667762068362e-9, - "angularVelocity": 9.975414552450845e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.18669366815997418, - "timestamp": 1.6089957473165624 - }, - { - "x": 5.700497694806164, - "y": 7.606344898883323, - "heading": -3.6183660187139743e-9, - "angularVelocity": 8.521512579724434e-15, - "velocityX": 3.6172078282560163, - "velocityY": -0.18669366815997426, - "timestamp": 1.697887579024032 - }, - { - "x": 6.02203792432644, - "y": 7.589749356752397, - "heading": -3.618365173816459e-9, - "angularVelocity": 9.504782370286536e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.1866936681599742, - "timestamp": 1.7867794107315016 - }, - { - "x": 6.343578153846717, - "y": 7.57315381462147, - "heading": -3.6183643942346165e-9, - "angularVelocity": 8.770005387165316e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.1866936681599742, - "timestamp": 1.8756712424389712 - }, - { - "x": 6.665118383366993, - "y": 7.556558272490544, - "heading": -3.6183635769900644e-9, - "angularVelocity": 9.193696838193051e-15, - "velocityX": 3.6172078282560163, - "velocityY": -0.18669366815997424, - "timestamp": 1.9645630741464408 - }, - { - "x": 6.986658612887269, - "y": 7.539962730359617, - "heading": -3.6183627012139706e-9, - "angularVelocity": 9.85215488559423e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.1866936681599742, - "timestamp": 2.0534549058539104 - }, - { - "x": 7.3081988424075455, - "y": 7.523367188228691, - "heading": -3.618361818393418e-9, - "angularVelocity": 9.931402417986347e-15, - "velocityX": 3.617207828256016, - "velocityY": -0.1866936681599742, - "timestamp": 2.1423467375613803 - }, - { - "x": 7.629739071927805, - "y": 7.506771646097765, - "heading": -3.6183609804060354e-9, - "angularVelocity": 9.427045955569345e-15, - "velocityX": 3.617207828255831, - "velocityY": -0.1866936681599647, - "timestamp": 2.23123856926885 - }, - { - "x": 7.922054241421621, - "y": 7.491684486652487, - "heading": -2.171017261999522e-9, - "angularVelocity": 1.6282077786061535e-8, - "velocityX": 3.288436787485536, - "velocityY": -0.16972492472567544, - "timestamp": 2.32013040097632 - }, - { - "x": 8.14129063555876, - "y": 7.480369116190248, - "heading": -1.0855088008297502e-9, - "angularVelocity": 1.221156590340072e-8, - "velocityX": 2.466327782046558, - "velocityY": -0.12729370342458965, - "timestamp": 2.40902223268379 - }, - { - "x": 8.287448236582586, - "y": 7.472825535627514, - "heading": -3.6183629850910975e-10, - "angularVelocity": 8.141046127861815e-9, - "velocityX": 1.6442185768520245, - "velocityY": -0.08486247181359102, - "timestamp": 2.4979140643912596 - }, - { - "x": 8.360527038574219, - "y": 7.469053745269775, - "heading": 6.2013561613280124e-27, - "angularVelocity": 4.070523596587155e-9, - "velocityX": 0.8221093050723073, - "velocityY": -0.042431236765954845, - "timestamp": 2.5868058960987295 - }, - { - "x": 8.360527038574219, - "y": 7.469053745269775, - "heading": 4.2684027559613635e-27, - "angularVelocity": -4.179791912699714e-28, - "velocityX": 2.3088842991913657e-27, - "velocityY": -4.6090179027433225e-27, - "timestamp": 2.6756977278061993 - }, - { - "x": 8.313753662903038, - "y": 7.460040831603572, - "heading": 0.010953449074032317, - "angularVelocity": 0.1521110200792272, - "velocityX": -0.6495438868437788, - "velocityY": -0.12516272111059268, - "timestamp": 2.74770729731883 - }, - { - "x": 8.220205864520521, - "y": 7.442024067957108, - "heading": 0.03284727412772724, - "angularVelocity": 0.30404049353266294, - "velocityX": -1.299102313979371, - "velocityY": -0.2501995744232807, - "timestamp": 2.819716866831461 - }, - { - "x": 8.079881048506913, - "y": 7.415017178316717, - "heading": 0.06565325181712828, - "angularVelocity": 0.45557802819035925, - "velocityX": -1.9486967768776025, - "velocityY": -0.37504584214539577, - "timestamp": 2.8917264363440918 - }, - { - "x": 7.892775351387043, - "y": 7.379038650911545, - "heading": 0.10933893425739379, - "angularVelocity": 0.6066649576706988, - "velocityX": -2.598344892022307, - "velocityY": -0.4996353630313118, - "timestamp": 2.9637360058567226 - }, - { - "x": 7.65888403905923, - "y": 7.3341118085455905, - "heading": 0.1638822878842525, - "angularVelocity": 0.7574459060929595, - "velocityX": -3.248058749841399, - "velocityY": -0.623900999131414, - "timestamp": 3.0357455753693534 - }, - { - "x": 7.402696123218692, - "y": 7.2851734321166655, - "heading": 0.1638823285777561, - "angularVelocity": 5.651124407641853e-7, - "velocityX": -3.557692645219899, - "velocityY": -0.6796093458153674, - "timestamp": 3.1077551448819842 - }, - { - "x": 7.146508206567032, - "y": 7.2362350599332865, - "heading": 0.16388236927081118, - "angularVelocity": 5.651062127144245e-7, - "velocityX": -3.557692656483975, - "velocityY": -0.6796092868572866, - "timestamp": 3.179764714394615 - }, - { - "x": 6.8903202899153655, - "y": 7.187296687749944, - "heading": 0.16388240996386633, - "angularVelocity": 5.651062132417643e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567837, - "timestamp": 3.251774283907246 - }, - { - "x": 6.634132373263698, - "y": 7.138358315566601, - "heading": 0.16388245065692145, - "angularVelocity": 5.651062127692066e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567831, - "timestamp": 3.3237838534198767 - }, - { - "x": 6.377944456612031, - "y": 7.089419943383258, - "heading": 0.1638824913499765, - "angularVelocity": 5.651062124923317e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567826, - "timestamp": 3.3957934229325075 - }, - { - "x": 6.121756539960365, - "y": 7.040481571199916, - "heading": 0.1638825320430316, - "angularVelocity": 5.65106212535781e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567821, - "timestamp": 3.4678029924451383 - }, - { - "x": 5.865568623308698, - "y": 6.991543199016573, - "heading": 0.16388257273608667, - "angularVelocity": 5.651062121582895e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567813, - "timestamp": 3.539812561957769 - }, - { - "x": 5.6093807066570305, - "y": 6.94260482683323, - "heading": 0.16388261342914182, - "angularVelocity": 5.651062135141695e-7, - "velocityX": -3.5576926564840714, - "velocityY": -0.6796092868567811, - "timestamp": 3.6118221314704 - }, - { - "x": 5.35319279000551, - "y": 6.893666454649124, - "heading": 0.16388265412219696, - "angularVelocity": 5.651062132389692e-7, - "velocityX": -3.5576926564820495, - "velocityY": -0.6796092868673679, - "timestamp": 3.683831700983031 - }, - { - "x": 5.0970048904418945, - "y": 6.844727993011475, - "heading": 0.16388269481525294, - "angularVelocity": 5.651062246743821e-7, - "velocityX": -3.5576924191815573, - "velocityY": -0.6796105291126032, - "timestamp": 3.7558412704956616 - }, - { - "x": 4.821698218246571, - "y": 6.778961174770714, - "heading": 0.16388272951436206, - "angularVelocity": 4.4401904271117674e-7, - "velocityX": -3.522897507544664, - "velocityY": -0.8415697237266404, - "timestamp": 3.833989065487064 - }, - { - "x": 4.546391565680663, - "y": 6.7131942743592505, - "heading": 0.163882764213471, - "angularVelocity": 4.440190404819063e-7, - "velocityX": -3.5228972563614294, - "velocityY": -0.8415707752048409, - "timestamp": 3.9121368604784665 - }, - { - "x": 4.271084913114876, - "y": 6.647427373947279, - "heading": 0.16388279891257995, - "angularVelocity": 4.440190403154282e-7, - "velocityX": -3.522897256359876, - "velocityY": -0.8415707752113447, - "timestamp": 3.990284655469869 - }, - { - "x": 3.995778260549088, - "y": 6.5816604735353135, - "heading": 0.1638828336116889, - "angularVelocity": 4.440190406386135e-7, - "velocityX": -3.5228972563598964, - "velocityY": -0.8415707752112599, - "timestamp": 4.068432450461271 - }, - { - "x": 3.7204716077277675, - "y": 6.515893574193041, - "heading": 0.16388286831080823, - "angularVelocity": 4.440191733622333e-7, - "velocityX": -3.522897259629757, - "velocityY": -0.8415707615231752, - "timestamp": 4.146580245452674 - }, - { - "x": 3.446523890940015, - "y": 6.450633718978252, - "heading": 0.1690942590558449, - "angularVelocity": 0.06668634406908087, - "velocityX": -3.50550795218074, - "velocityY": -0.8350824898126525, - "timestamp": 4.224728040444077 - }, - { - "x": 3.2268656747177786, - "y": 6.398260340243826, - "heading": 0.190922516728536, - "angularVelocity": 0.27932019931071, - "velocityX": -2.8108050425018627, - "velocityY": -0.670183704353886, - "timestamp": 4.30287583543548 - }, - { - "x": 3.062219316413325, - "y": 6.3589866212900406, - "heading": 0.21130201229843995, - "angularVelocity": 0.2607814535540765, - "velocityX": -2.1068586557377182, - "velocityY": -0.5025569686016857, - "timestamp": 4.381023630426883 - }, - { - "x": 2.9525091880643317, - "y": 6.3328115268917164, - "heading": 0.22637932021512078, - "angularVelocity": 0.1929332480633594, - "velocityX": -1.4038800245235723, - "velocityY": -0.334943479866623, - "timestamp": 4.459171425418286 - }, - { - "x": 2.897675037384033, - "y": 6.319726943969727, - "heading": 0.23441548450132177, - "angularVelocity": 0.10283289870283706, - "velocityX": -0.7016723976195487, - "velocityY": -0.1674338082530608, - "timestamp": 4.537319220409689 - }, - { - "x": 2.897675037384033, - "y": 6.319726943969727, - "heading": 0.23441548450132177, - "angularVelocity": 2.348167343500178e-28, - "velocityX": 2.328514438851991e-27, - "velocityY": -3.011007529624761e-27, - "timestamp": 4.615467015401092 - }, - { - "x": 2.931995611661183, - "y": 6.317030660120305, - "heading": 0.22827399101803314, - "angularVelocity": -0.1004895685231517, - "velocityX": 0.5615669396966905, - "velocityY": -0.04411767290505802, - "timestamp": 4.6765827468501096 - }, - { - "x": 3.0006366597167062, - "y": 6.311640001770152, - "heading": 0.21599961790492203, - "angularVelocity": -0.20083819373658737, - "velocityX": 1.1231322349922719, - "velocityY": -0.08820410428451562, - "timestamp": 4.737698478299127 - }, - { - "x": 3.1035985534895527, - "y": 6.303558029226905, - "heading": 0.1976138856084477, - "angularVelocity": -0.30083469281246417, - "velocityX": 1.684703616101463, - "velocityY": -0.13224046168846443, - "timestamp": 4.798814209748145 - }, - { - "x": 3.2408820865427965, - "y": 6.29278900601246, - "heading": 0.173149008298291, - "angularVelocity": -0.4003040907816843, - "velocityX": 2.2462879818065202, - "velocityY": -0.17620705764487352, - "timestamp": 4.859929941197163 - }, - { - "x": 3.412488399373154, - "y": 6.279338451783206, - "heading": 0.14264484283477577, - "angularVelocity": -0.4991213348884074, - "velocityX": 2.807891008774219, - "velocityY": -0.22008333877955472, - "timestamp": 4.921045672646181 - }, - { - "x": 3.618418876396286, - "y": 6.263213190116882, - "heading": 0.10614503413984216, - "angularVelocity": -0.597224443355985, - "velocityX": 3.3695166881037326, - "velocityY": -0.26384796981077935, - "timestamp": 4.982161404095199 - }, - { - "x": 3.839130420023726, - "y": 6.246248871896369, - "heading": 0.10614500290272542, - "angularVelocity": -5.111141764182067e-7, - "velocityX": 3.611370401605275, - "velocityY": -0.2775769481653736, - "timestamp": 5.0432771355442165 - }, - { - "x": 4.059841964366672, - "y": 6.229284562983422, - "heading": 0.10614497166605225, - "angularVelocity": -5.111069183870487e-7, - "velocityX": 3.6113704133126814, - "velocityY": -0.2775767958712516, - "timestamp": 5.104392866993234 - }, - { - "x": 4.280553508709572, - "y": 6.212320254069855, - "heading": 0.10614494042937911, - "angularVelocity": -5.111069180282712e-7, - "velocityX": 3.6113704133119002, - "velocityY": -0.2775767958814193, - "timestamp": 5.165508598442252 - }, - { - "x": 4.501265048980713, - "y": 6.1953558921813965, - "heading": 0.1061449091927048, - "angularVelocity": -5.111069369418327e-7, - "velocityX": 3.611370346688179, - "velocityY": -0.27757766267774153, - "timestamp": 5.22662432989127 - }, - { - "x": 4.810869637193169, - "y": 6.16135848513122, - "heading": 0.10614488355477533, - "angularVelocity": -2.98142570668873e-7, - "velocityX": 3.6003807524808256, - "velocityY": -0.39535463826432365, - "timestamp": 5.312616509468145 - }, - { - "x": 5.120474220145008, - "y": 6.12736103017416, - "heading": 0.1061448579168455, - "angularVelocity": -2.9814257488756455e-7, - "velocityX": 3.600380691305304, - "velocityY": -0.39535519537177727, - "timestamp": 5.3986086890450204 - }, - { - "x": 5.430078803096823, - "y": 6.093363575216878, - "heading": 0.1061448322789156, - "angularVelocity": -2.981425755600158e-7, - "velocityX": 3.6003806913050207, - "velocityY": -0.39535519537435887, - "timestamp": 5.484600868621896 - }, - { - "x": 5.739683386048638, - "y": 6.059366120259595, - "heading": 0.10614480664098574, - "angularVelocity": -2.981425750224502e-7, - "velocityX": 3.60038069130502, - "velocityY": -0.39535519537435865, - "timestamp": 5.570593048198771 - }, - { - "x": 6.049287969000453, - "y": 6.025368665302312, - "heading": 0.10614478100305595, - "angularVelocity": -2.981425744595309e-7, - "velocityX": 3.60038069130502, - "velocityY": -0.3953551953743586, - "timestamp": 5.656585227775646 - }, - { - "x": 6.358892551952268, - "y": 5.991371210345029, - "heading": 0.1061447553651262, - "angularVelocity": -2.98142573863633e-7, - "velocityX": 3.6003806913050203, - "velocityY": -0.3953551953743583, - "timestamp": 5.742577407352521 - }, - { - "x": 6.668497134904083, - "y": 5.9573737553877475, - "heading": 0.10614472972719659, - "angularVelocity": -2.981425722498942e-7, - "velocityX": 3.6003806913050203, - "velocityY": -0.3953551953743583, - "timestamp": 5.8285695869293965 - }, - { - "x": 6.978101717855898, - "y": 5.923376300430465, - "heading": 0.10614470408926703, - "angularVelocity": -2.981425716615336e-7, - "velocityX": 3.6003806913050207, - "velocityY": -0.39535519537435826, - "timestamp": 5.914561766506272 - }, - { - "x": 7.287706300807711, - "y": 5.889378845473171, - "heading": 0.10614467845133739, - "angularVelocity": -2.9814257255193557e-7, - "velocityX": 3.600380691305006, - "velocityY": -0.39535519537448677, - "timestamp": 6.000553946083147 - }, - { - "x": 7.597310883497341, - "y": 5.855381388128685, - "heading": 0.10614465281325862, - "angularVelocity": -2.981443068468836e-7, - "velocityX": 3.600380688256082, - "velocityY": -0.39535522313506416, - "timestamp": 6.086546125660022 - }, - { - "x": 7.868541995298217, - "y": 5.825436456932061, - "heading": 0.06366050775847867, - "angularVelocity": -0.4940466128876299, - "velocityX": 3.154136959145237, - "velocityY": -0.34822854059484143, - "timestamp": 6.172538305236897 - }, - { - "x": 8.071966668862895, - "y": 5.802984598737895, - "heading": 0.03182759417957115, - "angularVelocity": -0.3701838206164972, - "velocityX": 2.365618298845671, - "velocityY": -0.2610918609650363, - "timestamp": 6.258530484813773 - }, - { - "x": 8.207583377724607, - "y": 5.788018984348283, - "heading": 0.01061131016646268, - "angularVelocity": -0.24672341272780052, - "velocityX": 1.577081887318283, - "velocityY": -0.17403459783495048, - "timestamp": 6.344522664390648 - }, - { - "x": 8.275391578674316, - "y": 5.78053617477417, - "heading": 6.762629528326585e-25, - "angularVelocity": -0.12339854878287405, - "velocityX": 0.7885391588323608, - "velocityY": -0.08701732658634945, - "timestamp": 6.430514843967523 - }, - { - "x": 8.275391578674316, - "y": 5.78053617477417, - "heading": 6.890807484355009e-25, - "angularVelocity": 1.714109587590018e-25, - "velocityX": -1.5632941987910777e-25, - "velocityY": 4.01842027987716e-26, - "timestamp": 6.516507023544398 - }, - { - "x": 8.217699478068795, - "y": 5.791359796793642, - "heading": 0.01104341509549353, - "angularVelocity": 0.13833447495381113, - "velocityX": -0.7226755833441465, - "velocityY": 0.1355812541876774, - "timestamp": 6.596338281075688 - }, - { - "x": 8.10231615629365, - "y": 5.8130147597317485, - "heading": 0.03315104134696604, - "angularVelocity": 0.276929450131827, - "velocityX": -1.4453401505033163, - "velocityY": 0.2712591985616573, - "timestamp": 6.676169538606978 - }, - { - "x": 7.929243964127431, - "y": 5.845512314072997, - "heading": 0.06637865596420629, - "angularVelocity": 0.4162231141632274, - "velocityX": -2.167975270818996, - "velocityY": 0.40707807124934986, - "timestamp": 6.756000796138268 - }, - { - "x": 7.698485965371198, - "y": 5.888866692718938, - "heading": 0.11081243304487726, - "angularVelocity": 0.5565962312851562, - "velocityX": -2.8905720126704284, - "velocityY": 0.5430752312645023, - "timestamp": 6.8358320536695585 - }, - { - "x": 7.41422585651022, - "y": 5.941821981236076, - "heading": 0.11081246572396981, - "angularVelocity": 4.0935209556931575e-7, - "velocityX": -3.5607620079085063, - "velocityY": 0.6633402774142934, - "timestamp": 6.9156633112008485 - }, - { - "x": 7.129965619426801, - "y": 5.994776581882574, - "heading": 0.11081249814230922, - "angularVelocity": 4.060857915201533e-7, - "velocityX": -3.5607636140768695, - "velocityY": 0.6633316608565432, - "timestamp": 6.995494568732139 - }, - { - "x": 6.8457053823426035, - "y": 6.047731182524891, - "heading": 0.11081253056064869, - "angularVelocity": 4.060857924168534e-7, - "velocityX": -3.560763614086623, - "velocityY": 0.6633316608041847, - "timestamp": 7.075325826263429 - }, - { - "x": 6.561445145258405, - "y": 6.100685783167208, - "heading": 0.11081256297898824, - "angularVelocity": 4.060857934452354e-7, - "velocityX": -3.5607636140866235, - "velocityY": 0.6633316608041844, - "timestamp": 7.155157083794719 - }, - { - "x": 6.277184908174208, - "y": 6.153640383809526, - "heading": 0.11081259539732799, - "angularVelocity": 4.060857956958186e-7, - "velocityX": -3.560763614086623, - "velocityY": 0.6633316608041847, - "timestamp": 7.234988341326009 - }, - { - "x": 5.99292467109001, - "y": 6.206594984451843, - "heading": 0.11081262781566778, - "angularVelocity": 4.060857964387038e-7, - "velocityX": -3.560763614086623, - "velocityY": 0.6633316608041848, - "timestamp": 7.314819598857299 - }, - { - "x": 5.708664434005791, - "y": 6.259549585094043, - "heading": 0.11081266023400761, - "angularVelocity": 4.060857968841719e-7, - "velocityX": -3.5607636140868966, - "velocityY": 0.6633316608027189, - "timestamp": 7.394650856388589 - }, - { - "x": 5.424404193332451, - "y": 6.312504166469845, - "heading": 0.11081269265234713, - "angularVelocity": 4.060857929641026e-7, - "velocityX": -3.5607636590457346, - "velocityY": 0.6633314194636794, - "timestamp": 7.474482113919879 - }, - { - "x": 5.139572620391846, - "y": 6.3622941970825195, - "heading": 0.11081272507571231, - "angularVelocity": 4.061487466795785e-7, - "velocityX": -3.567920407980088, - "velocityY": 0.6236909219820286, - "timestamp": 7.554313371451169 - }, - { - "x": 4.8822045552292, - "y": 6.360012293211402, - "heading": 0.1108127534765096, - "angularVelocity": 3.996777164840354e-7, - "velocityX": -3.6218800325558864, - "velocityY": -0.03211269456367951, - "timestamp": 7.625372617712571 - }, - { - "x": 4.624881625896748, - "y": 6.3546795533312395, - "heading": 0.11081278178743512, - "angularVelocity": 3.98412972255652e-7, - "velocityX": -3.6212448466713205, - "velocityY": -0.07504638960770253, - "timestamp": 7.696431863973974 - }, - { - "x": 4.367558697083316, - "y": 6.349346788406661, - "heading": 0.11081281009836048, - "angularVelocity": 3.9841297029584103e-7, - "velocityX": -3.62124483936728, - "velocityY": -0.07504674205185301, - "timestamp": 7.767491110235376 - }, - { - "x": 4.110235768269888, - "y": 6.344014023481877, - "heading": 0.11081283840928584, - "angularVelocity": 3.9841296996573985e-7, - "velocityX": -3.6212448393672196, - "velocityY": -0.075046742054734, - "timestamp": 7.8385503564967784 - }, - { - "x": 3.852912839456459, - "y": 6.338681258557136, - "heading": 0.11081286672021123, - "angularVelocity": 3.984129707844367e-7, - "velocityX": -3.621244839367232, - "velocityY": -0.07504674205415067, - "timestamp": 7.909609602758181 - }, - { - "x": 3.5955899105380604, - "y": 6.333348498701343, - "heading": 0.110812895031426, - "angularVelocity": 3.98417042898143e-7, - "velocityX": -3.6212448408444415, - "velocityY": -0.07504667072002474, - "timestamp": 7.980668849019583 - }, - { - "x": 3.3629508998707167, - "y": 6.328797181710379, - "heading": 0.15195588309543634, - "angularVelocity": 0.5789955597426332, - "velocityX": -3.2738738856241922, - "velocityY": -0.06404960973301323, - "timestamp": 8.051728095280986 - }, - { - "x": 3.176840519639808, - "y": 6.325164863882251, - "heading": 0.18492044940341654, - "angularVelocity": 0.4639025607830811, - "velocityX": -2.6190874519872214, - "velocityY": -0.05111675143253586, - "timestamp": 8.122787341542388 - }, - { - "x": 3.0372579905429093, - "y": 6.322444909479725, - "heading": 0.2096671002374919, - "angularVelocity": 0.3482537760538718, - "velocityX": -1.9643119852893276, - "velocityY": -0.03827727629589979, - "timestamp": 8.19384658780379 - }, - { - "x": 2.944202841875484, - "y": 6.320632970128358, - "heading": 0.22616886731909797, - "angularVelocity": 0.2322254731059452, - "velocityX": -1.3095431427052717, - "velocityY": -0.025498994806420823, - "timestamp": 8.264905834065193 - }, - { - "x": 2.897675037384033, - "y": 6.319726943969727, - "heading": 0.23441548450132177, - "angularVelocity": 0.11605269709570704, - "velocityX": -0.6547748103081646, - "velocityY": -0.012750292274397987, - "timestamp": 8.335965080326595 - }, - { - "x": 2.897675037384033, - "y": 6.319726943969727, - "heading": 0.23441548450132177, - "angularVelocity": 6.477597733926451e-26, - "velocityX": -1.5355730051117952e-25, - "velocityY": -1.2183012914286086e-25, - "timestamp": 8.407024326587997 - } - ], - "eventMarkers": [] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterlineDisrupt.1.traj b/src/main/deploy/choreo/CenterlineDisrupt.1.traj new file mode 100644 index 0000000..67b49d1 --- /dev/null +++ b/src/main/deploy/choreo/CenterlineDisrupt.1.traj @@ -0,0 +1,815 @@ +{ + "samples": [ + { + "x": 0.6499595046043396, + "y": 4.397637367248535, + "heading": -1.05302514059915, + "angularVelocity": -1.3781959946272502e-22, + "velocityX": -5.582540900425554e-24, + "velocityY": 3.959360287559598e-24, + "timestamp": 0 + }, + { + "x": 0.7439386139597359, + "y": 4.319907691298174, + "heading": -1.0530251410513847, + "angularVelocity": -4.008663071191835e-9, + "velocityX": 0.8330420701810625, + "velocityY": -0.689005148190141, + "timestamp": 0.112814361626382 + }, + { + "x": 0.8743373653931026, + "y": 4.212055501990732, + "heading": -1.0547735019704858, + "angularVelocity": -0.015497680383030333, + "velocityX": 1.1558701352689535, + "velocityY": -0.9560147108275632, + "timestamp": 0.225628723252764 + }, + { + "x": 1.0047361168261997, + "y": 4.104203312682963, + "heading": -1.0570727239889381, + "angularVelocity": -0.020380579079698136, + "velocityX": 1.1558701352665623, + "velocityY": -0.9560147108304606, + "timestamp": 0.338443084879146 + }, + { + "x": 1.1351348682592939, + "y": 3.996351123375191, + "heading": -1.058055269010971, + "angularVelocity": -0.008709396639472093, + "velocityX": 1.155870135266536, + "velocityY": -0.9560147108304925, + "timestamp": 0.451257446505528 + }, + { + "x": 1.2655336196923908, + "y": 3.8884989340674223, + "heading": -1.055819907754512, + "angularVelocity": 0.019814509644278368, + "velocityX": 1.1558701352665606, + "velocityY": -0.9560147108304629, + "timestamp": 0.56407180813191 + }, + { + "x": 1.3959323711255218, + "y": 3.7806467447596948, + "heading": -1.0483128977990617, + "angularVelocity": 0.06654303447917355, + "velocityX": 1.1558701352668668, + "velocityY": -0.9560147108300927, + "timestamp": 0.6768861697582921 + }, + { + "x": 1.5263311225587595, + "y": 3.672794555452096, + "heading": -1.0334248940554078, + "angularVelocity": 0.13196904657369468, + "velocityX": 1.1558701352678078, + "velocityY": -0.9560147108289547, + "timestamp": 0.7897005313846741 + }, + { + "x": 1.6567298739922285, + "y": 3.564942366144777, + "heading": -1.0086121652128852, + "angularVelocity": 0.21994299737029419, + "velocityX": 1.1558701352698606, + "velocityY": -0.9560147108264728, + "timestamp": 0.9025148930110561 + }, + { + "x": 1.7871286254260874, + "y": 3.45709017683793, + "heading": -0.9712069684409036, + "angularVelocity": 0.3315641398198931, + "velocityX": 1.1558701352733156, + "velocityY": -0.9560147108222955, + "timestamp": 1.0153292546374382 + }, + { + "x": 1.917527376860465, + "y": 3.34923798753171, + "heading": -0.9178895550179713, + "angularVelocity": 0.47261193215371433, + "velocityX": 1.155870135277915, + "velocityY": -0.9560147108167347, + "timestamp": 1.12814361626382 + }, + { + "x": 2.04792612829522, + "y": 3.2413857982259455, + "heading": -0.8450478857031731, + "angularVelocity": 0.6456772725092832, + "velocityX": 1.1558701352812586, + "velocityY": -0.9560147108126921, + "timestamp": 1.240957977890202 + }, + { + "x": 2.1783248797294235, + "y": 3.1335336089195143, + "heading": -0.7482891781356035, + "angularVelocity": 0.8576807613202174, + "velocityX": 1.1558701352763678, + "velocityY": -0.9560147108186051, + "timestamp": 1.353772339516584 + }, + { + "x": 2.3087236351774485, + "y": 3.025681424465998, + "heading": -0.6227458179192954, + "angularVelocity": 1.112831366560242, + "velocityX": 1.1558701708553702, + "velocityY": -0.9560146678017903, + "timestamp": 1.4665867011429659 + }, + { + "x": 2.44897985458374, + "y": 2.931002378463745, + "heading": -0.4636471861382337, + "angularVelocity": 1.4102693086892948, + "velocityX": 1.2432479108536778, + "velocityY": -0.8392463923681126, + "timestamp": 1.5794010627693478 + }, + { + "x": 2.59560005766312, + "y": 2.8420710526217885, + "heading": -0.35525539273168516, + "angularVelocity": 1.4102693459256186, + "velocityX": 1.9076534431041812, + "velocityY": -1.1570721249811602, + "timestamp": 1.6562599931004234 + }, + { + "x": 2.793501016986051, + "y": 2.729420570845893, + "heading": -0.24993052581458297, + "angularVelocity": 1.370366025957004, + "velocityX": 2.5748596613361308, + "velocityY": -1.465678500736941, + "timestamp": 1.733118923431499 + }, + { + "x": 3.0191753411397784, + "y": 2.6099749336612343, + "heading": -0.20571979775764684, + "angularVelocity": 0.5752191432601933, + "velocityX": 2.9362147401950485, + "velocityY": -1.5540892472759904, + "timestamp": 1.8099778537625746 + }, + { + "x": 3.2599710072289283, + "y": 2.4899899991638152, + "heading": -0.20571974745826738, + "angularVelocity": 6.544376725031248e-7, + "velocityX": 3.132956249220541, + "velocityY": -1.5611059636215943, + "timestamp": 1.8868367840936502 + }, + { + "x": 3.5007667103292572, + "y": 2.370005138943224, + "heading": -0.20571969715956012, + "angularVelocity": 6.54428926357924e-7, + "velocityX": 3.132956730767442, + "velocityY": -1.561104997216945, + "timestamp": 1.9636957144247258 + }, + { + "x": 3.7415624134298047, + "y": 2.2500202787230723, + "heading": -0.20571964686085287, + "angularVelocity": 6.544289265734677e-7, + "velocityX": 3.1329567307702857, + "velocityY": -1.5611049972112399, + "timestamp": 2.0405546447558014 + }, + { + "x": 3.9823581165303517, + "y": 2.1300354185029193, + "heading": -0.20571959656214558, + "angularVelocity": 6.544289263933649e-7, + "velocityX": 3.1329567307702852, + "velocityY": -1.5611049972112403, + "timestamp": 2.1174135750868768 + }, + { + "x": 4.223153819630899, + "y": 2.010050558282767, + "heading": -0.20571954626343825, + "angularVelocity": 6.544289274928786e-7, + "velocityX": 3.132956730770285, + "velocityY": -1.561104997211241, + "timestamp": 2.194272505417952 + }, + { + "x": 4.463949522731447, + "y": 1.890065698062614, + "heading": -0.20571949596473096, + "angularVelocity": 6.544289268381233e-7, + "velocityX": 3.1329567307702844, + "velocityY": -1.561104997211242, + "timestamp": 2.2711314357490275 + }, + { + "x": 4.704745225831994, + "y": 1.7700808378424613, + "heading": -0.2057194456660236, + "angularVelocity": 6.544289275764684e-7, + "velocityX": 3.1329567307702844, + "velocityY": -1.561104997211243, + "timestamp": 2.347990366080103 + }, + { + "x": 4.945540928932542, + "y": 1.6500959776223083, + "heading": -0.20571939536731623, + "angularVelocity": 6.544289278568051e-7, + "velocityX": 3.1329567307702844, + "velocityY": -1.5611049972112438, + "timestamp": 2.4248492964111783 + }, + { + "x": 5.186336632033089, + "y": 1.5301111174021556, + "heading": -0.20571934506860895, + "angularVelocity": 6.544289269879176e-7, + "velocityX": 3.1329567307702844, + "velocityY": -1.5611049972112447, + "timestamp": 2.5017082267422537 + }, + { + "x": 5.427132335133637, + "y": 1.4101262571820024, + "heading": -0.20571929476990156, + "angularVelocity": 6.544289278426302e-7, + "velocityX": 3.132956730770284, + "velocityY": -1.5611049972112456, + "timestamp": 2.578567157073329 + }, + { + "x": 5.667928038234183, + "y": 1.2901413969618496, + "heading": -0.20571924447119422, + "angularVelocity": 6.544289274178593e-7, + "velocityX": 3.132956730770284, + "velocityY": -1.5611049972112463, + "timestamp": 2.6554260874044044 + }, + { + "x": 5.908723741334731, + "y": 1.1701565367416964, + "heading": -0.2057191941724868, + "angularVelocity": 6.544289285142327e-7, + "velocityX": 3.132956730770284, + "velocityY": -1.5611049972112474, + "timestamp": 2.73228501773548 + }, + { + "x": 6.1495194444352785, + "y": 1.0501716765215432, + "heading": -0.20571914387377943, + "angularVelocity": 6.54428927715182e-7, + "velocityX": 3.1329567307702835, + "velocityY": -1.561104997211248, + "timestamp": 2.809143948066555 + }, + { + "x": 6.390315147536474, + "y": 0.9301868163026915, + "heading": -0.205719093575072, + "angularVelocity": 6.544289283620361e-7, + "velocityX": 3.1329567307787203, + "velocityY": -1.5611049971943174, + "timestamp": 2.8860028783976306 + }, + { + "x": 6.6311109604445875, + "y": 0.81020217645385, + "heading": -0.2057190432763632, + "angularVelocity": 6.544289466547929e-7, + "velocityX": 3.1329581594600384, + "velocityY": -1.5611021299932077, + "timestamp": 2.962861808728706 + }, + { + "x": 6.8837742528519295, + "y": 0.7177889873928526, + "heading": -0.2057189912917875, + "angularVelocity": 6.763635073735201e-7, + "velocityX": 3.28736415298749, + "velocityY": -1.202374124424067, + "timestamp": 3.0397207390597814 + }, + { + "x": 7.150181265246966, + "y": 0.6802891513062552, + "heading": -0.20571892659810667, + "angularVelocity": 8.417197660629371e-7, + "velocityX": 3.4661816297399604, + "velocityY": -0.48790473566395065, + "timestamp": 3.1165796693908567 + }, + { + "x": 7.418537576371213, + "y": 0.6993630962988201, + "heading": -0.20571881834451866, + "angularVelocity": 0.0000014084711760366945, + "velocityX": 3.491543662763481, + "velocityY": 0.24816823380708655, + "timestamp": 3.193438599721932 + }, + { + "x": 7.6769622174729975, + "y": 0.7741663820293505, + "heading": -0.2057181437677739, + "angularVelocity": 0.000008776816719474182, + "velocityX": 3.362324195621788, + "velocityY": 0.9732543168153605, + "timestamp": 3.2702975300530075 + }, + { + "x": 7.913700103759766, + "y": 0.9012491106987, + "heading": -0.20468215348647234, + "angularVelocity": 0.013479113966834618, + "velocityX": 3.0801610856019264, + "velocityY": 1.6534542976584665, + "timestamp": 3.347156460384083 + }, + { + "x": 8.069994078152508, + "y": 1.0191752960928648, + "heading": -0.20375887243633364, + "angularVelocity": 0.016481161319135098, + "velocityX": 2.7899480930412865, + "velocityY": 2.105058351343339, + "timestamp": 3.403176848283977 + }, + { + "x": 8.206503178207031, + "y": 1.1597450199010308, + "heading": -0.2032892345162591, + "angularVelocity": 0.0083833393105698, + "velocityX": 2.4367753450486496, + "velocityY": 2.5092600940100236, + "timestamp": 3.459197236183871 + }, + { + "x": 8.319991028205546, + "y": 1.3196579099497696, + "heading": -0.20328837774767694, + "angularVelocity": 0.00001529387093322081, + "velocityX": 2.0258312063335517, + "velocityY": 2.854548068008667, + "timestamp": 3.5152176240837654 + }, + { + "x": 8.407691921317648, + "y": 1.49504392254657, + "heading": -0.20328818040115396, + "angularVelocity": 0.0000035227625227330623, + "velocityX": 1.5655174196369026, + "velocityY": 3.1307532698668137, + "timestamp": 3.5712380119836595 + }, + { + "x": 8.467543059563889, + "y": 1.6817778190681203, + "heading": -0.2032880311174836, + "angularVelocity": 0.0000026648096518103063, + "velocityX": 1.0683813606074055, + "velocityY": 3.3333203057293357, + "timestamp": 3.6272583998835537 + }, + { + "x": 8.498136804155573, + "y": 1.8754676048297587, + "heading": -0.20328791350897857, + "angularVelocity": 0.0000020993875521670102, + "velocityX": 0.5461180427088816, + "velocityY": 3.4574874081156675, + "timestamp": 3.683278787783448 + }, + { + "x": 8.498753931533233, + "y": 2.0715577148212954, + "heading": -0.20328781917254074, + "angularVelocity": 0.0000016839661657876831, + "velocityX": 0.011016121108670174, + "velocityY": 3.5003347413791763, + "timestamp": 3.739299175683342 + }, + { + "x": 8.483800369138562, + "y": 2.267077799474399, + "heading": -0.20328773289772806, + "angularVelocity": 0.0000015400609657437253, + "velocityX": -0.2669307185339211, + "velocityY": 3.4901594220034653, + "timestamp": 3.795319563583236 + }, + { + "x": 8.468846321105957, + "y": 2.4625978469848633, + "heading": -0.20328764662310683, + "angularVelocity": 0.0000015400575482772301, + "velocityX": -0.2669393874838815, + "velocityY": 3.4901587589834318, + "timestamp": 3.8513399514831304 + }, + { + "x": 8.443202001044153, + "y": 2.6658472038837897, + "heading": -0.20328756938243672, + "angularVelocity": 0.0000013197722342835025, + "velocityX": -0.4381715168852488, + "velocityY": 3.4728188855744704, + "timestamp": 3.909865709259671 + }, + { + "x": 8.417557533819485, + "y": 2.869096542214876, + "heading": -0.20328749214181333, + "angularVelocity": 0.0000013197714364112358, + "velocityX": -0.4381740313826044, + "velocityY": 3.4728185683151858, + "timestamp": 3.9683914670362115 + }, + { + "x": 8.391913066592853, + "y": 3.0723458805457144, + "heading": -0.20328741490118712, + "angularVelocity": 0.0000013197714837580768, + "velocityX": -0.4381740314161576, + "velocityY": 3.472818568310951, + "timestamp": 4.026917224812752 + }, + { + "x": 8.36626859936622, + "y": 3.275595218876553, + "heading": -0.20328733766055812, + "angularVelocity": 0.0000013197715321871082, + "velocityX": -0.43817403141615957, + "velocityY": 3.4728185683109496, + "timestamp": 4.085442982589292 + }, + { + "x": 8.340624132139588, + "y": 3.4788445572073914, + "heading": -0.2032872604199263, + "angularVelocity": 0.0000013197715800752577, + "velocityX": -0.4381740314161615, + "velocityY": 3.4728185683109474, + "timestamp": 4.143968740365833 + }, + { + "x": 8.314979664912956, + "y": 3.6820938955382294, + "heading": -0.2032871831792916, + "angularVelocity": 0.0000013197716295854668, + "velocityX": -0.43817403141616307, + "velocityY": 3.4728185683109456, + "timestamp": 4.202494498142373 + }, + { + "x": 8.289335197686324, + "y": 3.8853432338690674, + "heading": -0.20328710593865404, + "angularVelocity": 0.0000013197716777984883, + "velocityX": -0.4381740314161649, + "velocityY": 3.4728185683109443, + "timestamp": 4.261020255918914 + }, + { + "x": 8.26369073045969, + "y": 4.0885925721999055, + "heading": -0.20328702869801377, + "angularVelocity": 0.0000013197717251490696, + "velocityX": -0.43817403141616196, + "velocityY": 3.472818568310943, + "timestamp": 4.319546013695454 + }, + { + "x": 8.23804626325309, + "y": 4.291841910533271, + "heading": -0.20328695145737063, + "angularVelocity": 0.000001319771773148376, + "velocityX": -0.43817403107387076, + "velocityY": 3.4728185683541293, + "timestamp": 4.378071771471995 + }, + { + "x": 8.212403297424316, + "y": 4.495091438293457, + "heading": -0.2032868742161759, + "angularVelocity": 0.0000013197811983448172, + "velocityX": -0.43814837779087523, + "velocityY": 3.472821804994326, + "timestamp": 4.436597529248536 + }, + { + "x": 8.211122236288011, + "y": 4.6734367956380565, + "heading": -0.20328678936705338, + "angularVelocity": 0.0000016652754099072682, + "velocityX": -0.025142506437536676, + "velocityY": 3.500261750249818, + "timestamp": 4.487549535146915 + }, + { + "x": 8.209842685648681, + "y": 4.851782163826195, + "heading": -0.2032867045174544, + "angularVelocity": 0.000001665284761962191, + "velocityX": -0.025112860951584333, + "velocityY": 3.5002619630684717, + "timestamp": 4.538501541045294 + }, + { + "x": 8.208563135039839, + "y": 5.030127532014551, + "heading": -0.203286619667852, + "angularVelocity": 0.0000016652848281480006, + "velocityX": -0.02511286035320464, + "velocityY": 3.500261963072762, + "timestamp": 4.589453546943673 + }, + { + "x": 8.207283584430996, + "y": 5.208472900202908, + "heading": -0.20328653481824618, + "angularVelocity": 0.000001665284895011747, + "velocityX": -0.02511286035319842, + "velocityY": 3.5002619630727594, + "timestamp": 4.6404055528420525 + }, + { + "x": 8.20600403382877, + "y": 5.386818268391311, + "heading": -0.2032864499686371, + "angularVelocity": 0.0000016652849598802982, + "velocityX": -0.025112860223413327, + "velocityY": 3.500261963073688, + "timestamp": 4.691357558740432 + }, + { + "x": 8.204724810861068, + "y": 5.565163638930035, + "heading": -0.20328636511888853, + "angularVelocity": 0.000001665287697022985, + "velocityX": -0.02510642996572845, + "velocityY": 3.5002620092018013, + "timestamp": 4.742309564638811 + }, + { + "x": 8.215723661757586, + "y": 5.743174123417012, + "heading": -0.20328627431671312, + "angularVelocity": 0.00000178211188758009, + "velocityX": 0.2158668869377758, + "velocityY": 3.4936894308343382, + "timestamp": 4.79326157053719 + }, + { + "x": 8.251384735107422, + "y": 5.917922496795654, + "heading": -0.20328616810400457, + "angularVelocity": 0.0000020845638294801198, + "velocityX": 0.6998953764638769, + "velocityY": 3.4296662181890745, + "timestamp": 4.844213576435569 + }, + { + "x": 8.319257308251775, + "y": 6.1013423979758015, + "heading": -0.20328607751404962, + "angularVelocity": 0.0000016213569308236664, + "velocityX": 1.21476676906589, + "velocityY": 3.282804679662124, + "timestamp": 4.900086501899726 + }, + { + "x": 8.406005091870338, + "y": 6.276626068328336, + "heading": -0.20328594240584189, + "angularVelocity": 0.000002418133767316469, + "velocityX": 1.5525906849858964, + "velocityY": 3.137184403651478, + "timestamp": 4.955959427363883 + }, + { + "x": 8.465469026478623, + "y": 6.461147567415963, + "heading": -0.19586462259478266, + "angularVelocity": 0.13282497290785408, + "velocityX": 1.0642710062932257, + "velocityY": 3.3025208104773145, + "timestamp": 5.01183235282804 + }, + { + "x": 8.497914422229119, + "y": 6.6500341576515325, + "heading": -0.1767449328748443, + "angularVelocity": 0.34219954586419293, + "velocityX": 0.5806997840360463, + "velocityY": 3.3806461477794345, + "timestamp": 5.0677052782921965 + }, + { + "x": 8.501533639733335, + "y": 6.83931682794884, + "heading": -0.15028692647848005, + "angularVelocity": 0.4735389489017775, + "velocityX": 0.0647758726458128, + "velocityY": 3.387735092173306, + "timestamp": 5.123578203756353 + }, + { + "x": 8.476232221452912, + "y": 7.02332099314858, + "heading": -0.1187876803017823, + "angularVelocity": 0.563765829603908, + "velocityX": -0.4528386167403034, + "velocityY": 3.293261694660663, + "timestamp": 5.17945112922051 + }, + { + "x": 8.424761116580283, + "y": 7.193085836951261, + "heading": -0.08469316743475845, + "angularVelocity": 0.6102152801878227, + "velocityX": -0.9212172880700084, + "velocityY": 3.038409791368238, + "timestamp": 5.235324054684667 + }, + { + "x": 8.349328318639657, + "y": 7.344985965125413, + "heading": -0.04934981017907167, + "angularVelocity": 0.6325667926294624, + "velocityX": -1.350077829538702, + "velocityY": 2.7186714658712017, + "timestamp": 5.291196980148824 + }, + { + "x": 8.251384735107422, + "y": 7.477188587188721, + "heading": -0.01358857618434331, + "angularVelocity": 0.640045848640404, + "velocityX": -1.752970382677875, + "velocityY": 2.366130303094952, + "timestamp": 5.3470699056129805 + }, + { + "x": 8.031164941682688, + "y": 7.638765513905215, + "heading": 0.04509310199225384, + "angularVelocity": 0.6364876296474269, + "velocityX": -2.388602007879415, + "velocityY": 1.7525353447119325, + "timestamp": 5.439266007223161 + }, + { + "x": 7.758143694279688, + "y": 7.7384188409284755, + "heading": 0.10025127172354484, + "angularVelocity": 0.5982700869989958, + "velocityX": -2.961310105685135, + "velocityY": 1.0808843897175913, + "timestamp": 5.531462108833341 + }, + { + "x": 7.44957756042063, + "y": 7.765731167037329, + "heading": 0.14290265837394545, + "angularVelocity": 0.4626159447688721, + "velocityX": -3.3468457827395444, + "velocityY": 0.296241659157495, + "timestamp": 5.623658210443521 + }, + { + "x": 7.1346990672621375, + "y": 7.715555620338561, + "heading": 0.15752386010214686, + "angularVelocity": 0.1585880690489728, + "velocityX": -3.4153124444441856, + "velocityY": -0.5442263373663762, + "timestamp": 5.715854312053701 + }, + { + "x": 6.838625346521012, + "y": 7.587150764950785, + "heading": 0.15752392264967224, + "angularVelocity": 6.784183309784681e-7, + "velocityX": -3.211347503530804, + "velocityY": -1.392736277838426, + "timestamp": 5.8080504136638815 + }, + { + "x": 6.564867571222415, + "y": 7.4162585819512294, + "heading": 0.15752396711824912, + "angularVelocity": 4.823259995470857e-7, + "velocityX": -2.9692988154324316, + "velocityY": -1.8535727651708478, + "timestamp": 5.900246515274062 + }, + { + "x": 6.291109923852278, + "y": 7.2453661940190575, + "heading": 0.15752401158682702, + "angularVelocity": 4.823260107723234e-7, + "velocityX": -2.969297427863337, + "velocityY": -1.8535749879613355, + "timestamp": 5.992442616884242 + }, + { + "x": 6.0173522764825655, + "y": 7.07447380608621, + "heading": 0.15752405605540484, + "angularVelocity": 4.823260098033014e-7, + "velocityX": -2.969297427858765, + "velocityY": -1.85357498796866, + "timestamp": 6.084638718494422 + }, + { + "x": 5.743594629112851, + "y": 6.903581418153362, + "heading": 0.1575241005239826, + "angularVelocity": 4.823260094668572e-7, + "velocityX": -2.9692974278587645, + "velocityY": -1.8535749879686596, + "timestamp": 6.176834820104602 + }, + { + "x": 5.469836981743135, + "y": 6.732689030220513, + "heading": 0.15752414499256026, + "angularVelocity": 4.823260082473403e-7, + "velocityX": -2.969297427858765, + "velocityY": -1.853574987968659, + "timestamp": 6.269030921714783 + }, + { + "x": 5.196079334373785, + "y": 6.5617966422870815, + "heading": 0.15752418946113778, + "angularVelocity": 4.823260064419635e-7, + "velocityX": -2.969297427854807, + "velocityY": -1.8535749879749983, + "timestamp": 6.361227023324963 + }, + { + "x": 4.922321797721961, + "y": 6.390904077000082, + "heading": 0.1575242339450377, + "angularVelocity": 4.824922002870454e-7, + "velocityX": -2.969296226963193, + "velocityY": -1.8535769116308105, + "timestamp": 6.453423124935143 + }, + { + "x": 4.71573342050983, + "y": 6.261612353964941, + "heading": 0.17376063284232746, + "angularVelocity": 0.1761072172654304, + "velocityX": -2.2407495935741277, + "velocityY": -1.4023556395237395, + "timestamp": 6.545619226545323 + }, + { + "x": 4.578007277346407, + "y": 6.1754183483776055, + "heading": 0.18457807169202894, + "angularVelocity": 0.11733076193871327, + "velocityX": -1.493839118553516, + "velocityY": -0.9348985920443467, + "timestamp": 6.6378153281555035 + }, + { + "x": 4.509144306182861, + "y": 6.132321357727051, + "heading": 0.18998797382164115, + "angularVelocity": 0.05867820911220429, + "velocityX": -0.7469184700965933, + "velocityY": -0.4674491643125701, + "timestamp": 6.730011429765684 + }, + { + "x": 4.509144306182861, + "y": 6.132321357727051, + "heading": 0.18998797382164115, + "angularVelocity": -1.332314755606386e-22, + "velocityX": 6.083262433472185e-23, + "velocityY": -4.699572509600775e-23, + "timestamp": 6.822207531375864 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4 Note.auto b/src/main/deploy/pathplanner/autos/4 Note Champs.auto similarity index 100% rename from src/main/deploy/pathplanner/autos/4 Note.auto rename to src/main/deploy/pathplanner/autos/4 Note Champs.auto diff --git a/src/main/deploy/pathplanner/paths/CenterNoteToBottomNote.path b/src/main/deploy/pathplanner/paths/CenterNoteToBottomNote.path index 17a1ba6..4bf8141 100644 --- a/src/main/deploy/pathplanner/paths/CenterNoteToBottomNote.path +++ b/src/main/deploy/pathplanner/paths/CenterNoteToBottomNote.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.71, - "y": 5.55 + "x": 2.9026396514331485, + "y": 5.55472336015897 }, "prevControl": null, "nextControl": { - "x": 1.7646700551619987, - "y": 6.150625753187941 + "x": 1.9573097065951472, + "y": 6.155349113346912 }, "isLocked": false, - "linkedName": null + "linkedName": "spike-middle" }, { "anchor": { - "x": 2.82, - "y": 6.83 + "x": 2.7272273347965497, + "y": 6.911245275482004 }, "prevControl": { - "x": 1.8995074325107804, - "y": 6.471030737761858 + "x": 1.8067347673073302, + "y": 6.552276013243862 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Centerline 1 to Shot.path b/src/main/deploy/pathplanner/paths/Centerline 1 to Shot.path index 3d9a302..124370e 100644 --- a/src/main/deploy/pathplanner/paths/Centerline 1 to Shot.path +++ b/src/main/deploy/pathplanner/paths/Centerline 1 to Shot.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.71, - "y": 5.55 + "x": 2.8909454969907094, + "y": 5.5430292057165325 }, "prevControl": { - "x": 7.211301936709776, - "y": 7.058691480664211 + "x": 7.392247433700485, + "y": 7.051720686380744 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Centerline 4 to shot point.path b/src/main/deploy/pathplanner/paths/Centerline 4 to shot point.path index 3324bc4..95f5708 100644 --- a/src/main/deploy/pathplanner/paths/Centerline 4 to shot point.path +++ b/src/main/deploy/pathplanner/paths/Centerline 4 to shot point.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 5.220329239171783, - "y": 1.0906197179259107 + "x": 5.091835361817993, + "y": 0.5557362183607982 }, "isLocked": false, "linkedName": "Centerline4" @@ -20,8 +20,8 @@ "y": 3.3091489994467262 }, "prevControl": { - "x": 4.1148072171774785, - "y": 1.504780349665863 + "x": 4.263105913385225, + "y": 0.0 }, "nextControl": null, "isLocked": false, @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.25, - "rotationDegrees": -70.0, + "rotationDegrees": -33.545806856999825, "rotateFast": false } ], diff --git a/src/main/deploy/pathplanner/paths/SpeakerToTopNote.path b/src/main/deploy/pathplanner/paths/SpeakerToTopNote.path index 9e52cc9..fb03c20 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerToTopNote.path +++ b/src/main/deploy/pathplanner/paths/SpeakerToTopNote.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.4090249243143167, - "y": 4.529749423648154 + "x": 2.3179319293111518, + "y": 4.665967622533537 }, "isLocked": false, "linkedName": "BottomNote" @@ -45,7 +45,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -32.98000000000002, + "rotation": -30.697102489780317, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Top Spike to Centerline 1.path b/src/main/deploy/pathplanner/paths/Top Spike to Centerline 1.path index 554f213..1290e7d 100644 --- a/src/main/deploy/pathplanner/paths/Top Spike to Centerline 1.path +++ b/src/main/deploy/pathplanner/paths/Top Spike to Centerline 1.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 2.82, - "y": 6.83 + "x": 2.7272273347965497, + "y": 6.911245275482004 }, "prevControl": null, "nextControl": { - "x": 3.478494160289644, - "y": 7.1328920181305 + "x": 3.385721495086194, + "y": 7.214137293612504 }, "isLocked": false, - "linkedName": null + "linkedName": "WallSpike" }, { "anchor": { diff --git a/src/main/deploy/pathplanner/paths/TopNoteToCenterNote.path b/src/main/deploy/pathplanner/paths/TopNoteToCenterNote.path index 24142af..6983c3f 100644 --- a/src/main/deploy/pathplanner/paths/TopNoteToCenterNote.path +++ b/src/main/deploy/pathplanner/paths/TopNoteToCenterNote.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 2.71, - "y": 5.5548446759057155 + "x": 2.9026396514331485, + "y": 5.55472336015897 }, "prevControl": { - "x": 1.0143476058462082, - "y": 5.320961587056917 + "x": 1.2069872572793567, + "y": 5.320840271310172 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "spike-middle" } ], "rotationTargets": [ diff --git a/src/main/deploy/pathplanner/paths/centerline 5 to shot point.path b/src/main/deploy/pathplanner/paths/centerline 5 to shot point.path index 20f1435..a209db4 100644 --- a/src/main/deploy/pathplanner/paths/centerline 5 to shot point.path +++ b/src/main/deploy/pathplanner/paths/centerline 5 to shot point.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 5.175604333196231, - "y": 1.9905671990919738 + "x": 1.9329136995566167, + "y": 3.3051680355142192 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 3.3091489994467262 }, "prevControl": { - "x": 6.118110187327796, - "y": 1.1904956371542255 + "x": 1.8939146666891922, + "y": 3.3051680355142192 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -41.31353520608083, + "rotation": -52.02310241843031, "rotateFast": false }, "reversed": false, diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 0d47677..da0c918 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -1,69 +1,279 @@ package frc.robot; import com.choreo.lib.Choreo; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.GeometryUtil; +import com.choreo.lib.ChoreoControlFunction; +import com.choreo.lib.ChoreoTrajectory; +import com.choreo.lib.ChoreoTrajectoryState; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; 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.button.Trigger; +import frc.robot.arm.Arm; +import frc.robot.arm.Arm.ArmSetpoints; import frc.robot.drivetrain.Drivetrain; import frc.robot.feeder.Feeder; import frc.robot.intake.Intake; +import frc.robot.lights.Lights; import frc.robot.shooter.Shooter; +import frc.robot.vision.GamePieceDetection; +import org.littletonrobotics.junction.Logger; public class Autos { + private final double kAimAndShootTimeoutSeconds = 3.0; + private final Drivetrain m_drivetrain; private final Shooter m_shooter; private final Feeder m_feeder; private final Intake m_intake; + private final Arm m_arm; + private final Lights m_lights; + private final GamePieceDetection m_gamePieceDetection; + + private final PIDController m_translationController = new PIDController(5.0, 0.0, 0.0); + private final PIDController m_rotationController = new PIDController(5.0, 0.0, 0.0); + private final PIDController m_gamePieceCorrectionController = new PIDController(0.04, 0.0, 0.0); - public Autos(Drivetrain drivetrain, Shooter shooter, Feeder feeder, Intake intake) { + public Autos( + Drivetrain drivetrain, + Shooter shooter, + Feeder feeder, + Intake intake, + Arm arm, + Lights lights, + GamePieceDetection gamePieceDetection) { m_drivetrain = drivetrain; m_shooter = shooter; m_feeder = feeder; m_intake = intake; + m_arm = arm; + m_lights = lights; + m_gamePieceDetection = gamePieceDetection; + } + + public Command subShot() { + return shoot(false) + .deadlineWith(m_arm.aimWrist(Arm.kSubwooferWristAngleRad)) + .withName("Sub Shot"); } - public Command centerlineTwoPiece() { + public Command sixPiece() { return Commands.sequence( - resetPose("Centerline Auto"), - shoot(), - intake().deadlineWith(getPathFollowingCommand("Centerline Auto.1")), - getPathFollowingCommand("Centerline Auto.2"), - shoot(), - intake().deadlineWith(getPathFollowingCommand("Centerline Auto.3")), - getPathFollowingCommand("Centerline Auto.4"), - shoot()) - .withName("Centerline 3 Piece"); + resetPose("6Piece"), + aimAndShoot(), + intake().deadlineWith(getPathFollowingCommand("6Piece.1", false)), + aimAndShoot(), + intake().deadlineWith(getPathFollowingCommand("6Piece.2", false)), + aimAndShoot(), + intake().deadlineWith(getPathFollowingCommand("6Piece.3", false)), + aimAndShoot(), + intake().deadlineWith(getPathFollowingCommand("6Piece.4", false)), + getPathFollowingCommand("6Piece.5", false), + aimAndShoot(), + intake().deadlineWith(getPathFollowingCommand("6Piece.6", false)), + getPathFollowingCommand("6Piece.7", false), + aimAndShoot()) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming) + .withName("Six Piece"); + } + + public Command threePieceSourceSide() { + return Commands.sequence( + resetPose("3PieceSourceSide"), + subShot(), + intake() + .raceWith( + getPathFollowingCommand("3PieceSourceSide.1", true), + m_arm.goToSetpoint(ArmSetpoints.kStowed)), + getPathFollowingCommand("3PieceSourceSide.2", false), + stopIntake(), + shoot(false), + intake().raceWith(getPathFollowingCommand("3PieceSourceSide.3", true)), + getPathFollowingCommand("3PieceSourceSide.4", false), + stopIntake(), + shoot(false)) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming) + .withName("Three Piece Source Side"); + } + + public Command threePieceSourceSideLower() { + return Commands.sequence( + resetPose("3PieceSourceSideLower"), + subShot(), + intake() + .raceWith( + getPathFollowingCommand("3PieceSourceSideLower.1", true), + m_arm.goToSetpoint(ArmSetpoints.kStowed)), + getPathFollowingCommand("3PieceSourceSideLower.2", false), + stopIntake(), + shoot(false), + intake().raceWith(getPathFollowingCommand("3PieceSourceSideLower.3", true)), + getPathFollowingCommand("3PieceSourceSideLower.4", false), + stopIntake(), + shoot(false)) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming) + .withName("Three Piece Source Side Lower"); + } + + public Command threePieceAmpSide() { + return Commands.sequence( + resetPose("3PieceAmpSide"), + subShot(), + intake() + .raceWith( + getPathFollowingCommand("3PieceAmpSide.1", true), + m_arm.goToSetpoint(ArmSetpoints.kStowed)), + getPathFollowingCommand("3PieceAmpSide.2", false), + shoot(false), + intake().raceWith(getPathFollowingCommand("3PieceAmpSide.3", true)), + getPathFollowingCommand("3PieceAmpSide.4", false), + shoot(false)) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming) + .withName("Three Piece Amp Side"); + } + + private Command getPathFollowingCommand( + String trajectoryName, ChoreoControlFunction controlFunction) { + final ChoreoTrajectory trajectory = Choreo.getTrajectory(trajectoryName); + return Choreo.choreoSwerveCommand( + trajectory, + PoseEstimation.getInstance()::getPose, + controlFunction, + m_drivetrain::runVelocity, + Constants.onRedAllianceSupplier, + m_drivetrain) + .andThen(Commands.waitSeconds(0.5)) + .beforeStarting(() -> Logger.recordOutput("Drivetrain/Trajectory", trajectory.getPoses())); } - private Command getPathFollowingCommand(String trajectoryName) { - return AutoBuilder.followPath(PathPlannerPath.fromChoreoTrajectory(trajectoryName)); + private Command getPathFollowingCommand(String trajectoryName, boolean useGamePieceCorrection) { + return getPathFollowingCommand( + trajectoryName, + choreoSwerveController( + m_translationController, + m_translationController, + m_rotationController, + useGamePieceCorrection)); + } + + private ChoreoControlFunction choreoSwerveController( + PIDController xController, + PIDController yController, + PIDController rotationController, + boolean useGamePieceCorrection) { + rotationController.enableContinuousInput(-Math.PI, Math.PI); + return (Pose2d pose, ChoreoTrajectoryState referenceState) -> { + Logger.recordOutput( + "Drivetrain/TrajectorySetpoint", + new Pose2d(referenceState.x, referenceState.y, new Rotation2d(referenceState.heading))); + final double xFF = referenceState.velocityX; + final double yFF = referenceState.velocityY; + final double rotationFF = referenceState.angularVelocity; + + final double xFeedback = xController.calculate(pose.getX(), referenceState.x); + final double yFeedback, yCorrection; + if (m_gamePieceDetection.hasValidTarget.getAsBoolean() && useGamePieceCorrection) { + yCorrection = + m_gamePieceCorrectionController.calculate( + m_gamePieceDetection.horizontalErrorDeg.getAsDouble(), 0.0); + yFeedback = 0.0; + } else { + yCorrection = 0.0; + yFeedback = yController.calculate(pose.getY(), referenceState.y); + } + final double rotationFeedback = + rotationController.calculate(pose.getRotation().getRadians(), referenceState.heading); + return generateChassisSpeeds( + xFF + xFeedback, + yFF + yFeedback, + rotationFF + rotationFeedback, + pose.getRotation(), + yCorrection); + }; + } + + private ChassisSpeeds generateChassisSpeeds( + double vxMetersPerSecond, + double vyMetersPerSecond, + double omegaRadiansPerSecond, + Rotation2d robotAngle, + double vyRobotRelativeCorrectionMetersPerSecond) { + final Translation2d fieldRelativeAdjustedSpeeds = + new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus()); + return new ChassisSpeeds( + fieldRelativeAdjustedSpeeds.getX(), + fieldRelativeAdjustedSpeeds.getY() + vyRobotRelativeCorrectionMetersPerSecond, + omegaRadiansPerSecond); } private Command resetPose(String trajectoryName) { - Pose2d startingPose = Choreo.getTrajectory(trajectoryName).getInitialPose(); + final ChoreoTrajectory trajectory = Choreo.getTrajectory(trajectoryName); return Commands.runOnce( () -> m_drivetrain.resetPose.accept( - m_drivetrain.onRedAllianceSupplier.getAsBoolean() - ? GeometryUtil.flipFieldPose(startingPose) - : startingPose)); + Constants.onRedAllianceSupplier.getAsBoolean() + ? trajectory.getFlippedInitialPose() + : trajectory.getInitialPose())); } private Command intake() { - return Superstructure.sensorIntake(m_feeder, m_intake) + return Superstructure.sensorIntake(m_feeder, m_intake).andThen(stopIntake()); + } + + private Command stopIntake() { + return Commands.sequence(m_intake.stop(), m_feeder.stop()); + } + + private Command aim() { + return Superstructure.aimAtGoal(m_drivetrain, m_shooter, m_arm, m_lights) + .alongWith(m_drivetrain.blankDrive()) + .withName("Aim"); + } + + private Command aimAndShoot() { + final var state = + new Object() { + private final Timer timeoutTimer = new Timer(); + private final Trigger hasTimedOut = + new Trigger(() -> timeoutTimer.hasElapsed(kAimAndShootTimeoutSeconds)); + private boolean hasShot = false; + }; + return Commands.race( + aim().until(() -> state.hasShot), + Commands.sequence(feedShooter(), Commands.runOnce(() -> state.hasShot = true)) + .onlyIf( + m_drivetrain.atHeadingGoal.and(m_shooter.readyToShoot).or(state.hasTimedOut)) + .repeatedly()) + .beforeStarting(() -> state.timeoutTimer.start()) + .andThen(Commands.print("WARNING: Aim and shoot timed out").onlyIf(state.hasTimedOut)) + .andThen(m_arm.stop()) + .finallyDo( + () -> { + state.timeoutTimer.stop(); + state.timeoutTimer.reset(); + state.hasShot = false; + }) + .withName("Aim and Shoot"); + } + + private Command feedShooter() { + return m_feeder + .shoot() + .alongWith(m_intake.intake()) + .withTimeout(0.5) .andThen(m_intake.stop(), m_feeder.stop()); } - private Command shoot() { + private Command shoot(boolean disableShooterAfter) { return Commands.race( m_shooter.runShooter(), - Commands.waitUntil(m_shooter.readyToShoot) - .andThen(m_feeder.shoot().alongWith(m_intake.intake()).withTimeout(0.5))) - .andThen(m_shooter.stop(), m_feeder.stop(), m_intake.stop()) - .finallyDo(() -> System.out.println("Shooting complete!")); + Commands.waitUntil(m_shooter.readyToShoot).andThen(feedShooter())) + .andThen(m_shooter.stop().unless(() -> !disableShooterAfter)); } } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c78cd71..19f69d2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -2,6 +2,8 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Filesystem; @@ -11,11 +13,15 @@ public final class Constants { public static final Mode kCurrentMode = Mode.kReal; - public static final RobotName kRobot = RobotName.kViper; + + // Whether or not the current robot is Viper or Black Mamba + public static final boolean kIsViper = false; public static final boolean kIsSim = Constants.kCurrentMode.equals(Mode.kSim); public static final String kDrivetrainCanBus = "CANivore1"; public static final String kSuperstructureCanBus = "CANivore2"; public static final boolean kInTuningMode = true; + public static final Translation3d poseOffset = + new Translation3d(Units.inchesToMeters(2.5), 0.0, 0.0); public static final BooleanSupplier onRedAllianceSupplier = () -> DriverStation.getAlliance().isPresent() @@ -43,9 +49,4 @@ public static enum Mode { kSim, kReplay } - - public static enum RobotName { - kViper, - kCobra - } } diff --git a/src/main/java/frc/robot/FieldPositions.java b/src/main/java/frc/robot/FieldConstants.java similarity index 67% rename from src/main/java/frc/robot/FieldPositions.java rename to src/main/java/frc/robot/FieldConstants.java index dd25207..a0eda07 100644 --- a/src/main/java/frc/robot/FieldPositions.java +++ b/src/main/java/frc/robot/FieldConstants.java @@ -1,14 +1,29 @@ package frc.robot; import com.pathplanner.lib.util.GeometryUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; +import java.util.function.Supplier; + +public class FieldConstants { + public static final double kFieldLength = Constants.fieldLayout.getFieldLength(); + public static final double kFieldWidth = Constants.fieldLayout.getFieldWidth(); + public static final Pose2d trapPose = new Pose2d(4.64, 4.66, new Rotation2d(-1.05)); + private static final Translation2d stashPosition = + FieldConstants.Speaker.centerSpeakerOpening + .toTranslation2d() + .interpolate(new Translation2d(Units.inchesToMeters(72.455), kFieldWidth), 0.5); + public static final Supplier stashPositionSupplier = + () -> + Constants.onRedAllianceSupplier.getAsBoolean() + ? GeometryUtil.flipFieldPosition(stashPosition) + : stashPosition; -public class FieldPositions { public static class NotePositions { - public static final double fieldLength = Constants.fieldLayout.getFieldLength(); - private static final double kCenterlineX = fieldLength / 2.0; + private static final double kCenterlineX = kFieldLength / 2.0; private static final double kCenterlineFirstY = Units.inchesToMeters(29.638); private static final double kCenterlineSeparationY = Units.inchesToMeters(66); private static final double kSpikeX = Units.inchesToMeters(114); diff --git a/src/main/java/frc/robot/NoteVisualizer.java b/src/main/java/frc/robot/NoteVisualizer.java index 86a8ca4..458d312 100644 --- a/src/main/java/frc/robot/NoteVisualizer.java +++ b/src/main/java/frc/robot/NoteVisualizer.java @@ -27,7 +27,7 @@ public class NoteVisualizer { public static void showStagedNotes() { if (m_notes.isEmpty()) { - Logger.recordOutput("NoteVisualizer/StagedNotes"); + Logger.recordOutput("NoteVisualizer/StagedNotes", new Pose3d[] {}); } // Show auto notes Stream presentNotes = m_notes.stream().filter(Objects::nonNull); @@ -46,9 +46,9 @@ public static void showStagedNotes() { public static void showHeldNotes() { if (m_hasNote) { - Logger.recordOutput("NoteVisualizer/HeldNotes", new Pose3d[] {getNotePose()}); + Logger.recordOutput("NoteVisualizer/HeldNote", getNotePose()); } else { - Logger.recordOutput("NoteVisualizer/HeldNotes", new Pose3d()); + Logger.recordOutput("NoteVisualizer/HeldNote", new Pose3d[] {}); } } @@ -63,7 +63,7 @@ public static void removeNote(int note) { public static void resetNotes() { m_notes.clear(); - for (Translation2d note : FieldPositions.NotePositions.noteTranslations) m_notes.add(note); + for (Translation2d note : FieldConstants.NotePositions.noteTranslations) m_notes.add(note); } public static Command shoot() { @@ -74,7 +74,7 @@ public static Command shoot() { m_hasNote = false; final Pose3d startPose = getNotePose(); final Translation3d speakerTranslation = - FieldPositions.Speaker.centerSpeakerOpening; + FieldConstants.Speaker.centerSpeakerOpening; final Pose3d endPose; if (Constants.onRedAllianceSupplier.getAsBoolean()) { double flippedX = @@ -100,9 +100,11 @@ public static Command shoot() { startPose.interpolate(endPose, timer.get() / duration) })) .until(() -> timer.hasElapsed(duration)) - .finallyDo(() -> Logger.recordOutput("NoteVisualizer/ShotNotes")); + .finallyDo( + () -> Logger.recordOutput("NoteVisualizer/ShotNotes", new Pose3d[] {})); }, Set.of()) + .withName("Note Shot Visualization") .ignoringDisable(true)); } @@ -111,14 +113,11 @@ public static void setWristPoseSupplier(Supplier wristPoseSupplier) { } private static Pose3d getNotePose() { - if (m_wristPoseSupplier == null) { - return new Pose3d(); - } Pose3d wristPose = m_wristPoseSupplier.get(); Transform3d noteTransform = new Transform3d( wristPose.getX(), wristPose.getY(), wristPose.getZ(), wristPose.getRotation()) - .plus(new Transform3d(0.0, 0.0, 0.0, new Rotation3d())); + .plus(new Transform3d(0.1, 0.0, 0.0, new Rotation3d())); return new Pose3d(PoseEstimation.getInstance().getPose()).transformBy(noteTransform); } } diff --git a/src/main/java/frc/robot/PoseEstimation.java b/src/main/java/frc/robot/PoseEstimation.java index 0a32b24..63bd329 100644 --- a/src/main/java/frc/robot/PoseEstimation.java +++ b/src/main/java/frc/robot/PoseEstimation.java @@ -9,17 +9,21 @@ import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.arm.Arm.Joint; import frc.robot.drivetrain.Drivetrain; import frc.robot.vision.Vision.VisionUpdate; +import java.util.function.BooleanSupplier; import org.littletonrobotics.junction.AutoLogOutput; +// why is this in our code? Shou;dn't this be in a library we use? public class PoseEstimation { private static PoseEstimation m_instance; private final SwerveDrivePoseEstimator m_poseEstimator; - private final boolean kLookaheadDisable = true; - private final double kLookaheadSeconds = 0.0; + private final BooleanSupplier lookaheadDisable = () -> false; + private final double kLookaheadSeconds = 0.35; + private final double jointSwitchDistanceMeters = Double.POSITIVE_INFINITY; private Twist2d m_robotVelocity = new Twist2d(); private AimingParameters m_lastAimingParameters = null; @@ -27,19 +31,23 @@ public class PoseEstimation { private static final InterpolatingDoubleTreeMap elbowAngleMap = new InterpolatingDoubleTreeMap(); static { - wristAngleMap.put(1.0, 0.0); - wristAngleMap.put(1.0, 0.0); + wristAngleMap.put(1.2, 119.3); + wristAngleMap.put(2.13, 139.0); + wristAngleMap.put(2.22, 137.1); + wristAngleMap.put(2.88, 142.0); } static { - elbowAngleMap.put(1.0, 0.0); + elbowAngleMap.put(2.4, -31.4); + elbowAngleMap.put(4.07, -15.4); } public record AimingParameters( Rotation2d driveHeading, Rotation2d armAngle, double effectiveDistance, - double driveFeedVelocity) {} + double driveFeedVelocity, + Joint aimingJointIndex) {} public PoseEstimation() { m_poseEstimator = @@ -76,7 +84,7 @@ public void addVisionMeasurement(VisionUpdate visionUpdate) { public Pose2d getPose() { return m_poseEstimator .getEstimatedPosition() - .plus(new Transform2d(new Translation2d(Units.inchesToMeters(2.5), 0.0), new Rotation2d())); + .transformBy(new Transform2d(Constants.poseOffset.toTranslation2d(), new Rotation2d())); } @AutoLogOutput(key = "PoseEstimation/FieldVelocity") @@ -87,6 +95,7 @@ public Twist2d getFieldVelocity() { linearFieldVelocity.getX(), linearFieldVelocity.getY(), m_robotVelocity.dtheta); } + @AutoLogOutput(key = "PoseEstimation/PredictedPose") public Pose2d getPredictedPose(double translationLookaheadS, double rotationLookaheadS) { return getPose() .exp( @@ -99,14 +108,16 @@ public Pose2d getPredictedPose(double translationLookaheadS, double rotationLook public AimingParameters getAimingParameters() { if (m_lastAimingParameters != null) return m_lastAimingParameters; - Translation2d speakerPosition = FieldPositions.Speaker.centerSpeakerOpening.toTranslation2d(); + Translation2d speakerPosition = FieldConstants.Speaker.centerSpeakerOpening.toTranslation2d(); speakerPosition = Constants.onRedAllianceSupplier.getAsBoolean() ? GeometryUtil.flipFieldPosition(speakerPosition) : speakerPosition; final Transform2d fieldToTarget = new Transform2d(speakerPosition, new Rotation2d()); final Pose2d fieldToPredictedVehicle = - kLookaheadDisable ? getPose() : getPredictedPose(kLookaheadSeconds, kLookaheadSeconds); + lookaheadDisable.getAsBoolean() || DriverStation.isAutonomousEnabled() + ? getPose() + : getPredictedPose(kLookaheadSeconds, kLookaheadSeconds); final Pose2d fieldToPredictedVehicleFixed = new Pose2d(fieldToPredictedVehicle.getTranslation(), new Rotation2d()); @@ -130,15 +141,30 @@ public AimingParameters getAimingParameters() { m_robotVelocity.dx * vehicleToGoalDirection.getSin() / targetDistance - m_robotVelocity.dy * vehicleToGoalDirection.getCos() / targetDistance; + Joint jointToAim = targetDistance < jointSwitchDistanceMeters ? Joint.kWrist : Joint.kElbow; + double armAngle = + jointToAim == Joint.kWrist + ? wristAngleMap.get(targetDistance) + : elbowAngleMap.get(targetDistance); + m_lastAimingParameters = new AimingParameters( targetVehicleDirection, - Rotation2d.fromDegrees(elbowAngleMap.get(targetDistance)), + Rotation2d.fromDegrees(armAngle), targetDistance, - feedVelocity); + feedVelocity, + jointToAim); return m_lastAimingParameters; } + public Rotation2d getFeedHeading() { + return FieldConstants.stashPositionSupplier + .get() + .minus(getPose().getTranslation()) + .getAngle() + .rotateBy(new Rotation2d(Math.PI)); + } + private Pose2d poseInverse(Pose2d pose) { Rotation2d rotationInverse = pose.getRotation().unaryMinus(); return new Pose2d( @@ -149,7 +175,12 @@ public void resetPose( Rotation2d currentYawPosition, SwerveModulePosition[] currentModulePositions, Pose2d newPose) { - m_poseEstimator.resetPosition(currentYawPosition, currentModulePositions, newPose); + m_poseEstimator.resetPosition( + currentYawPosition, + currentModulePositions, + newPose.transformBy( + new Transform2d( + Constants.poseOffset.toTranslation2d().unaryMinus(), new Rotation2d()))); } public static PoseEstimation getInstance() { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 430cf7b..1eb626a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,8 +1,11 @@ package frc.robot; +import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -10,14 +13,22 @@ import edu.wpi.first.wpilibj2.command.ScheduleCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.PoseEstimation.AimingParameters; import frc.robot.arm.Arm; +import frc.robot.arm.Arm.ArmSetpoints; import frc.robot.climber.Climber; +import frc.robot.drivetrain.DriveToPose; import frc.robot.drivetrain.Drivetrain; +import frc.robot.drivetrain.WheelRadiusCharacterization; +import frc.robot.drivetrain.WheelRadiusCharacterization.Direction; import frc.robot.feeder.Feeder; import frc.robot.intake.Intake; import frc.robot.lights.Lights; import frc.robot.shooter.Shooter; +import frc.robot.vision.GamePieceDetection; import frc.robot.vision.Vision; +import java.util.HashMap; +import java.util.function.BiConsumer; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -37,6 +48,7 @@ public class Robot extends LoggedRobot { private final Shooter m_shooter; private final Feeder m_feeder; private final Lights m_lights; + private final GamePieceDetection m_gamePieceDetection; @SuppressWarnings("unused") private final PoseEstimation m_poseEstimation = PoseEstimation.getInstance(); @@ -47,10 +59,13 @@ public class Robot extends LoggedRobot { public Robot() { recordMetadeta(); DriverStation.silenceJoystickConnectionWarning(true); + SignalLogger.enableAutoLogging(true); switch (Constants.kCurrentMode) { case kReal: + if (RobotBase.isSimulation()) throw new RuntimeException("Wrong robot mode."); Logger.addDataReceiver(new WPILOGWriter("/home/lvuser/logs")); Logger.addDataReceiver(new NT4Publisher()); + PortForwarder.add(5800, "10.49.9.17", 5800); break; case kSim: Logger.addDataReceiver(new WPILOGWriter()); @@ -70,46 +85,57 @@ public Robot() { case kReal: m_drivetrain = Subsystems.createTalonFXDrivetrain(); m_vision = Subsystems.createFourCameraVision(); - m_intake = Subsystems.createSparkMAXIntake(); + m_intake = + Constants.kIsViper + ? Subsystems.createSparkMAXIntake() + : Subsystems.createTalonFXIntake(); m_arm = Subsystems.createTalonFXArm(); - m_climber = Subsystems.createSparkMAXClimber(); + m_climber = + Constants.kIsViper + ? Subsystems.createSparkMAXClimber() + : Subsystems.createTalonFXClimber(); m_shooter = Subsystems.createTalonFXShooter(); m_feeder = Subsystems.createTalonFXFeeder(); + m_gamePieceDetection = Subsystems.createLimelightGamePieceDetection(); break; case kSim: m_drivetrain = Subsystems.createSimDrivetrain(); m_vision = Subsystems.createFourCameraVision(); m_intake = Subsystems.createSimIntake(); - m_arm = Subsystems.createTalonFXArm(); + m_arm = Subsystems.createSimArm(); m_climber = Subsystems.createBlankClimber(); m_shooter = Subsystems.createSimShooter(); m_feeder = Subsystems.createSimFeeder(); + m_gamePieceDetection = Subsystems.createBlankGamePieceDetection(); break; default: m_drivetrain = Subsystems.createBlankDrivetrain(); - m_vision = Subsystems.createBlankFourCameraVision(); + m_vision = Subsystems.createBlankVision(); m_intake = Subsystems.createBlankIntake(); m_arm = Subsystems.createBlankArm(); m_climber = Subsystems.createBlankClimber(); m_shooter = Subsystems.createBlankShooter(); m_feeder = Subsystems.createBlankFeeder(); + m_gamePieceDetection = Subsystems.createBlankGamePieceDetection(); break; } m_lights = new Lights(); - // NoteVisualizer.setWristPoseSupplier(m_arm.wristPoseSupplier); + NoteVisualizer.setWristPoseSupplier(m_arm.wristPoseSupplier); NoteVisualizer.resetNotes(); NoteVisualizer.showStagedNotes(); - final Autos autos = new Autos(m_drivetrain, m_shooter, m_feeder, m_intake); + final Autos autos = + new Autos( + m_drivetrain, m_shooter, m_feeder, m_intake, m_arm, m_lights, m_gamePieceDetection); NamedCommands.registerCommand("intake", m_intake.intake()); NamedCommands.registerCommand("intakeOff", m_intake.idle()); NamedCommands.registerCommand("enableShooter", new ScheduleCommand(m_shooter.runShooter())); NamedCommands.registerCommand("runShooter", m_shooter.runShooter().withTimeout(0.1)); - NamedCommands.registerCommand("subShot", m_arm.goToSetpoint(-0.52, 2.083, 0.0, 0.0)); + NamedCommands.registerCommand("subShot", m_arm.aimWrist(2.083)); NamedCommands.registerCommand("feederOn", m_feeder.feed().withTimeout(.3)); NamedCommands.registerCommand("feederOnTest", m_feeder.feed()); NamedCommands.registerCommand("feederOff", m_feeder.idle().withTimeout(1)); NamedCommands.registerCommand("sensorIntake", Superstructure.sensorIntake(m_feeder, m_intake)); - NamedCommands.registerCommand("armDown", m_arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); + NamedCommands.registerCommand("armDown", m_arm.goToSetpoint(ArmSetpoints.kStowed)); m_autoChooser = new LoggedDashboardChooser<>("Auto Chooser", AutoBuilder.buildAutoChooser()); m_autoChooser.addOption( @@ -138,16 +164,40 @@ public Robot() { m_drivetrain.sysIdRotationDynamic(SysIdRoutine.Direction.kReverse)); m_autoChooser.addOption("Slip Current SysId", m_drivetrain.sysIdSlipCurrent()); m_autoChooser.addOption( - "Shooter SysId (Quasistatic Forward)", - m_shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + "Radius Characterization Clockwise", + new WheelRadiusCharacterization(m_drivetrain, Direction.CLOCKWISE)); + m_autoChooser.addOption( + "Radius Characterization Counterclockwise", + new WheelRadiusCharacterization(m_drivetrain, Direction.COUNTER_CLOCKWISE)); + m_autoChooser.addOption("Shooter SysId", m_shooter.sysId()); + m_autoChooser.addOption( + "Elbow SysId (Quasistatic Forward)", + m_arm.sysIdElbowQuasistatic(SysIdRoutine.Direction.kForward)); + m_autoChooser.addOption( + "Elbow SysId (Quasistatic Reverse)", + m_arm.sysIdElbowQuasistatic(SysIdRoutine.Direction.kReverse)); + m_autoChooser.addOption( + "Elbow SysId (Dynamic Forward)", m_arm.sysIdElbowDynamic(SysIdRoutine.Direction.kForward)); + m_autoChooser.addOption( + "Elbow SysId (Dynamic Reverse)", m_arm.sysIdElbowDynamic(SysIdRoutine.Direction.kReverse)); m_autoChooser.addOption( - "Shooter SysId (Quasistatic Reverse)", - m_shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + "Wrist SysId (Quasistatic Forward)", + m_arm.sysIdWristQuasistatic(SysIdRoutine.Direction.kForward)); m_autoChooser.addOption( - "Shooter SysId (Dynamic Forward)", m_shooter.sysIdDynamic(SysIdRoutine.Direction.kForward)); + "Wrist SysId (Quasistatic Reverse)", + m_arm.sysIdWristQuasistatic(SysIdRoutine.Direction.kReverse)); m_autoChooser.addOption( - "Shooter SysId (Dynamic Reverse)", m_shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - m_autoChooser.addOption("3 Piece Centerline", autos.centerlineTwoPiece()); + "Wrist SysId (Dynamic Forward)", m_arm.sysIdWristDynamic(SysIdRoutine.Direction.kForward)); + m_autoChooser.addOption( + "Wrist SysId (Dynamic Reverse)", m_arm.sysIdWristDynamic(SysIdRoutine.Direction.kReverse)); + + m_autoChooser.addOption("Start Signal Logger", Commands.runOnce(SignalLogger::start)); + m_autoChooser.addOption("End Signal Logger", Commands.runOnce(SignalLogger::stop)); + m_autoChooser.addOption("Sub Shot", autos.subShot()); + m_autoChooser.addOption("6 Piece", autos.sixPiece()); + m_autoChooser.addOption("3 Note Source Side Upper", autos.threePieceSourceSide()); + m_autoChooser.addOption("3 Note Source Side Lower", autos.threePieceSourceSideLower()); + m_autoChooser.addOption("3 Note Amp Side", autos.threePieceAmpSide()); m_drivetrain.setDefaultCommand( m_drivetrain.joystickDrive( () -> -m_driverController.getLeftY(), @@ -158,8 +208,16 @@ public Robot() { .hasIntookPieceSim .or(m_feeder.hasNote) .onTrue(Commands.runOnce(() -> NoteVisualizer.setHasNote(true))); - m_feeder.hasNote.whileTrue(m_lights.setBlink(Color.kOrangeRed)); - m_drivetrain.inRangeOfGoal.whileTrue(m_lights.setBlink(Color.kBlue)); + m_feeder.hasNote.onTrue(m_lights.noteBlink()); + m_feeder + .hasNote + .and(DriverStation::isTeleopEnabled) + .onTrue(Superstructure.shortRumble(m_driverController)); + m_gamePieceDetection + .hasValidTarget + .and(() -> DriverStation.isAutonomous()) + .onTrue(m_lights.startBlink(Color.kBlue)) + .onFalse(m_lights.idle()); m_driverController .rightTrigger() @@ -167,61 +225,99 @@ public Robot() { .onFalse(Commands.runOnce(() -> m_shooter.getCurrentCommand().cancel())); m_driverController .leftTrigger() - .whileTrue(Superstructure.aimAtGoal(m_drivetrain, m_shooter, m_lights)); + .and(m_drivetrain.inRangeOfGoal) + .whileTrue(Superstructure.aimAtGoal(m_drivetrain, m_shooter, m_arm, m_lights)) + .onFalse(m_arm.goToSetpoint(ArmSetpoints.kStowed)); + m_driverController + .leftTrigger() + .and(m_drivetrain.inRangeOfGoal.negate()) + .whileTrue(Superstructure.feedShotHigh(m_drivetrain, m_shooter, m_arm, m_lights)) + .onFalse(m_arm.goToSetpoint(ArmSetpoints.kStowed)); + m_driverController + .leftTrigger() + .and(m_driverController.a()) + .and(m_drivetrain.inRangeOfGoal.negate()) + .whileTrue(Superstructure.feedShotLow(m_drivetrain, m_shooter, m_arm, m_lights)) + .onFalse(m_arm.goToSetpoint(ArmSetpoints.kStowed)); m_driverController.start().onTrue(m_drivetrain.zeroGyro()); - // m_driverController.leftStick().toggleOnTrue(m_arm.aimElbowForTuning()); - // m_driverController.rightStick().toggleOnTrue(m_arm.aimWristForTuning()); + m_operatorController + .a() + .toggleOnTrue(m_arm.aimElbowForTuning(() -> -m_operatorController.getRightY())); + m_operatorController + .b() + .toggleOnTrue(m_arm.aimWristForTuning(() -> -m_operatorController.getRightY())); m_driverController.rightBumper().whileTrue(Superstructure.spit(m_shooter, m_feeder, m_intake)); - m_operatorController.leftStick().onTrue(m_arm.goToSetpoint(1.633, -2.371, 0.0, 0.0)); - m_driverController.b().whileTrue(Commands.parallel(m_arm.idleCoast(), m_climber.windWinch())); + m_operatorController.leftStick().onTrue(m_arm.goToSetpoint(ArmSetpoints.kClimb)); + m_operatorController + .rightStick() + .whileTrue(m_arm.goToSetpoint(ArmSetpoints.kFeedLow)) + .onFalse(m_arm.goToSetpoint(ArmSetpoints.kStowed)); + m_driverController.b().onTrue(m_arm.goToSetpoint(ArmSetpoints.kTrap)); + m_driverController.b().whileTrue(Commands.parallel(m_climber.windWinch())); + m_driverController + .x() + .whileTrue(Superstructure.autoTrap(m_arm, m_climber, m_shooter, m_lights)); + m_driverController + .y() + .whileTrue(new DriveToPose(FieldConstants.trapPose, m_drivetrain, m_lights)); + m_driverController.povRight().onTrue(m_shooter.trapShot()); + m_driverController.povLeft().whileTrue(m_shooter.trapAssist()); m_driverController.leftBumper().whileTrue(Superstructure.sensorIntake(m_feeder, m_intake)); - m_operatorController .leftTrigger() .whileTrue( Commands.parallel( - m_intake.feed(), - m_shooter.ampShot(), - m_arm.goToSetpoint(1.49 + 0.0873, -2.307, 0.0, 0.0))) - .onFalse(m_arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); - m_operatorController.rightTrigger().onTrue(m_arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); + m_intake.feed(), m_shooter.ampShot(), m_arm.goToSetpoint(ArmSetpoints.kAmp))) + .onFalse(m_arm.goToSetpoint(ArmSetpoints.kStowed)); + m_operatorController.rightTrigger().onTrue(m_arm.goToSetpoint(ArmSetpoints.kStowed)); m_operatorController .povUp() - .onTrue( - Commands.parallel(m_arm.goToSetpoint(-0.52, 2.083, 0.0, 0.0), m_shooter.runShooter())) - .onFalse(m_arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); + .onTrue(Commands.parallel(m_arm.aimWrist(2.083), m_shooter.runShooter())) + .onFalse(m_arm.goToSetpoint(ArmSetpoints.kStowed)); - m_operatorController.y().whileTrue(m_shooter.runShooter()); - m_operatorController.a().whileTrue(m_shooter.ampShot()); + m_driverController.povDown().whileTrue(m_feeder.feed().alongWith(m_intake.spit())); + m_operatorController.y().whileTrue(m_shooter.revUpShooter()); m_operatorController .leftBumper() .onTrue(Superstructure.sensorCatch(m_shooter, m_feeder, m_intake, m_arm)) - .onFalse(m_arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); + .onFalse(m_arm.goToSetpoint(ArmSetpoints.kStowed)); + + final HashMap commandCounts = new HashMap<>(); + final BiConsumer logCommandConsumer = + (Command command, Boolean active) -> { + final String name = command.getName(); + final int count = commandCounts.getOrDefault(name, 0) + (active ? 1 : -1); + commandCounts.put(name, count); + Logger.recordOutput( + "CommandsUnique/" + name + "_" + Integer.toHexString(command.hashCode()), active); + Logger.recordOutput("CommandsAll/" + name, count > 0); + }; + CommandScheduler.getInstance() + .onCommandInitialize(command -> logCommandConsumer.accept(command, true)); + CommandScheduler.getInstance() + .onCommandFinish(command -> logCommandConsumer.accept(command, false)); + CommandScheduler.getInstance() + .onCommandInterrupt(command -> logCommandConsumer.accept(command, false)); } - /** - * This function is called every robot packet, no matter the mode. Use this for items like - * diagnostics that you want ran during disabled, autonomous, teleoperated and test. - * - *

This runs after the mode specific periodic functions, but before LiveWindow and` - * SmartDashboard integrated updating. - */ @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); m_vision.periodic(); + m_gamePieceDetection.periodic(); NoteVisualizer.showHeldNotes(); + logAimingParameters(); } @Override public void autonomousInit() { NoteVisualizer.resetNotes(); NoteVisualizer.setHasNote(true); + m_intake.resetSimIntookPieces(); + Command autonomousCommand = m_autoChooser.get(); - if (autonomousCommand != null) { - autonomousCommand.schedule(); - } + if (autonomousCommand != null) autonomousCommand.schedule(); } @Override @@ -234,7 +330,9 @@ public void teleopInit() {} public void teleopPeriodic() {} @Override - public void disabledInit() {} + public void disabledInit() { + if (m_lights.getCurrentCommand() != null) m_lights.getCurrentCommand().cancel(); + } @Override public void disabledPeriodic() {} @@ -272,4 +370,18 @@ private void recordMetadeta() { break; } } + + private void logAimingParameters() { + final AimingParameters aimingParameters = PoseEstimation.getInstance().getAimingParameters(); + Logger.recordOutput( + "PoseEstimation/AimingParameters/DriveHeading", aimingParameters.driveHeading()); + Logger.recordOutput("PoseEstimation/AimingParameters/ArmAngle", aimingParameters.armAngle()); + Logger.recordOutput( + "PoseEstimation/AimingParameters/DriveFeedVelocity", aimingParameters.driveFeedVelocity()); + Logger.recordOutput( + "PoseEstimation/AimingParameters/EffectiveDistance", + PoseEstimation.getInstance().getAimingParameters().effectiveDistance()); + Logger.recordOutput( + "PoseEstimation/AimingParameters/AimingJointIndex", aimingParameters.aimingJointIndex()); + } } diff --git a/src/main/java/frc/robot/Subsystems.java b/src/main/java/frc/robot/Subsystems.java index 624e9a2..9424bdf 100644 --- a/src/main/java/frc/robot/Subsystems.java +++ b/src/main/java/frc/robot/Subsystems.java @@ -11,6 +11,7 @@ import frc.robot.climber.Climber; import frc.robot.climber.ClimberIO; import frc.robot.climber.ClimberIOSparkMAX; +import frc.robot.climber.ClimberIOTalonFX; import frc.robot.drivetrain.Drivetrain; import frc.robot.drivetrain.ImuIO; import frc.robot.drivetrain.ImuIOPigeon2; @@ -24,12 +25,15 @@ import frc.robot.intake.IntakeIO; import frc.robot.intake.IntakeIOSim; import frc.robot.intake.IntakeIOSparkMAX; +import frc.robot.intake.IntakeIOTalonFX; import frc.robot.shooter.Shooter; import frc.robot.shooter.ShooterIO; import frc.robot.shooter.ShooterIOSim; import frc.robot.shooter.ShooterIOTalonFX; +import frc.robot.vision.GamePieceDetection; +import frc.robot.vision.GamePieceDetectionIO; +import frc.robot.vision.GamePieceDetectionIOLimelight; import frc.robot.vision.Vision; -import frc.robot.vision.VisionIO; import frc.robot.vision.VisionIOPhotonVision; public class Subsystems { @@ -83,8 +87,12 @@ public static Vision createFourCameraVision() { Units.degreesToRadians(-150.0))))); } - public static Vision createBlankFourCameraVision() { - return new Vision(new VisionIO() {}, new VisionIO() {}, new VisionIO() {}, new VisionIO() {}); + public static Vision createBlankVision() { + return new Vision(); + } + + public static Intake createTalonFXIntake() { + return new Intake(new IntakeIOTalonFX()); } public static Intake createSparkMAXIntake() { @@ -115,6 +123,10 @@ public static Climber createSparkMAXClimber() { return new Climber(new ClimberIOSparkMAX()); } + public static Climber createTalonFXClimber() { + return new Climber(new ClimberIOTalonFX()); + } + public static Climber createBlankClimber() { return new Climber(new ClimberIO() {}); } @@ -142,4 +154,12 @@ public static Feeder createSimFeeder() { public static Feeder createBlankFeeder() { return new Feeder(new FeederIO() {}); } + + public static GamePieceDetection createLimelightGamePieceDetection() { + return new GamePieceDetection(new GamePieceDetectionIOLimelight() {}); + } + + public static GamePieceDetection createBlankGamePieceDetection() { + return new GamePieceDetection(new GamePieceDetectionIO() {}); + } } diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index b2a73dc..8647b7a 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -1,10 +1,16 @@ package frc.robot; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.util.Color; 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.ScheduleCommand; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.arm.Arm; +import frc.robot.arm.Arm.ArmSetpoints; +import frc.robot.climber.Climber; import frc.robot.drivetrain.Drivetrain; import frc.robot.feeder.Feeder; import frc.robot.intake.Intake; @@ -14,7 +20,6 @@ /** Class of factories for commands that combine multiple subsystems. */ public class Superstructure { public static Command sensorIntake(Feeder feeder, Intake intake) { - // In sim just wait 5 seconds since we don't have a sensor final Command feedUntilHasNote = Commands.either( feeder.feed().until(intake.hasIntookPieceSim), @@ -27,7 +32,7 @@ public static Command sensorIntake(Feeder feeder, Intake intake) { public static Command sensorCatch(Shooter shooter, Feeder feeder, Intake intake, Arm arm) { return Commands.parallel( - arm.goToSetpoint(-0.558, 2.264 - Units.degreesToRadians(5.0), 0, 0), + arm.aimWrist(2.264 - Units.degreesToRadians(5.0)), feeder.enterCoast(), shooter.catchNote()) .andThen(feeder.pullBack().until(feeder.hasNote)) @@ -35,16 +40,15 @@ public static Command sensorCatch(Shooter shooter, Feeder feeder, Intake intake, } public static Command ampShot(Arm arm, Shooter shooter) { - return Commands.parallel( - arm.goToSetpoint(1.49 + 0.0873, -2.307, 0.0, 0.0), shooter.runShooter()) - .finallyDo(() -> arm.goToSetpoint(-0.548, 2.485, 0.15, 0.0)); + return Commands.parallel(arm.goToSetpoint(ArmSetpoints.kAmp), shooter.runShooter()) + .finallyDo(() -> arm.goToSetpoint(ArmSetpoints.kStowed)); } public static Command spit(Shooter shooter, Feeder feeder, Intake intake) { return Commands.parallel(shooter.spit(), feeder.spit(), intake.spit()).withName("Spit"); } - public static Command aimAtGoal(Drivetrain drivetrain, Shooter shooter, Lights lights) { + public static Command aimAtGoal(Drivetrain drivetrain, Shooter shooter, Arm arm, Lights lights) { return Commands.parallel( Commands.startEnd( () -> @@ -52,8 +56,53 @@ public static Command aimAtGoal(Drivetrain drivetrain, Shooter shooter, Lights l () -> PoseEstimation.getInstance().getAimingParameters().driveHeading()), drivetrain::clearHeadingGoal), shooter.runShooter(), - lights.showReadyToShootStatus(drivetrain.atHeadingGoal.and(shooter.readyToShoot))) - .finallyDo(() -> lights.getCurrentCommand().cancel()) + // arm.aim( + // () -> PoseEstimation.getInstance().getAimingParameters().aimingJointIndex(), + // () -> PoseEstimation.getInstance().getAimingParameters().armAngle()), + lights.showReadyToShootStatus( + drivetrain.atHeadingGoal.and(shooter.readyToShoot).and(arm.atGoal))) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); + } + + public static Command feedShotHigh( + Drivetrain drivetrain, Shooter shooter, Arm arm, Lights lights) { + return Commands.parallel( + Commands.startEnd( + () -> drivetrain.setHeadingGoal(PoseEstimation.getInstance()::getFeedHeading), + drivetrain::clearHeadingGoal), + shooter.feederShot(), + arm.goToSetpoint(ArmSetpoints.kFeedHigh), + lights.showReadyToShootStatus( + drivetrain.atHeadingGoal.and(arm.atGoal).and(shooter.readyToShoot))) .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); } + + public static Command feedShotLow( + Drivetrain drivetrain, Shooter shooter, Arm arm, Lights lights) { + return Commands.parallel( + Commands.startEnd( + () -> drivetrain.setHeadingGoal(PoseEstimation.getInstance()::getFeedHeading), + drivetrain::clearHeadingGoal), + shooter.runShooter(), + arm.goToSetpoint(ArmSetpoints.kFeedLow), + lights.showReadyToShootStatus(drivetrain.atHeadingGoal.and(arm.atGoal))) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); + } + + public static Command autoTrap(Arm arm, Climber climber, Shooter shooter, Lights lights) { + return Commands.sequence( + new ScheduleCommand(arm.goToSetpoint(ArmSetpoints.kTrap)), + climber.goToTrapLimit(), + new ScheduleCommand(lights.startBlink(Color.kGreen)), + new ScheduleCommand(shooter.trapShot())) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming); + } + + public static Command shortRumble(CommandXboxController controller) { + return Commands.sequence( + Commands.runOnce(() -> controller.getHID().setRumble(RumbleType.kBothRumble, 0.5)), + Commands.waitSeconds(0.3), + Commands.runOnce(() -> controller.getHID().setRumble(RumbleType.kBothRumble, 0.0))) + .withName("Short Rumble"); + } } diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java index 9430387..8809eaa 100644 --- a/src/main/java/frc/robot/arm/Arm.java +++ b/src/main/java/frc/robot/arm/Arm.java @@ -1,95 +1,70 @@ package frc.robot.arm; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; + +import com.ctre.phoenix6.SignalLogger; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.numbers.N2; -import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; +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 frc.lib.LoggedTunableNumber; -import frc.robot.Constants; -import java.util.Optional; -import java.util.function.BiFunction; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Arm extends SubsystemBase { - private final double kSDeadband = 0.05; + public static final double kCatchWristAngleRad = 2.264 - Units.degreesToRadians(5.0); + public static final double kSubwooferWristAngleRad = 2.083; + private final double kJointTolerenceDegrees = 2.0; + private final ArmIO m_io; private final ArmIOInputsAutoLogged m_inputs = new ArmIOInputsAutoLogged(); - private final ArmModel m_armModel = new ArmModel(); - private final ArmKinematics m_kinematics = new ArmKinematics(); - private final ArmVisualizer m_measuredVisualizer; - private final ArmVisualizer m_setpointVisualizer; - private ProfiledPIDController m_elbowController = - new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(0.0, 0.0)); - private ProfiledPIDController m_wristController = - new ProfiledPIDController(0.0, 0.0, 0.0, new TrapezoidProfile.Constraints(0.0, 0.0)); - - private static final LoggedTunableNumber elbowkP = new LoggedTunableNumber("Arm/Elbow/kP"); - private static final LoggedTunableNumber elbowkD = new LoggedTunableNumber("Arm/Elbow/kD"); - private static final LoggedTunableNumber elbowkS = new LoggedTunableNumber("Arm/Elbow/kS"); - private static final LoggedTunableNumber elbowCruiseVelocityRadPerSec = - new LoggedTunableNumber("Arm/Elbow/CruiseVelocity"); - private static final LoggedTunableNumber elbowMaxAccelerationRadPerSecSq = - new LoggedTunableNumber("Arm/Elbow/MaxAcceleration"); - private static final LoggedTunableNumber wristkP = new LoggedTunableNumber("Arm/Wrist/kP"); - private static final LoggedTunableNumber wristkD = new LoggedTunableNumber("Arm/Wrist/kD"); - private static final LoggedTunableNumber wristkS = new LoggedTunableNumber("Arm/Wrist/kS"); - private static final LoggedTunableNumber wristCruiseVelocityRadPerSec = - new LoggedTunableNumber("Arm/Wrist/CruiseVelocity"); - private static final LoggedTunableNumber wristMaxAccelerationRadPerSecSq = - new LoggedTunableNumber("Arm/Wrist/MaxAcceleration"); - - // We don't want to add kS to the feedback when the output is close to 0 - private final BiFunction m_deadbandkS = - (volts, kS) -> Math.abs(volts) < kSDeadband ? volts : volts + Math.copySign(kS, volts); + private final ArmVisualizer m_goalVisualizer, m_setpointVisualizer, m_measuredVisualizer; - private Vector m_profileInitialAngles; - private double m_elbowPositionRad, m_wristPositionRad; - - static { - switch (Constants.kCurrentMode) { - case kReal: - elbowkP.initDefault(3.8); - elbowkD.initDefault(0); - elbowkS.initDefault(0.55); - elbowCruiseVelocityRadPerSec.initDefault(5.0); - elbowMaxAccelerationRadPerSecSq.initDefault(8.0); - wristkP.initDefault(3.3); - wristkD.initDefault(0); - wristkS.initDefault(0.4); - wristCruiseVelocityRadPerSec.initDefault(4.0); - wristMaxAccelerationRadPerSecSq.initDefault(8.0); - break; - case kSim: - // We do not want kS in simulation - elbowkP.initDefault(5.0); - elbowkD.initDefault(0.0); - elbowCruiseVelocityRadPerSec.initDefault(3.0); - elbowMaxAccelerationRadPerSecSq.initDefault(2.0); - wristkP.initDefault(20.0); - wristkD.initDefault(0.0); - wristCruiseVelocityRadPerSec.initDefault(5.0); - wristMaxAccelerationRadPerSecSq.initDefault(2.0); - break; - default: - break; - } - } + private Vector m_initialAngles; + private double m_lastElbowSetpoint; + private double m_lastWristSetpoint; + public Trigger atGoal = new Trigger(this::jointsAtGoal).debounce(0.1, DebounceType.kBoth); + public Supplier wristPoseSupplier; + private final SysIdRoutine m_sysIdRoutineElbow, m_sysIdRoutineWrist; public Arm(ArmIO io) { m_io = io; - m_measuredVisualizer = new ArmVisualizer("ArmMeasured"); - m_setpointVisualizer = new ArmVisualizer("ArmSetpoint"); - m_wristController.enableContinuousInput(-Math.PI, Math.PI); - m_elbowController.setTolerance(Units.degreesToRadians(0.25)); - m_wristController.setTolerance(Units.degreesToRadians(0.25)); + m_goalVisualizer = new ArmVisualizer("ArmGoal", 2, Color.kGreen); + m_setpointVisualizer = new ArmVisualizer("ArmSetpoint", 4, Color.kGreen); + m_measuredVisualizer = new ArmVisualizer("ArmMeasured", 6, Color.kDarkGreen); + wristPoseSupplier = () -> m_measuredVisualizer.getWristPose(); + m_sysIdRoutineElbow = + new SysIdRoutine( + new SysIdRoutine.Config( + Volts.of(0.25).per(Seconds.of(1.0)), + Volts.of(2.0), + null, + state -> SignalLogger.writeString("ElbowSysIdState", state.toString())), + new SysIdRoutine.Mechanism( + voltage -> m_io.setElbowVoltage(voltage.in(Volts)), null, this)); + m_sysIdRoutineWrist = + new SysIdRoutine( + new SysIdRoutine.Config( + Volts.of(0.25).per(Seconds.of(1.0)), + Volts.of(2.0), + null, + state -> SignalLogger.writeString("WristSysIdState", state.toString())), + new SysIdRoutine.Mechanism( + voltage -> m_io.setWristVoltage(voltage.in(Volts)), null, this)); setDefaultCommand(idle()); } @@ -97,237 +72,202 @@ public void periodic() { m_io.updateInputs(m_inputs); Logger.processInputs("ArmInputs", m_inputs); - m_elbowPositionRad = m_inputs.elbowRelativePositionRad; - m_wristPositionRad = m_inputs.wristRelativePositionRad; - - if (DriverStation.isDisabled()) { - m_io.setElbowVoltage(0.0); - m_io.setWristVoltage(0.0); - } - - // #region Update controllers wßhen tunables change. - if (elbowkP.hasChanged(hashCode()) || elbowkD.hasChanged(hashCode())) { - m_elbowController.setP(elbowkP.get()); - m_elbowController.setD(elbowkD.get()); - } - if (wristkP.hasChanged(hashCode()) || wristkD.hasChanged(hashCode())) { - m_wristController.setP(wristkP.get()); - m_wristController.setD(wristkD.get()); - } - - if (elbowCruiseVelocityRadPerSec.hasChanged(hashCode()) - || elbowMaxAccelerationRadPerSecSq.hasChanged(hashCode())) { - m_elbowController.setConstraints( - new TrapezoidProfile.Constraints( - elbowCruiseVelocityRadPerSec.get(), elbowMaxAccelerationRadPerSecSq.get())); - } - if (wristCruiseVelocityRadPerSec.hasChanged(hashCode()) - || wristMaxAccelerationRadPerSecSq.hasChanged(hashCode())) { - m_wristController.setConstraints( - new TrapezoidProfile.Constraints( - wristCruiseVelocityRadPerSec.get(), wristMaxAccelerationRadPerSecSq.get())); - } - // #endregion + if (DriverStation.isDisabled()) m_io.stop(); Logger.recordOutput( - "Arm/Forward Kinematics Position", - m_kinematics.forward(VecBuilder.fill(m_elbowPositionRad, m_wristPositionRad))); - m_measuredVisualizer.update(m_elbowPositionRad, m_wristPositionRad); + "Arm/Current Elbow Angle Degrees", Units.rotationsToDegrees(m_inputs.elbowPositionRot)); Logger.recordOutput( - "Arm/Current Command", - getCurrentCommand() == null ? "Null" : getCurrentCommand().getName()); - } - - private void setSetpoint(Vector setpointAnglesRad) { - double elbowAngle = setpointAnglesRad.get(0, 0); - double wristAngle = setpointAnglesRad.get(1, 0); - Logger.recordOutput("Arm/Goal Arm Position", m_kinematics.forward(setpointAnglesRad)); - Logger.recordOutput("Arm/Goal Elbow Angle", elbowAngle); - Logger.recordOutput("Arm/Goal Wrist Angle", wristAngle); - m_setpointVisualizer.update(elbowAngle, wristAngle); - m_elbowController.reset(m_elbowPositionRad); - m_elbowController.setGoal(elbowAngle); - m_wristController.reset(m_wristPositionRad); - m_wristController.setGoal(wristAngle); - m_profileInitialAngles = VecBuilder.fill(m_elbowPositionRad, m_wristPositionRad); - } - - private void setSetpoint(Translation2d setpoint) { - Optional> setpointAnglesRad = m_kinematics.inverse(setpoint); - setpointAnglesRad.ifPresent(angles -> setSetpoint(angles)); + "Arm/Current Wrist Angle Degrees", Units.rotationsToDegrees(m_inputs.wristPositionRot)); + m_measuredVisualizer.update( + Units.rotationsToRadians(m_inputs.elbowPositionRot), + Units.rotationsToRadians(m_inputs.wristPositionRot)); + m_setpointVisualizer.update( + Units.rotationsToRadians(m_inputs.elbowPositionSetpointRot), + Units.rotationsToRadians(m_inputs.wristPositionSetpointRot)); } /** * @param elbowDelay The percent that the wrist must travel before the elbow moves. * @param wristDelay The percent that the elbow must travel before the wrist moves. */ - private void runSetpoint(double elbowDelay, double wristDelay) { + private void runSetpoint( + double elbowAngleRad, double wristAngleRad, double elbowDelay, double wristDelay) { if (elbowDelay > 0.0 && wristDelay > 0.0) throw new IllegalArgumentException("Only one joint can be delayed at a time."); if (elbowDelay < 0.0 || wristDelay < 0.0) throw new IllegalArgumentException("Percent delay can't be negative."); - double elbowGoalPosition = m_elbowController.getSetpoint().position; - double wristGoalPosition = m_wristController.getSetpoint().position; - double elbowGoalVelocity = m_elbowController.getSetpoint().velocity; - double wristGoalVelocity = m_wristController.getSetpoint().velocity; + m_lastElbowSetpoint = elbowAngleRad; + m_lastWristSetpoint = wristAngleRad; + Logger.recordOutput("Arm/Goal Elbow Angle", Units.radiansToDegrees(elbowAngleRad)); + Logger.recordOutput("Arm/Goal Wrist Angle", Units.radiansToDegrees(wristAngleRad)); + m_goalVisualizer.update(elbowAngleRad, wristAngleRad); double elbowProgress = - Math.abs(m_elbowPositionRad - m_profileInitialAngles.get(0, 0)) - / Math.abs(m_elbowController.getGoal().position - m_profileInitialAngles.get(0, 0)); + Math.abs(m_inputs.elbowPositionRot - m_initialAngles.get(0, 0)) + / Math.abs(Units.degreesToRotations(elbowAngleRad) - m_initialAngles.get(0, 0)); double wristProgress = - Math.abs(m_wristPositionRad - m_profileInitialAngles.get(1, 0)) - / Math.abs(m_wristController.getGoal().position - m_profileInitialAngles.get(1, 0)); + Math.abs(m_inputs.wristPositionRot - m_initialAngles.get(1, 0)) + / Math.abs(Units.degreesToRotations(wristAngleRad) - m_initialAngles.get(1, 0)); Logger.recordOutput("Arm/Elbow Progress", elbowProgress); Logger.recordOutput("Arm/Wrist Progress", wristProgress); - double elbowFeedbackVolts = 0.0; - double wristFeedbackVolts = 0.0; - if (wristProgress < elbowDelay) { - elbowGoalPosition = m_elbowPositionRad; - elbowGoalVelocity = 0.0; - } else - elbowFeedbackVolts = - m_deadbandkS.apply(m_elbowController.calculate(m_elbowPositionRad), elbowkS.get()); - if (elbowProgress < wristDelay) { - wristGoalPosition = m_wristPositionRad; - wristGoalVelocity = 0.0; - } else - wristFeedbackVolts = - m_deadbandkS.apply(m_wristController.calculate(m_wristPositionRad), wristkS.get()); - Vector feedforwardVolts = - m_armModel.feedforward( - VecBuilder.fill(elbowGoalPosition, wristGoalPosition), - VecBuilder.fill(elbowGoalVelocity, wristGoalVelocity)); - double elbowFeedForwardVolts = feedforwardVolts.get(0, 0); - double wristFeedForwardVolts = feedforwardVolts.get(1, 0); - Logger.recordOutput("Arm/Elbow Feed Forward", elbowFeedForwardVolts); - Logger.recordOutput("Arm/Wrist Feed Forward", wristFeedForwardVolts); - m_io.setElbowVoltage(elbowFeedbackVolts + elbowFeedForwardVolts); - m_io.setWristVoltage(wristFeedbackVolts + wristFeedForwardVolts); - } - public Command goToSetpoint(ArmSetpoints setpoint) { - return this.runOnce( - () -> { - setSetpoint(setpoint.getTranslation()); - }) - .andThen( - this.run( - () -> runSetpoint(setpoint.getDelay().getFirst(), setpoint.getDelay().getSecond()))) - .withName("Set Inverse Setpoint"); + // if (wristProgress < elbowDelay) m_io.setElbowRotations(m_inputs.elbowPositionRot); + m_io.setElbowRotations(Units.radiansToRotations(elbowAngleRad)); + // if (elbowProgress < wristDelay) m_io.setWristRotations(m_inputs.wristPositionRot); + m_io.setWristRotations(Units.radiansToRotations(wristAngleRad)); } - public Command goToSetpoint( - double elbowAngleRad, double wristAngleRad, double elbowDelay, double wristDelay) { - return this.runOnce(() -> setSetpoint(VecBuilder.fill(elbowAngleRad, wristAngleRad))) - .andThen(this.run(() -> runSetpoint(elbowDelay, wristDelay))) - .withName("Set Forward Setpoint"); + @AutoLogOutput(key = "Arm/JointsAtGoal") + private boolean jointsAtGoal() { + return (MathUtil.isNear( + m_lastElbowSetpoint, + Units.rotationsToRadians(m_inputs.elbowPositionRot), + Units.degreesToRadians(kJointTolerenceDegrees)) + && MathUtil.isNear( + m_lastWristSetpoint, + Units.rotationsToRadians(m_inputs.wristPositionRot), + Units.degreesToRadians(kJointTolerenceDegrees))); } - public Command idle() { - return this.run( - () -> { - m_io.setElbowVoltage(0.0); - m_io.setWristVoltage(0.0); - }) - .withName("Idle"); + public Command storeInitialAngles() { + return this.runOnce( + () -> + m_initialAngles = + VecBuilder.fill(m_inputs.elbowPositionRot, m_inputs.wristPositionRot)); } - public Command idleCoast() { - return this.runOnce(() -> m_io.setBrakeMode(false)) + public Command aim(Supplier jointSupplier, Supplier angleSupplier) { + return storeInitialAngles() .andThen( this.run( () -> { - m_io.setElbowVoltage(0.0); - m_io.setWristVoltage(0.0); + if (jointSupplier.get() == Joint.kElbow) + runSetpoint( + angleSupplier.get().getRadians(), + ArmSetpoints.kStowed.wristAngle, + 0.0, + 0.0); + else if (jointSupplier.get() == Joint.kWrist) + runSetpoint( + ArmSetpoints.kStowed.elbowAngle, + angleSupplier.get().getRadians(), + 0.0, + 0.0); + else throw new IllegalArgumentException("Invalid joint index."); })) - .finallyDo(() -> m_io.setBrakeMode(true)) - .ignoringDisable(true) - .withName("Idle Coast"); + .withName("Aim (Arm)"); } - public Command setBrake() { - return this.runOnce(() -> m_io.setBrakeMode(true)); + public Command aimWrist(double wristAngleRad) { + return storeInitialAngles() + .andThen( + this.run(() -> runSetpoint(ArmSetpoints.kStowed.elbowAngle, wristAngleRad, 0.0, 0.0))) + .withName("Aim Wrist"); } - class ArmKinematics { - - public Translation2d forward(Vector angles) { + public Command aimElbowForTuning(DoubleSupplier driveEffort) { + var state = + new Object() { + double currentPos = ArmSetpoints.kStowed.elbowAngle; + }; + return Commands.run( + () -> state.currentPos += MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 0.02) + .alongWith(aim(() -> Joint.kElbow, () -> Rotation2d.fromRadians(state.currentPos))) + .withName("Aim Elbow for Tuning"); + } - double elbowAngle = angles.get(0, 0); - double wristAngle = angles.get(1, 0); + public Command aimWristForTuning(DoubleSupplier driveEffort) { + var state = + new Object() { + double currentPos = ArmSetpoints.kStowed.wristAngle; + }; + return Commands.run( + () -> state.currentPos += MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 0.02) + .alongWith(aim(() -> Joint.kWrist, () -> Rotation2d.fromRadians(state.currentPos))) + .withName("Aim Wrist For Tuning"); + } - return new Translation2d( - ArmModel.origin.getX() - + ArmModel.kElbowLengthMeters * Math.cos(elbowAngle) - + ArmModel.kWristLengthMeters * Math.cos(elbowAngle + (wristAngle - elbowAngle)), - ArmModel.origin.getY() - + ArmModel.kElbowLengthMeters * Math.sin(elbowAngle) - + ArmModel.kWristLengthMeters * Math.sin(elbowAngle + (wristAngle - elbowAngle))); - } + public Command holdSetpoint() { + return storeInitialAngles() + .andThen( + this.run( + () -> + runSetpoint( + Units.rotationsToRadians(m_initialAngles.get(0)), + Units.rotationsToRadians(m_initialAngles.get(1)), + 0.0, + 0.0))) + .withName("Hold Setpoint (Arm)"); + } - public Translation2d forwardWristRelativeToElbow(Vector angles) { + public Command goToSetpoint(ArmSetpoints setpoint) { + return storeInitialAngles() + .andThen( + this.run( + () -> + runSetpoint( + setpoint.elbowAngle, + setpoint.wristAngle, + setpoint.elbowDelay, + setpoint.wristDelay))) + .withName("Go To Setpoint (Arm)"); + } - return new Translation2d( - ArmModel.origin.getX() - + ArmModel.kElbowLengthMeters * Math.cos(angles.get(0, 0)) - + ArmModel.kWristLengthMeters * Math.cos(angles.get(0, 0) + angles.get(1, 0)), - ArmModel.origin.getY() - + ArmModel.kElbowLengthMeters * Math.sin(angles.get(0, 0)) - + ArmModel.kWristLengthMeters * Math.sin(angles.get(0, 0) + angles.get(1, 0))); - } + public Command idle() { + return this.run(m_io::stop).withName("Idle (Arm)"); + } - // This is still broken - public Optional> inverse(Translation2d position) { - Translation2d relativePosition = position.minus(ArmModel.origin); + public Command stop() { + return this.runOnce(m_io::stop); + } - // Flip when X is negative - boolean isFlipped = true; // relativePosition.getX() < 0.0; - if (isFlipped) - relativePosition = new Translation2d(-relativePosition.getX(), relativePosition.getY()); + public Command idleCoast() { + return this.runOnce(() -> m_io.setBrakeMode(false)) + .andThen(this.run(() -> m_io.stop())) + .finallyDo(() -> m_io.setBrakeMode(true)) + .ignoringDisable(true) + .withName("Idle Coast (Arm)"); + } - // Calculate angles - double wristAngle = - -Math.acos( - MathUtil.clamp( - (Math.pow(relativePosition.getX(), 2) - + Math.pow(relativePosition.getY(), 2) - - Math.pow(ArmModel.kElbowLengthMeters, 2) - - Math.pow(ArmModel.kWristLengthMeters, 2)) - / (2 * ArmModel.kElbowLengthMeters * ArmModel.kWristLengthMeters), - -1.0, - 1.0)); - double elbowAngle = - Math.atan2(relativePosition.getY(), relativePosition.getX()) - - Math.atan2( - (ArmModel.kWristLengthMeters * Math.sin(wristAngle)), - (ArmModel.kElbowLengthMeters - + ArmModel.kWristLengthMeters * Math.cos(wristAngle))); + public Command sysIdElbowQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineElbow.quasistatic(direction); + } - // Invert elbow angle if invalid - Translation2d testPosition = - forwardWristRelativeToElbow(VecBuilder.fill(elbowAngle, wristAngle)) - .minus(ArmModel.origin); - if (testPosition.getDistance(relativePosition) > 1e-3) { - elbowAngle += Math.PI; - } + public Command sysIdElbowDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineElbow.dynamic(direction); + } - // Flip angles - if (isFlipped) { - elbowAngle = Math.PI - elbowAngle; - wristAngle = -wristAngle; - } + public Command sysIdWristQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineWrist.quasistatic(direction); + } - // Wrap angles to correct ranges - elbowAngle = MathUtil.angleModulus(elbowAngle); - wristAngle = MathUtil.angleModulus(wristAngle); + public Command sysIdWristDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineWrist.dynamic(direction); + } - // Exit if outside valid ranges for the joints - if (elbowAngle < ArmModel.kElbowMinAngleRad - || elbowAngle > ArmModel.kElbowMaxAngleRad - || wristAngle < ArmModel.kWristMinAngleRad - || wristAngle > ArmModel.kWristMaxAngleRad) { - return Optional.empty(); - } - return Optional.of(VecBuilder.fill(elbowAngle, wristAngle + elbowAngle)); + public enum ArmSetpoints { + kStowed(Units.degreesToRadians(-31), Units.degreesToRadians(142), 0.15, 0.0), + kAmp(1.49 + 0.0873 + Units.degreesToRadians(8.0), Units.degreesToRadians(228), 0.0, 0.0), + kClimb(1.633, -2.371, 0.0, 0.0), + kTrap(Units.degreesToRadians(53.0), Units.degreesToRadians(80.0), 0.0, 0.0), + kFeedHigh(Units.degreesToRadians(-31), 2.083, 0.0, 0.0), + kFeedLow(Units.degreesToRadians(10.0), Units.degreesToRadians(170.0), 0.0, 0.0); + + public final double elbowAngle; + public final double wristAngle; + public final double elbowDelay; + public final double wristDelay; + + private ArmSetpoints( + double elbowAngle, double wristAngle, double elbowDelay, double wristDelay) { + this.elbowAngle = elbowAngle; + this.wristAngle = wristAngle; + this.elbowDelay = elbowDelay; + this.wristDelay = wristDelay; } } + + public enum Joint { + kElbow, + kWrist + } } diff --git a/src/main/java/frc/robot/arm/ArmConstants.java b/src/main/java/frc/robot/arm/ArmConstants.java new file mode 100644 index 0000000..08a4464 --- /dev/null +++ b/src/main/java/frc/robot/arm/ArmConstants.java @@ -0,0 +1,29 @@ +package frc.robot.arm; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; + +public class ArmConstants { + private static final double kElbowGearboxReduction = 3.0 * 5.0 * 5.0; + private static final double kElbowChainReduction = 36.0 / 22.0; + public static final double kElbowReduction = kElbowGearboxReduction * kElbowChainReduction; + public static final double kElbowLengthMeters = Units.inchesToMeters(19.0); + public static final double kElbowMinAngleRad = -Math.PI; + public static final double kElbowMaxAngleRad = Math.PI; + public static final DCMotor kElbowGearbox = DCMotor.getFalcon500Foc(2); + public static final double kElbowMassKg = Units.lbsToKilograms(18.5); + public static final double kElbowMoiKgMetersSq = + SingleJointedArmSim.estimateMOI(kElbowLengthMeters, kElbowMassKg); + + private static final double kWristGearboxReduction = 3.0 * 5.0; + private static final double kWristChainReduction = 36.0 / 22.0; + public static final double kWristReduction = kWristGearboxReduction * kWristChainReduction; + public static final double kWristLengthMeters = Units.inchesToMeters(15.0); + public static final double kWristMinAngleRad = Double.NEGATIVE_INFINITY; + public static final double kWristMaxAngleRad = Double.POSITIVE_INFINITY; + public static final DCMotor kWristGearbox = DCMotor.getFalcon500Foc(2); + public static final double kWristMassKg = Units.lbsToKilograms(11.5); + public static final double kWristMoiKgMetersSq = + SingleJointedArmSim.estimateMOI(kWristLengthMeters, kWristMassKg); +} diff --git a/src/main/java/frc/robot/arm/ArmIO.java b/src/main/java/frc/robot/arm/ArmIO.java index f6ab4ef..366a29a 100644 --- a/src/main/java/frc/robot/arm/ArmIO.java +++ b/src/main/java/frc/robot/arm/ArmIO.java @@ -6,30 +6,34 @@ public interface ArmIO { @AutoLog public static class ArmIOInputs { - public double elbowAbsolutePositionRad = 0.0; - public boolean elbowAbsoluteEncoderConnected = false; - public double elbowEncoderRaw = 0.0; - public double elbowRelativePositionRad = 0.0; - public double elbowVelocityRadPerSec = 0.0; + public double elbowPositionRot = 0.0; + public double elbowPositionSetpointRot = 0.0; + public double elbowVelocityRps = 0.0; public double elbowAppliedVolts = 0.0; - public double[] elbowCurrentAmps = new double[] {}; + public double elbowTorqueCurrentAmps = 0.0; + public double[] elbowStatorCurrentAmps = new double[] {}; - public double wristAbsolutePositionRad = 0.0; - public boolean wristAbsoluteEncoderConnected = false; - public double wristEncoderRaw = 0.0; - public double wristRelativePositionRad = 0.0; - public double wristVelocityRadPerSec = 0.0; + public double wristPositionRot = 0.0; + public double wristPositionSetpointRot = 0.0; + public double wristVelocityRps = 0.0; public double wristAppliedVolts = 0.0; - public double[] wristCurrentAmps = new double[] {}; + public double wristTorqueCurrentAmps = 0.0; + public double[] wristStatorCurrentAmps = new double[] {}; public boolean allMotorsConnected = false; } public default void updateInputs(ArmIOInputs inputs) {} + public default void setElbowRotations(double angleRot) {} + + public default void setWristRotations(double angleRot) {} + public default void setElbowVoltage(double volts) {} public default void setWristVoltage(double volts) {} + public default void stop() {} + public default void setBrakeMode(boolean enableBrakeMode) {} } diff --git a/src/main/java/frc/robot/arm/ArmIOSim.java b/src/main/java/frc/robot/arm/ArmIOSim.java index 111e365..bc92745 100644 --- a/src/main/java/frc/robot/arm/ArmIOSim.java +++ b/src/main/java/frc/robot/arm/ArmIOSim.java @@ -1,52 +1,109 @@ package frc.robot.arm; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.Vector; -import edu.wpi.first.math.numbers.N4; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import frc.robot.arm.Arm.ArmSetpoints; public class ArmIOSim implements ArmIO { - private Vector m_elbowWristSimStates = VecBuilder.fill(0.0, 0.0, 0.0, 0.0); - private ArmModel m_model = new ArmModel(); + + private final SingleJointedArmSim m_elbowSim, m_wristSim; + private final PIDController m_elbowFeedback, m_wristFeedback; + private final ArmFeedforward m_elbowFeedforward, m_wristFeedforward; + private double m_currentElbowAngleRot, m_currentWristAngleRot; private double m_elbowAppliedVolts = 0.0; private double m_wristAppliedVolts = 0.0; - public ArmIOSim() {} + public ArmIOSim() { + m_elbowSim = + new SingleJointedArmSim( + ArmConstants.kElbowGearbox, + ArmConstants.kElbowReduction, + ArmConstants.kElbowMoiKgMetersSq, + ArmConstants.kElbowLengthMeters, + ArmSetpoints.kStowed.elbowAngle, + ArmConstants.kElbowMaxAngleRad, + false, + ArmSetpoints.kStowed.elbowAngle); + m_wristSim = + new SingleJointedArmSim( + ArmConstants.kWristGearbox, + ArmConstants.kWristReduction, + ArmConstants.kWristMoiKgMetersSq, + ArmConstants.kWristLengthMeters, + ArmConstants.kWristMinAngleRad, + ArmConstants.kWristMaxAngleRad, + false, + ArmSetpoints.kStowed.wristAngle); + m_elbowFeedback = new PIDController(25.0, 0.0, 0.0); + m_wristFeedback = new PIDController(20.0, 0.0, 0.0); + m_elbowFeedforward = new ArmFeedforward(0.0, 0.0, 0.0); + m_wristFeedforward = new ArmFeedforward(0.0, 0.0, 0.0); + m_wristFeedback.enableContinuousInput(-0.5, 0.5); + } public void updateInputs(ArmIOInputs inputs) { - m_elbowWristSimStates = - m_model.simulate( - m_elbowWristSimStates, VecBuilder.fill(m_elbowAppliedVolts, m_wristAppliedVolts), 0.02); - inputs.elbowAbsolutePositionRad = m_elbowWristSimStates.get(0, 0); - inputs.elbowAbsoluteEncoderConnected = true; - inputs.elbowRelativePositionRad = m_elbowWristSimStates.get(0, 0); - inputs.elbowVelocityRadPerSec = m_elbowWristSimStates.get(2, 0); + m_elbowSim.setInputVoltage(m_elbowAppliedVolts); + m_wristSim.setInputVoltage(m_wristAppliedVolts); + m_elbowSim.update(0.02); + m_wristSim.update(0.02); + + inputs.elbowPositionRot = Units.radiansToRotations(m_elbowSim.getAngleRads()); + m_currentElbowAngleRot = inputs.elbowPositionRot; + inputs.elbowVelocityRps = Units.radiansToRotations(m_elbowSim.getVelocityRadPerSec()); inputs.elbowAppliedVolts = m_elbowAppliedVolts; - inputs.elbowCurrentAmps = + inputs.elbowStatorCurrentAmps = new double[] { - ArmModel.kElbowGearbox.getCurrent(m_elbowWristSimStates.get(2, 0), m_elbowAppliedVolts) + ArmConstants.kElbowGearbox.getCurrent( + m_elbowSim.getVelocityRadPerSec(), m_elbowAppliedVolts) }; - inputs.wristAbsolutePositionRad = m_elbowWristSimStates.get(1, 0); - inputs.wristAbsoluteEncoderConnected = true; - inputs.wristRelativePositionRad = m_elbowWristSimStates.get(1, 0); - inputs.wristVelocityRadPerSec = m_elbowWristSimStates.get(3, 0); + inputs.wristPositionRot = Units.radiansToRotations(m_wristSim.getAngleRads()); + m_currentWristAngleRot = inputs.wristPositionRot; + inputs.wristVelocityRps = Units.radiansToRotations(m_wristSim.getVelocityRadPerSec()); inputs.wristAppliedVolts = m_wristAppliedVolts; - inputs.wristCurrentAmps = + inputs.wristStatorCurrentAmps = new double[] { - ArmModel.kWristGearbox.getCurrent(m_elbowWristSimStates.get(3, 0), m_wristAppliedVolts) + ArmConstants.kWristGearbox.getCurrent( + m_wristSim.getVelocityRadPerSec(), m_wristAppliedVolts) }; inputs.allMotorsConnected = true; } + @Override + public void setElbowRotations(double angleRot) { + m_elbowFeedback.reset(); + m_elbowAppliedVolts = + m_elbowFeedback.calculate(m_currentElbowAngleRot, angleRot) + + m_elbowFeedforward.calculate(Units.rotationsToRadians(angleRot), 0.0); + } + + @Override + public void setWristRotations(double angleRot) { + m_wristFeedback.reset(); + m_wristAppliedVolts = + m_wristFeedback.calculate(m_currentWristAngleRot, angleRot) + + m_wristFeedforward.calculate(Units.rotationsToRadians(angleRot), 0.0); + } + + @Override public void setElbowVoltage(double volts) { m_elbowAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); } + @Override public void setWristVoltage(double volts) { m_wristAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); } + + @Override + public void stop() { + m_elbowAppliedVolts = 0.0; + m_wristAppliedVolts = 0.0; + } } diff --git a/src/main/java/frc/robot/arm/ArmIOTalonFX.java b/src/main/java/frc/robot/arm/ArmIOTalonFX.java index 0d8a0d1..b63c20f 100644 --- a/src/main/java/frc/robot/arm/ArmIOTalonFX.java +++ b/src/main/java/frc/robot/arm/ArmIOTalonFX.java @@ -1,50 +1,45 @@ package frc.robot.arm; import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.ParentDevice; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DutyCycleEncoder; import frc.robot.Constants; public class ArmIOTalonFX implements ArmIO { // Offsets to the horizontal - private final double kElbowRelativeEncoderOffsetRad = 0.558; // .558; - private final double kWristRelativeEncoderOffsetRad = 2.5; // 2.5; - private final boolean kInvertWristAbsoluteEncoder = false; - private final boolean kInvertElbowAbsoluteEncoder = false; + private final double kElbowRelativeEncoderOffsetRad = -0.558; + private final double kWristRelativeEncoderOffsetRad = 2.5; private final TalonFX m_elbowLeftMotor, m_elbowRightFollowerMotor, m_wristLeftMotor, m_wristRightFollowerMotor; - private final DutyCycleEncoder m_elbowAbsoluteEncoder, m_wristAbsoluteEncoder; - private final VoltageOut m_elbowControl, m_wristControl; + private final PositionVoltage m_elbowControl; + private final MotionMagicVoltage m_wristControl; private final StatusSignal m_elbowPositionSignal, + m_elbowPositionSetpointSignal, m_elbowVelocitySignal, m_elbowAppliedVoltsSignal, - m_elbowCurrentSignal, - m_elbowFollowerCurrentSignal, + m_elbowTorqueCurrentSignal, + m_elbowStatorCurrentSignal, + m_elbowFollowerStatorCurrentSignal, m_wristPositionSignal, + m_wristPositionSetpointSignal, m_wristVelocitySignal, m_wristAppliedVoltsSignal, - m_wristCurrentSignal, - m_wristFollowerCurrentSignal; - - private final Rotation2d elbowAbsoluteEncoderOffset = new Rotation2d(2.471); - private final Rotation2d wristAbsoluteEncoderOffset = new Rotation2d(0.646); - // private final Rotation2d elbowAbsoluteEncoderOffset; - // private final Rotation2d wristAbsoluteEncoderOffset; + m_wristTorqueCurrentSignal, + m_wristStatorCurrentSignal, + m_wristFollowerStatorCurrentSignal; public ArmIOTalonFX() { m_elbowLeftMotor = new TalonFX(15, Constants.kSuperstructureCanBus); @@ -52,140 +47,175 @@ public ArmIOTalonFX() { m_wristLeftMotor = new TalonFX(16, Constants.kSuperstructureCanBus); m_wristRightFollowerMotor = new TalonFX(18, Constants.kSuperstructureCanBus); - m_elbowAbsoluteEncoder = new DutyCycleEncoder(8); - m_wristAbsoluteEncoder = new DutyCycleEncoder(9); - m_elbowAbsoluteEncoder.setDutyCycleRange(1.0 / 1025.0, 1024.0 / 1025.0); - m_wristAbsoluteEncoder.setDutyCycleRange(1.0 / 1025.0, 1024.0 / 1025.0); - - m_elbowLeftMotor.setPosition(0.0); - m_wristLeftMotor.setPosition(0.0); - m_elbowRightFollowerMotor.setPosition(0.0); - m_wristRightFollowerMotor.setPosition(0.0); + m_elbowLeftMotor.setPosition(Units.radiansToRotations(kElbowRelativeEncoderOffsetRad), 1.0); + m_wristLeftMotor.setPosition(Units.radiansToRotations(kWristRelativeEncoderOffsetRad), 1.0); final CurrentLimitsConfigs currentLimitsConfig = new CurrentLimitsConfigs(); - currentLimitsConfig.SupplyCurrentLimit = 80.0; - currentLimitsConfig.SupplyCurrentLimitEnable = true; + currentLimitsConfig.StatorCurrentLimit = 60.0; + currentLimitsConfig.StatorCurrentLimitEnable = true; final TalonFXConfiguration elbowLeftMotorConfig = new TalonFXConfiguration(); - m_elbowLeftMotor.getConfigurator().apply(elbowLeftMotorConfig); elbowLeftMotorConfig.CurrentLimits = currentLimitsConfig; elbowLeftMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; elbowLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - elbowLeftMotorConfig.Voltage.PeakReverseVoltage = -4.0; - m_elbowLeftMotor.getConfigurator().apply(elbowLeftMotorConfig); + elbowLeftMotorConfig.Feedback.SensorToMechanismRatio = ArmConstants.kElbowReduction; + elbowLeftMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + elbowLeftMotorConfig.Slot0.kP = 62.122; + elbowLeftMotorConfig.Slot0.kD = 10.118; + elbowLeftMotorConfig.Slot0.kS = 0.74887; + elbowLeftMotorConfig.Slot0.kV = 13.863; + elbowLeftMotorConfig.Slot0.kA = 1.2605; + elbowLeftMotorConfig.Slot0.kG = 0.37766; + elbowLeftMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 0.5; + elbowLeftMotorConfig.MotionMagic.MotionMagicAcceleration = 2.0; + elbowLeftMotorConfig.MotorOutput.DutyCycleNeutralDeadband = 0.01; + elbowLeftMotorConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40.0; + elbowLeftMotorConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40.0; + elbowLeftMotorConfig.Voltage.PeakForwardVoltage = 6.0; + elbowLeftMotorConfig.Voltage.PeakReverseVoltage = -6.0; + m_elbowLeftMotor.getConfigurator().apply(elbowLeftMotorConfig, 1.0); final TalonFXConfiguration wristLeftMotorConfig = new TalonFXConfiguration(); - m_wristLeftMotor.getConfigurator().apply(wristLeftMotorConfig); wristLeftMotorConfig.CurrentLimits = currentLimitsConfig; wristLeftMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; wristLeftMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - m_wristLeftMotor.getConfigurator().apply(wristLeftMotorConfig); + wristLeftMotorConfig.ClosedLoopGeneral.ContinuousWrap = true; + wristLeftMotorConfig.Feedback.SensorToMechanismRatio = ArmConstants.kWristReduction; + wristLeftMotorConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine; + wristLeftMotorConfig.Slot0.kP = 22.742; + wristLeftMotorConfig.Slot0.kD = 5.5071; + wristLeftMotorConfig.Slot0.kS = 0.57211; + wristLeftMotorConfig.Slot0.kV = 2.5239; + wristLeftMotorConfig.Slot0.kA = 0.10939; + wristLeftMotorConfig.Slot0.kG = 0.45457; + wristLeftMotorConfig.MotionMagic.MotionMagicCruiseVelocity = 0.5; + wristLeftMotorConfig.MotionMagic.MotionMagicAcceleration = 2.0; + wristLeftMotorConfig.Voltage.PeakForwardVoltage = 3.0; + wristLeftMotorConfig.Voltage.PeakReverseVoltage = -3.0; + wristLeftMotorConfig.MotorOutput.DutyCycleNeutralDeadband = 0.01; + wristLeftMotorConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40.0; + wristLeftMotorConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40.0; + + m_wristLeftMotor.getConfigurator().apply(wristLeftMotorConfig, 1.0); final TalonFXConfiguration elbowRightMotorConfig = new TalonFXConfiguration(); - m_elbowRightFollowerMotor.getConfigurator().apply(elbowRightMotorConfig); elbowRightMotorConfig.CurrentLimits = currentLimitsConfig; elbowRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - m_elbowRightFollowerMotor.getConfigurator().apply(elbowRightMotorConfig); + m_elbowRightFollowerMotor.getConfigurator().apply(elbowRightMotorConfig, 1.0); m_elbowRightFollowerMotor.setControl(new Follower(m_elbowLeftMotor.getDeviceID(), true)); final TalonFXConfiguration wristRightMotorConfig = new TalonFXConfiguration(); - m_wristRightFollowerMotor.getConfigurator().apply(wristRightMotorConfig); wristRightMotorConfig.CurrentLimits = currentLimitsConfig; wristRightMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - m_wristRightFollowerMotor.getConfigurator().apply(wristRightMotorConfig); + m_wristRightFollowerMotor.getConfigurator().apply(wristRightMotorConfig, 1.0); m_wristRightFollowerMotor.setControl(new Follower(m_wristLeftMotor.getDeviceID(), true)); m_elbowPositionSignal = m_elbowLeftMotor.getPosition(); + m_elbowPositionSetpointSignal = m_elbowLeftMotor.getClosedLoopReference(); m_elbowVelocitySignal = m_elbowLeftMotor.getVelocity(); m_elbowAppliedVoltsSignal = m_elbowLeftMotor.getMotorVoltage(); - m_elbowCurrentSignal = m_elbowLeftMotor.getStatorCurrent(); - m_elbowFollowerCurrentSignal = m_elbowRightFollowerMotor.getStatorCurrent(); + m_elbowTorqueCurrentSignal = m_elbowLeftMotor.getTorqueCurrent(); + m_elbowStatorCurrentSignal = m_elbowLeftMotor.getStatorCurrent(); + m_elbowFollowerStatorCurrentSignal = m_elbowRightFollowerMotor.getStatorCurrent(); m_wristPositionSignal = m_wristLeftMotor.getPosition(); + m_wristPositionSetpointSignal = m_wristLeftMotor.getClosedLoopReference(); m_wristVelocitySignal = m_wristLeftMotor.getVelocity(); m_wristAppliedVoltsSignal = m_wristLeftMotor.getMotorVoltage(); - m_wristCurrentSignal = m_wristLeftMotor.getStatorCurrent(); - m_wristFollowerCurrentSignal = m_wristRightFollowerMotor.getStatorCurrent(); + m_wristTorqueCurrentSignal = m_wristLeftMotor.getTorqueCurrent(); + m_wristStatorCurrentSignal = m_wristLeftMotor.getStatorCurrent(); + m_wristFollowerStatorCurrentSignal = m_wristRightFollowerMotor.getStatorCurrent(); BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, + 100.0, m_elbowPositionSignal, + m_elbowPositionSetpointSignal, m_elbowVelocitySignal, m_elbowAppliedVoltsSignal, - m_elbowCurrentSignal, - m_elbowFollowerCurrentSignal, + m_elbowTorqueCurrentSignal, + m_elbowStatorCurrentSignal, + m_elbowFollowerStatorCurrentSignal, m_wristPositionSignal, + m_wristPositionSetpointSignal, m_wristVelocitySignal, m_wristAppliedVoltsSignal, - m_wristCurrentSignal, - m_wristFollowerCurrentSignal); + m_wristTorqueCurrentSignal, + m_wristStatorCurrentSignal, + m_wristFollowerStatorCurrentSignal); ParentDevice.optimizeBusUtilizationForAll( m_elbowLeftMotor, m_elbowRightFollowerMotor, m_wristLeftMotor, m_wristRightFollowerMotor); - m_elbowControl = new VoltageOut(0.0, true, false, false, false).withUpdateFreqHz(0.0); - m_wristControl = new VoltageOut(0.0, true, false, false, false).withUpdateFreqHz(0.0); + m_elbowControl = new PositionVoltage(0.0, 0.0, true, 0.0, 0, false, false, false); + m_wristControl = new MotionMagicVoltage(0.0, true, 0.0, 0, false, false, false); } + @Override public void updateInputs(ArmIOInputs inputs) { inputs.allMotorsConnected = BaseStatusSignal.refreshAll( m_elbowPositionSignal, + m_elbowPositionSetpointSignal, m_elbowVelocitySignal, m_elbowAppliedVoltsSignal, - m_elbowCurrentSignal, - m_elbowFollowerCurrentSignal, + m_elbowTorqueCurrentSignal, + m_elbowStatorCurrentSignal, + m_elbowFollowerStatorCurrentSignal, m_wristPositionSignal, + m_wristPositionSetpointSignal, m_wristVelocitySignal, m_wristAppliedVoltsSignal, - m_wristCurrentSignal, - m_wristFollowerCurrentSignal) - .equals(StatusCode.OK); - - inputs.elbowEncoderRaw = m_elbowPositionSignal.getValue(); - inputs.wristEncoderRaw = m_wristPositionSignal.getValue(); - inputs.elbowAbsolutePositionRad = - MathUtil.angleModulus( - Units.rotationsToRadians(m_elbowAbsoluteEncoder.get() / ArmModel.kElbowChainReduction) - * (kInvertElbowAbsoluteEncoder ? -1 : 1) - - elbowAbsoluteEncoderOffset.getRadians()); - inputs.elbowAbsoluteEncoderConnected = m_elbowAbsoluteEncoder.isConnected(); - inputs.elbowRelativePositionRad = - MathUtil.angleModulus( - Units.rotationsToRadians( - m_elbowPositionSignal.getValue() / ArmModel.kElbowFinalReduction) - - kElbowRelativeEncoderOffsetRad); - inputs.elbowVelocityRadPerSec = - Units.rotationsToRadians(m_elbowVelocitySignal.getValue() / ArmModel.kElbowFinalReduction); + m_wristTorqueCurrentSignal, + m_wristStatorCurrentSignal, + m_wristFollowerStatorCurrentSignal) + .isOK(); + + inputs.elbowPositionRot = m_elbowPositionSignal.getValue(); + inputs.elbowPositionSetpointRot = m_elbowPositionSetpointSignal.getValue(); + inputs.elbowVelocityRps = m_elbowVelocitySignal.getValue(); inputs.elbowAppliedVolts = m_elbowAppliedVoltsSignal.getValue(); - inputs.elbowCurrentAmps = - new double[] {m_elbowCurrentSignal.getValue(), m_elbowFollowerCurrentSignal.getValue()}; - - inputs.wristAbsolutePositionRad = - MathUtil.angleModulus( - Units.rotationsToRadians(m_wristAbsoluteEncoder.get() / ArmModel.kWristChainReduction) - * (kInvertWristAbsoluteEncoder ? -1 : 1) - - wristAbsoluteEncoderOffset.getRadians()); - - inputs.wristAbsoluteEncoderConnected = m_wristAbsoluteEncoder.isConnected(); - inputs.wristRelativePositionRad = - MathUtil.angleModulus( - Units.rotationsToRadians( - m_wristPositionSignal.getValue() / ArmModel.kWristFinalReduction) - + kWristRelativeEncoderOffsetRad); - inputs.wristVelocityRadPerSec = - Units.rotationsToRadians(m_wristVelocitySignal.getValue() / ArmModel.kWristFinalReduction); + inputs.elbowTorqueCurrentAmps = m_elbowTorqueCurrentSignal.getValue(); + inputs.elbowStatorCurrentAmps = + new double[] { + m_elbowStatorCurrentSignal.getValue(), m_elbowFollowerStatorCurrentSignal.getValue() + }; + + inputs.wristPositionRot = m_wristPositionSignal.getValue(); + inputs.wristPositionSetpointRot = m_wristPositionSetpointSignal.getValue(); + inputs.wristVelocityRps = m_wristVelocitySignal.getValue(); inputs.wristAppliedVolts = m_wristAppliedVoltsSignal.getValue(); - inputs.wristCurrentAmps = - new double[] {m_wristCurrentSignal.getValue(), m_wristFollowerCurrentSignal.getValue()}; + inputs.wristTorqueCurrentAmps = m_wristTorqueCurrentSignal.getValue(); + inputs.wristStatorCurrentAmps = + new double[] { + m_wristStatorCurrentSignal.getValue(), m_wristFollowerStatorCurrentSignal.getValue() + }; + } + + @Override + public void setElbowRotations(double angleRot) { + m_elbowLeftMotor.setControl(m_elbowControl.withPosition(angleRot)); } + @Override + public void setWristRotations(double angleRot) { + m_wristLeftMotor.setControl(m_wristControl.withPosition(angleRot)); + } + + @Override public void setElbowVoltage(double volts) { - m_elbowLeftMotor.setControl(m_elbowControl.withOutput(volts)); + m_elbowLeftMotor.setVoltage(volts); } + @Override public void setWristVoltage(double volts) { - m_wristLeftMotor.setControl(m_wristControl.withOutput(volts)); + m_wristLeftMotor.setVoltage(volts); + } + + @Override + public void stop() { + m_elbowLeftMotor.setVoltage(0.0); + m_wristLeftMotor.setVoltage(0.0); } + @Override public void setBrakeMode(boolean enableBrakeMode) { final NeutralModeValue neutralModeValue = enableBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast; diff --git a/src/main/java/frc/robot/arm/ArmModel.java b/src/main/java/frc/robot/arm/ArmModel.java deleted file mode 100644 index 6ed7c7a..0000000 --- a/src/main/java/frc/robot/arm/ArmModel.java +++ /dev/null @@ -1,103 +0,0 @@ -package frc.robot.arm; - -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.Vector; -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.numbers.N2; -import edu.wpi.first.math.numbers.N4; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; - -public class ArmModel { - - public static final Translation2d origin = - new Translation2d(Units.inchesToMeters(-9.2), Units.inchesToMeters(20)); - - public static final double kElbowGearboxReduction = 3.0 * 5.0 * 5.0; - public static final double kElbowChainReduction = 36.0 / 22.0; - public static final double kElbowFinalReduction = kElbowGearboxReduction * kElbowChainReduction; - public static final double kElbowLengthMeters = Units.inchesToMeters(19.0); - public static final double kElbowMinAngleRad = -Math.PI; - public static final double kElbowMaxAngleRad = Math.PI; - public static final DCMotor kElbowGearbox = DCMotor.getFalcon500Foc(2); - private final double kElbowMassKg = Units.lbsToKilograms(7.0); - private final double kElbowMoiKgMetersSq = - SingleJointedArmSim.estimateMOI(kElbowLengthMeters, kElbowMassKg); - private final double elbowkG = 0.15; - private final double elbowkV = 2.31; - - public static final double kWristGearboxReduction = 3.0 * 5.0; - public static final double kWristChainReduction = 36.0 / 22.0; - public static final double kWristFinalReduction = kWristGearboxReduction * kWristChainReduction; - public static final double kWristLengthMeters = Units.inchesToMeters(15.0); - public static final double kWristMinAngleRad = -Math.PI; - public static final double kWristMaxAngleRad = Math.PI; - public static final DCMotor kWristGearbox = DCMotor.getFalcon500Foc(2); - private final double kWristMassKg = Units.lbsToKilograms(11.5); - private final double kWristMoiKgMetersSq = - SingleJointedArmSim.estimateMOI(kWristLengthMeters, kWristMassKg); - private final double wristkG = 0.51; - private final double wristkV = 0.8; - - private final ArmFeedforward m_elbowFeedForward, m_wristFeedForward; - private final SingleJointedArmSim m_elbowSim, m_wristSim; - - public ArmModel() { - m_elbowFeedForward = new ArmFeedforward(0.0, elbowkG, elbowkV, 0.0); - m_wristFeedForward = new ArmFeedforward(0.0, wristkG, wristkV, 0.0); - m_elbowSim = - new SingleJointedArmSim( - kElbowGearbox, - kElbowFinalReduction, - kElbowMoiKgMetersSq, - kElbowLengthMeters, - kElbowMinAngleRad, - kElbowMaxAngleRad, - true, - 0.0); - m_wristSim = - new SingleJointedArmSim( - kWristGearbox, - kWristFinalReduction, - kWristMoiKgMetersSq, - kWristLengthMeters, - kWristMinAngleRad, - kWristMaxAngleRad, - true, - 0.0); - } - - public Vector feedforward(Vector position) { - return feedforward(position, VecBuilder.fill(0.0, 0.0)); - } - - public Vector feedforward(Vector position, Vector velocity) { - return VecBuilder.fill( - m_elbowFeedForward.calculate(position.get(0, 0), velocity.get(0, 0)), - m_wristFeedForward.calculate(position.get(0, 0), velocity.get(1, 0))); - } - - /** - * Adjusts the simulated state of the arm based on applied voltages. - * - * @param state The current state of the arm as (position_0, position_1, velocity_0, velocity_1) - * @param voltage The applied voltage of each joint. - * @param dt The step length in seconds. - * @return The new state of the arm as (position_0, position_1, velocity_0, velocity_1) - */ - public Vector simulate(Vector state, Vector voltage, double dt) { - m_elbowSim.setState(state.get(0, 0), state.get(2, 0)); - m_wristSim.setState(state.get(1, 0), state.get(3, 0)); - m_elbowSim.setInputVoltage(voltage.get(0, 0)); - m_wristSim.setInputVoltage(voltage.get(1, 0)); - m_elbowSim.update(dt); - m_wristSim.update(dt); - return VecBuilder.fill( - m_elbowSim.getAngleRads(), - m_wristSim.getAngleRads(), - m_elbowSim.getVelocityRadPerSec(), - m_wristSim.getVelocityRadPerSec()); - } -} diff --git a/src/main/java/frc/robot/arm/ArmSetpoints.java b/src/main/java/frc/robot/arm/ArmSetpoints.java deleted file mode 100644 index d766200..0000000 --- a/src/main/java/frc/robot/arm/ArmSetpoints.java +++ /dev/null @@ -1,37 +0,0 @@ -package frc.robot.arm; - -import edu.wpi.first.math.Pair; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; - -public enum ArmSetpoints { - kStowed(new Translation2d(-0.13, 0.48), 0.95, 0.0), - kAmp(new Translation2d(-0.412, 0.785), 0.0, 0.2); - - private final Translation2d m_setpoint; - private final double m_elbowDelay; - private final double m_wristDelay; - - private ArmSetpoints(Translation2d setpoint, double elbowDelay, double wristDelay) { - m_setpoint = setpoint; - m_elbowDelay = elbowDelay; - m_wristDelay = wristDelay; - } - - private ArmSetpoints( - double xPositionInches, double yPositionInches, double elbowDelay, double wristDelay) { - this( - new Translation2d( - Units.inchesToMeters(xPositionInches), Units.inchesToMeters(yPositionInches)), - elbowDelay, - wristDelay); - } - - public Translation2d getTranslation() { - return m_setpoint; - } - - public Pair getDelay() { - return Pair.of(m_elbowDelay, m_wristDelay); - } -} diff --git a/src/main/java/frc/robot/arm/ArmVisualizer.java b/src/main/java/frc/robot/arm/ArmVisualizer.java index 3ee6a64..b6a4209 100644 --- a/src/main/java/frc/robot/arm/ArmVisualizer.java +++ b/src/main/java/frc/robot/arm/ArmVisualizer.java @@ -3,6 +3,7 @@ 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.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; @@ -16,51 +17,48 @@ public class ArmVisualizer { // Value from CAD, angle from horizontal to shoulder. private final double kShoulderAngleDegrees = 103.985388; + private final Translation2d origin = + new Translation2d(Units.inchesToMeters(-9.2), Units.inchesToMeters(20)); private final String m_logKey; private final Mechanism2d m_mechanism; private final MechanismRoot2d m_mechanismRoot; - private final MechanismLigament2d m_fixedShoulderLigament; - private final MechanismLigament2d m_elbowLigament; - private final MechanismLigament2d m_wristLigament; + private final MechanismLigament2d m_fixedShoulderLigament, m_elbowLigament, m_wristLigament; + private Pose3d m_elbowPose, m_wristPose; - public ArmVisualizer(String logKey) { + public ArmVisualizer(String logKey, double ligamentWidth, Color color) { m_logKey = logKey; m_mechanism = new Mechanism2d(4, 3, new Color8Bit(Color.kGray)); - m_mechanismRoot = m_mechanism.getRoot("Arm", 2 + ArmModel.origin.getX(), 0); + m_mechanismRoot = m_mechanism.getRoot("Arm", 2 + origin.getX(), 0); m_fixedShoulderLigament = m_mechanismRoot.append( new MechanismLigament2d( - "Shoulder", - ArmModel.origin.getY(), - kShoulderAngleDegrees, - 6, - new Color8Bit(Color.kBlack))); + "Shoulder", origin.getY(), kShoulderAngleDegrees, 6, new Color8Bit(Color.kBlack))); m_elbowLigament = m_fixedShoulderLigament.append( new MechanismLigament2d( - "Elbow", ArmModel.kElbowLengthMeters, 0, 4, new Color8Bit(Color.kGreen))); + "Elbow", ArmConstants.kElbowLengthMeters, 0, ligamentWidth, new Color8Bit(color))); m_wristLigament = m_elbowLigament.append( new MechanismLigament2d( - "Wrist", ArmModel.kWristLengthMeters, 0, 4, new Color8Bit(Color.kDarkGreen))); + "Wrist", ArmConstants.kWristLengthMeters, 0, ligamentWidth, new Color8Bit(color))); } - public void update(double elbowAngle, double wristAngle) { - m_elbowLigament.setAngle(Units.radiansToDegrees(elbowAngle) - kShoulderAngleDegrees); - m_wristLigament.setAngle(Units.radiansToDegrees(wristAngle - elbowAngle)); - Logger.recordOutput("Mechanism2d/" + m_logKey, m_mechanism); + public void update(double elbowAngleRad, double wristAngleRad) { + m_elbowLigament.setAngle(Units.radiansToDegrees(elbowAngleRad) - kShoulderAngleDegrees); + m_wristLigament.setAngle(Units.radiansToDegrees(wristAngleRad - elbowAngleRad)); + Logger.recordOutput("Arm/Mechanism2d/" + m_logKey, m_mechanism); - Pose3d elbowPose = - new Pose3d( - ArmModel.origin.getX(), - 0.0, - ArmModel.origin.getY(), - new Rotation3d(0.0, -elbowAngle, 0.0)); - Pose3d wristPose = - elbowPose.transformBy( + m_elbowPose = + new Pose3d(origin.getX(), 0.0, origin.getY(), new Rotation3d(0.0, -elbowAngleRad, 0.0)); + m_wristPose = + m_elbowPose.transformBy( new Transform3d( - new Translation3d(ArmModel.kElbowLengthMeters, 0.0, 0.0), - new Rotation3d(0.0, -(wristAngle - elbowAngle), 0.0))); - Logger.recordOutput("Mechanism3d/" + m_logKey, elbowPose, wristPose); + new Translation3d(ArmConstants.kElbowLengthMeters, 0.0, 0.0), + new Rotation3d(0.0, -(wristAngleRad - elbowAngleRad), 0.0))); + Logger.recordOutput("Arm/Mechanism3d/" + m_logKey, m_elbowPose, m_wristPose); + } + + public Pose3d getWristPose() { + return m_wristPose; } } diff --git a/src/main/java/frc/robot/climber/Climber.java b/src/main/java/frc/robot/climber/Climber.java index 941a399..9ee7a21 100644 --- a/src/main/java/frc/robot/climber/Climber.java +++ b/src/main/java/frc/robot/climber/Climber.java @@ -1,15 +1,28 @@ package frc.robot.climber; +import edu.wpi.first.math.controller.PIDController; 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.button.Trigger; import org.littletonrobotics.junction.Logger; public class Climber extends SubsystemBase { + public static final double kWinchReduction = 20.0; private final ClimberIO m_io; private final ClimberIOInputsAutoLogged m_inputs = new ClimberIOInputsAutoLogged(); + private final double kTrapSetpointRot = 10.5; + private final PIDController m_leftController, m_rightController; + public final Trigger atTrapSetpoint; public Climber(ClimberIO climberIO) { m_io = climberIO; + m_leftController = new PIDController(120.0, 0.0, 0.0); + m_rightController = new PIDController(120.0, 0.0, 0.0); + m_leftController.setTolerance(0.05); + m_rightController.setTolerance(0.05); + atTrapSetpoint = + new Trigger(() -> m_leftController.atSetpoint() && m_rightController.atSetpoint()); setDefaultCommand(idle()); } @@ -22,24 +35,55 @@ public void periodic() { public Command idle() { return this.run( () -> { - m_io.setLeftDutyCycle(0.0); - m_io.setRightDutyCycle(0.0); + m_io.setLeftVoltage(0.0); + m_io.setRightVoltage(0.0); }); } public Command windWinch() { return this.run( () -> { - m_io.setLeftDutyCycle(1.0); - m_io.setRightDutyCycle(1.0); + m_io.setLeftVoltage(12.0); + m_io.setRightVoltage(12.0); }); } public Command unwindWinch() { return this.run( () -> { - m_io.setLeftDutyCycle(-0.25); - m_io.setRightDutyCycle(-0.25); + m_io.setLeftVoltage(-4.0); + m_io.setRightVoltage(-4.0); }); } + + public Command goToTrapLimit() { + return Commands.parallel( + this.run( + () -> { + if (m_inputs.leftWinchPositionRot <= kTrapSetpointRot) m_io.setLeftVoltage(12.0); + else m_io.setLeftVoltage(0.0); + if (m_inputs.leftWinchPositionRot <= kTrapSetpointRot) m_io.setRightVoltage(12.0); + else m_io.setRightVoltage(0.0); + })) + .until( + () -> + m_inputs.leftWinchPositionRot > kTrapSetpointRot + && m_inputs.leftWinchPositionRot > kTrapSetpointRot); + } + + public Command setTrapSetpoint() { + return this.run( + () -> { + m_io.setLeftVoltage( + m_leftController.calculate(m_inputs.leftWinchPositionRot, kTrapSetpointRot)); + m_io.setRightVoltage( + m_rightController.calculate(m_inputs.rightWinchPositionRot, kTrapSetpointRot)); + }) + .finallyDo( + () -> { + m_leftController.reset(); + m_rightController.reset(); + }) + .withName("Trap Setpoint (Climber)"); + } } diff --git a/src/main/java/frc/robot/climber/ClimberIO.java b/src/main/java/frc/robot/climber/ClimberIO.java index 5d7410a..90f014f 100644 --- a/src/main/java/frc/robot/climber/ClimberIO.java +++ b/src/main/java/frc/robot/climber/ClimberIO.java @@ -5,20 +5,22 @@ public interface ClimberIO { @AutoLog public class ClimberIOInputs { - public double leftWinchPositionRad = 0.0; - public double leftWinchVelocityRadPerSec = 0.0; + public double leftWinchPositionRot = 0.0; + public double leftWinchVelocityRpm = 0.0; public double leftWinchAppliedVolts = 0.0; public double leftWinchCurrentAmps = 0.0; - public double rightWinchPositionRad = 0.0; - public double rightWinchVelocityRadPerSec = 0.0; + public double rightWinchPositionRot = 0.0; + public double rightWinchVelocityRpm = 0.0; public double rightWinchAppliedVolts = 0.0; public double rightWinchCurrentAmps = 0.0; + + public boolean allMotorsConnected = false; } public default void updateInputs(ClimberIOInputs inputs) {} - public default void setLeftDutyCycle(double outputDutyCycle) {} + public default void setLeftVoltage(double volts) {} - public default void setRightDutyCycle(double outputDutyCycle) {} + public default void setRightVoltage(double volts) {} } diff --git a/src/main/java/frc/robot/climber/ClimberIOSparkMAX.java b/src/main/java/frc/robot/climber/ClimberIOSparkMAX.java index 4c019de..582c18f 100644 --- a/src/main/java/frc/robot/climber/ClimberIOSparkMAX.java +++ b/src/main/java/frc/robot/climber/ClimberIOSparkMAX.java @@ -3,11 +3,9 @@ import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; -import edu.wpi.first.math.util.Units; public class ClimberIOSparkMAX implements ClimberIO { - private final double kWinchReduction = 1.0; private final CANSparkMax m_leftWinchMotor, m_rightWinchMotor; private final RelativeEncoder m_leftWinchEncoder, m_rightWinchEncoder; @@ -22,38 +20,40 @@ public ClimberIOSparkMAX() { m_leftWinchEncoder = m_leftWinchMotor.getEncoder(); m_rightWinchEncoder = m_rightWinchMotor.getEncoder(); + m_leftWinchEncoder.setPositionConversionFactor(1.0 / Climber.kWinchReduction); + m_leftWinchEncoder.setVelocityConversionFactor(1.0 / Climber.kWinchReduction); + m_rightWinchEncoder.setPositionConversionFactor(1.0 / Climber.kWinchReduction); + m_rightWinchEncoder.setVelocityConversionFactor(1.0 / Climber.kWinchReduction); + + m_leftWinchEncoder.setPosition(0.0); + m_rightWinchEncoder.setPosition(0.0); + m_leftWinchMotor.setIdleMode(IdleMode.kBrake); m_rightWinchMotor.setIdleMode(IdleMode.kBrake); } @Override public void updateInputs(ClimberIOInputs inputs) { - inputs.leftWinchPositionRad = - Units.rotationsToRadians(m_leftWinchEncoder.getPosition() / kWinchReduction); - inputs.leftWinchVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond( - m_leftWinchEncoder.getVelocity() / kWinchReduction); + inputs.leftWinchPositionRot = m_leftWinchEncoder.getPosition() / Climber.kWinchReduction; + inputs.leftWinchVelocityRpm = m_leftWinchEncoder.getVelocity() / Climber.kWinchReduction; inputs.leftWinchAppliedVolts = m_leftWinchMotor.getAppliedOutput() * m_leftWinchMotor.getBusVoltage(); inputs.leftWinchCurrentAmps = m_leftWinchMotor.getOutputCurrent(); - inputs.rightWinchPositionRad = - Units.rotationsToRadians(m_rightWinchEncoder.getPosition() / kWinchReduction); - inputs.rightWinchVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond( - m_rightWinchEncoder.getVelocity() / kWinchReduction); + inputs.rightWinchPositionRot = m_rightWinchEncoder.getPosition() / Climber.kWinchReduction; + inputs.rightWinchVelocityRpm = m_rightWinchEncoder.getVelocity() / Climber.kWinchReduction; inputs.rightWinchAppliedVolts = m_rightWinchMotor.getAppliedOutput() * m_rightWinchMotor.getBusVoltage(); inputs.rightWinchCurrentAmps = m_rightWinchMotor.getOutputCurrent(); } @Override - public void setLeftDutyCycle(double outputDutyCycle) { + public void setLeftVoltage(double outputDutyCycle) { m_leftWinchMotor.set(outputDutyCycle); } @Override - public void setRightDutyCycle(double outputDutyCycle) { + public void setRightVoltage(double outputDutyCycle) { m_rightWinchMotor.set(outputDutyCycle); } } diff --git a/src/main/java/frc/robot/climber/ClimberIOTalonFX.java b/src/main/java/frc/robot/climber/ClimberIOTalonFX.java new file mode 100644 index 0000000..d00f041 --- /dev/null +++ b/src/main/java/frc/robot/climber/ClimberIOTalonFX.java @@ -0,0 +1,107 @@ +package frc.robot.climber; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import frc.robot.Constants; + +public class ClimberIOTalonFX implements ClimberIO { + + private final TalonFX m_leftWinchMotor, m_rightWinchMotor; + private final StatusSignal m_leftWinchPositionSignal, + m_leftWinchVelocitySignal, + m_leftWinchAppliedVoltageSignal, + m_leftWinchCurrentSignal, + m_rightWinchPositionSignal, + m_rightWinchVelocitySignal, + m_rightWinchAppliedVoltageSignal, + m_rightWinchCurrentSignal; + + public ClimberIOTalonFX() { + + m_leftWinchMotor = new TalonFX(25, Constants.kDrivetrainCanBus); + m_rightWinchMotor = new TalonFX(26, Constants.kDrivetrainCanBus); + + final FeedbackConfigs feedbackConfigs = + new FeedbackConfigs().withSensorToMechanismRatio(Climber.kWinchReduction); + final MotorOutputConfigs motorOutputConfigs = + new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake); + + m_leftWinchMotor + .getConfigurator() + .apply( + new TalonFXConfiguration() + .withFeedback(feedbackConfigs) + .withMotorOutput(motorOutputConfigs)); + m_rightWinchMotor + .getConfigurator() + .apply( + new TalonFXConfiguration() + .withFeedback(feedbackConfigs) + .withMotorOutput(motorOutputConfigs)); + + m_leftWinchPositionSignal = m_leftWinchMotor.getPosition(); + m_leftWinchVelocitySignal = m_leftWinchMotor.getVelocity(); + m_leftWinchAppliedVoltageSignal = m_leftWinchMotor.getMotorVoltage(); + m_leftWinchCurrentSignal = m_leftWinchMotor.getSupplyCurrent(); + + m_rightWinchPositionSignal = m_rightWinchMotor.getPosition(); + m_rightWinchVelocitySignal = m_rightWinchMotor.getVelocity(); + m_rightWinchAppliedVoltageSignal = m_rightWinchMotor.getMotorVoltage(); + m_rightWinchCurrentSignal = m_rightWinchMotor.getSupplyCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + m_leftWinchPositionSignal, + m_leftWinchVelocitySignal, + m_leftWinchAppliedVoltageSignal, + m_leftWinchCurrentSignal, + m_rightWinchPositionSignal, + m_rightWinchVelocitySignal, + m_rightWinchAppliedVoltageSignal, + m_rightWinchCurrentSignal); + + ParentDevice.optimizeBusUtilizationForAll(m_leftWinchMotor, m_rightWinchMotor); + } + + @Override + public void updateInputs(ClimberIOInputs inputs) { + inputs.allMotorsConnected = + BaseStatusSignal.refreshAll( + m_leftWinchPositionSignal, + m_leftWinchVelocitySignal, + m_leftWinchAppliedVoltageSignal, + m_leftWinchCurrentSignal, + m_rightWinchPositionSignal, + m_rightWinchVelocitySignal, + m_rightWinchAppliedVoltageSignal, + m_rightWinchCurrentSignal) + .isOK(); + + inputs.leftWinchPositionRot = m_leftWinchPositionSignal.getValue(); + inputs.leftWinchVelocityRpm = m_leftWinchVelocitySignal.getValue(); + inputs.leftWinchAppliedVolts = m_leftWinchAppliedVoltageSignal.getValue(); + inputs.leftWinchCurrentAmps = m_leftWinchCurrentSignal.getValue(); + + inputs.rightWinchPositionRot = m_rightWinchPositionSignal.getValue(); + inputs.rightWinchVelocityRpm = m_rightWinchVelocitySignal.getValue(); + inputs.rightWinchAppliedVolts = m_rightWinchAppliedVoltageSignal.getValue(); + inputs.rightWinchCurrentAmps = m_rightWinchCurrentSignal.getValue(); + } + + @Override + public void setLeftVoltage(double volts) { + m_leftWinchMotor.setControl(new VoltageOut(volts).withUpdateFreqHz(0.0)); + } + + @Override + public void setRightVoltage(double volts) { + m_rightWinchMotor.setVoltage(volts); + } +} diff --git a/src/main/java/frc/robot/drivetrain/DriveToNote.java b/src/main/java/frc/robot/drivetrain/DriveToNote.java new file mode 100644 index 0000000..b8ba654 --- /dev/null +++ b/src/main/java/frc/robot/drivetrain/DriveToNote.java @@ -0,0 +1,50 @@ +package frc.robot.drivetrain; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.intake.Intake; +import frc.robot.lights.Lights; +import frc.robot.vision.GamePieceDetection; + +public class DriveToNote extends Command { + private Drivetrain m_drivetrain; + private Lights m_lights; + private GamePieceDetection m_gamePieceDetection; + + private PIDController m_thetaController; + + private double m_horizontalOffset; + + public DriveToNote( + Drivetrain drivetrain, Lights lights, GamePieceDetection gamePieceDetection, Intake intake) { + m_drivetrain = drivetrain; + m_lights = lights; + m_gamePieceDetection = gamePieceDetection; + + m_thetaController = new PIDController(5.0, 0.0, 0.0); + + m_horizontalOffset = m_gamePieceDetection.getHorizontalOffsetDeg(); + + m_thetaController.setTolerance(1); + } + + @Override + public void initialize() {} + + @Override + public void execute() { + m_horizontalOffset = m_gamePieceDetection.getHorizontalOffsetDeg(); + m_drivetrain.runVelocity( + new ChassisSpeeds(0.0, 0.0, m_thetaController.calculate(m_horizontalOffset, 0))); + // m_lights.setBlink(Color.kMidnightBlue); + } + + @Override + public void end(boolean interrupted) {} + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/drivetrain/DriveToPose.java b/src/main/java/frc/robot/drivetrain/DriveToPose.java index 37cbb83..c4e0e38 100644 --- a/src/main/java/frc/robot/drivetrain/DriveToPose.java +++ b/src/main/java/frc/robot/drivetrain/DriveToPose.java @@ -11,19 +11,23 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; import frc.robot.PoseEstimation; +import frc.robot.lights.Lights; import org.littletonrobotics.junction.Logger; public class DriveToPose extends Command { private final Drivetrain m_drivetrain; + private final Lights m_lights; private final ProfiledPIDController m_translationController, m_thetaController; private Translation2d m_lastSetpointTranslation; private Pose2d m_pose; private Pose2d m_goalPose; - public DriveToPose(Pose2d pose, Drivetrain drivetrain) { + public DriveToPose(Pose2d pose, Drivetrain drivetrain, Lights lights) { m_pose = pose; m_translationController = new ProfiledPIDController(2.0, 0.0, 0.0, new TrapezoidProfile.Constraints(3.5, 2.2)); @@ -31,13 +35,15 @@ public DriveToPose(Pose2d pose, Drivetrain drivetrain) { new ProfiledPIDController( 5.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2 * Math.PI, 4 * Math.PI)); this.m_drivetrain = drivetrain; - addRequirements(drivetrain); + this.m_lights = lights; + addRequirements(drivetrain, lights); } @Override public void initialize() { + m_lights.setBlink(Color.kWhite); m_goalPose = - m_drivetrain.onRedAllianceSupplier.getAsBoolean() + Constants.onRedAllianceSupplier.getAsBoolean() ? GeometryUtil.flipFieldPose(m_pose) : m_pose; Pose2d initialPose = PoseEstimation.getInstance().getPose(); @@ -66,15 +72,17 @@ public void initialize() { @Override public void execute() { Pose2d currentPose = PoseEstimation.getInstance().getPose(); - double currentDistance = currentPose.getTranslation().getDistance(m_goalPose.getTranslation()); - double ffScaler = MathUtil.clamp((currentDistance - 0.2) / (0.8 - 0.2), 0.0, 1.0); + double distanceToGoalPose = + currentPose.getTranslation().getDistance(m_goalPose.getTranslation()); + double ffScaler = MathUtil.clamp((distanceToGoalPose - 0.2) / (0.8 - 0.2), 0.0, 1.0); m_translationController.reset( m_lastSetpointTranslation.getDistance(m_goalPose.getTranslation()), m_translationController.getSetpoint().velocity); double driveVelocityScalar = m_translationController.getSetpoint().velocity * ffScaler - + m_translationController.calculate(currentDistance, 0.0); - if (currentDistance < m_translationController.getPositionTolerance()) driveVelocityScalar = 0.0; + + m_translationController.calculate(distanceToGoalPose, 0.0); + if (distanceToGoalPose < m_translationController.getPositionTolerance()) + driveVelocityScalar = 0.0; m_lastSetpointTranslation = new Pose2d( m_goalPose.getTranslation(), @@ -89,9 +97,9 @@ public void execute() { m_thetaController.getSetpoint().velocity * ffScaler + m_thetaController.calculate( currentPose.getRotation().getRadians(), m_goalPose.getRotation().getRadians()); - double thetaErrorAbs = + double thetaErrorAbsolute = Math.abs(currentPose.getRotation().minus(m_goalPose.getRotation()).getRadians()); - if (thetaErrorAbs < m_thetaController.getPositionTolerance()) thetaVelocity = 0.0; + if (thetaErrorAbsolute < m_thetaController.getPositionTolerance()) thetaVelocity = 0.0; Translation2d driveVelocity = new Pose2d( @@ -104,15 +112,11 @@ public void execute() { ChassisSpeeds.fromFieldRelativeSpeeds( driveVelocity.getX(), driveVelocity.getY(), thetaVelocity, currentPose.getRotation())); - Logger.recordOutput("DriveToPose/DistanceMeasured", currentDistance); + Logger.recordOutput("Drivetrain/DriveToPose/DistanceToGoalPose", distanceToGoalPose); Logger.recordOutput( - "DriveToPose/DistanceSetpoint", m_translationController.getSetpoint().position); - Logger.recordOutput("DriveToPose/ThetaMeasured", currentPose.getRotation().getRadians()); - Logger.recordOutput("DriveToPose/ThetaSetpoint", m_thetaController.getSetpoint().position); - Logger.recordOutput( - "Odometry/DriveToPoseSetpoint", + "Drivetrain/DriveToPose/Setpoint", new Pose2d( m_lastSetpointTranslation, new Rotation2d(m_thetaController.getSetpoint().position))); - Logger.recordOutput("Odometry/DriveToPoseGoal", m_goalPose); + Logger.recordOutput("Drivetrain/DriveToPose/Goal", m_goalPose); } } diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java index b5d4b81..ef6e519 100644 --- a/src/main/java/frc/robot/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/drivetrain/Drivetrain.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; +import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.util.GeometryUtil; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; @@ -20,7 +21,6 @@ import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; 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 edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -33,7 +33,6 @@ import java.util.Arrays; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; -import java.util.function.BooleanSupplier; import java.util.function.Consumer; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -44,10 +43,11 @@ public class Drivetrain extends SubsystemBase { public static final Lock odometryLock = new ReentrantLock(); - public final BooleanSupplier onRedAllianceSupplier; private static final double kTrackwidthMeters = Units.inchesToMeters(20.75); private static double kWheelbaseMeters = Units.inchesToMeters(15.75); + + // Front left, front right, back left, back right private static final Translation2d[] m_modulePositions = new Translation2d[] { new Translation2d(kTrackwidthMeters / 2.0, kWheelbaseMeters / 2.0), @@ -57,12 +57,11 @@ public class Drivetrain extends SubsystemBase { }; public static final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics(m_modulePositions); + public static final double kDriveBaseRadius = Math.hypot(kTrackwidthMeters / 2.0, kWheelbaseMeters / 2.0); - private static final LoggedTunableNumber inRangeRadius = - new LoggedTunableNumber("Drivetrain/InRangeRadius", 2.10); - private static final LoggedTunableNumber inRangeTolerance = - new LoggedTunableNumber("Drivetrain/InRangeTolerance", 0.25); + private static final LoggedTunableNumber speakerRangeMeters = + new LoggedTunableNumber("Drivetrain/InRangeRadius", 7.0); private final double kMaxLinearSpeedMetersPerSecond = Units.feetToMeters(16); private final double kMaxAngularSpeedRadPerSec = 2 * Math.PI; private final double kDeadband = 0.1; @@ -96,19 +95,16 @@ public Drivetrain( m_modules[1] = new Module(frontRightModuleIO, 1); m_modules[2] = new Module(backLeftModuleIO, 2); m_modules[3] = new Module(backRightModuleIO, 3); + PhoenixOdometryThread.getInstance().start(); - onRedAllianceSupplier = - () -> - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == Alliance.Red; m_sysIdRoutineDrive = new SysIdRoutine( new SysIdRoutine.Config( null, null, null, - (state) -> Logger.recordOutput("Drivetrain/SysIdState", state.toString())), + (state) -> SignalLogger.writeString("Drivetrain/SysIdState", state.toString())), new SysIdRoutine.Mechanism( (voltage) -> { for (Module module : m_modules) { @@ -153,11 +149,9 @@ public void periodic() { } if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); - Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); - for (Module module : m_modules) { - module.stop(); - } + Logger.recordOutput("Drivetrain/SwerveStates/Setpoints", new SwerveModuleState[] {}); + Logger.recordOutput("Drivetrain/SwerveStates/SetpointsOptimized", new SwerveModuleState[] {}); + for (Module module : m_modules) module.stop(); } final double[] sampleTimestamps = m_modules[0].getOdometryTimestamps(); @@ -179,6 +173,8 @@ public void periodic() { if (m_imuInputs.connected) { m_gyroRotation = m_imuInputs.odometryYawPositions[updateIndex]; } else { + // When the gyro is not connected, we can simulate the robot's heading using the module + // states through the rotational component of the derived chassis speeds final Twist2d twist = swerveKinematics.toTwist2d(moduleDeltas); m_gyroRotation = m_gyroRotation.plus(new Rotation2d(twist.dtheta)); } @@ -211,8 +207,16 @@ public void runVelocity(ChassisSpeeds speeds) { m_modules[moduleIndex].setSetpoint(setpointStates[moduleIndex]); } - Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); + Logger.recordOutput("Drivetrain/SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("Drivetrain/SwerveStates/SetpointsOptimized", optimizedSetpointStates); + } + + public void runWheelRadiusCharacterization(double characterizationInput) { + runVelocity(new ChassisSpeeds(0.0, 0.0, characterizationInput)); + } + + public double[] getWheelRadiusCharacterizationPosition() { + return Arrays.stream(m_modules).mapToDouble(Module::getDrivePositionRad).toArray(); } public Command joystickDrive( @@ -240,13 +244,21 @@ public Command joystickDrive( .withName("Joystick Drive"); } + /** + * This allows us to run auto-aim in auto when no other driving commands are active (no joystick + * drive, no path following) + */ + public Command blankDrive() { + return this.run(() -> runVelocity(new ChassisSpeeds(0.0, 0.0, 0.0))); + } + public Command zeroGyro() { return Commands.runOnce( () -> resetPose.accept( new Pose2d( PoseEstimation.getInstance().getPose().getTranslation(), - onRedAllianceSupplier.getAsBoolean() + Constants.onRedAllianceSupplier.getAsBoolean() ? GeometryUtil.flipFieldRotation(new Rotation2d()) : new Rotation2d()))) .ignoringDisable(true); @@ -293,30 +305,26 @@ private void configurePathing() { () -> swerveKinematics.toChassisSpeeds(getModuleStates()), this::runVelocity, new HolonomicPathFollowerConfig( - new PIDConstants(5.0, 0.0, 0.0), - new PIDConstants(5.0, 0.0, 0.0), + new PIDConstants(3.0, 0.0, 0.0), + new PIDConstants(3.0, 0.0, 0.0), kMaxLinearSpeedMetersPerSecond, kDriveBaseRadius, new ReplanningConfig()), - onRedAllianceSupplier, + Constants.onRedAllianceSupplier, this); PathPlannerLogging.setLogActivePathCallback( - (activePath) -> { - Logger.recordOutput( - "Drivetrain/Trajectory", activePath.toArray(new Pose2d[activePath.size()])); - }); + activePath -> + Logger.recordOutput("Drivetrain/Trajectory", activePath.toArray(Pose2d[]::new))); PathPlannerLogging.setLogTargetPoseCallback( - (targetPose) -> { - Logger.recordOutput("Drivetrain/TrajectorySetpoint", targetPose); - }); + targetPose -> Logger.recordOutput("Drivetrain/TrajectorySetpoint", targetPose)); } - @AutoLogOutput(key = "SwerveStates/StatesMeasured") + @AutoLogOutput(key = "Drivetrain/SwerveStates/StatesMeasured") private SwerveModuleState[] getModuleStates() { return Arrays.stream(m_modules).map(Module::getState).toArray(SwerveModuleState[]::new); } - @AutoLogOutput(key = "SwerveStates/PositionsMeasured") + @AutoLogOutput(key = "Drivetrain/SwerveStates/PositionsMeasured") private SwerveModulePosition[] getModulePositions() { return Arrays.stream(m_modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new); } @@ -331,19 +339,17 @@ public void setHeadingGoal(Supplier headingGoal) { public void clearHeadingGoal() { m_headingController = null; - Logger.recordOutput("HeadingController/Setpoint", new Pose2d()); + Logger.recordOutput("Drivetrain/HeadingController/Setpoint", new Pose2d()); } - @AutoLogOutput(key = "Drivetrain/AtHeadingGoal") + @AutoLogOutput(key = "Drivetrain/HeadingController/AtGoal") public boolean atHeadingGoal() { return m_headingController != null && m_headingController.atGoal(); } @AutoLogOutput(key = "Drivetrain/InRangeOfGoal") public boolean inRange() { - return MathUtil.isNear( - inRangeRadius.get(), - PoseEstimation.getInstance().getAimingParameters().effectiveDistance(), - inRangeTolerance.get()); + return PoseEstimation.getInstance().getAimingParameters().effectiveDistance() + <= speakerRangeMeters.get(); } } diff --git a/src/main/java/frc/robot/drivetrain/HeadingController.java b/src/main/java/frc/robot/drivetrain/HeadingController.java index e3bdcb0..cd21787 100644 --- a/src/main/java/frc/robot/drivetrain/HeadingController.java +++ b/src/main/java/frc/robot/drivetrain/HeadingController.java @@ -6,72 +6,54 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import frc.lib.LoggedTunableNumber; import frc.robot.PoseEstimation; import java.util.function.Supplier; -import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class HeadingController { - private static final LoggedTunableNumber kP = - new LoggedTunableNumber("HeadingController/kP", 5.0); - private static final LoggedTunableNumber kD = - new LoggedTunableNumber("HeadingController/kD", 0.0); - private static final LoggedTunableNumber maxVelocityMultipler = - new LoggedTunableNumber("HeadingController/MaxVelocityMultipler", 0.8); - private static final LoggedTunableNumber maxAccelerationMultipler = - new LoggedTunableNumber("HeadingController/MaxAccelerationMultipler", 0.8); - private static final LoggedTunableNumber toleranceDegrees = - new LoggedTunableNumber("HeadingController/ToleranceDegrees", 1.0); + private static final double kToleranceDegrees = 1.0; + private static final double kP = 7.0; + private static final double kMaxVelocityMultipler = 0.8; + private static final double kMaxAccelerationMultipler = 0.8; - private final ProfiledPIDController m_controller; + private final ProfiledPIDController m_controller = + new ProfiledPIDController(kP, 0.0, 0.0, new TrapezoidProfile.Constraints(0.0, 0.0), 0.02); private final Supplier m_goalHeadingSupplier; public HeadingController(Supplier goalHeadingSupplier) { - m_controller = - new ProfiledPIDController( - kP.get(), 0, kD.get(), new TrapezoidProfile.Constraints(0.0, 0.0), 0.02); + final double maxAngularAcceleration = + 9.0 / Drivetrain.kDriveBaseRadius * kMaxAccelerationMultipler; + final double maxAngularVelocity = 3.5 / Drivetrain.kDriveBaseRadius * kMaxVelocityMultipler; + m_controller.setConstraints( + new TrapezoidProfile.Constraints(maxAngularVelocity, maxAngularAcceleration)); m_controller.enableContinuousInput(-Math.PI, Math.PI); - m_controller.setTolerance(Units.degreesToRadians(toleranceDegrees.get())); + m_controller.setTolerance(Units.degreesToRadians(kToleranceDegrees)); m_goalHeadingSupplier = goalHeadingSupplier; - m_controller.reset( PoseEstimation.getInstance().getPose().getRotation().getRadians(), PoseEstimation.getInstance().getFieldVelocity().dtheta); } public double update() { - m_controller.setPID(kP.get(), 0, kD.get()); - m_controller.setTolerance(Units.degreesToRadians(toleranceDegrees.get())); - - final double maxAngularAcceleration = - 9.0 / Drivetrain.kDriveBaseRadius * maxAccelerationMultipler.get(); - final double maxAngularVelocity = - 3.5 / Drivetrain.kDriveBaseRadius * maxVelocityMultipler.get(); - m_controller.setConstraints( - new TrapezoidProfile.Constraints(maxAngularVelocity, maxAngularAcceleration)); - final double output = m_controller.calculate( PoseEstimation.getInstance().getPose().getRotation().getRadians(), m_goalHeadingSupplier.get().getRadians()); - Logger.recordOutput( - "HeadingController/Setpoint", + "Drivetrain/HeadingController/Setpoint", new Pose2d( PoseEstimation.getInstance().getPose().getTranslation(), m_goalHeadingSupplier.get())); Logger.recordOutput( - "HeadingController/HeadingErrorDegrees", + "Drivetrain/HeadingController/HeadingErrorDegrees", Units.radiansToDegrees(m_controller.getPositionError())); return output; } - @AutoLogOutput(key = "HeadingController/AtGoal") public boolean atGoal() { return MathUtil.isNear( m_controller.getGoal().position, m_controller.getSetpoint().position, - Units.degreesToRadians(toleranceDegrees.get())); + Units.degreesToRadians(kToleranceDegrees)); } } diff --git a/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java b/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java index 0d703ca..55edb8d 100644 --- a/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java +++ b/src/main/java/frc/robot/drivetrain/ImuIOPigeon2.java @@ -4,9 +4,11 @@ import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.Pigeon2Configuration; +import com.ctre.phoenix6.configs.Pigeon2FeaturesConfigs; import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Constants; import java.util.Queue; @@ -16,14 +18,18 @@ public class ImuIOPigeon2 implements ImuIO { private final StatusSignal m_yawVelocitySignal = m_imu.getAngularVelocityZWorld(); private final Queue m_yawPositionQueue, m_yawTimestampQueue; + private boolean lastEnabled = false; + public ImuIOPigeon2() { - final Pigeon2Configuration imuConfig = new Pigeon2Configuration(); - m_imu.getConfigurator().apply(imuConfig); - m_imu.setYaw(0.0); - m_yawSignal.setUpdateFrequency(PhoenixOdometryThread.kOdometryFrequencyHz); - m_yawVelocitySignal.setUpdateFrequency(100.0); + final Pigeon2Configuration imuConfiguration = new Pigeon2Configuration(); + m_imu.getConfigurator().apply(imuConfiguration); + m_imu.getConfigurator().setYaw(0.0); + BaseStatusSignal.setUpdateFrequencyForAll( + PhoenixOdometryThread.kOdometryFrequencyHz, m_yawSignal, m_yawVelocitySignal); m_imu.optimizeBusUtilization(); - m_yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(m_imu, m_imu.getYaw()); + m_yawPositionQueue = + PhoenixOdometryThread.getInstance() + .registerSignals(m_imu, m_yawSignal.clone(), m_yawVelocitySignal.clone()); m_yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); } @@ -36,11 +42,25 @@ public void updateInputs(ImuIOInputs inputs) { inputs.odometryYawPositions = m_yawPositionQueue.stream() - .map((Double value) -> Rotation2d.fromDegrees(value)) + .map(value -> Rotation2d.fromDegrees(value)) .toArray(Rotation2d[]::new); inputs.odometryYawTimestamps = - m_yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + m_yawTimestampQueue.stream().mapToDouble(Double::valueOf).toArray(); m_yawPositionQueue.clear(); m_yawTimestampQueue.clear(); + + boolean enabled = DriverStation.isEnabled(); + if (enabled && !lastEnabled) { + m_imu + .getConfigurator() + .apply(new Pigeon2FeaturesConfigs().withDisableNoMotionCalibration(true)); + } + if (!enabled && lastEnabled) { + m_imu + .getConfigurator() + .apply(new Pigeon2FeaturesConfigs().withDisableNoMotionCalibration(false)); + } + + lastEnabled = enabled; } } diff --git a/src/main/java/frc/robot/drivetrain/Module.java b/src/main/java/frc/robot/drivetrain/Module.java index 461077b..53cd7b7 100644 --- a/src/main/java/frc/robot/drivetrain/Module.java +++ b/src/main/java/frc/robot/drivetrain/Module.java @@ -4,17 +4,19 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import frc.robot.Constants; import org.littletonrobotics.junction.Logger; public class Module { // https://www.swervedrivespecialties.com/products/mk4i-swerve-module - // However, our first stage is 50:13 instead of 50:14 because we have a different gear. - public static final double kDriveRatio = (50.0 / 13.0) * (16.0 / 28.0) * (45.0 / 15.0); + // For Viper our first stage is 50:13 instead of 50:14 because we have a different pinion. + private static final double kDriveRatioFirstStage = + Constants.kIsViper ? (50.0 / 13.0) : (50.0 / 14.0); + public static final double kDriveRatio = kDriveRatioFirstStage * (16.0 / 28.0) * (45.0 / 15.0); public static final double kSteerRatio = 150.0 / 7.0; - private final double kWheelDiameterMeters = Units.inchesToMeters(3.87); - private final double kWheelRadiusMeters = kWheelDiameterMeters / 2.0; - private final double kCouplingGearRatio = 50.0 / 13.0; + private final double kWheelRadiusMeters = Units.inchesToMeters(1.73); + private final double kCouplingRatio = kDriveRatioFirstStage; private final ModuleIO m_io; private final ModuleIOInputsAutoLogged m_inputs = new ModuleIOInputsAutoLogged(); @@ -39,9 +41,9 @@ public void periodic() { for (int i = 0; i < sampleCount; i++) { double driveRotations = Units.radiansToRotations(m_inputs.odometryDrivePositionsRad[i]); Rotation2d steerAngle = m_inputs.odometryTurnPositions[i]; - driveRotations -= steerAngle.getRotations() * kCouplingGearRatio; + driveRotations -= steerAngle.getRotations() * kCouplingRatio; double driveRadians = Units.rotationsToRadians(driveRotations); - double positionMeters = driveRadians / (kDriveRatio / kWheelRadiusMeters); + double positionMeters = driveRadians * kWheelRadiusMeters; m_odometryPositions[i] = new SwerveModulePosition(positionMeters, steerAngle); m_lastPosition = m_odometryPositions[i]; } @@ -51,17 +53,14 @@ public SwerveModuleState setSetpoint(SwerveModuleState state) { final SwerveModuleState optimizedState = SwerveModuleState.optimize(state, m_inputs.steerPosition); double setpointVelocityRPS = - Units.radiansToRotations(optimizedState.speedMetersPerSecond / kWheelRadiusMeters) - * kDriveRatio; + Units.radiansToRotations(optimizedState.speedMetersPerSecond / kWheelRadiusMeters); double angleError = optimizedState.angle.getRadians() - m_inputs.steerPosition.getRadians(); setpointVelocityRPS *= Math.max(0.0, Math.cos(angleError)); - if (optimizedState.speedMetersPerSecond != 0.0) { - double azimuthVelocityRPS = Units.radiansToRotations(m_inputs.steerVelocityRadPerSec); - double driveRateBackOut = azimuthVelocityRPS *= kCouplingGearRatio; - setpointVelocityRPS += driveRateBackOut; - } + double azimuthVelocityRPS = Units.radiansToRotations(m_inputs.steerVelocityRadPerSec); + double driveRateBackOut = azimuthVelocityRPS * kCouplingRatio; + setpointVelocityRPS += driveRateBackOut; m_io.setSteerRotations(optimizedState.angle.getRotations()); m_io.setDriveRPS(setpointVelocityRPS); @@ -91,11 +90,14 @@ public double[] getOdometryTimestamps() { return m_inputs.odometryTimestamps; } - // getOdometryPositions() should be used for performant odometry updates, not this. public SwerveModulePosition getPosition() { return m_lastPosition; } + public double getDrivePositionRad() { + return m_inputs.drivePositionRad; + } + public SwerveModuleState getState() { return new SwerveModuleState( m_inputs.driveVelocityRadPerSec * kWheelRadiusMeters, m_inputs.steerPosition); diff --git a/src/main/java/frc/robot/drivetrain/ModuleIO.java b/src/main/java/frc/robot/drivetrain/ModuleIO.java index 4fdf464..3b9422a 100644 --- a/src/main/java/frc/robot/drivetrain/ModuleIO.java +++ b/src/main/java/frc/robot/drivetrain/ModuleIO.java @@ -22,6 +22,7 @@ public static class ModuleIOInputs { public double[] odometryDrivePositionsRad = new double[] {}; public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + public double totalOdometryLoopTime = 0.0; public boolean devicesConnected = false; } diff --git a/src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java b/src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java index bd52459..66bca4d 100644 --- a/src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/drivetrain/ModuleIOTalonFX.java @@ -1,7 +1,6 @@ package frc.robot.drivetrain; import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.TalonFXConfiguration; @@ -20,7 +19,6 @@ import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.simulation.DCMotorSim; @@ -29,13 +27,19 @@ public class ModuleIOTalonFX implements ModuleIO { - private final double kDrivekP = 0.031509; - private final double kDrivekS = 0.21806; - private final double kDrivekV = 0.13279; - private final double kDrivekA = 0.0057523; + private final int kConfigAttempts = 3; + private final double kDrivekP = 0.15; + private final double kDrivekS = 0.13; + private final double kDrivekV = 0.72; private final double kSteerkP = 100.0; private final double kSteerkD = 0.2; - private final double kSlipCurrent = Constants.kIsSim ? 400.0 : 58.0; + private static final double kSlipCurrent; + + static { + if (Constants.kIsSim) kSlipCurrent = 400.0; + else if (Constants.kIsViper) kSlipCurrent = 58.0; + else kSlipCurrent = 64.0; + } private final TalonFX m_driveMotor, m_steerMotor; private final CANcoder m_azimuthEncoder; @@ -54,10 +58,7 @@ public class ModuleIOTalonFX implements ModuleIO { private final MotionMagicExpoVoltage m_steerControl; private final VelocityVoltage m_driveControl; private final DCMotorSim m_driveSim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(kDrivekV, kDrivekA), - DCMotor.getKrakenX60Foc(1), - Module.kDriveRatio); + new DCMotorSim(DCMotor.getKrakenX60Foc(1), Module.kDriveRatio, 0.025); private final DCMotorSim m_steerSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), Module.kSteerRatio, 0.004); @@ -67,53 +68,49 @@ public ModuleIOTalonFX(int index) { m_driveMotor = new TalonFX(4, Constants.kDrivetrainCanBus); m_steerMotor = new TalonFX(3, Constants.kDrivetrainCanBus); m_azimuthEncoder = new CANcoder(12, Constants.kDrivetrainCanBus); - m_absoluteEncoderMagnetOffset = 0.37353515625; + m_absoluteEncoderMagnetOffset = Constants.kIsViper ? 0.37353515625 : 0.05029296875; break; case 1: // FR m_driveMotor = new TalonFX(6, Constants.kDrivetrainCanBus); m_steerMotor = new TalonFX(5, Constants.kDrivetrainCanBus); m_azimuthEncoder = new CANcoder(13, Constants.kDrivetrainCanBus); - m_absoluteEncoderMagnetOffset = -0.090576171875; + m_absoluteEncoderMagnetOffset = Constants.kIsViper ? -0.090576171875 : -0.439697265625; break; case 2: // BL m_driveMotor = new TalonFX(2, Constants.kDrivetrainCanBus); m_steerMotor = new TalonFX(1, Constants.kDrivetrainCanBus); m_azimuthEncoder = new CANcoder(11, Constants.kDrivetrainCanBus); - m_absoluteEncoderMagnetOffset = 0.275634765625; + m_absoluteEncoderMagnetOffset = Constants.kIsViper ? 0.275634765625 : 0.089599609375; break; case 3: // BR m_driveMotor = new TalonFX(7, Constants.kDrivetrainCanBus); m_steerMotor = new TalonFX(8, Constants.kDrivetrainCanBus); m_azimuthEncoder = new CANcoder(14, Constants.kDrivetrainCanBus); - m_absoluteEncoderMagnetOffset = 0.314208984375; + m_absoluteEncoderMagnetOffset = Constants.kIsViper ? 0.314208984375 : -0.43701171875; break; default: throw new RuntimeException("Invalid module index"); } final CANcoderConfiguration azimuthEncoderConfig = new CANcoderConfiguration(); - m_azimuthEncoder.getConfigurator().apply(azimuthEncoderConfig); // Factory Default azimuthEncoderConfig.MagnetSensor.AbsoluteSensorRange = AbsoluteSensorRangeValue.Signed_PlusMinusHalf; azimuthEncoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive; azimuthEncoderConfig.MagnetSensor.MagnetOffset = m_absoluteEncoderMagnetOffset; - m_azimuthEncoder.getConfigurator().apply(azimuthEncoderConfig); final TalonFXConfiguration driveMotorConfig = new TalonFXConfiguration(); - m_driveMotor.getConfigurator().apply(driveMotorConfig); // Factory Default driveMotorConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + driveMotorConfig.Feedback.SensorToMechanismRatio = Module.kDriveRatio; driveMotorConfig.Slot0.kP = kDrivekP; driveMotorConfig.Slot0.kS = kDrivekS; driveMotorConfig.Slot0.kV = kDrivekV; driveMotorConfig.CurrentLimits.StatorCurrentLimit = kSlipCurrent; driveMotorConfig.CurrentLimits.StatorCurrentLimitEnable = true; driveMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - m_driveMotor.getConfigurator().apply(driveMotorConfig); m_driveMotor.setPosition(0.0); final TalonFXConfiguration steerMotorConfig = new TalonFXConfiguration(); - m_steerMotor.getConfigurator().apply(steerMotorConfig); // Factory Default steerMotorConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; steerMotorConfig.Feedback.FeedbackRemoteSensorID = m_azimuthEncoder.getDeviceID(); steerMotorConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; @@ -127,7 +124,17 @@ public ModuleIOTalonFX(int index) { steerMotorConfig.MotionMagic.MotionMagicExpo_kA = 0.1; steerMotorConfig.Slot0.kP = kSteerkP; steerMotorConfig.Slot0.kD = kSteerkD; - m_steerMotor.getConfigurator().apply(steerMotorConfig); + steerMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + for (int i = 0; i < kConfigAttempts; i++) { + boolean errorPresent = m_driveMotor.getConfigurator().apply(driveMotorConfig, 0.1).isOK(); + errorPresent = + errorPresent && (m_steerMotor.getConfigurator().apply(steerMotorConfig, 0.1).isOK()); + errorPresent = + errorPresent + && (m_azimuthEncoder.getConfigurator().apply(azimuthEncoderConfig, 0.1).isOK()); + if (!errorPresent) break; + } m_drivePositionSignal = m_driveMotor.getPosition(); m_driveVelocitySignal = m_driveMotor.getVelocity(); @@ -141,21 +148,25 @@ public ModuleIOTalonFX(int index) { m_drivePositionQueue = PhoenixOdometryThread.getInstance() - .registerSignal(m_driveMotor, m_driveMotor.getPosition()); + .registerSignals( + m_driveMotor, m_drivePositionSignal.clone(), m_driveVelocitySignal.clone()); m_steerPositionQueue = PhoenixOdometryThread.getInstance() - .registerSignal(m_steerMotor, m_steerMotor.getPosition()); + .registerSignals( + m_steerMotor, m_steerPositionSignal.clone(), m_steerVelocitySignal.clone()); m_timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); BaseStatusSignal.setUpdateFrequencyForAll( - PhoenixOdometryThread.kOdometryFrequencyHz, m_drivePositionSignal, m_steerPositionSignal); + PhoenixOdometryThread.kOdometryFrequencyHz, + m_drivePositionSignal, + m_steerPositionSignal, + m_driveVelocitySignal, + m_steerVelocitySignal); BaseStatusSignal.setUpdateFrequencyForAll( 50.0, - m_driveVelocitySignal, m_driveAppliedVoltsSignal, m_driveCurrentSignal, m_steerAbsolutePositionSignal, - m_steerVelocitySignal, m_steerAppliedVoltsSignal, m_steerCurrentSignal); ParentDevice.optimizeBusUtilizationForAll(m_driveMotor, m_steerMotor, m_azimuthEncoder); @@ -180,18 +191,16 @@ public void updateInputs(ModuleIOInputs inputs) { m_steerVelocitySignal, m_steerAppliedVoltsSignal, m_steerCurrentSignal) - .equals(StatusCode.OK); + .isOK(); - inputs.drivePositionRad = - Units.rotationsToRadians(m_drivePositionSignal.getValueAsDouble()) / Module.kDriveRatio; + inputs.drivePositionRad = Units.rotationsToRadians(m_drivePositionSignal.getValueAsDouble()); inputs.driveVelocityRadPerSec = - Units.rotationsToRadians(m_driveVelocitySignal.getValueAsDouble()) / Module.kDriveRatio; + Units.rotationsToRadians(m_driveVelocitySignal.getValueAsDouble()); inputs.driveAppliedVolts = m_driveAppliedVoltsSignal.getValueAsDouble(); inputs.driveCurrentAmps = m_driveCurrentSignal.getValueAsDouble(); inputs.steerAbsolutePosition = Rotation2d.fromRotations(m_steerAbsolutePositionSignal.getValueAsDouble()); - // V*s / rad -> V*s / rot /** * NOTE We do not need to divide by the steer ratio since we're fusing a cancoder with the ratio * already accounted for. @@ -202,16 +211,17 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.steerAppliedVolts = m_steerAppliedVoltsSignal.getValueAsDouble(); inputs.steerCurrentAmps = m_steerCurrentSignal.getValueAsDouble(); + inputs.totalOdometryLoopTime = + PhoenixOdometryThread.getInstance().averageLoopTimeSupplier.get(); inputs.odometryDrivePositionsRad = m_drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value)) + .mapToDouble(value -> Units.rotationsToRadians(value)) .toArray(); inputs.odometryTurnPositions = m_steerPositionQueue.stream() .map((Double value) -> Rotation2d.fromRotations(value)) .toArray(Rotation2d[]::new); - inputs.odometryTimestamps = - m_timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryTimestamps = m_timestampQueue.stream().mapToDouble(Double::valueOf).toArray(); m_drivePositionQueue.clear(); m_steerPositionQueue.clear(); m_timestampQueue.clear(); diff --git a/src/main/java/frc/robot/drivetrain/PhoenixOdometryThread.java b/src/main/java/frc/robot/drivetrain/PhoenixOdometryThread.java index 6fa3d7e..7bc96d4 100644 --- a/src/main/java/frc/robot/drivetrain/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/drivetrain/PhoenixOdometryThread.java @@ -2,14 +2,18 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.hardware.ParentDevice; +import edu.wpi.first.math.Pair; +import edu.wpi.first.math.filter.LinearFilter; +import edu.wpi.first.math.filter.MedianFilter; import java.util.ArrayList; import java.util.List; import java.util.Queue; import java.util.concurrent.ArrayBlockingQueue; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; -import org.littletonrobotics.junction.LoggedRobot; +import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; public class PhoenixOdometryThread extends Thread { @@ -17,9 +21,15 @@ public class PhoenixOdometryThread extends Thread { public static final double kOdometryFrequencyHz = 250.0; private final Lock m_signalsLock = new ReentrantLock(); private BaseStatusSignal[] m_signals = new BaseStatusSignal[0]; - private final List> m_queues = new ArrayList<>(); + // State is defined as + private final List, StatusSignal>> allStateSignals = + new ArrayList<>(); + private final List> m_positionQueues = new ArrayList<>(); private final List> m_timestampQueues = new ArrayList<>(); + private double m_averageLoopTimeMs = 0.0; + public final Supplier averageLoopTimeSupplier = () -> m_averageLoopTimeMs; + private static PhoenixOdometryThread instance = null; public static PhoenixOdometryThread getInstance() { @@ -39,16 +49,19 @@ public void start() { if (m_timestampQueues.size() > 0) super.start(); } - public Queue registerSignal(ParentDevice device, StatusSignal signal) { + public Queue registerSignals( + ParentDevice device, StatusSignal signal, StatusSignal signalSlope) { Queue queue = new ArrayBlockingQueue<>(20); m_signalsLock.lock(); Drivetrain.odometryLock.lock(); try { - BaseStatusSignal[] newSignals = new BaseStatusSignal[m_signals.length + 1]; + BaseStatusSignal[] newSignals = new BaseStatusSignal[m_signals.length + 2]; System.arraycopy(m_signals, 0, newSignals, 0, m_signals.length); newSignals[m_signals.length] = signal; + newSignals[m_signals.length + 1] = signalSlope; m_signals = newSignals; - m_queues.add(queue); + m_positionQueues.add(queue); + allStateSignals.add(new Pair<>(signal, signalSlope)); } finally { m_signalsLock.unlock(); Drivetrain.odometryLock.unlock(); @@ -69,29 +82,41 @@ public Queue makeTimestampQueue() { @Override public void run() { + final MedianFilter peakRemover = new MedianFilter(3); + final LinearFilter lowPassFilter = LinearFilter.movingAverage(50); + double lastTime = 0.0; + double currentTime = 0.0; while (true) { m_signalsLock.lock(); try { - BaseStatusSignal.waitForAll(LoggedRobot.defaultPeriodSecs, m_signals); + BaseStatusSignal.waitForAll(2.0 / kOdometryFrequencyHz, m_signals); } finally { m_signalsLock.unlock(); } Drivetrain.odometryLock.lock(); try { + lastTime = currentTime; + currentTime = Utils.getCurrentTimeSeconds(); + m_averageLoopTimeMs = + lowPassFilter.calculate(peakRemover.calculate(currentTime - lastTime)) * 1e3; + double timestamp = Logger.getRealTimestamp() / 1e6; double totalLatency = 0.0; - for (BaseStatusSignal signal : m_signals) { + for (BaseStatusSignal signal : m_signals) totalLatency += signal.getTimestamp().getLatency(); + if (m_signals.length > 0) timestamp -= totalLatency / m_signals.length; + + for (int i = 0; i < m_positionQueues.size(); i++) { + final Pair, StatusSignal> stateSignals = + allStateSignals.get(i); + m_positionQueues + .get(i) + .offer( + BaseStatusSignal.getLatencyCompensatedValue( + stateSignals.getFirst(), stateSignals.getSecond())); } - if (m_signals.length > 0) { - timestamp -= totalLatency / m_signals.length; - } - for (int i = 0; i < m_signals.length; i++) { - m_queues.get(i).offer(m_signals[i].getValueAsDouble()); - } - for (int i = 0; i < m_timestampQueues.size(); i++) { + for (int i = 0; i < m_timestampQueues.size(); i++) m_timestampQueues.get(i).offer(timestamp); - } } finally { Drivetrain.odometryLock.unlock(); } diff --git a/src/main/java/frc/robot/drivetrain/WheelRadiusCharacterization.java b/src/main/java/frc/robot/drivetrain/WheelRadiusCharacterization.java new file mode 100644 index 0000000..c6b0339 --- /dev/null +++ b/src/main/java/frc/robot/drivetrain/WheelRadiusCharacterization.java @@ -0,0 +1,89 @@ +package frc.robot.drivetrain; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import frc.lib.LoggedTunableNumber; +import frc.robot.PoseEstimation; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; + +public class WheelRadiusCharacterization extends Command { + private static final LoggedTunableNumber characterizationSpeed = + new LoggedTunableNumber("WheelRadiusCharacterization/SpeedRadsPerSec", 1.0); + private static final double driveRadius = Drivetrain.kDriveBaseRadius; + private static final DoubleSupplier gyroYawRadsSupplier = + () -> PoseEstimation.getInstance().getPose().getRotation().getRadians(); + + public enum Direction { + CLOCKWISE(-1), + COUNTER_CLOCKWISE(1); + + private final int m_value; + + private Direction(int value) { + m_value = value; + } + } + + private final Drivetrain m_drivetrain; + private final Direction m_omegaDirection; + private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(1.0); + + private double lastGyroYawRads = 0.0; + private double accumGyroYawRads = 0.0; + + private double[] startWheelPositions; + + private double currentEffectiveWheelRadius = 0.0; + + public WheelRadiusCharacterization(Drivetrain drive, Direction omegaDirection) { + this.m_drivetrain = drive; + this.m_omegaDirection = omegaDirection; + addRequirements(drive); + } + + @Override + public void initialize() { + // Reset + lastGyroYawRads = gyroYawRadsSupplier.getAsDouble(); + accumGyroYawRads = 0.0; + startWheelPositions = m_drivetrain.getWheelRadiusCharacterizationPosition(); + omegaLimiter.reset(0); + } + + @Override + public void execute() { + m_drivetrain.runWheelRadiusCharacterization( + omegaLimiter.calculate(m_omegaDirection.m_value * characterizationSpeed.get())); + + accumGyroYawRads += MathUtil.angleModulus(gyroYawRadsSupplier.getAsDouble() - lastGyroYawRads); + lastGyroYawRads = gyroYawRadsSupplier.getAsDouble(); + double averageWheelPosition = 0.0; + double[] wheelPositiions = m_drivetrain.getWheelRadiusCharacterizationPosition(); + for (int i = 0; i < 4; i++) { + averageWheelPosition += Math.abs(wheelPositiions[i] - startWheelPositions[i]); + } + averageWheelPosition /= 4.0; + + currentEffectiveWheelRadius = (accumGyroYawRads * driveRadius) / averageWheelPosition; + Logger.recordOutput("Drivetrain/RadiusCharacterization/DrivePosition", averageWheelPosition); + Logger.recordOutput("Drivetrain/RadiusCharacterization/AccumGyroYawRads", accumGyroYawRads); + Logger.recordOutput( + "Drivetrain/RadiusCharacterization/CurrentWheelRadiusInches", + Units.metersToInches(currentEffectiveWheelRadius)); + } + + @Override + public void end(boolean interrupted) { + if (accumGyroYawRads <= Math.PI * 2.0) { + System.out.println("Not enough data for characterization"); + } else { + System.out.println( + "Effective Wheel Radius: " + + Units.metersToInches(currentEffectiveWheelRadius) + + " inches"); + } + } +} diff --git a/src/main/java/frc/robot/feeder/Feeder.java b/src/main/java/frc/robot/feeder/Feeder.java index 2d93b2b..bdb92aa 100644 --- a/src/main/java/frc/robot/feeder/Feeder.java +++ b/src/main/java/frc/robot/feeder/Feeder.java @@ -25,12 +25,10 @@ public Feeder(FeederIO io) { public void periodic() { m_io.updateInputs(m_inputs); Logger.processInputs("FeederInputs", m_inputs); - Logger.recordOutput( - "Feeder/Command", getCurrentCommand() == null ? "" : getCurrentCommand().getName()); } public Command idle() { - return this.run(() -> m_io.setRollerSpeedVolts(0.0)); + return this.run(() -> m_io.setRollerSpeedVolts(0.0)).withName("Idle (Feeder)"); } public Command stop() { @@ -39,16 +37,17 @@ public Command stop() { public Command shoot() { return Commands.parallel( - this.run(() -> m_io.setRollerSpeedVolts(-5.0)), - Commands.waitUntil(hasNote.negate()).andThen(NoteVisualizer.shoot())); + this.run(() -> m_io.setRollerSpeedVolts(-5.0)), + Commands.waitUntil(hasNote.negate()).andThen(NoteVisualizer.shoot())) + .withName("Shoot (Feeder)"); } public Command feed() { - return this.run(() -> m_io.setRollerSpeedVolts(-6.0)); + return this.run(() -> m_io.setRollerSpeedVolts(-6.0)).withName("Feed (Feeder)"); } public Command spit() { - return this.run(() -> m_io.setRollerSpeedVolts(12.0)); + return this.run(() -> m_io.setRollerSpeedVolts(12.0)).withName("Spit (Feeder)"); } public Command enterCoast() { diff --git a/src/main/java/frc/robot/feeder/FeederIOTalonFX.java b/src/main/java/frc/robot/feeder/FeederIOTalonFX.java index 4bbe2aa..391e729 100644 --- a/src/main/java/frc/robot/feeder/FeederIOTalonFX.java +++ b/src/main/java/frc/robot/feeder/FeederIOTalonFX.java @@ -1,17 +1,19 @@ package frc.robot.feeder; import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj.DigitalGlitchFilter; import edu.wpi.first.wpilibj.DigitalInput; import frc.robot.Constants; +import java.time.Duration; public class FeederIOTalonFX implements FeederIO { private final TalonFX m_feederMotor; - private final DigitalInput m_topNoteSensor; + private final DigitalInput m_noteSensor; + private final DigitalGlitchFilter m_glitchFilter; private final StatusSignal m_rollerPositionSignal, m_rollerVelocitySignal, @@ -20,21 +22,29 @@ public class FeederIOTalonFX implements FeederIO { public FeederIOTalonFX() { m_feederMotor = new TalonFX(19, Constants.kSuperstructureCanBus); - m_topNoteSensor = new DigitalInput(0); + m_noteSensor = new DigitalInput(0); + m_glitchFilter = new DigitalGlitchFilter(); + m_glitchFilter.setPeriodNanoSeconds(Duration.ofMillis(1).toNanos()); + m_glitchFilter.add(m_noteSensor); final TalonFXConfiguration feederMotorConfig = new TalonFXConfiguration(); m_feederMotor.getConfigurator().apply(feederMotorConfig); feederMotorConfig.CurrentLimits.SupplyCurrentLimit = 40.0; feederMotorConfig.CurrentLimits.SupplyCurrentLimitEnable = true; feederMotorConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + feederMotorConfig.Feedback.SensorToMechanismRatio = Feeder.kFeederReduction; m_feederMotor.getConfigurator().apply(feederMotorConfig); m_rollerPositionSignal = m_feederMotor.getPosition(); m_rollerVelocitySignal = m_feederMotor.getVelocity(); m_rollerAppliedVoltageSignal = m_feederMotor.getMotorVoltage(); - m_rollerCurrentSignal = m_feederMotor.getStatorCurrent(); + m_rollerCurrentSignal = m_feederMotor.getSupplyCurrent(); BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, m_rollerVelocitySignal, m_rollerAppliedVoltageSignal, m_rollerCurrentSignal); + 50.0, + m_rollerPositionSignal, + m_rollerVelocitySignal, + m_rollerAppliedVoltageSignal, + m_rollerCurrentSignal); m_feederMotor.optimizeBusUtilization(); } @@ -46,13 +56,13 @@ public void updateInputs(FeederIOInputs inputs) { m_rollerVelocitySignal, m_rollerAppliedVoltageSignal, m_rollerCurrentSignal) - .equals(StatusCode.OK); + .isOK(); - inputs.rollerPositionRot = m_rollerPositionSignal.getValue() / Feeder.kFeederReduction; - inputs.rollerVelocityRps = m_rollerVelocitySignal.getValue() / Feeder.kFeederReduction; + inputs.rollerPositionRot = m_rollerPositionSignal.getValue(); + inputs.rollerVelocityRps = m_rollerVelocitySignal.getValue(); inputs.rollerAppliedVolts = m_rollerAppliedVoltageSignal.getValue(); inputs.rollerCurrentAmps = m_rollerCurrentSignal.getValue(); - inputs.topNoteSensorTripped = m_topNoteSensor.get(); + inputs.topNoteSensorTripped = m_noteSensor.get(); } @Override diff --git a/src/main/java/frc/robot/intake/Intake.java b/src/main/java/frc/robot/intake/Intake.java index 6e7c8c1..4b7635e 100644 --- a/src/main/java/frc/robot/intake/Intake.java +++ b/src/main/java/frc/robot/intake/Intake.java @@ -2,6 +2,8 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer.DebounceType; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; @@ -9,7 +11,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; -import frc.robot.FieldPositions.NotePositions; +import frc.robot.FieldConstants.NotePositions; import frc.robot.NoteVisualizer; import frc.robot.PoseEstimation; import java.util.ArrayList; @@ -39,11 +41,12 @@ public void periodic() { public Command idle() { return this.run( - () -> { - m_io.setTopRollerDutyCycle(0.0); - m_io.setBottomRollerDutyCycle(0.0); - m_io.setCenteringMotorsDutyCycle(0.0); - }); + () -> { + m_io.setFrontRollersVoltage(0.0); + m_io.setBackRollersVoltage(0.0); + m_io.setCenteringMotorsVoltage(0.0); + }) + .withName("Idle (Intake)"); } public Command stop() { @@ -52,33 +55,38 @@ public Command stop() { public Command spit() { return this.run( - () -> { - m_io.setTopRollerDutyCycle(-0.8); - m_io.setBottomRollerDutyCycle(-0.8); - m_io.setCenteringMotorsDutyCycle(-0.5); - }); + () -> { + m_io.setFrontRollersVoltage(-9.6); + m_io.setBackRollersVoltage(-9.6); + m_io.setCenteringMotorsVoltage(-10.2); + }) + .withName("Spit (Intake)"); } public Command intake() { return this.run( () -> { - m_io.setTopRollerDutyCycle(0.8); - m_io.setBottomRollerDutyCycle(0.8); - m_io.setCenteringMotorsDutyCycle(0.85); + m_io.setFrontRollersVoltage(9.6); + m_io.setBackRollersVoltage(9.6); + m_io.setCenteringMotorsVoltage(10.2); }) .alongWith( Commands.run(() -> m_isIntakingPieceSim = simulateIsIntakingPiece()) .until(() -> m_isIntakingPieceSim) .andThen(Commands.run(() -> m_isIntakingPieceSim = true)) .finallyDo(() -> m_isIntakingPieceSim = false) - .unless(() -> !Constants.kIsSim)); + .unless(() -> !Constants.kIsSim)) + .withName("Intake (Intake)"); } private boolean simulateIsIntakingPiece() { final Translation2d intakeOffsetFromRobotCenter = new Translation2d(Units.inchesToMeters(10.0), 0.0); final Translation2d intakePosition = - PoseEstimation.getInstance().getPose().getTranslation().plus(intakeOffsetFromRobotCenter); + PoseEstimation.getInstance() + .getPose() + .transformBy(new Transform2d(intakeOffsetFromRobotCenter, new Rotation2d())) + .getTranslation(); final double kIntakeXRangeInches = 9.0; final double kIntakeYRangeInches = 27.0 / 2.0; for (int noteIndex = 0; noteIndex < NotePositions.noteTranslations.length; noteIndex++) { @@ -97,12 +105,16 @@ private boolean simulateIsIntakingPiece() { return false; } + public void resetSimIntookPieces() { + m_intookPieces.clear(); + } + public Command feed() { return this.run( () -> { - m_io.setTopRollerDutyCycle(0.4); - m_io.setBottomRollerDutyCycle(0.4); - m_io.setCenteringMotorsDutyCycle(0.5); + m_io.setFrontRollersVoltage(4.8); + m_io.setBackRollersVoltage(4.8); + m_io.setCenteringMotorsVoltage(6.0); }); } } diff --git a/src/main/java/frc/robot/intake/IntakeIO.java b/src/main/java/frc/robot/intake/IntakeIO.java index de895c3..0ad6bf3 100644 --- a/src/main/java/frc/robot/intake/IntakeIO.java +++ b/src/main/java/frc/robot/intake/IntakeIO.java @@ -5,25 +5,28 @@ public interface IntakeIO { @AutoLog public class IntakeIOInputs { - public double topRollerVelocityRadPerSec = 0.0; + public double topRollerVelocityRpm = 0.0; public double topRollerAppliedVolts = 0.0; public double topRollerCurrentAmps = 0.0; - public double bottomRollerVelocityRadPerSec = 0.0; + public double bottomRollerVelocityRpm = 0.0; public double bottomRollerAppliedVolts = 0.0; public double bottomRollerCurrentAmps = 0.0; public double centeringBagMotorsAppliedVolts = 0.0; public double centeringBagMotorsCurrentAmps = 0.0; + + public boolean rollerMotorsConnected = false; + public boolean centeringMotorConnected = false; } public default void updateInputs(IntakeIOInputs inputs) {} - public default void setTopRollerDutyCycle(double outputDutyCycle) {} + public default void setFrontRollersVoltage(double volts) {} - public default void setBottomRollerDutyCycle(double outputDutyCycle) {} + public default void setBackRollersVoltage(double volts) {} - public default void setCenteringMotorsDutyCycle(double outputDutyCycle) {} + public default void setCenteringMotorsVoltage(double volts) {} public default void stopRollers() {} } diff --git a/src/main/java/frc/robot/intake/IntakeIOSim.java b/src/main/java/frc/robot/intake/IntakeIOSim.java index 0094c83..a5dc869 100644 --- a/src/main/java/frc/robot/intake/IntakeIOSim.java +++ b/src/main/java/frc/robot/intake/IntakeIOSim.java @@ -2,14 +2,16 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.simulation.DCMotorSim; public class IntakeIOSim implements IntakeIO { private double m_topRollerAppliedVolts, m_bottomRollerAppliedVolts, m_centeringRollerAppliedVolts; - private final DCMotorSim m_topRollerSim = new DCMotorSim(DCMotor.getNEO(1), 1.0, 0.01); - private final DCMotorSim m_bottomRollerSim = new DCMotorSim(DCMotor.getNEO(1), 1.0, 0.01); + private final DCMotorSim m_topRollerSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), 1.0, 0.01); + private final DCMotorSim m_bottomRollerSim = + new DCMotorSim(DCMotor.getFalcon500Foc(1), 1.0, 0.01); private final DCMotorSim m_centeringRollersSim = new DCMotorSim(DCMotor.getBag(2), 1.0, 0.01); @Override @@ -22,11 +24,13 @@ public void updateInputs(IntakeIOInputs inputs) { m_bottomRollerSim.update(0.02); m_centeringRollersSim.update(0.02); - inputs.topRollerVelocityRadPerSec = m_topRollerSim.getAngularVelocityRadPerSec(); + inputs.topRollerVelocityRpm = + Units.radiansPerSecondToRotationsPerMinute(m_topRollerSim.getAngularVelocityRadPerSec()); inputs.topRollerAppliedVolts = m_topRollerAppliedVolts; inputs.topRollerCurrentAmps = m_topRollerSim.getCurrentDrawAmps(); - inputs.bottomRollerVelocityRadPerSec = m_bottomRollerSim.getAngularVelocityRadPerSec(); + inputs.bottomRollerVelocityRpm = + Units.radiansPerSecondToRotationsPerMinute(m_bottomRollerSim.getAngularVelocityRadPerSec()); inputs.bottomRollerAppliedVolts = m_bottomRollerAppliedVolts; inputs.bottomRollerCurrentAmps = m_bottomRollerSim.getCurrentDrawAmps(); @@ -35,18 +39,18 @@ public void updateInputs(IntakeIOInputs inputs) { } @Override - public void setTopRollerDutyCycle(double outputDutyCycle) { - m_topRollerAppliedVolts = MathUtil.clamp(outputDutyCycle * 12.0, -12.0, 12.0); + public void setFrontRollersVoltage(double voltage) { + m_topRollerAppliedVolts = MathUtil.clamp(voltage, -12.0, 12.0); } @Override - public void setBottomRollerDutyCycle(double outputDutyCycle) { - m_bottomRollerAppliedVolts = MathUtil.clamp(outputDutyCycle * 12.0, -12.0, 12.0); + public void setBackRollersVoltage(double voltage) { + m_bottomRollerAppliedVolts = MathUtil.clamp(voltage, -12.0, 12.0); } @Override - public void setCenteringMotorsDutyCycle(double outputDutyCycle) { - m_centeringRollerAppliedVolts = MathUtil.clamp(outputDutyCycle * 12.0, -12.0, 12.0); + public void setCenteringMotorsVoltage(double voltage) { + m_centeringRollerAppliedVolts = MathUtil.clamp(voltage, -12.0, 12.0); } @Override diff --git a/src/main/java/frc/robot/intake/IntakeIOSparkMAX.java b/src/main/java/frc/robot/intake/IntakeIOSparkMAX.java index 04a5277..1a3a823 100644 --- a/src/main/java/frc/robot/intake/IntakeIOSparkMAX.java +++ b/src/main/java/frc/robot/intake/IntakeIOSparkMAX.java @@ -1,68 +1,74 @@ package frc.robot.intake; import com.revrobotics.CANSparkMax; +import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; import edu.wpi.first.math.util.Units; public class IntakeIOSparkMAX implements IntakeIO { - private final CANSparkMax topRollerMotor, bottomRollerMotor, centeringMotors; + private final CANSparkMax m_topRollerMotor, m_bottomRollerMotor, m_centeringMotors; private final RelativeEncoder m_topRollerEncoder, m_bottomRollerEncoder; public IntakeIOSparkMAX() { - topRollerMotor = new CANSparkMax(5, CANSparkMax.MotorType.kBrushless); - bottomRollerMotor = new CANSparkMax(6, CANSparkMax.MotorType.kBrushless); - centeringMotors = new CANSparkMax(8, CANSparkMax.MotorType.kBrushed); + m_topRollerMotor = new CANSparkMax(5, CANSparkMax.MotorType.kBrushless); + m_bottomRollerMotor = new CANSparkMax(6, CANSparkMax.MotorType.kBrushless); + m_centeringMotors = new CANSparkMax(8, CANSparkMax.MotorType.kBrushed); - m_topRollerEncoder = topRollerMotor.getEncoder(); - m_bottomRollerEncoder = bottomRollerMotor.getEncoder(); + m_topRollerEncoder = m_topRollerMotor.getEncoder(); + m_bottomRollerEncoder = m_bottomRollerMotor.getEncoder(); - topRollerMotor.restoreFactoryDefaults(); - bottomRollerMotor.restoreFactoryDefaults(); + m_topRollerMotor.restoreFactoryDefaults(); + m_bottomRollerMotor.restoreFactoryDefaults(); - topRollerMotor.setSmartCurrentLimit(40); - bottomRollerMotor.setSmartCurrentLimit(40); - centeringMotors.setSmartCurrentLimit(40); + m_topRollerMotor.setSmartCurrentLimit(40); + m_bottomRollerMotor.setSmartCurrentLimit(40); + m_centeringMotors.setSmartCurrentLimit(40); } @Override public void updateInputs(IntakeIOInputs inputs) { - inputs.topRollerVelocityRadPerSec = + inputs.topRollerVelocityRpm = Units.rotationsPerMinuteToRadiansPerSecond(m_topRollerEncoder.getVelocity()); inputs.topRollerAppliedVolts = - topRollerMotor.getAppliedOutput() * topRollerMotor.getBusVoltage(); - inputs.topRollerCurrentAmps = topRollerMotor.getOutputCurrent(); + m_topRollerMotor.getAppliedOutput() * m_topRollerMotor.getBusVoltage(); + inputs.topRollerCurrentAmps = m_topRollerMotor.getOutputCurrent(); - inputs.bottomRollerVelocityRadPerSec = + inputs.bottomRollerVelocityRpm = Units.rotationsPerMinuteToRadiansPerSecond(m_bottomRollerEncoder.getVelocity()); inputs.bottomRollerAppliedVolts = - bottomRollerMotor.getAppliedOutput() * bottomRollerMotor.getBusVoltage(); - inputs.bottomRollerCurrentAmps = bottomRollerMotor.getOutputCurrent(); + m_bottomRollerMotor.getAppliedOutput() * m_bottomRollerMotor.getBusVoltage(); + inputs.bottomRollerCurrentAmps = m_bottomRollerMotor.getOutputCurrent(); inputs.centeringBagMotorsAppliedVolts = - centeringMotors.getAppliedOutput() * centeringMotors.getBusVoltage(); - inputs.bottomRollerCurrentAmps = centeringMotors.getOutputCurrent(); + m_centeringMotors.getAppliedOutput() * m_centeringMotors.getBusVoltage(); + inputs.bottomRollerCurrentAmps = m_centeringMotors.getOutputCurrent(); + + inputs.rollerMotorsConnected = + m_topRollerMotor.getLastError() == REVLibError.kOk + && m_bottomRollerMotor.getLastError() == REVLibError.kOk; + inputs.centeringMotorConnected = m_centeringMotors.getLastError() == REVLibError.kOk; } @Override - public void setTopRollerDutyCycle(double outputDutyCycle) { - topRollerMotor.set(outputDutyCycle); + public void setFrontRollersVoltage(double volts) { + m_topRollerMotor.setVoltage(volts); } @Override - public void setBottomRollerDutyCycle(double outputDutyCycle) { - bottomRollerMotor.set(outputDutyCycle); + public void setBackRollersVoltage(double volts) { + m_bottomRollerMotor.setVoltage(volts); } @Override - public void setCenteringMotorsDutyCycle(double outputDutyCycle) { - centeringMotors.set(outputDutyCycle); + public void setCenteringMotorsVoltage(double volts) { + m_centeringMotors.setVoltage(volts); } @Override public void stopRollers() { - topRollerMotor.stopMotor(); - bottomRollerMotor.stopMotor(); - centeringMotors.stopMotor(); + m_topRollerMotor.stopMotor(); + m_bottomRollerMotor.stopMotor(); + m_centeringMotors.stopMotor(); } } diff --git a/src/main/java/frc/robot/intake/IntakeIOTalonFX.java b/src/main/java/frc/robot/intake/IntakeIOTalonFX.java new file mode 100644 index 0000000..1cad50c --- /dev/null +++ b/src/main/java/frc/robot/intake/IntakeIOTalonFX.java @@ -0,0 +1,113 @@ +package frc.robot.intake; + +import com.ctre.phoenix.ErrorCode; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; +import com.ctre.phoenix.motorcontrol.can.BaseTalon; +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; + +public class IntakeIOTalonFX implements IntakeIO { + + private final TalonFX m_frontRollersMotor, m_backRollersMotor; + private final BaseTalon m_centeringMotors; + + private final StatusSignal m_topRollerVelocitySignal, + m_topRollerAppliedVoltageSignal, + m_topRollerCurrentSignal, + m_bottomRollerVelocitySignal, + m_bottomRollerAppliedVoltageSignal, + m_bottomRollerCurrentSignal; + + public IntakeIOTalonFX() { + m_frontRollersMotor = new TalonFX(22, "rio"); + m_backRollersMotor = new TalonFX(23, "rio"); + m_centeringMotors = new BaseTalon(24, "Talon SRX", "rio"); + final CurrentLimitsConfigs currentLimitsConfigs = + new CurrentLimitsConfigs().withSupplyCurrentLimit(40.0).withSupplyCurrentLimitEnable(true); + final MotorOutputConfigs motorOutputConfigs = + new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive); + final TalonFXConfiguration rollerConfigs = + new TalonFXConfiguration() + .withCurrentLimits(currentLimitsConfigs) + .withMotorOutput(motorOutputConfigs); + m_frontRollersMotor.getConfigurator().apply(rollerConfigs); + m_backRollersMotor.getConfigurator().apply(rollerConfigs); + + m_centeringMotors.configSupplyCurrentLimit( + new SupplyCurrentLimitConfiguration(true, 40.0, 0.0, 1.0), 1000); + m_centeringMotors.configVoltageCompSaturation(12.0); + m_centeringMotors.enableVoltageCompensation(true); + + m_topRollerVelocitySignal = m_frontRollersMotor.getVelocity(); + m_topRollerAppliedVoltageSignal = m_frontRollersMotor.getMotorVoltage(); + m_topRollerCurrentSignal = m_frontRollersMotor.getSupplyCurrent(); + + m_bottomRollerVelocitySignal = m_backRollersMotor.getVelocity(); + m_bottomRollerAppliedVoltageSignal = m_backRollersMotor.getMotorVoltage(); + m_bottomRollerCurrentSignal = m_backRollersMotor.getSupplyCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + m_topRollerVelocitySignal, + m_topRollerAppliedVoltageSignal, + m_topRollerCurrentSignal, + m_bottomRollerVelocitySignal, + m_bottomRollerAppliedVoltageSignal, + m_bottomRollerCurrentSignal); + ParentDevice.optimizeBusUtilizationForAll(m_frontRollersMotor, m_backRollersMotor); + } + + @Override + public void updateInputs(IntakeIOInputs inputs) { + inputs.rollerMotorsConnected = + BaseStatusSignal.refreshAll( + m_topRollerVelocitySignal, + m_topRollerAppliedVoltageSignal, + m_topRollerCurrentSignal, + m_bottomRollerVelocitySignal, + m_bottomRollerAppliedVoltageSignal, + m_bottomRollerCurrentSignal) + .isOK(); + inputs.centeringMotorConnected = m_centeringMotors.getLastError() == ErrorCode.OK; + + inputs.topRollerVelocityRpm = m_topRollerVelocitySignal.getValue() * 60.0; + inputs.topRollerAppliedVolts = m_topRollerAppliedVoltageSignal.getValue(); + inputs.topRollerCurrentAmps = m_topRollerCurrentSignal.getValue(); + + inputs.bottomRollerVelocityRpm = m_bottomRollerVelocitySignal.getValue() * 60.0; + inputs.bottomRollerAppliedVolts = m_bottomRollerAppliedVoltageSignal.getValue(); + inputs.bottomRollerCurrentAmps = m_bottomRollerCurrentSignal.getValue(); + + inputs.centeringBagMotorsAppliedVolts = m_centeringMotors.getMotorOutputVoltage(); + inputs.centeringBagMotorsCurrentAmps = m_centeringMotors.getSupplyCurrent(); + } + + @Override + public void setFrontRollersVoltage(double volts) { + m_frontRollersMotor.setVoltage(volts); + } + + @Override + public void setBackRollersVoltage(double volts) { + m_backRollersMotor.setVoltage(volts); + } + + @Override + public void setCenteringMotorsVoltage(double volts) { + m_centeringMotors.set(ControlMode.PercentOutput, volts / 12.0); + } + + @Override + public void stopRollers() { + m_frontRollersMotor.stopMotor(); + m_backRollersMotor.stopMotor(); + m_centeringMotors.set(ControlMode.PercentOutput, 0.0); + } +} diff --git a/src/main/java/frc/robot/lights/Lights.java b/src/main/java/frc/robot/lights/Lights.java index 4c76f39..3606a65 100644 --- a/src/main/java/frc/robot/lights/Lights.java +++ b/src/main/java/frc/robot/lights/Lights.java @@ -1,10 +1,15 @@ package frc.robot.lights; +import com.ctre.phoenix.ErrorCode; import com.ctre.phoenix.led.CANdle; import com.ctre.phoenix.led.CANdle.LEDStripType; import com.ctre.phoenix.led.CANdleConfiguration; +import com.ctre.phoenix.led.LarsonAnimation; +import com.ctre.phoenix.led.LarsonAnimation.BounceMode; import com.ctre.phoenix.led.RainbowAnimation; +import com.ctre.phoenix.led.SingleFadeAnimation; import com.ctre.phoenix.led.StrobeAnimation; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -17,7 +22,8 @@ public class Lights extends SubsystemBase { - private final int kLedCount = 8; + private final int kLedCount = Constants.kIsViper ? 8 : 32; + private final int kCandleLedOffset = Constants.kIsViper ? 0 : 8; private final CANdle m_ledController; public Lights() { @@ -27,7 +33,7 @@ public Lights() { config.disableWhenLOS = true; config.brightnessScalar = 1; m_ledController.configAllSettings(config); - setDefaultCommand(setRainbow()); + setDefaultCommand(idle()); } @Override @@ -36,61 +42,84 @@ public void periodic() { Logger.recordOutput("CANdle/5VRailVoltage", m_ledController.get5VRailVoltage()); Logger.recordOutput("CANdle/Current", m_ledController.getCurrent()); Logger.recordOutput("CANdle/Temperature", m_ledController.getTemperature()); + Logger.recordOutput("CANdle/Connected", m_ledController.getLastError() == ErrorCode.OK); } - public Command setRainbow() { + public Command idle() { + return Commands.either( + startGreenLarson().until(DriverStation::isEnabled), + this.runOnce(() -> m_ledController.animate(new SingleFadeAnimation(0, 0, 0, 0, 0, 0))) + .andThen(this.run(() -> {})), + DriverStation::isDisabled) + .ignoringDisable(true); + } + + public Command startRainbow() { return this.runOnce(() -> m_ledController.animate(new RainbowAnimation(1.0, 0.1, kLedCount))) .andThen(this.run(() -> {})) .ignoringDisable(true); } - public Command setBlink(Color color) { - final int[] blinkColor = colorToRGB(color); - return Commands.sequence( - this.runOnce( - () -> - m_ledController.animate( - new StrobeAnimation( - blinkColor[0], blinkColor[1], blinkColor[2], 0, 0.4, kLedCount))), - this.run(() -> {})) + public Command startGreenLarson() { + final ColorRGB color = colorToRGB(Color.kGreen); + return this.runOnce( + () -> + m_ledController.animate( + new LarsonAnimation( + color.red, + color.green, + color.blue, + 0, + 0.0, + kLedCount - kCandleLedOffset, + BounceMode.Center, + 7, + kCandleLedOffset))) + .andThen(this.run(() -> {})) + .ignoringDisable(true); + } + + public Command startBlink(Color color) { + return Commands.sequence(this.runOnce(() -> setBlink(color)), this.run(() -> {})) .ignoringDisable(true); } - public void setBlinkMethod(Color color) { - final int[] blinkColor = colorToRGB(color); + public Command noteBlink() { + return Commands.sequence( + this.runOnce(() -> setBlink(Color.kOrangeRed)), this.run(() -> {}).withTimeout(2.0)) + .ignoringDisable(true); + } + public void setBlink(Color color) { + final ColorRGB blinkColor = colorToRGB(color); m_ledController.animate( - new StrobeAnimation(blinkColor[0], blinkColor[1], blinkColor[2], 0, 0.4, kLedCount)); + new StrobeAnimation(blinkColor.red, blinkColor.green, blinkColor.blue, 0, 0.4, kLedCount)); } - /** - * @param color - * @return an array of integers representing the RGB values of the given color: [0] is red, [1] is - * green, [2] is blue - */ - private int[] colorToRGB(Color color) { + private ColorRGB colorToRGB(Color color) { final Function mapValue = value -> (int) (value * 255.0); - return new int[] { - mapValue.apply(color.red), mapValue.apply(color.green), mapValue.apply(color.blue) - }; + return new ColorRGB( + mapValue.apply(color.red), mapValue.apply(color.green), mapValue.apply(color.blue)); } public Command showReadyToShootStatus(Trigger readyToShoot) { - var lastState = + var state = new Object() { - boolean value = false; + boolean lastTriggerState = false; }; return new FunctionalCommand( - () -> setBlinkMethod(Color.kRed), + () -> setBlink(Color.kRed), () -> { - boolean currentTriggerState = readyToShoot.getAsBoolean(); - if (lastState.value == currentTriggerState) return; - if (readyToShoot.getAsBoolean()) setBlinkMethod(Color.kGreen); - else setBlinkMethod(Color.kRed); - lastState.value = readyToShoot.getAsBoolean(); + final boolean currentTriggerState = readyToShoot.getAsBoolean(); + if (state.lastTriggerState == currentTriggerState) return; + if (readyToShoot.getAsBoolean()) setBlink(Color.kGreen); + else setBlink(Color.kRed); + state.lastTriggerState = readyToShoot.getAsBoolean(); }, - (interrupted) -> {}, + (interrupted) -> startRainbow().schedule(), () -> false, this); } + + private record ColorRGB(int red, int green, int blue) {} } diff --git a/src/main/java/frc/robot/shooter/Shooter.java b/src/main/java/frc/robot/shooter/Shooter.java index c3832e2..4315512 100644 --- a/src/main/java/frc/robot/shooter/Shooter.java +++ b/src/main/java/frc/robot/shooter/Shooter.java @@ -2,61 +2,58 @@ import static edu.wpi.first.units.Units.Volts; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import com.ctre.phoenix6.SignalLogger; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.Debouncer.DebounceType; 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.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; +import frc.robot.Constants; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Shooter extends SubsystemBase { - private final double kFarShotVelocityRpm = 5600.0; - private final double kAmpshot = 5000.0; - private final double kReadyToShootToleranceRps = 3.0; + public static final double kShooterStepUp = Constants.kIsViper ? 1.0 : 1.0; + public static final double kFarShotVelocityRpm = 5800.0; + public static final double kRevUpVelocityRpm = 0.75 * kFarShotVelocityRpm; + private final double kTrapShot = 400.0; + private final double kAmpShot = 5000.0; + private final double kReadyToShootToleranceRps = 5.0; // Denominator for gains here are in rotations - public static final double topRollerkS = 0.18039; - public static final double topRollerkV = 0.11968; - public static final double topRollerkA = 0.0089044; - public static final double bottomRollerkS = 0.19936; - public static final double bottomRollerkV = 0.12041; - public static final double bottomRollerkA = 0.0071461; - - private final double topRollerkP = 0.13085; - private final double bottomRollerkP = 0.11992; - - private final PIDController m_topRollerController, m_bottomRollerController; - private final SimpleMotorFeedforward m_topRollerFeedforward, m_bottomRollerFeedforward; - private final SysIdRoutine m_sysIdRoutine; + public static final double topRollerkS = 0.15945; + public static final double topRollerkV = 0.11596; + public static final double topRollerkA = 0.013645; + public static final double bottomRollerkS = 0.11589; + public static final double bottomRollerkV = 0.11618; + public static final double bottomRollerkA = 0.015157; + + public static final double topRollerkP = 0.176; + public static final double bottomRollerkP = 0.176; + private final SysIdRoutine m_sysIdRoutine; private final ShooterIO m_io; private final ShooterIOInputsAutoLogged m_inputs = new ShooterIOInputsAutoLogged(); public final Trigger readyToShoot = - new Trigger(() -> getRollersAtSetpoint()).debounce(0.1, DebounceType.kBoth); + new Trigger(this::getRollersAtSetpoint).debounce(0.1, DebounceType.kBoth); + + private double m_lastSetpoint; public Shooter(ShooterIO io) { m_io = io; - m_topRollerFeedforward = new SimpleMotorFeedforward(topRollerkS, topRollerkV, topRollerkA); - m_topRollerController = new PIDController(topRollerkP, 0.0, 0.0); - m_bottomRollerFeedforward = - new SimpleMotorFeedforward(bottomRollerkS, bottomRollerkV, bottomRollerkA); - m_bottomRollerController = new PIDController(bottomRollerkP, 0.0, 0.0); - - m_topRollerController.setTolerance(kReadyToShootToleranceRps); - m_bottomRollerController.setTolerance(kReadyToShootToleranceRps); m_sysIdRoutine = new SysIdRoutine( new SysIdRoutine.Config( null, null, null, - state -> Logger.recordOutput("Shooter/SysIdState", state.toString())), + state -> SignalLogger.writeString("Shooter/SysIdState", state.toString())), new SysIdRoutine.Mechanism( voltage -> { m_io.setTopRollerVoltage(voltage.in(Volts)); @@ -71,34 +68,34 @@ public Shooter(ShooterIO io) { public void periodic() { m_io.updateInputs(m_inputs); Logger.processInputs("ShooterInputs", m_inputs); - Logger.recordOutput( - "Shooter/Command", getCurrentCommand() == null ? "" : getCurrentCommand().getName()); } private void setRollersSetpointRpm(double velocityRpm) { double goalVelocityRps = velocityRpm / 60.0; Logger.recordOutput("Shooter/Goal Roller RPS", goalVelocityRps); - double topRollerFeedforwardOutput = m_topRollerFeedforward.calculate(goalVelocityRps); - double bottomRollerFeedforwardOutput = m_bottomRollerFeedforward.calculate(goalVelocityRps); - double topRollerFeedbackOutput = - m_topRollerController.calculate(m_inputs.topRollerVelocityRps, goalVelocityRps); - double bottomRollerFeedbackOutput = - m_bottomRollerController.calculate(m_inputs.bottomRollerVelocityRps, goalVelocityRps); - m_io.setTopRollerVoltage(topRollerFeedforwardOutput + topRollerFeedbackOutput); - m_io.setBottomRollerVoltage(bottomRollerFeedforwardOutput + bottomRollerFeedbackOutput); + m_lastSetpoint = goalVelocityRps; + m_io.setTopRollerVelocity(goalVelocityRps); + m_io.setBottomRollerVelocity(goalVelocityRps); } @AutoLogOutput(key = "Shooter/RollersAtSetpoint") private boolean getRollersAtSetpoint() { - return m_bottomRollerController.atSetpoint() && m_topRollerController.atSetpoint(); + if (m_lastSetpoint == 0) return false; + return MathUtil.isNear( + m_lastSetpoint, m_inputs.bottomRollerVelocityRps, kReadyToShootToleranceRps) + && MathUtil.isNear( + m_lastSetpoint, m_inputs.topRollerVelocityRps, kReadyToShootToleranceRps); } - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutine.quasistatic(direction); - } - - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutine.dynamic(direction); + public Command sysId() { + return Commands.sequence( + m_sysIdRoutine.quasistatic(Direction.kForward), + Commands.waitSeconds(15.0), + m_sysIdRoutine.quasistatic(Direction.kReverse), + Commands.waitSeconds(15.0), + m_sysIdRoutine.dynamic(Direction.kForward), + Commands.waitSeconds(15.0), + m_sysIdRoutine.dynamic(Direction.kReverse)); } public Command idle() { @@ -107,7 +104,7 @@ public Command idle() { m_io.setTopRollerVoltage(0.0); m_io.setBottomRollerVoltage(0.0); }) - .withName("Idle"); + .withName("Idle (Shooter)"); } public Command stop() { @@ -115,20 +112,38 @@ public Command stop() { } public Command runShooter() { + return this.run(() -> setRollersSetpointRpm(kFarShotVelocityRpm)) + .finallyDo(() -> Logger.recordOutput("Shooter/Goal Roller RPS", 0.0)) + .withName("Run Shooter (Shooter)"); + } + + public Command revUpShooter() { + return this.run(() -> setRollersSetpointRpm(kRevUpVelocityRpm)) + .finallyDo(() -> Logger.recordOutput("Shooter/Goal Roller RPS", 0.0)) + .withName("Rev Shooter (Shooter)"); + } + + public Command trapShot() { + return this.run(() -> setRollersSetpointRpm(kTrapShot)) + .finallyDo(() -> Logger.recordOutput("Shooter/Goal Roller RPS", 0.0)) + .withName("Trap Shot (Shooter)"); + } + + public Command trapAssist() { return this.run( () -> { - setRollersSetpointRpm(kFarShotVelocityRpm); + m_io.setTopRollerVelocity(kTrapShot / 60.0); + m_io.setBottomRollerVelocity(-kTrapShot / 60.0); }) - .finallyDo( - () -> { - Logger.recordOutput("Shooter/Goal Roller RPS", 0.0); - m_topRollerController.reset(); - m_bottomRollerController.reset(); - }); + .withName("Trap Assist"); + } + + public Command feederShot() { + return this.run(() -> setRollersSetpointRpm(3500.0)).withName("Feeder Shot (Shooter)"); } public Command ampShot() { - return this.run(() -> setRollersSetpointRpm(kAmpshot)); + return this.run(() -> setRollersSetpointRpm(kAmpShot)); } public Command catchNote() { @@ -137,7 +152,7 @@ public Command catchNote() { m_io.setTopRollerVoltage(-9.0); m_io.setBottomRollerVoltage(-9.0); }) - .withName("Catch Note"); + .withName("Catch (Shooter)"); } public Command spit() { @@ -146,12 +161,6 @@ public Command spit() { m_io.setTopRollerVoltage(-6.0); m_io.setBottomRollerVoltage(-6.0); }) - .withName("Spit"); - } - - public Command shootWithFeederDelay() { - return this.run(() -> setRollersSetpointRpm(kFarShotVelocityRpm)) - .until(() -> getRollersAtSetpoint()) - .andThen(runShooter()); + .withName("Spit (Shooter)"); } } diff --git a/src/main/java/frc/robot/shooter/ShooterIO.java b/src/main/java/frc/robot/shooter/ShooterIO.java index f05ce8e..d98e872 100644 --- a/src/main/java/frc/robot/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/shooter/ShooterIO.java @@ -25,7 +25,9 @@ public default void setTopRollerVoltage(double volts) {} public default void setBottomRollerVoltage(double volts) {} - public default void stopRollers() {} + public default void setTopRollerVelocity(double velocityRps) {} + + public default void setBottomRollerVelocity(double velocityRps) {} - public default void setRollerDutyCycle(double volts) {} + public default void stopRollers() {} } diff --git a/src/main/java/frc/robot/shooter/ShooterIOSim.java b/src/main/java/frc/robot/shooter/ShooterIOSim.java index 49d8521..ea5267b 100644 --- a/src/main/java/frc/robot/shooter/ShooterIOSim.java +++ b/src/main/java/frc/robot/shooter/ShooterIOSim.java @@ -1,6 +1,8 @@ package frc.robot.shooter; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; @@ -20,13 +22,23 @@ public class ShooterIOSim implements ShooterIO { private final DCMotorSim m_bottomRollerSim = new DCMotorSim(m_bottomRollerPlant, m_rollerGearbox, 1.0); + private final SimpleMotorFeedforward m_topRollerFeedforward = + new SimpleMotorFeedforward(Shooter.topRollerkS, Shooter.topRollerkV, Shooter.topRollerkA); + private final PIDController m_topRollerController = + new PIDController(Shooter.topRollerkP, 0.0, 0.0); + private final SimpleMotorFeedforward m_bottomRollerFeedforward = + new SimpleMotorFeedforward( + Shooter.bottomRollerkS, Shooter.bottomRollerkV, Shooter.bottomRollerkA); + private final PIDController m_bottomRollerController = + new PIDController(Shooter.bottomRollerkP, 0.0, 0.0); + private double m_topRollerAppliedVolts = 0.0; private double m_bottomRollerAppliedVolts = 0.0; + public ShooterIOSim() {} + @Override public void updateInputs(ShooterIOInputs inputs) { - m_topRollerSim.setInputVoltage(m_topRollerAppliedVolts); - m_bottomRollerSim.setInputVoltage(m_bottomRollerAppliedVolts); m_topRollerSim.update(0.02); m_bottomRollerSim.update(0.02); @@ -57,11 +69,29 @@ public void updateInputs(ShooterIOInputs inputs) { @Override public void setTopRollerVoltage(double volts) { m_topRollerAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + m_topRollerSim.setInputVoltage(m_topRollerAppliedVolts); } @Override public void setBottomRollerVoltage(double volts) { m_bottomRollerAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); + m_bottomRollerSim.setInputVoltage(m_bottomRollerAppliedVolts); + } + + @Override + public void setTopRollerVelocity(double velocityRps) { + setTopRollerVoltage( + m_topRollerFeedforward.calculate(velocityRps) + + m_topRollerController.calculate( + m_topRollerSim.getAngularVelocityRadPerSec(), velocityRps)); + } + + @Override + public void setBottomRollerVelocity(double velocityRps) { + setBottomRollerVoltage( + m_bottomRollerFeedforward.calculate(velocityRps) + + m_bottomRollerController.calculate( + m_bottomRollerSim.getAngularVelocityRadPerSec(), velocityRps)); } @Override diff --git a/src/main/java/frc/robot/shooter/ShooterIOTalonFX.java b/src/main/java/frc/robot/shooter/ShooterIOTalonFX.java index 4503ae5..3e495bd 100644 --- a/src/main/java/frc/robot/shooter/ShooterIOTalonFX.java +++ b/src/main/java/frc/robot/shooter/ShooterIOTalonFX.java @@ -3,9 +3,10 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusCode; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotionMagicConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.DutyCycleOut; -import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage; import com.ctre.phoenix6.hardware.ParentDevice; import com.ctre.phoenix6.hardware.TalonFX; import frc.robot.Constants; @@ -13,7 +14,7 @@ public class ShooterIOTalonFX implements ShooterIO { private final TalonFX m_topRoller, m_bottomRoller; - private final VoltageOut m_topRollerControl, m_bottomRollerControl; + private final MotionMagicVelocityVoltage m_topRollerControl, m_bottomRollerControl; private final StatusSignal m_topRollerPositionSignal, m_topRollerVelocitySignal, m_topRollerAppliedVoltageSignal, @@ -27,22 +28,42 @@ public ShooterIOTalonFX() { m_topRoller = new TalonFX(20, Constants.kSuperstructureCanBus); m_bottomRoller = new TalonFX(21, Constants.kSuperstructureCanBus); + final MotionMagicConfigs motionMagicConfigs = + new MotionMagicConfigs() + .withMotionMagicAcceleration(Shooter.kFarShotVelocityRpm * (1 / 0.25)); + final CurrentLimitsConfigs currentLimitConfigs = + new CurrentLimitsConfigs().withSupplyCurrentLimit(60.0).withSupplyCurrentLimitEnable(true); final TalonFXConfiguration topRollerMotorConfig = new TalonFXConfiguration(); - m_topRoller.getConfigurator().apply(topRollerMotorConfig); + + topRollerMotorConfig.Slot0.kS = Shooter.topRollerkS; + topRollerMotorConfig.Slot0.kV = Shooter.topRollerkV; + topRollerMotorConfig.Slot0.kA = Shooter.topRollerkA; + topRollerMotorConfig.Slot0.kP = Shooter.topRollerkP; + topRollerMotorConfig.MotionMagic = motionMagicConfigs; + topRollerMotorConfig.CurrentLimits = currentLimitConfigs; + topRollerMotorConfig.Feedback.SensorToMechanismRatio = Shooter.kShooterStepUp; + m_topRoller.getConfigurator().apply(topRollerMotorConfig, 1.0); final TalonFXConfiguration bottomRollerMotorConfig = new TalonFXConfiguration(); - m_bottomRoller.getConfigurator().apply(bottomRollerMotorConfig); + bottomRollerMotorConfig.Slot0.kS = Shooter.bottomRollerkS; + bottomRollerMotorConfig.Slot0.kV = Shooter.bottomRollerkV; + bottomRollerMotorConfig.Slot0.kA = Shooter.bottomRollerkA; + bottomRollerMotorConfig.Slot0.kP = Shooter.bottomRollerkP; + bottomRollerMotorConfig.MotionMagic = motionMagicConfigs; + bottomRollerMotorConfig.CurrentLimits = currentLimitConfigs; + bottomRollerMotorConfig.Feedback.SensorToMechanismRatio = Shooter.kShooterStepUp; + m_bottomRoller.getConfigurator().apply(bottomRollerMotorConfig, 1.0); m_topRollerPositionSignal = m_topRoller.getPosition(); m_topRollerVelocitySignal = m_topRoller.getVelocity(); m_topRollerAppliedVoltageSignal = m_topRoller.getMotorVoltage(); - m_topRollerCurrentSignal = m_topRoller.getStatorCurrent(); + m_topRollerCurrentSignal = m_topRoller.getSupplyCurrent(); m_bottomRollerPositionSignal = m_bottomRoller.getPosition(); m_bottomRollerVelocitySignal = m_bottomRoller.getVelocity(); m_bottomRollerAppliedVoltageSignal = m_bottomRoller.getMotorVoltage(); - m_bottomRollerCurrentSignal = m_bottomRoller.getStatorCurrent(); + m_bottomRollerCurrentSignal = m_bottomRoller.getSupplyCurrent(); BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, + 100.0, m_topRollerVelocitySignal, m_topRollerAppliedVoltageSignal, m_topRollerCurrentSignal, @@ -52,8 +73,8 @@ public ShooterIOTalonFX() { ParentDevice.optimizeBusUtilizationForAll(m_topRoller, m_bottomRoller); - m_topRollerControl = new VoltageOut(0, true, false, false, false).withUpdateFreqHz(0.0); - m_bottomRollerControl = new VoltageOut(0, true, false, false, false).withUpdateFreqHz(0.0); + m_topRollerControl = new MotionMagicVelocityVoltage(0, 0, true, 0, 0, false, false, false); + m_bottomRollerControl = new MotionMagicVelocityVoltage(0, 0, true, 0, 0, false, false, false); } @Override @@ -82,21 +103,26 @@ public void updateInputs(ShooterIOInputs inputs) { @Override public void setTopRollerVoltage(double volts) { - m_topRoller.setControl(m_topRollerControl.withOutput(volts)); + m_topRoller.setVoltage(volts); } @Override public void setBottomRollerVoltage(double volts) { - m_bottomRoller.setControl(m_bottomRollerControl.withOutput(volts)); + m_bottomRoller.setVoltage(volts); + } + + @Override + public void setTopRollerVelocity(double velocityRps) { + m_topRoller.setControl(m_topRollerControl.withVelocity(velocityRps)); + } + + @Override + public void setBottomRollerVelocity(double velocityRps) { + m_bottomRoller.setControl(m_bottomRollerControl.withVelocity(velocityRps)); } public void stopRollers() { m_topRoller.setVoltage(0.0); m_bottomRoller.setVoltage(0.0); } - - public void setRollerDutyCycle(double volts) { - m_topRoller.setControl(new DutyCycleOut(volts)); - m_bottomRoller.setControl(new DutyCycleOut(volts)); - } } diff --git a/src/main/java/frc/robot/vision/GamePieceDetection.java b/src/main/java/frc/robot/vision/GamePieceDetection.java new file mode 100644 index 0000000..41831dd --- /dev/null +++ b/src/main/java/frc/robot/vision/GamePieceDetection.java @@ -0,0 +1,43 @@ +package frc.robot.vision; + +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; + +public class GamePieceDetection { + private final double kMinimumTargetAreaPercent = 0.8; + private final GamePieceDetectionIO m_io; + private final GamePieceDetectionIOInputsAutoLogged m_inputs = + new GamePieceDetectionIOInputsAutoLogged(); + + public final Trigger hasValidTarget = + new Trigger( + () -> + m_inputs.connected + && m_inputs.hasTarget + && m_inputs.targetArea > kMinimumTargetAreaPercent); + public final DoubleSupplier horizontalErrorDeg = + () -> + m_inputs.targetHorizontalOffsetDegrees + * (Constants.onRedAllianceSupplier.getAsBoolean() ? 1 : -1); + + public GamePieceDetection(GamePieceDetectionIO io) { + m_io = io; + } + + public void periodic() { + m_io.updateInputs(m_inputs); + Logger.processInputs("VisionInputs/Limelight", m_inputs); + Logger.recordOutput("GamePieceDetection/ValidTarget", hasValidTarget.getAsBoolean()); + Logger.recordOutput("GamePieceDetection/HorizontalErrorDeg", horizontalErrorDeg.getAsDouble()); + } + + public double getHorizontalOffsetDeg() { + return horizontalErrorDeg.getAsDouble(); + } + + public double getVerticalOffsetDeg() { + return m_inputs.targetVerticalOffsetDegrees; + } +} diff --git a/src/main/java/frc/robot/vision/GamePieceDetectionIO.java b/src/main/java/frc/robot/vision/GamePieceDetectionIO.java new file mode 100644 index 0000000..11705ad --- /dev/null +++ b/src/main/java/frc/robot/vision/GamePieceDetectionIO.java @@ -0,0 +1,18 @@ +package frc.robot.vision; + +import org.littletonrobotics.junction.AutoLog; + +public interface GamePieceDetectionIO { + + @AutoLog + public static class GamePieceDetectionIOInputs { + public boolean hasTarget = false; + public double targetArea = 0.0; + public double targetHorizontalOffsetDegrees = 0.0; + public double targetVerticalOffsetDegrees = 0.0; + public int fps; + public boolean connected = false; + } + + public default void updateInputs(GamePieceDetectionIOInputs inputs) {} +} diff --git a/src/main/java/frc/robot/vision/GamePieceDetectionIOLimelight.java b/src/main/java/frc/robot/vision/GamePieceDetectionIOLimelight.java new file mode 100644 index 0000000..04251d2 --- /dev/null +++ b/src/main/java/frc/robot/vision/GamePieceDetectionIOLimelight.java @@ -0,0 +1,36 @@ +package frc.robot.vision; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.Timer; + +public class GamePieceDetectionIOLimelight implements GamePieceDetectionIO { + + private final NetworkTable m_limelightTable; + private final double kDisconnectedTimeoutSeconds = 0.5; + private final Timer m_disconnectedTimer = new Timer(); + private double m_lastHeartbeatValue = 0.0; + + public GamePieceDetectionIOLimelight() { + m_limelightTable = NetworkTableInstance.getDefault().getTable("limelight"); + m_limelightTable.getEntry("pipeline").setNumber(0); + } + + @Override + public void updateInputs(GamePieceDetectionIOInputs inputs) { + inputs.hasTarget = m_limelightTable.getEntry("tv").getDouble(0) == 1.0; + inputs.targetArea = m_limelightTable.getEntry("ta").getDouble(0.0); + inputs.targetHorizontalOffsetDegrees = m_limelightTable.getEntry("tx").getDouble(0.0); + inputs.targetVerticalOffsetDegrees = m_limelightTable.getEntry("ty").getDouble(0.0); + inputs.fps = (int) m_limelightTable.getEntry("hw").getDoubleArray(new double[] {0.0})[3]; + + double currentHeartbeatValue = m_limelightTable.getEntry("hb").getDouble(0.0); + if (m_lastHeartbeatValue != currentHeartbeatValue) { + m_lastHeartbeatValue = currentHeartbeatValue; + inputs.connected = true; + m_disconnectedTimer.reset(); + } else if (m_disconnectedTimer.hasElapsed(kDisconnectedTimeoutSeconds)) { + inputs.connected = false; + } + } +} diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index 7b9afbc..494732e 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/src/main/java/frc/robot/vision/Vision.java @@ -5,18 +5,19 @@ import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; 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.Transform2d; 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.DriverStation; import edu.wpi.first.wpilibj.Timer; import frc.robot.Constants; +import frc.robot.FieldConstants; import frc.robot.PoseEstimation; import frc.robot.vision.VisionIO.VisionIOInputs; import java.util.ArrayList; @@ -25,7 +26,7 @@ import java.util.List; import java.util.Map; import java.util.Optional; -import java.util.stream.Collectors; +import java.util.function.UnaryOperator; import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; @@ -41,20 +42,26 @@ public class Vision { private static final double kTargetLogTimeSecs = 0.1; + private final double kXYStdDevCoefficient = 0.005; + private final double kThetaStdDevCoefficient = 0.01; + private final double kFieldBorderMargin = 0.5; + private final double kZMargin = 0.75; // #region Set these to false to disable vision measurements; private final boolean kTrustVisionXY = true; private final boolean kTrustVisionTheta = true; private final boolean kIgnoreVisionInSim = true; - private final boolean kIgnoreVisionInAuto = true; + private final boolean kIgnoreVisionInAuto = Constants.kIsViper ? true : false; // #endregion private final VisionIO[] io; private final VisionIOInputs[] m_inputs; private final PhotonPoseEstimator[] m_poseEstimators; private VisionSystemSim m_visionSimSystem = null; - private Map m_lastDetectionTimeIds = new HashMap<>(); - private ArrayList m_newVisionUpdates; - private final double kXYStdDevCoefficient = 0.1; - private final double kThetaStdDevCoefficient = 0.1; + private final Map m_lastDetectionTimeIds = new HashMap<>(); + private final ArrayList m_newVisionUpdates = new ArrayList<>(); + private final List m_allEstimatedPosesToLog = new ArrayList<>(); + private final List m_validEstimatedPosesToLog = new ArrayList<>(); + private final UnaryOperator applyPoseOffset = + pose -> pose.transformBy(new Transform3d(Constants.poseOffset, new Rotation3d())); public Vision(VisionIO... io) { this.io = io; @@ -75,7 +82,7 @@ public Vision(VisionIO... io) { m_visionSimSystem = new VisionSystemSim("sim_vision_system"); m_visionSimSystem.addAprilTags(fieldLayout); for (int index = 0; index < io.length; index++) { - PhotonCameraSim cameraSim = + final PhotonCameraSim cameraSim = new PhotonCameraSim( new PhotonCamera(m_inputs[index].cameraName), getSimCameraProperties()); cameraSim.enableDrawWireframe(true); @@ -83,12 +90,7 @@ public Vision(VisionIO... io) { } } - fieldLayout - .getTags() - .forEach( - (AprilTag tag) -> { - m_lastDetectionTimeIds.put(tag.ID, 0.0); - }); + fieldLayout.getTags().forEach((AprilTag tag) -> m_lastDetectionTimeIds.put(tag.ID, 0.0)); } public void periodic() { @@ -97,8 +99,9 @@ public void periodic() { Logger.processInputs("VisionInputs/Cam" + Integer.toString(index), m_inputs[index]); } - m_newVisionUpdates = new ArrayList<>(); - final List estimatedPosesToLog = new ArrayList<>(); + m_newVisionUpdates.clear(); + m_allEstimatedPosesToLog.clear(); + m_validEstimatedPosesToLog.clear(); for (int ioIndex = 0; ioIndex < io.length; ioIndex++) { for (int resultIndex = 0; resultIndex < m_inputs[ioIndex].results.length; resultIndex++) { final byte[] rawResult = m_inputs[ioIndex].results[resultIndex]; @@ -114,8 +117,8 @@ public void periodic() { final Optional poseOptional = m_poseEstimators[ioIndex].update(result); poseOptional.ifPresent( pose -> { - Pose3d estimatedPose = pose.estimatedPose; - List targets = result.getTargets(); + final Pose3d estimatedPose = pose.estimatedPose; + final List targets = result.getTargets(); targets.forEach( target -> { fieldLayout @@ -123,18 +126,17 @@ public void periodic() { .ifPresent( tagPose -> m_lastDetectionTimeIds.put( - target.getFiducialId(), Timer.getFPGATimestamp())); + target.getFiducialId(), + // The timestamp used here is not important, as it is only used + // for visualization purposes. + Timer.getFPGATimestamp())); }); - estimatedPosesToLog.add( - estimatedPose.plus( - new Transform3d( - new Translation3d(Units.inchesToMeters(2.5), 0.0, 0.0), - new Rotation3d()))); + m_allEstimatedPosesToLog.add(estimatedPose); m_newVisionUpdates.add( new VisionUpdate( estimatedPose.toPose2d(), result.getTimestampSeconds(), - getEstimationStdDevs(estimatedPose.toPose2d(), targets))); + getEstimationStdDevs(estimatedPose, targets))); }); } @@ -145,27 +147,41 @@ public void periodic() { } Logger.recordOutput( - "Vision/EstimatedPoses", - estimatedPosesToLog.toArray(new Pose3d[estimatedPosesToLog.size()])); - List targetPose3ds = new ArrayList<>(); + "Vision/InvalidEstimatedPoses", + m_allEstimatedPosesToLog.stream().map(applyPoseOffset).toArray(Pose3d[]::new)); + Logger.recordOutput( + "Vision/ValidEstimatedPoses", + m_validEstimatedPosesToLog.stream().map(applyPoseOffset).toArray(Pose3d[]::new)); + final List targetPose3ds = new ArrayList<>(); for (Map.Entry detectionEntry : m_lastDetectionTimeIds.entrySet()) if (Timer.getFPGATimestamp() - detectionEntry.getValue() < kTargetLogTimeSecs) fieldLayout.getTagPose(detectionEntry.getKey()).ifPresent(pose -> targetPose3ds.add(pose)); - Logger.recordOutput("Vision/TagPoses", targetPose3ds.toArray(new Pose3d[targetPose3ds.size()])); + Logger.recordOutput("Vision/TagPoses", targetPose3ds.toArray(Pose3d[]::new)); Logger.recordOutput( "Vision/TagPoses2D", - targetPose3ds.stream() - .map(pose3d -> pose3d.toPose2d()) - .collect(Collectors.toList()) - .toArray(new Pose2d[targetPose3ds.size()])); + targetPose3ds.stream().map(pose3d -> pose3d.toPose2d()).toArray(Pose2d[]::new)); } public Matrix getEstimationStdDevs( - Pose2d estimatedPose, List targets) { + Pose3d estimatedPose, List targets) { + final Vector rejectMeasurement = + VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + final boolean moving = + Math.abs(PoseEstimation.getInstance().getFieldVelocity().dx) >= 1.5 + || Math.abs(PoseEstimation.getInstance().getFieldVelocity().dy) >= 1.5 + || Math.abs(PoseEstimation.getInstance().getFieldVelocity().dtheta) >= 1.5; if ((Constants.kIsSim && kIgnoreVisionInSim) - || (DriverStation.isAutonomous() && kIgnoreVisionInAuto)) - return VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + || (DriverStation.isAutonomous() && (kIgnoreVisionInAuto || moving))) + return rejectMeasurement; + + if (estimatedPose.getX() < -kFieldBorderMargin + || estimatedPose.getX() > FieldConstants.kFieldLength + kFieldBorderMargin + || estimatedPose.getY() < -kFieldBorderMargin + || estimatedPose.getY() > FieldConstants.kFieldWidth + kFieldBorderMargin + || estimatedPose.getZ() < -kZMargin + || estimatedPose.getZ() > kZMargin) return rejectMeasurement; + int tagCount = 0; double totalDistance = 0; for (PhotonTrackedTarget target : targets) { @@ -174,42 +190,51 @@ public Matrix getEstimationStdDevs( if (tagPose.isEmpty()) continue; tagCount++; totalDistance += - tagPose.get().toPose2d().getTranslation().getDistance(estimatedPose.getTranslation()); + tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.toPose2d().getTranslation()); } - double averageDistance = totalDistance / tagCount; + final double averageDistance = totalDistance / tagCount; /** * Reject tags is there are no valid tags OR there is only one tag and it has a high pose * ambiguity OR there is only one tag and the tag is too far. */ if ((tagCount == 0) - || (tagCount == 1 && targets.get(0).getPoseAmbiguity() > 0.25) - || (tagCount == 1 && averageDistance > 4)) - return VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + || (tagCount == 1 && targets.get(0).getPoseAmbiguity() > 0.15) + || (tagCount == 1 && averageDistance > 4)) return rejectMeasurement; - double xyStdDev = + final double xyStdDev = kTrustVisionXY ? kXYStdDevCoefficient * Math.pow(averageDistance, 2) / tagCount : Double.MAX_VALUE; - double thetaStdDev = + final double thetaStdDev = kTrustVisionTheta ? kThetaStdDevCoefficient * Math.pow(averageDistance, 2) / tagCount : Double.MAX_VALUE; + + m_allEstimatedPosesToLog.remove(estimatedPose); + m_validEstimatedPosesToLog.add(estimatedPose); return VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev); } private SimCameraProperties getSimCameraProperties() { final SimCameraProperties cameraProperties = new SimCameraProperties(); cameraProperties.setCalibration(1280, 720, Rotation2d.fromDegrees(75)); - cameraProperties.setCalibError(0.35, 0.10); + cameraProperties.setCalibError(0.15, 0.05); cameraProperties.setFPS(20); - cameraProperties.setAvgLatencyMs(50); + cameraProperties.setAvgLatencyMs(30); cameraProperties.setLatencyStdDevMs(15); return cameraProperties; } public void updateSim(Pose2d currentPose) { - m_visionSimSystem.update(currentPose); + m_visionSimSystem.update( + currentPose.transformBy( + new Transform2d( + Constants.poseOffset.toTranslation2d().unaryMinus(), new Rotation2d()))); } public static record VisionUpdate( diff --git a/src/main/java/frc/robot/vision/VisionIOPhotonVision.java b/src/main/java/frc/robot/vision/VisionIOPhotonVision.java index c0a8583..1d1b148 100644 --- a/src/main/java/frc/robot/vision/VisionIOPhotonVision.java +++ b/src/main/java/frc/robot/vision/VisionIOPhotonVision.java @@ -9,9 +9,9 @@ public class VisionIOPhotonVision implements VisionIO { - private static final double kDisconnectedTimeout = 0.5; - private RawSubscriber m_photonDataSubscriber; + private final double kDisconnectedTimeoutSeconds = 0.5; private final Timer m_disconnectedTimer = new Timer(); + private RawSubscriber m_photonDataSubscriber; private final String m_cameraName; private final Transform3d m_robotToCamera; @@ -44,7 +44,7 @@ public void updateInputs(VisionIOInputs inputs) { if (dataQueue.length > 0) { inputs.connected = true; m_disconnectedTimer.reset(); - } else if (m_disconnectedTimer.hasElapsed(kDisconnectedTimeout)) { + } else if (m_disconnectedTimer.hasElapsed(kDisconnectedTimeoutSeconds)) { inputs.connected = false; } } diff --git a/vendordeps/ChoreoLib.json b/vendordeps/ChoreoLib.json index 8fbf7ab..5480d50 100644 --- a/vendordeps/ChoreoLib.json +++ b/vendordeps/ChoreoLib.json @@ -1,18 +1,19 @@ { "fileName": "ChoreoLib.json", "name": "ChoreoLib", - "version": "2024.1.3", + "version": "2024.2.3", "uuid": "287cff6e-1b60-4412-8059-f6834fb30e30", "frcYear": "2024", "mavenUrls": [ - "https://SleipnirGroup.github.io/ChoreoLib/dep" + "https://lib.choreo.autos/dep", + "https://repo1.maven.org/maven2" ], - "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib.json", + "jsonUrl": "https://lib.choreo.autos/dep/ChoreoLib.json", "javaDependencies": [ { "groupId": "com.choreo.lib", "artifactId": "ChoreoLib-java", - "version": "2024.1.3" + "version": "2024.2.3" }, { "groupId": "com.google.code.gson", @@ -25,7 +26,7 @@ { "groupId": "com.choreo.lib", "artifactId": "ChoreoLib-cpp", - "version": "2024.1.3", + "version": "2024.2.3", "libName": "ChoreoLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 0cd337c..f528a51 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.6", + "version": "2024.2.8", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.6" + "version": "2024.2.8" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.6", + "version": "2024.2.8", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index e9113c8..1f0cf3a 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.2.0", + "version": "24.3.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.2.0" + "version": "24.3.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 237b5e3..9eda5ac 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.3", + "version": "2024.2.4", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.3" + "version": "2024.2.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index cba2533..bef469e 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.2.10", + "version": "v2024.3.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2024", "mavenUrls": [ @@ -14,7 +14,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.2.10", + "version": "v2024.3.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +29,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.2.10", + "version": "v2024.3.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +46,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.2.10" + "version": "v2024.3.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.2.10" + "version": "v2024.3.1" } ] }