diff --git a/ChoreoProject.chor b/ChoreoProject.chor index 2a875ac..b3a16d1 100644 --- a/ChoreoProject.chor +++ b/ChoreoProject.chor @@ -4487,6 +4487,1012 @@ ], "eventMarkers": [], "isTrajectoryStale": false + }, + "CenterlineDisrupt": { + "waypoints": [ + { + "x": 0.6499595046043396, + "y": 4.397637367248535, + "heading": -1.05302514059915, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 2.4489798545837402, + "y": 2.931002378463745, + "heading": -0.4636471861382337, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 7.913700103759766, + "y": 0.9012491106987, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 9 + }, + { + "x": 8.468846321105957, + "y": 2.4625978469848633, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 10 + }, + { + "x": 8.212403297424316, + "y": 4.495091438293457, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 8 + }, + { + "x": 8.251384735107422, + "y": 5.917922496795654, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 9 + }, + { + "x": 8.251384735107422, + "y": 7.477188587188721, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 16 + }, + { + "x": 4.509144306182861, + "y": 6.132321357727051, + "heading": 0.18998797382164115, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.6499595046043396, + "y": 4.397637367248535, + "heading": -1.05302514059915, + "angularVelocity": -1.3781959946272502e-22, + "velocityX": -5.582540900425554e-24, + "velocityY": 3.959360287559598e-24, + "timestamp": 0 + }, + { + "x": 0.7439386139597359, + "y": 4.319907691298174, + "heading": -1.0530251410513847, + "angularVelocity": -4.008663071191835e-9, + "velocityX": 0.8330420701810625, + "velocityY": -0.689005148190141, + "timestamp": 0.112814361626382 + }, + { + "x": 0.8743373653931026, + "y": 4.212055501990732, + "heading": -1.0547735019704858, + "angularVelocity": -0.015497680383030333, + "velocityX": 1.1558701352689535, + "velocityY": -0.9560147108275632, + "timestamp": 0.225628723252764 + }, + { + "x": 1.0047361168261997, + "y": 4.104203312682963, + "heading": -1.0570727239889381, + "angularVelocity": -0.020380579079698136, + "velocityX": 1.1558701352665623, + "velocityY": -0.9560147108304606, + "timestamp": 0.338443084879146 + }, + { + "x": 1.1351348682592939, + "y": 3.996351123375191, + "heading": -1.058055269010971, + "angularVelocity": -0.008709396639472093, + "velocityX": 1.155870135266536, + "velocityY": -0.9560147108304925, + "timestamp": 0.451257446505528 + }, + { + "x": 1.2655336196923908, + "y": 3.8884989340674223, + "heading": -1.055819907754512, + "angularVelocity": 0.019814509644278368, + "velocityX": 1.1558701352665606, + "velocityY": -0.9560147108304629, + "timestamp": 0.56407180813191 + }, + { + "x": 1.3959323711255218, + "y": 3.7806467447596948, + "heading": -1.0483128977990617, + "angularVelocity": 0.06654303447917355, + "velocityX": 1.1558701352668668, + "velocityY": -0.9560147108300927, + "timestamp": 0.6768861697582921 + }, + { + "x": 1.5263311225587595, + "y": 3.672794555452096, + "heading": -1.0334248940554078, + "angularVelocity": 0.13196904657369468, + "velocityX": 1.1558701352678078, + "velocityY": -0.9560147108289547, + "timestamp": 0.7897005313846741 + }, + { + "x": 1.6567298739922285, + "y": 3.564942366144777, + "heading": -1.0086121652128852, + "angularVelocity": 0.21994299737029419, + "velocityX": 1.1558701352698606, + "velocityY": -0.9560147108264728, + "timestamp": 0.9025148930110561 + }, + { + "x": 1.7871286254260874, + "y": 3.45709017683793, + "heading": -0.9712069684409036, + "angularVelocity": 0.3315641398198931, + "velocityX": 1.1558701352733156, + "velocityY": -0.9560147108222955, + "timestamp": 1.0153292546374382 + }, + { + "x": 1.917527376860465, + "y": 3.34923798753171, + "heading": -0.9178895550179713, + "angularVelocity": 0.47261193215371433, + "velocityX": 1.155870135277915, + "velocityY": -0.9560147108167347, + "timestamp": 1.12814361626382 + }, + { + "x": 2.04792612829522, + "y": 3.2413857982259455, + "heading": -0.8450478857031731, + "angularVelocity": 0.6456772725092832, + "velocityX": 1.1558701352812586, + "velocityY": -0.9560147108126921, + "timestamp": 1.240957977890202 + }, + { + "x": 2.1783248797294235, + "y": 3.1335336089195143, + "heading": -0.7482891781356035, + "angularVelocity": 0.8576807613202174, + "velocityX": 1.1558701352763678, + "velocityY": -0.9560147108186051, + "timestamp": 1.353772339516584 + }, + { + "x": 2.3087236351774485, + "y": 3.025681424465998, + "heading": -0.6227458179192954, + "angularVelocity": 1.112831366560242, + "velocityX": 1.1558701708553702, + "velocityY": -0.9560146678017903, + "timestamp": 1.4665867011429659 + }, + { + "x": 2.44897985458374, + "y": 2.931002378463745, + "heading": -0.4636471861382337, + "angularVelocity": 1.4102693086892948, + "velocityX": 1.2432479108536778, + "velocityY": -0.8392463923681126, + "timestamp": 1.5794010627693478 + }, + { + "x": 2.59560005766312, + "y": 2.8420710526217885, + "heading": -0.35525539273168516, + "angularVelocity": 1.4102693459256186, + "velocityX": 1.9076534431041812, + "velocityY": -1.1570721249811602, + "timestamp": 1.6562599931004234 + }, + { + "x": 2.793501016986051, + "y": 2.729420570845893, + "heading": -0.24993052581458297, + "angularVelocity": 1.370366025957004, + "velocityX": 2.5748596613361308, + "velocityY": -1.465678500736941, + "timestamp": 1.733118923431499 + }, + { + "x": 3.0191753411397784, + "y": 2.6099749336612343, + "heading": -0.20571979775764684, + "angularVelocity": 0.5752191432601933, + "velocityX": 2.9362147401950485, + "velocityY": -1.5540892472759904, + "timestamp": 1.8099778537625746 + }, + { + "x": 3.2599710072289283, + "y": 2.4899899991638152, + "heading": -0.20571974745826738, + "angularVelocity": 6.544376725031248e-7, + "velocityX": 3.132956249220541, + "velocityY": -1.5611059636215943, + "timestamp": 1.8868367840936502 + }, + { + "x": 3.5007667103292572, + "y": 2.370005138943224, + "heading": -0.20571969715956012, + "angularVelocity": 6.54428926357924e-7, + "velocityX": 3.132956730767442, + "velocityY": -1.561104997216945, + "timestamp": 1.9636957144247258 + }, + { + "x": 3.7415624134298047, + "y": 2.2500202787230723, + "heading": -0.20571964686085287, + "angularVelocity": 6.544289265734677e-7, + "velocityX": 3.1329567307702857, + "velocityY": -1.5611049972112399, + "timestamp": 2.0405546447558014 + }, + { + "x": 3.9823581165303517, + "y": 2.1300354185029193, + "heading": -0.20571959656214558, + "angularVelocity": 6.544289263933649e-7, + "velocityX": 3.1329567307702852, + "velocityY": -1.5611049972112403, + "timestamp": 2.1174135750868768 + }, + { + "x": 4.223153819630899, + "y": 2.010050558282767, + "heading": -0.20571954626343825, + "angularVelocity": 6.544289274928786e-7, + "velocityX": 3.132956730770285, + "velocityY": -1.561104997211241, + "timestamp": 2.194272505417952 + }, + { + "x": 4.463949522731447, + "y": 1.890065698062614, + "heading": -0.20571949596473096, + "angularVelocity": 6.544289268381233e-7, + "velocityX": 3.1329567307702844, + "velocityY": -1.561104997211242, + "timestamp": 2.2711314357490275 + }, + { + "x": 4.704745225831994, + "y": 1.7700808378424613, + "heading": -0.2057194456660236, + "angularVelocity": 6.544289275764684e-7, + "velocityX": 3.1329567307702844, + "velocityY": -1.561104997211243, + "timestamp": 2.347990366080103 + }, + { + "x": 4.945540928932542, + "y": 1.6500959776223083, + "heading": -0.20571939536731623, + "angularVelocity": 6.544289278568051e-7, + "velocityX": 3.1329567307702844, + "velocityY": -1.5611049972112438, + "timestamp": 2.4248492964111783 + }, + { + "x": 5.186336632033089, + "y": 1.5301111174021556, + "heading": -0.20571934506860895, + "angularVelocity": 6.544289269879176e-7, + "velocityX": 3.1329567307702844, + "velocityY": -1.5611049972112447, + "timestamp": 2.5017082267422537 + }, + { + "x": 5.427132335133637, + "y": 1.4101262571820024, + "heading": -0.20571929476990156, + "angularVelocity": 6.544289278426302e-7, + "velocityX": 3.132956730770284, + "velocityY": -1.5611049972112456, + "timestamp": 2.578567157073329 + }, + { + "x": 5.667928038234183, + "y": 1.2901413969618496, + "heading": -0.20571924447119422, + "angularVelocity": 6.544289274178593e-7, + "velocityX": 3.132956730770284, + "velocityY": -1.5611049972112463, + "timestamp": 2.6554260874044044 + }, + { + "x": 5.908723741334731, + "y": 1.1701565367416964, + "heading": -0.2057191941724868, + "angularVelocity": 6.544289285142327e-7, + "velocityX": 3.132956730770284, + "velocityY": -1.5611049972112474, + "timestamp": 2.73228501773548 + }, + { + "x": 6.1495194444352785, + "y": 1.0501716765215432, + "heading": -0.20571914387377943, + "angularVelocity": 6.54428927715182e-7, + "velocityX": 3.1329567307702835, + "velocityY": -1.561104997211248, + "timestamp": 2.809143948066555 + }, + { + "x": 6.390315147536474, + "y": 0.9301868163026915, + "heading": -0.205719093575072, + "angularVelocity": 6.544289283620361e-7, + "velocityX": 3.1329567307787203, + "velocityY": -1.5611049971943174, + "timestamp": 2.8860028783976306 + }, + { + "x": 6.6311109604445875, + "y": 0.81020217645385, + "heading": -0.2057190432763632, + "angularVelocity": 6.544289466547929e-7, + "velocityX": 3.1329581594600384, + "velocityY": -1.5611021299932077, + "timestamp": 2.962861808728706 + }, + { + "x": 6.8837742528519295, + "y": 0.7177889873928526, + "heading": -0.2057189912917875, + "angularVelocity": 6.763635073735201e-7, + "velocityX": 3.28736415298749, + "velocityY": -1.202374124424067, + "timestamp": 3.0397207390597814 + }, + { + "x": 7.150181265246966, + "y": 0.6802891513062552, + "heading": -0.20571892659810667, + "angularVelocity": 8.417197660629371e-7, + "velocityX": 3.4661816297399604, + "velocityY": -0.48790473566395065, + "timestamp": 3.1165796693908567 + }, + { + "x": 7.418537576371213, + "y": 0.6993630962988201, + "heading": -0.20571881834451866, + "angularVelocity": 0.0000014084711760366945, + "velocityX": 3.491543662763481, + "velocityY": 0.24816823380708655, + "timestamp": 3.193438599721932 + }, + { + "x": 7.6769622174729975, + "y": 0.7741663820293505, + "heading": -0.2057181437677739, + "angularVelocity": 0.000008776816719474182, + "velocityX": 3.362324195621788, + "velocityY": 0.9732543168153605, + "timestamp": 3.2702975300530075 + }, + { + "x": 7.913700103759766, + "y": 0.9012491106987, + "heading": -0.20468215348647234, + "angularVelocity": 0.013479113966834618, + "velocityX": 3.0801610856019264, + "velocityY": 1.6534542976584665, + "timestamp": 3.347156460384083 + }, + { + "x": 8.069994078152508, + "y": 1.0191752960928648, + "heading": -0.20375887243633364, + "angularVelocity": 0.016481161319135098, + "velocityX": 2.7899480930412865, + "velocityY": 2.105058351343339, + "timestamp": 3.403176848283977 + }, + { + "x": 8.206503178207031, + "y": 1.1597450199010308, + "heading": -0.2032892345162591, + "angularVelocity": 0.0083833393105698, + "velocityX": 2.4367753450486496, + "velocityY": 2.5092600940100236, + "timestamp": 3.459197236183871 + }, + { + "x": 8.319991028205546, + "y": 1.3196579099497696, + "heading": -0.20328837774767694, + "angularVelocity": 0.00001529387093322081, + "velocityX": 2.0258312063335517, + "velocityY": 2.854548068008667, + "timestamp": 3.5152176240837654 + }, + { + "x": 8.407691921317648, + "y": 1.49504392254657, + "heading": -0.20328818040115396, + "angularVelocity": 0.0000035227625227330623, + "velocityX": 1.5655174196369026, + "velocityY": 3.1307532698668137, + "timestamp": 3.5712380119836595 + }, + { + "x": 8.467543059563889, + "y": 1.6817778190681203, + "heading": -0.2032880311174836, + "angularVelocity": 0.0000026648096518103063, + "velocityX": 1.0683813606074055, + "velocityY": 3.3333203057293357, + "timestamp": 3.6272583998835537 + }, + { + "x": 8.498136804155573, + "y": 1.8754676048297587, + "heading": -0.20328791350897857, + "angularVelocity": 0.0000020993875521670102, + "velocityX": 0.5461180427088816, + "velocityY": 3.4574874081156675, + "timestamp": 3.683278787783448 + }, + { + "x": 8.498753931533233, + "y": 2.0715577148212954, + "heading": -0.20328781917254074, + "angularVelocity": 0.0000016839661657876831, + "velocityX": 0.011016121108670174, + "velocityY": 3.5003347413791763, + "timestamp": 3.739299175683342 + }, + { + "x": 8.483800369138562, + "y": 2.267077799474399, + "heading": -0.20328773289772806, + "angularVelocity": 0.0000015400609657437253, + "velocityX": -0.2669307185339211, + "velocityY": 3.4901594220034653, + "timestamp": 3.795319563583236 + }, + { + "x": 8.468846321105957, + "y": 2.4625978469848633, + "heading": -0.20328764662310683, + "angularVelocity": 0.0000015400575482772301, + "velocityX": -0.2669393874838815, + "velocityY": 3.4901587589834318, + "timestamp": 3.8513399514831304 + }, + { + "x": 8.443202001044153, + "y": 2.6658472038837897, + "heading": -0.20328756938243672, + "angularVelocity": 0.0000013197722342835025, + "velocityX": -0.4381715168852488, + "velocityY": 3.4728188855744704, + "timestamp": 3.909865709259671 + }, + { + "x": 8.417557533819485, + "y": 2.869096542214876, + "heading": -0.20328749214181333, + "angularVelocity": 0.0000013197714364112358, + "velocityX": -0.4381740313826044, + "velocityY": 3.4728185683151858, + "timestamp": 3.9683914670362115 + }, + { + "x": 8.391913066592853, + "y": 3.0723458805457144, + "heading": -0.20328741490118712, + "angularVelocity": 0.0000013197714837580768, + "velocityX": -0.4381740314161576, + "velocityY": 3.472818568310951, + "timestamp": 4.026917224812752 + }, + { + "x": 8.36626859936622, + "y": 3.275595218876553, + "heading": -0.20328733766055812, + "angularVelocity": 0.0000013197715321871082, + "velocityX": -0.43817403141615957, + "velocityY": 3.4728185683109496, + "timestamp": 4.085442982589292 + }, + { + "x": 8.340624132139588, + "y": 3.4788445572073914, + "heading": -0.2032872604199263, + "angularVelocity": 0.0000013197715800752577, + "velocityX": -0.4381740314161615, + "velocityY": 3.4728185683109474, + "timestamp": 4.143968740365833 + }, + { + "x": 8.314979664912956, + "y": 3.6820938955382294, + "heading": -0.2032871831792916, + "angularVelocity": 0.0000013197716295854668, + "velocityX": -0.43817403141616307, + "velocityY": 3.4728185683109456, + "timestamp": 4.202494498142373 + }, + { + "x": 8.289335197686324, + "y": 3.8853432338690674, + "heading": -0.20328710593865404, + "angularVelocity": 0.0000013197716777984883, + "velocityX": -0.4381740314161649, + "velocityY": 3.4728185683109443, + "timestamp": 4.261020255918914 + }, + { + "x": 8.26369073045969, + "y": 4.0885925721999055, + "heading": -0.20328702869801377, + "angularVelocity": 0.0000013197717251490696, + "velocityX": -0.43817403141616196, + "velocityY": 3.472818568310943, + "timestamp": 4.319546013695454 + }, + { + "x": 8.23804626325309, + "y": 4.291841910533271, + "heading": -0.20328695145737063, + "angularVelocity": 0.000001319771773148376, + "velocityX": -0.43817403107387076, + "velocityY": 3.4728185683541293, + "timestamp": 4.378071771471995 + }, + { + "x": 8.212403297424316, + "y": 4.495091438293457, + "heading": -0.2032868742161759, + "angularVelocity": 0.0000013197811983448172, + "velocityX": -0.43814837779087523, + "velocityY": 3.472821804994326, + "timestamp": 4.436597529248536 + }, + { + "x": 8.211122236288011, + "y": 4.6734367956380565, + "heading": -0.20328678936705338, + "angularVelocity": 0.0000016652754099072682, + "velocityX": -0.025142506437536676, + "velocityY": 3.500261750249818, + "timestamp": 4.487549535146915 + }, + { + "x": 8.209842685648681, + "y": 4.851782163826195, + "heading": -0.2032867045174544, + "angularVelocity": 0.000001665284761962191, + "velocityX": -0.025112860951584333, + "velocityY": 3.5002619630684717, + "timestamp": 4.538501541045294 + }, + { + "x": 8.208563135039839, + "y": 5.030127532014551, + "heading": -0.203286619667852, + "angularVelocity": 0.0000016652848281480006, + "velocityX": -0.02511286035320464, + "velocityY": 3.500261963072762, + "timestamp": 4.589453546943673 + }, + { + "x": 8.207283584430996, + "y": 5.208472900202908, + "heading": -0.20328653481824618, + "angularVelocity": 0.000001665284895011747, + "velocityX": -0.02511286035319842, + "velocityY": 3.5002619630727594, + "timestamp": 4.6404055528420525 + }, + { + "x": 8.20600403382877, + "y": 5.386818268391311, + "heading": -0.2032864499686371, + "angularVelocity": 0.0000016652849598802982, + "velocityX": -0.025112860223413327, + "velocityY": 3.500261963073688, + "timestamp": 4.691357558740432 + }, + { + "x": 8.204724810861068, + "y": 5.565163638930035, + "heading": -0.20328636511888853, + "angularVelocity": 0.000001665287697022985, + "velocityX": -0.02510642996572845, + "velocityY": 3.5002620092018013, + "timestamp": 4.742309564638811 + }, + { + "x": 8.215723661757586, + "y": 5.743174123417012, + "heading": -0.20328627431671312, + "angularVelocity": 0.00000178211188758009, + "velocityX": 0.2158668869377758, + "velocityY": 3.4936894308343382, + "timestamp": 4.79326157053719 + }, + { + "x": 8.251384735107422, + "y": 5.917922496795654, + "heading": -0.20328616810400457, + "angularVelocity": 0.0000020845638294801198, + "velocityX": 0.6998953764638769, + "velocityY": 3.4296662181890745, + "timestamp": 4.844213576435569 + }, + { + "x": 8.319257308251775, + "y": 6.1013423979758015, + "heading": -0.20328607751404962, + "angularVelocity": 0.0000016213569308236664, + "velocityX": 1.21476676906589, + "velocityY": 3.282804679662124, + "timestamp": 4.900086501899726 + }, + { + "x": 8.406005091870338, + "y": 6.276626068328336, + "heading": -0.20328594240584189, + "angularVelocity": 0.000002418133767316469, + "velocityX": 1.5525906849858964, + "velocityY": 3.137184403651478, + "timestamp": 4.955959427363883 + }, + { + "x": 8.465469026478623, + "y": 6.461147567415963, + "heading": -0.19586462259478266, + "angularVelocity": 0.13282497290785408, + "velocityX": 1.0642710062932257, + "velocityY": 3.3025208104773145, + "timestamp": 5.01183235282804 + }, + { + "x": 8.497914422229119, + "y": 6.6500341576515325, + "heading": -0.1767449328748443, + "angularVelocity": 0.34219954586419293, + "velocityX": 0.5806997840360463, + "velocityY": 3.3806461477794345, + "timestamp": 5.0677052782921965 + }, + { + "x": 8.501533639733335, + "y": 6.83931682794884, + "heading": -0.15028692647848005, + "angularVelocity": 0.4735389489017775, + "velocityX": 0.0647758726458128, + "velocityY": 3.387735092173306, + "timestamp": 5.123578203756353 + }, + { + "x": 8.476232221452912, + "y": 7.02332099314858, + "heading": -0.1187876803017823, + "angularVelocity": 0.563765829603908, + "velocityX": -0.4528386167403034, + "velocityY": 3.293261694660663, + "timestamp": 5.17945112922051 + }, + { + "x": 8.424761116580283, + "y": 7.193085836951261, + "heading": -0.08469316743475845, + "angularVelocity": 0.6102152801878227, + "velocityX": -0.9212172880700084, + "velocityY": 3.038409791368238, + "timestamp": 5.235324054684667 + }, + { + "x": 8.349328318639657, + "y": 7.344985965125413, + "heading": -0.04934981017907167, + "angularVelocity": 0.6325667926294624, + "velocityX": -1.350077829538702, + "velocityY": 2.7186714658712017, + "timestamp": 5.291196980148824 + }, + { + "x": 8.251384735107422, + "y": 7.477188587188721, + "heading": -0.01358857618434331, + "angularVelocity": 0.640045848640404, + "velocityX": -1.752970382677875, + "velocityY": 2.366130303094952, + "timestamp": 5.3470699056129805 + }, + { + "x": 8.031164941682688, + "y": 7.638765513905215, + "heading": 0.04509310199225384, + "angularVelocity": 0.6364876296474269, + "velocityX": -2.388602007879415, + "velocityY": 1.7525353447119325, + "timestamp": 5.439266007223161 + }, + { + "x": 7.758143694279688, + "y": 7.7384188409284755, + "heading": 0.10025127172354484, + "angularVelocity": 0.5982700869989958, + "velocityX": -2.961310105685135, + "velocityY": 1.0808843897175913, + "timestamp": 5.531462108833341 + }, + { + "x": 7.44957756042063, + "y": 7.765731167037329, + "heading": 0.14290265837394545, + "angularVelocity": 0.4626159447688721, + "velocityX": -3.3468457827395444, + "velocityY": 0.296241659157495, + "timestamp": 5.623658210443521 + }, + { + "x": 7.1346990672621375, + "y": 7.715555620338561, + "heading": 0.15752386010214686, + "angularVelocity": 0.1585880690489728, + "velocityX": -3.4153124444441856, + "velocityY": -0.5442263373663762, + "timestamp": 5.715854312053701 + }, + { + "x": 6.838625346521012, + "y": 7.587150764950785, + "heading": 0.15752392264967224, + "angularVelocity": 6.784183309784681e-7, + "velocityX": -3.211347503530804, + "velocityY": -1.392736277838426, + "timestamp": 5.8080504136638815 + }, + { + "x": 6.564867571222415, + "y": 7.4162585819512294, + "heading": 0.15752396711824912, + "angularVelocity": 4.823259995470857e-7, + "velocityX": -2.9692988154324316, + "velocityY": -1.8535727651708478, + "timestamp": 5.900246515274062 + }, + { + "x": 6.291109923852278, + "y": 7.2453661940190575, + "heading": 0.15752401158682702, + "angularVelocity": 4.823260107723234e-7, + "velocityX": -2.969297427863337, + "velocityY": -1.8535749879613355, + "timestamp": 5.992442616884242 + }, + { + "x": 6.0173522764825655, + "y": 7.07447380608621, + "heading": 0.15752405605540484, + "angularVelocity": 4.823260098033014e-7, + "velocityX": -2.969297427858765, + "velocityY": -1.85357498796866, + "timestamp": 6.084638718494422 + }, + { + "x": 5.743594629112851, + "y": 6.903581418153362, + "heading": 0.1575241005239826, + "angularVelocity": 4.823260094668572e-7, + "velocityX": -2.9692974278587645, + "velocityY": -1.8535749879686596, + "timestamp": 6.176834820104602 + }, + { + "x": 5.469836981743135, + "y": 6.732689030220513, + "heading": 0.15752414499256026, + "angularVelocity": 4.823260082473403e-7, + "velocityX": -2.969297427858765, + "velocityY": -1.853574987968659, + "timestamp": 6.269030921714783 + }, + { + "x": 5.196079334373785, + "y": 6.5617966422870815, + "heading": 0.15752418946113778, + "angularVelocity": 4.823260064419635e-7, + "velocityX": -2.969297427854807, + "velocityY": -1.8535749879749983, + "timestamp": 6.361227023324963 + }, + { + "x": 4.922321797721961, + "y": 6.390904077000082, + "heading": 0.1575242339450377, + "angularVelocity": 4.824922002870454e-7, + "velocityX": -2.969296226963193, + "velocityY": -1.8535769116308105, + "timestamp": 6.453423124935143 + }, + { + "x": 4.71573342050983, + "y": 6.261612353964941, + "heading": 0.17376063284232746, + "angularVelocity": 0.1761072172654304, + "velocityX": -2.2407495935741277, + "velocityY": -1.4023556395237395, + "timestamp": 6.545619226545323 + }, + { + "x": 4.578007277346407, + "y": 6.1754183483776055, + "heading": 0.18457807169202894, + "angularVelocity": 0.11733076193871327, + "velocityX": -1.493839118553516, + "velocityY": -0.9348985920443467, + "timestamp": 6.6378153281555035 + }, + { + "x": 4.509144306182861, + "y": 6.132321357727051, + "heading": 0.18998797382164115, + "angularVelocity": 0.05867820911220429, + "velocityX": -0.7469184700965933, + "velocityY": -0.4674491643125701, + "timestamp": 6.730011429765684 + }, + { + "x": 4.509144306182861, + "y": 6.132321357727051, + "heading": 0.18998797382164115, + "angularVelocity": -1.332314755606386e-22, + "velocityX": 6.083262433472185e-23, + "velocityY": -4.699572509600775e-23, + "timestamp": 6.822207531375864 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.6499595046043396, + "y": 4.397637367248535, + "heading": -1.05302514059915, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "timestamp": 1.5794010627693478, + "isStopPoint": false, + "x": 2.4489798545837402, + "y": 2.931002378463745, + "heading": -0.4636471861382337, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "timestamp": 3.347156460384083, + "isStopPoint": false, + "x": 7.913700103759766, + "y": 0.9012491106987, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 9 + }, + { + "timestamp": 3.8513399514831304, + "isStopPoint": false, + "x": 8.468846321105957, + "y": 2.4625978469848633, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 10 + }, + { + "timestamp": 4.436597529248536, + "isStopPoint": false, + "x": 8.212403297424316, + "y": 4.495091438293457, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 8 + }, + { + "timestamp": 4.844213576435569, + "isStopPoint": false, + "x": 8.251384735107422, + "y": 5.917922496795654, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 9 + }, + { + "timestamp": 5.3470699056129805, + "isStopPoint": false, + "x": 8.251384735107422, + "y": 7.477188587188721, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 16 + }, + { + "timestamp": 6.822207531375864, + "isStopPoint": true, + "x": 4.509144306182861, + "y": 6.132321357727051, + "heading": 0.18998797382164115, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + "first", + 1 + ], + "type": "MaxVelocity", + "velocity": 1.5 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false } }, "splitTrajectoriesAtStopPoints": true, diff --git a/src/main/deploy/choreo/CenterlineDisrupt.1.traj b/src/main/deploy/choreo/CenterlineDisrupt.1.traj new file mode 100644 index 0000000..67b49d1 --- /dev/null +++ b/src/main/deploy/choreo/CenterlineDisrupt.1.traj @@ -0,0 +1,815 @@ +{ + "samples": [ + { + "x": 0.6499595046043396, + "y": 4.397637367248535, + "heading": -1.05302514059915, + "angularVelocity": -1.3781959946272502e-22, + "velocityX": -5.582540900425554e-24, + "velocityY": 3.959360287559598e-24, + "timestamp": 0 + }, + { + "x": 0.7439386139597359, + "y": 4.319907691298174, + "heading": -1.0530251410513847, + "angularVelocity": -4.008663071191835e-9, + "velocityX": 0.8330420701810625, + "velocityY": -0.689005148190141, + "timestamp": 0.112814361626382 + }, + { + "x": 0.8743373653931026, + "y": 4.212055501990732, + "heading": -1.0547735019704858, + "angularVelocity": -0.015497680383030333, + "velocityX": 1.1558701352689535, + "velocityY": -0.9560147108275632, + "timestamp": 0.225628723252764 + }, + { + "x": 1.0047361168261997, + "y": 4.104203312682963, + "heading": -1.0570727239889381, + "angularVelocity": -0.020380579079698136, + "velocityX": 1.1558701352665623, + "velocityY": -0.9560147108304606, + "timestamp": 0.338443084879146 + }, + { + "x": 1.1351348682592939, + "y": 3.996351123375191, + "heading": -1.058055269010971, + "angularVelocity": -0.008709396639472093, + "velocityX": 1.155870135266536, + "velocityY": -0.9560147108304925, + "timestamp": 0.451257446505528 + }, + { + "x": 1.2655336196923908, + "y": 3.8884989340674223, + "heading": -1.055819907754512, + "angularVelocity": 0.019814509644278368, + "velocityX": 1.1558701352665606, + "velocityY": -0.9560147108304629, + "timestamp": 0.56407180813191 + }, + { + "x": 1.3959323711255218, + "y": 3.7806467447596948, + "heading": -1.0483128977990617, + "angularVelocity": 0.06654303447917355, + "velocityX": 1.1558701352668668, + "velocityY": -0.9560147108300927, + "timestamp": 0.6768861697582921 + }, + { + "x": 1.5263311225587595, + "y": 3.672794555452096, + "heading": -1.0334248940554078, + "angularVelocity": 0.13196904657369468, + "velocityX": 1.1558701352678078, + "velocityY": -0.9560147108289547, + "timestamp": 0.7897005313846741 + }, + { + "x": 1.6567298739922285, + "y": 3.564942366144777, + "heading": -1.0086121652128852, + "angularVelocity": 0.21994299737029419, + "velocityX": 1.1558701352698606, + "velocityY": -0.9560147108264728, + "timestamp": 0.9025148930110561 + }, + { + "x": 1.7871286254260874, + "y": 3.45709017683793, + "heading": -0.9712069684409036, + "angularVelocity": 0.3315641398198931, + "velocityX": 1.1558701352733156, + "velocityY": -0.9560147108222955, + "timestamp": 1.0153292546374382 + }, + { + "x": 1.917527376860465, + "y": 3.34923798753171, + "heading": -0.9178895550179713, + "angularVelocity": 0.47261193215371433, + "velocityX": 1.155870135277915, + "velocityY": -0.9560147108167347, + "timestamp": 1.12814361626382 + }, + { + "x": 2.04792612829522, + "y": 3.2413857982259455, + "heading": -0.8450478857031731, + "angularVelocity": 0.6456772725092832, + "velocityX": 1.1558701352812586, + "velocityY": -0.9560147108126921, + "timestamp": 1.240957977890202 + }, + { + "x": 2.1783248797294235, + "y": 3.1335336089195143, + "heading": -0.7482891781356035, + "angularVelocity": 0.8576807613202174, + "velocityX": 1.1558701352763678, + "velocityY": -0.9560147108186051, + "timestamp": 1.353772339516584 + }, + { + "x": 2.3087236351774485, + "y": 3.025681424465998, + "heading": -0.6227458179192954, + "angularVelocity": 1.112831366560242, + "velocityX": 1.1558701708553702, + "velocityY": -0.9560146678017903, + "timestamp": 1.4665867011429659 + }, + { + "x": 2.44897985458374, + "y": 2.931002378463745, + "heading": -0.4636471861382337, + "angularVelocity": 1.4102693086892948, + "velocityX": 1.2432479108536778, + "velocityY": -0.8392463923681126, + "timestamp": 1.5794010627693478 + }, + { + "x": 2.59560005766312, + "y": 2.8420710526217885, + "heading": -0.35525539273168516, + "angularVelocity": 1.4102693459256186, + "velocityX": 1.9076534431041812, + "velocityY": -1.1570721249811602, + "timestamp": 1.6562599931004234 + }, + { + "x": 2.793501016986051, + "y": 2.729420570845893, + "heading": -0.24993052581458297, + "angularVelocity": 1.370366025957004, + "velocityX": 2.5748596613361308, + "velocityY": -1.465678500736941, + "timestamp": 1.733118923431499 + }, + { + "x": 3.0191753411397784, + "y": 2.6099749336612343, + "heading": -0.20571979775764684, + "angularVelocity": 0.5752191432601933, + "velocityX": 2.9362147401950485, + "velocityY": -1.5540892472759904, + "timestamp": 1.8099778537625746 + }, + { + "x": 3.2599710072289283, + "y": 2.4899899991638152, + "heading": -0.20571974745826738, + "angularVelocity": 6.544376725031248e-7, + "velocityX": 3.132956249220541, + "velocityY": -1.5611059636215943, + "timestamp": 1.8868367840936502 + }, + { + "x": 3.5007667103292572, + "y": 2.370005138943224, + "heading": -0.20571969715956012, + "angularVelocity": 6.54428926357924e-7, + "velocityX": 3.132956730767442, + "velocityY": -1.561104997216945, + "timestamp": 1.9636957144247258 + }, + { + "x": 3.7415624134298047, + "y": 2.2500202787230723, + "heading": -0.20571964686085287, + "angularVelocity": 6.544289265734677e-7, + "velocityX": 3.1329567307702857, + "velocityY": -1.5611049972112399, + "timestamp": 2.0405546447558014 + }, + { + "x": 3.9823581165303517, + "y": 2.1300354185029193, + "heading": -0.20571959656214558, + "angularVelocity": 6.544289263933649e-7, + "velocityX": 3.1329567307702852, + "velocityY": -1.5611049972112403, + "timestamp": 2.1174135750868768 + }, + { + "x": 4.223153819630899, + "y": 2.010050558282767, + "heading": -0.20571954626343825, + "angularVelocity": 6.544289274928786e-7, + "velocityX": 3.132956730770285, + "velocityY": -1.561104997211241, + "timestamp": 2.194272505417952 + }, + { + "x": 4.463949522731447, + "y": 1.890065698062614, + "heading": -0.20571949596473096, + "angularVelocity": 6.544289268381233e-7, + "velocityX": 3.1329567307702844, + "velocityY": -1.561104997211242, + "timestamp": 2.2711314357490275 + }, + { + "x": 4.704745225831994, + "y": 1.7700808378424613, + "heading": -0.2057194456660236, + "angularVelocity": 6.544289275764684e-7, + "velocityX": 3.1329567307702844, + "velocityY": -1.561104997211243, + "timestamp": 2.347990366080103 + }, + { + "x": 4.945540928932542, + "y": 1.6500959776223083, + "heading": -0.20571939536731623, + "angularVelocity": 6.544289278568051e-7, + "velocityX": 3.1329567307702844, + "velocityY": -1.5611049972112438, + "timestamp": 2.4248492964111783 + }, + { + "x": 5.186336632033089, + "y": 1.5301111174021556, + "heading": -0.20571934506860895, + "angularVelocity": 6.544289269879176e-7, + "velocityX": 3.1329567307702844, + "velocityY": -1.5611049972112447, + "timestamp": 2.5017082267422537 + }, + { + "x": 5.427132335133637, + "y": 1.4101262571820024, + "heading": -0.20571929476990156, + "angularVelocity": 6.544289278426302e-7, + "velocityX": 3.132956730770284, + "velocityY": -1.5611049972112456, + "timestamp": 2.578567157073329 + }, + { + "x": 5.667928038234183, + "y": 1.2901413969618496, + "heading": -0.20571924447119422, + "angularVelocity": 6.544289274178593e-7, + "velocityX": 3.132956730770284, + "velocityY": -1.5611049972112463, + "timestamp": 2.6554260874044044 + }, + { + "x": 5.908723741334731, + "y": 1.1701565367416964, + "heading": -0.2057191941724868, + "angularVelocity": 6.544289285142327e-7, + "velocityX": 3.132956730770284, + "velocityY": -1.5611049972112474, + "timestamp": 2.73228501773548 + }, + { + "x": 6.1495194444352785, + "y": 1.0501716765215432, + "heading": -0.20571914387377943, + "angularVelocity": 6.54428927715182e-7, + "velocityX": 3.1329567307702835, + "velocityY": -1.561104997211248, + "timestamp": 2.809143948066555 + }, + { + "x": 6.390315147536474, + "y": 0.9301868163026915, + "heading": -0.205719093575072, + "angularVelocity": 6.544289283620361e-7, + "velocityX": 3.1329567307787203, + "velocityY": -1.5611049971943174, + "timestamp": 2.8860028783976306 + }, + { + "x": 6.6311109604445875, + "y": 0.81020217645385, + "heading": -0.2057190432763632, + "angularVelocity": 6.544289466547929e-7, + "velocityX": 3.1329581594600384, + "velocityY": -1.5611021299932077, + "timestamp": 2.962861808728706 + }, + { + "x": 6.8837742528519295, + "y": 0.7177889873928526, + "heading": -0.2057189912917875, + "angularVelocity": 6.763635073735201e-7, + "velocityX": 3.28736415298749, + "velocityY": -1.202374124424067, + "timestamp": 3.0397207390597814 + }, + { + "x": 7.150181265246966, + "y": 0.6802891513062552, + "heading": -0.20571892659810667, + "angularVelocity": 8.417197660629371e-7, + "velocityX": 3.4661816297399604, + "velocityY": -0.48790473566395065, + "timestamp": 3.1165796693908567 + }, + { + "x": 7.418537576371213, + "y": 0.6993630962988201, + "heading": -0.20571881834451866, + "angularVelocity": 0.0000014084711760366945, + "velocityX": 3.491543662763481, + "velocityY": 0.24816823380708655, + "timestamp": 3.193438599721932 + }, + { + "x": 7.6769622174729975, + "y": 0.7741663820293505, + "heading": -0.2057181437677739, + "angularVelocity": 0.000008776816719474182, + "velocityX": 3.362324195621788, + "velocityY": 0.9732543168153605, + "timestamp": 3.2702975300530075 + }, + { + "x": 7.913700103759766, + "y": 0.9012491106987, + "heading": -0.20468215348647234, + "angularVelocity": 0.013479113966834618, + "velocityX": 3.0801610856019264, + "velocityY": 1.6534542976584665, + "timestamp": 3.347156460384083 + }, + { + "x": 8.069994078152508, + "y": 1.0191752960928648, + "heading": -0.20375887243633364, + "angularVelocity": 0.016481161319135098, + "velocityX": 2.7899480930412865, + "velocityY": 2.105058351343339, + "timestamp": 3.403176848283977 + }, + { + "x": 8.206503178207031, + "y": 1.1597450199010308, + "heading": -0.2032892345162591, + "angularVelocity": 0.0083833393105698, + "velocityX": 2.4367753450486496, + "velocityY": 2.5092600940100236, + "timestamp": 3.459197236183871 + }, + { + "x": 8.319991028205546, + "y": 1.3196579099497696, + "heading": -0.20328837774767694, + "angularVelocity": 0.00001529387093322081, + "velocityX": 2.0258312063335517, + "velocityY": 2.854548068008667, + "timestamp": 3.5152176240837654 + }, + { + "x": 8.407691921317648, + "y": 1.49504392254657, + "heading": -0.20328818040115396, + "angularVelocity": 0.0000035227625227330623, + "velocityX": 1.5655174196369026, + "velocityY": 3.1307532698668137, + "timestamp": 3.5712380119836595 + }, + { + "x": 8.467543059563889, + "y": 1.6817778190681203, + "heading": -0.2032880311174836, + "angularVelocity": 0.0000026648096518103063, + "velocityX": 1.0683813606074055, + "velocityY": 3.3333203057293357, + "timestamp": 3.6272583998835537 + }, + { + "x": 8.498136804155573, + "y": 1.8754676048297587, + "heading": -0.20328791350897857, + "angularVelocity": 0.0000020993875521670102, + "velocityX": 0.5461180427088816, + "velocityY": 3.4574874081156675, + "timestamp": 3.683278787783448 + }, + { + "x": 8.498753931533233, + "y": 2.0715577148212954, + "heading": -0.20328781917254074, + "angularVelocity": 0.0000016839661657876831, + "velocityX": 0.011016121108670174, + "velocityY": 3.5003347413791763, + "timestamp": 3.739299175683342 + }, + { + "x": 8.483800369138562, + "y": 2.267077799474399, + "heading": -0.20328773289772806, + "angularVelocity": 0.0000015400609657437253, + "velocityX": -0.2669307185339211, + "velocityY": 3.4901594220034653, + "timestamp": 3.795319563583236 + }, + { + "x": 8.468846321105957, + "y": 2.4625978469848633, + "heading": -0.20328764662310683, + "angularVelocity": 0.0000015400575482772301, + "velocityX": -0.2669393874838815, + "velocityY": 3.4901587589834318, + "timestamp": 3.8513399514831304 + }, + { + "x": 8.443202001044153, + "y": 2.6658472038837897, + "heading": -0.20328756938243672, + "angularVelocity": 0.0000013197722342835025, + "velocityX": -0.4381715168852488, + "velocityY": 3.4728188855744704, + "timestamp": 3.909865709259671 + }, + { + "x": 8.417557533819485, + "y": 2.869096542214876, + "heading": -0.20328749214181333, + "angularVelocity": 0.0000013197714364112358, + "velocityX": -0.4381740313826044, + "velocityY": 3.4728185683151858, + "timestamp": 3.9683914670362115 + }, + { + "x": 8.391913066592853, + "y": 3.0723458805457144, + "heading": -0.20328741490118712, + "angularVelocity": 0.0000013197714837580768, + "velocityX": -0.4381740314161576, + "velocityY": 3.472818568310951, + "timestamp": 4.026917224812752 + }, + { + "x": 8.36626859936622, + "y": 3.275595218876553, + "heading": -0.20328733766055812, + "angularVelocity": 0.0000013197715321871082, + "velocityX": -0.43817403141615957, + "velocityY": 3.4728185683109496, + "timestamp": 4.085442982589292 + }, + { + "x": 8.340624132139588, + "y": 3.4788445572073914, + "heading": -0.2032872604199263, + "angularVelocity": 0.0000013197715800752577, + "velocityX": -0.4381740314161615, + "velocityY": 3.4728185683109474, + "timestamp": 4.143968740365833 + }, + { + "x": 8.314979664912956, + "y": 3.6820938955382294, + "heading": -0.2032871831792916, + "angularVelocity": 0.0000013197716295854668, + "velocityX": -0.43817403141616307, + "velocityY": 3.4728185683109456, + "timestamp": 4.202494498142373 + }, + { + "x": 8.289335197686324, + "y": 3.8853432338690674, + "heading": -0.20328710593865404, + "angularVelocity": 0.0000013197716777984883, + "velocityX": -0.4381740314161649, + "velocityY": 3.4728185683109443, + "timestamp": 4.261020255918914 + }, + { + "x": 8.26369073045969, + "y": 4.0885925721999055, + "heading": -0.20328702869801377, + "angularVelocity": 0.0000013197717251490696, + "velocityX": -0.43817403141616196, + "velocityY": 3.472818568310943, + "timestamp": 4.319546013695454 + }, + { + "x": 8.23804626325309, + "y": 4.291841910533271, + "heading": -0.20328695145737063, + "angularVelocity": 0.000001319771773148376, + "velocityX": -0.43817403107387076, + "velocityY": 3.4728185683541293, + "timestamp": 4.378071771471995 + }, + { + "x": 8.212403297424316, + "y": 4.495091438293457, + "heading": -0.2032868742161759, + "angularVelocity": 0.0000013197811983448172, + "velocityX": -0.43814837779087523, + "velocityY": 3.472821804994326, + "timestamp": 4.436597529248536 + }, + { + "x": 8.211122236288011, + "y": 4.6734367956380565, + "heading": -0.20328678936705338, + "angularVelocity": 0.0000016652754099072682, + "velocityX": -0.025142506437536676, + "velocityY": 3.500261750249818, + "timestamp": 4.487549535146915 + }, + { + "x": 8.209842685648681, + "y": 4.851782163826195, + "heading": -0.2032867045174544, + "angularVelocity": 0.000001665284761962191, + "velocityX": -0.025112860951584333, + "velocityY": 3.5002619630684717, + "timestamp": 4.538501541045294 + }, + { + "x": 8.208563135039839, + "y": 5.030127532014551, + "heading": -0.203286619667852, + "angularVelocity": 0.0000016652848281480006, + "velocityX": -0.02511286035320464, + "velocityY": 3.500261963072762, + "timestamp": 4.589453546943673 + }, + { + "x": 8.207283584430996, + "y": 5.208472900202908, + "heading": -0.20328653481824618, + "angularVelocity": 0.000001665284895011747, + "velocityX": -0.02511286035319842, + "velocityY": 3.5002619630727594, + "timestamp": 4.6404055528420525 + }, + { + "x": 8.20600403382877, + "y": 5.386818268391311, + "heading": -0.2032864499686371, + "angularVelocity": 0.0000016652849598802982, + "velocityX": -0.025112860223413327, + "velocityY": 3.500261963073688, + "timestamp": 4.691357558740432 + }, + { + "x": 8.204724810861068, + "y": 5.565163638930035, + "heading": -0.20328636511888853, + "angularVelocity": 0.000001665287697022985, + "velocityX": -0.02510642996572845, + "velocityY": 3.5002620092018013, + "timestamp": 4.742309564638811 + }, + { + "x": 8.215723661757586, + "y": 5.743174123417012, + "heading": -0.20328627431671312, + "angularVelocity": 0.00000178211188758009, + "velocityX": 0.2158668869377758, + "velocityY": 3.4936894308343382, + "timestamp": 4.79326157053719 + }, + { + "x": 8.251384735107422, + "y": 5.917922496795654, + "heading": -0.20328616810400457, + "angularVelocity": 0.0000020845638294801198, + "velocityX": 0.6998953764638769, + "velocityY": 3.4296662181890745, + "timestamp": 4.844213576435569 + }, + { + "x": 8.319257308251775, + "y": 6.1013423979758015, + "heading": -0.20328607751404962, + "angularVelocity": 0.0000016213569308236664, + "velocityX": 1.21476676906589, + "velocityY": 3.282804679662124, + "timestamp": 4.900086501899726 + }, + { + "x": 8.406005091870338, + "y": 6.276626068328336, + "heading": -0.20328594240584189, + "angularVelocity": 0.000002418133767316469, + "velocityX": 1.5525906849858964, + "velocityY": 3.137184403651478, + "timestamp": 4.955959427363883 + }, + { + "x": 8.465469026478623, + "y": 6.461147567415963, + "heading": -0.19586462259478266, + "angularVelocity": 0.13282497290785408, + "velocityX": 1.0642710062932257, + "velocityY": 3.3025208104773145, + "timestamp": 5.01183235282804 + }, + { + "x": 8.497914422229119, + "y": 6.6500341576515325, + "heading": -0.1767449328748443, + "angularVelocity": 0.34219954586419293, + "velocityX": 0.5806997840360463, + "velocityY": 3.3806461477794345, + "timestamp": 5.0677052782921965 + }, + { + "x": 8.501533639733335, + "y": 6.83931682794884, + "heading": -0.15028692647848005, + "angularVelocity": 0.4735389489017775, + "velocityX": 0.0647758726458128, + "velocityY": 3.387735092173306, + "timestamp": 5.123578203756353 + }, + { + "x": 8.476232221452912, + "y": 7.02332099314858, + "heading": -0.1187876803017823, + "angularVelocity": 0.563765829603908, + "velocityX": -0.4528386167403034, + "velocityY": 3.293261694660663, + "timestamp": 5.17945112922051 + }, + { + "x": 8.424761116580283, + "y": 7.193085836951261, + "heading": -0.08469316743475845, + "angularVelocity": 0.6102152801878227, + "velocityX": -0.9212172880700084, + "velocityY": 3.038409791368238, + "timestamp": 5.235324054684667 + }, + { + "x": 8.349328318639657, + "y": 7.344985965125413, + "heading": -0.04934981017907167, + "angularVelocity": 0.6325667926294624, + "velocityX": -1.350077829538702, + "velocityY": 2.7186714658712017, + "timestamp": 5.291196980148824 + }, + { + "x": 8.251384735107422, + "y": 7.477188587188721, + "heading": -0.01358857618434331, + "angularVelocity": 0.640045848640404, + "velocityX": -1.752970382677875, + "velocityY": 2.366130303094952, + "timestamp": 5.3470699056129805 + }, + { + "x": 8.031164941682688, + "y": 7.638765513905215, + "heading": 0.04509310199225384, + "angularVelocity": 0.6364876296474269, + "velocityX": -2.388602007879415, + "velocityY": 1.7525353447119325, + "timestamp": 5.439266007223161 + }, + { + "x": 7.758143694279688, + "y": 7.7384188409284755, + "heading": 0.10025127172354484, + "angularVelocity": 0.5982700869989958, + "velocityX": -2.961310105685135, + "velocityY": 1.0808843897175913, + "timestamp": 5.531462108833341 + }, + { + "x": 7.44957756042063, + "y": 7.765731167037329, + "heading": 0.14290265837394545, + "angularVelocity": 0.4626159447688721, + "velocityX": -3.3468457827395444, + "velocityY": 0.296241659157495, + "timestamp": 5.623658210443521 + }, + { + "x": 7.1346990672621375, + "y": 7.715555620338561, + "heading": 0.15752386010214686, + "angularVelocity": 0.1585880690489728, + "velocityX": -3.4153124444441856, + "velocityY": -0.5442263373663762, + "timestamp": 5.715854312053701 + }, + { + "x": 6.838625346521012, + "y": 7.587150764950785, + "heading": 0.15752392264967224, + "angularVelocity": 6.784183309784681e-7, + "velocityX": -3.211347503530804, + "velocityY": -1.392736277838426, + "timestamp": 5.8080504136638815 + }, + { + "x": 6.564867571222415, + "y": 7.4162585819512294, + "heading": 0.15752396711824912, + "angularVelocity": 4.823259995470857e-7, + "velocityX": -2.9692988154324316, + "velocityY": -1.8535727651708478, + "timestamp": 5.900246515274062 + }, + { + "x": 6.291109923852278, + "y": 7.2453661940190575, + "heading": 0.15752401158682702, + "angularVelocity": 4.823260107723234e-7, + "velocityX": -2.969297427863337, + "velocityY": -1.8535749879613355, + "timestamp": 5.992442616884242 + }, + { + "x": 6.0173522764825655, + "y": 7.07447380608621, + "heading": 0.15752405605540484, + "angularVelocity": 4.823260098033014e-7, + "velocityX": -2.969297427858765, + "velocityY": -1.85357498796866, + "timestamp": 6.084638718494422 + }, + { + "x": 5.743594629112851, + "y": 6.903581418153362, + "heading": 0.1575241005239826, + "angularVelocity": 4.823260094668572e-7, + "velocityX": -2.9692974278587645, + "velocityY": -1.8535749879686596, + "timestamp": 6.176834820104602 + }, + { + "x": 5.469836981743135, + "y": 6.732689030220513, + "heading": 0.15752414499256026, + "angularVelocity": 4.823260082473403e-7, + "velocityX": -2.969297427858765, + "velocityY": -1.853574987968659, + "timestamp": 6.269030921714783 + }, + { + "x": 5.196079334373785, + "y": 6.5617966422870815, + "heading": 0.15752418946113778, + "angularVelocity": 4.823260064419635e-7, + "velocityX": -2.969297427854807, + "velocityY": -1.8535749879749983, + "timestamp": 6.361227023324963 + }, + { + "x": 4.922321797721961, + "y": 6.390904077000082, + "heading": 0.1575242339450377, + "angularVelocity": 4.824922002870454e-7, + "velocityX": -2.969296226963193, + "velocityY": -1.8535769116308105, + "timestamp": 6.453423124935143 + }, + { + "x": 4.71573342050983, + "y": 6.261612353964941, + "heading": 0.17376063284232746, + "angularVelocity": 0.1761072172654304, + "velocityX": -2.2407495935741277, + "velocityY": -1.4023556395237395, + "timestamp": 6.545619226545323 + }, + { + "x": 4.578007277346407, + "y": 6.1754183483776055, + "heading": 0.18457807169202894, + "angularVelocity": 0.11733076193871327, + "velocityX": -1.493839118553516, + "velocityY": -0.9348985920443467, + "timestamp": 6.6378153281555035 + }, + { + "x": 4.509144306182861, + "y": 6.132321357727051, + "heading": 0.18998797382164115, + "angularVelocity": 0.05867820911220429, + "velocityX": -0.7469184700965933, + "velocityY": -0.4674491643125701, + "timestamp": 6.730011429765684 + }, + { + "x": 4.509144306182861, + "y": 6.132321357727051, + "heading": 0.18998797382164115, + "angularVelocity": -1.332314755606386e-22, + "velocityX": 6.083262433472185e-23, + "velocityY": -4.699572509600775e-23, + "timestamp": 6.822207531375864 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/CenterlineDisrupt.traj b/src/main/deploy/choreo/CenterlineDisrupt.traj new file mode 100644 index 0000000..67b49d1 --- /dev/null +++ b/src/main/deploy/choreo/CenterlineDisrupt.traj @@ -0,0 +1,815 @@ +{ + "samples": [ + { + "x": 0.6499595046043396, + "y": 4.397637367248535, + "heading": -1.05302514059915, + "angularVelocity": -1.3781959946272502e-22, + "velocityX": -5.582540900425554e-24, + "velocityY": 3.959360287559598e-24, + "timestamp": 0 + }, + { + "x": 0.7439386139597359, + "y": 4.319907691298174, + "heading": -1.0530251410513847, + "angularVelocity": -4.008663071191835e-9, + "velocityX": 0.8330420701810625, + "velocityY": -0.689005148190141, + "timestamp": 0.112814361626382 + }, + { + "x": 0.8743373653931026, + "y": 4.212055501990732, + "heading": -1.0547735019704858, + "angularVelocity": -0.015497680383030333, + "velocityX": 1.1558701352689535, + "velocityY": -0.9560147108275632, + "timestamp": 0.225628723252764 + }, + { + "x": 1.0047361168261997, + "y": 4.104203312682963, + "heading": -1.0570727239889381, + "angularVelocity": -0.020380579079698136, + "velocityX": 1.1558701352665623, + "velocityY": -0.9560147108304606, + "timestamp": 0.338443084879146 + }, + { + "x": 1.1351348682592939, + "y": 3.996351123375191, + "heading": -1.058055269010971, + "angularVelocity": -0.008709396639472093, + "velocityX": 1.155870135266536, + "velocityY": -0.9560147108304925, + "timestamp": 0.451257446505528 + }, + { + "x": 1.2655336196923908, + "y": 3.8884989340674223, + "heading": -1.055819907754512, + "angularVelocity": 0.019814509644278368, + "velocityX": 1.1558701352665606, + "velocityY": -0.9560147108304629, + "timestamp": 0.56407180813191 + }, + { + "x": 1.3959323711255218, + "y": 3.7806467447596948, + "heading": -1.0483128977990617, + "angularVelocity": 0.06654303447917355, + "velocityX": 1.1558701352668668, + "velocityY": -0.9560147108300927, + "timestamp": 0.6768861697582921 + }, + { + "x": 1.5263311225587595, + "y": 3.672794555452096, + "heading": -1.0334248940554078, + "angularVelocity": 0.13196904657369468, + "velocityX": 1.1558701352678078, + "velocityY": -0.9560147108289547, + "timestamp": 0.7897005313846741 + }, + { + "x": 1.6567298739922285, + "y": 3.564942366144777, + "heading": -1.0086121652128852, + "angularVelocity": 0.21994299737029419, + "velocityX": 1.1558701352698606, + "velocityY": -0.9560147108264728, + "timestamp": 0.9025148930110561 + }, + { + "x": 1.7871286254260874, + "y": 3.45709017683793, + "heading": -0.9712069684409036, + "angularVelocity": 0.3315641398198931, + "velocityX": 1.1558701352733156, + "velocityY": -0.9560147108222955, + "timestamp": 1.0153292546374382 + }, + { + "x": 1.917527376860465, + "y": 3.34923798753171, + "heading": -0.9178895550179713, + "angularVelocity": 0.47261193215371433, + "velocityX": 1.155870135277915, + "velocityY": -0.9560147108167347, + "timestamp": 1.12814361626382 + }, + { + "x": 2.04792612829522, + "y": 3.2413857982259455, + "heading": -0.8450478857031731, + "angularVelocity": 0.6456772725092832, + "velocityX": 1.1558701352812586, + "velocityY": -0.9560147108126921, + "timestamp": 1.240957977890202 + }, + { + "x": 2.1783248797294235, + "y": 3.1335336089195143, + "heading": -0.7482891781356035, + "angularVelocity": 0.8576807613202174, + "velocityX": 1.1558701352763678, + "velocityY": -0.9560147108186051, + "timestamp": 1.353772339516584 + }, + { + "x": 2.3087236351774485, + "y": 3.025681424465998, + "heading": -0.6227458179192954, + "angularVelocity": 1.112831366560242, + "velocityX": 1.1558701708553702, + "velocityY": -0.9560146678017903, + "timestamp": 1.4665867011429659 + }, + { + "x": 2.44897985458374, + "y": 2.931002378463745, + "heading": -0.4636471861382337, + "angularVelocity": 1.4102693086892948, + "velocityX": 1.2432479108536778, + "velocityY": -0.8392463923681126, + "timestamp": 1.5794010627693478 + }, + { + "x": 2.59560005766312, + "y": 2.8420710526217885, + "heading": -0.35525539273168516, + "angularVelocity": 1.4102693459256186, + "velocityX": 1.9076534431041812, + "velocityY": -1.1570721249811602, + "timestamp": 1.6562599931004234 + }, + { + "x": 2.793501016986051, + "y": 2.729420570845893, + "heading": -0.24993052581458297, + "angularVelocity": 1.370366025957004, + "velocityX": 2.5748596613361308, + "velocityY": -1.465678500736941, + "timestamp": 1.733118923431499 + }, + { + "x": 3.0191753411397784, + "y": 2.6099749336612343, + "heading": -0.20571979775764684, + "angularVelocity": 0.5752191432601933, + "velocityX": 2.9362147401950485, + "velocityY": -1.5540892472759904, + "timestamp": 1.8099778537625746 + }, + { + "x": 3.2599710072289283, + "y": 2.4899899991638152, + "heading": -0.20571974745826738, + "angularVelocity": 6.544376725031248e-7, + "velocityX": 3.132956249220541, + "velocityY": -1.5611059636215943, + "timestamp": 1.8868367840936502 + }, + { + "x": 3.5007667103292572, + "y": 2.370005138943224, + "heading": -0.20571969715956012, + "angularVelocity": 6.54428926357924e-7, + "velocityX": 3.132956730767442, + "velocityY": -1.561104997216945, + "timestamp": 1.9636957144247258 + }, + { + "x": 3.7415624134298047, + "y": 2.2500202787230723, + "heading": -0.20571964686085287, + "angularVelocity": 6.544289265734677e-7, + "velocityX": 3.1329567307702857, + "velocityY": -1.5611049972112399, + "timestamp": 2.0405546447558014 + }, + { + "x": 3.9823581165303517, + "y": 2.1300354185029193, + "heading": -0.20571959656214558, + "angularVelocity": 6.544289263933649e-7, + "velocityX": 3.1329567307702852, + "velocityY": -1.5611049972112403, + "timestamp": 2.1174135750868768 + }, + { + "x": 4.223153819630899, + "y": 2.010050558282767, + "heading": -0.20571954626343825, + "angularVelocity": 6.544289274928786e-7, + "velocityX": 3.132956730770285, + "velocityY": -1.561104997211241, + "timestamp": 2.194272505417952 + }, + { + "x": 4.463949522731447, + "y": 1.890065698062614, + "heading": -0.20571949596473096, + "angularVelocity": 6.544289268381233e-7, + "velocityX": 3.1329567307702844, + "velocityY": -1.561104997211242, + "timestamp": 2.2711314357490275 + }, + { + "x": 4.704745225831994, + "y": 1.7700808378424613, + "heading": -0.2057194456660236, + "angularVelocity": 6.544289275764684e-7, + "velocityX": 3.1329567307702844, + "velocityY": -1.561104997211243, + "timestamp": 2.347990366080103 + }, + { + "x": 4.945540928932542, + "y": 1.6500959776223083, + "heading": -0.20571939536731623, + "angularVelocity": 6.544289278568051e-7, + "velocityX": 3.1329567307702844, + "velocityY": -1.5611049972112438, + "timestamp": 2.4248492964111783 + }, + { + "x": 5.186336632033089, + "y": 1.5301111174021556, + "heading": -0.20571934506860895, + "angularVelocity": 6.544289269879176e-7, + "velocityX": 3.1329567307702844, + "velocityY": -1.5611049972112447, + "timestamp": 2.5017082267422537 + }, + { + "x": 5.427132335133637, + "y": 1.4101262571820024, + "heading": -0.20571929476990156, + "angularVelocity": 6.544289278426302e-7, + "velocityX": 3.132956730770284, + "velocityY": -1.5611049972112456, + "timestamp": 2.578567157073329 + }, + { + "x": 5.667928038234183, + "y": 1.2901413969618496, + "heading": -0.20571924447119422, + "angularVelocity": 6.544289274178593e-7, + "velocityX": 3.132956730770284, + "velocityY": -1.5611049972112463, + "timestamp": 2.6554260874044044 + }, + { + "x": 5.908723741334731, + "y": 1.1701565367416964, + "heading": -0.2057191941724868, + "angularVelocity": 6.544289285142327e-7, + "velocityX": 3.132956730770284, + "velocityY": -1.5611049972112474, + "timestamp": 2.73228501773548 + }, + { + "x": 6.1495194444352785, + "y": 1.0501716765215432, + "heading": -0.20571914387377943, + "angularVelocity": 6.54428927715182e-7, + "velocityX": 3.1329567307702835, + "velocityY": -1.561104997211248, + "timestamp": 2.809143948066555 + }, + { + "x": 6.390315147536474, + "y": 0.9301868163026915, + "heading": -0.205719093575072, + "angularVelocity": 6.544289283620361e-7, + "velocityX": 3.1329567307787203, + "velocityY": -1.5611049971943174, + "timestamp": 2.8860028783976306 + }, + { + "x": 6.6311109604445875, + "y": 0.81020217645385, + "heading": -0.2057190432763632, + "angularVelocity": 6.544289466547929e-7, + "velocityX": 3.1329581594600384, + "velocityY": -1.5611021299932077, + "timestamp": 2.962861808728706 + }, + { + "x": 6.8837742528519295, + "y": 0.7177889873928526, + "heading": -0.2057189912917875, + "angularVelocity": 6.763635073735201e-7, + "velocityX": 3.28736415298749, + "velocityY": -1.202374124424067, + "timestamp": 3.0397207390597814 + }, + { + "x": 7.150181265246966, + "y": 0.6802891513062552, + "heading": -0.20571892659810667, + "angularVelocity": 8.417197660629371e-7, + "velocityX": 3.4661816297399604, + "velocityY": -0.48790473566395065, + "timestamp": 3.1165796693908567 + }, + { + "x": 7.418537576371213, + "y": 0.6993630962988201, + "heading": -0.20571881834451866, + "angularVelocity": 0.0000014084711760366945, + "velocityX": 3.491543662763481, + "velocityY": 0.24816823380708655, + "timestamp": 3.193438599721932 + }, + { + "x": 7.6769622174729975, + "y": 0.7741663820293505, + "heading": -0.2057181437677739, + "angularVelocity": 0.000008776816719474182, + "velocityX": 3.362324195621788, + "velocityY": 0.9732543168153605, + "timestamp": 3.2702975300530075 + }, + { + "x": 7.913700103759766, + "y": 0.9012491106987, + "heading": -0.20468215348647234, + "angularVelocity": 0.013479113966834618, + "velocityX": 3.0801610856019264, + "velocityY": 1.6534542976584665, + "timestamp": 3.347156460384083 + }, + { + "x": 8.069994078152508, + "y": 1.0191752960928648, + "heading": -0.20375887243633364, + "angularVelocity": 0.016481161319135098, + "velocityX": 2.7899480930412865, + "velocityY": 2.105058351343339, + "timestamp": 3.403176848283977 + }, + { + "x": 8.206503178207031, + "y": 1.1597450199010308, + "heading": -0.2032892345162591, + "angularVelocity": 0.0083833393105698, + "velocityX": 2.4367753450486496, + "velocityY": 2.5092600940100236, + "timestamp": 3.459197236183871 + }, + { + "x": 8.319991028205546, + "y": 1.3196579099497696, + "heading": -0.20328837774767694, + "angularVelocity": 0.00001529387093322081, + "velocityX": 2.0258312063335517, + "velocityY": 2.854548068008667, + "timestamp": 3.5152176240837654 + }, + { + "x": 8.407691921317648, + "y": 1.49504392254657, + "heading": -0.20328818040115396, + "angularVelocity": 0.0000035227625227330623, + "velocityX": 1.5655174196369026, + "velocityY": 3.1307532698668137, + "timestamp": 3.5712380119836595 + }, + { + "x": 8.467543059563889, + "y": 1.6817778190681203, + "heading": -0.2032880311174836, + "angularVelocity": 0.0000026648096518103063, + "velocityX": 1.0683813606074055, + "velocityY": 3.3333203057293357, + "timestamp": 3.6272583998835537 + }, + { + "x": 8.498136804155573, + "y": 1.8754676048297587, + "heading": -0.20328791350897857, + "angularVelocity": 0.0000020993875521670102, + "velocityX": 0.5461180427088816, + "velocityY": 3.4574874081156675, + "timestamp": 3.683278787783448 + }, + { + "x": 8.498753931533233, + "y": 2.0715577148212954, + "heading": -0.20328781917254074, + "angularVelocity": 0.0000016839661657876831, + "velocityX": 0.011016121108670174, + "velocityY": 3.5003347413791763, + "timestamp": 3.739299175683342 + }, + { + "x": 8.483800369138562, + "y": 2.267077799474399, + "heading": -0.20328773289772806, + "angularVelocity": 0.0000015400609657437253, + "velocityX": -0.2669307185339211, + "velocityY": 3.4901594220034653, + "timestamp": 3.795319563583236 + }, + { + "x": 8.468846321105957, + "y": 2.4625978469848633, + "heading": -0.20328764662310683, + "angularVelocity": 0.0000015400575482772301, + "velocityX": -0.2669393874838815, + "velocityY": 3.4901587589834318, + "timestamp": 3.8513399514831304 + }, + { + "x": 8.443202001044153, + "y": 2.6658472038837897, + "heading": -0.20328756938243672, + "angularVelocity": 0.0000013197722342835025, + "velocityX": -0.4381715168852488, + "velocityY": 3.4728188855744704, + "timestamp": 3.909865709259671 + }, + { + "x": 8.417557533819485, + "y": 2.869096542214876, + "heading": -0.20328749214181333, + "angularVelocity": 0.0000013197714364112358, + "velocityX": -0.4381740313826044, + "velocityY": 3.4728185683151858, + "timestamp": 3.9683914670362115 + }, + { + "x": 8.391913066592853, + "y": 3.0723458805457144, + "heading": -0.20328741490118712, + "angularVelocity": 0.0000013197714837580768, + "velocityX": -0.4381740314161576, + "velocityY": 3.472818568310951, + "timestamp": 4.026917224812752 + }, + { + "x": 8.36626859936622, + "y": 3.275595218876553, + "heading": -0.20328733766055812, + "angularVelocity": 0.0000013197715321871082, + "velocityX": -0.43817403141615957, + "velocityY": 3.4728185683109496, + "timestamp": 4.085442982589292 + }, + { + "x": 8.340624132139588, + "y": 3.4788445572073914, + "heading": -0.2032872604199263, + "angularVelocity": 0.0000013197715800752577, + "velocityX": -0.4381740314161615, + "velocityY": 3.4728185683109474, + "timestamp": 4.143968740365833 + }, + { + "x": 8.314979664912956, + "y": 3.6820938955382294, + "heading": -0.2032871831792916, + "angularVelocity": 0.0000013197716295854668, + "velocityX": -0.43817403141616307, + "velocityY": 3.4728185683109456, + "timestamp": 4.202494498142373 + }, + { + "x": 8.289335197686324, + "y": 3.8853432338690674, + "heading": -0.20328710593865404, + "angularVelocity": 0.0000013197716777984883, + "velocityX": -0.4381740314161649, + "velocityY": 3.4728185683109443, + "timestamp": 4.261020255918914 + }, + { + "x": 8.26369073045969, + "y": 4.0885925721999055, + "heading": -0.20328702869801377, + "angularVelocity": 0.0000013197717251490696, + "velocityX": -0.43817403141616196, + "velocityY": 3.472818568310943, + "timestamp": 4.319546013695454 + }, + { + "x": 8.23804626325309, + "y": 4.291841910533271, + "heading": -0.20328695145737063, + "angularVelocity": 0.000001319771773148376, + "velocityX": -0.43817403107387076, + "velocityY": 3.4728185683541293, + "timestamp": 4.378071771471995 + }, + { + "x": 8.212403297424316, + "y": 4.495091438293457, + "heading": -0.2032868742161759, + "angularVelocity": 0.0000013197811983448172, + "velocityX": -0.43814837779087523, + "velocityY": 3.472821804994326, + "timestamp": 4.436597529248536 + }, + { + "x": 8.211122236288011, + "y": 4.6734367956380565, + "heading": -0.20328678936705338, + "angularVelocity": 0.0000016652754099072682, + "velocityX": -0.025142506437536676, + "velocityY": 3.500261750249818, + "timestamp": 4.487549535146915 + }, + { + "x": 8.209842685648681, + "y": 4.851782163826195, + "heading": -0.2032867045174544, + "angularVelocity": 0.000001665284761962191, + "velocityX": -0.025112860951584333, + "velocityY": 3.5002619630684717, + "timestamp": 4.538501541045294 + }, + { + "x": 8.208563135039839, + "y": 5.030127532014551, + "heading": -0.203286619667852, + "angularVelocity": 0.0000016652848281480006, + "velocityX": -0.02511286035320464, + "velocityY": 3.500261963072762, + "timestamp": 4.589453546943673 + }, + { + "x": 8.207283584430996, + "y": 5.208472900202908, + "heading": -0.20328653481824618, + "angularVelocity": 0.000001665284895011747, + "velocityX": -0.02511286035319842, + "velocityY": 3.5002619630727594, + "timestamp": 4.6404055528420525 + }, + { + "x": 8.20600403382877, + "y": 5.386818268391311, + "heading": -0.2032864499686371, + "angularVelocity": 0.0000016652849598802982, + "velocityX": -0.025112860223413327, + "velocityY": 3.500261963073688, + "timestamp": 4.691357558740432 + }, + { + "x": 8.204724810861068, + "y": 5.565163638930035, + "heading": -0.20328636511888853, + "angularVelocity": 0.000001665287697022985, + "velocityX": -0.02510642996572845, + "velocityY": 3.5002620092018013, + "timestamp": 4.742309564638811 + }, + { + "x": 8.215723661757586, + "y": 5.743174123417012, + "heading": -0.20328627431671312, + "angularVelocity": 0.00000178211188758009, + "velocityX": 0.2158668869377758, + "velocityY": 3.4936894308343382, + "timestamp": 4.79326157053719 + }, + { + "x": 8.251384735107422, + "y": 5.917922496795654, + "heading": -0.20328616810400457, + "angularVelocity": 0.0000020845638294801198, + "velocityX": 0.6998953764638769, + "velocityY": 3.4296662181890745, + "timestamp": 4.844213576435569 + }, + { + "x": 8.319257308251775, + "y": 6.1013423979758015, + "heading": -0.20328607751404962, + "angularVelocity": 0.0000016213569308236664, + "velocityX": 1.21476676906589, + "velocityY": 3.282804679662124, + "timestamp": 4.900086501899726 + }, + { + "x": 8.406005091870338, + "y": 6.276626068328336, + "heading": -0.20328594240584189, + "angularVelocity": 0.000002418133767316469, + "velocityX": 1.5525906849858964, + "velocityY": 3.137184403651478, + "timestamp": 4.955959427363883 + }, + { + "x": 8.465469026478623, + "y": 6.461147567415963, + "heading": -0.19586462259478266, + "angularVelocity": 0.13282497290785408, + "velocityX": 1.0642710062932257, + "velocityY": 3.3025208104773145, + "timestamp": 5.01183235282804 + }, + { + "x": 8.497914422229119, + "y": 6.6500341576515325, + "heading": -0.1767449328748443, + "angularVelocity": 0.34219954586419293, + "velocityX": 0.5806997840360463, + "velocityY": 3.3806461477794345, + "timestamp": 5.0677052782921965 + }, + { + "x": 8.501533639733335, + "y": 6.83931682794884, + "heading": -0.15028692647848005, + "angularVelocity": 0.4735389489017775, + "velocityX": 0.0647758726458128, + "velocityY": 3.387735092173306, + "timestamp": 5.123578203756353 + }, + { + "x": 8.476232221452912, + "y": 7.02332099314858, + "heading": -0.1187876803017823, + "angularVelocity": 0.563765829603908, + "velocityX": -0.4528386167403034, + "velocityY": 3.293261694660663, + "timestamp": 5.17945112922051 + }, + { + "x": 8.424761116580283, + "y": 7.193085836951261, + "heading": -0.08469316743475845, + "angularVelocity": 0.6102152801878227, + "velocityX": -0.9212172880700084, + "velocityY": 3.038409791368238, + "timestamp": 5.235324054684667 + }, + { + "x": 8.349328318639657, + "y": 7.344985965125413, + "heading": -0.04934981017907167, + "angularVelocity": 0.6325667926294624, + "velocityX": -1.350077829538702, + "velocityY": 2.7186714658712017, + "timestamp": 5.291196980148824 + }, + { + "x": 8.251384735107422, + "y": 7.477188587188721, + "heading": -0.01358857618434331, + "angularVelocity": 0.640045848640404, + "velocityX": -1.752970382677875, + "velocityY": 2.366130303094952, + "timestamp": 5.3470699056129805 + }, + { + "x": 8.031164941682688, + "y": 7.638765513905215, + "heading": 0.04509310199225384, + "angularVelocity": 0.6364876296474269, + "velocityX": -2.388602007879415, + "velocityY": 1.7525353447119325, + "timestamp": 5.439266007223161 + }, + { + "x": 7.758143694279688, + "y": 7.7384188409284755, + "heading": 0.10025127172354484, + "angularVelocity": 0.5982700869989958, + "velocityX": -2.961310105685135, + "velocityY": 1.0808843897175913, + "timestamp": 5.531462108833341 + }, + { + "x": 7.44957756042063, + "y": 7.765731167037329, + "heading": 0.14290265837394545, + "angularVelocity": 0.4626159447688721, + "velocityX": -3.3468457827395444, + "velocityY": 0.296241659157495, + "timestamp": 5.623658210443521 + }, + { + "x": 7.1346990672621375, + "y": 7.715555620338561, + "heading": 0.15752386010214686, + "angularVelocity": 0.1585880690489728, + "velocityX": -3.4153124444441856, + "velocityY": -0.5442263373663762, + "timestamp": 5.715854312053701 + }, + { + "x": 6.838625346521012, + "y": 7.587150764950785, + "heading": 0.15752392264967224, + "angularVelocity": 6.784183309784681e-7, + "velocityX": -3.211347503530804, + "velocityY": -1.392736277838426, + "timestamp": 5.8080504136638815 + }, + { + "x": 6.564867571222415, + "y": 7.4162585819512294, + "heading": 0.15752396711824912, + "angularVelocity": 4.823259995470857e-7, + "velocityX": -2.9692988154324316, + "velocityY": -1.8535727651708478, + "timestamp": 5.900246515274062 + }, + { + "x": 6.291109923852278, + "y": 7.2453661940190575, + "heading": 0.15752401158682702, + "angularVelocity": 4.823260107723234e-7, + "velocityX": -2.969297427863337, + "velocityY": -1.8535749879613355, + "timestamp": 5.992442616884242 + }, + { + "x": 6.0173522764825655, + "y": 7.07447380608621, + "heading": 0.15752405605540484, + "angularVelocity": 4.823260098033014e-7, + "velocityX": -2.969297427858765, + "velocityY": -1.85357498796866, + "timestamp": 6.084638718494422 + }, + { + "x": 5.743594629112851, + "y": 6.903581418153362, + "heading": 0.1575241005239826, + "angularVelocity": 4.823260094668572e-7, + "velocityX": -2.9692974278587645, + "velocityY": -1.8535749879686596, + "timestamp": 6.176834820104602 + }, + { + "x": 5.469836981743135, + "y": 6.732689030220513, + "heading": 0.15752414499256026, + "angularVelocity": 4.823260082473403e-7, + "velocityX": -2.969297427858765, + "velocityY": -1.853574987968659, + "timestamp": 6.269030921714783 + }, + { + "x": 5.196079334373785, + "y": 6.5617966422870815, + "heading": 0.15752418946113778, + "angularVelocity": 4.823260064419635e-7, + "velocityX": -2.969297427854807, + "velocityY": -1.8535749879749983, + "timestamp": 6.361227023324963 + }, + { + "x": 4.922321797721961, + "y": 6.390904077000082, + "heading": 0.1575242339450377, + "angularVelocity": 4.824922002870454e-7, + "velocityX": -2.969296226963193, + "velocityY": -1.8535769116308105, + "timestamp": 6.453423124935143 + }, + { + "x": 4.71573342050983, + "y": 6.261612353964941, + "heading": 0.17376063284232746, + "angularVelocity": 0.1761072172654304, + "velocityX": -2.2407495935741277, + "velocityY": -1.4023556395237395, + "timestamp": 6.545619226545323 + }, + { + "x": 4.578007277346407, + "y": 6.1754183483776055, + "heading": 0.18457807169202894, + "angularVelocity": 0.11733076193871327, + "velocityX": -1.493839118553516, + "velocityY": -0.9348985920443467, + "timestamp": 6.6378153281555035 + }, + { + "x": 4.509144306182861, + "y": 6.132321357727051, + "heading": 0.18998797382164115, + "angularVelocity": 0.05867820911220429, + "velocityX": -0.7469184700965933, + "velocityY": -0.4674491643125701, + "timestamp": 6.730011429765684 + }, + { + "x": 4.509144306182861, + "y": 6.132321357727051, + "heading": 0.18998797382164115, + "angularVelocity": -1.332314755606386e-22, + "velocityX": 6.083262433472185e-23, + "velocityY": -4.699572509600775e-23, + "timestamp": 6.822207531375864 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index f862bba..fcbbc3b 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -4,6 +4,8 @@ import com.choreo.lib.ChoreoControlFunction; import com.choreo.lib.ChoreoTrajectory; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior; @@ -15,6 +17,8 @@ import frc.robot.intake.Intake; import frc.robot.lights.Lights; import frc.robot.shooter.Shooter; +import java.util.Set; +import java.util.function.BooleanSupplier; public class Autos { @@ -27,6 +31,10 @@ public class Autos { private final Arm m_arm; private final Lights m_lights; + private final PIDController m_translationController = new PIDController(3.0, 0.0, 0.0); + private final PIDController m_rotationController = new PIDController(3.0, 0.0, 0.0); + private boolean shootingWhileMoving = false; + public Autos( Drivetrain drivetrain, Shooter shooter, @@ -75,6 +83,23 @@ public Command sixPiece() { .withName("Six Piece"); } + public Command centerlineDisrupt() { + return Commands.sequence( + resetPose("CenterlineDisrupt"), + toggleShootingWhileMoving(), + Commands.deadline( + getPathFollowingCommand( + "CenterlineDisrupt.1", + shootWhileMovingControlFunction(() -> shootingWhileMoving)), + aimAndShoot().andThen(toggleShootingWhileMoving()))) + .withInterruptBehavior(InterruptionBehavior.kCancelIncoming) + .withName("Centerline Disrupt"); + } + + private Command toggleShootingWhileMoving() { + return Commands.runOnce(() -> shootingWhileMoving = !shootingWhileMoving); + } + private Command getPathFollowingCommand( String trajectoryName, ChoreoControlFunction controlFunction) { return Choreo.choreoSwerveCommand( @@ -90,9 +115,28 @@ private Command getPathFollowingCommand(String trajectoryName) { return getPathFollowingCommand( trajectoryName, Choreo.choreoSwerveController( - new PIDController(3.0, 0.0, 0.0), - new PIDController(3.0, 0.0, 0.0), - new PIDController(3.0, 0.0, 0.0))); + m_translationController, m_translationController, m_rotationController)); + } + + private ChoreoControlFunction shootWhileMovingControlFunction(BooleanSupplier shootingOnTheMove) { + return (pose, referenceState) -> { + double xFF = referenceState.velocityX; + double yFF = referenceState.velocityY; + double rotationFF = referenceState.angularVelocity; + + double xFeedback = m_translationController.calculate(pose.getX(), referenceState.x); + double yFeedback = m_translationController.calculate(pose.getY(), referenceState.y); + double rotationFeedback = + m_rotationController.calculate(pose.getRotation().getRadians(), referenceState.heading); + double omegaRadiansPerSecond = + shootingOnTheMove.getAsBoolean() ? 0.0 : rotationFF + rotationFeedback; + Rotation2d robotRotation = + shootingOnTheMove.getAsBoolean() + ? PoseEstimation.getInstance().getAimingParameters().driveHeading() + : pose.getRotation(); + return ChassisSpeeds.fromFieldRelativeSpeeds( + xFF + xFeedback, yFF + yFeedback, omegaRadiansPerSecond, robotRotation); + }; } private Command resetPose(String trajectoryName) { @@ -112,7 +156,24 @@ private Command intake() { private Command aim() { return Superstructure.aimAtGoal(m_drivetrain, m_shooter, m_arm, m_lights) - .alongWith(m_drivetrain.blankDrive()); + .alongWith( + Commands.defer( + () -> { + if (shootingWhileMoving) { + return Commands.startEnd( + () -> + m_drivetrain.setHeadingGoal( + () -> + PoseEstimation.getInstance() + .getAimingParameters() + .driveHeading()), + m_drivetrain::clearHeadingGoal); + } else { + return m_drivetrain.blankDrive(); + } + }, + Set.of())) + .withName("Aim"); } private Command aimAndShoot() { @@ -135,12 +196,14 @@ private Command aimAndShoot() { .repeatedly()) .beforeStarting(() -> state.timeoutTimer.start()) .andThen(Commands.print("WARNING: Aim and shoot timed out").onlyIf(state.hasTimedOut)) + .andThen(m_arm.stop()) .finallyDo( () -> { state.timeoutTimer.stop(); state.timeoutTimer.reset(); state.hasShot = false; - }); + }) + .withName("Aim and Shoot"); } private Command feedShooter() { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 94c9706..a6203cd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -197,6 +197,7 @@ public Robot() { m_autoChooser.addOption("Start Signal Logger", Commands.runOnce(SignalLogger::start)); m_autoChooser.addOption("End Signal Logger", Commands.runOnce(SignalLogger::stop)); m_autoChooser.addOption("6 Piece", autos.sixPiece()); + m_autoChooser.addOption("Centerline Disrupt", autos.centerlineDisrupt()); m_drivetrain.setDefaultCommand( m_drivetrain.joystickDrive( () -> -m_driverController.getLeftY(), diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java index 9504795..5416686 100644 --- a/src/main/java/frc/robot/vision/Vision.java +++ b/src/main/java/frc/robot/vision/Vision.java @@ -49,7 +49,7 @@ public class Vision { // #region Set these to false to disable vision measurements; private final boolean kTrustVisionXY = true; private final boolean kTrustVisionTheta = true; - private final boolean kIgnoreVisionInSim = false; + private final boolean kIgnoreVisionInSim = true; private final boolean kIgnoreVisionInAuto = false; // #endregion private final VisionIO[] io;