diff --git a/Crescendo2024.chor b/Crescendo2024.chor new file mode 100644 index 00000000..a180f910 --- /dev/null +++ b/Crescendo2024.chor @@ -0,0 +1,2246 @@ +{ + "version": "v0.2.2", + "robotConfiguration": { + "mass": 74.08797700309194, + "rotationalInertia": 6, + "motorMaxTorque": 0.7759834368530021, + "motorMaxVelocity": 5800, + "gearing": 5.731, + "wheelbase": 0.4724397448825378, + "trackWidth": 0.4724397448825378, + "bumperLength": 0.6095996708161777, + "bumperWidth": 0.5587996982481629, + "wheelRadius": 0.050799972568014815 + }, + "paths": { + "Test1": { + "waypoints": [ + { + "x": 1.4869295358657837, + "y": 5.564249038696289, + "heading": 3.141, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 2.6648950576782227, + "y": 6.813176155090332, + "heading": 3.8960730372009595, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 28 + }, + { + "x": 2.1113932132720947, + "y": 6.231289386749268, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 25 + }, + { + "x": 2.59393310546875, + "y": 5.550056457519531, + "heading": 3.14, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 25 + }, + { + "x": 1.941084861755371, + "y": 4.939785003662109, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 23 + }, + { + "x": 2.5655484199523926, + "y": 4.244359970092773, + "heading": 2.649783348569472, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.4869295358657837, + "y": 5.564249038696289, + "heading": 3.141, + "angularVelocity": 8.029589971503427e-22, + "velocityX": 1.7351458241819956e-21, + "velocityY": 6.201477276822387e-22, + "timestamp": 0 + }, + { + "x": 1.506153170031305, + "y": 5.579314508687984, + "heading": 3.1569331073832556, + "angularVelocity": 0.2189381700102546, + "velocityX": 0.2641535755648618, + "velocityY": 0.20701589156378014, + "timestamp": 0.07277446131256805 + }, + { + "x": 1.5445753142190985, + "y": 5.609481391551554, + "heading": 3.1887853931444132, + "angularVelocity": 0.4376849403852184, + "velocityX": 0.5279619181620503, + "velocityY": 0.41452567727023953, + "timestamp": 0.1455489226251361 + }, + { + "x": 1.6021601427736685, + "y": 5.654796415146542, + "heading": 3.2365776272564357, + "angularVelocity": 0.6567171127073494, + "velocityX": 0.7912779774108067, + "velocityY": 0.6226775544288833, + "timestamp": 0.21832338393770417 + }, + { + "x": 1.6788531458253628, + "y": 5.715327275578862, + "heading": 3.3003664786278266, + "angularVelocity": 0.8765279772723583, + "velocityX": 1.0538450119513225, + "velocityY": 0.8317596494783837, + "timestamp": 0.2910978452502722 + }, + { + "x": 1.7745670450726352, + "y": 5.791182757948707, + "heading": 3.380219753358853, + "angularVelocity": 1.097270571169936, + "velocityX": 1.3152127480020601, + "velocityY": 1.0423365697486087, + "timestamp": 0.36387230656284025 + }, + { + "x": 1.8891522077377947, + "y": 5.8825573533612445, + "heading": 3.4761247222373646, + "angularVelocity": 1.317838251891654, + "velocityX": 1.5745243674565124, + "velocityY": 1.255586008669736, + "timestamp": 0.4366467678754083 + }, + { + "x": 2.0223089309237166, + "y": 5.989857408352276, + "heading": 3.5877133538349852, + "angularVelocity": 1.5333487817703626, + "velocityX": 1.829717744168665, + "velocityY": 1.4744190895508729, + "timestamp": 0.5094212291879764 + }, + { + "x": 2.1730917573654285, + "y": 6.114310741146903, + "heading": 3.713064984762661, + "angularVelocity": 1.7224673143135665, + "velocityX": 2.0719195130019052, + "velocityY": 1.7101237240368758, + "timestamp": 0.5821956905005444 + }, + { + "x": 2.304244374220996, + "y": 6.249503988496409, + "heading": 3.8044927424626787, + "angularVelocity": 1.2563165161379026, + "velocityX": 1.8021791503514413, + "velocityY": 1.8577017941616667, + "timestamp": 0.6549701518131125 + }, + { + "x": 2.414916720860036, + "y": 6.372175349075486, + "heading": 3.8745670686179303, + "angularVelocity": 0.9628972154706921, + "velocityX": 1.5207580330096784, + "velocityY": 1.6856374937933305, + "timestamp": 0.7277446131256805 + }, + { + "x": 2.505647288657887, + "y": 6.481209512574969, + "heading": 3.9250064522902046, + "angularVelocity": 0.6930918176863726, + "velocityX": 1.2467363709936818, + "velocityY": 1.4982476205653745, + "timestamp": 0.8005190744382485 + }, + { + "x": 2.576640808380743, + "y": 6.576219510667492, + "heading": 3.9564510372491593, + "angularVelocity": 0.432082689336569, + "velocityX": 0.975527931672858, + "velocityY": 1.305540382971075, + "timestamp": 0.8732935357508166 + }, + { + "x": 2.628005259573955, + "y": 6.657012046349283, + "heading": 3.969203722939461, + "angularVelocity": 0.17523572775796842, + "velocityX": 0.7058032483758355, + "velocityY": 1.1101770349736413, + "timestamp": 0.9460679970633846 + }, + { + "x": 2.6598078437532253, + "y": 6.723475709345355, + "heading": 3.963391945850161, + "angularVelocity": -0.07986011829532402, + "velocityX": 0.4370019867639682, + "velocityY": 0.9132827890076579, + "timestamp": 1.0188424583759528 + }, + { + "x": 2.6720941279928496, + "y": 6.775543377099927, + "heading": 3.9390315825966313, + "angularVelocity": -0.3347378024400806, + "velocityX": 0.16882686615644266, + "velocityY": 0.7154662063514367, + "timestamp": 1.0916169196885208 + }, + { + "x": 2.6648950576782227, + "y": 6.813176155090332, + "heading": 3.896073037200959, + "angularVelocity": -0.5902969890929726, + "velocityX": -0.09892303130498589, + "velocityY": 0.5171151707845827, + "timestamp": 1.1643913810010889 + }, + { + "x": 2.659637231596543, + "y": 6.825031823463049, + "heading": 3.877816032306183, + "angularVelocity": -0.6840416135896586, + "velocityX": -0.19699681615987222, + "velocityY": 0.44420049020077496, + "timestamp": 1.191081284849143 + }, + { + "x": 2.6517927610129908, + "y": 6.834893701518374, + "heading": 3.857106562184998, + "angularVelocity": -0.775928989444263, + "velocityX": -0.29391153404714393, + "velocityY": 0.36949844823228756, + "timestamp": 1.2177711886971971 + }, + { + "x": 2.641397527843252, + "y": 6.842708311525597, + "heading": 3.834005474054525, + "angularVelocity": -0.865536581247652, + "velocityX": -0.38948185159896215, + "velocityY": 0.2927927373478789, + "timestamp": 1.2444610925452513 + }, + { + "x": 2.628493660297315, + "y": 6.848415370045661, + "heading": 3.808587288042731, + "angularVelocity": -0.9523521012476956, + "velocityX": -0.4834737367133058, + "velocityY": 0.2138283656829084, + "timestamp": 1.2711509963933054 + }, + { + "x": 2.6131312860336138, + "y": 6.85194659507741, + "heading": 3.7809426101756807, + "angularVelocity": -1.0357728534516963, + "velocityX": -0.5755874712460319, + "velocityY": 0.13230564830254915, + "timestamp": 1.2978409002413596 + }, + { + "x": 2.5953708985643273, + "y": 6.853224371337149, + "heading": 3.7511803744930092, + "angularVelocity": -1.1151121357389708, + "velocityX": -0.6654346741148351, + "velocityY": 0.0478748918322195, + "timestamp": 1.3245308040894137 + }, + { + "x": 2.5752865454465885, + "y": 6.852160328589509, + "heading": 3.7194297145938124, + "angularVelocity": -1.1896131241218921, + "velocityX": -0.752507511157751, + "velocityY": -0.03986686327897337, + "timestamp": 1.3512207079374678 + }, + { + "x": 2.5529701073140734, + "y": 6.848653945190506, + "heading": 3.6858413880737997, + "angularVelocity": -1.2584656247257915, + "velocityX": -0.8361378242335784, + "velocityY": -0.1313748981249787, + "timestamp": 1.377910611785522 + }, + { + "x": 2.5285370169793717, + "y": 6.842591376045072, + "heading": 3.650589102058915, + "angularVelocity": -1.3208097794422922, + "velocityX": -0.9154431755842751, + "velocityY": -0.2271484071260898, + "timestamp": 1.4046005156335761 + }, + { + "x": 2.502133887928051, + "y": 6.833844846711091, + "heading": 3.613872024847483, + "angularVelocity": -1.375691625584805, + "velocityX": -0.9892553079859069, + "velocityY": -0.32770928601974636, + "timestamp": 1.4312904194816303 + }, + { + "x": 2.4739487064187284, + "y": 6.822273243464229, + "heading": 3.5759212160148097, + "angularVelocity": -1.4219162814796094, + "velocityX": -1.0560240932219476, + "velocityY": -0.43355732237695227, + "timestamp": 1.4579803233296844 + }, + { + "x": 2.4442244144939402, + "y": 6.807725217915028, + "heading": 3.5370136666138436, + "angularVelocity": -1.4577628163243674, + "velocityX": -1.113690483637882, + "velocityY": -0.54507598199017, + "timestamp": 1.4846702271777386 + }, + { + "x": 2.4132762941421024, + "y": 6.79004764808573, + "heading": 3.497494895227568, + "angularVelocity": -1.4806636850869153, + "velocityX": -1.1595440930782523, + "velocityY": -0.6623317165148433, + "timestamp": 1.5113601310257927 + }, + { + "x": 2.3815108616317744, + "y": 6.769104478274178, + "heading": 3.45780088591702, + "angularVelocity": -1.4872293859328474, + "velocityX": -1.1901666147307672, + "velocityY": -0.7846850978100616, + "timestamp": 1.5380500348738468 + }, + { + "x": 2.3494374003667597, + "y": 6.744810785756249, + "heading": 3.4184626098399313, + "angularVelocity": -1.4739010039542162, + "velocityX": -1.2017076362510986, + "velocityY": -0.910220308631834, + "timestamp": 1.564739938721901 + }, + { + "x": 2.317656105280672, + "y": 6.717179467626764, + "heading": 3.3800987305060795, + "angularVelocity": -1.4373929390026206, + "velocityX": -1.190760943427104, + "velocityY": -1.0352722994728694, + "timestamp": 1.5914298425699551 + }, + { + "x": 2.2868057964945567, + "y": 6.686361396326711, + "heading": 3.343389515038899, + "angularVelocity": -1.3753970668521929, + "velocityX": -1.1558793527974576, + "velocityY": -1.1546714995865741, + "timestamp": 1.6181197464180093 + }, + { + "x": 2.25748075399746, + "y": 6.652639268574606, + "heading": 3.308925014353095, + "angularVelocity": -1.291293549876024, + "velocityX": -1.0987316651286732, + "velocityY": -1.263478802474685, + "timestamp": 1.6448096502660634 + }, + { + "x": 2.2301787041350396, + "y": 6.616374519308364, + "heading": 3.2770382136187797, + "angularVelocity": -1.194713960599006, + "velocityX": -1.022935489683713, + "velocityY": -1.3587440956212415, + "timestamp": 1.6714995541141175 + }, + { + "x": 2.2052911587380817, + "y": 6.577965167741144, + "heading": 3.2479010535236887, + "angularVelocity": -1.0916922091952401, + "velocityX": -0.9324704029899484, + "velocityY": -1.4390966631383846, + "timestamp": 1.6981894579621717 + }, + { + "x": 2.183095873519221, + "y": 6.537812097974725, + "heading": 3.2217255041499464, + "angularVelocity": -0.9807285002883435, + "velocityX": -0.8315985454731583, + "velocityY": -1.504429165238335, + "timestamp": 1.7248793618102258 + }, + { + "x": 2.163764936777705, + "y": 6.4962772278935255, + "heading": 3.198795874429834, + "angularVelocity": -0.8591124887766933, + "velocityX": -0.72427899521733, + "velocityY": -1.5562015628702863, + "timestamp": 1.75156926565828 + }, + { + "x": 2.1473967460074728, + "y": 6.453660043976164, + "heading": 3.1793849811459, + "angularVelocity": -0.7272747550699427, + "velocityX": -0.61327275150245, + "velocityY": -1.596752995439064, + "timestamp": 1.7782591695063341 + }, + { + "x": 2.134047988560311, + "y": 6.410199882298709, + "heading": 3.163697789085614, + "angularVelocity": -0.5877575336948715, + "velocityX": -0.5001425828716374, + "velocityY": -1.628337139199698, + "timestamp": 1.8049490733543883 + }, + { + "x": 2.1237534679344243, + "y": 6.366088523528368, + "heading": 3.151864532890396, + "angularVelocity": -0.443360765276075, + "velocityX": -0.38570841935190137, + "velocityY": -1.6527357693556206, + "timestamp": 1.8316389772024424 + }, + { + "x": 2.116536197487218, + "y": 6.321482513089734, + "heading": 3.143955647543306, + "angularVelocity": -0.29632498461271933, + "velocityX": -0.27041200628876155, + "velocityY": -1.671269057115243, + "timestamp": 1.8583288810504965 + }, + { + "x": 2.1124122616124033, + "y": 6.276512434450225, + "heading": 3.14, + "angularVelocity": -0.1482076355848078, + "velocityX": -0.15451295359818015, + "velocityY": -1.6849097282449244, + "timestamp": 1.8850187848985507 + }, + { + "x": 2.1113932132720947, + "y": 6.231289386749268, + "heading": 3.14, + "angularVelocity": 1.1480104978612554e-22, + "velocityX": -0.038181042019109025, + "velocityY": -1.6943878089038094, + "timestamp": 1.9117086887466048 + }, + { + "x": 2.1139722457961088, + "y": 6.184041017313733, + "heading": 3.14, + "angularVelocity": 1.326030822415817e-23, + "velocityX": 0.09287464315336291, + "velocityY": -1.7014812376514836, + "timestamp": 1.9394776516890055 + }, + { + "x": 2.1201947185383916, + "y": 6.136700429098736, + "heading": 3.14, + "angularVelocity": 1.3260179507784046e-23, + "velocityX": 0.224080126981686, + "velocityY": -1.7048021675563763, + "timestamp": 1.9672466146314063 + }, + { + "x": 2.13006161842343, + "y": 6.089396035622716, + "heading": 3.14, + "angularVelocity": 1.326007041519161e-23, + "velocityX": 0.35532115136976367, + "velocityY": -1.7034987433322955, + "timestamp": 1.995015577573807 + }, + { + "x": 2.1435678181376194, + "y": 6.042288248235824, + "heading": 3.14, + "angularVelocity": 1.3260437690465338e-23, + "velocityX": 0.4863775338749469, + "velocityY": -1.6964186773774967, + "timestamp": 2.022784540516208 + }, + { + "x": 2.1606964661173613, + "y": 5.995581737791835, + "heading": 3.14, + "angularVelocity": 1.325999769616951e-23, + "velocityX": 0.6168270675167464, + "velocityY": -1.6819681217793025, + "timestamp": 2.050553503458609 + }, + { + "x": 2.181407907708738, + "y": 5.949543614413858, + "heading": 3.14, + "angularVelocity": 1.3260185229673763e-23, + "velocityX": 0.7458485804578618, + "velocityY": -1.6578985493073877, + "timestamp": 2.07832246640101 + }, + { + "x": 2.2056170475643233, + "y": 5.904529822011414, + "heading": 3.14, + "angularVelocity": 1.3260157963898354e-23, + "velocityX": 0.871805688451557, + "velocityY": -1.6210109284892367, + "timestamp": 2.1060914293434108 + }, + { + "x": 2.2331463169007804, + "y": 5.861019319967013, + "heading": 3.14, + "angularVelocity": 1.326020172336188e-23, + "velocityX": 0.9913682910506778, + "velocityY": -1.566875296519094, + "timestamp": 2.1338603922858117 + }, + { + "x": 2.2636325105277852, + "y": 5.819639483049468, + "heading": 3.14, + "angularVelocity": 1.32601511090322e-23, + "velocityX": 1.0978513562152263, + "velocityY": -1.4901470034504147, + "timestamp": 2.1616293552282126 + }, + { + "x": 2.2963834267173127, + "y": 5.781115195207252, + "heading": 3.14, + "angularVelocity": 1.3260107576996925e-23, + "velocityX": 1.1794072489296905, + "velocityY": -1.387314604514559, + "timestamp": 2.1893983181706136 + }, + { + "x": 2.3303148904385163, + "y": 5.746039013141612, + "heading": 3.14, + "angularVelocity": 1.3260240855176863e-23, + "velocityX": 1.221920450957617, + "velocityY": -1.2631433928013844, + "timestamp": 2.2171672811130145 + }, + { + "x": 2.3642062447518297, + "y": 5.714607213055508, + "heading": 3.14, + "angularVelocity": 1.326023555417506e-23, + "velocityX": 1.2204760539171806, + "velocityY": -1.1319039948052778, + "timestamp": 2.2449362440554155 + }, + { + "x": 2.397064742551735, + "y": 5.68667060145179, + "heading": 3.14, + "angularVelocity": 1.3260077501265654e-23, + "velocityX": 1.1832814163085987, + "velocityY": -1.0060372676381828, + "timestamp": 2.2727052069978164 + }, + { + "x": 2.4282178000518853, + "y": 5.661954954768778, + "heading": 3.14, + "angularVelocity": 1.3260249833501151e-23, + "velocityX": 1.1218660763374204, + "velocityY": -0.8900457224231765, + "timestamp": 2.3004741699402174 + }, + { + "x": 2.457231034537111, + "y": 5.640189613922927, + "heading": 3.14, + "angularVelocity": 1.3260141947431602e-23, + "velocityX": 1.0448079946451803, + "velocityY": -0.7838009972139435, + "timestamp": 2.3282431328826183 + }, + { + "x": 2.483818861573163, + "y": 5.621144668272307, + "heading": 3.14, + "angularVelocity": 1.3260165607649488e-23, + "velocityX": 0.9574656097601072, + "velocityY": -0.6858356824532662, + "timestamp": 2.3560120958250192 + }, + { + "x": 2.507786212179532, + "y": 5.604632755958765, + "heading": 3.14, + "angularVelocity": 1.3260126001429812e-23, + "velocityX": 0.8630985123961222, + "velocityY": -0.5946175356923091, + "timestamp": 2.38378105876742 + }, + { + "x": 2.5289943781295285, + "y": 5.590502031840209, + "heading": 3.14, + "angularVelocity": 1.3260018717887446e-23, + "velocityX": 0.763736333761809, + "velocityY": -0.5088675492802951, + "timestamp": 2.411550021709821 + }, + { + "x": 2.547341071809568, + "y": 5.578628548447832, + "heading": 3.14, + "angularVelocity": 1.3260377381509874e-23, + "velocityX": 0.6606906321310845, + "velocityY": -0.4275810881740915, + "timestamp": 2.439318984652222 + }, + { + "x": 2.562748457639991, + "y": 5.568909973472095, + "heading": 3.14, + "angularVelocity": 1.3260175762620676e-23, + "velocityX": 0.5548419601546171, + "velocityY": -0.3499797596293578, + "timestamp": 2.467087947594623 + }, + { + "x": 2.575155705150767, + "y": 5.561260778200535, + "heading": 3.14, + "angularVelocity": 1.3260285574981282e-23, + "velocityX": 0.4468026961075851, + "velocityY": -0.2754584421256751, + "timestamp": 2.494856910537024 + }, + { + "x": 2.5845141850851863, + "y": 5.555608625671519, + "heading": 3.14, + "angularVelocity": 1.326006595026007e-23, + "velocityX": 0.33701222310069334, + "velocityY": -0.2035420818825477, + "timestamp": 2.522625873479425 + }, + { + "x": 2.5907842648688533, + "y": 5.55189166019714, + "heading": 3.14, + "angularVelocity": 1.3260302513246253e-23, + "velocityX": 0.22579452450827125, + "velocityY": -0.1338532332694335, + "timestamp": 2.550394836421826 + }, + { + "x": 2.59393310546875, + "y": 5.550056457519531, + "heading": 3.14, + "angularVelocity": 1.3260089047889746e-23, + "velocityX": 0.11339424545409957, + "velocityY": -0.06608826845335893, + "timestamp": 2.578163799364227 + }, + { + "x": 2.59393310546875, + "y": 5.550056457519531, + "heading": 3.14, + "angularVelocity": 4.554749859713047e-24, + "velocityX": -4.596278509340535e-25, + "velocityY": -1.5195442763313795e-23, + "timestamp": 2.6059327623066277 + }, + { + "x": 2.5899783566862378, + "y": 5.5489813066194555, + "heading": 3.1367490851985087, + "angularVelocity": -0.1085265960704612, + "velocityX": -0.13202296888338985, + "velocityY": -0.03589219483504766, + "timestamp": 2.6358877679845065 + }, + { + "x": 2.5820812048170954, + "y": 5.546779098674632, + "heading": 3.130275233897137, + "angularVelocity": -0.21611918124764368, + "velocityX": -0.26363379643671836, + "velocityY": -0.07351719337009978, + "timestamp": 2.6658427736623853 + }, + { + "x": 2.5702568313825034, + "y": 5.5433888083370855, + "heading": 3.1206149262957497, + "angularVelocity": -0.32249393324338926, + "velocityX": -0.3947378131636905, + "velocityY": -0.1131794256360211, + "timestamp": 2.695797779340264 + }, + { + "x": 2.554524436678559, + "y": 5.5387376635070815, + "heading": 3.107814789143658, + "angularVelocity": -0.42731212571741595, + "velocityX": -0.5252008586852795, + "velocityY": -0.15527103817038457, + "timestamp": 2.725752785018143 + }, + { + "x": 2.5349090755324424, + "y": 5.532737501107672, + "heading": 3.0919335422287366, + "angularVelocity": -0.5301700519005096, + "velocityX": -0.6548274888361012, + "velocityY": -0.20030583415450579, + "timestamp": 2.7557077906960217 + }, + { + "x": 2.511444606178007, + "y": 5.52527960958652, + "heading": 3.0730442108839475, + "angularVelocity": -0.6305901440285391, + "velocityX": -0.7833238159511905, + "velocityY": -0.24896979160511046, + "timestamp": 2.7856627963739005 + }, + { + "x": 2.4841786133457258, + "y": 5.5162272720643095, + "heading": 3.0512365155339536, + "angularVelocity": -0.7280150631418102, + "velocityX": -0.9102316028741883, + "velocityY": -0.3021978236143635, + "timestamp": 2.8156178020517793 + }, + { + "x": 2.4531809940870857, + "y": 5.505404760057258, + "heading": 3.026619340133933, + "angularVelocity": -0.8218050653951327, + "velocityX": -1.03480598842053, + "velocityY": -0.3612922702613114, + "timestamp": 2.845572807729658 + }, + { + "x": 2.4185596883958564, + "y": 5.492580836854246, + "heading": 2.999323661656212, + "angularVelocity": -0.9112226107130541, + "velocityX": -1.1557769697502154, + "velocityY": -0.4281061850200961, + "timestamp": 2.875527813407537 + }, + { + "x": 2.380491125275952, + "y": 5.477444069032361, + "heading": 2.9695096535475827, + "angularVelocity": -0.9952930214480606, + "velocityX": -1.27085815069692, + "velocityY": -0.5053168069690293, + "timestamp": 2.9054828190854156 + }, + { + "x": 2.3392828051052046, + "y": 5.459567907623698, + "heading": 2.9373981254397274, + "angularVelocity": -1.0719920554570026, + "velocityX": -1.3756739228789079, + "velocityY": -0.5967670846367005, + "timestamp": 2.9354378247632944 + }, + { + "x": 2.2955095509891357, + "y": 5.438376261665861, + "heading": 2.9034010779477137, + "angularVelocity": -1.1349371072601777, + "velocityX": -1.4613001441823985, + "velocityY": -0.7074492385586875, + "timestamp": 2.965392830441173 + }, + { + "x": 2.2502877957375587, + "y": 5.413212847808716, + "heading": 2.8684201412995205, + "angularVelocity": -1.1677826746007212, + "velocityX": -1.5096560400578645, + "velocityY": -0.8400403634617765, + "timestamp": 2.995347836119052 + }, + { + "x": 2.205434968136323, + "y": 5.383827174903616, + "heading": 2.8338778225381454, + "angularVelocity": -1.15314011730883, + "velocityX": -1.4973399799539564, + "velocityY": -0.9809937351072376, + "timestamp": 3.0253028417969308 + }, + { + "x": 2.1627117665288242, + "y": 5.35082401983714, + "heading": 2.8010931886625263, + "angularVelocity": -1.0944626159703914, + "velocityX": -1.426245819043514, + "velocityY": -1.1017575967561364, + "timestamp": 3.0552578474748096 + }, + { + "x": 2.1231203758060415, + "y": 5.315102619215835, + "heading": 2.770927863402835, + "angularVelocity": -1.0070211831729985, + "velocityX": -1.3216953169206167, + "velocityY": -1.1925018811692434, + "timestamp": 3.0852128531526883 + }, + { + "x": 2.0871120973907478, + "y": 5.277368588566571, + "heading": 2.743899688375806, + "angularVelocity": -0.9022924354505801, + "velocityX": -1.2020788379247493, + "velocityY": -1.2596903187079023, + "timestamp": 3.115167858830567 + }, + { + "x": 2.0548974336097707, + "y": 5.238102066889186, + "heading": 2.7202319811811844, + "angularVelocity": -0.7901085864957776, + "velocityX": -1.0754350751055555, + "velocityY": -1.310850082942305, + "timestamp": 3.145122864508446 + }, + { + "x": 2.026587153392112, + "y": 5.197634903381892, + "heading": 2.6999923980627787, + "angularVelocity": -0.6756661419481035, + "velocityX": -0.9450934685873016, + "velocityY": -1.350931591950202, + "timestamp": 3.1750778701863247 + }, + { + "x": 2.002245678733676, + "y": 5.1562068665429, + "heading": 2.6831832138201195, + "angularVelocity": -0.5611477568529534, + "velocityX": -0.812601236674494, + "velocityY": -1.3830088127670148, + "timestamp": 3.2050328758642035 + }, + { + "x": 1.9819131544340292, + "y": 5.113998389763002, + "heading": 2.66978325115483, + "angularVelocity": -0.44733634202529504, + "velocityX": -0.6787688347748394, + "velocityY": -1.4090625531434064, + "timestamp": 3.2349878815420823 + }, + { + "x": 1.9656157831702816, + "y": 5.071149791653941, + "heading": 2.659765787935313, + "angularVelocity": -0.33441700286222414, + "velocityX": -0.5440616983685925, + "velocityY": -1.4304319808793762, + "timestamp": 3.264942887219961 + }, + { + "x": 1.9533711923999129, + "y": 5.027773084636261, + "heading": 2.6531059130951715, + "angularVelocity": -0.22232927984587103, + "velocityX": -0.4087660974610013, + "velocityY": -1.4480620529380386, + "timestamp": 3.29489789289784 + }, + { + "x": 1.9451914498185667, + "y": 4.983959581247479, + "heading": 2.649783348569472, + "angularVelocity": -0.11091850762536054, + "velocityX": -0.27306763581709725, + "velocityY": -1.4626438018383596, + "timestamp": 3.3248528985757186 + }, + { + "x": 1.941084861755371, + "y": 4.939785003662109, + "heading": 2.649783348569472, + "angularVelocity": 6.132292113202646e-25, + "velocityX": -0.13709188064779135, + "velocityY": -1.4746976869375825, + "timestamp": 3.3548079042535974 + }, + { + "x": 1.9419022015392158, + "y": 4.889062683793002, + "heading": 2.649783348569472, + "angularVelocity": 4.614715107153485e-26, + "velocityX": 0.023936675164732843, + "velocityY": -1.4854577230996109, + "timestamp": 3.3889538237774923 + }, + { + "x": 1.948223657679148, + "y": 4.838070468392074, + "heading": 2.649783348569472, + "angularVelocity": 4.653945144562686e-26, + "velocityX": 0.18513064600613907, + "velocityY": -1.4933619042018718, + "timestamp": 3.423099743301387 + }, + { + "x": 1.9600538072506943, + "y": 4.786928471515363, + "heading": 2.649783348569472, + "angularVelocity": 4.629261284078221e-26, + "velocityX": 0.3464586614300294, + "velocityY": -1.4977484159101515, + "timestamp": 3.457245662825282 + }, + { + "x": 1.977394685635232, + "y": 4.735788039854744, + "heading": 2.649783348569472, + "angularVelocity": 4.640038416252255e-26, + "velocityX": 0.5078462851879747, + "velocityY": -1.4977025768725385, + "timestamp": 3.491391582349177 + }, + { + "x": 2.0002427480647933, + "y": 4.684845229382865, + "heading": 2.649783348569472, + "angularVelocity": 4.6490332666751425e-26, + "velocityX": 0.6691300966012094, + "velocityY": -1.4919150276866968, + "timestamp": 3.525537501873072 + }, + { + "x": 2.0285822715254445, + "y": 4.634362832128866, + "heading": 2.649783348569472, + "angularVelocity": 4.622795940866032e-26, + "velocityX": 0.8299534426308102, + "velocityY": -1.478431331119124, + "timestamp": 3.559683421396967 + }, + { + "x": 2.0623699922411927, + "y": 4.5847082412928, + "heading": 2.649783348569472, + "angularVelocity": 4.634016030607817e-26, + "velocityX": 0.9895097624213414, + "velocityY": -1.454188129311252, + "timestamp": 3.5938293409208617 + }, + { + "x": 2.101495965027088, + "y": 4.536421557771227, + "heading": 2.649783348569472, + "angularVelocity": 4.645177261974479e-26, + "velocityX": 1.1458462191511727, + "velocityY": -1.4141274915083855, + "timestamp": 3.6279752604447566 + }, + { + "x": 2.145673252803998, + "y": 4.4903376692314145, + "heading": 2.649783348569472, + "angularVelocity": 4.6233288536561975e-26, + "velocityX": 1.2937794147261192, + "velocityY": -1.3496162698902718, + "timestamp": 3.6621211799686515 + }, + { + "x": 2.1941116699379357, + "y": 4.447748182132556, + "heading": 2.649783348569472, + "angularVelocity": 4.6479494614855375e-26, + "velocityX": 1.4185711736373225, + "velocityY": -1.2472789631292271, + "timestamp": 3.6962670994925464 + }, + { + "x": 2.2448201323304646, + "y": 4.410180142680831, + "heading": 2.649783348569472, + "angularVelocity": 4.6329904581670297e-26, + "velocityX": 1.485051891985029, + "velocityY": -1.1002204648621148, + "timestamp": 3.7304130190164413 + }, + { + "x": 2.294852293597042, + "y": 4.3780811675391975, + "heading": 2.649783348569472, + "angularVelocity": 4.6446166642422895e-26, + "velocityX": 1.4652456856979683, + "velocityY": -0.9400530309096446, + "timestamp": 3.764558938540336 + }, + { + "x": 2.342149827765447, + "y": 4.350766530180341, + "heading": 2.649783348569472, + "angularVelocity": 4.633506801188826e-26, + "velocityX": 1.3851591882100787, + "velocityY": -0.7999385501902068, + "timestamp": 3.798704858064231 + }, + { + "x": 2.3856944951859207, + "y": 4.3274872473595005, + "heading": 2.649783348569472, + "angularVelocity": 4.6381778689211406e-26, + "velocityX": 1.2752524467821442, + "velocityY": -0.6817588498253565, + "timestamp": 3.832850777588126 + }, + { + "x": 2.4249725857215654, + "y": 4.307695719032298, + "heading": 2.649783348569472, + "angularVelocity": 4.6432377599415885e-26, + "velocityX": 1.150301151156826, + "velocityY": -0.5796162060697082, + "timestamp": 3.866996697112021 + }, + { + "x": 2.4596964858202144, + "y": 4.291007007021927, + "heading": 2.649783348569472, + "angularVelocity": 4.6331179794053255e-26, + "velocityX": 1.0169267831358226, + "velocityY": -0.4887468910800007, + "timestamp": 3.9011426166359158 + }, + { + "x": 2.4896890256889828, + "y": 4.277143601542489, + "heading": 2.649783348569472, + "angularVelocity": 4.6467164999430504e-26, + "velocityX": 0.8783638070657234, + "velocityY": -0.4060047488173824, + "timestamp": 3.9352885361598107 + }, + { + "x": 2.514832571254234, + "y": 4.265898475268687, + "heading": 2.649783348569472, + "angularVelocity": 4.632856845272505e-26, + "velocityX": 0.7363557905551701, + "velocityY": -0.3293256245723021, + "timestamp": 3.9694344556837056 + }, + { + "x": 2.5350443768209585, + "y": 4.257112226026017, + "heading": 2.649783348569472, + "angularVelocity": 4.629729705599566e-26, + "velocityX": 0.5919244773180152, + "velocityY": -0.2573147645510306, + "timestamp": 4.0035803752076005 + }, + { + "x": 2.550263563230007, + "y": 4.2506587632470065, + "heading": 2.649783348569472, + "angularVelocity": 4.62810585566047e-26, + "velocityX": 0.445710252388941, + "velocityY": -0.18899660249284694, + "timestamp": 4.037726294731495 + }, + { + "x": 2.5604437164785643, + "y": 4.246436059535304, + "heading": 2.649783348569472, + "angularVelocity": 4.6519462435401186e-26, + "velocityX": 0.29813674343821717, + "velocityY": -0.12366642253542871, + "timestamp": 4.07187221425539 + }, + { + "x": 2.5655484199523926, + "y": 4.244359970092773, + "heading": 2.649783348569472, + "angularVelocity": 4.6699948971014915e-26, + "velocityX": 0.14949673474910496, + "velocityY": -0.06080051354533503, + "timestamp": 4.106018133779285 + }, + { + "x": 2.5655484199523926, + "y": 4.244359970092773, + "heading": 2.649783348569472, + "angularVelocity": 1.535197914331468e-26, + "velocityX": 1.1963425727073795e-24, + "velocityY": -7.168329076335369e-24, + "timestamp": 4.14016405330318 + } + ], + "constraints": [ + { + "scope": [ + 2, + 3 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 4, + 5 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 3 + ], + "type": "WptZeroVelocity" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "Test2": { + "waypoints": [ + { + "x": 0.3137660622596741, + "y": 6.865104675292969, + "heading": 3.135568639860519, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 27 + }, + { + "x": 1.7816849946975708, + "y": 6.948645114898682, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 25 + }, + { + "x": 2.6290202140808105, + "y": 6.972513675689697, + "heading": 3.286405336491994, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 32 + }, + { + "x": 2.4533867835998535, + "y": 5.739410400390625, + "heading": 5.008312945270705, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.3137660622596754, + "y": 6.865104675292969, + "heading": 3.135568639860517, + "angularVelocity": -8.5693301650697e-28, + "velocityX": -3.4362695197914885e-17, + "velocityY": -4.743570524232907e-17, + "timestamp": 0 + }, + { + "x": 0.3176438425782737, + "y": 6.865425616765028, + "heading": 3.1355686398605154, + "angularVelocity": -5.1868461530169816e-17, + "velocityX": 0.13515003367410133, + "velocityY": 0.011185587421808195, + "timestamp": 0.028692411042588198 + }, + { + "x": 0.3253999903453479, + "y": 6.866060324505518, + "heading": 3.1355686398605136, + "angularVelocity": -5.186846153227154e-17, + "velocityX": 0.2703205302461453, + "velocityY": 0.022121101623313478, + "timestamp": 0.057384822085176396 + }, + { + "x": 0.3370351106624937, + "y": 6.867001226604551, + "heading": 3.135568639860512, + "angularVelocity": -5.186846153153239e-17, + "velocityX": 0.40551211607398097, + "velocityY": 0.032792716430754286, + "timestamp": 0.0860772331277646 + }, + { + "x": 0.352549826865152, + "y": 6.86824032070545, + "heading": 3.13556863986051, + "angularVelocity": -5.186846153220684e-17, + "velocityX": 0.5407254266512643, + "velocityY": 0.04318542973123231, + "timestamp": 0.11476964417035279 + }, + { + "x": 0.3719447806630506, + "y": 6.869769136338039, + "heading": 3.1355686398605083, + "angularVelocity": -5.1868461532349994e-17, + "velocityX": 0.6759611023663795, + "velocityY": 0.053282926635904025, + "timestamp": 0.14346205521294098 + }, + { + "x": 0.3952206321171861, + "y": 6.871578692739439, + "heading": 3.135568639860506, + "angularVelocity": -5.1868461531715824e-17, + "velocityX": 0.8112197828054859, + "velocityY": 0.06306742220841621, + "timestamp": 0.17215446625552916 + }, + { + "x": 0.42237805939934603, + "y": 6.8736594514681, + "heading": 3.1355686398605043, + "angularVelocity": -5.1868461532243226e-17, + "velocityX": 0.9465020991734979, + "velocityY": 0.07251947999676765, + "timestamp": 0.20084687729811734 + }, + { + "x": 0.45341775826418784, + "y": 6.876001262985059, + "heading": 3.1355686398605025, + "angularVelocity": -5.1868461531961763e-17, + "velocityX": 1.081808664276005, + "velocityY": 0.08161780177636822, + "timestamp": 0.22953928834070553 + }, + { + "x": 0.4883404411428946, + "y": 6.878593306214626, + "heading": 3.1355686398605007, + "angularVelocity": -5.186846153181617e-17, + "velocityX": 1.2171400593303083, + "velocityY": 0.09033898286621334, + "timestamp": 0.2582316993832937 + }, + { + "x": 0.5271468357396778, + "y": 6.881424019896891, + "heading": 3.135568639860499, + "angularVelocity": -5.186846153207198e-17, + "velocityX": 1.3524968166384297, + "velocityY": 0.09865722605409555, + "timestamp": 0.2869241104258819 + }, + { + "x": 0.5698376829754908, + "y": 6.884481024296066, + "heading": 3.135568639860497, + "angularVelocity": -5.18684615321628e-17, + "velocityX": 1.4878793968358233, + "velocityY": 0.10654400547371551, + "timestamp": 0.3156165214684701 + }, + { + "x": 0.6164137340738928, + "y": 6.887751031517755, + "heading": 3.1355686398604954, + "angularVelocity": -5.186846153204876e-17, + "velocityX": 1.6232881589932495, + "velocityY": 0.11396766959864212, + "timestamp": 0.34430893251105826 + }, + { + "x": 0.666875746517318, + "y": 6.891219742296381, + "heading": 3.1355686398604936, + "angularVelocity": -5.186846153160267e-17, + "velocityX": 1.758723321247663, + "velocityY": 0.12089296969422078, + "timestamp": 0.37300134355364645 + }, + { + "x": 0.7212244785112947, + "y": 6.894871726616115, + "heading": 3.135568639860492, + "angularVelocity": -5.186846153215831e-17, + "velocityX": 1.894184908800638, + "velocityY": 0.1272804963763128, + "timestamp": 0.40169375459623463 + }, + { + "x": 0.7794606814696723, + "y": 6.898690284891088, + "heading": 3.13556863986049, + "angularVelocity": -5.186846153213201e-17, + "velocityX": 2.02967268494572, + "velocityY": 0.1330860020548787, + "timestamp": 0.4303861656388228 + }, + { + "x": 0.8415850898614354, + "y": 6.9026572856068995, + "heading": 3.1355686398604883, + "angularVelocity": -5.186846153171342e-17, + "velocityX": 2.165186059113343, + "velocityY": 0.13825958055332968, + "timestamp": 0.459078576681411 + }, + { + "x": 0.9075984075182041, + "y": 6.906752974251311, + "heading": 3.1355686398604865, + "angularVelocity": -5.1868461532457525e-17, + "velocityX": 2.3007239635171715, + "velocityY": 0.14274466646709222, + "timestamp": 0.4877709877239992 + }, + { + "x": 0.9775012891595924, + "y": 6.910955746947304, + "heading": 3.1355686398604847, + "angularVelocity": -5.186846153166985e-17, + "velocityX": 2.43628468648487, + "velocityY": 0.14647680495568935, + "timestamp": 0.5164633987665874 + }, + { + "x": 1.051294315403438, + "y": 6.915241880318482, + "heading": 3.1355686398604825, + "angularVelocity": -5.1868461532007456e-17, + "velocityX": 2.571865645390099, + "velocityY": 0.1493821263335398, + "timestamp": 0.5451558098091757 + }, + { + "x": 1.1289779588157673, + "y": 6.919585206580955, + "heading": 3.1355686398604807, + "angularVelocity": -5.186846153206435e-17, + "velocityX": 2.7074630743656276, + "velocityY": 0.15137543708073178, + "timestamp": 0.5738482208517639 + }, + { + "x": 1.210552537505391, + "y": 6.923956719399065, + "heading": 3.135568639860479, + "angularVelocity": -5.1868461531968826e-17, + "velocityX": 2.843071590203452, + "velocityY": 0.1523578067950584, + "timestamp": 0.6025406318943521 + }, + { + "x": 1.29601815119489, + "y": 6.9283240912670765, + "heading": 3.135568639860477, + "angularVelocity": -5.186846153216223e-17, + "velocityX": 2.978683581614699, + "velocityY": 0.15221348465724693, + "timestamp": 0.6312330429369404 + }, + { + "x": 1.3853745923008893, + "y": 6.932651076487761, + "heading": 3.1355686398604754, + "angularVelocity": -5.1868461531841376e-17, + "velocityX": 3.1142883382426785, + "velocityY": 0.15080591220658612, + "timestamp": 0.6599254539795286 + }, + { + "x": 1.478621220826743, + "y": 6.936896764298285, + "heading": 3.1355686398604736, + "angularVelocity": -5.186846153215644e-17, + "velocityX": 3.2498707894378844, + "velocityY": 0.1479725006107481, + "timestamp": 0.6886178650221169 + }, + { + "x": 1.5757567859462194, + "y": 6.941014632922591, + "heading": 3.135568639860472, + "angularVelocity": -5.186846153173054e-17, + "velocityX": 3.3854096463101833, + "velocityY": 0.14351769247253987, + "timestamp": 0.7173102760647051 + }, + { + "x": 1.676779167520274, + "y": 6.94495133505747, + "heading": 3.13556863986047, + "angularVelocity": -5.186846153388186e-17, + "velocityX": 3.52087461120313, + "velocityY": 0.13720360164351214, + "timestamp": 0.7460026871072933 + }, + { + "x": 1.7816849946975712, + "y": 6.948645114898683, + "heading": 3.1355686398604683, + "angularVelocity": -1.6596882408000968e-16, + "velocityX": 3.6562220937649346, + "velocityY": 0.12873717150261788, + "timestamp": 0.7746950981498816 + }, + { + "x": 1.8121461784699802, + "y": 6.949692765946351, + "heading": 3.1362037838100756, + "angularVelocity": 0.07689012502825784, + "velocityX": 3.687611651202223, + "velocityY": 0.12682797354934053, + "timestamp": 0.7829555080448543 + }, + { + "x": 1.8428693964358074, + "y": 6.950724930078729, + "heading": 3.1374589452138397, + "angularVelocity": 0.15194904607940982, + "velocityX": 3.7193333450103565, + "velocityY": 0.12495313737441759, + "timestamp": 0.7912159179398269 + }, + { + "x": 1.8738574968140793, + "y": 6.95174197296817, + "heading": 3.139318096432083, + "angularVelocity": 0.22506767120320992, + "velocityX": 3.751399842413374, + "velocityY": 0.1231225692636009, + "timestamp": 0.7994763278347996 + }, + { + "x": 1.9051134303883683, + "y": 6.952744343319344, + "heading": 3.1417642609142864, + "angularVelocity": 0.2961311258529596, + "velocityX": 3.783823559810237, + "velocityY": 0.12134632105632295, + "timestamp": 0.8077367377297723 + }, + { + "x": 1.9366402470506743, + "y": 6.953732574884012, + "heading": 3.1447794649491048, + "angularVelocity": 0.36501869436959006, + "velocityX": 3.8166164952046153, + "velocityY": 0.11963468849991928, + "timestamp": 0.815997147624745 + }, + { + "x": 1.96844109067084, + "y": 6.9547072895390265, + "heading": 3.148344689757031, + "angularVelocity": 0.43160386146187707, + "velocityX": 3.849790025494764, + "velocityY": 0.117998339961998, + "timestamp": 0.8242575575197176 + }, + { + "x": 2.0005191919654135, + "y": 6.955669201722795, + "heading": 3.152439824908413, + "angularVelocity": 0.495754472653526, + "velocityX": 3.883354664287875, + "velocityY": 0.11644848088619933, + "timestamp": 0.8325179674146903 + }, + { + "x": 2.032877859008433, + "y": 6.9566191245667985, + "heading": 3.1570436241813726, + "angularVelocity": 0.5573330296555058, + "velocityX": 3.91731977643278, + "velocityY": 0.11499705899340439, + "timestamp": 0.840778377309663 + }, + { + "x": 2.065520465002766, + "y": 6.957557978090485, + "heading": 3.1621336650538163, + "angularVelocity": 0.6161971303075195, + "velocityX": 3.951693246384564, + "velocityY": 0.11365701407324179, + "timestamp": 0.8490387872046357 + }, + { + "x": 2.0984504329177724, + "y": 6.958486799841183, + "heading": 3.1676863130152344, + "angularVelocity": 0.6722000520579139, + "velocityX": 3.9864810988430395, + "velocityY": 0.11244257397721802, + "timestamp": 0.8572991970996083 + }, + { + "x": 2.1316712166001497, + "y": 6.959406758344203, + "heading": 3.1736766917418935, + "angularVelocity": 0.7251914617828771, + "velocityX": 4.021687071799445, + "velocityY": 0.1113695948160438, + "timestamp": 0.865559606994581 + }, + { + "x": 2.165186277981829, + "y": 6.960319169668765, + "heading": 3.1800786598480077, + "angularVelocity": 0.7750182118701426, + "velocityX": 4.057312144046869, + "velocityY": 0.11045593816320934, + "timestamp": 0.8738200168895537 + }, + { + "x": 2.1989990600414004, + "y": 6.961225517299853, + "heading": 3.1868647943382302, + "angularVelocity": 0.8215251514763765, + "velocityX": 4.093354021105941, + "velocityY": 0.10972187126423899, + "timestamp": 0.8820804267845264 + }, + { + "x": 2.2331129552203137, + "y": 6.962127475323903, + "heading": 3.19400637995892, + "angularVelocity": 0.8645558406293681, + "velocityX": 4.1298065849823535, + "velocityY": 0.10919046821108658, + "timestamp": 0.890340836679499 + }, + { + "x": 2.2675312690432237, + "y": 6.963026934685406, + "heading": 3.201473402262516, + "angularVelocity": 0.9039529997346885, + "velocityX": 4.1666593135838506, + "velocityY": 0.10888798170273117, + "timestamp": 0.8986012465744717 + }, + { + "x": 2.302257178726397, + "y": 6.963926031963223, + "heading": 3.2092345401896574, + "angularVelocity": 0.9395584511933502, + "velocityX": 4.203896673978105, + "velocityY": 0.10884414808067534, + "timestamp": 0.9068616564694444 + }, + { + "x": 2.3372936865473584, + "y": 6.964827179782931, + "heading": 3.2172571510612844, + "angularVelocity": 0.9712122005605541, + "velocityX": 4.241497488191654, + "velocityY": 0.10909238538572823, + "timestamp": 0.9151220663644171 + }, + { + "x": 2.372643567633051, + "y": 6.965733097685614, + "heading": 3.225507236598066, + "angularVelocity": 0.9987501397242283, + "velocityX": 4.279434257518503, + "velocityY": 0.10966984861524744, + "timestamp": 0.9233824762593897 + }, + { + "x": 2.4083093114869696, + "y": 6.9666468421059164, + "heading": 3.233949372180996, + "angularVelocity": 1.0219995969049733, + "velocityX": 4.31767240456481, + "velocityY": 0.11061732189065114, + "timestamp": 0.9316428861543624 + }, + { + "x": 2.44429305579275, + "y": 6.967571834188082, + "heading": 3.2425465717794997, + "angularVelocity": 1.0407715486052915, + "velocityX": 4.356169338240552, + "velocityY": 0.11197895672538041, + "timestamp": 0.9399032960493351 + }, + { + "x": 2.480596509367599, + "y": 6.968511884597095, + "heading": 3.251260045781759, + "angularVelocity": 1.054847654420144, + "velocityX": 4.394873140247142, + "velocityY": 0.11380190825438415, + "timestamp": 0.9481637059443078 + }, + { + "x": 2.517220857768171, + "y": 6.969471215284249, + "heading": 3.260048785175124, + "angularVelocity": 1.0639592350877263, + "velocityX": 4.433720464992984, + "velocityY": 0.11613596653794503, + "timestamp": 0.9564241158392804 + }, + { + "x": 2.55416663843719, + "y": 6.970454479070239, + "heading": 3.2688688682505744, + "angularVelocity": 1.0677536814268536, + "velocityX": 4.4726328522150816, + "velocityY": 0.11903329235341958, + "timestamp": 0.9646845257342531 + }, + { + "x": 2.5914335596417093, + "y": 6.971466777824258, + "heading": 3.2776723283732965, + "angularVelocity": 1.065741317277861, + "velocityX": 4.511509922431134, + "velocityY": 0.12254824722845813, + "timestamp": 0.9729449356292258 + }, + { + "x": 2.6290202140808088, + "y": 6.9725136756896955, + "heading": 3.286405336491996, + "angularVelocity": 1.0572124422077387, + "velocityX": 4.550216625687468, + "velocityY": 0.12673679378462502, + "timestamp": 0.9812053455241985 + }, + { + "x": 2.9717276780335036, + "y": 6.984363087388876, + "heading": 3.35108767309623, + "angularVelocity": 0.9203746590757935, + "velocityX": 4.876435853393211, + "velocityY": 0.16860705449782717, + "timestamp": 1.0514836131475132 + }, + { + "x": 3.3366815448761877, + "y": 6.999743371642888, + "heading": 3.3973428408952557, + "angularVelocity": 0.6581717131524953, + "velocityX": 5.192983253355129, + "velocityY": 0.21884836912099448, + "timestamp": 1.1217618807708278 + }, + { + "x": 3.71402485435163, + "y": 7.027526643127482, + "heading": 3.3973428368677423, + "angularVelocity": -5.730809398962461e-8, + "velocityX": 5.369274488921224, + "velocityY": 0.3953323327989187, + "timestamp": 1.1920401483941425 + }, + { + "x": 4.090360227255461, + "y": 7.066661937541599, + "heading": 3.3973428243317834, + "angularVelocity": -1.783760359331945e-7, + "velocityX": 5.354932408421835, + "velocityY": 0.5568619679682045, + "timestamp": 1.2623184160174572 + }, + { + "x": 4.466694898854889, + "y": 7.105803975313799, + "heading": 3.3973428117958613, + "angularVelocity": -1.7837551225317533e-7, + "velocityX": 5.354922429456401, + "velocityY": 0.5569579202208789, + "timestamp": 1.3325966836407719 + }, + { + "x": 4.8430291421579215, + "y": 7.144950130722809, + "heading": 3.397342799228156, + "angularVelocity": -1.7882775854425254e-7, + "velocityX": 5.354916335162799, + "velocityY": 0.5570165106920909, + "timestamp": 1.4028749512640866 + }, + { + "x": 5.214361577079249, + "y": 7.183676617694927, + "heading": 3.378511132398759, + "angularVelocity": -0.26795860891639617, + "velocityX": 5.283744854264688, + "velocityY": 0.55104498562348, + "timestamp": 1.4731532188874013 + }, + { + "x": 5.562524060208366, + "y": 7.219962031584869, + "heading": 3.355414523374288, + "angularVelocity": -0.32864511043819594, + "velocityX": 4.9540561385951625, + "velocityY": 0.5163105909843465, + "timestamp": 1.543431486510716 + }, + { + "x": 5.887470499962918, + "y": 7.253817931255832, + "heading": 3.331666055733143, + "angularVelocity": -0.3379205043646441, + "velocityX": 4.6237115788942855, + "velocityY": 0.48174066914158636, + "timestamp": 1.6137097541340306 + }, + { + "x": 6.189199807150463, + "y": 7.285249171486667, + "heading": 3.308363686841363, + "angularVelocity": -0.33157289841972754, + "velocityX": 4.29335152091094, + "velocityY": 0.4472398266744939, + "timestamp": 1.6839880217573453 + }, + { + "x": 6.467713626267079, + "y": 7.31425833816948, + "heading": 3.2860397702782755, + "angularVelocity": -0.3176503536305448, + "velocityX": 3.963014862708697, + "velocityY": 0.41277577925367004, + "timestamp": 1.75426628938066 + }, + { + "x": 6.723013594809074, + "y": 7.34084703024564, + "heading": 3.265008761402615, + "angularVelocity": -0.29925337642620564, + "velocityX": 3.6327015046867763, + "velocityY": 0.37833448340917136, + "timestamp": 1.8245445570039747 + }, + { + "x": 6.955101065031071, + "y": 7.365016333172441, + "heading": 3.245478308115181, + "angularVelocity": -0.27790174612893576, + "velocityX": 3.302407388098493, + "velocityY": 0.34390863269912847, + "timestamp": 1.8948228246272893 + }, + { + "x": 7.163977127241494, + "y": 7.386767031992689, + "heading": 3.2275957745381954, + "angularVelocity": -0.2544532496565816, + "velocityX": 2.972128785672115, + "velocityY": 0.3094939524808096, + "timestamp": 1.965101092250604 + }, + { + "x": 7.349642665971283, + "y": 7.4060997209757735, + "heading": 3.2114711639593976, + "angularVelocity": -0.22943949992088322, + "velocityX": 2.6418627693690517, + "velocityY": 0.2750877282106925, + "timestamp": 2.035379359873919 + }, + { + "x": 7.512098408217057, + "y": 7.423014865697383, + "heading": 3.1971897253730983, + "angularVelocity": -0.2032127294720464, + "velocityX": 2.3116070976097918, + "velocityY": 0.24068812868678965, + "timestamp": 2.1056576274972336 + }, + { + "x": 7.651344959888608, + "y": 7.43751284079522, + "heading": 3.184819461924971, + "angularVelocity": -0.17601833207433357, + "velocityX": 1.981360047431725, + "velocityY": 0.20629385993890986, + "timestamp": 2.1759358951205483 + }, + { + "x": 7.767382832665261, + "y": 7.449593954241994, + "heading": 3.174415882660127, + "angularVelocity": -0.14803408815663477, + "velocityX": 1.6511202780154728, + "velocityY": 0.1719039733808395, + "timestamp": 2.246214162743863 + }, + { + "x": 7.860212463836391, + "y": 7.459258463658005, + "heading": 3.1660251568616267, + "angularVelocity": -0.11939289459260129, + "velocityX": 1.3208867308552614, + "velocityY": 0.13751775254062693, + "timestamp": 2.3164924303671777 + }, + { + "x": 7.929834231127341, + "y": 7.466506587678373, + "heading": 3.159686290431417, + "angularVelocity": -0.09019668020541635, + "velocityX": 0.9906585584055106, + "velocityY": 0.10313464269233455, + "timestamp": 2.3867706979904924 + }, + { + "x": 7.976248463934981, + "y": 7.471338514116222, + "heading": 3.1554326760982865, + "angularVelocity": -0.06052531567696676, + "velocityX": 0.6604350729932147, + "velocityY": 0.06875420526505516, + "timestamp": 2.457048965613807 + }, + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "angularVelocity": -0.03044253542482025, + "velocityX": 0.33021570985126986, + "velocityY": 0.03437608724320182, + "timestamp": 2.5273272332371217 + }, + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "angularVelocity": -3.79407884851814e-18, + "velocityX": 2.546987187300363e-16, + "velocityY": -1.0256209789219734e-16, + "timestamp": 2.5976055008604364 + }, + { + "x": 7.9777911181971035, + "y": 7.466979623755239, + "heading": 3.153293227446569, + "angularVelocity": 4.0010921973360205e-29, + "velocityX": -0.31261406582002, + "velocityY": -0.09775939742848679, + "timestamp": 2.6669060734735086 + }, + { + "x": 7.934462450883723, + "y": 7.453430059384792, + "heading": 3.153293227446569, + "angularVelocity": -2.051733815514594e-34, + "velocityX": -0.6252281284210741, + "velocityY": -0.1955187938503514, + "timestamp": 2.736206646086581 + }, + { + "x": 7.869469450281316, + "y": 7.433105712944096, + "heading": 3.153293227446569, + "angularVelocity": -1.98522229630168e-34, + "velocityX": -0.9378421873262779, + "velocityY": -0.29327818911646475, + "timestamp": 2.805507218699653 + }, + { + "x": 7.782812116686986, + "y": 7.40600658452606, + "heading": 3.153293227446569, + "angularVelocity": -1.9190015159804115e-34, + "velocityX": -1.2504562419442953, + "velocityY": -0.39103758304190667, + "timestamp": 2.8748077913127252 + }, + { + "x": 7.6744904504495075, + "y": 7.372132674239751, + "heading": 3.153293227446569, + "angularVelocity": -8.200637720207044e-29, + "velocityX": -1.5630702915295283, + "velocityY": -0.4887969753935167, + "timestamp": 2.9441083639257974 + }, + { + "x": 7.544504451984089, + "y": 7.331483982215012, + "heading": 3.153293227446569, + "angularVelocity": 6.610929004011549e-29, + "velocityX": -1.8756843351233508, + "velocityY": -0.5865563658715173, + "timestamp": 3.0134089365388697 + }, + { + "x": 7.392854121793352, + "y": 7.28406050860902, + "heading": 3.153293227446569, + "angularVelocity": -6.483737543755527e-29, + "velocityX": -2.188298371464413, + "velocityY": -0.6843157540814643, + "timestamp": 3.082709509151942 + }, + { + "x": 7.219539460498179, + "y": 7.229862253615935, + "heading": 3.153293227446569, + "angularVelocity": 8.073376116568391e-29, + "velocityX": -2.5009123988461828, + "velocityY": -0.7820751394896976, + "timestamp": 3.152010081765014 + }, + { + "x": 7.024560468885024, + "y": 7.168889217481695, + "heading": 3.153293227446569, + "angularVelocity": -3.526812750348743e-30, + "velocityX": -2.813526414879514, + "velocityY": -0.8798345213490937, + "timestamp": 3.2213106543780863 + }, + { + "x": 6.807917147982327, + "y": 7.101141400527908, + "heading": 3.153293227446569, + "angularVelocity": -9.438045628849687e-29, + "velocityX": -3.1261404160725776, + "velocityY": -0.9775938985677018, + "timestamp": 3.2906112269911585 + }, + { + "x": 6.569609499192503, + "y": 7.026618803193132, + "heading": 3.153293227446569, + "angularVelocity": 9.790681244582267e-29, + "velocityX": -3.438754397028912, + "velocityY": -1.0753532694579622, + "timestamp": 3.3599117996042307 + }, + { + "x": 6.309637524541267, + "y": 6.94532142611084, + "heading": 3.153293227446569, + "angularVelocity": -1.3895984503554753e-34, + "velocityX": -3.7513683487544087, + "velocityY": -1.1731126312072742, + "timestamp": 3.429212372217303 + }, + { + "x": 6.028001227211883, + "y": 6.85724927027649, + "heading": 3.153293227446569, + "angularVelocity": -1.323503512813964e-34, + "velocityX": -4.063982254545725, + "velocityY": -1.2708719785922367, + "timestamp": 3.498512944830375 + }, + { + "x": 5.724700612934229, + "y": 6.762402337481905, + "heading": 3.153293227446569, + "angularVelocity": -2.1989550257903952e-29, + "velocityX": -4.376596077655505, + "velocityY": -1.3686313001213652, + "timestamp": 3.5678135174434473 + }, + { + "x": 5.399735695078014, + "y": 6.660780631908008, + "heading": 3.153293227446569, + "angularVelocity": 1.8024841235265752e-29, + "velocityX": -4.689209707841794, + "velocityY": -1.4663905613202457, + "timestamp": 3.6371140900565195 + }, + { + "x": 5.053106540487171, + "y": 6.552384174457959, + "heading": 3.153293227446569, + "angularVelocity": 1.896211807861914e-29, + "velocityX": -5.001822373477154, + "velocityY": -1.564149520888688, + "timestamp": 3.7064146626695917 + }, + { + "x": 4.7281416226309565, + "y": 6.4507624688840615, + "heading": 3.153293227446569, + "angularVelocity": -1.4997927313419928e-29, + "velocityX": -4.689209707841795, + "velocityY": -1.4663905613202457, + "timestamp": 3.775715235282664 + }, + { + "x": 4.424841008353304, + "y": 6.355915536089477, + "heading": 3.153293227446569, + "angularVelocity": -4.4185947102012545e-35, + "velocityX": -4.376596077655505, + "velocityY": -1.3686313001213652, + "timestamp": 3.845015807895736 + }, + { + "x": 4.14320471102392, + "y": 6.267843380255127, + "heading": 3.153293227446569, + "angularVelocity": -9.264733448000688e-35, + "velocityX": -4.063982254545726, + "velocityY": -1.2708719785922367, + "timestamp": 3.9143163805088084 + }, + { + "x": 3.883232736372683, + "y": 6.186546003172835, + "heading": 3.153293227446569, + "angularVelocity": 3.5265482976182584e-30, + "velocityX": -3.751368348754409, + "velocityY": -1.1731126312072742, + "timestamp": 3.9836169531218806 + }, + { + "x": 3.644925087582859, + "y": 6.112023405838059, + "heading": 3.153293227446569, + "angularVelocity": 2.156768838744686e-29, + "velocityX": -3.438754397028912, + "velocityY": -1.0753532694579622, + "timestamp": 4.052917525734953 + }, + { + "x": 3.4282817666801613, + "y": 6.044275588884272, + "heading": 3.153293227446569, + "angularVelocity": -2.105657187574439e-28, + "velocityX": -3.1261404160725776, + "velocityY": -0.9775938985677018, + "timestamp": 4.122218098348026 + }, + { + "x": 3.2333027750670063, + "y": 5.983302552750032, + "heading": 3.153293227446569, + "angularVelocity": 1.8547117767579628e-28, + "velocityX": -2.813526414879514, + "velocityY": -0.8798345213490937, + "timestamp": 4.1915186709610985 + }, + { + "x": 3.059988113771833, + "y": 5.929104297756947, + "heading": 3.153293227446569, + "angularVelocity": -5.955568710068408e-35, + "velocityX": -2.5009123988461828, + "velocityY": -0.7820751394896976, + "timestamp": 4.260819243574171 + }, + { + "x": 2.908337783581096, + "y": 5.881680824150955, + "heading": 3.153293227446569, + "angularVelocity": -2.0810713408746708e-29, + "velocityX": -2.1882983714644135, + "velocityY": -0.6843157540814642, + "timestamp": 4.330119816187244 + }, + { + "x": 2.7783517851156776, + "y": 5.841032132126216, + "heading": 3.153293227446569, + "angularVelocity": 2.081061415014159e-29, + "velocityX": -1.875684335123351, + "velocityY": -0.5865563658715173, + "timestamp": 4.3994203888003165 + }, + { + "x": 2.6700301188781994, + "y": 5.807158221839908, + "heading": 3.153293227446569, + "angularVelocity": 7.326893682940581e-29, + "velocityX": -1.5630702915295283, + "velocityY": -0.48879697539351663, + "timestamp": 4.468720961413389 + }, + { + "x": 2.583372785283869, + "y": 5.780059093421871, + "heading": 3.153293227446569, + "angularVelocity": -7.326900961882318e-29, + "velocityX": -1.2504562419442953, + "velocityY": -0.39103758304190656, + "timestamp": 4.538021534026462 + }, + { + "x": 2.5183797846814615, + "y": 5.759734746981175, + "heading": 3.153293227446569, + "angularVelocity": -2.64698564160025e-35, + "velocityX": -0.9378421873262781, + "velocityY": -0.2932781891164647, + "timestamp": 4.6073221066395345 + }, + { + "x": 2.4750511173680816, + "y": 5.746185182610728, + "heading": 3.153293227446569, + "angularVelocity": -4.5099737006589516e-29, + "velocityX": -0.6252281284210743, + "velocityY": -0.1955187938503513, + "timestamp": 4.676622679252607 + }, + { + "x": 2.4533867835998535, + "y": 5.739410400390625, + "heading": 3.153293227446569, + "angularVelocity": 1.9207053764808128e-28, + "velocityX": -0.31261406582002027, + "velocityY": -0.09775939742848665, + "timestamp": 4.74592325186568 + }, + { + "x": 2.4533867835998535, + "y": 5.739410400390625, + "heading": 3.153293227446569, + "angularVelocity": -4.899028012023128e-29, + "velocityX": 1.275108562409782e-22, + "velocityY": 3.987692290669026e-23, + "timestamp": 4.815223824478752 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 0, + 1 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3, + 4 + ], + "type": "ZeroAngularVelocity" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + } + }, + "splitTrajectoriesAtStopPoints": true, + "usesObstacles": false +} \ No newline at end of file diff --git a/build.gradle b/build.gradle index 82d1238f..d025c73f 100644 --- a/build.gradle +++ b/build.gradle @@ -50,12 +50,12 @@ deploy { frcJava(getArtifactTypeClass('FRCJavaArtifact')) { // Enable VisualVM connection - jvmArgs.add("-Dcom.sun.management.jmxremote=true") - jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") - jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") - jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") - jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") - jvmArgs.add("-Djava.rmi.server.hostname=10.44.67.2") // Replace XX.XX with team number +// jvmArgs.add("-Dcom.sun.management.jmxremote=true") +// jvmArgs.add("-Dcom.sun.management.jmxremote.port=1198") +// jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false") +// jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false") +// jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false") +// jvmArgs.add("-Djava.rmi.server.hostname=10.44.67.2") // Replace XX.XX with team number } // Static files artifact diff --git a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat index f2a66205..333fb9d2 100644 Binary files a/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 01 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat index dc553476..3db23480 100644 Binary files a/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 010 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 016 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 016 - 0 - ext.dat index 235336d0..be834f1e 100644 Binary files a/ctre_sim/Talon FX vers. C - 016 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 016 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 017 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 017 - 0 - ext.dat index 2ae33101..bae3be2a 100644 Binary files a/ctre_sim/Talon FX vers. C - 017 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 017 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat index f3019bae..81d44a58 100644 Binary files a/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 04 - 0 - ext.dat differ diff --git a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat index 552f487d..7d179b99 100644 Binary files a/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat and b/ctre_sim/Talon FX vers. C - 07 - 0 - ext.dat differ diff --git a/logs/FRC_20240310_203436.wpilog b/logs/FRC_20240310_203436.wpilog new file mode 100644 index 00000000..c9f7a3dd Binary files /dev/null and b/logs/FRC_20240310_203436.wpilog differ diff --git a/logs/FRC_20240310_203649.wpilog b/logs/FRC_20240310_203649.wpilog new file mode 100644 index 00000000..4a0a9c04 Binary files /dev/null and b/logs/FRC_20240310_203649.wpilog differ diff --git a/logs/FRC_20240310_221225.wpilog b/logs/FRC_20240310_221225.wpilog new file mode 100644 index 00000000..869abcd5 Binary files /dev/null and b/logs/FRC_20240310_221225.wpilog differ diff --git a/logs/FRC_20240310_221327.wpilog b/logs/FRC_20240310_221327.wpilog new file mode 100644 index 00000000..71056d22 Binary files /dev/null and b/logs/FRC_20240310_221327.wpilog differ diff --git a/logs/FRC_20240310_221513.wpilog b/logs/FRC_20240310_221513.wpilog new file mode 100644 index 00000000..d9f00e84 Binary files /dev/null and b/logs/FRC_20240310_221513.wpilog differ diff --git a/logs/FRC_20240310_221546.wpilog b/logs/FRC_20240310_221546.wpilog new file mode 100644 index 00000000..4e24f1de Binary files /dev/null and b/logs/FRC_20240310_221546.wpilog differ diff --git a/logs/FRC_20240310_221717.wpilog b/logs/FRC_20240310_221717.wpilog new file mode 100644 index 00000000..8b29e3c5 Binary files /dev/null and b/logs/FRC_20240310_221717.wpilog differ diff --git a/logs/FRC_20240310_221825.wpilog b/logs/FRC_20240310_221825.wpilog new file mode 100644 index 00000000..ebe6ce20 Binary files /dev/null and b/logs/FRC_20240310_221825.wpilog differ diff --git a/logs/FRC_20240310_221848.wpilog b/logs/FRC_20240310_221848.wpilog new file mode 100644 index 00000000..d9f596e9 Binary files /dev/null and b/logs/FRC_20240310_221848.wpilog differ diff --git a/logs/FRC_20240310_222046.wpilog b/logs/FRC_20240310_222046.wpilog new file mode 100644 index 00000000..e43651d5 Binary files /dev/null and b/logs/FRC_20240310_222046.wpilog differ diff --git a/logs/FRC_20240310_222123.wpilog b/logs/FRC_20240310_222123.wpilog new file mode 100644 index 00000000..75d29f57 Binary files /dev/null and b/logs/FRC_20240310_222123.wpilog differ diff --git a/logs/FRC_20240310_222300.wpilog b/logs/FRC_20240310_222300.wpilog new file mode 100644 index 00000000..098b959e Binary files /dev/null and b/logs/FRC_20240310_222300.wpilog differ diff --git a/logs/FRC_20240310_222316.wpilog b/logs/FRC_20240310_222316.wpilog new file mode 100644 index 00000000..e5f5b985 Binary files /dev/null and b/logs/FRC_20240310_222316.wpilog differ diff --git a/logs/FRC_20240310_222414.wpilog b/logs/FRC_20240310_222414.wpilog new file mode 100644 index 00000000..b3404005 Binary files /dev/null and b/logs/FRC_20240310_222414.wpilog differ diff --git a/logs/FRC_20240310_222606.wpilog b/logs/FRC_20240310_222606.wpilog new file mode 100644 index 00000000..51eedb5e Binary files /dev/null and b/logs/FRC_20240310_222606.wpilog differ diff --git a/logs/FRC_20240310_222655.wpilog b/logs/FRC_20240310_222655.wpilog new file mode 100644 index 00000000..63d0a10e Binary files /dev/null and b/logs/FRC_20240310_222655.wpilog differ diff --git a/logs/FRC_20240310_222707.wpilog b/logs/FRC_20240310_222707.wpilog new file mode 100644 index 00000000..3aa97de1 Binary files /dev/null and b/logs/FRC_20240310_222707.wpilog differ diff --git a/logs/FRC_20240310_222808.wpilog b/logs/FRC_20240310_222808.wpilog new file mode 100644 index 00000000..0d51f938 Binary files /dev/null and b/logs/FRC_20240310_222808.wpilog differ diff --git a/logs/FRC_20240310_222820.wpilog b/logs/FRC_20240310_222820.wpilog new file mode 100644 index 00000000..9ee78198 Binary files /dev/null and b/logs/FRC_20240310_222820.wpilog differ diff --git a/logs/FRC_20240310_222837.wpilog b/logs/FRC_20240310_222837.wpilog new file mode 100644 index 00000000..fc8d96a5 Binary files /dev/null and b/logs/FRC_20240310_222837.wpilog differ diff --git a/logs/FRC_20240310_222944.wpilog b/logs/FRC_20240310_222944.wpilog new file mode 100644 index 00000000..2bdb48ae Binary files /dev/null and b/logs/FRC_20240310_222944.wpilog differ diff --git a/logs/FRC_20240310_223012.wpilog b/logs/FRC_20240310_223012.wpilog new file mode 100644 index 00000000..cbaa85fa Binary files /dev/null and b/logs/FRC_20240310_223012.wpilog differ diff --git a/logs/FRC_20240310_223030.wpilog b/logs/FRC_20240310_223030.wpilog new file mode 100644 index 00000000..3b1bc118 Binary files /dev/null and b/logs/FRC_20240310_223030.wpilog differ diff --git a/logs/FRC_20240310_223043.wpilog b/logs/FRC_20240310_223043.wpilog new file mode 100644 index 00000000..bdf1ecaa Binary files /dev/null and b/logs/FRC_20240310_223043.wpilog differ diff --git a/logs/FRC_20240310_223255.wpilog b/logs/FRC_20240310_223255.wpilog new file mode 100644 index 00000000..a5e708b0 Binary files /dev/null and b/logs/FRC_20240310_223255.wpilog differ diff --git a/logs/FRC_20240310_223342.wpilog b/logs/FRC_20240310_223342.wpilog new file mode 100644 index 00000000..45479602 Binary files /dev/null and b/logs/FRC_20240310_223342.wpilog differ diff --git a/logs/FRC_20240310_223413.wpilog b/logs/FRC_20240310_223413.wpilog new file mode 100644 index 00000000..30cf3c74 Binary files /dev/null and b/logs/FRC_20240310_223413.wpilog differ diff --git a/logs/FRC_20240310_223523.wpilog b/logs/FRC_20240310_223523.wpilog new file mode 100644 index 00000000..36edc473 Binary files /dev/null and b/logs/FRC_20240310_223523.wpilog differ diff --git a/logs/FRC_20240310_223729.wpilog b/logs/FRC_20240310_223729.wpilog new file mode 100644 index 00000000..5bac5fc8 Binary files /dev/null and b/logs/FRC_20240310_223729.wpilog differ diff --git a/logs/FRC_20240310_223759.wpilog b/logs/FRC_20240310_223759.wpilog new file mode 100644 index 00000000..f8c615ce Binary files /dev/null and b/logs/FRC_20240310_223759.wpilog differ diff --git a/logs/FRC_20240310_223939.wpilog b/logs/FRC_20240310_223939.wpilog new file mode 100644 index 00000000..223b84cf Binary files /dev/null and b/logs/FRC_20240310_223939.wpilog differ diff --git a/logs/FRC_20240310_224020.wpilog b/logs/FRC_20240310_224020.wpilog new file mode 100644 index 00000000..c9328c7d Binary files /dev/null and b/logs/FRC_20240310_224020.wpilog differ diff --git a/logs/FRC_20240310_233720.wpilog b/logs/FRC_20240310_233720.wpilog new file mode 100644 index 00000000..0d6eba21 Binary files /dev/null and b/logs/FRC_20240310_233720.wpilog differ diff --git a/logs/FRC_20240310_233831.wpilog b/logs/FRC_20240310_233831.wpilog new file mode 100644 index 00000000..c2e3fc21 Binary files /dev/null and b/logs/FRC_20240310_233831.wpilog differ diff --git a/logs/FRC_20240310_234038.wpilog b/logs/FRC_20240310_234038.wpilog new file mode 100644 index 00000000..d8ca42bd Binary files /dev/null and b/logs/FRC_20240310_234038.wpilog differ diff --git a/logs/FRC_20240311_001406.wpilog b/logs/FRC_20240311_001406.wpilog new file mode 100644 index 00000000..3bc82ee1 Binary files /dev/null and b/logs/FRC_20240311_001406.wpilog differ diff --git a/logs/FRC_20240311_001514.wpilog b/logs/FRC_20240311_001514.wpilog new file mode 100644 index 00000000..c5f62212 Binary files /dev/null and b/logs/FRC_20240311_001514.wpilog differ diff --git a/logs/FRC_20240311_001544.wpilog b/logs/FRC_20240311_001544.wpilog new file mode 100644 index 00000000..99badd68 Binary files /dev/null and b/logs/FRC_20240311_001544.wpilog differ diff --git a/logs/FRC_20240311_001637.wpilog b/logs/FRC_20240311_001637.wpilog new file mode 100644 index 00000000..5d8540b1 Binary files /dev/null and b/logs/FRC_20240311_001637.wpilog differ diff --git a/logs/FRC_20240311_001653.wpilog b/logs/FRC_20240311_001653.wpilog new file mode 100644 index 00000000..4ff2eccf Binary files /dev/null and b/logs/FRC_20240311_001653.wpilog differ diff --git a/logs/FRC_20240311_001744.wpilog b/logs/FRC_20240311_001744.wpilog new file mode 100644 index 00000000..d76c1a36 Binary files /dev/null and b/logs/FRC_20240311_001744.wpilog differ diff --git a/logs/FRC_20240311_001807.wpilog b/logs/FRC_20240311_001807.wpilog new file mode 100644 index 00000000..8e61ec15 Binary files /dev/null and b/logs/FRC_20240311_001807.wpilog differ diff --git a/logs/FRC_20240311_001902.wpilog b/logs/FRC_20240311_001902.wpilog new file mode 100644 index 00000000..8b9265c8 Binary files /dev/null and b/logs/FRC_20240311_001902.wpilog differ diff --git a/logs/sim_2024-03-10_16-34-27.hoot b/logs/sim_2024-03-10_16-34-27.hoot new file mode 100644 index 00000000..1fc1cdbe Binary files /dev/null and b/logs/sim_2024-03-10_16-34-27.hoot differ diff --git a/networktables.json b/networktables.json index de79f40f..d14632ec 100644 --- a/networktables.json +++ b/networktables.json @@ -531,5 +531,325 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/Shooter/Left PID.mm.kp", + "type": "double", + "value": 0.19623, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Left PID.mm.kD", + "type": "double", + "value": 0.005, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Left PID.mm.kS", + "type": "double", + "value": 0.21963, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Left PID.mm.kV", + "type": "double", + "value": 0.13041, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right PID.mm.kp", + "type": "double", + "value": 0.19623, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right PID.mm.kD", + "type": "double", + "value": 0.005, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right PID.mm.kS", + "type": "double", + "value": 0.21963, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right PID.mm.kV", + "type": "double", + "value": 0.13041, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Left RPM", + "type": "double", + "value": 3600.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right RPM", + "type": "double", + "value": 3600.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kp", + "type": "double", + "value": 72.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.ki", + "type": "double", + "value": 6.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kD", + "type": "double", + "value": 3.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kS", + "type": "double", + "value": 0.375, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kV", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kG", + "type": "double", + "value": 0.375, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kp", + "type": "double", + "value": 108.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.ki", + "type": "double", + "value": 3.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kD", + "type": "double", + "value": 3.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kS", + "type": "double", + "value": 0.375, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kV", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kG", + "type": "double", + "value": 0.35, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm Max Vel DegsS", + "type": "double", + "value": 120.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist Max Vel DegsS", + "type": "double", + "value": 120.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Acceleration Time Secs", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kp", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kS", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kV", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kG", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kp", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kS", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kV", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kG", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/ArmPoses/Amp IntermediateArm Angle", + "type": "double", + "value": 60.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/ArmPoses/Amp IntermediateWrist Angle", + "type": "double", + "value": 145.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Wrist Angle", + "type": "double", + "value": 45.0, + "properties": { + "persistent": true + } } ] diff --git a/networktables.json.bck b/networktables.json.bck index c2f36cad..4168d9b3 100644 --- a/networktables.json.bck +++ b/networktables.json.bck @@ -106,9 +106,10 @@ { "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.kp", "type": "double", - "value": 7.0, + "value": 43.982, "properties": { - "persistent": true + "persistent": true, + "retained": true } }, { @@ -530,5 +531,317 @@ "properties": { "persistent": true } + }, + { + "name": "/Preferences/Shooter/Left PID.mm.kp", + "type": "double", + "value": 0.19623, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Left PID.mm.kD", + "type": "double", + "value": 0.005, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Left PID.mm.kS", + "type": "double", + "value": 0.21963, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Left PID.mm.kV", + "type": "double", + "value": 0.13041, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right PID.mm.kp", + "type": "double", + "value": 0.19623, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right PID.mm.kD", + "type": "double", + "value": 0.005, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right PID.mm.kS", + "type": "double", + "value": 0.21963, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right PID.mm.kV", + "type": "double", + "value": 0.13041, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Left RPM", + "type": "double", + "value": 3600.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Shooter/Right RPM", + "type": "double", + "value": 3600.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kp", + "type": "double", + "value": 72.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.ki", + "type": "double", + "value": 6.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kD", + "type": "double", + "value": 3.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kS", + "type": "double", + "value": 0.375, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kV", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm PID.mm.kG", + "type": "double", + "value": 0.375, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kp", + "type": "double", + "value": 108.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.ki", + "type": "double", + "value": 3.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kD", + "type": "double", + "value": 3.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kS", + "type": "double", + "value": 0.375, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kV", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist PID.mm.kG", + "type": "double", + "value": 0.35, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Arm Max Vel DegsS", + "type": "double", + "value": 120.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Wrist Max Vel DegsS", + "type": "double", + "value": 120.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Arm/Acceleration Time Secs", + "type": "double", + "value": 1.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kp", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kS", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kV", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Left Climber.mm.kG", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kp", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.ki", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kS", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kV", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/Climber/Right Climber.mm.kG", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/ArmPoses/Amp IntermediateArm Angle", + "type": "double", + "value": 60.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/ArmPoses/Amp IntermediateWrist Angle", + "type": "double", + "value": 145.0, + "properties": { + "persistent": true + } } ] diff --git a/src/main/deploy/choreo/NewPath.1.traj b/src/main/deploy/choreo/NewPath.1.traj deleted file mode 100644 index 38751a08..00000000 --- a/src/main/deploy/choreo/NewPath.1.traj +++ /dev/null @@ -1,670 +0,0 @@ -{ - "samples": [ - { - "x": 1.373390793800354, - "y": 5.677787780761719, - "heading": 0, - "angularVelocity": 1.7597793138716488e-8, - "velocityX": -0.24611796880717063, - "velocityY": -3.3913013287386686, - "timestamp": 0 - }, - { - "x": 1.352399870888487, - "y": 5.388550285801571, - "heading": 3.752867032965106e-9, - "angularVelocity": 4.895503936124812e-8, - "velocityX": -0.27382037475691573, - "velocityY": -3.773017489334137, - "timestamp": 0.07665946308963235 - }, - { - "x": 1.3314089479766167, - "y": 5.099312790841379, - "heading": 7.505725037785982e-9, - "angularVelocity": 4.8954921591779524e-8, - "velocityX": -0.27382037475695803, - "velocityY": -3.7730174893347197, - "timestamp": 0.1533189261792647 - }, - { - "x": 1.3104180250647461, - "y": 4.8100752958811865, - "heading": 1.1258583081678353e-8, - "angularVelocity": 4.895492210145562e-8, - "velocityX": -0.27382037475695814, - "velocityY": -3.77301748933472, - "timestamp": 0.22997838926889708 - }, - { - "x": 1.2894271021528758, - "y": 4.520837800920995, - "heading": 1.5011441030111597e-8, - "angularVelocity": 4.895492085621956e-8, - "velocityX": -0.2738203747569581, - "velocityY": -3.77301748933472, - "timestamp": 0.3066378523585294 - }, - { - "x": 1.2684361792410237, - "y": 4.231600305960802, - "heading": 1.876429908089443e-8, - "angularVelocity": 4.895492219133962e-8, - "velocityX": -0.2738203747567213, - "velocityY": -3.773017489334737, - "timestamp": 0.38329731544816176 - }, - { - "x": 1.2474452563299916, - "y": 3.9423628110005486, - "heading": 2.251715713543734e-8, - "angularVelocity": 4.8954922240388724e-8, - "velocityX": -0.27382037474602466, - "velocityY": -3.7730174893355133, - "timestamp": 0.4599567785377941 - }, - { - "x": 1.2264544553180678, - "y": 3.6531253071936995, - "heading": 2.6270015297030064e-8, - "angularVelocity": 4.8954923636821905e-8, - "velocityX": -0.27381878460824305, - "velocityY": -3.773017604736736, - "timestamp": 0.5366162416274265 - }, - { - "x": 1.221016329808459, - "y": 3.3631781154569036, - "heading": 3.0068452251973244e-8, - "angularVelocity": 4.954948550189996e-8, - "velocityX": -0.07093873724696509, - "velocityY": -3.782275273670803, - "timestamp": 0.6132757047170588 - }, - { - "x": 1.2644718237504313, - "y": 3.076455070529721, - "heading": 0.0000024276011016837486, - "angularVelocity": 0.00003127510359229755, - "velocityX": 0.5668640529240702, - "velocityY": -3.740217233088862, - "timestamp": 0.6899351678066912 - }, - { - "x": 1.3174690376414906, - "y": 2.8354946463151376, - "heading": 0.053981349204806524, - "angularVelocity": 0.7041390511774281, - "velocityX": 0.6913329647129592, - "velocityY": -3.1432573944960427, - "timestamp": 0.7665946308963235 - }, - { - "x": 1.3769500323372545, - "y": 2.642804109504741, - "heading": 0.127260696547334, - "angularVelocity": 0.9559073908050616, - "velocityX": 0.7759119657049679, - "velocityY": -2.5135910042194918, - "timestamp": 0.8432540939859559 - }, - { - "x": 1.4418762753700956, - "y": 2.498725528039788, - "heading": 0.2108122800980793, - "angularVelocity": 1.0899056709157284, - "velocityX": 0.8469436181274491, - "velocityY": -1.879462438922806, - "timestamp": 0.9199135570755882 - }, - { - "x": 1.5117489112481097, - "y": 2.403367219851462, - "heading": 0.3004574317472213, - "angularVelocity": 1.1693944626813566, - "velocityX": 0.9114678483505286, - "velocityY": -1.2439208982827146, - "timestamp": 0.9965730201652205 - }, - { - "x": 1.5862760543823242, - "y": 2.3567769527435303, - "heading": 0.393778947091327, - "angularVelocity": 1.2173515386481575, - "velocityX": 0.9721845174818851, - "velocityY": -0.607756240784745, - "timestamp": 1.073232483254853 - }, - { - "x": 1.6648735689075516, - "y": 2.3587409751636828, - "heading": 0.4887400714151268, - "angularVelocity": 1.2445985149202732, - "velocityX": 1.0301304934108237, - "velocityY": 0.025741264173079863, - "timestamp": 1.1495310833137506 - }, - { - "x": 1.747708145966923, - "y": 2.4090584282818597, - "heading": 0.5850785373582581, - "angularVelocity": 1.2626505056287294, - "velocityX": 1.0856631313736873, - "velocityY": 0.6594806861375633, - "timestamp": 1.2258296833726483 - }, - { - "x": 1.8345057249855814, - "y": 2.507753992923127, - "heading": 0.6817510390114973, - "angularVelocity": 1.2670285113830946, - "velocityX": 1.1376038217169926, - "velocityY": 1.2935435848767918, - "timestamp": 1.302128283431546 - }, - { - "x": 1.9248152988345435, - "y": 2.6548610401322725, - "heading": 0.7770428443944881, - "angularVelocity": 1.248932553276624, - "velocityX": 1.1836334320583706, - "velocityY": 1.9280438578897694, - "timestamp": 1.3784268834904436 - }, - { - "x": 2.017756729148212, - "y": 2.8504188772247816, - "heading": 0.8676164076432323, - "angularVelocity": 1.187093382825206, - "velocityX": 1.2181275965997216, - "velocityY": 2.5630593083169373, - "timestamp": 1.4547254835493413 - }, - { - "x": 2.110868096637194, - "y": 3.094357922878678, - "heading": 0.9442027964608976, - "angularVelocity": 1.003771874694233, - "velocityX": 1.2203548612570354, - "velocityY": 3.197162798080057, - "timestamp": 1.531024083608239 - }, - { - "x": 2.1793890973429906, - "y": 3.3747396477963205, - "heading": 0.9442028150467319, - "angularVelocity": 2.435933862401011e-7, - "velocityX": 0.898063668965131, - "velocityY": 3.6747951430459493, - "timestamp": 1.6073226836671366 - }, - { - "x": 2.2112713834031283, - "y": 3.6616064511690984, - "heading": 0.9442028192329045, - "angularVelocity": 5.486565302861873e-8, - "velocityX": 0.41786200579730515, - "velocityY": 3.7597911777062087, - "timestamp": 1.6836212837260343 - }, - { - "x": 2.243152894403425, - "y": 3.9484733406807293, - "heading": 0.9442028234190755, - "angularVelocity": 5.486563331370058e-8, - "velocityX": 0.41785184755271976, - "velocityY": 3.7597923066765064, - "timestamp": 1.759919883784932 - }, - { - "x": 2.275034438637313, - "y": 4.235340226498878, - "heading": 0.9442028276052465, - "angularVelocity": 5.4865633459900915e-8, - "velocityX": 0.41785228312547945, - "velocityY": 3.7597922582682526, - "timestamp": 1.8362184838438296 - }, - { - "x": 2.3114043136255, - "y": 4.521672682225187, - "heading": 0.9442028318036487, - "angularVelocity": 5.5025940885755316e-8, - "velocityX": 0.4766781429818065, - "velocityY": 3.752787803515112, - "timestamp": 1.9125170839027272 - }, - { - "x": 2.395240306854248, - "y": 4.7978620529174805, - "heading": 0.944202836290881, - "angularVelocity": 5.881146304501388e-8, - "velocityX": 1.098788092625971, - "velocityY": 3.6198484700779985, - "timestamp": 1.988815683961625 - }, - { - "x": 2.5138560537660655, - "y": 5.041648695718218, - "heading": 0.9442028403664345, - "angularVelocity": 5.686795868350027e-8, - "velocityX": 1.6550967466461282, - "velocityY": 3.401660318130064, - "timestamp": 2.0604826406831447 - }, - { - "x": 2.669379769810612, - "y": 5.263716246351053, - "heading": 0.9442028441855257, - "angularVelocity": 5.328942931675909e-8, - "velocityX": 2.170089580458573, - "velocityY": 3.098604444663902, - "timestamp": 2.1321495974046645 - }, - { - "x": 2.829793194306078, - "y": 5.482277808796993, - "heading": 0.9442028479893113, - "angularVelocity": 5.307586429308322e-8, - "velocityX": 2.2383177943328927, - "velocityY": 3.049683877260457, - "timestamp": 2.2038165541261843 - }, - { - "x": 2.9902066606790894, - "y": 5.70083934050687, - "heading": 0.9442028517930969, - "angularVelocity": 5.307586336486897e-8, - "velocityX": 2.2383183786683687, - "velocityY": 3.049683448386907, - "timestamp": 2.275483510847704 - }, - { - "x": 3.1506201270524525, - "y": 5.919400872216489, - "heading": 0.9442028555968824, - "angularVelocity": 5.307586343565819e-8, - "velocityX": 2.2383183786732843, - "velocityY": 3.0496834483832997, - "timestamp": 2.347150467569224 - }, - { - "x": 3.3110335934258157, - "y": 6.137962403926107, - "heading": 0.9442028594006682, - "angularVelocity": 5.3075864810896055e-8, - "velocityX": 2.2383183786732843, - "velocityY": 3.0496834483832993, - "timestamp": 2.4188174242907436 - }, - { - "x": 3.4714470597991793, - "y": 6.3565239356357255, - "heading": 0.9442028632044539, - "angularVelocity": 5.307586438143788e-8, - "velocityX": 2.2383183786732848, - "velocityY": 3.0496834483832993, - "timestamp": 2.4904843810122634 - }, - { - "x": 3.631860526174069, - "y": 6.575085467344224, - "heading": 0.9442028670082396, - "angularVelocity": 5.307586459415007e-8, - "velocityX": 2.2383183786945806, - "velocityY": 3.0496834483676687, - "timestamp": 2.5621513377337832 - }, - { - "x": 3.7922741739851236, - "y": 6.793646865887341, - "heading": 0.9442028708120256, - "angularVelocity": 5.307586821274769e-8, - "velocityX": 2.2383209103517525, - "velocityY": 3.0496815902535848, - "timestamp": 2.633818294455303 - }, - { - "x": 3.9681792086390733, - "y": 6.999945295226872, - "heading": 0.9442028746807025, - "angularVelocity": 5.3981320792667595e-8, - "velocityX": 2.4544789216804146, - "velocityY": 2.878571084595562, - "timestamp": 2.705485251176823 - }, - { - "x": 4.17437520362896, - "y": 7.175970389393745, - "heading": 0.9442028789510033, - "angularVelocity": 5.958535155683375e-8, - "velocityX": 2.8771417738737903, - "velocityY": 2.4561541639177027, - "timestamp": 2.7771522078983426 - }, - { - "x": 4.405717344151184, - "y": 7.3173296257195375, - "heading": 0.9442028842473172, - "angularVelocity": 7.390175547435562e-8, - "velocityX": 3.228016803073774, - "velocityY": 1.9724464773225439, - "timestamp": 2.8488191646198624 - }, - { - "x": 4.656433083655096, - "y": 7.420495670437542, - "heading": 0.9442028919014063, - "angularVelocity": 1.0680080940487753e-7, - "velocityX": 3.498344997097253, - "velocityY": 1.4395203792296967, - "timestamp": 2.920486121341382 - }, - { - "x": 4.920266408469536, - "y": 7.482894237840949, - "heading": 0.9442029059804885, - "angularVelocity": 1.964515145326873e-7, - "velocityX": 3.6813803304029413, - "velocityY": 0.8706741608391041, - "timestamp": 2.992153078062902 - }, - { - "x": 5.190633947862971, - "y": 7.502968306176006, - "heading": 0.9442029492980424, - "angularVelocity": 6.044285388451107e-7, - "velocityX": 3.772555048542354, - "velocityY": 0.2801021454427761, - "timestamp": 3.0638200347844218 - }, - { - "x": 5.460788726806641, - "y": 7.480216979980469, - "heading": 0.9442042125234751, - "angularVelocity": 0.00001762633003205625, - "velocityX": 3.7695863100958116, - "velocityY": -0.31745908067434947, - "timestamp": 3.1354869915059416 - }, - { - "x": 5.6861880959283475, - "y": 7.430105467011311, - "heading": 0.94453500375596, - "angularVelocity": 0.00541646085327845, - "velocityX": 3.6907473331858487, - "velocityY": -0.8205388221514872, - "timestamp": 3.196558462612879 - }, - { - "x": 5.902931349588688, - "y": 7.350124119825956, - "heading": 0.944535163065715, - "angularVelocity": 0.0000026085789673009567, - "velocityX": 3.5490098687948497, - "velocityY": -1.3096351821180778, - "timestamp": 3.2576299337198167 - }, - { - "x": 6.106969137969694, - "y": 7.241757655990003, - "heading": 0.9445351882967145, - "angularVelocity": 4.1313888378756724e-7, - "velocityX": 3.340967307365667, - "velocityY": -1.7744203942000976, - "timestamp": 3.3187014048267542 - }, - { - "x": 6.294604237914379, - "y": 7.106969701935095, - "heading": 0.9445352004387211, - "angularVelocity": 1.988163445738462e-7, - "velocityX": 3.0723854615542603, - "velocityY": -2.207052681257527, - "timestamp": 3.379772875933692 - }, - { - "x": 6.462436717197839, - "y": 6.94820261304716, - "heading": 0.9445352081247251, - "angularVelocity": 1.2585260943351558e-7, - "velocityX": 2.7481322496649945, - "velocityY": -2.599693212071642, - "timestamp": 3.4408443470406294 - }, - { - "x": 6.607425485391203, - "y": 6.7683332434118935, - "heading": 0.944535213822349, - "angularVelocity": 9.329435992491954e-8, - "velocityX": 2.3740834397043513, - "velocityY": -2.945227393005009, - "timestamp": 3.501915818147567 - }, - { - "x": 6.726943413314801, - "y": 6.570620826362678, - "heading": 0.9445352185350564, - "angularVelocity": 7.716708691487089e-8, - "velocityX": 1.9570173398037076, - "velocityY": -3.237394047754587, - "timestamp": 3.5629872892545045 - }, - { - "x": 6.818825069558712, - "y": 6.358647957429321, - "heading": 0.9445352227854475, - "angularVelocity": 6.959699896596626e-8, - "velocityX": 1.5044939081788649, - "velocityY": -3.470898360417538, - "timestamp": 3.624058760361442 - }, - { - "x": 6.885266799619373, - "y": 6.137378364635986, - "heading": 0.944535226910566, - "angularVelocity": 6.754575173659361e-8, - "velocityX": 1.0879340034945393, - "velocityY": -3.623125311749637, - "timestamp": 3.6851302314683796 - }, - { - "x": 6.951706477654193, - "y": 5.916108155680916, - "heading": 0.9445352310356804, - "angularVelocity": 6.754568510636454e-8, - "velocityX": 1.087900403094797, - "velocityY": -3.6231354009406083, - "timestamp": 3.746201702575317 - }, - { - "x": 7.007755279541016, - "y": 5.691980361938477, - "heading": 0.9445352351916124, - "angularVelocity": 6.805030176915139e-8, - "velocityX": 0.917757520343414, - "velocityY": -3.669926230366784, - "timestamp": 3.8072731736822547 - }, - { - "x": 7.033287944264436, - "y": 5.499297549749691, - "heading": 0.9445352391429122, - "angularVelocity": 7.690359033060341e-8, - "velocityX": 0.49693866281360616, - "velocityY": -3.750158476346985, - "timestamp": 3.858653085599064 - }, - { - "x": 7.048377702483028, - "y": 5.30551704932481, - "heading": 0.9445352430586353, - "angularVelocity": 7.621117031703296e-8, - "velocityX": 0.29368984211229937, - "velocityY": -3.7715226281165486, - "timestamp": 3.9100329975158736 - }, - { - "x": 7.063467124411225, - "y": 5.111736522713165, - "heading": 0.9445352469743582, - "angularVelocity": 7.621116311616718e-8, - "velocityX": 0.2936832969396488, - "velocityY": -3.7715231377858505, - "timestamp": 3.961412909432683 - }, - { - "x": 7.0785565463316305, - "y": 4.917955996100912, - "heading": 0.9445352508900811, - "angularVelocity": 7.621116383171076e-8, - "velocityX": 0.29368329678799265, - "velocityY": -3.7715231377976592, - "timestamp": 4.0127928213494926 - }, - { - "x": 7.093645968252035, - "y": 4.72417546948866, - "heading": 0.9445352548058039, - "angularVelocity": 7.621116339702062e-8, - "velocityX": 0.29368329678798916, - "velocityY": -3.77152313779766, - "timestamp": 4.064172733266302 - }, - { - "x": 7.1087353901724395, - "y": 4.530394942876407, - "heading": 0.9445352587215268, - "angularVelocity": 7.621116320689e-8, - "velocityX": 0.2936832967879892, - "velocityY": -3.77152313779766, - "timestamp": 4.1155526451831115 - }, - { - "x": 7.1238248120928445, - "y": 4.336614416264154, - "heading": 0.9445352626372497, - "angularVelocity": 7.621116316535163e-8, - "velocityX": 0.29368329678798916, - "velocityY": -3.77152313779766, - "timestamp": 4.166932557099921 - }, - { - "x": 7.138914234013249, - "y": 4.142833889651901, - "heading": 0.9445352665529726, - "angularVelocity": 7.621116395669404e-8, - "velocityX": 0.2936832967879891, - "velocityY": -3.7715231377976606, - "timestamp": 4.21831246901673 - }, - { - "x": 7.1540036559336535, - "y": 3.9490533630396487, - "heading": 0.9445352704686956, - "angularVelocity": 7.621116465419702e-8, - "velocityX": 0.29368329678798905, - "velocityY": -3.7715231377976597, - "timestamp": 4.26969238093354 - }, - { - "x": 7.1690930778540585, - "y": 3.755272836427396, - "heading": 0.9445352743844184, - "angularVelocity": 7.621116300190572e-8, - "velocityX": 0.29368329678798905, - "velocityY": -3.7715231377976597, - "timestamp": 4.321072292850349 - }, - { - "x": 7.184182499774463, - "y": 3.561492309815143, - "heading": 0.9445352783001413, - "angularVelocity": 7.621116375934217e-8, - "velocityX": 0.29368329678798927, - "velocityY": -3.7715231377976597, - "timestamp": 4.372452204767159 - }, - { - "x": 7.199271921694868, - "y": 3.367711783202891, - "heading": 0.9445352822158641, - "angularVelocity": 7.621116358329164e-8, - "velocityX": 0.2936832967879893, - "velocityY": -3.7715231377976597, - "timestamp": 4.423832116683968 - }, - { - "x": 7.2143613436152725, - "y": 3.173931256590638, - "heading": 0.944535286131587, - "angularVelocity": 7.621116395825444e-8, - "velocityX": 0.2936832967879889, - "velocityY": -3.77152313779766, - "timestamp": 4.475212028600778 - }, - { - "x": 7.2294507655356774, - "y": 2.980150729978386, - "heading": 0.94453529004731, - "angularVelocity": 7.621116318952045e-8, - "velocityX": 0.29368329678798905, - "velocityY": -3.77152313779766, - "timestamp": 4.526591940517587 - }, - { - "x": 7.244540187456082, - "y": 2.786370203366133, - "heading": 0.9445352939630328, - "angularVelocity": 7.621116338095618e-8, - "velocityX": 0.29368329678798905, - "velocityY": -3.77152313779766, - "timestamp": 4.577971852434397 - }, - { - "x": 7.2596296093764865, - "y": 2.5925896767538807, - "heading": 0.9445352978787557, - "angularVelocity": 7.621116372521823e-8, - "velocityX": 0.2936832967879889, - "velocityY": -3.77152313779766, - "timestamp": 4.629351764351206 - }, - { - "x": 7.2747190312968915, - "y": 2.398809150141628, - "heading": 0.9445353017944785, - "angularVelocity": 7.621116368417063e-8, - "velocityX": 0.2936832967879889, - "velocityY": -3.77152313779766, - "timestamp": 4.680731676268016 - }, - { - "x": 7.289808453217296, - "y": 2.2050286235293757, - "heading": 0.9445353057102015, - "angularVelocity": 7.621116284744578e-8, - "velocityX": 0.29368329678798905, - "velocityY": -3.77152313779766, - "timestamp": 4.732111588184825 - }, - { - "x": 7.3048978751377005, - "y": 2.011248096917123, - "heading": 0.9445353096259244, - "angularVelocity": 7.621116384489279e-8, - "velocityX": 0.29368329678798877, - "velocityY": -3.77152313779766, - "timestamp": 4.7834915001016345 - }, - { - "x": 7.3199872970581055, - "y": 1.8174675703048704, - "heading": 0.9445353135416472, - "angularVelocity": 7.621116322392135e-8, - "velocityX": 0.2936832967879892, - "velocityY": -3.77152313779766, - "timestamp": 4.834871412018444 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NewPath.traj b/src/main/deploy/choreo/NewPath.traj deleted file mode 100644 index 38751a08..00000000 --- a/src/main/deploy/choreo/NewPath.traj +++ /dev/null @@ -1,670 +0,0 @@ -{ - "samples": [ - { - "x": 1.373390793800354, - "y": 5.677787780761719, - "heading": 0, - "angularVelocity": 1.7597793138716488e-8, - "velocityX": -0.24611796880717063, - "velocityY": -3.3913013287386686, - "timestamp": 0 - }, - { - "x": 1.352399870888487, - "y": 5.388550285801571, - "heading": 3.752867032965106e-9, - "angularVelocity": 4.895503936124812e-8, - "velocityX": -0.27382037475691573, - "velocityY": -3.773017489334137, - "timestamp": 0.07665946308963235 - }, - { - "x": 1.3314089479766167, - "y": 5.099312790841379, - "heading": 7.505725037785982e-9, - "angularVelocity": 4.8954921591779524e-8, - "velocityX": -0.27382037475695803, - "velocityY": -3.7730174893347197, - "timestamp": 0.1533189261792647 - }, - { - "x": 1.3104180250647461, - "y": 4.8100752958811865, - "heading": 1.1258583081678353e-8, - "angularVelocity": 4.895492210145562e-8, - "velocityX": -0.27382037475695814, - "velocityY": -3.77301748933472, - "timestamp": 0.22997838926889708 - }, - { - "x": 1.2894271021528758, - "y": 4.520837800920995, - "heading": 1.5011441030111597e-8, - "angularVelocity": 4.895492085621956e-8, - "velocityX": -0.2738203747569581, - "velocityY": -3.77301748933472, - "timestamp": 0.3066378523585294 - }, - { - "x": 1.2684361792410237, - "y": 4.231600305960802, - "heading": 1.876429908089443e-8, - "angularVelocity": 4.895492219133962e-8, - "velocityX": -0.2738203747567213, - "velocityY": -3.773017489334737, - "timestamp": 0.38329731544816176 - }, - { - "x": 1.2474452563299916, - "y": 3.9423628110005486, - "heading": 2.251715713543734e-8, - "angularVelocity": 4.8954922240388724e-8, - "velocityX": -0.27382037474602466, - "velocityY": -3.7730174893355133, - "timestamp": 0.4599567785377941 - }, - { - "x": 1.2264544553180678, - "y": 3.6531253071936995, - "heading": 2.6270015297030064e-8, - "angularVelocity": 4.8954923636821905e-8, - "velocityX": -0.27381878460824305, - "velocityY": -3.773017604736736, - "timestamp": 0.5366162416274265 - }, - { - "x": 1.221016329808459, - "y": 3.3631781154569036, - "heading": 3.0068452251973244e-8, - "angularVelocity": 4.954948550189996e-8, - "velocityX": -0.07093873724696509, - "velocityY": -3.782275273670803, - "timestamp": 0.6132757047170588 - }, - { - "x": 1.2644718237504313, - "y": 3.076455070529721, - "heading": 0.0000024276011016837486, - "angularVelocity": 0.00003127510359229755, - "velocityX": 0.5668640529240702, - "velocityY": -3.740217233088862, - "timestamp": 0.6899351678066912 - }, - { - "x": 1.3174690376414906, - "y": 2.8354946463151376, - "heading": 0.053981349204806524, - "angularVelocity": 0.7041390511774281, - "velocityX": 0.6913329647129592, - "velocityY": -3.1432573944960427, - "timestamp": 0.7665946308963235 - }, - { - "x": 1.3769500323372545, - "y": 2.642804109504741, - "heading": 0.127260696547334, - "angularVelocity": 0.9559073908050616, - "velocityX": 0.7759119657049679, - "velocityY": -2.5135910042194918, - "timestamp": 0.8432540939859559 - }, - { - "x": 1.4418762753700956, - "y": 2.498725528039788, - "heading": 0.2108122800980793, - "angularVelocity": 1.0899056709157284, - "velocityX": 0.8469436181274491, - "velocityY": -1.879462438922806, - "timestamp": 0.9199135570755882 - }, - { - "x": 1.5117489112481097, - "y": 2.403367219851462, - "heading": 0.3004574317472213, - "angularVelocity": 1.1693944626813566, - "velocityX": 0.9114678483505286, - "velocityY": -1.2439208982827146, - "timestamp": 0.9965730201652205 - }, - { - "x": 1.5862760543823242, - "y": 2.3567769527435303, - "heading": 0.393778947091327, - "angularVelocity": 1.2173515386481575, - "velocityX": 0.9721845174818851, - "velocityY": -0.607756240784745, - "timestamp": 1.073232483254853 - }, - { - "x": 1.6648735689075516, - "y": 2.3587409751636828, - "heading": 0.4887400714151268, - "angularVelocity": 1.2445985149202732, - "velocityX": 1.0301304934108237, - "velocityY": 0.025741264173079863, - "timestamp": 1.1495310833137506 - }, - { - "x": 1.747708145966923, - "y": 2.4090584282818597, - "heading": 0.5850785373582581, - "angularVelocity": 1.2626505056287294, - "velocityX": 1.0856631313736873, - "velocityY": 0.6594806861375633, - "timestamp": 1.2258296833726483 - }, - { - "x": 1.8345057249855814, - "y": 2.507753992923127, - "heading": 0.6817510390114973, - "angularVelocity": 1.2670285113830946, - "velocityX": 1.1376038217169926, - "velocityY": 1.2935435848767918, - "timestamp": 1.302128283431546 - }, - { - "x": 1.9248152988345435, - "y": 2.6548610401322725, - "heading": 0.7770428443944881, - "angularVelocity": 1.248932553276624, - "velocityX": 1.1836334320583706, - "velocityY": 1.9280438578897694, - "timestamp": 1.3784268834904436 - }, - { - "x": 2.017756729148212, - "y": 2.8504188772247816, - "heading": 0.8676164076432323, - "angularVelocity": 1.187093382825206, - "velocityX": 1.2181275965997216, - "velocityY": 2.5630593083169373, - "timestamp": 1.4547254835493413 - }, - { - "x": 2.110868096637194, - "y": 3.094357922878678, - "heading": 0.9442027964608976, - "angularVelocity": 1.003771874694233, - "velocityX": 1.2203548612570354, - "velocityY": 3.197162798080057, - "timestamp": 1.531024083608239 - }, - { - "x": 2.1793890973429906, - "y": 3.3747396477963205, - "heading": 0.9442028150467319, - "angularVelocity": 2.435933862401011e-7, - "velocityX": 0.898063668965131, - "velocityY": 3.6747951430459493, - "timestamp": 1.6073226836671366 - }, - { - "x": 2.2112713834031283, - "y": 3.6616064511690984, - "heading": 0.9442028192329045, - "angularVelocity": 5.486565302861873e-8, - "velocityX": 0.41786200579730515, - "velocityY": 3.7597911777062087, - "timestamp": 1.6836212837260343 - }, - { - "x": 2.243152894403425, - "y": 3.9484733406807293, - "heading": 0.9442028234190755, - "angularVelocity": 5.486563331370058e-8, - "velocityX": 0.41785184755271976, - "velocityY": 3.7597923066765064, - "timestamp": 1.759919883784932 - }, - { - "x": 2.275034438637313, - "y": 4.235340226498878, - "heading": 0.9442028276052465, - "angularVelocity": 5.4865633459900915e-8, - "velocityX": 0.41785228312547945, - "velocityY": 3.7597922582682526, - "timestamp": 1.8362184838438296 - }, - { - "x": 2.3114043136255, - "y": 4.521672682225187, - "heading": 0.9442028318036487, - "angularVelocity": 5.5025940885755316e-8, - "velocityX": 0.4766781429818065, - "velocityY": 3.752787803515112, - "timestamp": 1.9125170839027272 - }, - { - "x": 2.395240306854248, - "y": 4.7978620529174805, - "heading": 0.944202836290881, - "angularVelocity": 5.881146304501388e-8, - "velocityX": 1.098788092625971, - "velocityY": 3.6198484700779985, - "timestamp": 1.988815683961625 - }, - { - "x": 2.5138560537660655, - "y": 5.041648695718218, - "heading": 0.9442028403664345, - "angularVelocity": 5.686795868350027e-8, - "velocityX": 1.6550967466461282, - "velocityY": 3.401660318130064, - "timestamp": 2.0604826406831447 - }, - { - "x": 2.669379769810612, - "y": 5.263716246351053, - "heading": 0.9442028441855257, - "angularVelocity": 5.328942931675909e-8, - "velocityX": 2.170089580458573, - "velocityY": 3.098604444663902, - "timestamp": 2.1321495974046645 - }, - { - "x": 2.829793194306078, - "y": 5.482277808796993, - "heading": 0.9442028479893113, - "angularVelocity": 5.307586429308322e-8, - "velocityX": 2.2383177943328927, - "velocityY": 3.049683877260457, - "timestamp": 2.2038165541261843 - }, - { - "x": 2.9902066606790894, - "y": 5.70083934050687, - "heading": 0.9442028517930969, - "angularVelocity": 5.307586336486897e-8, - "velocityX": 2.2383183786683687, - "velocityY": 3.049683448386907, - "timestamp": 2.275483510847704 - }, - { - "x": 3.1506201270524525, - "y": 5.919400872216489, - "heading": 0.9442028555968824, - "angularVelocity": 5.307586343565819e-8, - "velocityX": 2.2383183786732843, - "velocityY": 3.0496834483832997, - "timestamp": 2.347150467569224 - }, - { - "x": 3.3110335934258157, - "y": 6.137962403926107, - "heading": 0.9442028594006682, - "angularVelocity": 5.3075864810896055e-8, - "velocityX": 2.2383183786732843, - "velocityY": 3.0496834483832993, - "timestamp": 2.4188174242907436 - }, - { - "x": 3.4714470597991793, - "y": 6.3565239356357255, - "heading": 0.9442028632044539, - "angularVelocity": 5.307586438143788e-8, - "velocityX": 2.2383183786732848, - "velocityY": 3.0496834483832993, - "timestamp": 2.4904843810122634 - }, - { - "x": 3.631860526174069, - "y": 6.575085467344224, - "heading": 0.9442028670082396, - "angularVelocity": 5.307586459415007e-8, - "velocityX": 2.2383183786945806, - "velocityY": 3.0496834483676687, - "timestamp": 2.5621513377337832 - }, - { - "x": 3.7922741739851236, - "y": 6.793646865887341, - "heading": 0.9442028708120256, - "angularVelocity": 5.307586821274769e-8, - "velocityX": 2.2383209103517525, - "velocityY": 3.0496815902535848, - "timestamp": 2.633818294455303 - }, - { - "x": 3.9681792086390733, - "y": 6.999945295226872, - "heading": 0.9442028746807025, - "angularVelocity": 5.3981320792667595e-8, - "velocityX": 2.4544789216804146, - "velocityY": 2.878571084595562, - "timestamp": 2.705485251176823 - }, - { - "x": 4.17437520362896, - "y": 7.175970389393745, - "heading": 0.9442028789510033, - "angularVelocity": 5.958535155683375e-8, - "velocityX": 2.8771417738737903, - "velocityY": 2.4561541639177027, - "timestamp": 2.7771522078983426 - }, - { - "x": 4.405717344151184, - "y": 7.3173296257195375, - "heading": 0.9442028842473172, - "angularVelocity": 7.390175547435562e-8, - "velocityX": 3.228016803073774, - "velocityY": 1.9724464773225439, - "timestamp": 2.8488191646198624 - }, - { - "x": 4.656433083655096, - "y": 7.420495670437542, - "heading": 0.9442028919014063, - "angularVelocity": 1.0680080940487753e-7, - "velocityX": 3.498344997097253, - "velocityY": 1.4395203792296967, - "timestamp": 2.920486121341382 - }, - { - "x": 4.920266408469536, - "y": 7.482894237840949, - "heading": 0.9442029059804885, - "angularVelocity": 1.964515145326873e-7, - "velocityX": 3.6813803304029413, - "velocityY": 0.8706741608391041, - "timestamp": 2.992153078062902 - }, - { - "x": 5.190633947862971, - "y": 7.502968306176006, - "heading": 0.9442029492980424, - "angularVelocity": 6.044285388451107e-7, - "velocityX": 3.772555048542354, - "velocityY": 0.2801021454427761, - "timestamp": 3.0638200347844218 - }, - { - "x": 5.460788726806641, - "y": 7.480216979980469, - "heading": 0.9442042125234751, - "angularVelocity": 0.00001762633003205625, - "velocityX": 3.7695863100958116, - "velocityY": -0.31745908067434947, - "timestamp": 3.1354869915059416 - }, - { - "x": 5.6861880959283475, - "y": 7.430105467011311, - "heading": 0.94453500375596, - "angularVelocity": 0.00541646085327845, - "velocityX": 3.6907473331858487, - "velocityY": -0.8205388221514872, - "timestamp": 3.196558462612879 - }, - { - "x": 5.902931349588688, - "y": 7.350124119825956, - "heading": 0.944535163065715, - "angularVelocity": 0.0000026085789673009567, - "velocityX": 3.5490098687948497, - "velocityY": -1.3096351821180778, - "timestamp": 3.2576299337198167 - }, - { - "x": 6.106969137969694, - "y": 7.241757655990003, - "heading": 0.9445351882967145, - "angularVelocity": 4.1313888378756724e-7, - "velocityX": 3.340967307365667, - "velocityY": -1.7744203942000976, - "timestamp": 3.3187014048267542 - }, - { - "x": 6.294604237914379, - "y": 7.106969701935095, - "heading": 0.9445352004387211, - "angularVelocity": 1.988163445738462e-7, - "velocityX": 3.0723854615542603, - "velocityY": -2.207052681257527, - "timestamp": 3.379772875933692 - }, - { - "x": 6.462436717197839, - "y": 6.94820261304716, - "heading": 0.9445352081247251, - "angularVelocity": 1.2585260943351558e-7, - "velocityX": 2.7481322496649945, - "velocityY": -2.599693212071642, - "timestamp": 3.4408443470406294 - }, - { - "x": 6.607425485391203, - "y": 6.7683332434118935, - "heading": 0.944535213822349, - "angularVelocity": 9.329435992491954e-8, - "velocityX": 2.3740834397043513, - "velocityY": -2.945227393005009, - "timestamp": 3.501915818147567 - }, - { - "x": 6.726943413314801, - "y": 6.570620826362678, - "heading": 0.9445352185350564, - "angularVelocity": 7.716708691487089e-8, - "velocityX": 1.9570173398037076, - "velocityY": -3.237394047754587, - "timestamp": 3.5629872892545045 - }, - { - "x": 6.818825069558712, - "y": 6.358647957429321, - "heading": 0.9445352227854475, - "angularVelocity": 6.959699896596626e-8, - "velocityX": 1.5044939081788649, - "velocityY": -3.470898360417538, - "timestamp": 3.624058760361442 - }, - { - "x": 6.885266799619373, - "y": 6.137378364635986, - "heading": 0.944535226910566, - "angularVelocity": 6.754575173659361e-8, - "velocityX": 1.0879340034945393, - "velocityY": -3.623125311749637, - "timestamp": 3.6851302314683796 - }, - { - "x": 6.951706477654193, - "y": 5.916108155680916, - "heading": 0.9445352310356804, - "angularVelocity": 6.754568510636454e-8, - "velocityX": 1.087900403094797, - "velocityY": -3.6231354009406083, - "timestamp": 3.746201702575317 - }, - { - "x": 7.007755279541016, - "y": 5.691980361938477, - "heading": 0.9445352351916124, - "angularVelocity": 6.805030176915139e-8, - "velocityX": 0.917757520343414, - "velocityY": -3.669926230366784, - "timestamp": 3.8072731736822547 - }, - { - "x": 7.033287944264436, - "y": 5.499297549749691, - "heading": 0.9445352391429122, - "angularVelocity": 7.690359033060341e-8, - "velocityX": 0.49693866281360616, - "velocityY": -3.750158476346985, - "timestamp": 3.858653085599064 - }, - { - "x": 7.048377702483028, - "y": 5.30551704932481, - "heading": 0.9445352430586353, - "angularVelocity": 7.621117031703296e-8, - "velocityX": 0.29368984211229937, - "velocityY": -3.7715226281165486, - "timestamp": 3.9100329975158736 - }, - { - "x": 7.063467124411225, - "y": 5.111736522713165, - "heading": 0.9445352469743582, - "angularVelocity": 7.621116311616718e-8, - "velocityX": 0.2936832969396488, - "velocityY": -3.7715231377858505, - "timestamp": 3.961412909432683 - }, - { - "x": 7.0785565463316305, - "y": 4.917955996100912, - "heading": 0.9445352508900811, - "angularVelocity": 7.621116383171076e-8, - "velocityX": 0.29368329678799265, - "velocityY": -3.7715231377976592, - "timestamp": 4.0127928213494926 - }, - { - "x": 7.093645968252035, - "y": 4.72417546948866, - "heading": 0.9445352548058039, - "angularVelocity": 7.621116339702062e-8, - "velocityX": 0.29368329678798916, - "velocityY": -3.77152313779766, - "timestamp": 4.064172733266302 - }, - { - "x": 7.1087353901724395, - "y": 4.530394942876407, - "heading": 0.9445352587215268, - "angularVelocity": 7.621116320689e-8, - "velocityX": 0.2936832967879892, - "velocityY": -3.77152313779766, - "timestamp": 4.1155526451831115 - }, - { - "x": 7.1238248120928445, - "y": 4.336614416264154, - "heading": 0.9445352626372497, - "angularVelocity": 7.621116316535163e-8, - "velocityX": 0.29368329678798916, - "velocityY": -3.77152313779766, - "timestamp": 4.166932557099921 - }, - { - "x": 7.138914234013249, - "y": 4.142833889651901, - "heading": 0.9445352665529726, - "angularVelocity": 7.621116395669404e-8, - "velocityX": 0.2936832967879891, - "velocityY": -3.7715231377976606, - "timestamp": 4.21831246901673 - }, - { - "x": 7.1540036559336535, - "y": 3.9490533630396487, - "heading": 0.9445352704686956, - "angularVelocity": 7.621116465419702e-8, - "velocityX": 0.29368329678798905, - "velocityY": -3.7715231377976597, - "timestamp": 4.26969238093354 - }, - { - "x": 7.1690930778540585, - "y": 3.755272836427396, - "heading": 0.9445352743844184, - "angularVelocity": 7.621116300190572e-8, - "velocityX": 0.29368329678798905, - "velocityY": -3.7715231377976597, - "timestamp": 4.321072292850349 - }, - { - "x": 7.184182499774463, - "y": 3.561492309815143, - "heading": 0.9445352783001413, - "angularVelocity": 7.621116375934217e-8, - "velocityX": 0.29368329678798927, - "velocityY": -3.7715231377976597, - "timestamp": 4.372452204767159 - }, - { - "x": 7.199271921694868, - "y": 3.367711783202891, - "heading": 0.9445352822158641, - "angularVelocity": 7.621116358329164e-8, - "velocityX": 0.2936832967879893, - "velocityY": -3.7715231377976597, - "timestamp": 4.423832116683968 - }, - { - "x": 7.2143613436152725, - "y": 3.173931256590638, - "heading": 0.944535286131587, - "angularVelocity": 7.621116395825444e-8, - "velocityX": 0.2936832967879889, - "velocityY": -3.77152313779766, - "timestamp": 4.475212028600778 - }, - { - "x": 7.2294507655356774, - "y": 2.980150729978386, - "heading": 0.94453529004731, - "angularVelocity": 7.621116318952045e-8, - "velocityX": 0.29368329678798905, - "velocityY": -3.77152313779766, - "timestamp": 4.526591940517587 - }, - { - "x": 7.244540187456082, - "y": 2.786370203366133, - "heading": 0.9445352939630328, - "angularVelocity": 7.621116338095618e-8, - "velocityX": 0.29368329678798905, - "velocityY": -3.77152313779766, - "timestamp": 4.577971852434397 - }, - { - "x": 7.2596296093764865, - "y": 2.5925896767538807, - "heading": 0.9445352978787557, - "angularVelocity": 7.621116372521823e-8, - "velocityX": 0.2936832967879889, - "velocityY": -3.77152313779766, - "timestamp": 4.629351764351206 - }, - { - "x": 7.2747190312968915, - "y": 2.398809150141628, - "heading": 0.9445353017944785, - "angularVelocity": 7.621116368417063e-8, - "velocityX": 0.2936832967879889, - "velocityY": -3.77152313779766, - "timestamp": 4.680731676268016 - }, - { - "x": 7.289808453217296, - "y": 2.2050286235293757, - "heading": 0.9445353057102015, - "angularVelocity": 7.621116284744578e-8, - "velocityX": 0.29368329678798905, - "velocityY": -3.77152313779766, - "timestamp": 4.732111588184825 - }, - { - "x": 7.3048978751377005, - "y": 2.011248096917123, - "heading": 0.9445353096259244, - "angularVelocity": 7.621116384489279e-8, - "velocityX": 0.29368329678798877, - "velocityY": -3.77152313779766, - "timestamp": 4.7834915001016345 - }, - { - "x": 7.3199872970581055, - "y": 1.8174675703048704, - "heading": 0.9445353135416472, - "angularVelocity": 7.621116322392135e-8, - "velocityX": 0.2936832967879892, - "velocityY": -3.77152313779766, - "timestamp": 4.834871412018444 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/Test1.1.traj b/src/main/deploy/choreo/Test1.1.traj new file mode 100644 index 00000000..35cbe979 --- /dev/null +++ b/src/main/deploy/choreo/Test1.1.traj @@ -0,0 +1,634 @@ +{ + "samples": [ + { + "x": 1.4869295358657837, + "y": 5.564249038696289, + "heading": 3.141, + "angularVelocity": 8.029589971503427e-22, + "velocityX": 1.7351458241819956e-21, + "velocityY": 6.201477276822387e-22, + "timestamp": 0 + }, + { + "x": 1.506153170031305, + "y": 5.579314508687984, + "heading": 3.1569331073832556, + "angularVelocity": 0.2189381700102546, + "velocityX": 0.2641535755648618, + "velocityY": 0.20701589156378014, + "timestamp": 0.07277446131256805 + }, + { + "x": 1.5445753142190985, + "y": 5.609481391551554, + "heading": 3.1887853931444132, + "angularVelocity": 0.4376849403852184, + "velocityX": 0.5279619181620503, + "velocityY": 0.41452567727023953, + "timestamp": 0.1455489226251361 + }, + { + "x": 1.6021601427736685, + "y": 5.654796415146542, + "heading": 3.2365776272564357, + "angularVelocity": 0.6567171127073494, + "velocityX": 0.7912779774108067, + "velocityY": 0.6226775544288833, + "timestamp": 0.21832338393770417 + }, + { + "x": 1.6788531458253628, + "y": 5.715327275578862, + "heading": 3.3003664786278266, + "angularVelocity": 0.8765279772723583, + "velocityX": 1.0538450119513225, + "velocityY": 0.8317596494783837, + "timestamp": 0.2910978452502722 + }, + { + "x": 1.7745670450726352, + "y": 5.791182757948707, + "heading": 3.380219753358853, + "angularVelocity": 1.097270571169936, + "velocityX": 1.3152127480020601, + "velocityY": 1.0423365697486087, + "timestamp": 0.36387230656284025 + }, + { + "x": 1.8891522077377947, + "y": 5.8825573533612445, + "heading": 3.4761247222373646, + "angularVelocity": 1.317838251891654, + "velocityX": 1.5745243674565124, + "velocityY": 1.255586008669736, + "timestamp": 0.4366467678754083 + }, + { + "x": 2.0223089309237166, + "y": 5.989857408352276, + "heading": 3.5877133538349852, + "angularVelocity": 1.5333487817703626, + "velocityX": 1.829717744168665, + "velocityY": 1.4744190895508729, + "timestamp": 0.5094212291879764 + }, + { + "x": 2.1730917573654285, + "y": 6.114310741146903, + "heading": 3.713064984762661, + "angularVelocity": 1.7224673143135665, + "velocityX": 2.0719195130019052, + "velocityY": 1.7101237240368758, + "timestamp": 0.5821956905005444 + }, + { + "x": 2.304244374220996, + "y": 6.249503988496409, + "heading": 3.8044927424626787, + "angularVelocity": 1.2563165161379026, + "velocityX": 1.8021791503514413, + "velocityY": 1.8577017941616667, + "timestamp": 0.6549701518131125 + }, + { + "x": 2.414916720860036, + "y": 6.372175349075486, + "heading": 3.8745670686179303, + "angularVelocity": 0.9628972154706921, + "velocityX": 1.5207580330096784, + "velocityY": 1.6856374937933305, + "timestamp": 0.7277446131256805 + }, + { + "x": 2.505647288657887, + "y": 6.481209512574969, + "heading": 3.9250064522902046, + "angularVelocity": 0.6930918176863726, + "velocityX": 1.2467363709936818, + "velocityY": 1.4982476205653745, + "timestamp": 0.8005190744382485 + }, + { + "x": 2.576640808380743, + "y": 6.576219510667492, + "heading": 3.9564510372491593, + "angularVelocity": 0.432082689336569, + "velocityX": 0.975527931672858, + "velocityY": 1.305540382971075, + "timestamp": 0.8732935357508166 + }, + { + "x": 2.628005259573955, + "y": 6.657012046349283, + "heading": 3.969203722939461, + "angularVelocity": 0.17523572775796842, + "velocityX": 0.7058032483758355, + "velocityY": 1.1101770349736413, + "timestamp": 0.9460679970633846 + }, + { + "x": 2.6598078437532253, + "y": 6.723475709345355, + "heading": 3.963391945850161, + "angularVelocity": -0.07986011829532402, + "velocityX": 0.4370019867639682, + "velocityY": 0.9132827890076579, + "timestamp": 1.0188424583759528 + }, + { + "x": 2.6720941279928496, + "y": 6.775543377099927, + "heading": 3.9390315825966313, + "angularVelocity": -0.3347378024400806, + "velocityX": 0.16882686615644266, + "velocityY": 0.7154662063514367, + "timestamp": 1.0916169196885208 + }, + { + "x": 2.6648950576782227, + "y": 6.813176155090332, + "heading": 3.896073037200959, + "angularVelocity": -0.5902969890929726, + "velocityX": -0.09892303130498589, + "velocityY": 0.5171151707845827, + "timestamp": 1.1643913810010889 + }, + { + "x": 2.659637231596543, + "y": 6.825031823463049, + "heading": 3.877816032306183, + "angularVelocity": -0.6840416135896586, + "velocityX": -0.19699681615987222, + "velocityY": 0.44420049020077496, + "timestamp": 1.191081284849143 + }, + { + "x": 2.6517927610129908, + "y": 6.834893701518374, + "heading": 3.857106562184998, + "angularVelocity": -0.775928989444263, + "velocityX": -0.29391153404714393, + "velocityY": 0.36949844823228756, + "timestamp": 1.2177711886971971 + }, + { + "x": 2.641397527843252, + "y": 6.842708311525597, + "heading": 3.834005474054525, + "angularVelocity": -0.865536581247652, + "velocityX": -0.38948185159896215, + "velocityY": 0.2927927373478789, + "timestamp": 1.2444610925452513 + }, + { + "x": 2.628493660297315, + "y": 6.848415370045661, + "heading": 3.808587288042731, + "angularVelocity": -0.9523521012476956, + "velocityX": -0.4834737367133058, + "velocityY": 0.2138283656829084, + "timestamp": 1.2711509963933054 + }, + { + "x": 2.6131312860336138, + "y": 6.85194659507741, + "heading": 3.7809426101756807, + "angularVelocity": -1.0357728534516963, + "velocityX": -0.5755874712460319, + "velocityY": 0.13230564830254915, + "timestamp": 1.2978409002413596 + }, + { + "x": 2.5953708985643273, + "y": 6.853224371337149, + "heading": 3.7511803744930092, + "angularVelocity": -1.1151121357389708, + "velocityX": -0.6654346741148351, + "velocityY": 0.0478748918322195, + "timestamp": 1.3245308040894137 + }, + { + "x": 2.5752865454465885, + "y": 6.852160328589509, + "heading": 3.7194297145938124, + "angularVelocity": -1.1896131241218921, + "velocityX": -0.752507511157751, + "velocityY": -0.03986686327897337, + "timestamp": 1.3512207079374678 + }, + { + "x": 2.5529701073140734, + "y": 6.848653945190506, + "heading": 3.6858413880737997, + "angularVelocity": -1.2584656247257915, + "velocityX": -0.8361378242335784, + "velocityY": -0.1313748981249787, + "timestamp": 1.377910611785522 + }, + { + "x": 2.5285370169793717, + "y": 6.842591376045072, + "heading": 3.650589102058915, + "angularVelocity": -1.3208097794422922, + "velocityX": -0.9154431755842751, + "velocityY": -0.2271484071260898, + "timestamp": 1.4046005156335761 + }, + { + "x": 2.502133887928051, + "y": 6.833844846711091, + "heading": 3.613872024847483, + "angularVelocity": -1.375691625584805, + "velocityX": -0.9892553079859069, + "velocityY": -0.32770928601974636, + "timestamp": 1.4312904194816303 + }, + { + "x": 2.4739487064187284, + "y": 6.822273243464229, + "heading": 3.5759212160148097, + "angularVelocity": -1.4219162814796094, + "velocityX": -1.0560240932219476, + "velocityY": -0.43355732237695227, + "timestamp": 1.4579803233296844 + }, + { + "x": 2.4442244144939402, + "y": 6.807725217915028, + "heading": 3.5370136666138436, + "angularVelocity": -1.4577628163243674, + "velocityX": -1.113690483637882, + "velocityY": -0.54507598199017, + "timestamp": 1.4846702271777386 + }, + { + "x": 2.4132762941421024, + "y": 6.79004764808573, + "heading": 3.497494895227568, + "angularVelocity": -1.4806636850869153, + "velocityX": -1.1595440930782523, + "velocityY": -0.6623317165148433, + "timestamp": 1.5113601310257927 + }, + { + "x": 2.3815108616317744, + "y": 6.769104478274178, + "heading": 3.45780088591702, + "angularVelocity": -1.4872293859328474, + "velocityX": -1.1901666147307672, + "velocityY": -0.7846850978100616, + "timestamp": 1.5380500348738468 + }, + { + "x": 2.3494374003667597, + "y": 6.744810785756249, + "heading": 3.4184626098399313, + "angularVelocity": -1.4739010039542162, + "velocityX": -1.2017076362510986, + "velocityY": -0.910220308631834, + "timestamp": 1.564739938721901 + }, + { + "x": 2.317656105280672, + "y": 6.717179467626764, + "heading": 3.3800987305060795, + "angularVelocity": -1.4373929390026206, + "velocityX": -1.190760943427104, + "velocityY": -1.0352722994728694, + "timestamp": 1.5914298425699551 + }, + { + "x": 2.2868057964945567, + "y": 6.686361396326711, + "heading": 3.343389515038899, + "angularVelocity": -1.3753970668521929, + "velocityX": -1.1558793527974576, + "velocityY": -1.1546714995865741, + "timestamp": 1.6181197464180093 + }, + { + "x": 2.25748075399746, + "y": 6.652639268574606, + "heading": 3.308925014353095, + "angularVelocity": -1.291293549876024, + "velocityX": -1.0987316651286732, + "velocityY": -1.263478802474685, + "timestamp": 1.6448096502660634 + }, + { + "x": 2.2301787041350396, + "y": 6.616374519308364, + "heading": 3.2770382136187797, + "angularVelocity": -1.194713960599006, + "velocityX": -1.022935489683713, + "velocityY": -1.3587440956212415, + "timestamp": 1.6714995541141175 + }, + { + "x": 2.2052911587380817, + "y": 6.577965167741144, + "heading": 3.2479010535236887, + "angularVelocity": -1.0916922091952401, + "velocityX": -0.9324704029899484, + "velocityY": -1.4390966631383846, + "timestamp": 1.6981894579621717 + }, + { + "x": 2.183095873519221, + "y": 6.537812097974725, + "heading": 3.2217255041499464, + "angularVelocity": -0.9807285002883435, + "velocityX": -0.8315985454731583, + "velocityY": -1.504429165238335, + "timestamp": 1.7248793618102258 + }, + { + "x": 2.163764936777705, + "y": 6.4962772278935255, + "heading": 3.198795874429834, + "angularVelocity": -0.8591124887766933, + "velocityX": -0.72427899521733, + "velocityY": -1.5562015628702863, + "timestamp": 1.75156926565828 + }, + { + "x": 2.1473967460074728, + "y": 6.453660043976164, + "heading": 3.1793849811459, + "angularVelocity": -0.7272747550699427, + "velocityX": -0.61327275150245, + "velocityY": -1.596752995439064, + "timestamp": 1.7782591695063341 + }, + { + "x": 2.134047988560311, + "y": 6.410199882298709, + "heading": 3.163697789085614, + "angularVelocity": -0.5877575336948715, + "velocityX": -0.5001425828716374, + "velocityY": -1.628337139199698, + "timestamp": 1.8049490733543883 + }, + { + "x": 2.1237534679344243, + "y": 6.366088523528368, + "heading": 3.151864532890396, + "angularVelocity": -0.443360765276075, + "velocityX": -0.38570841935190137, + "velocityY": -1.6527357693556206, + "timestamp": 1.8316389772024424 + }, + { + "x": 2.116536197487218, + "y": 6.321482513089734, + "heading": 3.143955647543306, + "angularVelocity": -0.29632498461271933, + "velocityX": -0.27041200628876155, + "velocityY": -1.671269057115243, + "timestamp": 1.8583288810504965 + }, + { + "x": 2.1124122616124033, + "y": 6.276512434450225, + "heading": 3.14, + "angularVelocity": -0.1482076355848078, + "velocityX": -0.15451295359818015, + "velocityY": -1.6849097282449244, + "timestamp": 1.8850187848985507 + }, + { + "x": 2.1113932132720947, + "y": 6.231289386749268, + "heading": 3.14, + "angularVelocity": 1.1480104978612554e-22, + "velocityX": -0.038181042019109025, + "velocityY": -1.6943878089038094, + "timestamp": 1.9117086887466048 + }, + { + "x": 2.1139722457961088, + "y": 6.184041017313733, + "heading": 3.14, + "angularVelocity": 1.326030822415817e-23, + "velocityX": 0.09287464315336291, + "velocityY": -1.7014812376514836, + "timestamp": 1.9394776516890055 + }, + { + "x": 2.1201947185383916, + "y": 6.136700429098736, + "heading": 3.14, + "angularVelocity": 1.3260179507784046e-23, + "velocityX": 0.224080126981686, + "velocityY": -1.7048021675563763, + "timestamp": 1.9672466146314063 + }, + { + "x": 2.13006161842343, + "y": 6.089396035622716, + "heading": 3.14, + "angularVelocity": 1.326007041519161e-23, + "velocityX": 0.35532115136976367, + "velocityY": -1.7034987433322955, + "timestamp": 1.995015577573807 + }, + { + "x": 2.1435678181376194, + "y": 6.042288248235824, + "heading": 3.14, + "angularVelocity": 1.3260437690465338e-23, + "velocityX": 0.4863775338749469, + "velocityY": -1.6964186773774967, + "timestamp": 2.022784540516208 + }, + { + "x": 2.1606964661173613, + "y": 5.995581737791835, + "heading": 3.14, + "angularVelocity": 1.325999769616951e-23, + "velocityX": 0.6168270675167464, + "velocityY": -1.6819681217793025, + "timestamp": 2.050553503458609 + }, + { + "x": 2.181407907708738, + "y": 5.949543614413858, + "heading": 3.14, + "angularVelocity": 1.3260185229673763e-23, + "velocityX": 0.7458485804578618, + "velocityY": -1.6578985493073877, + "timestamp": 2.07832246640101 + }, + { + "x": 2.2056170475643233, + "y": 5.904529822011414, + "heading": 3.14, + "angularVelocity": 1.3260157963898354e-23, + "velocityX": 0.871805688451557, + "velocityY": -1.6210109284892367, + "timestamp": 2.1060914293434108 + }, + { + "x": 2.2331463169007804, + "y": 5.861019319967013, + "heading": 3.14, + "angularVelocity": 1.326020172336188e-23, + "velocityX": 0.9913682910506778, + "velocityY": -1.566875296519094, + "timestamp": 2.1338603922858117 + }, + { + "x": 2.2636325105277852, + "y": 5.819639483049468, + "heading": 3.14, + "angularVelocity": 1.32601511090322e-23, + "velocityX": 1.0978513562152263, + "velocityY": -1.4901470034504147, + "timestamp": 2.1616293552282126 + }, + { + "x": 2.2963834267173127, + "y": 5.781115195207252, + "heading": 3.14, + "angularVelocity": 1.3260107576996925e-23, + "velocityX": 1.1794072489296905, + "velocityY": -1.387314604514559, + "timestamp": 2.1893983181706136 + }, + { + "x": 2.3303148904385163, + "y": 5.746039013141612, + "heading": 3.14, + "angularVelocity": 1.3260240855176863e-23, + "velocityX": 1.221920450957617, + "velocityY": -1.2631433928013844, + "timestamp": 2.2171672811130145 + }, + { + "x": 2.3642062447518297, + "y": 5.714607213055508, + "heading": 3.14, + "angularVelocity": 1.326023555417506e-23, + "velocityX": 1.2204760539171806, + "velocityY": -1.1319039948052778, + "timestamp": 2.2449362440554155 + }, + { + "x": 2.397064742551735, + "y": 5.68667060145179, + "heading": 3.14, + "angularVelocity": 1.3260077501265654e-23, + "velocityX": 1.1832814163085987, + "velocityY": -1.0060372676381828, + "timestamp": 2.2727052069978164 + }, + { + "x": 2.4282178000518853, + "y": 5.661954954768778, + "heading": 3.14, + "angularVelocity": 1.3260249833501151e-23, + "velocityX": 1.1218660763374204, + "velocityY": -0.8900457224231765, + "timestamp": 2.3004741699402174 + }, + { + "x": 2.457231034537111, + "y": 5.640189613922927, + "heading": 3.14, + "angularVelocity": 1.3260141947431602e-23, + "velocityX": 1.0448079946451803, + "velocityY": -0.7838009972139435, + "timestamp": 2.3282431328826183 + }, + { + "x": 2.483818861573163, + "y": 5.621144668272307, + "heading": 3.14, + "angularVelocity": 1.3260165607649488e-23, + "velocityX": 0.9574656097601072, + "velocityY": -0.6858356824532662, + "timestamp": 2.3560120958250192 + }, + { + "x": 2.507786212179532, + "y": 5.604632755958765, + "heading": 3.14, + "angularVelocity": 1.3260126001429812e-23, + "velocityX": 0.8630985123961222, + "velocityY": -0.5946175356923091, + "timestamp": 2.38378105876742 + }, + { + "x": 2.5289943781295285, + "y": 5.590502031840209, + "heading": 3.14, + "angularVelocity": 1.3260018717887446e-23, + "velocityX": 0.763736333761809, + "velocityY": -0.5088675492802951, + "timestamp": 2.411550021709821 + }, + { + "x": 2.547341071809568, + "y": 5.578628548447832, + "heading": 3.14, + "angularVelocity": 1.3260377381509874e-23, + "velocityX": 0.6606906321310845, + "velocityY": -0.4275810881740915, + "timestamp": 2.439318984652222 + }, + { + "x": 2.562748457639991, + "y": 5.568909973472095, + "heading": 3.14, + "angularVelocity": 1.3260175762620676e-23, + "velocityX": 0.5548419601546171, + "velocityY": -0.3499797596293578, + "timestamp": 2.467087947594623 + }, + { + "x": 2.575155705150767, + "y": 5.561260778200535, + "heading": 3.14, + "angularVelocity": 1.3260285574981282e-23, + "velocityX": 0.4468026961075851, + "velocityY": -0.2754584421256751, + "timestamp": 2.494856910537024 + }, + { + "x": 2.5845141850851863, + "y": 5.555608625671519, + "heading": 3.14, + "angularVelocity": 1.326006595026007e-23, + "velocityX": 0.33701222310069334, + "velocityY": -0.2035420818825477, + "timestamp": 2.522625873479425 + }, + { + "x": 2.5907842648688533, + "y": 5.55189166019714, + "heading": 3.14, + "angularVelocity": 1.3260302513246253e-23, + "velocityX": 0.22579452450827125, + "velocityY": -0.1338532332694335, + "timestamp": 2.550394836421826 + }, + { + "x": 2.59393310546875, + "y": 5.550056457519531, + "heading": 3.14, + "angularVelocity": 1.3260089047889746e-23, + "velocityX": 0.11339424545409957, + "velocityY": -0.06608826845335893, + "timestamp": 2.578163799364227 + }, + { + "x": 2.59393310546875, + "y": 5.550056457519531, + "heading": 3.14, + "angularVelocity": 4.554749859713047e-24, + "velocityX": -4.596278509340535e-25, + "velocityY": -1.5195442763313795e-23, + "timestamp": 2.6059327623066277 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Test1.2.traj b/src/main/deploy/choreo/Test1.2.traj new file mode 100644 index 00000000..714f9e0e --- /dev/null +++ b/src/main/deploy/choreo/Test1.2.traj @@ -0,0 +1,445 @@ +{ + "samples": [ + { + "x": 2.59393310546875, + "y": 5.550056457519531, + "heading": 3.14, + "angularVelocity": 4.554749859713047e-24, + "velocityX": -4.596278509340535e-25, + "velocityY": -1.5195442763313795e-23, + "timestamp": 0 + }, + { + "x": 2.5899783566862378, + "y": 5.5489813066194555, + "heading": 3.1367490851985087, + "angularVelocity": -0.1085265960704612, + "velocityX": -0.13202296888338985, + "velocityY": -0.03589219483504766, + "timestamp": 0.029955005677878788 + }, + { + "x": 2.5820812048170954, + "y": 5.546779098674632, + "heading": 3.130275233897137, + "angularVelocity": -0.21611918124764368, + "velocityX": -0.26363379643671836, + "velocityY": -0.07351719337009978, + "timestamp": 0.059910011355757575 + }, + { + "x": 2.5702568313825034, + "y": 5.5433888083370855, + "heading": 3.1206149262957497, + "angularVelocity": -0.32249393324338926, + "velocityX": -0.3947378131636905, + "velocityY": -0.1131794256360211, + "timestamp": 0.08986501703363636 + }, + { + "x": 2.554524436678559, + "y": 5.5387376635070815, + "heading": 3.107814789143658, + "angularVelocity": -0.42731212571741595, + "velocityX": -0.5252008586852795, + "velocityY": -0.15527103817038457, + "timestamp": 0.11982002271151515 + }, + { + "x": 2.5349090755324424, + "y": 5.532737501107672, + "heading": 3.0919335422287366, + "angularVelocity": -0.5301700519005096, + "velocityX": -0.6548274888361012, + "velocityY": -0.20030583415450579, + "timestamp": 0.14977502838939394 + }, + { + "x": 2.511444606178007, + "y": 5.52527960958652, + "heading": 3.0730442108839475, + "angularVelocity": -0.6305901440285391, + "velocityX": -0.7833238159511905, + "velocityY": -0.24896979160511046, + "timestamp": 0.17973003406727273 + }, + { + "x": 2.4841786133457258, + "y": 5.5162272720643095, + "heading": 3.0512365155339536, + "angularVelocity": -0.7280150631418102, + "velocityX": -0.9102316028741883, + "velocityY": -0.3021978236143635, + "timestamp": 0.2096850397451515 + }, + { + "x": 2.4531809940870857, + "y": 5.505404760057258, + "heading": 3.026619340133933, + "angularVelocity": -0.8218050653951327, + "velocityX": -1.03480598842053, + "velocityY": -0.3612922702613114, + "timestamp": 0.2396400454230303 + }, + { + "x": 2.4185596883958564, + "y": 5.492580836854246, + "heading": 2.999323661656212, + "angularVelocity": -0.9112226107130541, + "velocityX": -1.1557769697502154, + "velocityY": -0.4281061850200961, + "timestamp": 0.2695950511009091 + }, + { + "x": 2.380491125275952, + "y": 5.477444069032361, + "heading": 2.9695096535475827, + "angularVelocity": -0.9952930214480606, + "velocityX": -1.27085815069692, + "velocityY": -0.5053168069690293, + "timestamp": 0.2995500567787879 + }, + { + "x": 2.3392828051052046, + "y": 5.459567907623698, + "heading": 2.9373981254397274, + "angularVelocity": -1.0719920554570026, + "velocityX": -1.3756739228789079, + "velocityY": -0.5967670846367005, + "timestamp": 0.32950506245666666 + }, + { + "x": 2.2955095509891357, + "y": 5.438376261665861, + "heading": 2.9034010779477137, + "angularVelocity": -1.1349371072601777, + "velocityX": -1.4613001441823985, + "velocityY": -0.7074492385586875, + "timestamp": 0.35946006813454545 + }, + { + "x": 2.2502877957375587, + "y": 5.413212847808716, + "heading": 2.8684201412995205, + "angularVelocity": -1.1677826746007212, + "velocityX": -1.5096560400578645, + "velocityY": -0.8400403634617765, + "timestamp": 0.38941507381242424 + }, + { + "x": 2.205434968136323, + "y": 5.383827174903616, + "heading": 2.8338778225381454, + "angularVelocity": -1.15314011730883, + "velocityX": -1.4973399799539564, + "velocityY": -0.9809937351072376, + "timestamp": 0.419370079490303 + }, + { + "x": 2.1627117665288242, + "y": 5.35082401983714, + "heading": 2.8010931886625263, + "angularVelocity": -1.0944626159703914, + "velocityX": -1.426245819043514, + "velocityY": -1.1017575967561364, + "timestamp": 0.4493250851681818 + }, + { + "x": 2.1231203758060415, + "y": 5.315102619215835, + "heading": 2.770927863402835, + "angularVelocity": -1.0070211831729985, + "velocityX": -1.3216953169206167, + "velocityY": -1.1925018811692434, + "timestamp": 0.4792800908460606 + }, + { + "x": 2.0871120973907478, + "y": 5.277368588566571, + "heading": 2.743899688375806, + "angularVelocity": -0.9022924354505801, + "velocityX": -1.2020788379247493, + "velocityY": -1.2596903187079023, + "timestamp": 0.5092350965239394 + }, + { + "x": 2.0548974336097707, + "y": 5.238102066889186, + "heading": 2.7202319811811844, + "angularVelocity": -0.7901085864957776, + "velocityX": -1.0754350751055555, + "velocityY": -1.310850082942305, + "timestamp": 0.5391901022018182 + }, + { + "x": 2.026587153392112, + "y": 5.197634903381892, + "heading": 2.6999923980627787, + "angularVelocity": -0.6756661419481035, + "velocityX": -0.9450934685873016, + "velocityY": -1.350931591950202, + "timestamp": 0.569145107879697 + }, + { + "x": 2.002245678733676, + "y": 5.1562068665429, + "heading": 2.6831832138201195, + "angularVelocity": -0.5611477568529534, + "velocityX": -0.812601236674494, + "velocityY": -1.3830088127670148, + "timestamp": 0.5991001135575758 + }, + { + "x": 1.9819131544340292, + "y": 5.113998389763002, + "heading": 2.66978325115483, + "angularVelocity": -0.44733634202529504, + "velocityX": -0.6787688347748394, + "velocityY": -1.4090625531434064, + "timestamp": 0.6290551192354545 + }, + { + "x": 1.9656157831702816, + "y": 5.071149791653941, + "heading": 2.659765787935313, + "angularVelocity": -0.33441700286222414, + "velocityX": -0.5440616983685925, + "velocityY": -1.4304319808793762, + "timestamp": 0.6590101249133333 + }, + { + "x": 1.9533711923999129, + "y": 5.027773084636261, + "heading": 2.6531059130951715, + "angularVelocity": -0.22232927984587103, + "velocityX": -0.4087660974610013, + "velocityY": -1.4480620529380386, + "timestamp": 0.6889651305912121 + }, + { + "x": 1.9451914498185667, + "y": 4.983959581247479, + "heading": 2.649783348569472, + "angularVelocity": -0.11091850762536054, + "velocityX": -0.27306763581709725, + "velocityY": -1.4626438018383596, + "timestamp": 0.7189201362690909 + }, + { + "x": 1.941084861755371, + "y": 4.939785003662109, + "heading": 2.649783348569472, + "angularVelocity": 6.132292113202646e-25, + "velocityX": -0.13709188064779135, + "velocityY": -1.4746976869375825, + "timestamp": 0.7488751419469697 + }, + { + "x": 1.9419022015392158, + "y": 4.889062683793002, + "heading": 2.649783348569472, + "angularVelocity": 4.614715107153485e-26, + "velocityX": 0.023936675164732843, + "velocityY": -1.4854577230996109, + "timestamp": 0.7830210614708646 + }, + { + "x": 1.948223657679148, + "y": 4.838070468392074, + "heading": 2.649783348569472, + "angularVelocity": 4.653945144562686e-26, + "velocityX": 0.18513064600613907, + "velocityY": -1.4933619042018718, + "timestamp": 0.8171669809947595 + }, + { + "x": 1.9600538072506943, + "y": 4.786928471515363, + "heading": 2.649783348569472, + "angularVelocity": 4.629261284078221e-26, + "velocityX": 0.3464586614300294, + "velocityY": -1.4977484159101515, + "timestamp": 0.8513129005186544 + }, + { + "x": 1.977394685635232, + "y": 4.735788039854744, + "heading": 2.649783348569472, + "angularVelocity": 4.640038416252255e-26, + "velocityX": 0.5078462851879747, + "velocityY": -1.4977025768725385, + "timestamp": 0.8854588200425493 + }, + { + "x": 2.0002427480647933, + "y": 4.684845229382865, + "heading": 2.649783348569472, + "angularVelocity": 4.6490332666751425e-26, + "velocityX": 0.6691300966012094, + "velocityY": -1.4919150276866968, + "timestamp": 0.9196047395664442 + }, + { + "x": 2.0285822715254445, + "y": 4.634362832128866, + "heading": 2.649783348569472, + "angularVelocity": 4.622795940866032e-26, + "velocityX": 0.8299534426308102, + "velocityY": -1.478431331119124, + "timestamp": 0.9537506590903391 + }, + { + "x": 2.0623699922411927, + "y": 4.5847082412928, + "heading": 2.649783348569472, + "angularVelocity": 4.634016030607817e-26, + "velocityX": 0.9895097624213414, + "velocityY": -1.454188129311252, + "timestamp": 0.987896578614234 + }, + { + "x": 2.101495965027088, + "y": 4.536421557771227, + "heading": 2.649783348569472, + "angularVelocity": 4.645177261974479e-26, + "velocityX": 1.1458462191511727, + "velocityY": -1.4141274915083855, + "timestamp": 1.0220424981381289 + }, + { + "x": 2.145673252803998, + "y": 4.4903376692314145, + "heading": 2.649783348569472, + "angularVelocity": 4.6233288536561975e-26, + "velocityX": 1.2937794147261192, + "velocityY": -1.3496162698902718, + "timestamp": 1.0561884176620238 + }, + { + "x": 2.1941116699379357, + "y": 4.447748182132556, + "heading": 2.649783348569472, + "angularVelocity": 4.6479494614855375e-26, + "velocityX": 1.4185711736373225, + "velocityY": -1.2472789631292271, + "timestamp": 1.0903343371859187 + }, + { + "x": 2.2448201323304646, + "y": 4.410180142680831, + "heading": 2.649783348569472, + "angularVelocity": 4.6329904581670297e-26, + "velocityX": 1.485051891985029, + "velocityY": -1.1002204648621148, + "timestamp": 1.1244802567098136 + }, + { + "x": 2.294852293597042, + "y": 4.3780811675391975, + "heading": 2.649783348569472, + "angularVelocity": 4.6446166642422895e-26, + "velocityX": 1.4652456856979683, + "velocityY": -0.9400530309096446, + "timestamp": 1.1586261762337084 + }, + { + "x": 2.342149827765447, + "y": 4.350766530180341, + "heading": 2.649783348569472, + "angularVelocity": 4.633506801188826e-26, + "velocityX": 1.3851591882100787, + "velocityY": -0.7999385501902068, + "timestamp": 1.1927720957576033 + }, + { + "x": 2.3856944951859207, + "y": 4.3274872473595005, + "heading": 2.649783348569472, + "angularVelocity": 4.6381778689211406e-26, + "velocityX": 1.2752524467821442, + "velocityY": -0.6817588498253565, + "timestamp": 1.2269180152814982 + }, + { + "x": 2.4249725857215654, + "y": 4.307695719032298, + "heading": 2.649783348569472, + "angularVelocity": 4.6432377599415885e-26, + "velocityX": 1.150301151156826, + "velocityY": -0.5796162060697082, + "timestamp": 1.2610639348053931 + }, + { + "x": 2.4596964858202144, + "y": 4.291007007021927, + "heading": 2.649783348569472, + "angularVelocity": 4.6331179794053255e-26, + "velocityX": 1.0169267831358226, + "velocityY": -0.4887468910800007, + "timestamp": 1.295209854329288 + }, + { + "x": 2.4896890256889828, + "y": 4.277143601542489, + "heading": 2.649783348569472, + "angularVelocity": 4.6467164999430504e-26, + "velocityX": 0.8783638070657234, + "velocityY": -0.4060047488173824, + "timestamp": 1.329355773853183 + }, + { + "x": 2.514832571254234, + "y": 4.265898475268687, + "heading": 2.649783348569472, + "angularVelocity": 4.632856845272505e-26, + "velocityX": 0.7363557905551701, + "velocityY": -0.3293256245723021, + "timestamp": 1.3635016933770778 + }, + { + "x": 2.5350443768209585, + "y": 4.257112226026017, + "heading": 2.649783348569472, + "angularVelocity": 4.629729705599566e-26, + "velocityX": 0.5919244773180152, + "velocityY": -0.2573147645510306, + "timestamp": 1.3976476129009727 + }, + { + "x": 2.550263563230007, + "y": 4.2506587632470065, + "heading": 2.649783348569472, + "angularVelocity": 4.62810585566047e-26, + "velocityX": 0.445710252388941, + "velocityY": -0.18899660249284694, + "timestamp": 1.4317935324248676 + }, + { + "x": 2.5604437164785643, + "y": 4.246436059535304, + "heading": 2.649783348569472, + "angularVelocity": 4.6519462435401186e-26, + "velocityX": 0.29813674343821717, + "velocityY": -0.12366642253542871, + "timestamp": 1.4659394519487625 + }, + { + "x": 2.5655484199523926, + "y": 4.244359970092773, + "heading": 2.649783348569472, + "angularVelocity": 4.6699948971014915e-26, + "velocityX": 0.14949673474910496, + "velocityY": -0.06080051354533503, + "timestamp": 1.5000853714726574 + }, + { + "x": 2.5655484199523926, + "y": 4.244359970092773, + "heading": 2.649783348569472, + "angularVelocity": 1.535197914331468e-26, + "velocityX": 1.1963425727073795e-24, + "velocityY": -7.168329076335369e-24, + "timestamp": 1.5342312909965523 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Test1.traj b/src/main/deploy/choreo/Test1.traj new file mode 100644 index 00000000..e709ba2f --- /dev/null +++ b/src/main/deploy/choreo/Test1.traj @@ -0,0 +1,1066 @@ +{ + "samples": [ + { + "x": 1.4869295358657837, + "y": 5.564249038696289, + "heading": 3.141, + "angularVelocity": 8.029589971503427e-22, + "velocityX": 1.7351458241819956e-21, + "velocityY": 6.201477276822387e-22, + "timestamp": 0 + }, + { + "x": 1.506153170031305, + "y": 5.579314508687984, + "heading": 3.1569331073832556, + "angularVelocity": 0.2189381700102546, + "velocityX": 0.2641535755648618, + "velocityY": 0.20701589156378014, + "timestamp": 0.07277446131256805 + }, + { + "x": 1.5445753142190985, + "y": 5.609481391551554, + "heading": 3.1887853931444132, + "angularVelocity": 0.4376849403852184, + "velocityX": 0.5279619181620503, + "velocityY": 0.41452567727023953, + "timestamp": 0.1455489226251361 + }, + { + "x": 1.6021601427736685, + "y": 5.654796415146542, + "heading": 3.2365776272564357, + "angularVelocity": 0.6567171127073494, + "velocityX": 0.7912779774108067, + "velocityY": 0.6226775544288833, + "timestamp": 0.21832338393770417 + }, + { + "x": 1.6788531458253628, + "y": 5.715327275578862, + "heading": 3.3003664786278266, + "angularVelocity": 0.8765279772723583, + "velocityX": 1.0538450119513225, + "velocityY": 0.8317596494783837, + "timestamp": 0.2910978452502722 + }, + { + "x": 1.7745670450726352, + "y": 5.791182757948707, + "heading": 3.380219753358853, + "angularVelocity": 1.097270571169936, + "velocityX": 1.3152127480020601, + "velocityY": 1.0423365697486087, + "timestamp": 0.36387230656284025 + }, + { + "x": 1.8891522077377947, + "y": 5.8825573533612445, + "heading": 3.4761247222373646, + "angularVelocity": 1.317838251891654, + "velocityX": 1.5745243674565124, + "velocityY": 1.255586008669736, + "timestamp": 0.4366467678754083 + }, + { + "x": 2.0223089309237166, + "y": 5.989857408352276, + "heading": 3.5877133538349852, + "angularVelocity": 1.5333487817703626, + "velocityX": 1.829717744168665, + "velocityY": 1.4744190895508729, + "timestamp": 0.5094212291879764 + }, + { + "x": 2.1730917573654285, + "y": 6.114310741146903, + "heading": 3.713064984762661, + "angularVelocity": 1.7224673143135665, + "velocityX": 2.0719195130019052, + "velocityY": 1.7101237240368758, + "timestamp": 0.5821956905005444 + }, + { + "x": 2.304244374220996, + "y": 6.249503988496409, + "heading": 3.8044927424626787, + "angularVelocity": 1.2563165161379026, + "velocityX": 1.8021791503514413, + "velocityY": 1.8577017941616667, + "timestamp": 0.6549701518131125 + }, + { + "x": 2.414916720860036, + "y": 6.372175349075486, + "heading": 3.8745670686179303, + "angularVelocity": 0.9628972154706921, + "velocityX": 1.5207580330096784, + "velocityY": 1.6856374937933305, + "timestamp": 0.7277446131256805 + }, + { + "x": 2.505647288657887, + "y": 6.481209512574969, + "heading": 3.9250064522902046, + "angularVelocity": 0.6930918176863726, + "velocityX": 1.2467363709936818, + "velocityY": 1.4982476205653745, + "timestamp": 0.8005190744382485 + }, + { + "x": 2.576640808380743, + "y": 6.576219510667492, + "heading": 3.9564510372491593, + "angularVelocity": 0.432082689336569, + "velocityX": 0.975527931672858, + "velocityY": 1.305540382971075, + "timestamp": 0.8732935357508166 + }, + { + "x": 2.628005259573955, + "y": 6.657012046349283, + "heading": 3.969203722939461, + "angularVelocity": 0.17523572775796842, + "velocityX": 0.7058032483758355, + "velocityY": 1.1101770349736413, + "timestamp": 0.9460679970633846 + }, + { + "x": 2.6598078437532253, + "y": 6.723475709345355, + "heading": 3.963391945850161, + "angularVelocity": -0.07986011829532402, + "velocityX": 0.4370019867639682, + "velocityY": 0.9132827890076579, + "timestamp": 1.0188424583759528 + }, + { + "x": 2.6720941279928496, + "y": 6.775543377099927, + "heading": 3.9390315825966313, + "angularVelocity": -0.3347378024400806, + "velocityX": 0.16882686615644266, + "velocityY": 0.7154662063514367, + "timestamp": 1.0916169196885208 + }, + { + "x": 2.6648950576782227, + "y": 6.813176155090332, + "heading": 3.896073037200959, + "angularVelocity": -0.5902969890929726, + "velocityX": -0.09892303130498589, + "velocityY": 0.5171151707845827, + "timestamp": 1.1643913810010889 + }, + { + "x": 2.659637231596543, + "y": 6.825031823463049, + "heading": 3.877816032306183, + "angularVelocity": -0.6840416135896586, + "velocityX": -0.19699681615987222, + "velocityY": 0.44420049020077496, + "timestamp": 1.191081284849143 + }, + { + "x": 2.6517927610129908, + "y": 6.834893701518374, + "heading": 3.857106562184998, + "angularVelocity": -0.775928989444263, + "velocityX": -0.29391153404714393, + "velocityY": 0.36949844823228756, + "timestamp": 1.2177711886971971 + }, + { + "x": 2.641397527843252, + "y": 6.842708311525597, + "heading": 3.834005474054525, + "angularVelocity": -0.865536581247652, + "velocityX": -0.38948185159896215, + "velocityY": 0.2927927373478789, + "timestamp": 1.2444610925452513 + }, + { + "x": 2.628493660297315, + "y": 6.848415370045661, + "heading": 3.808587288042731, + "angularVelocity": -0.9523521012476956, + "velocityX": -0.4834737367133058, + "velocityY": 0.2138283656829084, + "timestamp": 1.2711509963933054 + }, + { + "x": 2.6131312860336138, + "y": 6.85194659507741, + "heading": 3.7809426101756807, + "angularVelocity": -1.0357728534516963, + "velocityX": -0.5755874712460319, + "velocityY": 0.13230564830254915, + "timestamp": 1.2978409002413596 + }, + { + "x": 2.5953708985643273, + "y": 6.853224371337149, + "heading": 3.7511803744930092, + "angularVelocity": -1.1151121357389708, + "velocityX": -0.6654346741148351, + "velocityY": 0.0478748918322195, + "timestamp": 1.3245308040894137 + }, + { + "x": 2.5752865454465885, + "y": 6.852160328589509, + "heading": 3.7194297145938124, + "angularVelocity": -1.1896131241218921, + "velocityX": -0.752507511157751, + "velocityY": -0.03986686327897337, + "timestamp": 1.3512207079374678 + }, + { + "x": 2.5529701073140734, + "y": 6.848653945190506, + "heading": 3.6858413880737997, + "angularVelocity": -1.2584656247257915, + "velocityX": -0.8361378242335784, + "velocityY": -0.1313748981249787, + "timestamp": 1.377910611785522 + }, + { + "x": 2.5285370169793717, + "y": 6.842591376045072, + "heading": 3.650589102058915, + "angularVelocity": -1.3208097794422922, + "velocityX": -0.9154431755842751, + "velocityY": -0.2271484071260898, + "timestamp": 1.4046005156335761 + }, + { + "x": 2.502133887928051, + "y": 6.833844846711091, + "heading": 3.613872024847483, + "angularVelocity": -1.375691625584805, + "velocityX": -0.9892553079859069, + "velocityY": -0.32770928601974636, + "timestamp": 1.4312904194816303 + }, + { + "x": 2.4739487064187284, + "y": 6.822273243464229, + "heading": 3.5759212160148097, + "angularVelocity": -1.4219162814796094, + "velocityX": -1.0560240932219476, + "velocityY": -0.43355732237695227, + "timestamp": 1.4579803233296844 + }, + { + "x": 2.4442244144939402, + "y": 6.807725217915028, + "heading": 3.5370136666138436, + "angularVelocity": -1.4577628163243674, + "velocityX": -1.113690483637882, + "velocityY": -0.54507598199017, + "timestamp": 1.4846702271777386 + }, + { + "x": 2.4132762941421024, + "y": 6.79004764808573, + "heading": 3.497494895227568, + "angularVelocity": -1.4806636850869153, + "velocityX": -1.1595440930782523, + "velocityY": -0.6623317165148433, + "timestamp": 1.5113601310257927 + }, + { + "x": 2.3815108616317744, + "y": 6.769104478274178, + "heading": 3.45780088591702, + "angularVelocity": -1.4872293859328474, + "velocityX": -1.1901666147307672, + "velocityY": -0.7846850978100616, + "timestamp": 1.5380500348738468 + }, + { + "x": 2.3494374003667597, + "y": 6.744810785756249, + "heading": 3.4184626098399313, + "angularVelocity": -1.4739010039542162, + "velocityX": -1.2017076362510986, + "velocityY": -0.910220308631834, + "timestamp": 1.564739938721901 + }, + { + "x": 2.317656105280672, + "y": 6.717179467626764, + "heading": 3.3800987305060795, + "angularVelocity": -1.4373929390026206, + "velocityX": -1.190760943427104, + "velocityY": -1.0352722994728694, + "timestamp": 1.5914298425699551 + }, + { + "x": 2.2868057964945567, + "y": 6.686361396326711, + "heading": 3.343389515038899, + "angularVelocity": -1.3753970668521929, + "velocityX": -1.1558793527974576, + "velocityY": -1.1546714995865741, + "timestamp": 1.6181197464180093 + }, + { + "x": 2.25748075399746, + "y": 6.652639268574606, + "heading": 3.308925014353095, + "angularVelocity": -1.291293549876024, + "velocityX": -1.0987316651286732, + "velocityY": -1.263478802474685, + "timestamp": 1.6448096502660634 + }, + { + "x": 2.2301787041350396, + "y": 6.616374519308364, + "heading": 3.2770382136187797, + "angularVelocity": -1.194713960599006, + "velocityX": -1.022935489683713, + "velocityY": -1.3587440956212415, + "timestamp": 1.6714995541141175 + }, + { + "x": 2.2052911587380817, + "y": 6.577965167741144, + "heading": 3.2479010535236887, + "angularVelocity": -1.0916922091952401, + "velocityX": -0.9324704029899484, + "velocityY": -1.4390966631383846, + "timestamp": 1.6981894579621717 + }, + { + "x": 2.183095873519221, + "y": 6.537812097974725, + "heading": 3.2217255041499464, + "angularVelocity": -0.9807285002883435, + "velocityX": -0.8315985454731583, + "velocityY": -1.504429165238335, + "timestamp": 1.7248793618102258 + }, + { + "x": 2.163764936777705, + "y": 6.4962772278935255, + "heading": 3.198795874429834, + "angularVelocity": -0.8591124887766933, + "velocityX": -0.72427899521733, + "velocityY": -1.5562015628702863, + "timestamp": 1.75156926565828 + }, + { + "x": 2.1473967460074728, + "y": 6.453660043976164, + "heading": 3.1793849811459, + "angularVelocity": -0.7272747550699427, + "velocityX": -0.61327275150245, + "velocityY": -1.596752995439064, + "timestamp": 1.7782591695063341 + }, + { + "x": 2.134047988560311, + "y": 6.410199882298709, + "heading": 3.163697789085614, + "angularVelocity": -0.5877575336948715, + "velocityX": -0.5001425828716374, + "velocityY": -1.628337139199698, + "timestamp": 1.8049490733543883 + }, + { + "x": 2.1237534679344243, + "y": 6.366088523528368, + "heading": 3.151864532890396, + "angularVelocity": -0.443360765276075, + "velocityX": -0.38570841935190137, + "velocityY": -1.6527357693556206, + "timestamp": 1.8316389772024424 + }, + { + "x": 2.116536197487218, + "y": 6.321482513089734, + "heading": 3.143955647543306, + "angularVelocity": -0.29632498461271933, + "velocityX": -0.27041200628876155, + "velocityY": -1.671269057115243, + "timestamp": 1.8583288810504965 + }, + { + "x": 2.1124122616124033, + "y": 6.276512434450225, + "heading": 3.14, + "angularVelocity": -0.1482076355848078, + "velocityX": -0.15451295359818015, + "velocityY": -1.6849097282449244, + "timestamp": 1.8850187848985507 + }, + { + "x": 2.1113932132720947, + "y": 6.231289386749268, + "heading": 3.14, + "angularVelocity": 1.1480104978612554e-22, + "velocityX": -0.038181042019109025, + "velocityY": -1.6943878089038094, + "timestamp": 1.9117086887466048 + }, + { + "x": 2.1139722457961088, + "y": 6.184041017313733, + "heading": 3.14, + "angularVelocity": 1.326030822415817e-23, + "velocityX": 0.09287464315336291, + "velocityY": -1.7014812376514836, + "timestamp": 1.9394776516890055 + }, + { + "x": 2.1201947185383916, + "y": 6.136700429098736, + "heading": 3.14, + "angularVelocity": 1.3260179507784046e-23, + "velocityX": 0.224080126981686, + "velocityY": -1.7048021675563763, + "timestamp": 1.9672466146314063 + }, + { + "x": 2.13006161842343, + "y": 6.089396035622716, + "heading": 3.14, + "angularVelocity": 1.326007041519161e-23, + "velocityX": 0.35532115136976367, + "velocityY": -1.7034987433322955, + "timestamp": 1.995015577573807 + }, + { + "x": 2.1435678181376194, + "y": 6.042288248235824, + "heading": 3.14, + "angularVelocity": 1.3260437690465338e-23, + "velocityX": 0.4863775338749469, + "velocityY": -1.6964186773774967, + "timestamp": 2.022784540516208 + }, + { + "x": 2.1606964661173613, + "y": 5.995581737791835, + "heading": 3.14, + "angularVelocity": 1.325999769616951e-23, + "velocityX": 0.6168270675167464, + "velocityY": -1.6819681217793025, + "timestamp": 2.050553503458609 + }, + { + "x": 2.181407907708738, + "y": 5.949543614413858, + "heading": 3.14, + "angularVelocity": 1.3260185229673763e-23, + "velocityX": 0.7458485804578618, + "velocityY": -1.6578985493073877, + "timestamp": 2.07832246640101 + }, + { + "x": 2.2056170475643233, + "y": 5.904529822011414, + "heading": 3.14, + "angularVelocity": 1.3260157963898354e-23, + "velocityX": 0.871805688451557, + "velocityY": -1.6210109284892367, + "timestamp": 2.1060914293434108 + }, + { + "x": 2.2331463169007804, + "y": 5.861019319967013, + "heading": 3.14, + "angularVelocity": 1.326020172336188e-23, + "velocityX": 0.9913682910506778, + "velocityY": -1.566875296519094, + "timestamp": 2.1338603922858117 + }, + { + "x": 2.2636325105277852, + "y": 5.819639483049468, + "heading": 3.14, + "angularVelocity": 1.32601511090322e-23, + "velocityX": 1.0978513562152263, + "velocityY": -1.4901470034504147, + "timestamp": 2.1616293552282126 + }, + { + "x": 2.2963834267173127, + "y": 5.781115195207252, + "heading": 3.14, + "angularVelocity": 1.3260107576996925e-23, + "velocityX": 1.1794072489296905, + "velocityY": -1.387314604514559, + "timestamp": 2.1893983181706136 + }, + { + "x": 2.3303148904385163, + "y": 5.746039013141612, + "heading": 3.14, + "angularVelocity": 1.3260240855176863e-23, + "velocityX": 1.221920450957617, + "velocityY": -1.2631433928013844, + "timestamp": 2.2171672811130145 + }, + { + "x": 2.3642062447518297, + "y": 5.714607213055508, + "heading": 3.14, + "angularVelocity": 1.326023555417506e-23, + "velocityX": 1.2204760539171806, + "velocityY": -1.1319039948052778, + "timestamp": 2.2449362440554155 + }, + { + "x": 2.397064742551735, + "y": 5.68667060145179, + "heading": 3.14, + "angularVelocity": 1.3260077501265654e-23, + "velocityX": 1.1832814163085987, + "velocityY": -1.0060372676381828, + "timestamp": 2.2727052069978164 + }, + { + "x": 2.4282178000518853, + "y": 5.661954954768778, + "heading": 3.14, + "angularVelocity": 1.3260249833501151e-23, + "velocityX": 1.1218660763374204, + "velocityY": -0.8900457224231765, + "timestamp": 2.3004741699402174 + }, + { + "x": 2.457231034537111, + "y": 5.640189613922927, + "heading": 3.14, + "angularVelocity": 1.3260141947431602e-23, + "velocityX": 1.0448079946451803, + "velocityY": -0.7838009972139435, + "timestamp": 2.3282431328826183 + }, + { + "x": 2.483818861573163, + "y": 5.621144668272307, + "heading": 3.14, + "angularVelocity": 1.3260165607649488e-23, + "velocityX": 0.9574656097601072, + "velocityY": -0.6858356824532662, + "timestamp": 2.3560120958250192 + }, + { + "x": 2.507786212179532, + "y": 5.604632755958765, + "heading": 3.14, + "angularVelocity": 1.3260126001429812e-23, + "velocityX": 0.8630985123961222, + "velocityY": -0.5946175356923091, + "timestamp": 2.38378105876742 + }, + { + "x": 2.5289943781295285, + "y": 5.590502031840209, + "heading": 3.14, + "angularVelocity": 1.3260018717887446e-23, + "velocityX": 0.763736333761809, + "velocityY": -0.5088675492802951, + "timestamp": 2.411550021709821 + }, + { + "x": 2.547341071809568, + "y": 5.578628548447832, + "heading": 3.14, + "angularVelocity": 1.3260377381509874e-23, + "velocityX": 0.6606906321310845, + "velocityY": -0.4275810881740915, + "timestamp": 2.439318984652222 + }, + { + "x": 2.562748457639991, + "y": 5.568909973472095, + "heading": 3.14, + "angularVelocity": 1.3260175762620676e-23, + "velocityX": 0.5548419601546171, + "velocityY": -0.3499797596293578, + "timestamp": 2.467087947594623 + }, + { + "x": 2.575155705150767, + "y": 5.561260778200535, + "heading": 3.14, + "angularVelocity": 1.3260285574981282e-23, + "velocityX": 0.4468026961075851, + "velocityY": -0.2754584421256751, + "timestamp": 2.494856910537024 + }, + { + "x": 2.5845141850851863, + "y": 5.555608625671519, + "heading": 3.14, + "angularVelocity": 1.326006595026007e-23, + "velocityX": 0.33701222310069334, + "velocityY": -0.2035420818825477, + "timestamp": 2.522625873479425 + }, + { + "x": 2.5907842648688533, + "y": 5.55189166019714, + "heading": 3.14, + "angularVelocity": 1.3260302513246253e-23, + "velocityX": 0.22579452450827125, + "velocityY": -0.1338532332694335, + "timestamp": 2.550394836421826 + }, + { + "x": 2.59393310546875, + "y": 5.550056457519531, + "heading": 3.14, + "angularVelocity": 1.3260089047889746e-23, + "velocityX": 0.11339424545409957, + "velocityY": -0.06608826845335893, + "timestamp": 2.578163799364227 + }, + { + "x": 2.59393310546875, + "y": 5.550056457519531, + "heading": 3.14, + "angularVelocity": 4.554749859713047e-24, + "velocityX": -4.596278509340535e-25, + "velocityY": -1.5195442763313795e-23, + "timestamp": 2.6059327623066277 + }, + { + "x": 2.5899783566862378, + "y": 5.5489813066194555, + "heading": 3.1367490851985087, + "angularVelocity": -0.1085265960704612, + "velocityX": -0.13202296888338985, + "velocityY": -0.03589219483504766, + "timestamp": 2.6358877679845065 + }, + { + "x": 2.5820812048170954, + "y": 5.546779098674632, + "heading": 3.130275233897137, + "angularVelocity": -0.21611918124764368, + "velocityX": -0.26363379643671836, + "velocityY": -0.07351719337009978, + "timestamp": 2.6658427736623853 + }, + { + "x": 2.5702568313825034, + "y": 5.5433888083370855, + "heading": 3.1206149262957497, + "angularVelocity": -0.32249393324338926, + "velocityX": -0.3947378131636905, + "velocityY": -0.1131794256360211, + "timestamp": 2.695797779340264 + }, + { + "x": 2.554524436678559, + "y": 5.5387376635070815, + "heading": 3.107814789143658, + "angularVelocity": -0.42731212571741595, + "velocityX": -0.5252008586852795, + "velocityY": -0.15527103817038457, + "timestamp": 2.725752785018143 + }, + { + "x": 2.5349090755324424, + "y": 5.532737501107672, + "heading": 3.0919335422287366, + "angularVelocity": -0.5301700519005096, + "velocityX": -0.6548274888361012, + "velocityY": -0.20030583415450579, + "timestamp": 2.7557077906960217 + }, + { + "x": 2.511444606178007, + "y": 5.52527960958652, + "heading": 3.0730442108839475, + "angularVelocity": -0.6305901440285391, + "velocityX": -0.7833238159511905, + "velocityY": -0.24896979160511046, + "timestamp": 2.7856627963739005 + }, + { + "x": 2.4841786133457258, + "y": 5.5162272720643095, + "heading": 3.0512365155339536, + "angularVelocity": -0.7280150631418102, + "velocityX": -0.9102316028741883, + "velocityY": -0.3021978236143635, + "timestamp": 2.8156178020517793 + }, + { + "x": 2.4531809940870857, + "y": 5.505404760057258, + "heading": 3.026619340133933, + "angularVelocity": -0.8218050653951327, + "velocityX": -1.03480598842053, + "velocityY": -0.3612922702613114, + "timestamp": 2.845572807729658 + }, + { + "x": 2.4185596883958564, + "y": 5.492580836854246, + "heading": 2.999323661656212, + "angularVelocity": -0.9112226107130541, + "velocityX": -1.1557769697502154, + "velocityY": -0.4281061850200961, + "timestamp": 2.875527813407537 + }, + { + "x": 2.380491125275952, + "y": 5.477444069032361, + "heading": 2.9695096535475827, + "angularVelocity": -0.9952930214480606, + "velocityX": -1.27085815069692, + "velocityY": -0.5053168069690293, + "timestamp": 2.9054828190854156 + }, + { + "x": 2.3392828051052046, + "y": 5.459567907623698, + "heading": 2.9373981254397274, + "angularVelocity": -1.0719920554570026, + "velocityX": -1.3756739228789079, + "velocityY": -0.5967670846367005, + "timestamp": 2.9354378247632944 + }, + { + "x": 2.2955095509891357, + "y": 5.438376261665861, + "heading": 2.9034010779477137, + "angularVelocity": -1.1349371072601777, + "velocityX": -1.4613001441823985, + "velocityY": -0.7074492385586875, + "timestamp": 2.965392830441173 + }, + { + "x": 2.2502877957375587, + "y": 5.413212847808716, + "heading": 2.8684201412995205, + "angularVelocity": -1.1677826746007212, + "velocityX": -1.5096560400578645, + "velocityY": -0.8400403634617765, + "timestamp": 2.995347836119052 + }, + { + "x": 2.205434968136323, + "y": 5.383827174903616, + "heading": 2.8338778225381454, + "angularVelocity": -1.15314011730883, + "velocityX": -1.4973399799539564, + "velocityY": -0.9809937351072376, + "timestamp": 3.0253028417969308 + }, + { + "x": 2.1627117665288242, + "y": 5.35082401983714, + "heading": 2.8010931886625263, + "angularVelocity": -1.0944626159703914, + "velocityX": -1.426245819043514, + "velocityY": -1.1017575967561364, + "timestamp": 3.0552578474748096 + }, + { + "x": 2.1231203758060415, + "y": 5.315102619215835, + "heading": 2.770927863402835, + "angularVelocity": -1.0070211831729985, + "velocityX": -1.3216953169206167, + "velocityY": -1.1925018811692434, + "timestamp": 3.0852128531526883 + }, + { + "x": 2.0871120973907478, + "y": 5.277368588566571, + "heading": 2.743899688375806, + "angularVelocity": -0.9022924354505801, + "velocityX": -1.2020788379247493, + "velocityY": -1.2596903187079023, + "timestamp": 3.115167858830567 + }, + { + "x": 2.0548974336097707, + "y": 5.238102066889186, + "heading": 2.7202319811811844, + "angularVelocity": -0.7901085864957776, + "velocityX": -1.0754350751055555, + "velocityY": -1.310850082942305, + "timestamp": 3.145122864508446 + }, + { + "x": 2.026587153392112, + "y": 5.197634903381892, + "heading": 2.6999923980627787, + "angularVelocity": -0.6756661419481035, + "velocityX": -0.9450934685873016, + "velocityY": -1.350931591950202, + "timestamp": 3.1750778701863247 + }, + { + "x": 2.002245678733676, + "y": 5.1562068665429, + "heading": 2.6831832138201195, + "angularVelocity": -0.5611477568529534, + "velocityX": -0.812601236674494, + "velocityY": -1.3830088127670148, + "timestamp": 3.2050328758642035 + }, + { + "x": 1.9819131544340292, + "y": 5.113998389763002, + "heading": 2.66978325115483, + "angularVelocity": -0.44733634202529504, + "velocityX": -0.6787688347748394, + "velocityY": -1.4090625531434064, + "timestamp": 3.2349878815420823 + }, + { + "x": 1.9656157831702816, + "y": 5.071149791653941, + "heading": 2.659765787935313, + "angularVelocity": -0.33441700286222414, + "velocityX": -0.5440616983685925, + "velocityY": -1.4304319808793762, + "timestamp": 3.264942887219961 + }, + { + "x": 1.9533711923999129, + "y": 5.027773084636261, + "heading": 2.6531059130951715, + "angularVelocity": -0.22232927984587103, + "velocityX": -0.4087660974610013, + "velocityY": -1.4480620529380386, + "timestamp": 3.29489789289784 + }, + { + "x": 1.9451914498185667, + "y": 4.983959581247479, + "heading": 2.649783348569472, + "angularVelocity": -0.11091850762536054, + "velocityX": -0.27306763581709725, + "velocityY": -1.4626438018383596, + "timestamp": 3.3248528985757186 + }, + { + "x": 1.941084861755371, + "y": 4.939785003662109, + "heading": 2.649783348569472, + "angularVelocity": 6.132292113202646e-25, + "velocityX": -0.13709188064779135, + "velocityY": -1.4746976869375825, + "timestamp": 3.3548079042535974 + }, + { + "x": 1.9419022015392158, + "y": 4.889062683793002, + "heading": 2.649783348569472, + "angularVelocity": 4.614715107153485e-26, + "velocityX": 0.023936675164732843, + "velocityY": -1.4854577230996109, + "timestamp": 3.3889538237774923 + }, + { + "x": 1.948223657679148, + "y": 4.838070468392074, + "heading": 2.649783348569472, + "angularVelocity": 4.653945144562686e-26, + "velocityX": 0.18513064600613907, + "velocityY": -1.4933619042018718, + "timestamp": 3.423099743301387 + }, + { + "x": 1.9600538072506943, + "y": 4.786928471515363, + "heading": 2.649783348569472, + "angularVelocity": 4.629261284078221e-26, + "velocityX": 0.3464586614300294, + "velocityY": -1.4977484159101515, + "timestamp": 3.457245662825282 + }, + { + "x": 1.977394685635232, + "y": 4.735788039854744, + "heading": 2.649783348569472, + "angularVelocity": 4.640038416252255e-26, + "velocityX": 0.5078462851879747, + "velocityY": -1.4977025768725385, + "timestamp": 3.491391582349177 + }, + { + "x": 2.0002427480647933, + "y": 4.684845229382865, + "heading": 2.649783348569472, + "angularVelocity": 4.6490332666751425e-26, + "velocityX": 0.6691300966012094, + "velocityY": -1.4919150276866968, + "timestamp": 3.525537501873072 + }, + { + "x": 2.0285822715254445, + "y": 4.634362832128866, + "heading": 2.649783348569472, + "angularVelocity": 4.622795940866032e-26, + "velocityX": 0.8299534426308102, + "velocityY": -1.478431331119124, + "timestamp": 3.559683421396967 + }, + { + "x": 2.0623699922411927, + "y": 4.5847082412928, + "heading": 2.649783348569472, + "angularVelocity": 4.634016030607817e-26, + "velocityX": 0.9895097624213414, + "velocityY": -1.454188129311252, + "timestamp": 3.5938293409208617 + }, + { + "x": 2.101495965027088, + "y": 4.536421557771227, + "heading": 2.649783348569472, + "angularVelocity": 4.645177261974479e-26, + "velocityX": 1.1458462191511727, + "velocityY": -1.4141274915083855, + "timestamp": 3.6279752604447566 + }, + { + "x": 2.145673252803998, + "y": 4.4903376692314145, + "heading": 2.649783348569472, + "angularVelocity": 4.6233288536561975e-26, + "velocityX": 1.2937794147261192, + "velocityY": -1.3496162698902718, + "timestamp": 3.6621211799686515 + }, + { + "x": 2.1941116699379357, + "y": 4.447748182132556, + "heading": 2.649783348569472, + "angularVelocity": 4.6479494614855375e-26, + "velocityX": 1.4185711736373225, + "velocityY": -1.2472789631292271, + "timestamp": 3.6962670994925464 + }, + { + "x": 2.2448201323304646, + "y": 4.410180142680831, + "heading": 2.649783348569472, + "angularVelocity": 4.6329904581670297e-26, + "velocityX": 1.485051891985029, + "velocityY": -1.1002204648621148, + "timestamp": 3.7304130190164413 + }, + { + "x": 2.294852293597042, + "y": 4.3780811675391975, + "heading": 2.649783348569472, + "angularVelocity": 4.6446166642422895e-26, + "velocityX": 1.4652456856979683, + "velocityY": -0.9400530309096446, + "timestamp": 3.764558938540336 + }, + { + "x": 2.342149827765447, + "y": 4.350766530180341, + "heading": 2.649783348569472, + "angularVelocity": 4.633506801188826e-26, + "velocityX": 1.3851591882100787, + "velocityY": -0.7999385501902068, + "timestamp": 3.798704858064231 + }, + { + "x": 2.3856944951859207, + "y": 4.3274872473595005, + "heading": 2.649783348569472, + "angularVelocity": 4.6381778689211406e-26, + "velocityX": 1.2752524467821442, + "velocityY": -0.6817588498253565, + "timestamp": 3.832850777588126 + }, + { + "x": 2.4249725857215654, + "y": 4.307695719032298, + "heading": 2.649783348569472, + "angularVelocity": 4.6432377599415885e-26, + "velocityX": 1.150301151156826, + "velocityY": -0.5796162060697082, + "timestamp": 3.866996697112021 + }, + { + "x": 2.4596964858202144, + "y": 4.291007007021927, + "heading": 2.649783348569472, + "angularVelocity": 4.6331179794053255e-26, + "velocityX": 1.0169267831358226, + "velocityY": -0.4887468910800007, + "timestamp": 3.9011426166359158 + }, + { + "x": 2.4896890256889828, + "y": 4.277143601542489, + "heading": 2.649783348569472, + "angularVelocity": 4.6467164999430504e-26, + "velocityX": 0.8783638070657234, + "velocityY": -0.4060047488173824, + "timestamp": 3.9352885361598107 + }, + { + "x": 2.514832571254234, + "y": 4.265898475268687, + "heading": 2.649783348569472, + "angularVelocity": 4.632856845272505e-26, + "velocityX": 0.7363557905551701, + "velocityY": -0.3293256245723021, + "timestamp": 3.9694344556837056 + }, + { + "x": 2.5350443768209585, + "y": 4.257112226026017, + "heading": 2.649783348569472, + "angularVelocity": 4.629729705599566e-26, + "velocityX": 0.5919244773180152, + "velocityY": -0.2573147645510306, + "timestamp": 4.0035803752076005 + }, + { + "x": 2.550263563230007, + "y": 4.2506587632470065, + "heading": 2.649783348569472, + "angularVelocity": 4.62810585566047e-26, + "velocityX": 0.445710252388941, + "velocityY": -0.18899660249284694, + "timestamp": 4.037726294731495 + }, + { + "x": 2.5604437164785643, + "y": 4.246436059535304, + "heading": 2.649783348569472, + "angularVelocity": 4.6519462435401186e-26, + "velocityX": 0.29813674343821717, + "velocityY": -0.12366642253542871, + "timestamp": 4.07187221425539 + }, + { + "x": 2.5655484199523926, + "y": 4.244359970092773, + "heading": 2.649783348569472, + "angularVelocity": 4.6699948971014915e-26, + "velocityX": 0.14949673474910496, + "velocityY": -0.06080051354533503, + "timestamp": 4.106018133779285 + }, + { + "x": 2.5655484199523926, + "y": 4.244359970092773, + "heading": 2.649783348569472, + "angularVelocity": 1.535197914331468e-26, + "velocityX": 1.1963425727073795e-24, + "velocityY": -7.168329076335369e-24, + "timestamp": 4.14016405330318 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Test2.1.traj b/src/main/deploy/choreo/Test2.1.traj new file mode 100644 index 00000000..6858b3e4 --- /dev/null +++ b/src/main/deploy/choreo/Test2.1.traj @@ -0,0 +1,688 @@ +{ + "samples": [ + { + "x": 0.3137660622596754, + "y": 6.865104675292969, + "heading": 3.135568639860517, + "angularVelocity": -8.5693301650697e-28, + "velocityX": -3.4362695197914885e-17, + "velocityY": -4.743570524232907e-17, + "timestamp": 0 + }, + { + "x": 0.3176438425782737, + "y": 6.865425616765028, + "heading": 3.1355686398605154, + "angularVelocity": -5.1868461530169816e-17, + "velocityX": 0.13515003367410133, + "velocityY": 0.011185587421808195, + "timestamp": 0.028692411042588198 + }, + { + "x": 0.3253999903453479, + "y": 6.866060324505518, + "heading": 3.1355686398605136, + "angularVelocity": -5.186846153227154e-17, + "velocityX": 0.2703205302461453, + "velocityY": 0.022121101623313478, + "timestamp": 0.057384822085176396 + }, + { + "x": 0.3370351106624937, + "y": 6.867001226604551, + "heading": 3.135568639860512, + "angularVelocity": -5.186846153153239e-17, + "velocityX": 0.40551211607398097, + "velocityY": 0.032792716430754286, + "timestamp": 0.0860772331277646 + }, + { + "x": 0.352549826865152, + "y": 6.86824032070545, + "heading": 3.13556863986051, + "angularVelocity": -5.186846153220684e-17, + "velocityX": 0.5407254266512643, + "velocityY": 0.04318542973123231, + "timestamp": 0.11476964417035279 + }, + { + "x": 0.3719447806630506, + "y": 6.869769136338039, + "heading": 3.1355686398605083, + "angularVelocity": -5.1868461532349994e-17, + "velocityX": 0.6759611023663795, + "velocityY": 0.053282926635904025, + "timestamp": 0.14346205521294098 + }, + { + "x": 0.3952206321171861, + "y": 6.871578692739439, + "heading": 3.135568639860506, + "angularVelocity": -5.1868461531715824e-17, + "velocityX": 0.8112197828054859, + "velocityY": 0.06306742220841621, + "timestamp": 0.17215446625552916 + }, + { + "x": 0.42237805939934603, + "y": 6.8736594514681, + "heading": 3.1355686398605043, + "angularVelocity": -5.1868461532243226e-17, + "velocityX": 0.9465020991734979, + "velocityY": 0.07251947999676765, + "timestamp": 0.20084687729811734 + }, + { + "x": 0.45341775826418784, + "y": 6.876001262985059, + "heading": 3.1355686398605025, + "angularVelocity": -5.1868461531961763e-17, + "velocityX": 1.081808664276005, + "velocityY": 0.08161780177636822, + "timestamp": 0.22953928834070553 + }, + { + "x": 0.4883404411428946, + "y": 6.878593306214626, + "heading": 3.1355686398605007, + "angularVelocity": -5.186846153181617e-17, + "velocityX": 1.2171400593303083, + "velocityY": 0.09033898286621334, + "timestamp": 0.2582316993832937 + }, + { + "x": 0.5271468357396778, + "y": 6.881424019896891, + "heading": 3.135568639860499, + "angularVelocity": -5.186846153207198e-17, + "velocityX": 1.3524968166384297, + "velocityY": 0.09865722605409555, + "timestamp": 0.2869241104258819 + }, + { + "x": 0.5698376829754908, + "y": 6.884481024296066, + "heading": 3.135568639860497, + "angularVelocity": -5.18684615321628e-17, + "velocityX": 1.4878793968358233, + "velocityY": 0.10654400547371551, + "timestamp": 0.3156165214684701 + }, + { + "x": 0.6164137340738928, + "y": 6.887751031517755, + "heading": 3.1355686398604954, + "angularVelocity": -5.186846153204876e-17, + "velocityX": 1.6232881589932495, + "velocityY": 0.11396766959864212, + "timestamp": 0.34430893251105826 + }, + { + "x": 0.666875746517318, + "y": 6.891219742296381, + "heading": 3.1355686398604936, + "angularVelocity": -5.186846153160267e-17, + "velocityX": 1.758723321247663, + "velocityY": 0.12089296969422078, + "timestamp": 0.37300134355364645 + }, + { + "x": 0.7212244785112947, + "y": 6.894871726616115, + "heading": 3.135568639860492, + "angularVelocity": -5.186846153215831e-17, + "velocityX": 1.894184908800638, + "velocityY": 0.1272804963763128, + "timestamp": 0.40169375459623463 + }, + { + "x": 0.7794606814696723, + "y": 6.898690284891088, + "heading": 3.13556863986049, + "angularVelocity": -5.186846153213201e-17, + "velocityX": 2.02967268494572, + "velocityY": 0.1330860020548787, + "timestamp": 0.4303861656388228 + }, + { + "x": 0.8415850898614354, + "y": 6.9026572856068995, + "heading": 3.1355686398604883, + "angularVelocity": -5.186846153171342e-17, + "velocityX": 2.165186059113343, + "velocityY": 0.13825958055332968, + "timestamp": 0.459078576681411 + }, + { + "x": 0.9075984075182041, + "y": 6.906752974251311, + "heading": 3.1355686398604865, + "angularVelocity": -5.1868461532457525e-17, + "velocityX": 2.3007239635171715, + "velocityY": 0.14274466646709222, + "timestamp": 0.4877709877239992 + }, + { + "x": 0.9775012891595924, + "y": 6.910955746947304, + "heading": 3.1355686398604847, + "angularVelocity": -5.186846153166985e-17, + "velocityX": 2.43628468648487, + "velocityY": 0.14647680495568935, + "timestamp": 0.5164633987665874 + }, + { + "x": 1.051294315403438, + "y": 6.915241880318482, + "heading": 3.1355686398604825, + "angularVelocity": -5.1868461532007456e-17, + "velocityX": 2.571865645390099, + "velocityY": 0.1493821263335398, + "timestamp": 0.5451558098091757 + }, + { + "x": 1.1289779588157673, + "y": 6.919585206580955, + "heading": 3.1355686398604807, + "angularVelocity": -5.186846153206435e-17, + "velocityX": 2.7074630743656276, + "velocityY": 0.15137543708073178, + "timestamp": 0.5738482208517639 + }, + { + "x": 1.210552537505391, + "y": 6.923956719399065, + "heading": 3.135568639860479, + "angularVelocity": -5.1868461531968826e-17, + "velocityX": 2.843071590203452, + "velocityY": 0.1523578067950584, + "timestamp": 0.6025406318943521 + }, + { + "x": 1.29601815119489, + "y": 6.9283240912670765, + "heading": 3.135568639860477, + "angularVelocity": -5.186846153216223e-17, + "velocityX": 2.978683581614699, + "velocityY": 0.15221348465724693, + "timestamp": 0.6312330429369404 + }, + { + "x": 1.3853745923008893, + "y": 6.932651076487761, + "heading": 3.1355686398604754, + "angularVelocity": -5.1868461531841376e-17, + "velocityX": 3.1142883382426785, + "velocityY": 0.15080591220658612, + "timestamp": 0.6599254539795286 + }, + { + "x": 1.478621220826743, + "y": 6.936896764298285, + "heading": 3.1355686398604736, + "angularVelocity": -5.186846153215644e-17, + "velocityX": 3.2498707894378844, + "velocityY": 0.1479725006107481, + "timestamp": 0.6886178650221169 + }, + { + "x": 1.5757567859462194, + "y": 6.941014632922591, + "heading": 3.135568639860472, + "angularVelocity": -5.186846153173054e-17, + "velocityX": 3.3854096463101833, + "velocityY": 0.14351769247253987, + "timestamp": 0.7173102760647051 + }, + { + "x": 1.676779167520274, + "y": 6.94495133505747, + "heading": 3.13556863986047, + "angularVelocity": -5.186846153388186e-17, + "velocityX": 3.52087461120313, + "velocityY": 0.13720360164351214, + "timestamp": 0.7460026871072933 + }, + { + "x": 1.7816849946975712, + "y": 6.948645114898683, + "heading": 3.1355686398604683, + "angularVelocity": -1.6596882408000968e-16, + "velocityX": 3.6562220937649346, + "velocityY": 0.12873717150261788, + "timestamp": 0.7746950981498816 + }, + { + "x": 1.8121461784699802, + "y": 6.949692765946351, + "heading": 3.1362037838100756, + "angularVelocity": 0.07689012502825784, + "velocityX": 3.687611651202223, + "velocityY": 0.12682797354934053, + "timestamp": 0.7829555080448543 + }, + { + "x": 1.8428693964358074, + "y": 6.950724930078729, + "heading": 3.1374589452138397, + "angularVelocity": 0.15194904607940982, + "velocityX": 3.7193333450103565, + "velocityY": 0.12495313737441759, + "timestamp": 0.7912159179398269 + }, + { + "x": 1.8738574968140793, + "y": 6.95174197296817, + "heading": 3.139318096432083, + "angularVelocity": 0.22506767120320992, + "velocityX": 3.751399842413374, + "velocityY": 0.1231225692636009, + "timestamp": 0.7994763278347996 + }, + { + "x": 1.9051134303883683, + "y": 6.952744343319344, + "heading": 3.1417642609142864, + "angularVelocity": 0.2961311258529596, + "velocityX": 3.783823559810237, + "velocityY": 0.12134632105632295, + "timestamp": 0.8077367377297723 + }, + { + "x": 1.9366402470506743, + "y": 6.953732574884012, + "heading": 3.1447794649491048, + "angularVelocity": 0.36501869436959006, + "velocityX": 3.8166164952046153, + "velocityY": 0.11963468849991928, + "timestamp": 0.815997147624745 + }, + { + "x": 1.96844109067084, + "y": 6.9547072895390265, + "heading": 3.148344689757031, + "angularVelocity": 0.43160386146187707, + "velocityX": 3.849790025494764, + "velocityY": 0.117998339961998, + "timestamp": 0.8242575575197176 + }, + { + "x": 2.0005191919654135, + "y": 6.955669201722795, + "heading": 3.152439824908413, + "angularVelocity": 0.495754472653526, + "velocityX": 3.883354664287875, + "velocityY": 0.11644848088619933, + "timestamp": 0.8325179674146903 + }, + { + "x": 2.032877859008433, + "y": 6.9566191245667985, + "heading": 3.1570436241813726, + "angularVelocity": 0.5573330296555058, + "velocityX": 3.91731977643278, + "velocityY": 0.11499705899340439, + "timestamp": 0.840778377309663 + }, + { + "x": 2.065520465002766, + "y": 6.957557978090485, + "heading": 3.1621336650538163, + "angularVelocity": 0.6161971303075195, + "velocityX": 3.951693246384564, + "velocityY": 0.11365701407324179, + "timestamp": 0.8490387872046357 + }, + { + "x": 2.0984504329177724, + "y": 6.958486799841183, + "heading": 3.1676863130152344, + "angularVelocity": 0.6722000520579139, + "velocityX": 3.9864810988430395, + "velocityY": 0.11244257397721802, + "timestamp": 0.8572991970996083 + }, + { + "x": 2.1316712166001497, + "y": 6.959406758344203, + "heading": 3.1736766917418935, + "angularVelocity": 0.7251914617828771, + "velocityX": 4.021687071799445, + "velocityY": 0.1113695948160438, + "timestamp": 0.865559606994581 + }, + { + "x": 2.165186277981829, + "y": 6.960319169668765, + "heading": 3.1800786598480077, + "angularVelocity": 0.7750182118701426, + "velocityX": 4.057312144046869, + "velocityY": 0.11045593816320934, + "timestamp": 0.8738200168895537 + }, + { + "x": 2.1989990600414004, + "y": 6.961225517299853, + "heading": 3.1868647943382302, + "angularVelocity": 0.8215251514763765, + "velocityX": 4.093354021105941, + "velocityY": 0.10972187126423899, + "timestamp": 0.8820804267845264 + }, + { + "x": 2.2331129552203137, + "y": 6.962127475323903, + "heading": 3.19400637995892, + "angularVelocity": 0.8645558406293681, + "velocityX": 4.1298065849823535, + "velocityY": 0.10919046821108658, + "timestamp": 0.890340836679499 + }, + { + "x": 2.2675312690432237, + "y": 6.963026934685406, + "heading": 3.201473402262516, + "angularVelocity": 0.9039529997346885, + "velocityX": 4.1666593135838506, + "velocityY": 0.10888798170273117, + "timestamp": 0.8986012465744717 + }, + { + "x": 2.302257178726397, + "y": 6.963926031963223, + "heading": 3.2092345401896574, + "angularVelocity": 0.9395584511933502, + "velocityX": 4.203896673978105, + "velocityY": 0.10884414808067534, + "timestamp": 0.9068616564694444 + }, + { + "x": 2.3372936865473584, + "y": 6.964827179782931, + "heading": 3.2172571510612844, + "angularVelocity": 0.9712122005605541, + "velocityX": 4.241497488191654, + "velocityY": 0.10909238538572823, + "timestamp": 0.9151220663644171 + }, + { + "x": 2.372643567633051, + "y": 6.965733097685614, + "heading": 3.225507236598066, + "angularVelocity": 0.9987501397242283, + "velocityX": 4.279434257518503, + "velocityY": 0.10966984861524744, + "timestamp": 0.9233824762593897 + }, + { + "x": 2.4083093114869696, + "y": 6.9666468421059164, + "heading": 3.233949372180996, + "angularVelocity": 1.0219995969049733, + "velocityX": 4.31767240456481, + "velocityY": 0.11061732189065114, + "timestamp": 0.9316428861543624 + }, + { + "x": 2.44429305579275, + "y": 6.967571834188082, + "heading": 3.2425465717794997, + "angularVelocity": 1.0407715486052915, + "velocityX": 4.356169338240552, + "velocityY": 0.11197895672538041, + "timestamp": 0.9399032960493351 + }, + { + "x": 2.480596509367599, + "y": 6.968511884597095, + "heading": 3.251260045781759, + "angularVelocity": 1.054847654420144, + "velocityX": 4.394873140247142, + "velocityY": 0.11380190825438415, + "timestamp": 0.9481637059443078 + }, + { + "x": 2.517220857768171, + "y": 6.969471215284249, + "heading": 3.260048785175124, + "angularVelocity": 1.0639592350877263, + "velocityX": 4.433720464992984, + "velocityY": 0.11613596653794503, + "timestamp": 0.9564241158392804 + }, + { + "x": 2.55416663843719, + "y": 6.970454479070239, + "heading": 3.2688688682505744, + "angularVelocity": 1.0677536814268536, + "velocityX": 4.4726328522150816, + "velocityY": 0.11903329235341958, + "timestamp": 0.9646845257342531 + }, + { + "x": 2.5914335596417093, + "y": 6.971466777824258, + "heading": 3.2776723283732965, + "angularVelocity": 1.065741317277861, + "velocityX": 4.511509922431134, + "velocityY": 0.12254824722845813, + "timestamp": 0.9729449356292258 + }, + { + "x": 2.6290202140808088, + "y": 6.9725136756896955, + "heading": 3.286405336491996, + "angularVelocity": 1.0572124422077387, + "velocityX": 4.550216625687468, + "velocityY": 0.12673679378462502, + "timestamp": 0.9812053455241985 + }, + { + "x": 2.9717276780335036, + "y": 6.984363087388876, + "heading": 3.35108767309623, + "angularVelocity": 0.9203746590757935, + "velocityX": 4.876435853393211, + "velocityY": 0.16860705449782717, + "timestamp": 1.0514836131475132 + }, + { + "x": 3.3366815448761877, + "y": 6.999743371642888, + "heading": 3.3973428408952557, + "angularVelocity": 0.6581717131524953, + "velocityX": 5.192983253355129, + "velocityY": 0.21884836912099448, + "timestamp": 1.1217618807708278 + }, + { + "x": 3.71402485435163, + "y": 7.027526643127482, + "heading": 3.3973428368677423, + "angularVelocity": -5.730809398962461e-8, + "velocityX": 5.369274488921224, + "velocityY": 0.3953323327989187, + "timestamp": 1.1920401483941425 + }, + { + "x": 4.090360227255461, + "y": 7.066661937541599, + "heading": 3.3973428243317834, + "angularVelocity": -1.783760359331945e-7, + "velocityX": 5.354932408421835, + "velocityY": 0.5568619679682045, + "timestamp": 1.2623184160174572 + }, + { + "x": 4.466694898854889, + "y": 7.105803975313799, + "heading": 3.3973428117958613, + "angularVelocity": -1.7837551225317533e-7, + "velocityX": 5.354922429456401, + "velocityY": 0.5569579202208789, + "timestamp": 1.3325966836407719 + }, + { + "x": 4.8430291421579215, + "y": 7.144950130722809, + "heading": 3.397342799228156, + "angularVelocity": -1.7882775854425254e-7, + "velocityX": 5.354916335162799, + "velocityY": 0.5570165106920909, + "timestamp": 1.4028749512640866 + }, + { + "x": 5.214361577079249, + "y": 7.183676617694927, + "heading": 3.378511132398759, + "angularVelocity": -0.26795860891639617, + "velocityX": 5.283744854264688, + "velocityY": 0.55104498562348, + "timestamp": 1.4731532188874013 + }, + { + "x": 5.562524060208366, + "y": 7.219962031584869, + "heading": 3.355414523374288, + "angularVelocity": -0.32864511043819594, + "velocityX": 4.9540561385951625, + "velocityY": 0.5163105909843465, + "timestamp": 1.543431486510716 + }, + { + "x": 5.887470499962918, + "y": 7.253817931255832, + "heading": 3.331666055733143, + "angularVelocity": -0.3379205043646441, + "velocityX": 4.6237115788942855, + "velocityY": 0.48174066914158636, + "timestamp": 1.6137097541340306 + }, + { + "x": 6.189199807150463, + "y": 7.285249171486667, + "heading": 3.308363686841363, + "angularVelocity": -0.33157289841972754, + "velocityX": 4.29335152091094, + "velocityY": 0.4472398266744939, + "timestamp": 1.6839880217573453 + }, + { + "x": 6.467713626267079, + "y": 7.31425833816948, + "heading": 3.2860397702782755, + "angularVelocity": -0.3176503536305448, + "velocityX": 3.963014862708697, + "velocityY": 0.41277577925367004, + "timestamp": 1.75426628938066 + }, + { + "x": 6.723013594809074, + "y": 7.34084703024564, + "heading": 3.265008761402615, + "angularVelocity": -0.29925337642620564, + "velocityX": 3.6327015046867763, + "velocityY": 0.37833448340917136, + "timestamp": 1.8245445570039747 + }, + { + "x": 6.955101065031071, + "y": 7.365016333172441, + "heading": 3.245478308115181, + "angularVelocity": -0.27790174612893576, + "velocityX": 3.302407388098493, + "velocityY": 0.34390863269912847, + "timestamp": 1.8948228246272893 + }, + { + "x": 7.163977127241494, + "y": 7.386767031992689, + "heading": 3.2275957745381954, + "angularVelocity": -0.2544532496565816, + "velocityX": 2.972128785672115, + "velocityY": 0.3094939524808096, + "timestamp": 1.965101092250604 + }, + { + "x": 7.349642665971283, + "y": 7.4060997209757735, + "heading": 3.2114711639593976, + "angularVelocity": -0.22943949992088322, + "velocityX": 2.6418627693690517, + "velocityY": 0.2750877282106925, + "timestamp": 2.035379359873919 + }, + { + "x": 7.512098408217057, + "y": 7.423014865697383, + "heading": 3.1971897253730983, + "angularVelocity": -0.2032127294720464, + "velocityX": 2.3116070976097918, + "velocityY": 0.24068812868678965, + "timestamp": 2.1056576274972336 + }, + { + "x": 7.651344959888608, + "y": 7.43751284079522, + "heading": 3.184819461924971, + "angularVelocity": -0.17601833207433357, + "velocityX": 1.981360047431725, + "velocityY": 0.20629385993890986, + "timestamp": 2.1759358951205483 + }, + { + "x": 7.767382832665261, + "y": 7.449593954241994, + "heading": 3.174415882660127, + "angularVelocity": -0.14803408815663477, + "velocityX": 1.6511202780154728, + "velocityY": 0.1719039733808395, + "timestamp": 2.246214162743863 + }, + { + "x": 7.860212463836391, + "y": 7.459258463658005, + "heading": 3.1660251568616267, + "angularVelocity": -0.11939289459260129, + "velocityX": 1.3208867308552614, + "velocityY": 0.13751775254062693, + "timestamp": 2.3164924303671777 + }, + { + "x": 7.929834231127341, + "y": 7.466506587678373, + "heading": 3.159686290431417, + "angularVelocity": -0.09019668020541635, + "velocityX": 0.9906585584055106, + "velocityY": 0.10313464269233455, + "timestamp": 2.3867706979904924 + }, + { + "x": 7.976248463934981, + "y": 7.471338514116222, + "heading": 3.1554326760982865, + "angularVelocity": -0.06052531567696676, + "velocityX": 0.6604350729932147, + "velocityY": 0.06875420526505516, + "timestamp": 2.457048965613807 + }, + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "angularVelocity": -0.03044253542482025, + "velocityX": 0.33021570985126986, + "velocityY": 0.03437608724320182, + "timestamp": 2.5273272332371217 + }, + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "angularVelocity": -3.79407884851814e-18, + "velocityX": 2.546987187300363e-16, + "velocityY": -1.0256209789219734e-16, + "timestamp": 2.5976055008604364 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Test2.2.traj b/src/main/deploy/choreo/Test2.2.traj new file mode 100644 index 00000000..a0cb7c71 --- /dev/null +++ b/src/main/deploy/choreo/Test2.2.traj @@ -0,0 +1,301 @@ +{ + "samples": [ + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "angularVelocity": -3.79407884851814e-18, + "velocityX": 2.546987187300363e-16, + "velocityY": -1.0256209789219734e-16, + "timestamp": 0 + }, + { + "x": 7.9777911181971035, + "y": 7.466979623755239, + "heading": 3.153293227446569, + "angularVelocity": 4.0010921973360205e-29, + "velocityX": -0.31261406582002, + "velocityY": -0.09775939742848679, + "timestamp": 0.06930057261307221 + }, + { + "x": 7.934462450883723, + "y": 7.453430059384792, + "heading": 3.153293227446569, + "angularVelocity": -2.051733815514594e-34, + "velocityX": -0.6252281284210741, + "velocityY": -0.1955187938503514, + "timestamp": 0.13860114522614442 + }, + { + "x": 7.869469450281316, + "y": 7.433105712944096, + "heading": 3.153293227446569, + "angularVelocity": -1.98522229630168e-34, + "velocityX": -0.9378421873262779, + "velocityY": -0.29327818911646475, + "timestamp": 0.20790171783921663 + }, + { + "x": 7.782812116686986, + "y": 7.40600658452606, + "heading": 3.153293227446569, + "angularVelocity": -1.9190015159804115e-34, + "velocityX": -1.2504562419442953, + "velocityY": -0.39103758304190667, + "timestamp": 0.27720229045228884 + }, + { + "x": 7.6744904504495075, + "y": 7.372132674239751, + "heading": 3.153293227446569, + "angularVelocity": -8.200637720207044e-29, + "velocityX": -1.5630702915295283, + "velocityY": -0.4887969753935167, + "timestamp": 0.34650286306536104 + }, + { + "x": 7.544504451984089, + "y": 7.331483982215012, + "heading": 3.153293227446569, + "angularVelocity": 6.610929004011549e-29, + "velocityX": -1.8756843351233508, + "velocityY": -0.5865563658715173, + "timestamp": 0.41580343567843325 + }, + { + "x": 7.392854121793352, + "y": 7.28406050860902, + "heading": 3.153293227446569, + "angularVelocity": -6.483737543755527e-29, + "velocityX": -2.188298371464413, + "velocityY": -0.6843157540814643, + "timestamp": 0.48510400829150546 + }, + { + "x": 7.219539460498179, + "y": 7.229862253615935, + "heading": 3.153293227446569, + "angularVelocity": 8.073376116568391e-29, + "velocityX": -2.5009123988461828, + "velocityY": -0.7820751394896976, + "timestamp": 0.5544045809045777 + }, + { + "x": 7.024560468885024, + "y": 7.168889217481695, + "heading": 3.153293227446569, + "angularVelocity": -3.526812750348743e-30, + "velocityX": -2.813526414879514, + "velocityY": -0.8798345213490937, + "timestamp": 0.6237051535176499 + }, + { + "x": 6.807917147982327, + "y": 7.101141400527908, + "heading": 3.153293227446569, + "angularVelocity": -9.438045628849687e-29, + "velocityX": -3.1261404160725776, + "velocityY": -0.9775938985677018, + "timestamp": 0.6930057261307221 + }, + { + "x": 6.569609499192503, + "y": 7.026618803193132, + "heading": 3.153293227446569, + "angularVelocity": 9.790681244582267e-29, + "velocityX": -3.438754397028912, + "velocityY": -1.0753532694579622, + "timestamp": 0.7623062987437943 + }, + { + "x": 6.309637524541267, + "y": 6.94532142611084, + "heading": 3.153293227446569, + "angularVelocity": -1.3895984503554753e-34, + "velocityX": -3.7513683487544087, + "velocityY": -1.1731126312072742, + "timestamp": 0.8316068713568665 + }, + { + "x": 6.028001227211883, + "y": 6.85724927027649, + "heading": 3.153293227446569, + "angularVelocity": -1.323503512813964e-34, + "velocityX": -4.063982254545725, + "velocityY": -1.2708719785922367, + "timestamp": 0.9009074439699387 + }, + { + "x": 5.724700612934229, + "y": 6.762402337481905, + "heading": 3.153293227446569, + "angularVelocity": -2.1989550257903952e-29, + "velocityX": -4.376596077655505, + "velocityY": -1.3686313001213652, + "timestamp": 0.9702080165830109 + }, + { + "x": 5.399735695078014, + "y": 6.660780631908008, + "heading": 3.153293227446569, + "angularVelocity": 1.8024841235265752e-29, + "velocityX": -4.689209707841794, + "velocityY": -1.4663905613202457, + "timestamp": 1.0395085891960831 + }, + { + "x": 5.053106540487171, + "y": 6.552384174457959, + "heading": 3.153293227446569, + "angularVelocity": 1.896211807861914e-29, + "velocityX": -5.001822373477154, + "velocityY": -1.564149520888688, + "timestamp": 1.1088091618091553 + }, + { + "x": 4.7281416226309565, + "y": 6.4507624688840615, + "heading": 3.153293227446569, + "angularVelocity": -1.4997927313419928e-29, + "velocityX": -4.689209707841795, + "velocityY": -1.4663905613202457, + "timestamp": 1.1781097344222276 + }, + { + "x": 4.424841008353304, + "y": 6.355915536089477, + "heading": 3.153293227446569, + "angularVelocity": -4.4185947102012545e-35, + "velocityX": -4.376596077655505, + "velocityY": -1.3686313001213652, + "timestamp": 1.2474103070352998 + }, + { + "x": 4.14320471102392, + "y": 6.267843380255127, + "heading": 3.153293227446569, + "angularVelocity": -9.264733448000688e-35, + "velocityX": -4.063982254545726, + "velocityY": -1.2708719785922367, + "timestamp": 1.316710879648372 + }, + { + "x": 3.883232736372683, + "y": 6.186546003172835, + "heading": 3.153293227446569, + "angularVelocity": 3.5265482976182584e-30, + "velocityX": -3.751368348754409, + "velocityY": -1.1731126312072742, + "timestamp": 1.3860114522614442 + }, + { + "x": 3.644925087582859, + "y": 6.112023405838059, + "heading": 3.153293227446569, + "angularVelocity": 2.156768838744686e-29, + "velocityX": -3.438754397028912, + "velocityY": -1.0753532694579622, + "timestamp": 1.4553120248745168 + }, + { + "x": 3.4282817666801613, + "y": 6.044275588884272, + "heading": 3.153293227446569, + "angularVelocity": -2.105657187574439e-28, + "velocityX": -3.1261404160725776, + "velocityY": -0.9775938985677018, + "timestamp": 1.5246125974875895 + }, + { + "x": 3.2333027750670063, + "y": 5.983302552750032, + "heading": 3.153293227446569, + "angularVelocity": 1.8547117767579628e-28, + "velocityX": -2.813526414879514, + "velocityY": -0.8798345213490937, + "timestamp": 1.5939131701006621 + }, + { + "x": 3.059988113771833, + "y": 5.929104297756947, + "heading": 3.153293227446569, + "angularVelocity": -5.955568710068408e-35, + "velocityX": -2.5009123988461828, + "velocityY": -0.7820751394896976, + "timestamp": 1.6632137427137348 + }, + { + "x": 2.908337783581096, + "y": 5.881680824150955, + "heading": 3.153293227446569, + "angularVelocity": -2.0810713408746708e-29, + "velocityX": -2.1882983714644135, + "velocityY": -0.6843157540814642, + "timestamp": 1.7325143153268074 + }, + { + "x": 2.7783517851156776, + "y": 5.841032132126216, + "heading": 3.153293227446569, + "angularVelocity": 2.081061415014159e-29, + "velocityX": -1.875684335123351, + "velocityY": -0.5865563658715173, + "timestamp": 1.80181488793988 + }, + { + "x": 2.6700301188781994, + "y": 5.807158221839908, + "heading": 3.153293227446569, + "angularVelocity": 7.326893682940581e-29, + "velocityX": -1.5630702915295283, + "velocityY": -0.48879697539351663, + "timestamp": 1.8711154605529527 + }, + { + "x": 2.583372785283869, + "y": 5.780059093421871, + "heading": 3.153293227446569, + "angularVelocity": -7.326900961882318e-29, + "velocityX": -1.2504562419442953, + "velocityY": -0.39103758304190656, + "timestamp": 1.9404160331660254 + }, + { + "x": 2.5183797846814615, + "y": 5.759734746981175, + "heading": 3.153293227446569, + "angularVelocity": -2.64698564160025e-35, + "velocityX": -0.9378421873262781, + "velocityY": -0.2932781891164647, + "timestamp": 2.009716605779098 + }, + { + "x": 2.4750511173680816, + "y": 5.746185182610728, + "heading": 3.153293227446569, + "angularVelocity": -4.5099737006589516e-29, + "velocityX": -0.6252281284210743, + "velocityY": -0.1955187938503513, + "timestamp": 2.0790171783921707 + }, + { + "x": 2.4533867835998535, + "y": 5.739410400390625, + "heading": 3.153293227446569, + "angularVelocity": 1.9207053764808128e-28, + "velocityX": -0.31261406582002027, + "velocityY": -0.09775939742848665, + "timestamp": 2.1483177510052434 + }, + { + "x": 2.4533867835998535, + "y": 5.739410400390625, + "heading": 3.153293227446569, + "angularVelocity": -4.899028012023128e-29, + "velocityX": 1.275108562409782e-22, + "velocityY": 3.987692290669026e-23, + "timestamp": 2.217618323618316 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Test2.traj b/src/main/deploy/choreo/Test2.traj new file mode 100644 index 00000000..68c886e7 --- /dev/null +++ b/src/main/deploy/choreo/Test2.traj @@ -0,0 +1,976 @@ +{ + "samples": [ + { + "x": 0.3137660622596754, + "y": 6.865104675292969, + "heading": 3.135568639860517, + "angularVelocity": -8.5693301650697e-28, + "velocityX": -3.4362695197914885e-17, + "velocityY": -4.743570524232907e-17, + "timestamp": 0 + }, + { + "x": 0.3176438425782737, + "y": 6.865425616765028, + "heading": 3.1355686398605154, + "angularVelocity": -5.1868461530169816e-17, + "velocityX": 0.13515003367410133, + "velocityY": 0.011185587421808195, + "timestamp": 0.028692411042588198 + }, + { + "x": 0.3253999903453479, + "y": 6.866060324505518, + "heading": 3.1355686398605136, + "angularVelocity": -5.186846153227154e-17, + "velocityX": 0.2703205302461453, + "velocityY": 0.022121101623313478, + "timestamp": 0.057384822085176396 + }, + { + "x": 0.3370351106624937, + "y": 6.867001226604551, + "heading": 3.135568639860512, + "angularVelocity": -5.186846153153239e-17, + "velocityX": 0.40551211607398097, + "velocityY": 0.032792716430754286, + "timestamp": 0.0860772331277646 + }, + { + "x": 0.352549826865152, + "y": 6.86824032070545, + "heading": 3.13556863986051, + "angularVelocity": -5.186846153220684e-17, + "velocityX": 0.5407254266512643, + "velocityY": 0.04318542973123231, + "timestamp": 0.11476964417035279 + }, + { + "x": 0.3719447806630506, + "y": 6.869769136338039, + "heading": 3.1355686398605083, + "angularVelocity": -5.1868461532349994e-17, + "velocityX": 0.6759611023663795, + "velocityY": 0.053282926635904025, + "timestamp": 0.14346205521294098 + }, + { + "x": 0.3952206321171861, + "y": 6.871578692739439, + "heading": 3.135568639860506, + "angularVelocity": -5.1868461531715824e-17, + "velocityX": 0.8112197828054859, + "velocityY": 0.06306742220841621, + "timestamp": 0.17215446625552916 + }, + { + "x": 0.42237805939934603, + "y": 6.8736594514681, + "heading": 3.1355686398605043, + "angularVelocity": -5.1868461532243226e-17, + "velocityX": 0.9465020991734979, + "velocityY": 0.07251947999676765, + "timestamp": 0.20084687729811734 + }, + { + "x": 0.45341775826418784, + "y": 6.876001262985059, + "heading": 3.1355686398605025, + "angularVelocity": -5.1868461531961763e-17, + "velocityX": 1.081808664276005, + "velocityY": 0.08161780177636822, + "timestamp": 0.22953928834070553 + }, + { + "x": 0.4883404411428946, + "y": 6.878593306214626, + "heading": 3.1355686398605007, + "angularVelocity": -5.186846153181617e-17, + "velocityX": 1.2171400593303083, + "velocityY": 0.09033898286621334, + "timestamp": 0.2582316993832937 + }, + { + "x": 0.5271468357396778, + "y": 6.881424019896891, + "heading": 3.135568639860499, + "angularVelocity": -5.186846153207198e-17, + "velocityX": 1.3524968166384297, + "velocityY": 0.09865722605409555, + "timestamp": 0.2869241104258819 + }, + { + "x": 0.5698376829754908, + "y": 6.884481024296066, + "heading": 3.135568639860497, + "angularVelocity": -5.18684615321628e-17, + "velocityX": 1.4878793968358233, + "velocityY": 0.10654400547371551, + "timestamp": 0.3156165214684701 + }, + { + "x": 0.6164137340738928, + "y": 6.887751031517755, + "heading": 3.1355686398604954, + "angularVelocity": -5.186846153204876e-17, + "velocityX": 1.6232881589932495, + "velocityY": 0.11396766959864212, + "timestamp": 0.34430893251105826 + }, + { + "x": 0.666875746517318, + "y": 6.891219742296381, + "heading": 3.1355686398604936, + "angularVelocity": -5.186846153160267e-17, + "velocityX": 1.758723321247663, + "velocityY": 0.12089296969422078, + "timestamp": 0.37300134355364645 + }, + { + "x": 0.7212244785112947, + "y": 6.894871726616115, + "heading": 3.135568639860492, + "angularVelocity": -5.186846153215831e-17, + "velocityX": 1.894184908800638, + "velocityY": 0.1272804963763128, + "timestamp": 0.40169375459623463 + }, + { + "x": 0.7794606814696723, + "y": 6.898690284891088, + "heading": 3.13556863986049, + "angularVelocity": -5.186846153213201e-17, + "velocityX": 2.02967268494572, + "velocityY": 0.1330860020548787, + "timestamp": 0.4303861656388228 + }, + { + "x": 0.8415850898614354, + "y": 6.9026572856068995, + "heading": 3.1355686398604883, + "angularVelocity": -5.186846153171342e-17, + "velocityX": 2.165186059113343, + "velocityY": 0.13825958055332968, + "timestamp": 0.459078576681411 + }, + { + "x": 0.9075984075182041, + "y": 6.906752974251311, + "heading": 3.1355686398604865, + "angularVelocity": -5.1868461532457525e-17, + "velocityX": 2.3007239635171715, + "velocityY": 0.14274466646709222, + "timestamp": 0.4877709877239992 + }, + { + "x": 0.9775012891595924, + "y": 6.910955746947304, + "heading": 3.1355686398604847, + "angularVelocity": -5.186846153166985e-17, + "velocityX": 2.43628468648487, + "velocityY": 0.14647680495568935, + "timestamp": 0.5164633987665874 + }, + { + "x": 1.051294315403438, + "y": 6.915241880318482, + "heading": 3.1355686398604825, + "angularVelocity": -5.1868461532007456e-17, + "velocityX": 2.571865645390099, + "velocityY": 0.1493821263335398, + "timestamp": 0.5451558098091757 + }, + { + "x": 1.1289779588157673, + "y": 6.919585206580955, + "heading": 3.1355686398604807, + "angularVelocity": -5.186846153206435e-17, + "velocityX": 2.7074630743656276, + "velocityY": 0.15137543708073178, + "timestamp": 0.5738482208517639 + }, + { + "x": 1.210552537505391, + "y": 6.923956719399065, + "heading": 3.135568639860479, + "angularVelocity": -5.1868461531968826e-17, + "velocityX": 2.843071590203452, + "velocityY": 0.1523578067950584, + "timestamp": 0.6025406318943521 + }, + { + "x": 1.29601815119489, + "y": 6.9283240912670765, + "heading": 3.135568639860477, + "angularVelocity": -5.186846153216223e-17, + "velocityX": 2.978683581614699, + "velocityY": 0.15221348465724693, + "timestamp": 0.6312330429369404 + }, + { + "x": 1.3853745923008893, + "y": 6.932651076487761, + "heading": 3.1355686398604754, + "angularVelocity": -5.1868461531841376e-17, + "velocityX": 3.1142883382426785, + "velocityY": 0.15080591220658612, + "timestamp": 0.6599254539795286 + }, + { + "x": 1.478621220826743, + "y": 6.936896764298285, + "heading": 3.1355686398604736, + "angularVelocity": -5.186846153215644e-17, + "velocityX": 3.2498707894378844, + "velocityY": 0.1479725006107481, + "timestamp": 0.6886178650221169 + }, + { + "x": 1.5757567859462194, + "y": 6.941014632922591, + "heading": 3.135568639860472, + "angularVelocity": -5.186846153173054e-17, + "velocityX": 3.3854096463101833, + "velocityY": 0.14351769247253987, + "timestamp": 0.7173102760647051 + }, + { + "x": 1.676779167520274, + "y": 6.94495133505747, + "heading": 3.13556863986047, + "angularVelocity": -5.186846153388186e-17, + "velocityX": 3.52087461120313, + "velocityY": 0.13720360164351214, + "timestamp": 0.7460026871072933 + }, + { + "x": 1.7816849946975712, + "y": 6.948645114898683, + "heading": 3.1355686398604683, + "angularVelocity": -1.6596882408000968e-16, + "velocityX": 3.6562220937649346, + "velocityY": 0.12873717150261788, + "timestamp": 0.7746950981498816 + }, + { + "x": 1.8121461784699802, + "y": 6.949692765946351, + "heading": 3.1362037838100756, + "angularVelocity": 0.07689012502825784, + "velocityX": 3.687611651202223, + "velocityY": 0.12682797354934053, + "timestamp": 0.7829555080448543 + }, + { + "x": 1.8428693964358074, + "y": 6.950724930078729, + "heading": 3.1374589452138397, + "angularVelocity": 0.15194904607940982, + "velocityX": 3.7193333450103565, + "velocityY": 0.12495313737441759, + "timestamp": 0.7912159179398269 + }, + { + "x": 1.8738574968140793, + "y": 6.95174197296817, + "heading": 3.139318096432083, + "angularVelocity": 0.22506767120320992, + "velocityX": 3.751399842413374, + "velocityY": 0.1231225692636009, + "timestamp": 0.7994763278347996 + }, + { + "x": 1.9051134303883683, + "y": 6.952744343319344, + "heading": 3.1417642609142864, + "angularVelocity": 0.2961311258529596, + "velocityX": 3.783823559810237, + "velocityY": 0.12134632105632295, + "timestamp": 0.8077367377297723 + }, + { + "x": 1.9366402470506743, + "y": 6.953732574884012, + "heading": 3.1447794649491048, + "angularVelocity": 0.36501869436959006, + "velocityX": 3.8166164952046153, + "velocityY": 0.11963468849991928, + "timestamp": 0.815997147624745 + }, + { + "x": 1.96844109067084, + "y": 6.9547072895390265, + "heading": 3.148344689757031, + "angularVelocity": 0.43160386146187707, + "velocityX": 3.849790025494764, + "velocityY": 0.117998339961998, + "timestamp": 0.8242575575197176 + }, + { + "x": 2.0005191919654135, + "y": 6.955669201722795, + "heading": 3.152439824908413, + "angularVelocity": 0.495754472653526, + "velocityX": 3.883354664287875, + "velocityY": 0.11644848088619933, + "timestamp": 0.8325179674146903 + }, + { + "x": 2.032877859008433, + "y": 6.9566191245667985, + "heading": 3.1570436241813726, + "angularVelocity": 0.5573330296555058, + "velocityX": 3.91731977643278, + "velocityY": 0.11499705899340439, + "timestamp": 0.840778377309663 + }, + { + "x": 2.065520465002766, + "y": 6.957557978090485, + "heading": 3.1621336650538163, + "angularVelocity": 0.6161971303075195, + "velocityX": 3.951693246384564, + "velocityY": 0.11365701407324179, + "timestamp": 0.8490387872046357 + }, + { + "x": 2.0984504329177724, + "y": 6.958486799841183, + "heading": 3.1676863130152344, + "angularVelocity": 0.6722000520579139, + "velocityX": 3.9864810988430395, + "velocityY": 0.11244257397721802, + "timestamp": 0.8572991970996083 + }, + { + "x": 2.1316712166001497, + "y": 6.959406758344203, + "heading": 3.1736766917418935, + "angularVelocity": 0.7251914617828771, + "velocityX": 4.021687071799445, + "velocityY": 0.1113695948160438, + "timestamp": 0.865559606994581 + }, + { + "x": 2.165186277981829, + "y": 6.960319169668765, + "heading": 3.1800786598480077, + "angularVelocity": 0.7750182118701426, + "velocityX": 4.057312144046869, + "velocityY": 0.11045593816320934, + "timestamp": 0.8738200168895537 + }, + { + "x": 2.1989990600414004, + "y": 6.961225517299853, + "heading": 3.1868647943382302, + "angularVelocity": 0.8215251514763765, + "velocityX": 4.093354021105941, + "velocityY": 0.10972187126423899, + "timestamp": 0.8820804267845264 + }, + { + "x": 2.2331129552203137, + "y": 6.962127475323903, + "heading": 3.19400637995892, + "angularVelocity": 0.8645558406293681, + "velocityX": 4.1298065849823535, + "velocityY": 0.10919046821108658, + "timestamp": 0.890340836679499 + }, + { + "x": 2.2675312690432237, + "y": 6.963026934685406, + "heading": 3.201473402262516, + "angularVelocity": 0.9039529997346885, + "velocityX": 4.1666593135838506, + "velocityY": 0.10888798170273117, + "timestamp": 0.8986012465744717 + }, + { + "x": 2.302257178726397, + "y": 6.963926031963223, + "heading": 3.2092345401896574, + "angularVelocity": 0.9395584511933502, + "velocityX": 4.203896673978105, + "velocityY": 0.10884414808067534, + "timestamp": 0.9068616564694444 + }, + { + "x": 2.3372936865473584, + "y": 6.964827179782931, + "heading": 3.2172571510612844, + "angularVelocity": 0.9712122005605541, + "velocityX": 4.241497488191654, + "velocityY": 0.10909238538572823, + "timestamp": 0.9151220663644171 + }, + { + "x": 2.372643567633051, + "y": 6.965733097685614, + "heading": 3.225507236598066, + "angularVelocity": 0.9987501397242283, + "velocityX": 4.279434257518503, + "velocityY": 0.10966984861524744, + "timestamp": 0.9233824762593897 + }, + { + "x": 2.4083093114869696, + "y": 6.9666468421059164, + "heading": 3.233949372180996, + "angularVelocity": 1.0219995969049733, + "velocityX": 4.31767240456481, + "velocityY": 0.11061732189065114, + "timestamp": 0.9316428861543624 + }, + { + "x": 2.44429305579275, + "y": 6.967571834188082, + "heading": 3.2425465717794997, + "angularVelocity": 1.0407715486052915, + "velocityX": 4.356169338240552, + "velocityY": 0.11197895672538041, + "timestamp": 0.9399032960493351 + }, + { + "x": 2.480596509367599, + "y": 6.968511884597095, + "heading": 3.251260045781759, + "angularVelocity": 1.054847654420144, + "velocityX": 4.394873140247142, + "velocityY": 0.11380190825438415, + "timestamp": 0.9481637059443078 + }, + { + "x": 2.517220857768171, + "y": 6.969471215284249, + "heading": 3.260048785175124, + "angularVelocity": 1.0639592350877263, + "velocityX": 4.433720464992984, + "velocityY": 0.11613596653794503, + "timestamp": 0.9564241158392804 + }, + { + "x": 2.55416663843719, + "y": 6.970454479070239, + "heading": 3.2688688682505744, + "angularVelocity": 1.0677536814268536, + "velocityX": 4.4726328522150816, + "velocityY": 0.11903329235341958, + "timestamp": 0.9646845257342531 + }, + { + "x": 2.5914335596417093, + "y": 6.971466777824258, + "heading": 3.2776723283732965, + "angularVelocity": 1.065741317277861, + "velocityX": 4.511509922431134, + "velocityY": 0.12254824722845813, + "timestamp": 0.9729449356292258 + }, + { + "x": 2.6290202140808088, + "y": 6.9725136756896955, + "heading": 3.286405336491996, + "angularVelocity": 1.0572124422077387, + "velocityX": 4.550216625687468, + "velocityY": 0.12673679378462502, + "timestamp": 0.9812053455241985 + }, + { + "x": 2.9717276780335036, + "y": 6.984363087388876, + "heading": 3.35108767309623, + "angularVelocity": 0.9203746590757935, + "velocityX": 4.876435853393211, + "velocityY": 0.16860705449782717, + "timestamp": 1.0514836131475132 + }, + { + "x": 3.3366815448761877, + "y": 6.999743371642888, + "heading": 3.3973428408952557, + "angularVelocity": 0.6581717131524953, + "velocityX": 5.192983253355129, + "velocityY": 0.21884836912099448, + "timestamp": 1.1217618807708278 + }, + { + "x": 3.71402485435163, + "y": 7.027526643127482, + "heading": 3.3973428368677423, + "angularVelocity": -5.730809398962461e-8, + "velocityX": 5.369274488921224, + "velocityY": 0.3953323327989187, + "timestamp": 1.1920401483941425 + }, + { + "x": 4.090360227255461, + "y": 7.066661937541599, + "heading": 3.3973428243317834, + "angularVelocity": -1.783760359331945e-7, + "velocityX": 5.354932408421835, + "velocityY": 0.5568619679682045, + "timestamp": 1.2623184160174572 + }, + { + "x": 4.466694898854889, + "y": 7.105803975313799, + "heading": 3.3973428117958613, + "angularVelocity": -1.7837551225317533e-7, + "velocityX": 5.354922429456401, + "velocityY": 0.5569579202208789, + "timestamp": 1.3325966836407719 + }, + { + "x": 4.8430291421579215, + "y": 7.144950130722809, + "heading": 3.397342799228156, + "angularVelocity": -1.7882775854425254e-7, + "velocityX": 5.354916335162799, + "velocityY": 0.5570165106920909, + "timestamp": 1.4028749512640866 + }, + { + "x": 5.214361577079249, + "y": 7.183676617694927, + "heading": 3.378511132398759, + "angularVelocity": -0.26795860891639617, + "velocityX": 5.283744854264688, + "velocityY": 0.55104498562348, + "timestamp": 1.4731532188874013 + }, + { + "x": 5.562524060208366, + "y": 7.219962031584869, + "heading": 3.355414523374288, + "angularVelocity": -0.32864511043819594, + "velocityX": 4.9540561385951625, + "velocityY": 0.5163105909843465, + "timestamp": 1.543431486510716 + }, + { + "x": 5.887470499962918, + "y": 7.253817931255832, + "heading": 3.331666055733143, + "angularVelocity": -0.3379205043646441, + "velocityX": 4.6237115788942855, + "velocityY": 0.48174066914158636, + "timestamp": 1.6137097541340306 + }, + { + "x": 6.189199807150463, + "y": 7.285249171486667, + "heading": 3.308363686841363, + "angularVelocity": -0.33157289841972754, + "velocityX": 4.29335152091094, + "velocityY": 0.4472398266744939, + "timestamp": 1.6839880217573453 + }, + { + "x": 6.467713626267079, + "y": 7.31425833816948, + "heading": 3.2860397702782755, + "angularVelocity": -0.3176503536305448, + "velocityX": 3.963014862708697, + "velocityY": 0.41277577925367004, + "timestamp": 1.75426628938066 + }, + { + "x": 6.723013594809074, + "y": 7.34084703024564, + "heading": 3.265008761402615, + "angularVelocity": -0.29925337642620564, + "velocityX": 3.6327015046867763, + "velocityY": 0.37833448340917136, + "timestamp": 1.8245445570039747 + }, + { + "x": 6.955101065031071, + "y": 7.365016333172441, + "heading": 3.245478308115181, + "angularVelocity": -0.27790174612893576, + "velocityX": 3.302407388098493, + "velocityY": 0.34390863269912847, + "timestamp": 1.8948228246272893 + }, + { + "x": 7.163977127241494, + "y": 7.386767031992689, + "heading": 3.2275957745381954, + "angularVelocity": -0.2544532496565816, + "velocityX": 2.972128785672115, + "velocityY": 0.3094939524808096, + "timestamp": 1.965101092250604 + }, + { + "x": 7.349642665971283, + "y": 7.4060997209757735, + "heading": 3.2114711639593976, + "angularVelocity": -0.22943949992088322, + "velocityX": 2.6418627693690517, + "velocityY": 0.2750877282106925, + "timestamp": 2.035379359873919 + }, + { + "x": 7.512098408217057, + "y": 7.423014865697383, + "heading": 3.1971897253730983, + "angularVelocity": -0.2032127294720464, + "velocityX": 2.3116070976097918, + "velocityY": 0.24068812868678965, + "timestamp": 2.1056576274972336 + }, + { + "x": 7.651344959888608, + "y": 7.43751284079522, + "heading": 3.184819461924971, + "angularVelocity": -0.17601833207433357, + "velocityX": 1.981360047431725, + "velocityY": 0.20629385993890986, + "timestamp": 2.1759358951205483 + }, + { + "x": 7.767382832665261, + "y": 7.449593954241994, + "heading": 3.174415882660127, + "angularVelocity": -0.14803408815663477, + "velocityX": 1.6511202780154728, + "velocityY": 0.1719039733808395, + "timestamp": 2.246214162743863 + }, + { + "x": 7.860212463836391, + "y": 7.459258463658005, + "heading": 3.1660251568616267, + "angularVelocity": -0.11939289459260129, + "velocityX": 1.3208867308552614, + "velocityY": 0.13751775254062693, + "timestamp": 2.3164924303671777 + }, + { + "x": 7.929834231127341, + "y": 7.466506587678373, + "heading": 3.159686290431417, + "angularVelocity": -0.09019668020541635, + "velocityX": 0.9906585584055106, + "velocityY": 0.10313464269233455, + "timestamp": 2.3867706979904924 + }, + { + "x": 7.976248463934981, + "y": 7.471338514116222, + "heading": 3.1554326760982865, + "angularVelocity": -0.06052531567696676, + "velocityX": 0.6604350729932147, + "velocityY": 0.06875420526505516, + "timestamp": 2.457048965613807 + }, + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "angularVelocity": -0.03044253542482025, + "velocityX": 0.33021570985126986, + "velocityY": 0.03437608724320182, + "timestamp": 2.5273272332371217 + }, + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "angularVelocity": -3.79407884851814e-18, + "velocityX": 2.546987187300363e-16, + "velocityY": -1.0256209789219734e-16, + "timestamp": 2.5976055008604364 + }, + { + "x": 7.9777911181971035, + "y": 7.466979623755239, + "heading": 3.153293227446569, + "angularVelocity": 4.0010921973360205e-29, + "velocityX": -0.31261406582002, + "velocityY": -0.09775939742848679, + "timestamp": 2.6669060734735086 + }, + { + "x": 7.934462450883723, + "y": 7.453430059384792, + "heading": 3.153293227446569, + "angularVelocity": -2.051733815514594e-34, + "velocityX": -0.6252281284210741, + "velocityY": -0.1955187938503514, + "timestamp": 2.736206646086581 + }, + { + "x": 7.869469450281316, + "y": 7.433105712944096, + "heading": 3.153293227446569, + "angularVelocity": -1.98522229630168e-34, + "velocityX": -0.9378421873262779, + "velocityY": -0.29327818911646475, + "timestamp": 2.805507218699653 + }, + { + "x": 7.782812116686986, + "y": 7.40600658452606, + "heading": 3.153293227446569, + "angularVelocity": -1.9190015159804115e-34, + "velocityX": -1.2504562419442953, + "velocityY": -0.39103758304190667, + "timestamp": 2.8748077913127252 + }, + { + "x": 7.6744904504495075, + "y": 7.372132674239751, + "heading": 3.153293227446569, + "angularVelocity": -8.200637720207044e-29, + "velocityX": -1.5630702915295283, + "velocityY": -0.4887969753935167, + "timestamp": 2.9441083639257974 + }, + { + "x": 7.544504451984089, + "y": 7.331483982215012, + "heading": 3.153293227446569, + "angularVelocity": 6.610929004011549e-29, + "velocityX": -1.8756843351233508, + "velocityY": -0.5865563658715173, + "timestamp": 3.0134089365388697 + }, + { + "x": 7.392854121793352, + "y": 7.28406050860902, + "heading": 3.153293227446569, + "angularVelocity": -6.483737543755527e-29, + "velocityX": -2.188298371464413, + "velocityY": -0.6843157540814643, + "timestamp": 3.082709509151942 + }, + { + "x": 7.219539460498179, + "y": 7.229862253615935, + "heading": 3.153293227446569, + "angularVelocity": 8.073376116568391e-29, + "velocityX": -2.5009123988461828, + "velocityY": -0.7820751394896976, + "timestamp": 3.152010081765014 + }, + { + "x": 7.024560468885024, + "y": 7.168889217481695, + "heading": 3.153293227446569, + "angularVelocity": -3.526812750348743e-30, + "velocityX": -2.813526414879514, + "velocityY": -0.8798345213490937, + "timestamp": 3.2213106543780863 + }, + { + "x": 6.807917147982327, + "y": 7.101141400527908, + "heading": 3.153293227446569, + "angularVelocity": -9.438045628849687e-29, + "velocityX": -3.1261404160725776, + "velocityY": -0.9775938985677018, + "timestamp": 3.2906112269911585 + }, + { + "x": 6.569609499192503, + "y": 7.026618803193132, + "heading": 3.153293227446569, + "angularVelocity": 9.790681244582267e-29, + "velocityX": -3.438754397028912, + "velocityY": -1.0753532694579622, + "timestamp": 3.3599117996042307 + }, + { + "x": 6.309637524541267, + "y": 6.94532142611084, + "heading": 3.153293227446569, + "angularVelocity": -1.3895984503554753e-34, + "velocityX": -3.7513683487544087, + "velocityY": -1.1731126312072742, + "timestamp": 3.429212372217303 + }, + { + "x": 6.028001227211883, + "y": 6.85724927027649, + "heading": 3.153293227446569, + "angularVelocity": -1.323503512813964e-34, + "velocityX": -4.063982254545725, + "velocityY": -1.2708719785922367, + "timestamp": 3.498512944830375 + }, + { + "x": 5.724700612934229, + "y": 6.762402337481905, + "heading": 3.153293227446569, + "angularVelocity": -2.1989550257903952e-29, + "velocityX": -4.376596077655505, + "velocityY": -1.3686313001213652, + "timestamp": 3.5678135174434473 + }, + { + "x": 5.399735695078014, + "y": 6.660780631908008, + "heading": 3.153293227446569, + "angularVelocity": 1.8024841235265752e-29, + "velocityX": -4.689209707841794, + "velocityY": -1.4663905613202457, + "timestamp": 3.6371140900565195 + }, + { + "x": 5.053106540487171, + "y": 6.552384174457959, + "heading": 3.153293227446569, + "angularVelocity": 1.896211807861914e-29, + "velocityX": -5.001822373477154, + "velocityY": -1.564149520888688, + "timestamp": 3.7064146626695917 + }, + { + "x": 4.7281416226309565, + "y": 6.4507624688840615, + "heading": 3.153293227446569, + "angularVelocity": -1.4997927313419928e-29, + "velocityX": -4.689209707841795, + "velocityY": -1.4663905613202457, + "timestamp": 3.775715235282664 + }, + { + "x": 4.424841008353304, + "y": 6.355915536089477, + "heading": 3.153293227446569, + "angularVelocity": -4.4185947102012545e-35, + "velocityX": -4.376596077655505, + "velocityY": -1.3686313001213652, + "timestamp": 3.845015807895736 + }, + { + "x": 4.14320471102392, + "y": 6.267843380255127, + "heading": 3.153293227446569, + "angularVelocity": -9.264733448000688e-35, + "velocityX": -4.063982254545726, + "velocityY": -1.2708719785922367, + "timestamp": 3.9143163805088084 + }, + { + "x": 3.883232736372683, + "y": 6.186546003172835, + "heading": 3.153293227446569, + "angularVelocity": 3.5265482976182584e-30, + "velocityX": -3.751368348754409, + "velocityY": -1.1731126312072742, + "timestamp": 3.9836169531218806 + }, + { + "x": 3.644925087582859, + "y": 6.112023405838059, + "heading": 3.153293227446569, + "angularVelocity": 2.156768838744686e-29, + "velocityX": -3.438754397028912, + "velocityY": -1.0753532694579622, + "timestamp": 4.052917525734953 + }, + { + "x": 3.4282817666801613, + "y": 6.044275588884272, + "heading": 3.153293227446569, + "angularVelocity": -2.105657187574439e-28, + "velocityX": -3.1261404160725776, + "velocityY": -0.9775938985677018, + "timestamp": 4.122218098348026 + }, + { + "x": 3.2333027750670063, + "y": 5.983302552750032, + "heading": 3.153293227446569, + "angularVelocity": 1.8547117767579628e-28, + "velocityX": -2.813526414879514, + "velocityY": -0.8798345213490937, + "timestamp": 4.1915186709610985 + }, + { + "x": 3.059988113771833, + "y": 5.929104297756947, + "heading": 3.153293227446569, + "angularVelocity": -5.955568710068408e-35, + "velocityX": -2.5009123988461828, + "velocityY": -0.7820751394896976, + "timestamp": 4.260819243574171 + }, + { + "x": 2.908337783581096, + "y": 5.881680824150955, + "heading": 3.153293227446569, + "angularVelocity": -2.0810713408746708e-29, + "velocityX": -2.1882983714644135, + "velocityY": -0.6843157540814642, + "timestamp": 4.330119816187244 + }, + { + "x": 2.7783517851156776, + "y": 5.841032132126216, + "heading": 3.153293227446569, + "angularVelocity": 2.081061415014159e-29, + "velocityX": -1.875684335123351, + "velocityY": -0.5865563658715173, + "timestamp": 4.3994203888003165 + }, + { + "x": 2.6700301188781994, + "y": 5.807158221839908, + "heading": 3.153293227446569, + "angularVelocity": 7.326893682940581e-29, + "velocityX": -1.5630702915295283, + "velocityY": -0.48879697539351663, + "timestamp": 4.468720961413389 + }, + { + "x": 2.583372785283869, + "y": 5.780059093421871, + "heading": 3.153293227446569, + "angularVelocity": -7.326900961882318e-29, + "velocityX": -1.2504562419442953, + "velocityY": -0.39103758304190656, + "timestamp": 4.538021534026462 + }, + { + "x": 2.5183797846814615, + "y": 5.759734746981175, + "heading": 3.153293227446569, + "angularVelocity": -2.64698564160025e-35, + "velocityX": -0.9378421873262781, + "velocityY": -0.2932781891164647, + "timestamp": 4.6073221066395345 + }, + { + "x": 2.4750511173680816, + "y": 5.746185182610728, + "heading": 3.153293227446569, + "angularVelocity": -4.5099737006589516e-29, + "velocityX": -0.6252281284210743, + "velocityY": -0.1955187938503513, + "timestamp": 4.676622679252607 + }, + { + "x": 2.4533867835998535, + "y": 5.739410400390625, + "heading": 3.153293227446569, + "angularVelocity": 1.9207053764808128e-28, + "velocityX": -0.31261406582002027, + "velocityY": -0.09775939742848665, + "timestamp": 4.74592325186568 + }, + { + "x": 2.4533867835998535, + "y": 5.739410400390625, + "heading": 3.153293227446569, + "angularVelocity": -4.899028012023128e-29, + "velocityX": 1.275108562409782e-22, + "velocityY": 3.987692290669026e-23, + "timestamp": 4.815223824478752 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1M Auto.auto b/src/main/deploy/pathplanner/autos/1M Auto.auto deleted file mode 100644 index f0521a12..00000000 --- a/src/main/deploy/pathplanner/autos/1M Auto.auto +++ /dev/null @@ -1,44 +0,0 @@ -{ - "version": 1.0, - "startingPose": null, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "AimAndShoot" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "speakerToCenterNote" - } - }, - { - "type": "named", - "data": { - "name": "Run Intake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "AimAndShoot" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ChoreoTest2.auto b/src/main/deploy/pathplanner/autos/ChoreoTest2.auto new file mode 100644 index 00000000..4f6033f3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/ChoreoTest2.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.3137660622596754, + "y": 6.865104675292969 + }, + "rotation": 179.6548494375836 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Test2.1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "Test2.2" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 00000000..6ce2c5d3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,37 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.4869295358657837, + "y": 5.564249038696289 + }, + "rotation": 179.96604345059157 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Test1.1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "path", + "data": { + "pathName": "Test1.2" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test Auto 1.auto b/src/main/deploy/pathplanner/autos/Test Auto 1.auto deleted file mode 100644 index c8919ec2..00000000 --- a/src/main/deploy/pathplanner/autos/Test Auto 1.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": 1.0, - "startingPose": null, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Test Auto 1" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index 690f5db2..bab0da93 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1,1633 +1 @@ -{ - "field_size": { - "x": 16.54, - "y": 8.21 - }, - "nodeSizeMeters": 0.3, - "grid": [ - [ - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true - ], - [ - true, - true, - true, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true, - true - ], - [ - true, - true, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - false, - true, - true - ], - [ - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true - ], - [ - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true, - true - ] - ] -} +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Test Auto 1.path b/src/main/deploy/pathplanner/paths/Test Auto 1.path deleted file mode 100644 index 26042803..00000000 --- a/src/main/deploy/pathplanner/paths/Test Auto 1.path +++ /dev/null @@ -1,97 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.3676672858340169, - "y": 5.536568320639416 - }, - "prevControl": null, - "nextControl": { - "x": 2.0744483664328044, - "y": 5.527389345566705 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.148388449940052, - "y": 5.536568320639416 - }, - "prevControl": { - "x": 2.5241042231595987, - "y": 5.34928305260528 - }, - "nextControl": { - "x": 3.974496206484089, - "y": 5.784400647602627 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.314118284174416, - "y": 6.316781201819896 - }, - "prevControl": { - "x": 3.827632605320705, - "y": 5.848653473111607 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.15, - "rotationDegrees": 48.06970382251976, - "rotateFast": false - }, - { - "waypointRelativePos": 0, - "rotationDegrees": 179.62305952040285, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [ - { - "name": "Run Intake", - "waypointRelativePos": 0.55, - "command": { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Run Intake" - } - } - ] - } - } - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0.0, - "rotation": 47.91083782616781, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 179.6279547127967, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/speakerToCenterNote.path b/src/main/deploy/pathplanner/paths/speakerToCenterNote.path deleted file mode 100644 index 340e0ca3..00000000 --- a/src/main/deploy/pathplanner/paths/speakerToCenterNote.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.34, - "y": 5.55 - }, - "prevControl": null, - "nextControl": { - "x": 2.34, - "y": 5.55 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.4599653194866877, - "y": 5.55 - }, - "prevControl": { - "x": 1.4599653194866877, - "y": 5.55 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 1.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -179.61351583273776, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 179.7976785591257, - "velocity": 0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2a5f1693..500e5429 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -45,7 +45,7 @@ private Constants() { throw new IllegalStateException("Constants class should not be constructed"); } - public static final Mode currentMode = Mode.REAL; + public static final Mode currentMode = Mode.SIM; public enum Mode { /** Running on a real robot. */ @@ -88,7 +88,7 @@ private DriveConstants() { new Translation3d( Units.inchesToMeters(11.0351), // 11.0351 Units.inchesToMeters(10.023204), // 10.023204 - Units.inchesToMeters(4.1374)), // 4.1374 + Units.inchesToMeters(7.1374)), // 4.1374 new Rotation3d( Units.degreesToRadians(0.0), Units.degreesToRadians(-30.0), // -120.0 + 91.0 @@ -99,7 +99,7 @@ private DriveConstants() { new Translation3d( Units.inchesToMeters(11.0351), //11.0351 Units.inchesToMeters(-10.023204), //-10.023204 - Units.inchesToMeters(4.1374)), // 7.1374 + Units.inchesToMeters(7.1374)), // 7.1374 new Rotation3d( Units.degreesToRadians(0.0), Units.degreesToRadians(-30.0), // -30.0 - 1 @@ -114,7 +114,7 @@ private DriveConstants() { ); public static final HolonomicPathFollowerConfig HOLONOMIC_CONFIG = new HolonomicPathFollowerConfig( - new PIDConstants(1.0),new PIDConstants(1.0), + new PIDConstants(7.5, 0.75), new PIDConstants(7.5, 0.75), DriveConstants.MAX_LINEAR_SPEED * 0.25, DriveConstants.DRIVE_BASE_RADIUS, new ReplanningConfig()); public static final ModuleConstants FL_MOD_CONSTANTS = new ModuleConstants( @@ -177,12 +177,14 @@ private ArmConstants() {} * than the lowest point the arm and wrist will move to , and then compensate for that in our encoder reset code */ public static final double OFFSET_NUDGE = 45; public static final double ARM_OFFSET = -0.123779 + Units.degreesToRotations(OFFSET_NUDGE); - public static final double WRIST_OFFSET = -0.163330 + Units.degreesToRotations(OFFSET_NUDGE); - public static final double ARM_SENSOR_MECHANISM_RATIO = (56.0 / 12.0) * (66.0 / 18.0) * (80.0 / 18.0) * (64.0 / 24.0); + public static final double WRIST_OFFSET = 0.000000 + Units.degreesToRotations(OFFSET_NUDGE); + public static final double ARM_SENSOR_MECHANISM_RATIO = + (56.0 / 12.0) * (66.0 / 18.0) * (80.0 / 18.0) * (64.0 / 24.0); public static final double ARM_CANCODER_MECHANISM_RATIO = (26.0 / 36.0) * (64.0 / 24.0); // the pulley on the encoder is a 1:1 - public static final double WRIST_SENSOR_MECHANISM_RATIO = (56.0 / 12.0) * (66.0 / 18.0) * (80.0 / 18.0) * (48.0 / 24.0); + public static final double WRIST_SENSOR_MECHANISM_RATIO = + (56.0 / 12.0) * (66.0 / 18.0) * (80.0 / 18.0) * (48.0 / 24.0); public static final double WRIST_CANCODER_MECHANISM_RATIO = (48.0 / 24.0); public static final double WRIST_KP = 108.0; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e25bc7b6..3f35eda3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -115,7 +115,7 @@ public void robotPeriodic() { // the Command-based framework to work. CommandScheduler.getInstance().run(); TalonFXFactory.handleFaults(); - DataLogUtil.updateTables(); +// DataLogUtil.updateTables(); } /** This function is called once when the robot is disabled. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6b26f431..d216dec6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,6 +16,7 @@ import com.ctre.phoenix6.SignalLogger; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -182,7 +183,8 @@ public RobotContainer() { private void configureButtonBindings() { Trigger intakeTrigger = controller.y().and(controller.x().negate()) .and(controller.a().negate()) // make sure we don't amp - .and(controller.b().negate()); + .and(controller.b().negate()) + .debounce(0.1, Debouncer.DebounceType.kBoth); Trigger spinUpTrigger = controller.x().and(controller.y().negate()) .and(controller.a().negate()) // make sure we don't amp @@ -190,14 +192,17 @@ private void configureButtonBindings() { Trigger shootTrigger = controller.x().and(controller.y()) .and(controller.a().negate()) // make sure we don't amp - .and(controller.b().negate()); + .and(controller.b().negate()) + .debounce(0.1, Debouncer.DebounceType.kBoth); - Trigger ampLineupTrigger = controller.b().and(controller.a().negate()); + Trigger ampLineupTrigger = controller.b().and(controller.a().negate()) + .debounce(0.1, Debouncer.DebounceType.kBoth); Trigger ampDepositeTrigger = controller.b().and(controller.a()) .and(spinUpTrigger.negate()) // make sure we don't amp while trying to do anything else .and(shootTrigger.negate()) - .and(intakeTrigger.negate()); + .and(intakeTrigger.negate()) + .debounce(0.1, Debouncer.DebounceType.kBoth); intakeTrigger.whileTrue(m_shooter.intakeCommand(0.75, 0.5, 0.1) .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.INTAKE))); @@ -228,7 +233,7 @@ private void configureButtonBindings() { controller.pov(0).onTrue(Commands.runOnce(SignalLogger::stop)); controller.pov(180).whileTrue(m_driveSubsystem.pathfollowFactory( - () -> AllianceFlipUtil.apply(FieldConstants.AMP_LINEUP) + FieldConstants.AMP_LINEUP )); m_driveSubsystem.setDefaultCommand( @@ -266,7 +271,7 @@ private void configureNamedCommands() { } private void configureDashboard() { - ShuffleboardTab commandTab = Shuffleboard.getTab("Commads"); + ShuffleboardTab commandTab = Shuffleboard.getTab("Commands"); commandTab.add("Disable Arm Brake", m_armSubsystem.enableBrakeMode(false)); commandTab.add("Enable Arm Brake", m_armSubsystem.enableBrakeMode(true)); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 1a8e7370..3175eb3a 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -110,6 +110,12 @@ public void periodic() { m_io.setArmAngle(m_desiredArmPoseDegs, m_armVelocityMult); } + Logger.recordOutput("Arm/Arm Setpoint", m_desiredArmPoseDegs); + Logger.recordOutput("Arm/Wrist Setpoint", m_desiredWristPoseDegs); + + Logger.recordOutput("Arm/Arm Velocity Multiplier"); + Logger.recordOutput("Arm/Wrist Velocity Multiplier"); + // Update arm visualizers m_poseVisualizer.update(m_inputs.armPositionDegs, m_inputs.wristPositionDegs); m_setpointVisualizer.update(m_desiredArmPoseDegs, m_desiredWristPoseDegs); diff --git a/src/main/java/frc/robot/subsystems/arm/ArmVisualizer.java b/src/main/java/frc/robot/subsystems/arm/ArmVisualizer.java index dc8ebaf7..906baf6c 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmVisualizer.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmVisualizer.java @@ -13,6 +13,7 @@ import frc.robot.Constants.ArmConstants; import lib.logger.DataLogUtil; import lib.utils.AimbotUtils; +import org.littletonrobotics.junction.Logger; public class ArmVisualizer { private final Mechanism2d mechanism; @@ -33,8 +34,6 @@ public ArmVisualizer(String key, Color color) { wrist = new MechanismLigament2d("wrist", ArmConstants.WRIST_LENGTH_METERS, 45.0, 5, new Color8Bit(color)); root.append(arm); arm.append(wrist); - - DataLogUtil.getTable("Arm").addPose3dArray(key, () -> new Pose3d[]{pivotArm, pivotWrist}, true); } /** Update arm visualizer with current arm angle */ @@ -43,6 +42,8 @@ public void update(double armAngleDegs, double wristAngleDegs) { arm.setAngle(Rotation2d.fromDegrees(armAngleDegs)); wrist.setAngle(Rotation2d.fromDegrees(wristAngleDegs)); + Logger.recordOutput("Arm/" + key + "2d", mechanism); + // Log 3D poses pivotArm = new Pose3d(ArmConstants.PIVOT_JOINT_TRANSLATION.getX(), 0.0, @@ -51,5 +52,7 @@ public void update(double armAngleDegs, double wristAngleDegs) { pivotWrist = new Pose3d(AimbotUtils.getShooterTransformation(armAngleDegs).getTranslation(), new Rotation3d(0.0, Units.degreesToRadians(-wristAngleDegs), 0.0)); + + Logger.recordOutput("Arm/" + key + "3d", pivotArm, pivotWrist); } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 92f07a51..1405fb6a 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -17,7 +17,6 @@ import com.gos.lib.properties.pid.PidProperty; import com.gos.lib.properties.pid.WpiPidPropertyBuilder; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.pathfinding.LocalADStar; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.controller.PIDController; @@ -46,12 +45,14 @@ import frc.robot.subsystems.drive.module.PhoenixOdometryThread; import frc.robot.subsystems.vision.VisionSubsystem; +import java.util.List; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import java.util.function.Supplier; import lib.logger.DataLogUtil; import lib.logger.DataLogUtil.DataLogTable; +import lib.utils.AllianceFlipUtil; import lib.utils.LocalADStarAK; import lib.utils.PoseEstimator; import org.littletonrobotics.junction.AutoLogOutput; @@ -63,17 +64,15 @@ public class DriveSubsystem extends SubsystemBase { public static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; - private final GyroIO.GyroIOInputs gyroInputs = new GyroIO.GyroIOInputs(); + private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); private final Module[] modules = new Module[4]; // FL, FR, BL, BR private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); - private final PoseEstimator m_poseEstimator; private final SwerveDrivePoseEstimator m_wpiPoseEstimator; private final Field2d m_field = new Field2d(); - private Pose2d pose = new Pose2d(); private Rotation2d m_rawGyroRotation = new Rotation2d(); private SwerveModulePosition[] m_lastModulePositions = new SwerveModulePosition[] { new SwerveModulePosition(), @@ -87,22 +86,8 @@ public class DriveSubsystem extends SubsystemBase { private final PIDController m_thetaPid; private final PidProperty m_thetaPidProperty; - private final DataLogTable m_logTable = DataLogUtil.getTable("Swerve"); - private final SysIdRoutine m_sysId; - private final SwerveModuleState[] m_measureStates = new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() - }; - private SwerveModuleState[] m_setpointStates = new SwerveModuleState[] { - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState(), - new SwerveModuleState() - }; private final SwerveModuleState[] m_optimizedStates = new SwerveModuleState[] { new SwerveModuleState(), new SwerveModuleState(), @@ -149,21 +134,20 @@ public DriveSubsystem( .build(); m_cameras = cameras; - m_poseEstimator = - new PoseEstimator(VecBuilder.fill( - Units.inchesToMeters(1.0), - Units.inchesToMeters(1.0), - Units.degreesToRadians(30))); - - m_poseEstimator.resetPose(new Pose2d()); m_wpiPoseEstimator = new SwerveDrivePoseEstimator( kinematics, new Rotation2d(), getModulePositions(), new Pose2d(), - VecBuilder.fill(0.15, 0.15, 0.1), - VecBuilder.fill(1.0, 1.0, 1.25) + VecBuilder.fill( + Units.inchesToMeters(0.5), + Units.inchesToMeters(0.5), + Units.degreesToRadians(0.75)), + VecBuilder.fill( + Units.inchesToMeters(4.0), + Units.inchesToMeters(4.0), + Units.degreesToRadians(25.0)) ); // Configure AutoBuilder for PathPlanner @@ -179,13 +163,12 @@ public DriveSubsystem( this); Pathfinding.setPathfinder(new LocalADStarAK()); PathPlannerLogging.setLogActivePathCallback( - (activePath) -> Logger.recordOutput( + (List activePath) -> Logger.recordOutput( "Odometry/Trajectory", activePath.toArray(new Pose2d[0]))); PathPlannerLogging.setLogTargetPoseCallback( - (targetPose) -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose)); + (Pose2d targetPose) -> Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose)); // turn on logging -// setupLogging(); SmartDashboard.putData("Field",m_field); m_sysId = new SysIdRoutine( @@ -207,6 +190,7 @@ public DriveSubsystem( public void periodic() { odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); + Logger.processInputs("Swerve/Gyro", gyroInputs); for (var module : modules) { module.updateInputs(); } @@ -254,18 +238,29 @@ public void periodic() { m_wpiPoseEstimator.updateWithTime(sampleTimestamps[i], m_rawGyroRotation, modulePositions); } - for (VisionSubsystem camera : m_cameras) { - camera.updateInputs(); - camera.getPose(m_wpiPoseEstimator.getEstimatedPosition()).ifPresent( - (PoseEstimator.TimestampedVisionUpdate pose) -> - m_wpiPoseEstimator.addVisionMeasurement(pose.pose(), pose.timestamp(), pose.stdDevs()) - ); + // make sure we're not moving too fast before trying to update vision poses + if ((kinematics.toChassisSpeeds(getModuleStates()).vxMetersPerSecond <= Units.inchesToMeters(20)) + && (kinematics.toChassisSpeeds(getModuleStates()).vyMetersPerSecond <= Units.inchesToMeters(20)) + && (kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond <= Units.degreesToRadians(60))) { + for (VisionSubsystem camera : m_cameras) { + camera.updateInputs(); + camera.getPose(m_wpiPoseEstimator.getEstimatedPosition()).ifPresent( + (PoseEstimator.TimestampedVisionUpdate pose) -> + m_wpiPoseEstimator.addVisionMeasurement(pose.pose(), pose.timestamp(), pose.stdDevs()) + ); + } } m_wpiPoseEstimator.updateWithTime(Timer.getFPGATimestamp(), gyroInputs.yawPosition, getModulePositions()); m_thetaPidProperty.updateIfChanged(); m_field.setRobotPose(getVisionPose()); + + Logger.recordOutput("Swerve/LeftCamPose", + new Pose3d(getVisionPose()).plus(DriveConstants.LEFT_CAMERA_TRANSFORMATION)); + + Logger.recordOutput("Swerve/RightCamPose", + new Pose3d(getVisionPose()).plus(DriveConstants.RIGHT_CAMERA_TRANSFORMATION)); } /** @@ -276,7 +271,7 @@ public void periodic() { public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); - m_setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveModuleState[] m_setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); SwerveDriveKinematics.desaturateWheelSpeeds(m_setpointStates, DriveConstants.MAX_LINEAR_SPEED); // Send setpoints to modules @@ -284,6 +279,9 @@ public void runVelocity(ChassisSpeeds speeds) { // The module returns the optimized state, useful for logging m_optimizedStates[i] = modules[i].runSetpoint(m_setpointStates[i]); } + + Logger.recordOutput("SwerveStates/Setpoints", m_setpointStates); + Logger.recordOutput("SwerveStates/Optimized", m_optimizedStates); } /** @@ -353,12 +351,6 @@ private SwerveModulePosition[] getModulePositions() { return positions; } - /** Returns the current odometry pose. */ - @AutoLogOutput(key = "Odometry/Robot") - public Pose2d getPose() { - return pose; - } - /** Returns the robot pose with vision updates */ @AutoLogOutput(key = "Odometry/RobotVision") public Pose2d getVisionPose() { @@ -386,8 +378,9 @@ public double getMaxAngularSpeedRadPerSec() { return DriveConstants.MAX_ANGULAR_SPEED; } - public Command pathfollowFactory(Supplier pose) { - return AutoBuilder.pathfindToPose(pose.get(), DriveConstants.DEFAULT_CONSTRAINTS); + public Command pathfollowFactory(Pose2d pose) { + return AutoBuilder.pathfindToPoseFlipped( + pose, DriveConstants.DEFAULT_CONSTRAINTS); } /** Returns an array of module translations. */ @@ -407,24 +400,4 @@ public Command runSysidQuasistatic(SysIdRoutine.Direction direction) { public Command runSysidDynamic(SysIdRoutine.Direction direction) { return m_sysId.dynamic(direction); } - - public void setupLogging() { - for (int i = 0; i < modules.length; i++) { - Module module = modules[i]; - String moduleName = "Module" + i; - - m_logTable.addDouble(moduleName + "/AzimuthPose", () -> module.getAngle().getDegrees(), true); - m_logTable.addDouble(moduleName + "/DrivePose", module::getPositionMeters, true); - - m_logTable.addDouble(moduleName + "/VelocityMPS", module::getVelocityMetersPerSec, true); - } - - m_logTable.addPose2d("Vision Pose", this::getVisionPose, true); - - m_logTable.addSwerveModuleStateArray("MeasuredStates", () -> m_measureStates, true); - m_logTable.addSwerveModuleStateArray("SetpointStates", () -> m_setpointStates, true); - m_logTable.addSwerveModuleStateArray("OptimizedStates", () -> m_optimizedStates, true); - - m_logTable.addDouble("GyroHeading", () -> gyroInputs.yawPosition.getDegrees(), true); - } } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 49e398e2..1d386ae1 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -57,7 +57,7 @@ public void setYawVelocityRadPerSec(double yawVelocityRadPerSec) { } } - default void updateInputs(GyroIOInputs inputs) {} + default void updateInputs(GyroIOInputsAutoLogged inputs) {} default void resetGyro(double degrees) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java index 183004ef..dea3555a 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon1.java @@ -21,7 +21,7 @@ public GyroIOPigeon1(int id) { } @Override - public void updateInputs(GyroIOInputs inputs) { + public void updateInputs(GyroIOInputsAutoLogged inputs) { inputs.setConnected(m_pigeon.getState() == PigeonIMU.PigeonState.Ready); inputs.setYawPosition(Rotation2d.fromDegrees(m_pigeon.getYaw())); inputs.setYawVelocityRadPerSec((m_pigeon.getYaw() - prevYaw) / Units.millisecondsToSeconds(20)); diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 2476c75f..1b359561 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -50,7 +50,7 @@ public GyroIOPigeon2(boolean phoenixDrive) { } @Override - public void updateInputs(GyroIOInputs inputs) { + public void updateInputs(GyroIOInputsAutoLogged inputs) { inputs.setConnected(BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK)); inputs.setYawPosition(Rotation2d.fromDegrees(yaw.getValueAsDouble())); inputs.setYawVelocityRadPerSec(Units.degreesToRadians(yawVelocity.getValueAsDouble())); diff --git a/src/main/java/frc/robot/subsystems/drive/module/Module.java b/src/main/java/frc/robot/subsystems/drive/module/Module.java index 1bfd7b22..e2c7ba34 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/module/Module.java @@ -60,7 +60,7 @@ public void updateInputs() { } public void periodic() { - Logger.processInputs("Module" + m_index, m_inputs); + Logger.processInputs("Swerve/Module" + m_index, m_inputs); // On first cycle, reset relative turn encoder // Wait until absolute angle is nonzero in case it wasn't initialized yet @@ -77,12 +77,9 @@ public void periodic() { if (m_speedSetpoint != null) { // Run drive controller m_io.setDriveVelocityMPS(m_speedSetpoint); - SmartDashboard.putNumber("Module" + m_index + "/Desired MPS", m_speedSetpoint); } } - SmartDashboard.putNumber("Module" + m_index + "/Actual MPS", m_inputs.driveVelocityRadPerSec * WHEEL_RADIUS); - // Calculate positions for odometry int sampleCount = m_inputs.odometryTimestamps.length; // All signals are sampled together m_odometryPositions = new SwerveModulePosition[sampleCount]; diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java index 38e26258..8360be1c 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java @@ -32,8 +32,11 @@ public class ModuleIOSim implements ModuleIO { private static final double LOOP_PERIOD_SECS = 0.02; - private DCMotorSim driveSim = new DCMotorSim(DCMotor.getKrakenX60(1), ModuleConstants.GearRatios.L3.ratio, 0.025); - private DCMotorSim turnSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), ModuleConstants.GearRatios.TURN.ratio, 0.004); + private final DCMotorSim driveSim = + new DCMotorSim(DCMotor.getKrakenX60(1), ModuleConstants.GearRatios.L3_KRAKEN.ratio, 0.025); + + private final DCMotorSim turnSim = + new DCMotorSim(DCMotor.getFalcon500Foc(1), ModuleConstants.GearRatios.TURN.ratio, 0.004); private final Rotation2d turnAbsoluteInitPosition = new Rotation2d(Math.random() * 2.0 * Math.PI); private double driveAppliedVolts = 0.0; @@ -69,6 +72,7 @@ public void updateInputs(ModuleIOInputsAutoLogged inputs) { inputs.turnAppliedVolts = turnAppliedVolts; inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; + inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; } diff --git a/src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java b/src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java index 92d2619c..e859e47f 100644 --- a/src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java +++ b/src/main/java/frc/robot/subsystems/vision/PhotonVisionIOInputs.java @@ -1,5 +1,8 @@ package frc.robot.subsystems.vision; +import org.littletonrobotics.junction.AutoLog; + +@AutoLog public class PhotonVisionIOInputs { protected boolean camConnected = false; protected double camLatency = 0.0; diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index a36cec64..82ee25bf 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -12,8 +12,10 @@ import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; import lib.logger.DataLogUtil; import lib.utils.PoseEstimator; +import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -31,13 +33,13 @@ public class VisionSubsystem { private final String m_name; private Pose3d m_lastEstimatedPose = new Pose3d(); - private final double xyStdDevCoefficient = 0.03; - private final double thetaStdDevCoefficient = 0.05; + private final double xyStdDevCoefficient = Units.inchesToMeters(8.0); + private final double thetaStdDevCoefficient = Units.degreesToRadians(24.0); - private final double xyStdDevMultiTagCoefficient = 0.02; - private final double thetaStdDevMultiTagCoefficient = 0.04; + private final double xyStdDevMultiTagCoefficient = Units.inchesToMeters(4.0); + private final double thetaStdDevMultiTagCoefficient = Units.degreesToRadians(12.0); - private final PhotonVisionIOInputs inputs = new PhotonVisionIOInputs(); + private final PhotonVisionIOInputsAutoLogged inputs = new PhotonVisionIOInputsAutoLogged(); public VisionSubsystem(String camName, Transform3d camPose) { m_camera = new PhotonCamera(camName); @@ -51,12 +53,10 @@ public VisionSubsystem(String camName, Transform3d camPose) { m_camera, robotToCam); - m_photonPoseEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY); + m_photonPoseEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE); } catch (IOException e){ throw new IllegalStateException(e); } - - DataLogUtil.getTable(camName).addPose3d("Estimated Pose3d", () -> m_lastEstimatedPose, true); } public void updateInputs() { @@ -65,6 +65,18 @@ public void updateInputs() { inputs.setTagIds(getTargetIDs()); inputs.setNumTagsFound(m_camera.getLatestResult().targets.size()); inputs.setTagsFound(m_camera.getLatestResult().hasTargets()); + + Logger.processInputs("Vision/" + m_name, inputs); + } + + public double getTagDistance(){ + var latestResult = m_camera.getLatestResult(); + if (latestResult.hasTargets()){ + var bestTarget = latestResult.getBestTarget(); + var camToTarget = bestTarget.getBestCameraToTarget(); + return camToTarget.getX(); + } + return -1; } public Optional getPose(Pose2d prevEstimatedRobotPose) { @@ -72,6 +84,7 @@ public Optional getPose(Pose2d prevEstima m_photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); PhotonPipelineResult camResult = m_camera.getLatestResult(); + Optional opPose = m_photonPoseEstimator.update(camResult); if (opPose.isEmpty()) { @@ -80,20 +93,36 @@ public Optional getPose(Pose2d prevEstima EstimatedRobotPose estPose = opPose.get(); m_lastEstimatedPose = estPose.estimatedPose; - double distance = estPose.estimatedPose.toPose2d().getTranslation().getDistance( - robotToCam.getTranslation().toTranslation2d()); + // find average distance to tags + int numTags = 0; + double avgDist = 0; + for (var tgt : camResult.targets) { + var tagPose = m_photonPoseEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + + if (tagPose.isEmpty()) { + continue; + } + + numTags++; + avgDist += + tagPose.get().toPose2d().getTranslation().getDistance(estPose.estimatedPose.toPose2d().getTranslation()); + } + + avgDist /= numTags; double xyStdDev; double thetaStdDev; if (estPose.strategy != PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR) { - xyStdDev = xyStdDevCoefficient * Math.pow(distance, 2.0); - thetaStdDev = thetaStdDevCoefficient * Math.pow(distance, 2.0); + xyStdDev = xyStdDevCoefficient * Math.pow(avgDist, 2.0); + thetaStdDev = thetaStdDevCoefficient * Math.pow(avgDist, 2.0); } else { - xyStdDev = xyStdDevMultiTagCoefficient * Math.pow(distance, 2.0); - thetaStdDev = thetaStdDevMultiTagCoefficient * Math.pow(distance, 2.0); + xyStdDev = xyStdDevMultiTagCoefficient * Math.pow(avgDist, 2.0); + thetaStdDev = thetaStdDevMultiTagCoefficient * Math.pow(avgDist, 2.0); } + Logger.recordOutput("Vision/" + m_name + "/Estimated Pose", estPose.estimatedPose); + return Optional.of(new PoseEstimator.TimestampedVisionUpdate(estPose.timestampSeconds, estPose.estimatedPose.toPose2d(), VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev))); diff --git a/src/main/java/lib/utils/FieldConstants.java b/src/main/java/lib/utils/FieldConstants.java index 7e800aa2..4bee1313 100644 --- a/src/main/java/lib/utils/FieldConstants.java +++ b/src/main/java/lib/utils/FieldConstants.java @@ -30,11 +30,11 @@ private FieldConstants() {} new Translation2d(Units.inchesToMeters(72.455), Units.inchesToMeters(322.996)); public static final Pose2d AMP_LINEUP = - new Pose2d( - AMP_CENTER.getX() - Units.inchesToMeters(10), - AMP_CENTER.getY(), + AllianceFlipUtil.apply(new Pose2d( + AMP_CENTER.getX(), + AMP_CENTER.getY() - Units.inchesToMeters(20), Rotation2d.fromDegrees(90) - ); + )); /** Staging locations for each note */ public static final class StagingLocations {