diff --git a/Choreo_Trajectories.chor b/Choreo_Trajectories.chor index 8cd23df4..8bb63c74 100644 --- a/Choreo_Trajectories.chor +++ b/Choreo_Trajectories.chor @@ -4420,6 +4420,1241 @@ "circleObstacles": [], "eventMarkers": [], "isTrajectoryStale": false + }, + "Red AS to 6 + NT": { + "waypoints": [ + { + "x": 15.151942253112793, + "y": 6.983804225921631, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 13.663607597351074, + "y": 7.0030083656311035, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 11.85456371307373, + "y": 7.374666213989258, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 9.54304027557373, + "y": 7.392831802368164, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 9.996101379394531, + "y": 6.594287872314453, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 9.522457122802734, + "y": 6.075540542602539, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 9.942900657653809, + "y": 5.131283283233643, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 9.570499420166016, + "y": 4.33328104019165, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 10.246135711669922, + "y": 4.249108791351318, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 11.605405807495117, + "y": 4.306680679321289, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 12.301013946533203, + "y": 5.162186622619629, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 13.69665813446045, + "y": 6.036226272583008, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 15.151942253112791, + "y": 6.983804225921631, + "heading": 2.460658578276488e-30, + "angularVelocity": 1.1559528303191587e-30, + "velocityX": -8.450980122730519e-24, + "velocityY": -1.0227558064600819e-22, + "timestamp": 0 + }, + { + "x": 15.118082717686605, + "y": 6.983746166696196, + "heading": 9.024737996051924e-20, + "angularVelocity": 1.344825629304957e-18, + "velocityX": -0.5045594792879681, + "velocityY": -0.0008651723121594873, + "timestamp": 0.06710712377055983 + }, + { + "x": 15.050363647796937, + "y": 6.983630048249095, + "heading": 2.4316871133633307e-19, + "angularVelocity": 2.278764500415112e-18, + "velocityX": -1.0091189442301196, + "velocityY": -0.0017303445681617134, + "timestamp": 0.13421424754111966 + }, + { + "x": 14.948785044791569, + "y": 6.983455870585603, + "heading": 5.35970791472245e-19, + "angularVelocity": 4.3632041381261375e-18, + "velocityX": -1.5136783890882703, + "velocityY": -0.0025955167455443302, + "timestamp": 0.2013213713116795 + }, + { + "x": 14.813346910692148, + "y": 6.983223633713634, + "heading": 9.216072681731045e-19, + "angularVelocity": 5.746580318609512e-18, + "velocityX": -2.018237803820731, + "velocityY": -0.003460688804998752, + "timestamp": 0.2684284950822393 + }, + { + "x": 14.644049248868034, + "y": 6.982933337646379, + "heading": 1.4181741486552305e-18, + "angularVelocity": 7.399615012222265e-18, + "velocityX": -2.5227971683445407, + "velocityY": -0.00432586066790944, + "timestamp": 0.33553561885279914 + }, + { + "x": 14.440892066057732, + "y": 6.982584982410215, + "heading": 2.0075804989291975e-18, + "angularVelocity": 8.783066791637582e-18, + "velocityX": -3.0273564324541744, + "velocityY": -0.005191032137744908, + "timestamp": 0.402642742623359 + }, + { + "x": 14.203875382475081, + "y": 6.982178568084268, + "heading": 2.7079281574609036e-18, + "angularVelocity": 1.0436263978825298e-17, + "velocityX": -3.5319153953463394, + "velocityY": -0.006056202428452966, + "timestamp": 0.4697498663939188 + }, + { + "x": 13.93332208875509, + "y": 6.981715355253932, + "heading": 7.040762676561354e-20, + "angularVelocity": -3.930313776694512e-17, + "velocityX": -4.031662788067243, + "velocityY": -0.006902588045925373, + "timestamp": 0.5368569901644786 + }, + { + "x": 13.663607597351074, + "y": 7.0030083656311035, + "heading": -6.322941131722835e-29, + "angularVelocity": -1.0491826035121783e-18, + "velocityX": -4.019163335418371, + "velocityY": 0.3172988079473022, + "timestamp": 0.6039641139350385 + }, + { + "x": 13.48074370743158, + "y": 7.033373665695924, + "heading": 7.719060237002786e-19, + "angularVelocity": 1.6788609812813145e-17, + "velocityX": -3.9772075899908805, + "velocityY": 0.6604316573563296, + "timestamp": 0.6499420735415955 + }, + { + "x": 13.30115269323578, + "y": 7.079290127414415, + "heading": 1.80497495080332e-18, + "angularVelocity": 2.2468785827024268e-17, + "velocityX": -3.9060240109086752, + "velocityY": 0.9986624485168062, + "timestamp": 0.6959200331481525 + }, + { + "x": 13.12230781574745, + "y": 7.128031933339034, + "heading": 2.9475018721988785e-18, + "angularVelocity": 2.4849448110195845e-17, + "velocityX": -3.889795872168816, + "velocityY": 1.0601124178130732, + "timestamp": 0.7418979927547096 + }, + { + "x": 12.943462985340895, + "y": 7.1767739120171665, + "heading": 1.682270387833829e-18, + "angularVelocity": -2.7518217318289784e-17, + "velocityX": -3.8897948481613316, + "velocityY": 1.0601161751245307, + "timestamp": 0.7878759523612666 + }, + { + "x": 12.764618154935798, + "y": 7.225515890700651, + "heading": 1.3123960398950925e-18, + "angularVelocity": -8.044601175075305e-18, + "velocityX": -3.889794848129619, + "velocityY": 1.060116175240888, + "timestamp": 0.8338539119678237 + }, + { + "x": 12.585773321348107, + "y": 7.27425785770651, + "heading": 9.730502655500534e-19, + "angularVelocity": -7.380618391695251e-18, + "velocityX": -3.889794917349603, + "velocityY": 1.060115921257809, + "timestamp": 0.8798318715743807 + }, + { + "x": 12.406874821470067, + "y": 7.322802482427277, + "heading": -8.30897663402979e-20, + "angularVelocity": -2.297057200597194e-17, + "velocityX": -3.890962135094999, + "velocityY": 1.0558238150664485, + "timestamp": 0.9258098311809377 + }, + { + "x": 12.224481849887567, + "y": 7.3558790247482895, + "heading": -3.4573592051805234e-18, + "angularVelocity": -7.338884690956896e-17, + "velocityX": -3.966965327371491, + "velocityY": 0.7193999604170215, + "timestamp": 0.9717877907874948 + }, + { + "x": 12.039925875231576, + "y": 7.3732095068305945, + "heading": 3.5682697108658626e-18, + "angularVelocity": 1.5280427788067176e-16, + "velocityX": -4.014009674097683, + "velocityY": 0.37693021244540154, + "timestamp": 1.0177657503940518 + }, + { + "x": 11.85456371307373, + "y": 7.374666213989241, + "heading": 5.367949901095069e-29, + "angularVelocity": -7.760826581445421e-17, + "velocityX": -4.031543890682136, + "velocityY": 0.03168272735699931, + "timestamp": 1.0637437100006089 + }, + { + "x": 11.58382573658003, + "y": 7.376793864766986, + "heading": 3.611107370087949e-19, + "angularVelocity": 5.3772800315900255e-18, + "velocityX": -4.031544248842776, + "velocityY": 0.031682730171678136, + "timestamp": 1.1308986163548016 + }, + { + "x": 11.313087760086365, + "y": 7.378921515544708, + "heading": 6.613128665612877e-19, + "angularVelocity": 4.4702933241501485e-18, + "velocityX": -4.031544248842776, + "velocityY": 0.031682730171678136, + "timestamp": 1.1980535227089943 + }, + { + "x": 11.042349783592421, + "y": 7.381049166322431, + "heading": 1.7859545454902864e-18, + "angularVelocity": 1.674697710103153e-17, + "velocityX": -4.031544248842776, + "velocityY": 0.031682730171678136, + "timestamp": 1.265208429063187 + }, + { + "x": 10.771611807098852, + "y": 7.383176817100155, + "heading": 2.1088147678209187e-18, + "angularVelocity": 4.807693731778367e-18, + "velocityX": -4.031544248842776, + "velocityY": 0.031682730171678136, + "timestamp": 1.3323633354173798 + }, + { + "x": 10.500873830605814, + "y": 7.385304467877871, + "heading": 2.3845787851703935e-18, + "angularVelocity": 4.1063867456177466e-18, + "velocityX": -4.031544248830867, + "velocityY": 0.03168273017158455, + "timestamp": 1.3995182417715726 + }, + { + "x": 10.262320186935474, + "y": 7.387179191372732, + "heading": 8.261235716651573e-19, + "angularVelocity": -2.3206870474634867e-17, + "velocityX": -3.5522891270589096, + "velocityY": 0.02791640397763177, + "timestamp": 1.4666731481257653 + }, + { + "x": 10.057673285180528, + "y": 7.388787451674137, + "heading": -3.8938543453196834e-19, + "angularVelocity": -1.8100077450473204e-17, + "velocityX": -3.0473857066471712, + "velocityY": 0.023948515286777196, + "timestamp": 1.533828054479958 + }, + { + "x": 9.886933144144443, + "y": 7.39012924863431, + "heading": -1.0593998775036432e-18, + "angularVelocity": -9.977148049779185e-18, + "velocityX": -2.5424820062315496, + "velocityY": 0.019980624395453776, + "timestamp": 1.6009829608341508 + }, + { + "x": 9.750099770096462, + "y": 7.391204582203997, + "heading": -1.3638864252586915e-18, + "angularVelocity": -4.534092358681147e-18, + "velocityX": -2.0375782124799673, + "velocityY": 0.01601273277063028, + "timestamp": 1.6681378671883436 + }, + { + "x": 9.647173166169406, + "y": 7.392013452358564, + "heading": -1.2574112860570634e-18, + "angularVelocity": 1.5855154148597377e-18, + "velocityX": -1.5326743720601586, + "velocityY": 0.012044840779054804, + "timestamp": 1.7352927735425363 + }, + { + "x": 9.578153334243916, + "y": 7.392555859083237, + "heading": -7.668820492443921e-19, + "angularVelocity": 7.304443762183156e-18, + "velocityX": -1.0277705036393359, + "velocityY": 0.008076948567427526, + "timestamp": 1.802447679896729 + }, + { + "x": 9.54304027557373, + "y": 7.392831802368181, + "heading": 1.9853085976678412e-29, + "angularVelocity": 1.1419598223030353e-17, + "velocityX": -0.5228666165511381, + "velocityY": 0.004109056209099373, + "timestamp": 1.8696025862509218 + }, + { + "x": 9.550006403554065, + "y": 7.36520669470756, + "heading": 8.208453400112202e-18, + "angularVelocity": 9.126792762218019e-17, + "velocityX": 0.0774547936533035, + "velocityY": -0.3071572930535228, + "timestamp": 1.9595405700962467 + }, + { + "x": 9.609958464307818, + "y": 7.307727015265158, + "heading": 1.1900950012142629e-17, + "angularVelocity": 4.1056030546201247e-17, + "velocityX": 0.6665933367692368, + "velocityY": -0.639103490926102, + "timestamp": 2.049478553941572 + }, + { + "x": 9.720571256960566, + "y": 7.216597756490272, + "heading": 1.5776992410646012e-17, + "angularVelocity": 4.3096834427112364e-17, + "velocityX": 1.229878499866957, + "velocityY": -1.0132455151719792, + "timestamp": 2.139416537786897 + }, + { + "x": 9.872189918308363, + "y": 7.080553903052873, + "heading": 1.1667578992513797e-17, + "angularVelocity": -4.5691633750798436e-17, + "velocityX": 1.6858134334938062, + "velocityY": -1.5126406844005709, + "timestamp": 2.229354521632222 + }, + { + "x": 9.973633909336314, + "y": 6.910139985134272, + "heading": 4.645940827916874e-18, + "angularVelocity": -7.807199877514019e-17, + "velocityX": 1.127932678615688, + "velocityY": -1.8947936192529897, + "timestamp": 2.3192925054775473 + }, + { + "x": 10.014541282771518, + "y": 6.745567406575643, + "heading": 2.794830244897641e-18, + "angularVelocity": -2.0582077826162586e-17, + "velocityX": 0.4548397872199964, + "velocityY": -1.829845094611647, + "timestamp": 2.4092304893228724 + }, + { + "x": 9.996101379394531, + "y": 6.594287872314453, + "heading": -5.340124287151323e-29, + "angularVelocity": -3.10750822452399e-17, + "velocityX": -0.2050290943668352, + "velocityY": -1.6820427564994032, + "timestamp": 2.4991684731681976 + }, + { + "x": 9.957997396822789, + "y": 6.500960065054064, + "heading": -2.2271639579965374e-18, + "angularVelocity": -3.728616719364335e-17, + "velocityX": -0.6379195657653035, + "velocityY": -1.5624517507922602, + "timestamp": 2.5589001142976375 + }, + { + "x": 9.894155362526096, + "y": 6.415193876390115, + "heading": -4.219859523652492e-18, + "angularVelocity": -3.336080388780194e-17, + "velocityX": -1.0688143350748942, + "velocityY": -1.435858567456612, + "timestamp": 2.6186317554270775 + }, + { + "x": 9.804912436862404, + "y": 6.338054378980702, + "heading": -4.43830875194841e-19, + "angularVelocity": 6.321655620201416e-17, + "velocityX": -1.4940645188418495, + "velocityY": -1.2914344215363034, + "timestamp": 2.6783633965565175 + }, + { + "x": 9.694406652742682, + "y": 6.277270726595081, + "heading": 4.641364645868438e-18, + "angularVelocity": 8.51340332351435e-17, + "velocityX": -1.8500376355013004, + "velocityY": -1.0176122945274693, + "timestamp": 2.7380950376859574 + }, + { + "x": 9.610661177390286, + "y": 6.214613448346474, + "heading": 3.604917083029758e-18, + "angularVelocity": -1.7351734243372534e-17, + "velocityX": -1.4020287031946272, + "velocityY": -1.0489796875466244, + "timestamp": 2.7978266788153974 + }, + { + "x": 9.553384342427035, + "y": 6.147593104501113, + "heading": 1.4115319396353346e-18, + "angularVelocity": -3.6720657626017165e-17, + "velocityX": -0.9589027503719386, + "velocityY": -1.1220241496483352, + "timestamp": 2.8575583199448373 + }, + { + "x": 9.522457122802734, + "y": 6.075540542602539, + "heading": 7.836358279698028e-29, + "angularVelocity": -2.363122647941345e-17, + "velocityX": -0.5177694608671554, + "velocityY": -1.2062712581834834, + "timestamp": 2.9172899610742773 + }, + { + "x": 9.519123714897153, + "y": 5.993460121280647, + "heading": -6.857914452691464e-19, + "angularVelocity": -1.0867971841834439e-17, + "velocityX": -0.05282565639883304, + "velocityY": -1.300756540044744, + "timestamp": 2.980392024215985 + }, + { + "x": 9.54498029319509, + "y": 5.904726543842293, + "heading": -2.7444802624472655e-18, + "angularVelocity": -3.262474655691004e-17, + "velocityX": 0.40975804927125337, + "velocityY": -1.4061913829835633, + "timestamp": 3.0434940873576926 + }, + { + "x": 9.599724821974295, + "y": 5.808131289394199, + "heading": -1.9295807077760543e-18, + "angularVelocity": 1.2913992254151298e-17, + "velocityX": 0.8675552914373884, + "velocityY": -1.5307780703012885, + "timestamp": 3.1065961504994 + }, + { + "x": 9.682510378590626, + "y": 5.701046987518788, + "heading": -3.002183724926251e-19, + "angularVelocity": 2.5821062800849368e-17, + "velocityX": 1.3119310604856096, + "velocityY": -1.697001596206646, + "timestamp": 3.169698213641108 + }, + { + "x": 9.78801044548454, + "y": 5.574459625498355, + "heading": 1.5877410391790831e-18, + "angularVelocity": 2.991913921092726e-17, + "velocityX": 1.6718956820317172, + "velocityY": -2.0060732679398616, + "timestamp": 3.2328002767828155 + }, + { + "x": 9.86933763233761, + "y": 5.430209029921785, + "heading": 1.972367537583804e-18, + "angularVelocity": 6.095307811136636e-18, + "velocityX": 1.2888197755188047, + "velocityY": -2.2859885777843494, + "timestamp": 3.295902339924523 + }, + { + "x": 9.921073700834063, + "y": 5.281410358531107, + "heading": 1.7086835837171051e-18, + "angularVelocity": -4.178689899770491e-18, + "velocityX": 0.819879191275729, + "velocityY": -2.358063492417386, + "timestamp": 3.3590044030662307 + }, + { + "x": 9.942900657653809, + "y": 5.131283283233643, + "heading": -6.833866856560672e-29, + "angularVelocity": -2.707809378572145e-17, + "velocityX": 0.3458992580120465, + "velocityY": -2.37911516395788, + "timestamp": 3.4221064662079383 + }, + { + "x": 9.928880027543176, + "y": 4.960225125749519, + "heading": -2.857729865103397e-18, + "angularVelocity": -3.972939752645852e-17, + "velocityX": -0.19492086850405763, + "velocityY": -2.3781245463585914, + "timestamp": 3.4940363227606785 + }, + { + "x": 9.876199175238394, + "y": 4.793490015189766, + "heading": -6.713793512065014e-18, + "angularVelocity": -5.360866588277142e-17, + "velocityX": -0.7323920111832362, + "velocityY": -2.318023676823262, + "timestamp": 3.5659661793134187 + }, + { + "x": 9.787502173908543, + "y": 4.641456535864866, + "heading": -1.1563620063466963e-17, + "angularVelocity": -6.742438792180666e-17, + "velocityX": -1.233104104202085, + "velocityY": -2.113635235924965, + "timestamp": 3.637896035866159 + }, + { + "x": 9.689354804369618, + "y": 4.527158855547807, + "heading": -6.146646541729309e-18, + "angularVelocity": 7.530911058881295e-17, + "velocityX": -1.3644872135531292, + "velocityY": -1.5890158245102814, + "timestamp": 3.709825892418899 + }, + { + "x": 9.61650346743172, + "y": 4.442414714293235, + "heading": -6.652822488921985e-18, + "angularVelocity": -7.03707711124707e-18, + "velocityX": -1.0128108191690985, + "velocityY": -1.1781497324749555, + "timestamp": 3.7817557489716394 + }, + { + "x": 9.576163220155488, + "y": 4.379032468037305, + "heading": -3.620092957118811e-18, + "angularVelocity": 4.216231864176792e-17, + "velocityX": -0.5608275785543259, + "velocityY": -0.8811674218960207, + "timestamp": 3.8536856055243796 + }, + { + "x": 9.570499420166016, + "y": 4.33328104019165, + "heading": 6.389355081534522e-29, + "angularVelocity": 5.032809921647081e-17, + "velocityX": -0.07874059897949862, + "velocityY": -0.6360561530121928, + "timestamp": 3.9256154620771198 + }, + { + "x": 9.59025662728202, + "y": 4.3060861435258255, + "heading": 3.4405474170918484e-18, + "angularVelocity": 5.774970157610978e-17, + "velocityX": 0.3316253713844863, + "velocityY": -0.45646723515195037, + "timestamp": 3.98519234859072 + }, + { + "x": 9.634928766499318, + "y": 4.288453823273995, + "heading": 7.258280629472283e-18, + "angularVelocity": 6.40807775591821e-17, + "velocityX": 0.7498233263180065, + "velocityY": -0.2959590754680539, + "timestamp": 4.0447692351043205 + }, + { + "x": 9.705066673913727, + "y": 4.278802186634717, + "heading": 9.257468175317858e-18, + "angularVelocity": 3.3556428722662696e-17, + "velocityX": 1.17726708324039, + "velocityY": -0.16200303849505196, + "timestamp": 4.104346121617921 + }, + { + "x": 9.80128303549785, + "y": 4.274817191136606, + "heading": 6.671362808308425e-18, + "angularVelocity": -4.3407863659556446e-17, + "velocityX": 1.614994794368075, + "velocityY": -0.06688828052808415, + "timestamp": 4.163923008131521 + }, + { + "x": 9.924104879660334, + "y": 4.27291661282852, + "heading": 3.995960020606412e-18, + "angularVelocity": -4.490672380345128e-17, + "velocityX": 2.061568694672314, + "velocityY": -0.03190126942354246, + "timestamp": 4.223499894645121 + }, + { + "x": 10.073354763097997, + "y": 4.267307247053675, + "heading": 8.681036334086009e-19, + "angularVelocity": -5.250117235511423e-17, + "velocityX": 2.5051642032954144, + "velocityY": -0.09415338905910305, + "timestamp": 4.283076781158721 + }, + { + "x": 10.246135711669922, + "y": 4.249108791351318, + "heading": -6.39603491434045e-29, + "angularVelocity": -1.4571148045948685e-17, + "velocityX": 2.900133905662932, + "velocityY": -0.3054616776289984, + "timestamp": 4.342653667672321 + }, + { + "x": 10.397533223464032, + "y": 4.2194926871368, + "heading": -1.4214934612932198e-18, + "angularVelocity": -2.9172605523862634e-17, + "velocityX": 3.1070560712521975, + "velocityY": -0.6077966230494994, + "timestamp": 4.391380664650234 + }, + { + "x": 10.560063970116651, + "y": 4.175921674279642, + "heading": -8.355398486506036e-18, + "angularVelocity": -1.4230109498284303e-16, + "velocityX": 3.335537930365231, + "velocityY": -0.8941862942407067, + "timestamp": 4.440107661628147 + }, + { + "x": 10.733231337260525, + "y": 4.146687525463282, + "heading": -7.256703988845365e-18, + "angularVelocity": 2.2547962440387354e-17, + "velocityX": 3.553828019041837, + "velocityY": -0.5999579417876079, + "timestamp": 4.48883465860606 + }, + { + "x": 10.899397747529992, + "y": 4.13387516385498, + "heading": -4.91148810214039e-18, + "angularVelocity": 4.8129702879046774e-17, + "velocityX": 3.4101508522019155, + "velocityY": -0.26294174488341465, + "timestamp": 4.537561655583973 + }, + { + "x": 11.057563871843254, + "y": 4.137021634089263, + "heading": -2.6432866762848484e-18, + "angularVelocity": 4.65491732830131e-17, + "velocityX": 3.245964950086196, + "velocityY": 0.06457344858966835, + "timestamp": 4.586288652561886 + }, + { + "x": 11.207388703066018, + "y": 4.155951382100819, + "heading": -4.097530137639523e-18, + "angularVelocity": -2.984471753889731e-17, + "velocityX": 3.074780727625754, + "velocityY": 0.38848583302059725, + "timestamp": 4.635015649539799 + }, + { + "x": 11.34870051105692, + "y": 4.190572450443893, + "heading": -5.45090094005365e-18, + "angularVelocity": -2.777455797239923e-17, + "velocityX": 2.9000721726183385, + "velocityY": 0.7105110203850111, + "timestamp": 4.683742646517712 + }, + { + "x": 11.48139590202311, + "y": 4.240828303678194, + "heading": -1.3594070863501201e-18, + "angularVelocity": 8.396769978639009e-17, + "velocityX": 2.723241718063186, + "velocityY": 1.0313759589387619, + "timestamp": 4.732469643495625 + }, + { + "x": 11.605405807495115, + "y": 4.306680679321289, + "heading": 1.1498940545297757e-28, + "angularVelocity": 2.7898437638409835e-17, + "velocityX": 2.5449938055533585, + "velocityY": 1.351455655536201, + "timestamp": 4.781196640473538 + }, + { + "x": 11.704841734407355, + "y": 4.374190201271196, + "heading": 1.2913487502887817e-18, + "angularVelocity": 3.1065136615300746e-17, + "velocityX": 2.392065391928354, + "velocityY": 1.6240326418893487, + "timestamp": 4.822765707626873 + }, + { + "x": 11.798140777460658, + "y": 4.4531512570442136, + "heading": 1.880571563314064e-18, + "angularVelocity": 1.4174549810487774e-17, + "velocityX": 2.2444343701327476, + "velocityY": 1.8995147396922663, + "timestamp": 4.864334774780208 + }, + { + "x": 11.88562887896236, + "y": 4.543732635858822, + "heading": 6.144405284004916e-18, + "angularVelocity": 1.0257227338968366e-16, + "velocityX": 2.1046443303378966, + "velocityY": 2.179057289894958, + "timestamp": 4.905903841933544 + }, + { + "x": 11.96783586421051, + "y": 4.646184512849362, + "heading": 1.0774137839314388e-17, + "angularVelocity": 1.1137446356799422e-16, + "velocityX": 1.9775999529870423, + "velocityY": 2.4646181405184553, + "timestamp": 4.947472909086879 + }, + { + "x": 12.045762443377628, + "y": 4.7609032956811985, + "heading": 1.1033598708280907e-17, + "angularVelocity": 6.2416812945219875e-18, + "velocityX": 1.8746290091059812, + "velocityY": 2.7597151124097876, + "timestamp": 4.989041976240214 + }, + { + "x": 12.121881659014273, + "y": 4.88848800851973, + "heading": 1.0993808732719266e-17, + "angularVelocity": -9.57201553253283e-19, + "velocityX": 1.831150440683814, + "velocityY": 3.069222418870066, + "timestamp": 5.03061104339355 + }, + { + "x": 12.20514803910829, + "y": 5.026922440288341, + "heading": 8.23831147224171e-18, + "angularVelocity": -6.628720462737835e-17, + "velocityX": 2.003085125458145, + "velocityY": 3.330227047385233, + "timestamp": 5.072180110546885 + }, + { + "x": 12.301013946533203, + "y": 5.162186622619629, + "heading": -8.137476723677248e-29, + "angularVelocity": -1.9818369851840407e-16, + "velocityX": 2.306183756091877, + "velocityY": 3.2539624195159407, + "timestamp": 5.11374917770022 + }, + { + "x": 12.499881584010561, + "y": 5.374941033273301, + "heading": -1.68670430873798e-17, + "angularVelocity": -2.335032342813133e-16, + "velocityX": 2.7530751124840362, + "velocityY": 2.9453202163601047, + "timestamp": 5.185983907721052 + }, + { + "x": 12.723364661530127, + "y": 5.557147112844672, + "heading": -1.941639399809902e-17, + "angularVelocity": -3.529259277341286e-17, + "velocityX": 3.0938452660529427, + "velocityY": 2.522416564979513, + "timestamp": 5.2582186377418845 + }, + { + "x": 12.948963863963927, + "y": 5.700178683736988, + "heading": -1.793322327115035e-17, + "angularVelocity": 2.053265411966626e-17, + "velocityX": 3.123140383701003, + "velocityY": 1.980094213006201, + "timestamp": 5.330453367762717 + }, + { + "x": 13.15514980795229, + "y": 5.809118526061311, + "heading": -2.073342818682101e-17, + "angularVelocity": -3.876535448899066e-17, + "velocityX": 2.854387964472203, + "velocityY": 1.5081366303010209, + "timestamp": 5.402688097783549 + }, + { + "x": 13.332368406825639, + "y": 5.89160059122476, + "heading": -1.6325337936238524e-17, + "angularVelocity": 6.102452725036477e-17, + "velocityX": 2.4533710975625906, + "velocityY": 1.1418616106083745, + "timestamp": 5.474922827804381 + }, + { + "x": 13.476660624324403, + "y": 5.952752518585699, + "heading": -1.4094237433699954e-17, + "angularVelocity": 3.0886811674822726e-17, + "velocityX": 1.9975462974271516, + "velocityY": 0.8465723806720552, + "timestamp": 5.547157557825213 + }, + { + "x": 13.586113710702598, + "y": 5.995866804511295, + "heading": -8.214620498966636e-18, + "angularVelocity": 8.139598407904596e-17, + "velocityX": 1.5152418559137708, + "velocityY": 0.5968636681158963, + "timestamp": 5.619392287846045 + }, + { + "x": 13.659664660636231, + "y": 6.023164697186193, + "heading": -3.503128075700943e-18, + "angularVelocity": 6.52247529949022e-17, + "velocityX": 1.0182214277317942, + "velocityY": 0.37790537414654557, + "timestamp": 5.691627017866877 + }, + { + "x": 13.69665813446045, + "y": 6.036226272583008, + "heading": -5.6863338433321666e-30, + "angularVelocity": 4.8496451425533246e-17, + "velocityX": 0.5121286369250458, + "velocityY": 0.18082126690371453, + "timestamp": 5.763861747887709 + }, + { + "x": 13.69665813446045, + "y": 6.036226272583008, + "heading": -3.4479150184640595e-30, + "angularVelocity": -1.6743970569933744e-29, + "velocityX": 6.6279365304051e-27, + "velocityY": -2.722134005564902e-26, + "timestamp": 5.836096477908542 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 15.151942253112793, + "y": 6.983804225921631, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 0.6039641139350385, + "isStopPoint": false, + "x": 13.663607597351074, + "y": 7.0030083656311035, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 1.0637437100006089, + "isStopPoint": false, + "x": 11.85456371307373, + "y": 7.374666213989258, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "timestamp": 1.8696025862509218, + "isStopPoint": false, + "x": 9.54304027557373, + "y": 7.392831802368164, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 2.4991684731681976, + "isStopPoint": false, + "x": 9.996101379394531, + "y": 6.594287872314453, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 2.9172899610742773, + "isStopPoint": false, + "x": 9.522457122802734, + "y": 6.075540542602539, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "timestamp": 3.4221064662079383, + "isStopPoint": false, + "x": 9.942900657653809, + "y": 5.131283283233643, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 3.9256154620771198, + "isStopPoint": false, + "x": 9.570499420166016, + "y": 4.33328104019165, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 4.342653667672321, + "isStopPoint": false, + "x": 10.246135711669922, + "y": 4.249108791351318, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 4.781196640473538, + "isStopPoint": false, + "x": 11.605405807495117, + "y": 4.306680679321289, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "timestamp": 5.11374917770022, + "isStopPoint": false, + "x": 12.301013946533203, + "y": 5.162186622619629, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 5.836096477908542, + "isStopPoint": true, + "x": 13.69665813446045, + "y": 6.036226272583008, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 2, + 3 + ], + "type": "StraightLine" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [ + { + "name": "intake", + "target": 0, + "trajTargetIndex": 0, + "targetTimestamp": 0, + "offset": 0, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + }, + { + "name": "outtake + intake", + "target": 2, + "trajTargetIndex": 2, + "targetTimestamp": 1.0637437100006089, + "offset": 0, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "outtakeShooter" + } + }, + { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + ] + } + } + }, + { + "name": "outtake + intake", + "target": 4, + "trajTargetIndex": 4, + "targetTimestamp": 2.4991684731681976, + "offset": 0, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "outtakeShooter" + } + }, + { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + ] + } + } + }, + { + "name": "outtake + intake", + "target": 6, + "trajTargetIndex": 6, + "targetTimestamp": 3.4221064662079383, + "offset": 0, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "outtakeShooter" + } + }, + { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + ] + } + } + }, + { + "name": "outtake", + "target": 8, + "trajTargetIndex": 8, + "targetTimestamp": 4.342653667672321, + "offset": 0, + "command": { + "type": "named", + "data": { + "name": "outtakeShooter" + } + } + } + ], + "isTrajectoryStale": false } }, "splitTrajectoriesAtStopPoints": true, diff --git a/src/main/deploy/choreo/Red AS to 6 + NT.1.traj b/src/main/deploy/choreo/Red AS to 6 + NT.1.traj new file mode 100644 index 00000000..c27a10f4 --- /dev/null +++ b/src/main/deploy/choreo/Red AS to 6 + NT.1.traj @@ -0,0 +1,945 @@ +{ + "samples": [ + { + "x": 15.151942253112791, + "y": 6.983804225921631, + "heading": 2.460658578276488e-30, + "angularVelocity": 1.1559528303191587e-30, + "velocityX": -8.450980122730519e-24, + "velocityY": -1.0227558064600819e-22, + "timestamp": 0 + }, + { + "x": 15.118082717686605, + "y": 6.983746166696196, + "heading": 9.024737996051924e-20, + "angularVelocity": 1.344825629304957e-18, + "velocityX": -0.5045594792879681, + "velocityY": -0.0008651723121594873, + "timestamp": 0.06710712377055983 + }, + { + "x": 15.050363647796937, + "y": 6.983630048249095, + "heading": 2.4316871133633307e-19, + "angularVelocity": 2.278764500415112e-18, + "velocityX": -1.0091189442301196, + "velocityY": -0.0017303445681617134, + "timestamp": 0.13421424754111966 + }, + { + "x": 14.948785044791569, + "y": 6.983455870585603, + "heading": 5.35970791472245e-19, + "angularVelocity": 4.3632041381261375e-18, + "velocityX": -1.5136783890882703, + "velocityY": -0.0025955167455443302, + "timestamp": 0.2013213713116795 + }, + { + "x": 14.813346910692148, + "y": 6.983223633713634, + "heading": 9.216072681731045e-19, + "angularVelocity": 5.746580318609512e-18, + "velocityX": -2.018237803820731, + "velocityY": -0.003460688804998752, + "timestamp": 0.2684284950822393 + }, + { + "x": 14.644049248868034, + "y": 6.982933337646379, + "heading": 1.4181741486552305e-18, + "angularVelocity": 7.399615012222265e-18, + "velocityX": -2.5227971683445407, + "velocityY": -0.00432586066790944, + "timestamp": 0.33553561885279914 + }, + { + "x": 14.440892066057732, + "y": 6.982584982410215, + "heading": 2.0075804989291975e-18, + "angularVelocity": 8.783066791637582e-18, + "velocityX": -3.0273564324541744, + "velocityY": -0.005191032137744908, + "timestamp": 0.402642742623359 + }, + { + "x": 14.203875382475081, + "y": 6.982178568084268, + "heading": 2.7079281574609036e-18, + "angularVelocity": 1.0436263978825298e-17, + "velocityX": -3.5319153953463394, + "velocityY": -0.006056202428452966, + "timestamp": 0.4697498663939188 + }, + { + "x": 13.93332208875509, + "y": 6.981715355253932, + "heading": 7.040762676561354e-20, + "angularVelocity": -3.930313776694512e-17, + "velocityX": -4.031662788067243, + "velocityY": -0.006902588045925373, + "timestamp": 0.5368569901644786 + }, + { + "x": 13.663607597351074, + "y": 7.0030083656311035, + "heading": -6.322941131722835e-29, + "angularVelocity": -1.0491826035121783e-18, + "velocityX": -4.019163335418371, + "velocityY": 0.3172988079473022, + "timestamp": 0.6039641139350385 + }, + { + "x": 13.48074370743158, + "y": 7.033373665695924, + "heading": 7.719060237002786e-19, + "angularVelocity": 1.6788609812813145e-17, + "velocityX": -3.9772075899908805, + "velocityY": 0.6604316573563296, + "timestamp": 0.6499420735415955 + }, + { + "x": 13.30115269323578, + "y": 7.079290127414415, + "heading": 1.80497495080332e-18, + "angularVelocity": 2.2468785827024268e-17, + "velocityX": -3.9060240109086752, + "velocityY": 0.9986624485168062, + "timestamp": 0.6959200331481525 + }, + { + "x": 13.12230781574745, + "y": 7.128031933339034, + "heading": 2.9475018721988785e-18, + "angularVelocity": 2.4849448110195845e-17, + "velocityX": -3.889795872168816, + "velocityY": 1.0601124178130732, + "timestamp": 0.7418979927547096 + }, + { + "x": 12.943462985340895, + "y": 7.1767739120171665, + "heading": 1.682270387833829e-18, + "angularVelocity": -2.7518217318289784e-17, + "velocityX": -3.8897948481613316, + "velocityY": 1.0601161751245307, + "timestamp": 0.7878759523612666 + }, + { + "x": 12.764618154935798, + "y": 7.225515890700651, + "heading": 1.3123960398950925e-18, + "angularVelocity": -8.044601175075305e-18, + "velocityX": -3.889794848129619, + "velocityY": 1.060116175240888, + "timestamp": 0.8338539119678237 + }, + { + "x": 12.585773321348107, + "y": 7.27425785770651, + "heading": 9.730502655500534e-19, + "angularVelocity": -7.380618391695251e-18, + "velocityX": -3.889794917349603, + "velocityY": 1.060115921257809, + "timestamp": 0.8798318715743807 + }, + { + "x": 12.406874821470067, + "y": 7.322802482427277, + "heading": -8.30897663402979e-20, + "angularVelocity": -2.297057200597194e-17, + "velocityX": -3.890962135094999, + "velocityY": 1.0558238150664485, + "timestamp": 0.9258098311809377 + }, + { + "x": 12.224481849887567, + "y": 7.3558790247482895, + "heading": -3.4573592051805234e-18, + "angularVelocity": -7.338884690956896e-17, + "velocityX": -3.966965327371491, + "velocityY": 0.7193999604170215, + "timestamp": 0.9717877907874948 + }, + { + "x": 12.039925875231576, + "y": 7.3732095068305945, + "heading": 3.5682697108658626e-18, + "angularVelocity": 1.5280427788067176e-16, + "velocityX": -4.014009674097683, + "velocityY": 0.37693021244540154, + "timestamp": 1.0177657503940518 + }, + { + "x": 11.85456371307373, + "y": 7.374666213989241, + "heading": 5.367949901095069e-29, + "angularVelocity": -7.760826581445421e-17, + "velocityX": -4.031543890682136, + "velocityY": 0.03168272735699931, + "timestamp": 1.0637437100006089 + }, + { + "x": 11.58382573658003, + "y": 7.376793864766986, + "heading": 3.611107370087949e-19, + "angularVelocity": 5.3772800315900255e-18, + "velocityX": -4.031544248842776, + "velocityY": 0.031682730171678136, + "timestamp": 1.1308986163548016 + }, + { + "x": 11.313087760086365, + "y": 7.378921515544708, + "heading": 6.613128665612877e-19, + "angularVelocity": 4.4702933241501485e-18, + "velocityX": -4.031544248842776, + "velocityY": 0.031682730171678136, + "timestamp": 1.1980535227089943 + }, + { + "x": 11.042349783592421, + "y": 7.381049166322431, + "heading": 1.7859545454902864e-18, + "angularVelocity": 1.674697710103153e-17, + "velocityX": -4.031544248842776, + "velocityY": 0.031682730171678136, + "timestamp": 1.265208429063187 + }, + { + "x": 10.771611807098852, + "y": 7.383176817100155, + "heading": 2.1088147678209187e-18, + "angularVelocity": 4.807693731778367e-18, + "velocityX": -4.031544248842776, + "velocityY": 0.031682730171678136, + "timestamp": 1.3323633354173798 + }, + { + "x": 10.500873830605814, + "y": 7.385304467877871, + "heading": 2.3845787851703935e-18, + "angularVelocity": 4.1063867456177466e-18, + "velocityX": -4.031544248830867, + "velocityY": 0.03168273017158455, + "timestamp": 1.3995182417715726 + }, + { + "x": 10.262320186935474, + "y": 7.387179191372732, + "heading": 8.261235716651573e-19, + "angularVelocity": -2.3206870474634867e-17, + "velocityX": -3.5522891270589096, + "velocityY": 0.02791640397763177, + "timestamp": 1.4666731481257653 + }, + { + "x": 10.057673285180528, + "y": 7.388787451674137, + "heading": -3.8938543453196834e-19, + "angularVelocity": -1.8100077450473204e-17, + "velocityX": -3.0473857066471712, + "velocityY": 0.023948515286777196, + "timestamp": 1.533828054479958 + }, + { + "x": 9.886933144144443, + "y": 7.39012924863431, + "heading": -1.0593998775036432e-18, + "angularVelocity": -9.977148049779185e-18, + "velocityX": -2.5424820062315496, + "velocityY": 0.019980624395453776, + "timestamp": 1.6009829608341508 + }, + { + "x": 9.750099770096462, + "y": 7.391204582203997, + "heading": -1.3638864252586915e-18, + "angularVelocity": -4.534092358681147e-18, + "velocityX": -2.0375782124799673, + "velocityY": 0.01601273277063028, + "timestamp": 1.6681378671883436 + }, + { + "x": 9.647173166169406, + "y": 7.392013452358564, + "heading": -1.2574112860570634e-18, + "angularVelocity": 1.5855154148597377e-18, + "velocityX": -1.5326743720601586, + "velocityY": 0.012044840779054804, + "timestamp": 1.7352927735425363 + }, + { + "x": 9.578153334243916, + "y": 7.392555859083237, + "heading": -7.668820492443921e-19, + "angularVelocity": 7.304443762183156e-18, + "velocityX": -1.0277705036393359, + "velocityY": 0.008076948567427526, + "timestamp": 1.802447679896729 + }, + { + "x": 9.54304027557373, + "y": 7.392831802368181, + "heading": 1.9853085976678412e-29, + "angularVelocity": 1.1419598223030353e-17, + "velocityX": -0.5228666165511381, + "velocityY": 0.004109056209099373, + "timestamp": 1.8696025862509218 + }, + { + "x": 9.550006403554065, + "y": 7.36520669470756, + "heading": 8.208453400112202e-18, + "angularVelocity": 9.126792762218019e-17, + "velocityX": 0.0774547936533035, + "velocityY": -0.3071572930535228, + "timestamp": 1.9595405700962467 + }, + { + "x": 9.609958464307818, + "y": 7.307727015265158, + "heading": 1.1900950012142629e-17, + "angularVelocity": 4.1056030546201247e-17, + "velocityX": 0.6665933367692368, + "velocityY": -0.639103490926102, + "timestamp": 2.049478553941572 + }, + { + "x": 9.720571256960566, + "y": 7.216597756490272, + "heading": 1.5776992410646012e-17, + "angularVelocity": 4.3096834427112364e-17, + "velocityX": 1.229878499866957, + "velocityY": -1.0132455151719792, + "timestamp": 2.139416537786897 + }, + { + "x": 9.872189918308363, + "y": 7.080553903052873, + "heading": 1.1667578992513797e-17, + "angularVelocity": -4.5691633750798436e-17, + "velocityX": 1.6858134334938062, + "velocityY": -1.5126406844005709, + "timestamp": 2.229354521632222 + }, + { + "x": 9.973633909336314, + "y": 6.910139985134272, + "heading": 4.645940827916874e-18, + "angularVelocity": -7.807199877514019e-17, + "velocityX": 1.127932678615688, + "velocityY": -1.8947936192529897, + "timestamp": 2.3192925054775473 + }, + { + "x": 10.014541282771518, + "y": 6.745567406575643, + "heading": 2.794830244897641e-18, + "angularVelocity": -2.0582077826162586e-17, + "velocityX": 0.4548397872199964, + "velocityY": -1.829845094611647, + "timestamp": 2.4092304893228724 + }, + { + "x": 9.996101379394531, + "y": 6.594287872314453, + "heading": -5.340124287151323e-29, + "angularVelocity": -3.10750822452399e-17, + "velocityX": -0.2050290943668352, + "velocityY": -1.6820427564994032, + "timestamp": 2.4991684731681976 + }, + { + "x": 9.957997396822789, + "y": 6.500960065054064, + "heading": -2.2271639579965374e-18, + "angularVelocity": -3.728616719364335e-17, + "velocityX": -0.6379195657653035, + "velocityY": -1.5624517507922602, + "timestamp": 2.5589001142976375 + }, + { + "x": 9.894155362526096, + "y": 6.415193876390115, + "heading": -4.219859523652492e-18, + "angularVelocity": -3.336080388780194e-17, + "velocityX": -1.0688143350748942, + "velocityY": -1.435858567456612, + "timestamp": 2.6186317554270775 + }, + { + "x": 9.804912436862404, + "y": 6.338054378980702, + "heading": -4.43830875194841e-19, + "angularVelocity": 6.321655620201416e-17, + "velocityX": -1.4940645188418495, + "velocityY": -1.2914344215363034, + "timestamp": 2.6783633965565175 + }, + { + "x": 9.694406652742682, + "y": 6.277270726595081, + "heading": 4.641364645868438e-18, + "angularVelocity": 8.51340332351435e-17, + "velocityX": -1.8500376355013004, + "velocityY": -1.0176122945274693, + "timestamp": 2.7380950376859574 + }, + { + "x": 9.610661177390286, + "y": 6.214613448346474, + "heading": 3.604917083029758e-18, + "angularVelocity": -1.7351734243372534e-17, + "velocityX": -1.4020287031946272, + "velocityY": -1.0489796875466244, + "timestamp": 2.7978266788153974 + }, + { + "x": 9.553384342427035, + "y": 6.147593104501113, + "heading": 1.4115319396353346e-18, + "angularVelocity": -3.6720657626017165e-17, + "velocityX": -0.9589027503719386, + "velocityY": -1.1220241496483352, + "timestamp": 2.8575583199448373 + }, + { + "x": 9.522457122802734, + "y": 6.075540542602539, + "heading": 7.836358279698028e-29, + "angularVelocity": -2.363122647941345e-17, + "velocityX": -0.5177694608671554, + "velocityY": -1.2062712581834834, + "timestamp": 2.9172899610742773 + }, + { + "x": 9.519123714897153, + "y": 5.993460121280647, + "heading": -6.857914452691464e-19, + "angularVelocity": -1.0867971841834439e-17, + "velocityX": -0.05282565639883304, + "velocityY": -1.300756540044744, + "timestamp": 2.980392024215985 + }, + { + "x": 9.54498029319509, + "y": 5.904726543842293, + "heading": -2.7444802624472655e-18, + "angularVelocity": -3.262474655691004e-17, + "velocityX": 0.40975804927125337, + "velocityY": -1.4061913829835633, + "timestamp": 3.0434940873576926 + }, + { + "x": 9.599724821974295, + "y": 5.808131289394199, + "heading": -1.9295807077760543e-18, + "angularVelocity": 1.2913992254151298e-17, + "velocityX": 0.8675552914373884, + "velocityY": -1.5307780703012885, + "timestamp": 3.1065961504994 + }, + { + "x": 9.682510378590626, + "y": 5.701046987518788, + "heading": -3.002183724926251e-19, + "angularVelocity": 2.5821062800849368e-17, + "velocityX": 1.3119310604856096, + "velocityY": -1.697001596206646, + "timestamp": 3.169698213641108 + }, + { + "x": 9.78801044548454, + "y": 5.574459625498355, + "heading": 1.5877410391790831e-18, + "angularVelocity": 2.991913921092726e-17, + "velocityX": 1.6718956820317172, + "velocityY": -2.0060732679398616, + "timestamp": 3.2328002767828155 + }, + { + "x": 9.86933763233761, + "y": 5.430209029921785, + "heading": 1.972367537583804e-18, + "angularVelocity": 6.095307811136636e-18, + "velocityX": 1.2888197755188047, + "velocityY": -2.2859885777843494, + "timestamp": 3.295902339924523 + }, + { + "x": 9.921073700834063, + "y": 5.281410358531107, + "heading": 1.7086835837171051e-18, + "angularVelocity": -4.178689899770491e-18, + "velocityX": 0.819879191275729, + "velocityY": -2.358063492417386, + "timestamp": 3.3590044030662307 + }, + { + "x": 9.942900657653809, + "y": 5.131283283233643, + "heading": -6.833866856560672e-29, + "angularVelocity": -2.707809378572145e-17, + "velocityX": 0.3458992580120465, + "velocityY": -2.37911516395788, + "timestamp": 3.4221064662079383 + }, + { + "x": 9.928880027543176, + "y": 4.960225125749519, + "heading": -2.857729865103397e-18, + "angularVelocity": -3.972939752645852e-17, + "velocityX": -0.19492086850405763, + "velocityY": -2.3781245463585914, + "timestamp": 3.4940363227606785 + }, + { + "x": 9.876199175238394, + "y": 4.793490015189766, + "heading": -6.713793512065014e-18, + "angularVelocity": -5.360866588277142e-17, + "velocityX": -0.7323920111832362, + "velocityY": -2.318023676823262, + "timestamp": 3.5659661793134187 + }, + { + "x": 9.787502173908543, + "y": 4.641456535864866, + "heading": -1.1563620063466963e-17, + "angularVelocity": -6.742438792180666e-17, + "velocityX": -1.233104104202085, + "velocityY": -2.113635235924965, + "timestamp": 3.637896035866159 + }, + { + "x": 9.689354804369618, + "y": 4.527158855547807, + "heading": -6.146646541729309e-18, + "angularVelocity": 7.530911058881295e-17, + "velocityX": -1.3644872135531292, + "velocityY": -1.5890158245102814, + "timestamp": 3.709825892418899 + }, + { + "x": 9.61650346743172, + "y": 4.442414714293235, + "heading": -6.652822488921985e-18, + "angularVelocity": -7.03707711124707e-18, + "velocityX": -1.0128108191690985, + "velocityY": -1.1781497324749555, + "timestamp": 3.7817557489716394 + }, + { + "x": 9.576163220155488, + "y": 4.379032468037305, + "heading": -3.620092957118811e-18, + "angularVelocity": 4.216231864176792e-17, + "velocityX": -0.5608275785543259, + "velocityY": -0.8811674218960207, + "timestamp": 3.8536856055243796 + }, + { + "x": 9.570499420166016, + "y": 4.33328104019165, + "heading": 6.389355081534522e-29, + "angularVelocity": 5.032809921647081e-17, + "velocityX": -0.07874059897949862, + "velocityY": -0.6360561530121928, + "timestamp": 3.9256154620771198 + }, + { + "x": 9.59025662728202, + "y": 4.3060861435258255, + "heading": 3.4405474170918484e-18, + "angularVelocity": 5.774970157610978e-17, + "velocityX": 0.3316253713844863, + "velocityY": -0.45646723515195037, + "timestamp": 3.98519234859072 + }, + { + "x": 9.634928766499318, + "y": 4.288453823273995, + "heading": 7.258280629472283e-18, + "angularVelocity": 6.40807775591821e-17, + "velocityX": 0.7498233263180065, + "velocityY": -0.2959590754680539, + "timestamp": 4.0447692351043205 + }, + { + "x": 9.705066673913727, + "y": 4.278802186634717, + "heading": 9.257468175317858e-18, + "angularVelocity": 3.3556428722662696e-17, + "velocityX": 1.17726708324039, + "velocityY": -0.16200303849505196, + "timestamp": 4.104346121617921 + }, + { + "x": 9.80128303549785, + "y": 4.274817191136606, + "heading": 6.671362808308425e-18, + "angularVelocity": -4.3407863659556446e-17, + "velocityX": 1.614994794368075, + "velocityY": -0.06688828052808415, + "timestamp": 4.163923008131521 + }, + { + "x": 9.924104879660334, + "y": 4.27291661282852, + "heading": 3.995960020606412e-18, + "angularVelocity": -4.490672380345128e-17, + "velocityX": 2.061568694672314, + "velocityY": -0.03190126942354246, + "timestamp": 4.223499894645121 + }, + { + "x": 10.073354763097997, + "y": 4.267307247053675, + "heading": 8.681036334086009e-19, + "angularVelocity": -5.250117235511423e-17, + "velocityX": 2.5051642032954144, + "velocityY": -0.09415338905910305, + "timestamp": 4.283076781158721 + }, + { + "x": 10.246135711669922, + "y": 4.249108791351318, + "heading": -6.39603491434045e-29, + "angularVelocity": -1.4571148045948685e-17, + "velocityX": 2.900133905662932, + "velocityY": -0.3054616776289984, + "timestamp": 4.342653667672321 + }, + { + "x": 10.397533223464032, + "y": 4.2194926871368, + "heading": -1.4214934612932198e-18, + "angularVelocity": -2.9172605523862634e-17, + "velocityX": 3.1070560712521975, + "velocityY": -0.6077966230494994, + "timestamp": 4.391380664650234 + }, + { + "x": 10.560063970116651, + "y": 4.175921674279642, + "heading": -8.355398486506036e-18, + "angularVelocity": -1.4230109498284303e-16, + "velocityX": 3.335537930365231, + "velocityY": -0.8941862942407067, + "timestamp": 4.440107661628147 + }, + { + "x": 10.733231337260525, + "y": 4.146687525463282, + "heading": -7.256703988845365e-18, + "angularVelocity": 2.2547962440387354e-17, + "velocityX": 3.553828019041837, + "velocityY": -0.5999579417876079, + "timestamp": 4.48883465860606 + }, + { + "x": 10.899397747529992, + "y": 4.13387516385498, + "heading": -4.91148810214039e-18, + "angularVelocity": 4.8129702879046774e-17, + "velocityX": 3.4101508522019155, + "velocityY": -0.26294174488341465, + "timestamp": 4.537561655583973 + }, + { + "x": 11.057563871843254, + "y": 4.137021634089263, + "heading": -2.6432866762848484e-18, + "angularVelocity": 4.65491732830131e-17, + "velocityX": 3.245964950086196, + "velocityY": 0.06457344858966835, + "timestamp": 4.586288652561886 + }, + { + "x": 11.207388703066018, + "y": 4.155951382100819, + "heading": -4.097530137639523e-18, + "angularVelocity": -2.984471753889731e-17, + "velocityX": 3.074780727625754, + "velocityY": 0.38848583302059725, + "timestamp": 4.635015649539799 + }, + { + "x": 11.34870051105692, + "y": 4.190572450443893, + "heading": -5.45090094005365e-18, + "angularVelocity": -2.777455797239923e-17, + "velocityX": 2.9000721726183385, + "velocityY": 0.7105110203850111, + "timestamp": 4.683742646517712 + }, + { + "x": 11.48139590202311, + "y": 4.240828303678194, + "heading": -1.3594070863501201e-18, + "angularVelocity": 8.396769978639009e-17, + "velocityX": 2.723241718063186, + "velocityY": 1.0313759589387619, + "timestamp": 4.732469643495625 + }, + { + "x": 11.605405807495115, + "y": 4.306680679321289, + "heading": 1.1498940545297757e-28, + "angularVelocity": 2.7898437638409835e-17, + "velocityX": 2.5449938055533585, + "velocityY": 1.351455655536201, + "timestamp": 4.781196640473538 + }, + { + "x": 11.704841734407355, + "y": 4.374190201271196, + "heading": 1.2913487502887817e-18, + "angularVelocity": 3.1065136615300746e-17, + "velocityX": 2.392065391928354, + "velocityY": 1.6240326418893487, + "timestamp": 4.822765707626873 + }, + { + "x": 11.798140777460658, + "y": 4.4531512570442136, + "heading": 1.880571563314064e-18, + "angularVelocity": 1.4174549810487774e-17, + "velocityX": 2.2444343701327476, + "velocityY": 1.8995147396922663, + "timestamp": 4.864334774780208 + }, + { + "x": 11.88562887896236, + "y": 4.543732635858822, + "heading": 6.144405284004916e-18, + "angularVelocity": 1.0257227338968366e-16, + "velocityX": 2.1046443303378966, + "velocityY": 2.179057289894958, + "timestamp": 4.905903841933544 + }, + { + "x": 11.96783586421051, + "y": 4.646184512849362, + "heading": 1.0774137839314388e-17, + "angularVelocity": 1.1137446356799422e-16, + "velocityX": 1.9775999529870423, + "velocityY": 2.4646181405184553, + "timestamp": 4.947472909086879 + }, + { + "x": 12.045762443377628, + "y": 4.7609032956811985, + "heading": 1.1033598708280907e-17, + "angularVelocity": 6.2416812945219875e-18, + "velocityX": 1.8746290091059812, + "velocityY": 2.7597151124097876, + "timestamp": 4.989041976240214 + }, + { + "x": 12.121881659014273, + "y": 4.88848800851973, + "heading": 1.0993808732719266e-17, + "angularVelocity": -9.57201553253283e-19, + "velocityX": 1.831150440683814, + "velocityY": 3.069222418870066, + "timestamp": 5.03061104339355 + }, + { + "x": 12.20514803910829, + "y": 5.026922440288341, + "heading": 8.23831147224171e-18, + "angularVelocity": -6.628720462737835e-17, + "velocityX": 2.003085125458145, + "velocityY": 3.330227047385233, + "timestamp": 5.072180110546885 + }, + { + "x": 12.301013946533203, + "y": 5.162186622619629, + "heading": -8.137476723677248e-29, + "angularVelocity": -1.9818369851840407e-16, + "velocityX": 2.306183756091877, + "velocityY": 3.2539624195159407, + "timestamp": 5.11374917770022 + }, + { + "x": 12.499881584010561, + "y": 5.374941033273301, + "heading": -1.68670430873798e-17, + "angularVelocity": -2.335032342813133e-16, + "velocityX": 2.7530751124840362, + "velocityY": 2.9453202163601047, + "timestamp": 5.185983907721052 + }, + { + "x": 12.723364661530127, + "y": 5.557147112844672, + "heading": -1.941639399809902e-17, + "angularVelocity": -3.529259277341286e-17, + "velocityX": 3.0938452660529427, + "velocityY": 2.522416564979513, + "timestamp": 5.2582186377418845 + }, + { + "x": 12.948963863963927, + "y": 5.700178683736988, + "heading": -1.793322327115035e-17, + "angularVelocity": 2.053265411966626e-17, + "velocityX": 3.123140383701003, + "velocityY": 1.980094213006201, + "timestamp": 5.330453367762717 + }, + { + "x": 13.15514980795229, + "y": 5.809118526061311, + "heading": -2.073342818682101e-17, + "angularVelocity": -3.876535448899066e-17, + "velocityX": 2.854387964472203, + "velocityY": 1.5081366303010209, + "timestamp": 5.402688097783549 + }, + { + "x": 13.332368406825639, + "y": 5.89160059122476, + "heading": -1.6325337936238524e-17, + "angularVelocity": 6.102452725036477e-17, + "velocityX": 2.4533710975625906, + "velocityY": 1.1418616106083745, + "timestamp": 5.474922827804381 + }, + { + "x": 13.476660624324403, + "y": 5.952752518585699, + "heading": -1.4094237433699954e-17, + "angularVelocity": 3.0886811674822726e-17, + "velocityX": 1.9975462974271516, + "velocityY": 0.8465723806720552, + "timestamp": 5.547157557825213 + }, + { + "x": 13.586113710702598, + "y": 5.995866804511295, + "heading": -8.214620498966636e-18, + "angularVelocity": 8.139598407904596e-17, + "velocityX": 1.5152418559137708, + "velocityY": 0.5968636681158963, + "timestamp": 5.619392287846045 + }, + { + "x": 13.659664660636231, + "y": 6.023164697186193, + "heading": -3.503128075700943e-18, + "angularVelocity": 6.52247529949022e-17, + "velocityX": 1.0182214277317942, + "velocityY": 0.37790537414654557, + "timestamp": 5.691627017866877 + }, + { + "x": 13.69665813446045, + "y": 6.036226272583008, + "heading": -5.6863338433321666e-30, + "angularVelocity": 4.8496451425533246e-17, + "velocityX": 0.5121286369250458, + "velocityY": 0.18082126690371453, + "timestamp": 5.763861747887709 + }, + { + "x": 13.69665813446045, + "y": 6.036226272583008, + "heading": -3.4479150184640595e-30, + "angularVelocity": -1.6743970569933744e-29, + "velocityX": 6.6279365304051e-27, + "velocityY": -2.722134005564902e-26, + "timestamp": 5.836096477908542 + } + ], + "eventMarkers": [ + { + "timestamp": 0, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + }, + { + "timestamp": 1.0637437100006089, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "outtakeShooter" + } + }, + { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + ] + } + } + }, + { + "timestamp": 2.4991684731681976, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "outtakeShooter" + } + }, + { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + ] + } + } + }, + { + "timestamp": 3.4221064662079383, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "outtakeShooter" + } + }, + { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + ] + } + } + }, + { + "timestamp": 4.342653667672321, + "command": { + "type": "named", + "data": { + "name": "outtakeShooter" + } + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red AS to 6 + NT.traj b/src/main/deploy/choreo/Red AS to 6 + NT.traj new file mode 100644 index 00000000..c27a10f4 --- /dev/null +++ b/src/main/deploy/choreo/Red AS to 6 + NT.traj @@ -0,0 +1,945 @@ +{ + "samples": [ + { + "x": 15.151942253112791, + "y": 6.983804225921631, + "heading": 2.460658578276488e-30, + "angularVelocity": 1.1559528303191587e-30, + "velocityX": -8.450980122730519e-24, + "velocityY": -1.0227558064600819e-22, + "timestamp": 0 + }, + { + "x": 15.118082717686605, + "y": 6.983746166696196, + "heading": 9.024737996051924e-20, + "angularVelocity": 1.344825629304957e-18, + "velocityX": -0.5045594792879681, + "velocityY": -0.0008651723121594873, + "timestamp": 0.06710712377055983 + }, + { + "x": 15.050363647796937, + "y": 6.983630048249095, + "heading": 2.4316871133633307e-19, + "angularVelocity": 2.278764500415112e-18, + "velocityX": -1.0091189442301196, + "velocityY": -0.0017303445681617134, + "timestamp": 0.13421424754111966 + }, + { + "x": 14.948785044791569, + "y": 6.983455870585603, + "heading": 5.35970791472245e-19, + "angularVelocity": 4.3632041381261375e-18, + "velocityX": -1.5136783890882703, + "velocityY": -0.0025955167455443302, + "timestamp": 0.2013213713116795 + }, + { + "x": 14.813346910692148, + "y": 6.983223633713634, + "heading": 9.216072681731045e-19, + "angularVelocity": 5.746580318609512e-18, + "velocityX": -2.018237803820731, + "velocityY": -0.003460688804998752, + "timestamp": 0.2684284950822393 + }, + { + "x": 14.644049248868034, + "y": 6.982933337646379, + "heading": 1.4181741486552305e-18, + "angularVelocity": 7.399615012222265e-18, + "velocityX": -2.5227971683445407, + "velocityY": -0.00432586066790944, + "timestamp": 0.33553561885279914 + }, + { + "x": 14.440892066057732, + "y": 6.982584982410215, + "heading": 2.0075804989291975e-18, + "angularVelocity": 8.783066791637582e-18, + "velocityX": -3.0273564324541744, + "velocityY": -0.005191032137744908, + "timestamp": 0.402642742623359 + }, + { + "x": 14.203875382475081, + "y": 6.982178568084268, + "heading": 2.7079281574609036e-18, + "angularVelocity": 1.0436263978825298e-17, + "velocityX": -3.5319153953463394, + "velocityY": -0.006056202428452966, + "timestamp": 0.4697498663939188 + }, + { + "x": 13.93332208875509, + "y": 6.981715355253932, + "heading": 7.040762676561354e-20, + "angularVelocity": -3.930313776694512e-17, + "velocityX": -4.031662788067243, + "velocityY": -0.006902588045925373, + "timestamp": 0.5368569901644786 + }, + { + "x": 13.663607597351074, + "y": 7.0030083656311035, + "heading": -6.322941131722835e-29, + "angularVelocity": -1.0491826035121783e-18, + "velocityX": -4.019163335418371, + "velocityY": 0.3172988079473022, + "timestamp": 0.6039641139350385 + }, + { + "x": 13.48074370743158, + "y": 7.033373665695924, + "heading": 7.719060237002786e-19, + "angularVelocity": 1.6788609812813145e-17, + "velocityX": -3.9772075899908805, + "velocityY": 0.6604316573563296, + "timestamp": 0.6499420735415955 + }, + { + "x": 13.30115269323578, + "y": 7.079290127414415, + "heading": 1.80497495080332e-18, + "angularVelocity": 2.2468785827024268e-17, + "velocityX": -3.9060240109086752, + "velocityY": 0.9986624485168062, + "timestamp": 0.6959200331481525 + }, + { + "x": 13.12230781574745, + "y": 7.128031933339034, + "heading": 2.9475018721988785e-18, + "angularVelocity": 2.4849448110195845e-17, + "velocityX": -3.889795872168816, + "velocityY": 1.0601124178130732, + "timestamp": 0.7418979927547096 + }, + { + "x": 12.943462985340895, + "y": 7.1767739120171665, + "heading": 1.682270387833829e-18, + "angularVelocity": -2.7518217318289784e-17, + "velocityX": -3.8897948481613316, + "velocityY": 1.0601161751245307, + "timestamp": 0.7878759523612666 + }, + { + "x": 12.764618154935798, + "y": 7.225515890700651, + "heading": 1.3123960398950925e-18, + "angularVelocity": -8.044601175075305e-18, + "velocityX": -3.889794848129619, + "velocityY": 1.060116175240888, + "timestamp": 0.8338539119678237 + }, + { + "x": 12.585773321348107, + "y": 7.27425785770651, + "heading": 9.730502655500534e-19, + "angularVelocity": -7.380618391695251e-18, + "velocityX": -3.889794917349603, + "velocityY": 1.060115921257809, + "timestamp": 0.8798318715743807 + }, + { + "x": 12.406874821470067, + "y": 7.322802482427277, + "heading": -8.30897663402979e-20, + "angularVelocity": -2.297057200597194e-17, + "velocityX": -3.890962135094999, + "velocityY": 1.0558238150664485, + "timestamp": 0.9258098311809377 + }, + { + "x": 12.224481849887567, + "y": 7.3558790247482895, + "heading": -3.4573592051805234e-18, + "angularVelocity": -7.338884690956896e-17, + "velocityX": -3.966965327371491, + "velocityY": 0.7193999604170215, + "timestamp": 0.9717877907874948 + }, + { + "x": 12.039925875231576, + "y": 7.3732095068305945, + "heading": 3.5682697108658626e-18, + "angularVelocity": 1.5280427788067176e-16, + "velocityX": -4.014009674097683, + "velocityY": 0.37693021244540154, + "timestamp": 1.0177657503940518 + }, + { + "x": 11.85456371307373, + "y": 7.374666213989241, + "heading": 5.367949901095069e-29, + "angularVelocity": -7.760826581445421e-17, + "velocityX": -4.031543890682136, + "velocityY": 0.03168272735699931, + "timestamp": 1.0637437100006089 + }, + { + "x": 11.58382573658003, + "y": 7.376793864766986, + "heading": 3.611107370087949e-19, + "angularVelocity": 5.3772800315900255e-18, + "velocityX": -4.031544248842776, + "velocityY": 0.031682730171678136, + "timestamp": 1.1308986163548016 + }, + { + "x": 11.313087760086365, + "y": 7.378921515544708, + "heading": 6.613128665612877e-19, + "angularVelocity": 4.4702933241501485e-18, + "velocityX": -4.031544248842776, + "velocityY": 0.031682730171678136, + "timestamp": 1.1980535227089943 + }, + { + "x": 11.042349783592421, + "y": 7.381049166322431, + "heading": 1.7859545454902864e-18, + "angularVelocity": 1.674697710103153e-17, + "velocityX": -4.031544248842776, + "velocityY": 0.031682730171678136, + "timestamp": 1.265208429063187 + }, + { + "x": 10.771611807098852, + "y": 7.383176817100155, + "heading": 2.1088147678209187e-18, + "angularVelocity": 4.807693731778367e-18, + "velocityX": -4.031544248842776, + "velocityY": 0.031682730171678136, + "timestamp": 1.3323633354173798 + }, + { + "x": 10.500873830605814, + "y": 7.385304467877871, + "heading": 2.3845787851703935e-18, + "angularVelocity": 4.1063867456177466e-18, + "velocityX": -4.031544248830867, + "velocityY": 0.03168273017158455, + "timestamp": 1.3995182417715726 + }, + { + "x": 10.262320186935474, + "y": 7.387179191372732, + "heading": 8.261235716651573e-19, + "angularVelocity": -2.3206870474634867e-17, + "velocityX": -3.5522891270589096, + "velocityY": 0.02791640397763177, + "timestamp": 1.4666731481257653 + }, + { + "x": 10.057673285180528, + "y": 7.388787451674137, + "heading": -3.8938543453196834e-19, + "angularVelocity": -1.8100077450473204e-17, + "velocityX": -3.0473857066471712, + "velocityY": 0.023948515286777196, + "timestamp": 1.533828054479958 + }, + { + "x": 9.886933144144443, + "y": 7.39012924863431, + "heading": -1.0593998775036432e-18, + "angularVelocity": -9.977148049779185e-18, + "velocityX": -2.5424820062315496, + "velocityY": 0.019980624395453776, + "timestamp": 1.6009829608341508 + }, + { + "x": 9.750099770096462, + "y": 7.391204582203997, + "heading": -1.3638864252586915e-18, + "angularVelocity": -4.534092358681147e-18, + "velocityX": -2.0375782124799673, + "velocityY": 0.01601273277063028, + "timestamp": 1.6681378671883436 + }, + { + "x": 9.647173166169406, + "y": 7.392013452358564, + "heading": -1.2574112860570634e-18, + "angularVelocity": 1.5855154148597377e-18, + "velocityX": -1.5326743720601586, + "velocityY": 0.012044840779054804, + "timestamp": 1.7352927735425363 + }, + { + "x": 9.578153334243916, + "y": 7.392555859083237, + "heading": -7.668820492443921e-19, + "angularVelocity": 7.304443762183156e-18, + "velocityX": -1.0277705036393359, + "velocityY": 0.008076948567427526, + "timestamp": 1.802447679896729 + }, + { + "x": 9.54304027557373, + "y": 7.392831802368181, + "heading": 1.9853085976678412e-29, + "angularVelocity": 1.1419598223030353e-17, + "velocityX": -0.5228666165511381, + "velocityY": 0.004109056209099373, + "timestamp": 1.8696025862509218 + }, + { + "x": 9.550006403554065, + "y": 7.36520669470756, + "heading": 8.208453400112202e-18, + "angularVelocity": 9.126792762218019e-17, + "velocityX": 0.0774547936533035, + "velocityY": -0.3071572930535228, + "timestamp": 1.9595405700962467 + }, + { + "x": 9.609958464307818, + "y": 7.307727015265158, + "heading": 1.1900950012142629e-17, + "angularVelocity": 4.1056030546201247e-17, + "velocityX": 0.6665933367692368, + "velocityY": -0.639103490926102, + "timestamp": 2.049478553941572 + }, + { + "x": 9.720571256960566, + "y": 7.216597756490272, + "heading": 1.5776992410646012e-17, + "angularVelocity": 4.3096834427112364e-17, + "velocityX": 1.229878499866957, + "velocityY": -1.0132455151719792, + "timestamp": 2.139416537786897 + }, + { + "x": 9.872189918308363, + "y": 7.080553903052873, + "heading": 1.1667578992513797e-17, + "angularVelocity": -4.5691633750798436e-17, + "velocityX": 1.6858134334938062, + "velocityY": -1.5126406844005709, + "timestamp": 2.229354521632222 + }, + { + "x": 9.973633909336314, + "y": 6.910139985134272, + "heading": 4.645940827916874e-18, + "angularVelocity": -7.807199877514019e-17, + "velocityX": 1.127932678615688, + "velocityY": -1.8947936192529897, + "timestamp": 2.3192925054775473 + }, + { + "x": 10.014541282771518, + "y": 6.745567406575643, + "heading": 2.794830244897641e-18, + "angularVelocity": -2.0582077826162586e-17, + "velocityX": 0.4548397872199964, + "velocityY": -1.829845094611647, + "timestamp": 2.4092304893228724 + }, + { + "x": 9.996101379394531, + "y": 6.594287872314453, + "heading": -5.340124287151323e-29, + "angularVelocity": -3.10750822452399e-17, + "velocityX": -0.2050290943668352, + "velocityY": -1.6820427564994032, + "timestamp": 2.4991684731681976 + }, + { + "x": 9.957997396822789, + "y": 6.500960065054064, + "heading": -2.2271639579965374e-18, + "angularVelocity": -3.728616719364335e-17, + "velocityX": -0.6379195657653035, + "velocityY": -1.5624517507922602, + "timestamp": 2.5589001142976375 + }, + { + "x": 9.894155362526096, + "y": 6.415193876390115, + "heading": -4.219859523652492e-18, + "angularVelocity": -3.336080388780194e-17, + "velocityX": -1.0688143350748942, + "velocityY": -1.435858567456612, + "timestamp": 2.6186317554270775 + }, + { + "x": 9.804912436862404, + "y": 6.338054378980702, + "heading": -4.43830875194841e-19, + "angularVelocity": 6.321655620201416e-17, + "velocityX": -1.4940645188418495, + "velocityY": -1.2914344215363034, + "timestamp": 2.6783633965565175 + }, + { + "x": 9.694406652742682, + "y": 6.277270726595081, + "heading": 4.641364645868438e-18, + "angularVelocity": 8.51340332351435e-17, + "velocityX": -1.8500376355013004, + "velocityY": -1.0176122945274693, + "timestamp": 2.7380950376859574 + }, + { + "x": 9.610661177390286, + "y": 6.214613448346474, + "heading": 3.604917083029758e-18, + "angularVelocity": -1.7351734243372534e-17, + "velocityX": -1.4020287031946272, + "velocityY": -1.0489796875466244, + "timestamp": 2.7978266788153974 + }, + { + "x": 9.553384342427035, + "y": 6.147593104501113, + "heading": 1.4115319396353346e-18, + "angularVelocity": -3.6720657626017165e-17, + "velocityX": -0.9589027503719386, + "velocityY": -1.1220241496483352, + "timestamp": 2.8575583199448373 + }, + { + "x": 9.522457122802734, + "y": 6.075540542602539, + "heading": 7.836358279698028e-29, + "angularVelocity": -2.363122647941345e-17, + "velocityX": -0.5177694608671554, + "velocityY": -1.2062712581834834, + "timestamp": 2.9172899610742773 + }, + { + "x": 9.519123714897153, + "y": 5.993460121280647, + "heading": -6.857914452691464e-19, + "angularVelocity": -1.0867971841834439e-17, + "velocityX": -0.05282565639883304, + "velocityY": -1.300756540044744, + "timestamp": 2.980392024215985 + }, + { + "x": 9.54498029319509, + "y": 5.904726543842293, + "heading": -2.7444802624472655e-18, + "angularVelocity": -3.262474655691004e-17, + "velocityX": 0.40975804927125337, + "velocityY": -1.4061913829835633, + "timestamp": 3.0434940873576926 + }, + { + "x": 9.599724821974295, + "y": 5.808131289394199, + "heading": -1.9295807077760543e-18, + "angularVelocity": 1.2913992254151298e-17, + "velocityX": 0.8675552914373884, + "velocityY": -1.5307780703012885, + "timestamp": 3.1065961504994 + }, + { + "x": 9.682510378590626, + "y": 5.701046987518788, + "heading": -3.002183724926251e-19, + "angularVelocity": 2.5821062800849368e-17, + "velocityX": 1.3119310604856096, + "velocityY": -1.697001596206646, + "timestamp": 3.169698213641108 + }, + { + "x": 9.78801044548454, + "y": 5.574459625498355, + "heading": 1.5877410391790831e-18, + "angularVelocity": 2.991913921092726e-17, + "velocityX": 1.6718956820317172, + "velocityY": -2.0060732679398616, + "timestamp": 3.2328002767828155 + }, + { + "x": 9.86933763233761, + "y": 5.430209029921785, + "heading": 1.972367537583804e-18, + "angularVelocity": 6.095307811136636e-18, + "velocityX": 1.2888197755188047, + "velocityY": -2.2859885777843494, + "timestamp": 3.295902339924523 + }, + { + "x": 9.921073700834063, + "y": 5.281410358531107, + "heading": 1.7086835837171051e-18, + "angularVelocity": -4.178689899770491e-18, + "velocityX": 0.819879191275729, + "velocityY": -2.358063492417386, + "timestamp": 3.3590044030662307 + }, + { + "x": 9.942900657653809, + "y": 5.131283283233643, + "heading": -6.833866856560672e-29, + "angularVelocity": -2.707809378572145e-17, + "velocityX": 0.3458992580120465, + "velocityY": -2.37911516395788, + "timestamp": 3.4221064662079383 + }, + { + "x": 9.928880027543176, + "y": 4.960225125749519, + "heading": -2.857729865103397e-18, + "angularVelocity": -3.972939752645852e-17, + "velocityX": -0.19492086850405763, + "velocityY": -2.3781245463585914, + "timestamp": 3.4940363227606785 + }, + { + "x": 9.876199175238394, + "y": 4.793490015189766, + "heading": -6.713793512065014e-18, + "angularVelocity": -5.360866588277142e-17, + "velocityX": -0.7323920111832362, + "velocityY": -2.318023676823262, + "timestamp": 3.5659661793134187 + }, + { + "x": 9.787502173908543, + "y": 4.641456535864866, + "heading": -1.1563620063466963e-17, + "angularVelocity": -6.742438792180666e-17, + "velocityX": -1.233104104202085, + "velocityY": -2.113635235924965, + "timestamp": 3.637896035866159 + }, + { + "x": 9.689354804369618, + "y": 4.527158855547807, + "heading": -6.146646541729309e-18, + "angularVelocity": 7.530911058881295e-17, + "velocityX": -1.3644872135531292, + "velocityY": -1.5890158245102814, + "timestamp": 3.709825892418899 + }, + { + "x": 9.61650346743172, + "y": 4.442414714293235, + "heading": -6.652822488921985e-18, + "angularVelocity": -7.03707711124707e-18, + "velocityX": -1.0128108191690985, + "velocityY": -1.1781497324749555, + "timestamp": 3.7817557489716394 + }, + { + "x": 9.576163220155488, + "y": 4.379032468037305, + "heading": -3.620092957118811e-18, + "angularVelocity": 4.216231864176792e-17, + "velocityX": -0.5608275785543259, + "velocityY": -0.8811674218960207, + "timestamp": 3.8536856055243796 + }, + { + "x": 9.570499420166016, + "y": 4.33328104019165, + "heading": 6.389355081534522e-29, + "angularVelocity": 5.032809921647081e-17, + "velocityX": -0.07874059897949862, + "velocityY": -0.6360561530121928, + "timestamp": 3.9256154620771198 + }, + { + "x": 9.59025662728202, + "y": 4.3060861435258255, + "heading": 3.4405474170918484e-18, + "angularVelocity": 5.774970157610978e-17, + "velocityX": 0.3316253713844863, + "velocityY": -0.45646723515195037, + "timestamp": 3.98519234859072 + }, + { + "x": 9.634928766499318, + "y": 4.288453823273995, + "heading": 7.258280629472283e-18, + "angularVelocity": 6.40807775591821e-17, + "velocityX": 0.7498233263180065, + "velocityY": -0.2959590754680539, + "timestamp": 4.0447692351043205 + }, + { + "x": 9.705066673913727, + "y": 4.278802186634717, + "heading": 9.257468175317858e-18, + "angularVelocity": 3.3556428722662696e-17, + "velocityX": 1.17726708324039, + "velocityY": -0.16200303849505196, + "timestamp": 4.104346121617921 + }, + { + "x": 9.80128303549785, + "y": 4.274817191136606, + "heading": 6.671362808308425e-18, + "angularVelocity": -4.3407863659556446e-17, + "velocityX": 1.614994794368075, + "velocityY": -0.06688828052808415, + "timestamp": 4.163923008131521 + }, + { + "x": 9.924104879660334, + "y": 4.27291661282852, + "heading": 3.995960020606412e-18, + "angularVelocity": -4.490672380345128e-17, + "velocityX": 2.061568694672314, + "velocityY": -0.03190126942354246, + "timestamp": 4.223499894645121 + }, + { + "x": 10.073354763097997, + "y": 4.267307247053675, + "heading": 8.681036334086009e-19, + "angularVelocity": -5.250117235511423e-17, + "velocityX": 2.5051642032954144, + "velocityY": -0.09415338905910305, + "timestamp": 4.283076781158721 + }, + { + "x": 10.246135711669922, + "y": 4.249108791351318, + "heading": -6.39603491434045e-29, + "angularVelocity": -1.4571148045948685e-17, + "velocityX": 2.900133905662932, + "velocityY": -0.3054616776289984, + "timestamp": 4.342653667672321 + }, + { + "x": 10.397533223464032, + "y": 4.2194926871368, + "heading": -1.4214934612932198e-18, + "angularVelocity": -2.9172605523862634e-17, + "velocityX": 3.1070560712521975, + "velocityY": -0.6077966230494994, + "timestamp": 4.391380664650234 + }, + { + "x": 10.560063970116651, + "y": 4.175921674279642, + "heading": -8.355398486506036e-18, + "angularVelocity": -1.4230109498284303e-16, + "velocityX": 3.335537930365231, + "velocityY": -0.8941862942407067, + "timestamp": 4.440107661628147 + }, + { + "x": 10.733231337260525, + "y": 4.146687525463282, + "heading": -7.256703988845365e-18, + "angularVelocity": 2.2547962440387354e-17, + "velocityX": 3.553828019041837, + "velocityY": -0.5999579417876079, + "timestamp": 4.48883465860606 + }, + { + "x": 10.899397747529992, + "y": 4.13387516385498, + "heading": -4.91148810214039e-18, + "angularVelocity": 4.8129702879046774e-17, + "velocityX": 3.4101508522019155, + "velocityY": -0.26294174488341465, + "timestamp": 4.537561655583973 + }, + { + "x": 11.057563871843254, + "y": 4.137021634089263, + "heading": -2.6432866762848484e-18, + "angularVelocity": 4.65491732830131e-17, + "velocityX": 3.245964950086196, + "velocityY": 0.06457344858966835, + "timestamp": 4.586288652561886 + }, + { + "x": 11.207388703066018, + "y": 4.155951382100819, + "heading": -4.097530137639523e-18, + "angularVelocity": -2.984471753889731e-17, + "velocityX": 3.074780727625754, + "velocityY": 0.38848583302059725, + "timestamp": 4.635015649539799 + }, + { + "x": 11.34870051105692, + "y": 4.190572450443893, + "heading": -5.45090094005365e-18, + "angularVelocity": -2.777455797239923e-17, + "velocityX": 2.9000721726183385, + "velocityY": 0.7105110203850111, + "timestamp": 4.683742646517712 + }, + { + "x": 11.48139590202311, + "y": 4.240828303678194, + "heading": -1.3594070863501201e-18, + "angularVelocity": 8.396769978639009e-17, + "velocityX": 2.723241718063186, + "velocityY": 1.0313759589387619, + "timestamp": 4.732469643495625 + }, + { + "x": 11.605405807495115, + "y": 4.306680679321289, + "heading": 1.1498940545297757e-28, + "angularVelocity": 2.7898437638409835e-17, + "velocityX": 2.5449938055533585, + "velocityY": 1.351455655536201, + "timestamp": 4.781196640473538 + }, + { + "x": 11.704841734407355, + "y": 4.374190201271196, + "heading": 1.2913487502887817e-18, + "angularVelocity": 3.1065136615300746e-17, + "velocityX": 2.392065391928354, + "velocityY": 1.6240326418893487, + "timestamp": 4.822765707626873 + }, + { + "x": 11.798140777460658, + "y": 4.4531512570442136, + "heading": 1.880571563314064e-18, + "angularVelocity": 1.4174549810487774e-17, + "velocityX": 2.2444343701327476, + "velocityY": 1.8995147396922663, + "timestamp": 4.864334774780208 + }, + { + "x": 11.88562887896236, + "y": 4.543732635858822, + "heading": 6.144405284004916e-18, + "angularVelocity": 1.0257227338968366e-16, + "velocityX": 2.1046443303378966, + "velocityY": 2.179057289894958, + "timestamp": 4.905903841933544 + }, + { + "x": 11.96783586421051, + "y": 4.646184512849362, + "heading": 1.0774137839314388e-17, + "angularVelocity": 1.1137446356799422e-16, + "velocityX": 1.9775999529870423, + "velocityY": 2.4646181405184553, + "timestamp": 4.947472909086879 + }, + { + "x": 12.045762443377628, + "y": 4.7609032956811985, + "heading": 1.1033598708280907e-17, + "angularVelocity": 6.2416812945219875e-18, + "velocityX": 1.8746290091059812, + "velocityY": 2.7597151124097876, + "timestamp": 4.989041976240214 + }, + { + "x": 12.121881659014273, + "y": 4.88848800851973, + "heading": 1.0993808732719266e-17, + "angularVelocity": -9.57201553253283e-19, + "velocityX": 1.831150440683814, + "velocityY": 3.069222418870066, + "timestamp": 5.03061104339355 + }, + { + "x": 12.20514803910829, + "y": 5.026922440288341, + "heading": 8.23831147224171e-18, + "angularVelocity": -6.628720462737835e-17, + "velocityX": 2.003085125458145, + "velocityY": 3.330227047385233, + "timestamp": 5.072180110546885 + }, + { + "x": 12.301013946533203, + "y": 5.162186622619629, + "heading": -8.137476723677248e-29, + "angularVelocity": -1.9818369851840407e-16, + "velocityX": 2.306183756091877, + "velocityY": 3.2539624195159407, + "timestamp": 5.11374917770022 + }, + { + "x": 12.499881584010561, + "y": 5.374941033273301, + "heading": -1.68670430873798e-17, + "angularVelocity": -2.335032342813133e-16, + "velocityX": 2.7530751124840362, + "velocityY": 2.9453202163601047, + "timestamp": 5.185983907721052 + }, + { + "x": 12.723364661530127, + "y": 5.557147112844672, + "heading": -1.941639399809902e-17, + "angularVelocity": -3.529259277341286e-17, + "velocityX": 3.0938452660529427, + "velocityY": 2.522416564979513, + "timestamp": 5.2582186377418845 + }, + { + "x": 12.948963863963927, + "y": 5.700178683736988, + "heading": -1.793322327115035e-17, + "angularVelocity": 2.053265411966626e-17, + "velocityX": 3.123140383701003, + "velocityY": 1.980094213006201, + "timestamp": 5.330453367762717 + }, + { + "x": 13.15514980795229, + "y": 5.809118526061311, + "heading": -2.073342818682101e-17, + "angularVelocity": -3.876535448899066e-17, + "velocityX": 2.854387964472203, + "velocityY": 1.5081366303010209, + "timestamp": 5.402688097783549 + }, + { + "x": 13.332368406825639, + "y": 5.89160059122476, + "heading": -1.6325337936238524e-17, + "angularVelocity": 6.102452725036477e-17, + "velocityX": 2.4533710975625906, + "velocityY": 1.1418616106083745, + "timestamp": 5.474922827804381 + }, + { + "x": 13.476660624324403, + "y": 5.952752518585699, + "heading": -1.4094237433699954e-17, + "angularVelocity": 3.0886811674822726e-17, + "velocityX": 1.9975462974271516, + "velocityY": 0.8465723806720552, + "timestamp": 5.547157557825213 + }, + { + "x": 13.586113710702598, + "y": 5.995866804511295, + "heading": -8.214620498966636e-18, + "angularVelocity": 8.139598407904596e-17, + "velocityX": 1.5152418559137708, + "velocityY": 0.5968636681158963, + "timestamp": 5.619392287846045 + }, + { + "x": 13.659664660636231, + "y": 6.023164697186193, + "heading": -3.503128075700943e-18, + "angularVelocity": 6.52247529949022e-17, + "velocityX": 1.0182214277317942, + "velocityY": 0.37790537414654557, + "timestamp": 5.691627017866877 + }, + { + "x": 13.69665813446045, + "y": 6.036226272583008, + "heading": -5.6863338433321666e-30, + "angularVelocity": 4.8496451425533246e-17, + "velocityX": 0.5121286369250458, + "velocityY": 0.18082126690371453, + "timestamp": 5.763861747887709 + }, + { + "x": 13.69665813446045, + "y": 6.036226272583008, + "heading": -3.4479150184640595e-30, + "angularVelocity": -1.6743970569933744e-29, + "velocityX": 6.6279365304051e-27, + "velocityY": -2.722134005564902e-26, + "timestamp": 5.836096477908542 + } + ], + "eventMarkers": [ + { + "timestamp": 0, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + }, + { + "timestamp": 1.0637437100006089, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "outtakeShooter" + } + }, + { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + ] + } + } + }, + { + "timestamp": 2.4991684731681976, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "outtakeShooter" + } + }, + { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + ] + } + } + }, + { + "timestamp": 3.4221064662079383, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "outtakeShooter" + } + }, + { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + ] + } + } + }, + { + "timestamp": 4.342653667672321, + "command": { + "type": "named", + "data": { + "name": "outtakeShooter" + } + } + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/autos/AutoCommands.java b/src/main/java/frc/robot/autos/AutoCommands.java index 3a1d4c5f..b64fb309 100644 --- a/src/main/java/frc/robot/autos/AutoCommands.java +++ b/src/main/java/frc/robot/autos/AutoCommands.java @@ -89,13 +89,13 @@ public Command getMidlineNotesAmp456Command() { var redCenterWingShotTo6 = PathPlannerPath.fromChoreoTrajectory("Red CWS to 6"); var red6ToCenterWingShot = PathPlannerPath.fromChoreoTrajectory("Red 6 to CWS"); - // var blue4ToCenterWingShot = PathPlannerPath.fromPathFile("Blue 4 to CWS"); - // var blue4To5 = PathPlannerPath.fromPathFile("Blue 4 to 5"); - // var blueCenterWingShotTo5 = PathPlannerPath.fromPathFile("Blue CWS to 5"); - // var blue5ToCenterWingShot = PathPlannerPath.fromPathFile("Blue 5 to CWS"); - // var blue5To6 = PathPlannerPath.fromPathFile("Blue 5 to 6"); - // var blueCenterWingShotTo6 = PathPlannerPath.fromPathFile("Blue CWS to 6"); - // var blue6ToCenterWingShot = PathPlannerPath.fromPathFile("Blue 6 to CWS"); + // var blue4ToCenterWingShot = PathPlannerPath.fromPathFile("Blue 4 to CWS"); + // var blue4To5 = PathPlannerPath.fromPathFile("Blue 4 to 5"); + // var blueCenterWingShotTo5 = PathPlannerPath.fromPathFile("Blue CWS to 5"); + // var blue5ToCenterWingShot = PathPlannerPath.fromPathFile("Blue 5 to CWS"); + // var blue5To6 = PathPlannerPath.fromPathFile("Blue 5 to 6"); + // var blueCenterWingShotTo6 = PathPlannerPath.fromPathFile("Blue CWS to 6"); + // var blue6ToCenterWingShot = PathPlannerPath.fromPathFile("Blue 6 to CWS"); return Commands.sequence( Commands.either(