From 15e2b3f4b5001cc671de4b4f0606894c53fb2b23 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Fri, 5 Apr 2024 15:02:29 -0700 Subject: [PATCH] 3 to center --- src/main/ChoreoDoc.chor | 1896 +++++++++++------ src/main/deploy/choreo/6Y.traj | 265 +++ src/main/deploy/choreo/O14.traj | 238 +++ .../robot/commands/auton/AutonBuilder.java | 22 + 4 files changed, 1821 insertions(+), 600 deletions(-) create mode 100644 src/main/deploy/choreo/6Y.traj create mode 100644 src/main/deploy/choreo/O14.traj diff --git a/src/main/ChoreoDoc.chor b/src/main/ChoreoDoc.chor index f2b1f123..a1bd7dfa 100644 --- a/src/main/ChoreoDoc.chor +++ b/src/main/ChoreoDoc.chor @@ -430,6 +430,246 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, + "14": { + "waypoints": [ + { + "x": 2.9, + "y": 6.113, + "heading": 0.2425, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 + }, + { + "x": 7.179388046264648, + "y": 7.282280921936035, + "heading": 0.1932113056799297, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 2.9, + "y": 6.113, + "heading": 0.2425, + "angularVelocity": 0, + "velocityX": -1.7262570035586016e-28, + "velocityY": 1.362097567492257e-31, + "timestamp": 0 + }, + { + "x": 2.938903531068027, + "y": 6.123629827485452, + "heading": 0.2420519032802004, + "angularVelocity": -0.00453932999729365, + "velocityX": 0.3941023394250247, + "velocityY": 0.10768276721142236, + "timestamp": 0.09871428605266704 + }, + { + "x": 3.016710592745661, + "y": 6.144889482331082, + "heading": 0.2411557122702183, + "angularVelocity": -0.009078635381911062, + "velocityX": 0.7882046742061307, + "velocityY": 0.2153655331538105, + "timestamp": 0.19742857210533407 + }, + { + "x": 3.1334211844598885, + "y": 6.176778964380304, + "heading": 0.23981143000413516, + "angularVelocity": -0.013617910030465686, + "velocityX": 1.1823070031824825, + "velocityY": 0.3230482975099222, + "timestamp": 0.2961428581580011 + }, + { + "x": 3.2890353054740027, + "y": 6.219298273431795, + "heading": 0.2380190603780691, + "angularVelocity": -0.01815714521029317, + "velocityX": 1.5764093246958044, + "velocityY": 0.430731059826591, + "timestamp": 0.39485714421066814 + }, + { + "x": 3.4835529548057638, + "y": 6.272447409217136, + "heading": 0.23577860857806224, + "angularVelocity": -0.02269632785411698, + "velocityX": 1.9705116362588007, + "velocityY": 0.5384138194241223, + "timestamp": 0.49357143026333516 + }, + { + "x": 3.7169741310801077, + "y": 6.336226371360569, + "heading": 0.23309008184694724, + "angularVelocity": -0.02723543712609413, + "velocityX": 2.364613933892051, + "velocityY": 0.6460965752151011, + "timestamp": 0.5922857163160022 + }, + { + "x": 3.989298832234584, + "y": 6.4106351592985, + "heading": 0.2299534910085373, + "angularVelocity": -0.031774436749927436, + "velocityX": 2.7587162106321825, + "velocityY": 0.7537793252967542, + "timestamp": 0.6910000023686693 + }, + { + "x": 4.300527054832155, + "y": 6.4956737720917275, + "heading": 0.22636885398872517, + "angularVelocity": -0.03631325477963944, + "velocityX": 3.1528184525542584, + "velocityY": 0.8614620658640751, + "timestamp": 0.7897142884213364 + }, + { + "x": 4.65065879200003, + "y": 6.591342207862238, + "heading": 0.22233620618042166, + "angularVelocity": -0.04085171426963709, + "velocityX": 3.546920624853299, + "velocityY": 0.969144787406631, + "timestamp": 0.8884285744740035 + }, + { + "x": 5.039694023127982, + "y": 6.697640460978281, + "heading": 0.21785564962207069, + "angularVelocity": -0.04538914008275375, + "velocityX": 3.9410225883656986, + "velocityY": 1.0768274518982093, + "timestamp": 0.9871428605266706 + }, + { + "x": 5.428729254257294, + "y": 6.803938714091145, + "heading": 0.21337509407175914, + "angularVelocity": -0.04538912987106772, + "velocityX": 3.9410225883794716, + "velocityY": 1.0768274518659884, + "timestamp": 1.0858571465793376 + }, + { + "x": 5.778860991426467, + "y": 6.899607149858598, + "heading": 0.20934244723245, + "angularVelocity": -0.04085170445348799, + "velocityX": 3.5469206248664467, + "velocityY": 0.9691447873756708, + "timestamp": 1.1845714326320047 + }, + { + "x": 6.090089214025261, + "y": 6.984645762648941, + "heading": 0.20575781112613004, + "angularVelocity": -0.036313245525740584, + "velocityX": 3.1528184525666556, + "velocityY": 0.8614620658348401, + "timestamp": 1.2832857186846718 + }, + { + "x": 6.3624139151808645, + "y": 7.059054550584212, + "heading": 0.2026212211265867, + "angularVelocity": -0.03177442825200412, + "velocityX": 2.7587162106435894, + "velocityY": 0.7537793252698134, + "timestamp": 1.3820000047373389 + }, + { + "x": 6.595835091456214, + "y": 7.122833512725263, + "heading": 0.19993269514249148, + "angularVelocity": -0.027235429558601355, + "velocityX": 2.364613933902241, + "velocityY": 0.646096575190986, + "timestamp": 1.480714290790006 + }, + { + "x": 6.790352740788842, + "y": 7.17598264850855, + "heading": 0.19769224398322777, + "angularVelocity": -0.02269632136323262, + "velocityX": 1.9705116362675763, + "velocityY": 0.5384138194032987, + "timestamp": 1.579428576842673 + }, + { + "x": 6.945966861803667, + "y": 7.218501957558349, + "heading": 0.19589987488023405, + "angularVelocity": -0.01815713991144325, + "velocityX": 1.5764093247030033, + "velocityY": 0.43073105980945287, + "timestamp": 1.67814286289534 + }, + { + "x": 7.062677453518438, + "y": 7.2503914396062745, + "heading": 0.19455559301125175, + "angularVelocity": -0.013617906007737414, + "velocityX": 1.1823070031879788, + "velocityY": 0.32304829749678704, + "timestamp": 1.7768571489480072 + }, + { + "x": 7.140484515196437, + "y": 7.271651094451027, + "heading": 0.19365940226718134, + "angularVelocity": -0.009078632688161059, + "velocityX": 0.7882046742098351, + "velocityY": 0.21536553314491833, + "timestamp": 1.8755714350006742 + }, + { + "x": 7.179388046264648, + "y": 7.282280921936035, + "heading": 0.1932113056799297, + "angularVelocity": -0.004539328654550442, + "velocityX": 0.39410233942688466, + "velocityY": 0.10768276720693493, + "timestamp": 1.9742857210533413 + }, + { + "x": 7.179388046264648, + "y": 7.282280921936035, + "heading": 0.1932113056799297, + "angularVelocity": 0, + "velocityX": -3.174166790829149e-31, + "velocityY": 0, + "timestamp": 2.073000007106008 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, "21": { "waypoints": [ { @@ -3380,12 +3620,12 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 17 + "controlIntervalCount": 14 }, { - "x": 1.9, - "y": 7.01, - "heading": 0, + "x": 1.9784302711486816, + "y": 6.483112812042236, + "heading": 0.535, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -3397,163 +3637,136 @@ "x": 0.735704243183136, "y": 6.708577632904053, "heading": 1.05, - "angularVelocity": 5.129859183698386e-34, - "velocityX": -1.897188232792028e-32, + "angularVelocity": -3.2052264357833964e-33, + "velocityX": 0, "velocityY": 0, "timestamp": 0 }, { - "x": 0.7518942164195387, - "y": 6.712774885224698, - "heading": 1.0360266067067383, - "angularVelocity": -0.2176708788653528, - "velocityX": 0.2521997076310485, - "velocityY": 0.06538280160588324, - "timestamp": 0.06419505156638627 - }, - { - "x": 0.7842732990077126, - "y": 6.7211732237608, - "heading": 1.0080810493162287, - "angularVelocity": -0.4353226098994694, - "velocityX": 0.5043859580779306, - "velocityY": 0.13082532580283415, - "timestamp": 0.12839010313277255 - }, - { - "x": 0.8328395869801072, - "y": 6.733778092662747, - "heading": 0.9661471413335745, - "angularVelocity": -0.6532264864572923, - "velocityX": 0.7565425494233072, - "velocityY": 0.19635265638678046, - "timestamp": 0.19258515469915882 - }, - { - "x": 0.8975895806447234, - "y": 6.750595971117333, - "heading": 0.9101723598037398, - "angularVelocity": -0.8719485406433664, - "velocityX": 1.0086446242302025, - "velocityY": 0.2619809166629292, - "timestamp": 0.2567802062655451 - }, - { - "x": 0.9785176746344029, - "y": 6.771633282588873, - "heading": 0.8400465790839613, - "angularVelocity": -1.0923860797473, - "velocityX": 1.2606593812918574, - "velocityY": 0.3277092386129448, - "timestamp": 0.32097525783193137 - }, - { - "x": 1.0756159936179712, - "y": 6.796894668273899, - "heading": 0.75558565761099, - "angularVelocity": -1.3156920885970516, - "velocityX": 1.5125514601877916, - "velocityY": 0.3935098589164984, - "timestamp": 0.38517030939831765 - }, - { - "x": 1.1888749801301643, - "y": 6.826380733124152, - "heading": 0.6565316471315795, - "angularVelocity": -1.5430162927274755, - "velocityX": 1.764294657432715, - "velocityY": 0.4593199028707055, - "timestamp": 0.4493653609647039 - }, - { - "x": 1.3182851194837182, - "y": 6.860085809037716, - "heading": 0.5425841203452523, - "angularVelocity": -1.7750204105451466, - "velocityX": 2.0158896394019683, - "velocityY": 0.5250416518274347, - "timestamp": 0.5135604125310902 - }, - { - "x": 1.4475274189498422, - "y": 6.8934428420016385, - "heading": 0.4215466833591603, - "angularVelocity": -1.8854636616486855, - "velocityX": 2.0132751094135295, - "velocityY": 0.5196200041903175, - "timestamp": 0.5777554640974765 - }, - { - "x": 1.5606259047746505, - "y": 6.922606418122232, - "heading": 0.31578482859238616, - "angularVelocity": -1.6475079026519317, - "velocityX": 1.761794454014094, - "velocityY": 0.45429632672598025, - "timestamp": 0.6419505156638627 - }, - { - "x": 1.6575780935492714, - "y": 6.94758845811189, - "heading": 0.22531533794231037, - "angularVelocity": -1.4092907232345635, - "velocityX": 1.5102751132516714, - "velocityY": 0.3891583444531321, - "timestamp": 0.706145567230249 - }, - { - "x": 1.738379613314278, - "y": 6.968397960555784, - "heading": 0.15008332290666232, - "angularVelocity": -1.1719285708158644, - "velocityX": 1.2586876681833796, - "velocityY": 0.3241605378628613, - "timestamp": 0.7703406187966353 - }, - { - "x": 1.8030257964282208, - "y": 6.98504096016579, - "heading": 0.09000459900753742, - "angularVelocity": -0.93587780417925, - "velocityX": 1.0070275128152204, - "velocityY": 0.25925673714578995, - "timestamp": 0.8345356703630216 - }, - { - "x": 1.851512723927302, - "y": 6.9975211192975015, - "heading": 0.044997689562143374, - "angularVelocity": -0.7010962425795362, - "velocityX": 0.7553063096917884, - "velocityY": 0.19440998686332248, - "timestamp": 0.8987307219294078 - }, - { - "x": 1.883837797047269, - "y": 7.0058404673101915, - "heading": 0.015004206753892985, - "angularVelocity": -0.46722421863361946, - "velocityX": 0.5035446242540792, - "velocityY": 0.12959484897502807, - "timestamp": 0.9629257734957941 - }, - { - "x": 1.9, - "y": 7.01, - "heading": 1.0124926487877066e-33, - "angularVelocity": -0.23372840098705003, - "velocityX": 0.2517671153518252, - "velocityY": 0.06479522312567318, - "timestamp": 1.0271208250621804 - }, - { - "x": 1.9, - "y": 7.01, - "heading": 0, - "angularVelocity": -2.3076499101828583e-32, - "velocityX": -9.966354466455041e-35, - "velocityY": 0, - "timestamp": 1.0913158766285667 + "x": 0.761064337643975, + "y": 6.703974702961187, + "heading": 1.0394085866051899, + "angularVelocity": -0.13389480430550454, + "velocityX": 0.3205978993008612, + "velocityY": -0.058189439025577605, + "timestamp": 0.07910249729066414 + }, + { + "x": 0.8117846303959374, + "y": 6.694768796125527, + "heading": 1.0182331720265752, + "angularVelocity": -0.2676959047298492, + "velocityX": 0.6411971112061043, + "velocityY": -0.11637947158397395, + "timestamp": 0.15820499458132828 + }, + { + "x": 0.8878653305388823, + "y": 6.680959872996509, + "heading": 0.9864869362485963, + "angularVelocity": -0.40133038608536603, + "velocityX": 0.9617989665153607, + "velocityY": -0.17457000223742059, + "timestamp": 0.2373074918719924 + }, + { + "x": 0.9893067581195191, + "y": 6.662547949391534, + "heading": 0.9441886485207825, + "angularVelocity": -0.5347275898558242, + "velocityX": 1.2824048677994033, + "velocityY": -0.23276033293007722, + "timestamp": 0.31640998916265656 + }, + { + "x": 1.1161093426409772, + "y": 6.639533162216395, + "heading": 0.8913618933378138, + "angularVelocity": -0.6678266425503051, + "velocityX": 1.6030161987872353, + "velocityY": -0.2909489328835039, + "timestamp": 0.3955124864533207 + }, + { + "x": 1.2682736197755877, + "y": 6.61191586671661, + "heading": 0.82803387429705, + "angularVelocity": -0.8005817921027615, + "velocityX": 1.9236343016514252, + "velocityY": -0.34913304188493594, + "timestamp": 0.47461498374398486 + }, + { + "x": 1.4458123214779228, + "y": 6.5797129796333005, + "heading": 0.7547732843458872, + "angularVelocity": -0.9261476244165204, + "velocityX": 2.2444133596688465, + "velocityY": -0.4071032923901237, + "timestamp": 0.553717481034649 + }, + { + "x": 1.5979887104865267, + "y": 6.552111718249667, + "heading": 0.6919847501093475, + "angularVelocity": -0.7937617191252718, + "velocityX": 1.9237874178539225, + "velocityY": -0.3489303413798874, + "timestamp": 0.6328199783253131 + }, + { + "x": 1.7248025102359055, + "y": 6.529111475679183, + "heading": 0.6396601519231939, + "angularVelocity": -0.6614784612157752, + "velocityX": 1.60315797974618, + "velocityY": -0.2907650625234928, + "timestamp": 0.7119224756159772 + }, + { + "x": 1.8262535980988925, + "y": 6.510711767453513, + "heading": 0.5977979127482742, + "angularVelocity": -0.5292151399606996, + "velocityX": 1.2825269914071422, + "velocityY": -0.23260590823144406, + "timestamp": 0.7910249729066413 + }, + { + "x": 1.9023419276714661, + "y": 6.496912228683403, + "heading": 0.5663993287229007, + "angularVelocity": -0.39693543315072166, + "velocityX": 0.9618954164365399, + "velocityY": -0.1744513668058135, + "timestamp": 0.8701274701973054 + }, + { + "x": 1.9530674867918099, + "y": 6.487712617138059, + "heading": 0.545466318050499, + "angularVelocity": -0.2646314767469699, + "velocityX": 0.6412636877183707, + "velocityY": -0.11629988761973552, + "timestamp": 0.9492299674879695 + }, + { + "x": 1.9784302711486816, + "y": 6.483112812042236, + "heading": 0.535, + "angularVelocity": -0.13231337074024702, + "velocityX": 0.32063190449823076, + "velocityY": -0.05814993525325002, + "timestamp": 1.0283324647786336 + }, + { + "x": 1.9784302711486816, + "y": 6.483112812042236, + "heading": 0.535, + "angularVelocity": 2.6777041505112823e-32, + "velocityX": 0, + "velocityY": -2.6634846912610644e-34, + "timestamp": 1.1074349620692978 } ], "constraints": [ @@ -4661,12 +4874,12 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 14 }, { - "x": 1.9, - "y": 4.11, - "heading": 0, + "x": 2.018590211868286, + "y": 4.608981132507324, + "heading": -0.525, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -4678,154 +4891,136 @@ "x": 0.7390319108963013, "y": 4.394268035888672, "heading": -1.05, - "angularVelocity": -1.1172014722037894e-31, + "angularVelocity": -1.42039595844255e-32, "velocityX": 0, "velocityY": 0, "timestamp": 0 }, { - "x": 0.7571933032499027, - "y": 4.389818892605314, - "heading": -1.0342336922831046, - "angularVelocity": 0.23223812416934533, - "velocityX": 0.2675177833795592, - "velocityY": -0.06553599668617616, - "timestamp": 0.06788854230237745 - }, - { - "x": 0.7935151251431718, - "y": 4.38091553593728, - "heading": -1.002705893464414, - "angularVelocity": 0.4644052994725502, - "velocityX": 0.5350213844847446, - "velocityY": -0.13114667609709774, - "timestamp": 0.1357770846047549 - }, - { - "x": 0.8479950673400368, - "y": 4.367550754321936, - "heading": -0.9553984368970493, - "angularVelocity": 0.6968400699584296, - "velocityX": 0.8024909704823213, - "velocityY": -0.19686358201382703, - "timestamp": 0.20366562690713236 - }, - { - "x": 0.9206286419560185, - "y": 4.3497160444231815, - "heading": -0.8922424202408584, - "angularVelocity": 0.9302897737130964, - "velocityX": 1.0698944498243854, - "velocityY": -0.2627057422932853, - "timestamp": 0.2715541692095098 - }, - { - "x": 1.0114084585277832, - "y": 4.3274032827050695, - "heading": -0.8130879810632846, - "angularVelocity": 1.1659469550107187, - "velocityX": 1.3371890674486555, - "velocityY": -0.32866756246923856, - "timestamp": 0.33944271151188726 - }, - { - "x": 1.1203241193682845, - "y": 4.30060737967286, - "heading": -0.7176841911738273, - "angularVelocity": 1.4053003151036254, - "velocityX": 1.604330526871355, - "velocityY": -0.3947043510355547, - "timestamp": 0.4073312538142647 - }, - { - "x": 1.2473633964229762, - "y": 4.269329623801374, - "heading": -0.6056889773008814, - "angularVelocity": 1.6496924234153572, - "velocityX": 1.8712918667314336, - "velocityY": -0.46072216033411856, - "timestamp": 0.47521979611664217 - }, - { - "x": 1.392515159392732, - "y": 4.233580656924137, - "heading": -0.47672975284718977, - "angularVelocity": 1.8995727420291677, - "velocityX": 2.138089256994883, - "velocityY": -0.5265832151471296, - "timestamp": 0.5431083384190196 - }, - { - "x": 1.5193632658919598, - "y": 4.202650378846915, - "heading": -0.35714109334344585, - "angularVelocity": 1.7615440757454204, - "velocityX": 1.8684759194599092, - "velocityY": -0.4556038033554743, - "timestamp": 0.610996880721397 - }, - { - "x": 1.6281019189892685, - "y": 4.176160742808517, - "heading": -0.25481154838503933, - "angularVelocity": 1.507316868030967, - "velocityX": 1.6017231981942384, - "velocityY": -0.3901930302231607, - "timestamp": 0.6788854230237744 - }, - { - "x": 1.7187271243324231, - "y": 4.154098965427716, - "heading": -0.16971637395021472, - "angularVelocity": 1.2534541404027932, - "velocityX": 1.3349116400158851, - "velocityY": -0.32497055662996915, - "timestamp": 0.7467739653261518 - }, - { - "x": 1.7912335760671219, - "y": 4.136456276445617, - "heading": -0.10176996511823568, - "angularVelocity": 1.0008523754913499, - "velocityX": 1.0680219264651876, - "velocityY": -0.25987726917920023, - "timestamp": 0.8146625076285292 - }, - { - "x": 1.845616448865919, - "y": 4.123227280338756, - "heading": -0.050876879855985754, - "angularVelocity": 0.7496564742187419, - "velocityX": 0.80106113571528, - "velocityY": -0.19486345792990323, - "timestamp": 0.8825510499309066 - }, - { - "x": 1.8818723916386764, - "y": 4.114408992463889, - "heading": -0.01696431328583086, - "angularVelocity": 0.4995329906938869, - "velocityX": 0.5340509833201647, - "velocityY": -0.12989361055344087, - "timestamp": 0.950439592233284 - }, - { - "x": 1.9, - "y": 4.11, - "heading": -4.1561834640364843e-29, - "angularVelocity": 0.24988477746762355, - "velocityX": 0.26702014429154675, - "velocityY": -0.0649445740674598, - "timestamp": 1.0183281345356614 - }, - { - "x": 1.9, - "y": 4.11, - "heading": -1.087656704067454e-28, - "angularVelocity": 2.4807548250000706e-30, - "velocityX": 0, - "velocityY": 1.3216215980164627e-31, - "timestamp": 1.0862166768380388 + "x": 0.7651437671205386, + "y": 4.398651735511244, + "heading": -1.0392091078900427, + "angularVelocity": 0.13459888616794363, + "velocityX": 0.32570307697884177, + "velocityY": 0.054679546462018135, + "timestamp": 0.0801707385341453 + }, + { + "x": 0.817367588836673, + "y": 4.407419167420995, + "heading": -1.0176349612340314, + "angularVelocity": 0.26910250610729297, + "velocityX": 0.6514075168946079, + "velocityY": 0.10935950036205129, + "timestamp": 0.1603414770682906 + }, + { + "x": 0.8957035925551177, + "y": 4.420570347353103, + "heading": -0.9852910452071313, + "angularVelocity": 0.4034379203470197, + "velocityX": 0.9771146574267956, + "velocityY": 0.16403965053292643, + "timestamp": 0.2405122156024359 + }, + { + "x": 1.0001521051022733, + "y": 4.438105224493772, + "heading": -0.942196381665077, + "angularVelocity": 0.5375360677724069, + "velocityX": 1.3028258745885222, + "velocityY": 0.21871916688407136, + "timestamp": 0.3206829541365812 + }, + { + "x": 1.1307135580503673, + "y": 4.460023613513803, + "heading": -0.888374546540725, + "angularVelocity": 0.6713401436538962, + "velocityX": 1.6285424749141706, + "velocityY": 0.27339637155389257, + "timestamp": 0.4008536926707265 + }, + { + "x": 1.287388478944155, + "y": 4.486325092461348, + "heading": -0.8238521841946843, + "angularVelocity": 0.8048118743294378, + "velocityX": 1.9542656555054532, + "velocityY": 0.3280683130584222, + "timestamp": 0.4810244312048718 + }, + { + "x": 1.470188592457945, + "y": 4.516991294048374, + "heading": -0.7491443367227937, + "angularVelocity": 0.9318592897840392, + "velocityX": 2.280135082402091, + "velocityY": 0.3825111524196008, + "timestamp": 0.5611951697390172 + }, + { + "x": 1.626874682798592, + "y": 4.5432753433356226, + "heading": -0.6851111221374321, + "angularVelocity": 0.7987105489628158, + "velocityX": 1.9544049762484528, + "velocityY": 0.32785090630110136, + "timestamp": 0.6413659082731624 + }, + { + "x": 1.7574465106168242, + "y": 4.56517788745821, + "heading": -0.631746395437431, + "angularVelocity": 0.665638457069628, + "velocityX": 1.6286718846006478, + "velocityY": 0.2731987321441458, + "timestamp": 0.7215366468073077 + }, + { + "x": 1.8619039874752719, + "y": 4.582699429495963, + "heading": -0.5890503399232441, + "angularVelocity": 0.5325640788004256, + "velocityX": 1.3029376898400211, + "velocityY": 0.21855283309246581, + "timestamp": 0.8017073853414529 + }, + { + "x": 1.9402470937608582, + "y": 4.5958403420243075, + "heading": -0.5570256157366094, + "angularVelocity": 0.39945651957534534, + "velocityX": 0.9772032504380553, + "velocityY": 0.16391158131526215, + "timestamp": 0.8818781238755982 + }, + { + "x": 1.9924758344835722, + "y": 4.604600869409716, + "heading": -0.5356750146862916, + "angularVelocity": 0.26631413706167006, + "velocityX": 0.6514688735275852, + "velocityY": 0.10927337761367518, + "timestamp": 0.9620488624097434 + }, + { + "x": 2.018590211868286, + "y": 4.608981132507324, + "heading": -0.525, + "angularVelocity": 0.13315350315433505, + "velocityX": 0.32573452436878464, + "velocityY": 0.0546366813839713, + "timestamp": 1.0422196009438887 + }, + { + "x": 2.018590211868286, + "y": 4.608981132507324, + "heading": -0.525, + "angularVelocity": 2.602590767682023e-32, + "velocityX": -2.497311852298303e-34, + "velocityY": -2.1742604688427545e-32, + "timestamp": 1.122390339478034 } ], "constraints": [ @@ -5358,6 +5553,15 @@ "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 40 + }, + { + "x": 8.631598472595215, + "y": 6.001193523406982, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], "trajectory": [ @@ -6947,13 +7151,22 @@ "X6": { "waypoints": [ { - "x": 4.503084659576416, - "y": 4.545476913452148, - "heading": -0.3, + "x": 5.817, + "y": 6.847, + "heading": 0.234, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 14 + }, + { + "x": 6.7310991287231445, + "y": 5.664541721343994, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 }, { "x": 7.159897804260254, @@ -6967,175 +7180,256 @@ ], "trajectory": [ { - "x": 4.503084659576416, - "y": 4.545476913452148, - "heading": -0.3, - "angularVelocity": 0, - "velocityX": 1.3775324423698682e-39, - "velocityY": 2.667412759246627e-32, + "x": 5.817, + "y": 6.847, + "heading": 0.234, + "angularVelocity": 4.2225178427239466e-38, + "velocityX": 5.2861559836627645e-34, + "velocityY": 4.002568813289242e-37, "timestamp": 0 }, { - "x": 4.535884823966022, - "y": 4.5402829452206666, - "heading": -0.2962963545738625, - "angularVelocity": 0.041338894747682375, - "velocityX": 0.3661048473591824, - "velocityY": -0.05797339683997343, - "timestamp": 0.08959227015485637 - }, - { - "x": 4.60148515224976, - "y": 4.529895009628685, - "heading": -0.28888903544724676, - "angularVelocity": 0.0826781050844292, - "velocityX": 0.7322096891880244, - "velocityY": -0.11594678395833084, - "timestamp": 0.17918454030971273 - }, - { - "x": 4.699885643722846, - "y": 4.514313107942318, - "heading": -0.27777799238048634, - "angularVelocity": 0.12401787618011612, - "velocityX": 1.098314523150324, - "velocityY": -0.17392015694473859, - "timestamp": 0.2687768104645691 - }, - { - "x": 4.831086297424245, - "y": 4.49353724189291, - "heading": -0.26296315953262395, - "angularVelocity": 0.16535838217131477, - "velocityX": 1.4644193463858546, - "velocityY": -0.2318935106064126, - "timestamp": 0.35836908061942546 - }, - { - "x": 4.995087112032728, - "y": 4.46756741379766, - "heading": -0.24444446467786612, - "angularVelocity": 0.20669969432350724, - "velocityX": 1.8305241548742301, - "velocityY": -0.28986683840427246, - "timestamp": 0.44796135077428184 - }, - { - "x": 5.191888085608439, - "y": 4.436403626799179, - "heading": -0.22222184220566124, - "angularVelocity": 0.24804173879949776, - "velocityX": 2.1966289417105944, - "velocityY": -0.34784013112532575, - "timestamp": 0.5375536209291382 - }, - { - "x": 5.421489214846517, - "y": 4.4000458854654045, - "heading": -0.19629525254976224, - "angularVelocity": 0.289384224901166, - "velocityX": 2.5627336916591403, - "velocityY": -0.4058133728605406, - "timestamp": 0.6271458910839945 - }, - { - "x": 5.683890492166353, - "y": 4.358494197979682, - "heading": -0.1666647206388611, - "angularVelocity": 0.3307264327568205, - "velocityX": 2.928838356995378, - "velocityY": -0.463786523255888, - "timestamp": 0.7167381612388509 - }, - { - "x": 5.979091878252505, - "y": 4.311748596170855, - "heading": -0.13333055788996895, - "angularVelocity": 0.3720651646763175, - "velocityX": 3.2949425835053443, - "velocityY": -0.5217593183879428, - "timestamp": 0.8063304313937073 - }, - { - "x": 6.241493168416245, - "y": 4.27019697201608, - "heading": -0.10370083853079468, - "angularVelocity": 0.3307173633167286, - "velocityX": 2.9288385003548862, - "velocityY": -0.4637858163762743, - "timestamp": 0.8959227015485637 - }, - { - "x": 6.471094313755057, - "y": 4.233839308833591, - "heading": -0.0777751923679469, - "angularVelocity": 0.28937369393627815, - "velocityX": 2.562733871370336, - "velocityY": -0.4058125005611213, - "timestamp": 0.9855149717034201 - }, - { - "x": 6.667895303733172, - "y": 4.202675602068972, - "heading": -0.05555346222386864, - "angularVelocity": 0.2480317789209827, - "velocityX": 2.1966291247889234, - "velocityY": -0.3478392355808552, - "timestamp": 1.0751072418582763 - }, - { - "x": 6.83189613345896, - "y": 4.17670584917834, - "heading": -0.03703552336208346, - "angularVelocity": 0.20669125617397324, - "velocityX": 1.8305243236087168, - "velocityY": -0.2898659989946003, - "timestamp": 1.1646995120131327 - }, - { - "x": 6.963096800012264, - "y": 4.155930048478115, - "heading": -0.022221272692350115, - "angularVelocity": 0.16535188408695997, - "velocityX": 1.4644194898346672, - "velocityY": -0.23189278119992932, - "timestamp": 1.254291782167989 - }, - { - "x": 7.061497301461547, - "y": 4.140348198810691, - "heading": -0.011110631132494058, - "angularVelocity": 0.12401339468965188, - "velocityX": 1.0983146345014219, - "velocityY": -0.17391957632608143, - "timestamp": 1.3438840523228455 - }, - { - "x": 7.12709763649646, - "y": 4.129960299402646, - "heading": -0.0037035472512008906, - "angularVelocity": 0.08267547935207345, - "velocityX": 0.7322097645424621, - "velocityY": -0.11594638008490167, - "timestamp": 1.4334763224777018 + "x": 5.827092818691048, + "y": 6.836640444623551, + "heading": 0.23290827050378918, + "angularVelocity": -0.01846630380756111, + "velocityX": 0.1707172489800741, + "velocityY": -0.1752290265644902, + "timestamp": 0.059120087462432476 + }, + { + "x": 5.847162697988382, + "y": 6.81580981961747, + "heading": 0.23072211564801187, + "angularVelocity": -0.03697820740144453, + "velocityX": 0.3394764818317907, + "velocityY": -0.35234428601476925, + "timestamp": 0.11824017492486495 + }, + { + "x": 5.877072599439809, + "y": 6.784379259471569, + "heading": 0.22743855300065416, + "angularVelocity": -0.05554055801159481, + "velocityX": 0.5059177469998274, + "velocityY": -0.5316392700851961, + "timestamp": 0.17736026238729743 + }, + { + "x": 5.916657876422342, + "y": 6.7421982876337765, + "heading": 0.22305428725294427, + "angularVelocity": -0.07415864786221664, + "velocityX": 0.6695740598774843, + "velocityY": -0.7134795235984001, + "timestamp": 0.2364803498497299 + }, + { + "x": 5.965717226201233, + "y": 6.68908910795113, + "heading": 0.21756569779070734, + "angularVelocity": -0.09283797940462643, + "velocityX": 0.8298253924280056, + "velocityY": -0.8983271500806653, + "timestamp": 0.2956004373121624 + }, + { + "x": 6.023999328755403, + "y": 6.624838828190035, + "heading": 0.21096886342235058, + "angularVelocity": -0.11158363682308173, + "velocityX": 0.9858257160259868, + "velocityY": -1.0867757900717288, + "timestamp": 0.35472052477459487 + }, + { + "x": 6.091182353843822, + "y": 6.549188709495219, + "heading": 0.20325967797819372, + "angularVelocity": -0.13039874897099427, + "velocityX": 1.1363823697167148, + "velocityY": -1.2796009265528723, + "timestamp": 0.41384061223702734 + }, + { + "x": 6.166841087704512, + "y": 6.461819179818017, + "heading": 0.19443418504219476, + "angularVelocity": -0.1492807828068109, + "velocityX": 1.279746649711348, + "velocityY": -1.4778315362391972, + "timestamp": 0.4729606996994598 + }, + { + "x": 6.250391326549386, + "y": 6.362329172081929, + "heading": 0.1844894546954548, + "angularVelocity": -0.1682123754140137, + "velocityX": 1.4132292834980353, + "velocityY": -1.6828460850858449, + "timestamp": 0.5320807871618923 + }, + { + "x": 6.340989832538309, + "y": 6.250209683455231, + "heading": 0.17342587375941393, + "angularVelocity": -0.18713742504307268, + "velocityX": 1.5324487814144865, + "velocityY": -1.896470276671073, + "timestamp": 0.5912008746243248 + }, + { + "x": 6.437341958267089, + "y": 6.1248207212545625, + "heading": 0.16125340180931888, + "angularVelocity": -0.20589401119932219, + "velocityX": 1.6297696749857906, + "velocityY": -2.120919768265675, + "timestamp": 0.6503209620867573 + }, + { + "x": 6.537312703515491, + "y": 5.985428428886056, + "heading": 0.14800981318431106, + "angularVelocity": -0.22401165481061264, + "velocityX": 1.6909776277297848, + "velocityY": -2.3577822420692787, + "timestamp": 0.7094410495491897 + }, + { + "x": 6.637191132327962, + "y": 5.83157263911623, + "heading": 0.13381496006922441, + "angularVelocity": -0.24010203171817635, + "velocityX": 1.689416120636461, + "velocityY": -2.602428317914673, + "timestamp": 0.7685611370116222 + }, + { + "x": 6.7310991287231445, + "y": 5.664541721343994, + "heading": 0.11899191581046477, + "angularVelocity": -0.25072771193341653, + "velocityX": 1.588427900328373, + "velocityY": -2.825281980145458, + "timestamp": 0.8276812244740547 + }, + { + "x": 6.827071304911456, + "y": 5.451531523955929, + "heading": 0.1010502513823198, + "angularVelocity": -0.2525856403338842, + "velocityX": 1.3511117473992316, + "velocityY": -2.998791852360776, + "timestamp": 0.8987132293197303 + }, + { + "x": 6.902209054870137, + "y": 5.239913279632194, + "heading": 0.08404272380261314, + "angularVelocity": -0.23943471139040853, + "velocityX": 1.0578013407044498, + "velocityY": -2.979195713023971, + "timestamp": 0.9697452341654059 + }, + { + "x": 6.96109855546152, + "y": 5.041407006580284, + "heading": 0.06855695771575047, + "angularVelocity": -0.21801110809848362, + "velocityX": 0.8290558702281757, + "velocityY": -2.7946032705001835, + "timestamp": 1.0407772390110817 + }, + { + "x": 7.008382229837669, + "y": 4.86025636676961, + "heading": 0.05468561717579363, + "angularVelocity": -0.19528296533504552, + "velocityX": 0.6656671802925685, + "velocityY": -2.5502678715635776, + "timestamp": 1.1118092438567573 + }, + { + "x": 7.046818118928818, + "y": 4.698016877379498, + "heading": 0.04242022938635542, + "angularVelocity": -0.1726741039632221, + "velocityX": 0.5411066346030107, + "velocityY": -2.284033651346236, + "timestamp": 1.182841248702433 + }, + { + "x": 7.078089936261228, + "y": 4.555388481878325, + "heading": 0.03173911036922952, + "angularVelocity": -0.15037051312759409, + "velocityX": 0.4402496790052802, + "velocityY": -2.0079455142938496, + "timestamp": 1.2538732535481085 + }, + { + "x": 7.103305929625202, + "y": 4.4327413175795956, + "heading": 0.022621978547101074, + "angularVelocity": -0.1283524496026329, + "velocityX": 0.35499481422155044, + "velocityY": -1.7266465245517455, + "timestamp": 1.3249052583937841 + }, + { + "x": 7.123243650862787, + "y": 4.330294783654633, + "heading": 0.015051928802047752, + "angularVelocity": -0.10657237904941788, + "velocityX": 0.28068644945191357, + "velocityY": -1.4422587979536587, + "timestamp": 1.3959372632394598 + }, + { + "x": 7.138476280585093, + "y": 4.248190082970356, + "heading": 0.009015136402082834, + "angularVelocity": -0.08498693529882159, + "velocityX": 0.21444741360461653, + "velocityY": -1.1558831946621517, + "timestamp": 1.4669692680851354 + }, + { + "x": 7.149442842095887, + "y": 4.186523843939905, + "heading": 0.004500248099379863, + "angularVelocity": -0.06356132439893981, + "velocityX": 0.1543890185082034, + "velocityY": -0.8681472410137935, + "timestamp": 1.538001272930811 + }, + { + "x": 7.156489910575149, + "y": 4.145365390362028, + "heading": 0.001497840898069894, + "angularVelocity": -0.042268371951954174, + "velocityX": 0.09920976459235319, + "velocityY": -0.5794353357658713, + "timestamp": 1.6090332777764866 }, { "x": 7.159897804260254, "y": 4.1247663497924805, - "heading": 0, - "angularVelocity": 0.04133779895072974, - "velocityX": 0.36610488502078126, - "velocityY": -0.057973188994861924, - "timestamp": 1.5230685926325582 + "heading": -8.486883305020825e-34, + "angularVelocity": -0.021086845307606617, + "velocityX": 0.04797687595202675, + "velocityY": -0.2899966094762708, + "timestamp": 1.6800652826221623 }, { "x": 7.159897804260254, "y": 4.1247663497924805, - "heading": 0, - "angularVelocity": 0, - "velocityX": -1.5986633328718683e-34, + "heading": -4.844202103484773e-34, + "angularVelocity": 5.1282267159186814e-33, + "velocityX": 0, "velocityY": 0, - "timestamp": 1.6126608627874146 + "timestamp": 1.7510972874678379 } ], "constraints": [ @@ -7157,7 +7451,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "6X": { + "6Y": { "waypoints": [ { "x": 7.666658878326416, @@ -7169,12 +7463,21 @@ "controlIntervalCount": 15 }, { - "x": 5.698085308074951, - "y": 4.436619758605957, + "x": 5.6591033935546875, + "y": 4.339165210723877, "heading": -0.2, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 4.255763053894043, + "y": 4.96287202835083, + "heading": -0.15, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, "controlIntervalCount": 40 } ], @@ -7183,145 +7486,262 @@ "x": 7.666658878326416, "y": 4.085784435272217, "heading": 0, - "angularVelocity": -1.1067127343612795e-31, + "angularVelocity": 1.209482524629029e-34, "velocityX": 0, "velocityY": 0, "timestamp": 0 }, { - "x": 7.631505773615402, - "y": 4.092049347097207, - "heading": -0.003571118533822801, - "angularVelocity": -0.03844137718673683, - "velocityX": -0.37840630174613554, - "velocityY": 0.06743876917698166, - "timestamp": 0.09289777825792674 - }, - { - "x": 7.561199565031321, - "y": 4.104579170920473, - "heading": -0.010713386184546965, - "angularVelocity": -0.07688308358563711, - "velocityX": -0.7568125944721628, - "velocityY": 0.13487754021927925, - "timestamp": 0.18579555651585347 - }, - { - "x": 7.455740253806232, - "y": 4.123373907019809, - "heading": -0.021426855297674983, - "angularVelocity": -0.11532535345875038, - "velocityX": -1.1352188739356723, - "velocityY": 0.2023163142519348, - "timestamp": 0.2786933347737802 - }, - { - "x": 7.3151278417550785, - "y": 4.148433555758717, - "heading": -0.035711596112131795, - "angularVelocity": -0.15376837942018193, - "velocityX": -1.51362513386218, - "velocityY": 0.26975509219747784, - "timestamp": 0.37159111303170694 - }, - { - "x": 7.139362331727871, - "y": 4.1797581175230984, - "heading": -0.05356769126314415, - "angularVelocity": -0.19221229491017178, - "velocityX": -1.8920313631096928, - "velocityY": 0.3371938742970804, - "timestamp": 0.46448889128963367 - }, - { - "x": 6.928443728998723, - "y": 4.2173475924911346, - "heading": -0.07499522846089693, - "angularVelocity": -0.23065715455820832, - "velocityX": -2.2704375355839064, - "velocityY": 0.40463265831471423, - "timestamp": 0.5573866695475604 - }, - { - "x": 6.682372048313255, - "y": 4.261201979391781, - "heading": -0.09999429116464192, - "angularVelocity": -0.26910291260503705, - "velocityX": -2.6488435493286118, - "velocityY": 0.47207142865016644, - "timestamp": 0.6502844478054872 - }, - { - "x": 6.436300386509014, - "y": 4.3050564164367175, - "heading": -0.12499571783147595, - "angularVelocity": -0.26912835953319164, - "velocityX": -2.6488433460812497, - "velocityY": 0.4720719684293897, - "timestamp": 0.743182226063414 - }, - { - "x": 6.225381801811018, - "y": 4.342645939075802, - "heading": -0.14642552309962434, - "angularVelocity": -0.23068156924753846, - "velocityX": -2.270437341487206, - "velocityY": 0.4046331714706828, - "timestamp": 0.8360800043213408 - }, - { - "x": 6.049616308143143, - "y": 4.373970543797062, - "heading": -0.16428369122297962, - "angularVelocity": -0.19223460946259463, - "velocityX": -1.892031187009333, - "velocityY": 0.33719433670725885, - "timestamp": 0.9289777825792676 - }, - { - "x": 5.909003910073048, - "y": 4.399030228959261, - "heading": -0.17857021928140412, - "angularVelocity": -0.1537876182437709, - "velocityX": -1.51362498336279, - "velocityY": 0.2697554842767274, - "timestamp": 1.0218755608371943 - }, - { - "x": 5.803544609869108, - "y": 4.417824993537867, - "heading": -0.18928511062177825, - "angularVelocity": -0.11534066305250765, - "velocityX": -1.1352187552982904, - "velocityY": 0.20231662081759388, - "timestamp": 1.114773339095121 - }, - { - "x": 5.733238408896407, - "y": 4.430354836876777, - "heading": -0.19642836993898177, - "angularVelocity": -0.07689375839937317, - "velocityX": -0.7568125125393136, - "velocityY": 0.1348777502958473, - "timestamp": 1.2076711173530479 - }, - { - "x": 5.698085308074951, - "y": 4.436619758605957, + "x": 7.649455689544763, + "y": 4.086577766970284, + "heading": -0.0022997827801380754, + "angularVelocity": -0.03564306292608196, + "velocityX": -0.2666227200104806, + "velocityY": 0.012295409757681801, + "timestamp": 0.06452259125170103 + }, + { + "x": 7.615053343965649, + "y": 4.0882531279996925, + "heading": -0.0068609400958710215, + "angularVelocity": -0.07069085768642348, + "velocityX": -0.5331829505252058, + "velocityY": 0.02596549513754648, + "timestamp": 0.12904518250340205 + }, + { + "x": 7.563457156334565, + "y": 4.090914473875886, + "heading": -0.013638379729614996, + "angularVelocity": -0.10503979307504581, + "velocityX": -0.7996608107353951, + "velocityY": 0.041246729627017566, + "timestamp": 0.1935677737551031 + }, + { + "x": 7.494674268159643, + "y": 4.0946853102159935, + "heading": -0.022578417799948743, + "angularVelocity": -0.1385567116401054, + "velocityX": -1.0660279886559765, + "velocityY": 0.058442109452766046, + "timestamp": 0.2580903650068041 + }, + { + "x": 7.408714500593785, + "y": 4.09971475380561, + "heading": -0.03361607330634747, + "angularVelocity": -0.171066525573063, + "velocityX": -1.332242953952841, + "velocityY": 0.07794856796740106, + "timestamp": 0.3226129562585051 + }, + { + "x": 7.305591747951634, + "y": 4.10618637275211, + "heading": -0.0466711017764886, + "angularVelocity": -0.20233267475595532, + "velocityX": -1.598242578942184, + "velocityY": 0.10030004717657835, + "timestamp": 0.3871355475102061 + }, + { + "x": 7.18532637150992, + "y": 4.11433159194397, + "heading": -0.061641941793094415, + "angularVelocity": -0.23202477963424772, + "velocityX": -1.8639266357509128, + "velocityY": 0.1262382528948094, + "timestamp": 0.45165813876190714 + }, + { + "x": 7.047949574625153, + "y": 4.124451015500193, + "heading": -0.07839600669319866, + "angularVelocity": -0.25966199706313403, + "velocityX": -2.129127088973572, + "velocityY": 0.15683535580191885, + "timestamp": 0.5161807300136082 + }, + { + "x": 6.89351205381652, + "y": 4.136950431097767, + "heading": -0.09675311116467075, + "angularVelocity": -0.28450662187236314, + "velocityX": -2.3935418248497737, + "velocityY": 0.1937215377605359, + "timestamp": 0.5807033212653092 + }, + { + "x": 6.722102962824983, + "y": 4.152406451801245, + "heading": -0.11645479321673856, + "angularVelocity": -0.3053454870591373, + "velocityX": -2.6565748161426845, + "velocityY": 0.2395443270897933, + "timestamp": 0.6452259125170102 + }, + { + "x": 6.533897762196436, + "y": 4.171698896366532, + "heading": -0.13710103042159202, + "angularVelocity": -0.31998462560676116, + "velocityX": -2.9168884413578904, + "velocityY": 0.29900294131132776, + "timestamp": 0.7097485037687112 + }, + { + "x": 6.329305940744403, + "y": 4.196316085087732, + "heading": -0.1579986184766986, + "angularVelocity": -0.3238801735904445, + "velocityX": -3.1708556256509532, + "velocityY": 0.3815282096338637, + "timestamp": 0.7742710950204122 + }, + { + "x": 6.109588048034184, + "y": 4.229179536431487, + "heading": -0.1777172131023292, + "angularVelocity": -0.30560760569437684, + "velocityX": -3.405286248549814, + "velocityY": 0.5093324788453677, + "timestamp": 0.8387936862721133 + }, + { + "x": 5.880551755398176, + "y": 4.2764929326979475, + "heading": -0.19275157350982466, + "angularVelocity": -0.23300924708438825, + "velocityX": -3.5497069815832503, + "velocityY": 0.7332841931579067, + "timestamp": 0.9033162775238143 + }, + { + "x": 5.6591033935546875, + "y": 4.339165210723877, "heading": -0.2, - "angularVelocity": -0.038446883531506625, - "velocityX": -0.37840625987689963, - "velocityY": 0.06743887579081746, - "timestamp": 1.3005688956109747 - }, - { - "x": 5.698085308074951, - "y": 4.436619758605957, - "heading": -0.2, - "angularVelocity": 2.005600565422674e-31, + "angularVelocity": -0.11233935819312842, + "velocityX": -3.432105833747789, + "velocityY": 0.9713230174133182, + "timestamp": 0.9678388687755153 + }, + { + "x": 5.438463656346767, + "y": 4.41684559934421, + "heading": -0.20078792979416127, + "angularVelocity": -0.011394992631344103, + "velocityX": -3.190878423810921, + "velocityY": 1.1234090428973296, + "timestamp": 1.0369858897216806 + }, + { + "x": 5.237468412635918, + "y": 4.49671689701659, + "heading": -0.1976367711927584, + "angularVelocity": 0.045571863520430765, + "velocityX": -2.9067809568735434, + "velocityY": 1.1550938359956655, + "timestamp": 1.106132910667846 + }, + { + "x": 5.056130424824854, + "y": 4.574366670818957, + "heading": -0.19242246838266966, + "angularVelocity": 0.0754089292458229, + "velocityX": -2.6224989208464313, + "velocityY": 1.1229662932669573, + "timestamp": 1.1752799316140112 + }, + { + "x": 4.894040423510362, + "y": 4.647429210924394, + "heading": -0.18621230925465354, + "angularVelocity": 0.08981094258349935, + "velocityX": -2.344135713969339, + "velocityY": 1.0566259992938798, + "timestamp": 1.2444269525601765 + }, + { + "x": 4.750806708701635, + "y": 4.714490254294218, + "heading": -0.17966576596113534, + "angularVelocity": 0.09467570987063913, + "velocityX": -2.0714372484715344, + "velocityY": 0.9698327195040657, + "timestamp": 1.3135739735063419 + }, + { + "x": 4.626109277283802, + "y": 4.774623849709137, + "heading": -0.17322414817467333, + "angularVelocity": 0.09315828358646269, + "velocityX": -1.803366648505599, + "velocityY": 0.8696483896498642, + "timestamp": 1.3827209944525072 + }, + { + "x": 4.519692351368254, + "y": 4.827181352429608, + "heading": -0.1672014372604398, + "angularVelocity": 0.08710007794728412, + "velocityX": -1.5389950927661693, + "velocityY": 0.7600833991299484, + "timestamp": 1.4518680153986725 + }, + { + "x": 4.431350072395874, + "y": 4.871684848267837, + "heading": -0.16183165746625428, + "angularVelocity": 0.0776574279080811, + "velocityX": -1.277600650954415, + "velocityY": 0.6436068427716991, + "timestamp": 1.5210150363448378 + }, + { + "x": 4.360914453717203, + "y": 4.907768355386048, + "heading": -0.1572956227267302, + "angularVelocity": 0.06559985777341837, + "velocityX": -1.0186356218225097, + "velocityY": 0.5218374793948632, + "timestamp": 1.5901620572910031 + }, + { + "x": 4.308246377396777, + "y": 4.935142992782337, + "heading": -0.15373706400788933, + "angularVelocity": 0.051463659173564456, + "velocityX": -0.7616825077891644, + "velocityY": 0.39589033658590256, + "timestamp": 1.6593090782371684 + }, + { + "x": 4.273229022688589, + "y": 4.953575130372871, + "heading": -0.15127288861048924, + "angularVelocity": 0.035636754319735044, + "velocityX": -0.5064188482603104, + "velocityY": 0.26656444975242444, + "timestamp": 1.7284560991833338 + }, + { + "x": 4.255763053894043, + "y": 4.96287202835083, + "heading": -0.15, + "angularVelocity": 0.018408437457923484, + "velocityX": -0.2525917755465569, + "velocityY": 0.13445117158695233, + "timestamp": 1.797603120129499 + }, + { + "x": 4.255763053894043, + "y": 4.96287202835083, + "heading": -0.15, + "angularVelocity": 9.849002219211576e-31, "velocityX": 0, - "velocityY": 0, - "timestamp": 1.3934666738689014 + "velocityY": -4.0048098642858166e-33, + "timestamp": 1.8667501410756644 } ], "constraints": [ @@ -8368,6 +8788,282 @@ "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, "circleObstacles": [] + }, + "O14": { + "waypoints": [ + { + "x": 2.905118465423584, + "y": 7.004644393920898, + "heading": 0.72, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 25 + }, + { + "x": 7.2768425941467285, + "y": 7.477189540863037, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 2.905118465423584, + "y": 7.004644393920898, + "heading": 0.72, + "angularVelocity": 0, + "velocityX": -6.340996393748289e-32, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2.933132671641307, + "y": 7.0076689483976455, + "heading": 0.7131841747115845, + "angularVelocity": -0.082536230581912, + "velocityX": 0.33923800658135683, + "velocityY": 0.03662583988692675, + "timestamp": 0.08257979847256815 + }, + { + "x": 2.989163871014555, + "y": 7.0137186955153625, + "heading": 0.6997627791522665, + "angularVelocity": -0.16252637820662474, + "velocityX": 0.678509761583661, + "velocityY": 0.07325940762296092, + "timestamp": 0.1651595969451363 + }, + { + "x": 3.0732152969802553, + "y": 7.022794391598154, + "heading": 0.679988631625691, + "angularVelocity": -0.23945502279663347, + "velocityX": 1.0178206718876763, + "velocityY": 0.10990213406509869, + "timestamp": 0.24773939541770446 + }, + { + "x": 3.185290740233773, + "y": 7.034896943483482, + "heading": 0.6541718767823331, + "angularVelocity": -0.31262797100335904, + "velocityX": 1.3571774856139984, + "velocityY": 0.1465558418547046, + "timestamp": 0.3303191938902726 + }, + { + "x": 3.3253946894995794, + "y": 7.050027457703765, + "heading": 0.6227022863851476, + "angularVelocity": -0.3810809783754027, + "velocityX": 1.6965886555454899, + "velocityY": 0.18322294919768115, + "timestamp": 0.4128989923628407 + }, + { + "x": 3.493532501761929, + "y": 7.068187316986307, + "heading": 0.586084546531271, + "angularVelocity": -0.443422489879932, + "velocityX": 2.036064695873245, + "velocityY": 0.21990680067579577, + "timestamp": 0.49547879083540886 + }, + { + "x": 3.6897105743579695, + "y": 7.089378301369239, + "heading": 0.544997483466065, + "angularVelocity": -0.4975437555389184, + "velocityX": 2.3756182047497476, + "velocityY": 0.25661220752409203, + "timestamp": 0.578058589307977 + }, + { + "x": 3.913936381147418, + "y": 7.11360278172052, + "heading": 0.500401626620305, + "angularVelocity": -0.5400334908662322, + "velocityX": 2.7152622183241086, + "velocityY": 0.2933463243959419, + "timestamp": 0.6606383877805452 + }, + { + "x": 4.166217753482193, + "y": 7.140864021723552, + "heading": 0.4537574958800772, + "angularVelocity": -0.5648370618650594, + "velocityX": 3.055001065647807, + "velocityY": 0.33011996283916484, + "timestamp": 0.7432181862531133 + }, + { + "x": 4.446558237016815, + "y": 7.171166554841283, + "heading": 0.4075491838315556, + "angularVelocity": -0.5595595157871043, + "velocityX": 3.39478284907179, + "velocityY": 0.36694849924803646, + "timestamp": 0.8257979847256814 + }, + { + "x": 4.754925692230947, + "y": 7.204515676219626, + "heading": 0.36695341961296113, + "angularVelocity": -0.4915943725766812, + "velocityX": 3.734175439001076, + "velocityY": 0.4038411572217807, + "timestamp": 0.9083777831982496 + }, + { + "x": 5.090794320806147, + "y": 7.24092157776734, + "heading": 0.35005114522176756, + "angularVelocity": -0.20467807750326483, + "velocityX": 4.067200874636274, + "velocityY": 0.44085723410557004, + "timestamp": 0.9909575816708177 + }, + { + "x": 5.426785245989899, + "y": 7.277318209455475, + "heading": 0.3332919882121585, + "angularVelocity": -0.20294499769484936, + "velocityX": 4.0686818253129635, + "velocityY": 0.44074498074877944, + "timestamp": 1.0735373801433858 + }, + { + "x": 5.735187772945253, + "y": 7.3106664338840375, + "heading": 0.2947493152442794, + "angularVelocity": -0.4667324658368085, + "velocityX": 3.7346001402214513, + "velocityY": 0.4038302956090829, + "timestamp": 1.156117178615954 + }, + { + "x": 6.015558100776027, + "y": 7.3409683371760694, + "heading": 0.2510735292368369, + "angularVelocity": -0.5288918938625496, + "velocityX": 3.395144248550537, + "velocityY": 0.36694087237418926, + "timestamp": 1.2386969770885223 + }, + { + "x": 6.267869194711806, + "y": 7.368229399723306, + "heading": 0.20705575527296857, + "angularVelocity": -0.5330331967312758, + "velocityX": 3.0553609793497554, + "velocityY": 0.33011781393827844, + "timestamp": 1.3212767755610906 + }, + { + "x": 6.492124338906455, + "y": 7.392454000382105, + "heading": 0.16499936573610186, + "angularVelocity": -0.5092818136640006, + "velocityX": 2.7156174796073023, + "velocityY": 0.29334778125966826, + "timestamp": 1.4038565740336588 + }, + { + "x": 6.688330423757298, + "y": 7.413645250957243, + "heading": 0.12626243549483357, + "angularVelocity": -0.46908482411999763, + "velocityX": 2.375957419125304, + "velocityY": 0.25661543097834716, + "timestamp": 1.486436372506227 + }, + { + "x": 6.856494007240475, + "y": 7.43180542462103, + "heading": 0.09174153381590364, + "angularVelocity": -0.41803082979603484, + "velocityX": 2.0363767724498123, + "velocityY": 0.2199106076751445, + "timestamp": 1.5690161709787953 + }, + { + "x": 6.996620710350906, + "y": 7.446936247259988, + "heading": 0.06207294975812711, + "angularVelocity": -0.3592716936506282, + "velocityX": 1.6968641932074937, + "velocityY": 0.18322668399332662, + "timestamp": 1.6515959694513636 + }, + { + "x": 7.1087152508513825, + "y": 7.45903907311994, + "heading": 0.03773162314436687, + "angularVelocity": -0.2947612741205597, + "velocityX": 1.3574087437096964, + "velocityY": 0.14655915955014778, + "timestamp": 1.7341757679239318 + }, + { + "x": 7.192781591064824, + "y": 7.468114992444878, + "heading": 0.019085582000236086, + "angularVelocity": -0.22579421952452003, + "velocityX": 1.0180012759579458, + "velocityY": 0.10990483741565026, + "timestamp": 1.8167555663965 + }, + { + "x": 7.2488230839133125, + "y": 7.474164900066286, + "heading": 0.006428436704965813, + "angularVelocity": -0.1532716902886532, + "velocityX": 0.6786344104132683, + "velocityY": 0.0732613512422664, + "timestamp": 1.8993353648690683 + }, + { + "x": 7.276842594146728, + "y": 7.477189540863037, + "heading": 0, + "angularVelocity": -0.07784514886240033, + "velocityX": 0.339302235554834, + "velocityY": 0.036626885178919544, + "timestamp": 1.9819151633416365 + }, + { + "x": 7.276842594146728, + "y": 7.477189540863037, + "heading": 0, + "angularVelocity": 0, + "velocityX": 3.928920620966037e-34, + "velocityY": 0, + "timestamp": 2.0644949618142046 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] } }, "splitTrajectoriesAtStopPoints": false, diff --git a/src/main/deploy/choreo/6Y.traj b/src/main/deploy/choreo/6Y.traj new file mode 100644 index 00000000..29f053b2 --- /dev/null +++ b/src/main/deploy/choreo/6Y.traj @@ -0,0 +1,265 @@ +{ + "samples": [ + { + "x": 7.666658878326416, + "y": 4.085784435272217, + "heading": 0, + "angularVelocity": 1.209482524629029e-34, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 7.649455689544763, + "y": 4.086577766970284, + "heading": -0.0022997827801380754, + "angularVelocity": -0.03564306292608196, + "velocityX": -0.2666227200104806, + "velocityY": 0.012295409757681801, + "timestamp": 0.06452259125170103 + }, + { + "x": 7.615053343965649, + "y": 4.0882531279996925, + "heading": -0.0068609400958710215, + "angularVelocity": -0.07069085768642348, + "velocityX": -0.5331829505252058, + "velocityY": 0.02596549513754648, + "timestamp": 0.12904518250340205 + }, + { + "x": 7.563457156334565, + "y": 4.090914473875886, + "heading": -0.013638379729614996, + "angularVelocity": -0.10503979307504581, + "velocityX": -0.7996608107353951, + "velocityY": 0.041246729627017566, + "timestamp": 0.1935677737551031 + }, + { + "x": 7.494674268159643, + "y": 4.0946853102159935, + "heading": -0.022578417799948743, + "angularVelocity": -0.1385567116401054, + "velocityX": -1.0660279886559765, + "velocityY": 0.058442109452766046, + "timestamp": 0.2580903650068041 + }, + { + "x": 7.408714500593785, + "y": 4.09971475380561, + "heading": -0.03361607330634747, + "angularVelocity": -0.171066525573063, + "velocityX": -1.332242953952841, + "velocityY": 0.07794856796740106, + "timestamp": 0.3226129562585051 + }, + { + "x": 7.305591747951634, + "y": 4.10618637275211, + "heading": -0.0466711017764886, + "angularVelocity": -0.20233267475595532, + "velocityX": -1.598242578942184, + "velocityY": 0.10030004717657835, + "timestamp": 0.3871355475102061 + }, + { + "x": 7.18532637150992, + "y": 4.11433159194397, + "heading": -0.061641941793094415, + "angularVelocity": -0.23202477963424772, + "velocityX": -1.8639266357509128, + "velocityY": 0.1262382528948094, + "timestamp": 0.45165813876190714 + }, + { + "x": 7.047949574625153, + "y": 4.124451015500193, + "heading": -0.07839600669319866, + "angularVelocity": -0.25966199706313403, + "velocityX": -2.129127088973572, + "velocityY": 0.15683535580191885, + "timestamp": 0.5161807300136082 + }, + { + "x": 6.89351205381652, + "y": 4.136950431097767, + "heading": -0.09675311116467075, + "angularVelocity": -0.28450662187236314, + "velocityX": -2.3935418248497737, + "velocityY": 0.1937215377605359, + "timestamp": 0.5807033212653092 + }, + { + "x": 6.722102962824983, + "y": 4.152406451801245, + "heading": -0.11645479321673856, + "angularVelocity": -0.3053454870591373, + "velocityX": -2.6565748161426845, + "velocityY": 0.2395443270897933, + "timestamp": 0.6452259125170102 + }, + { + "x": 6.533897762196436, + "y": 4.171698896366532, + "heading": -0.13710103042159202, + "angularVelocity": -0.31998462560676116, + "velocityX": -2.9168884413578904, + "velocityY": 0.29900294131132776, + "timestamp": 0.7097485037687112 + }, + { + "x": 6.329305940744403, + "y": 4.196316085087732, + "heading": -0.1579986184766986, + "angularVelocity": -0.3238801735904445, + "velocityX": -3.1708556256509532, + "velocityY": 0.3815282096338637, + "timestamp": 0.7742710950204122 + }, + { + "x": 6.109588048034184, + "y": 4.229179536431487, + "heading": -0.1777172131023292, + "angularVelocity": -0.30560760569437684, + "velocityX": -3.405286248549814, + "velocityY": 0.5093324788453677, + "timestamp": 0.8387936862721133 + }, + { + "x": 5.880551755398176, + "y": 4.2764929326979475, + "heading": -0.19275157350982466, + "angularVelocity": -0.23300924708438825, + "velocityX": -3.5497069815832503, + "velocityY": 0.7332841931579067, + "timestamp": 0.9033162775238143 + }, + { + "x": 5.6591033935546875, + "y": 4.339165210723877, + "heading": -0.2, + "angularVelocity": -0.11233935819312842, + "velocityX": -3.432105833747789, + "velocityY": 0.9713230174133182, + "timestamp": 0.9678388687755153 + }, + { + "x": 5.438463656346767, + "y": 4.41684559934421, + "heading": -0.20078792979416127, + "angularVelocity": -0.011394992631344103, + "velocityX": -3.190878423810921, + "velocityY": 1.1234090428973296, + "timestamp": 1.0369858897216806 + }, + { + "x": 5.237468412635918, + "y": 4.49671689701659, + "heading": -0.1976367711927584, + "angularVelocity": 0.045571863520430765, + "velocityX": -2.9067809568735434, + "velocityY": 1.1550938359956655, + "timestamp": 1.106132910667846 + }, + { + "x": 5.056130424824854, + "y": 4.574366670818957, + "heading": -0.19242246838266966, + "angularVelocity": 0.0754089292458229, + "velocityX": -2.6224989208464313, + "velocityY": 1.1229662932669573, + "timestamp": 1.1752799316140112 + }, + { + "x": 4.894040423510362, + "y": 4.647429210924394, + "heading": -0.18621230925465354, + "angularVelocity": 0.08981094258349935, + "velocityX": -2.344135713969339, + "velocityY": 1.0566259992938798, + "timestamp": 1.2444269525601765 + }, + { + "x": 4.750806708701635, + "y": 4.714490254294218, + "heading": -0.17966576596113534, + "angularVelocity": 0.09467570987063913, + "velocityX": -2.0714372484715344, + "velocityY": 0.9698327195040657, + "timestamp": 1.3135739735063419 + }, + { + "x": 4.626109277283802, + "y": 4.774623849709137, + "heading": -0.17322414817467333, + "angularVelocity": 0.09315828358646269, + "velocityX": -1.803366648505599, + "velocityY": 0.8696483896498642, + "timestamp": 1.3827209944525072 + }, + { + "x": 4.519692351368254, + "y": 4.827181352429608, + "heading": -0.1672014372604398, + "angularVelocity": 0.08710007794728412, + "velocityX": -1.5389950927661693, + "velocityY": 0.7600833991299484, + "timestamp": 1.4518680153986725 + }, + { + "x": 4.431350072395874, + "y": 4.871684848267837, + "heading": -0.16183165746625428, + "angularVelocity": 0.0776574279080811, + "velocityX": -1.277600650954415, + "velocityY": 0.6436068427716991, + "timestamp": 1.5210150363448378 + }, + { + "x": 4.360914453717203, + "y": 4.907768355386048, + "heading": -0.1572956227267302, + "angularVelocity": 0.06559985777341837, + "velocityX": -1.0186356218225097, + "velocityY": 0.5218374793948632, + "timestamp": 1.5901620572910031 + }, + { + "x": 4.308246377396777, + "y": 4.935142992782337, + "heading": -0.15373706400788933, + "angularVelocity": 0.051463659173564456, + "velocityX": -0.7616825077891644, + "velocityY": 0.39589033658590256, + "timestamp": 1.6593090782371684 + }, + { + "x": 4.273229022688589, + "y": 4.953575130372871, + "heading": -0.15127288861048924, + "angularVelocity": 0.035636754319735044, + "velocityX": -0.5064188482603104, + "velocityY": 0.26656444975242444, + "timestamp": 1.7284560991833338 + }, + { + "x": 4.255763053894043, + "y": 4.96287202835083, + "heading": -0.15, + "angularVelocity": 0.018408437457923484, + "velocityX": -0.2525917755465569, + "velocityY": 0.13445117158695233, + "timestamp": 1.797603120129499 + }, + { + "x": 4.255763053894043, + "y": 4.96287202835083, + "heading": -0.15, + "angularVelocity": 9.849002219211576e-31, + "velocityX": 0, + "velocityY": -4.0048098642858166e-33, + "timestamp": 1.8667501410756644 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/O14.traj b/src/main/deploy/choreo/O14.traj new file mode 100644 index 00000000..c2eed1cc --- /dev/null +++ b/src/main/deploy/choreo/O14.traj @@ -0,0 +1,238 @@ +{ + "samples": [ + { + "x": 2.905118465423584, + "y": 7.004644393920898, + "heading": 0.72, + "angularVelocity": 0, + "velocityX": -6.340996393748289e-32, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2.933132671641307, + "y": 7.0076689483976455, + "heading": 0.7131841747115845, + "angularVelocity": -0.082536230581912, + "velocityX": 0.33923800658135683, + "velocityY": 0.03662583988692675, + "timestamp": 0.08257979847256815 + }, + { + "x": 2.989163871014555, + "y": 7.0137186955153625, + "heading": 0.6997627791522665, + "angularVelocity": -0.16252637820662474, + "velocityX": 0.678509761583661, + "velocityY": 0.07325940762296092, + "timestamp": 0.1651595969451363 + }, + { + "x": 3.0732152969802553, + "y": 7.022794391598154, + "heading": 0.679988631625691, + "angularVelocity": -0.23945502279663347, + "velocityX": 1.0178206718876763, + "velocityY": 0.10990213406509869, + "timestamp": 0.24773939541770446 + }, + { + "x": 3.185290740233773, + "y": 7.034896943483482, + "heading": 0.6541718767823331, + "angularVelocity": -0.31262797100335904, + "velocityX": 1.3571774856139984, + "velocityY": 0.1465558418547046, + "timestamp": 0.3303191938902726 + }, + { + "x": 3.3253946894995794, + "y": 7.050027457703765, + "heading": 0.6227022863851476, + "angularVelocity": -0.3810809783754027, + "velocityX": 1.6965886555454899, + "velocityY": 0.18322294919768115, + "timestamp": 0.4128989923628407 + }, + { + "x": 3.493532501761929, + "y": 7.068187316986307, + "heading": 0.586084546531271, + "angularVelocity": -0.443422489879932, + "velocityX": 2.036064695873245, + "velocityY": 0.21990680067579577, + "timestamp": 0.49547879083540886 + }, + { + "x": 3.6897105743579695, + "y": 7.089378301369239, + "heading": 0.544997483466065, + "angularVelocity": -0.4975437555389184, + "velocityX": 2.3756182047497476, + "velocityY": 0.25661220752409203, + "timestamp": 0.578058589307977 + }, + { + "x": 3.913936381147418, + "y": 7.11360278172052, + "heading": 0.500401626620305, + "angularVelocity": -0.5400334908662322, + "velocityX": 2.7152622183241086, + "velocityY": 0.2933463243959419, + "timestamp": 0.6606383877805452 + }, + { + "x": 4.166217753482193, + "y": 7.140864021723552, + "heading": 0.4537574958800772, + "angularVelocity": -0.5648370618650594, + "velocityX": 3.055001065647807, + "velocityY": 0.33011996283916484, + "timestamp": 0.7432181862531133 + }, + { + "x": 4.446558237016815, + "y": 7.171166554841283, + "heading": 0.4075491838315556, + "angularVelocity": -0.5595595157871043, + "velocityX": 3.39478284907179, + "velocityY": 0.36694849924803646, + "timestamp": 0.8257979847256814 + }, + { + "x": 4.754925692230947, + "y": 7.204515676219626, + "heading": 0.36695341961296113, + "angularVelocity": -0.4915943725766812, + "velocityX": 3.734175439001076, + "velocityY": 0.4038411572217807, + "timestamp": 0.9083777831982496 + }, + { + "x": 5.090794320806147, + "y": 7.24092157776734, + "heading": 0.35005114522176756, + "angularVelocity": -0.20467807750326483, + "velocityX": 4.067200874636274, + "velocityY": 0.44085723410557004, + "timestamp": 0.9909575816708177 + }, + { + "x": 5.426785245989899, + "y": 7.277318209455475, + "heading": 0.3332919882121585, + "angularVelocity": -0.20294499769484936, + "velocityX": 4.0686818253129635, + "velocityY": 0.44074498074877944, + "timestamp": 1.0735373801433858 + }, + { + "x": 5.735187772945253, + "y": 7.3106664338840375, + "heading": 0.2947493152442794, + "angularVelocity": -0.4667324658368085, + "velocityX": 3.7346001402214513, + "velocityY": 0.4038302956090829, + "timestamp": 1.156117178615954 + }, + { + "x": 6.015558100776027, + "y": 7.3409683371760694, + "heading": 0.2510735292368369, + "angularVelocity": -0.5288918938625496, + "velocityX": 3.395144248550537, + "velocityY": 0.36694087237418926, + "timestamp": 1.2386969770885223 + }, + { + "x": 6.267869194711806, + "y": 7.368229399723306, + "heading": 0.20705575527296857, + "angularVelocity": -0.5330331967312758, + "velocityX": 3.0553609793497554, + "velocityY": 0.33011781393827844, + "timestamp": 1.3212767755610906 + }, + { + "x": 6.492124338906455, + "y": 7.392454000382105, + "heading": 0.16499936573610186, + "angularVelocity": -0.5092818136640006, + "velocityX": 2.7156174796073023, + "velocityY": 0.29334778125966826, + "timestamp": 1.4038565740336588 + }, + { + "x": 6.688330423757298, + "y": 7.413645250957243, + "heading": 0.12626243549483357, + "angularVelocity": -0.46908482411999763, + "velocityX": 2.375957419125304, + "velocityY": 0.25661543097834716, + "timestamp": 1.486436372506227 + }, + { + "x": 6.856494007240475, + "y": 7.43180542462103, + "heading": 0.09174153381590364, + "angularVelocity": -0.41803082979603484, + "velocityX": 2.0363767724498123, + "velocityY": 0.2199106076751445, + "timestamp": 1.5690161709787953 + }, + { + "x": 6.996620710350906, + "y": 7.446936247259988, + "heading": 0.06207294975812711, + "angularVelocity": -0.3592716936506282, + "velocityX": 1.6968641932074937, + "velocityY": 0.18322668399332662, + "timestamp": 1.6515959694513636 + }, + { + "x": 7.1087152508513825, + "y": 7.45903907311994, + "heading": 0.03773162314436687, + "angularVelocity": -0.2947612741205597, + "velocityX": 1.3574087437096964, + "velocityY": 0.14655915955014778, + "timestamp": 1.7341757679239318 + }, + { + "x": 7.192781591064824, + "y": 7.468114992444878, + "heading": 0.019085582000236086, + "angularVelocity": -0.22579421952452003, + "velocityX": 1.0180012759579458, + "velocityY": 0.10990483741565026, + "timestamp": 1.8167555663965 + }, + { + "x": 7.2488230839133125, + "y": 7.474164900066286, + "heading": 0.006428436704965813, + "angularVelocity": -0.1532716902886532, + "velocityX": 0.6786344104132683, + "velocityY": 0.0732613512422664, + "timestamp": 1.8993353648690683 + }, + { + "x": 7.276842594146728, + "y": 7.477189540863037, + "heading": 0, + "angularVelocity": -0.07784514886240033, + "velocityX": 0.339302235554834, + "velocityY": 0.036626885178919544, + "timestamp": 1.9819151633416365 + }, + { + "x": 7.276842594146728, + "y": 7.477189540863037, + "heading": 0, + "angularVelocity": 0, + "velocityX": 3.928920620966037e-34, + "velocityY": 0, + "timestamp": 2.0644949618142046 + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auton/AutonBuilder.java b/src/main/java/frc/robot/commands/auton/AutonBuilder.java index 0d120373..01acaccc 100644 --- a/src/main/java/frc/robot/commands/auton/AutonBuilder.java +++ b/src/main/java/frc/robot/commands/auton/AutonBuilder.java @@ -281,6 +281,28 @@ public SequentialCommandGroup getTopFourPiece() { } + public SequentialCommandGroup getAmp1ThenCenter3() { + ChoreoTrajectory startToPiece1 = Choreo.getTrajectory("OA1"); + ChoreoTrajectory piece1ToPiece2 = Choreo.getTrajectory("O14"); + ChoreoTrajectory piece2ToWing = Choreo.getTrajectory("4X"); + ChoreoTrajectory wingToPiece3 = Choreo.getTrajectory("X5"); + ChoreoTrajectory piece3ToWing = Choreo.getTrajectory("5X"); + ChoreoTrajectory wingToPiece4 = Choreo.getTrajectory("X6"); + ChoreoTrajectory piece4ToWing = Choreo.getTrajectory("6Y"); + + return buildAuton( + new Pose2d(startToPiece1.getInitialPose().getTranslation(), new Rotation2d()), + goIntake(startToPiece1), + shoot(), + goIntake(piece1ToPiece2), + goShoot(piece2ToWing), + goIntake(wingToPiece3), + goShoot(piece3ToWing), + goIntake(wingToPiece4), + goShoot(piece4ToWing) + ); + } + /** Starts by the amp, shoots preloaded, gets 2 closest to amp wing notes, * then closest 2 to amp center note. */ public SequentialCommandGroup getTopTwoPieceThenCenter2() {