diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 2aaab015..3294b25d 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -4,8 +4,8 @@ "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 1.5, + "defaultMaxAccel": 1.5, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 4.5 diff --git a/Crescendo2024.chor b/Crescendo2024.chor index a180f910..558b1600 100644 --- a/Crescendo2024.chor +++ b/Crescendo2024.chor @@ -3,8 +3,8 @@ "robotConfiguration": { "mass": 74.08797700309194, "rotationalInertia": 6, - "motorMaxTorque": 0.7759834368530021, - "motorMaxVelocity": 5800, + "motorMaxTorque": 1.7, + "motorMaxVelocity": 1750, "gearing": 5.731, "wheelbase": 0.4724397448825378, "trackWidth": 0.4724397448825378, @@ -31,7 +31,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 28 + "controlIntervalCount": 26 }, { "x": 2.1113932132720947, @@ -40,7 +40,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 25 + "controlIntervalCount": 23 }, { "x": 2.59393310546875, @@ -49,7 +49,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 25 + "controlIntervalCount": 23 }, { "x": 1.941084861755371, @@ -58,7 +58,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 23 + "controlIntervalCount": 21 }, { "x": 2.5655484199523926, @@ -75,1063 +75,991 @@ "x": 1.4869295358657837, "y": 5.564249038696289, "heading": 3.141, - "angularVelocity": 8.029589971503427e-22, - "velocityX": 1.7351458241819956e-21, - "velocityY": 6.201477276822387e-22, + "angularVelocity": 3.7405663636389993e-19, + "velocityX": -1.1249906625590385e-18, + "velocityY": 2.3803295269896554e-19, "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, + "x": 1.5092825889004946, + "y": 5.579306288055266, + "heading": 3.4062283948642453, + "angularVelocity": 3.352121601432042, + "velocityX": 0.282511802606821, + "velocityY": 0.19030289294706518, + "timestamp": 0.07912254577845217 + }, + { + "x": 1.5673643497428493, + "y": 5.60987650843196, + "heading": 3.626081158643754, + "angularVelocity": 2.77863612218835, + "velocityX": 0.7340734587204384, + "velocityY": 0.38636547997701703, + "timestamp": 0.15824509155690433 + }, + { + "x": 1.639918967505268, + "y": 5.662681063987355, + "heading": 3.7707265189248944, + "angularVelocity": 1.828118128126927, + "velocityX": 0.9169904361466855, + "velocityY": 0.6673768523986875, + "timestamp": 0.23736763733535649 + }, + { + "x": 1.719103738696596, + "y": 5.732106202864848, + "heading": 3.861411095653706, + "angularVelocity": 1.1461281463659418, + "velocityX": 1.000786443513203, + "velocityY": 0.8774381333973873, + "timestamp": 0.31649018311380867 + }, + { + "x": 1.8011465793982657, + "y": 5.8124573886259565, + "heading": 3.9165727578512475, + "angularVelocity": 0.6971674338183805, + "velocityX": 1.0369085055907419, + "velocityY": 1.0155283171258929, + "timestamp": 0.39561272889226085 + }, + { + "x": 1.8844011608490543, + "y": 5.899562457377884, + "heading": 3.9495869202931786, + "angularVelocity": 0.41725354154267424, + "velocityX": 1.0522232396807132, + "velocityY": 1.1008880957373124, + "timestamp": 0.474735274670713 + }, + { + "x": 1.968168332271066, + "y": 5.990724119399961, + "heading": 3.96923386729424, + "angularVelocity": 0.24831034956931133, + "velocityX": 1.0587016709063517, + "velocityY": 1.1521578473642025, + "timestamp": 0.5538578204491652 + }, + { + "x": 2.05213613980173, + "y": 6.084290626509761, + "heading": 3.9810005346780404, + "angularVelocity": 0.1487144690307097, + "velocityX": 1.0612374349756033, + "velocityY": 1.1825517769839073, + "timestamp": 0.6329803662276173 + }, + { + "x": 2.1361397889017213, + "y": 6.179275610733387, + "heading": 3.988261371091814, + "angularVelocity": 0.0917669716303672, + "velocityX": 1.0616904230458737, + "velocityY": 1.2004793739775566, + "timestamp": 0.7121029120060695 + }, + { + "x": 2.2200564987148903, + "y": 6.2750981229467735, + "heading": 3.9931304198236632, + "angularVelocity": 0.061538069635463585, + "velocityX": 1.0605916302053886, + "velocityY": 1.2110645742073904, + "timestamp": 0.7912254577845216 + }, + { + "x": 2.3037490732526433, + "y": 6.3714204686821265, + "heading": 3.997036412999259, + "angularVelocity": 0.049366373859271756, + "velocityX": 1.0577588690345867, + "velocityY": 1.217381781484405, + "timestamp": 0.8703480035629737 + }, + { + "x": 2.3870186605553965, + "y": 6.4680502574631, + "heading": 4.001128510533661, + "angularVelocity": 0.05171847662561717, + "velocityX": 1.0524128929813872, + "velocityY": 1.221267438127472, + "timestamp": 0.9494705493414258 + }, + { + "x": 2.4696215654523987, + "y": 6.564870888609008, + "heading": 4.006432047635611, + "angularVelocity": 0.06702940419537926, + "velocityX": 1.0439869456209716, + "velocityY": 1.2236794227654333, + "timestamp": 1.028593095119878 + }, + { + "x": 2.5522006651677756, + "y": 6.661687676467445, + "heading": 4.0117852212202445, + "angularVelocity": 0.06765674097018534, + "velocityX": 1.043686080913052, + "velocityY": 1.223630848905317, + "timestamp": 1.1077156408983302 + }, + { + "x": 2.626171983754375, + "y": 6.756004010665758, + "heading": 4.047305777483365, + "angularVelocity": 0.44893090728627943, + "velocityX": 0.9348955832857486, + "velocityY": 1.1920285586159498, + "timestamp": 1.1868381866767823 + }, + { + "x": 2.664895057678222, "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, + "angularVelocity": -1.9113735382816162, + "velocityX": 0.4894063195625013, + "velocityY": 0.7225771600506798, + "timestamp": 1.2659607324552344 + }, + { + "x": 2.67401621750823, + "y": 6.8287147997218245, + "heading": 3.8216991351700837, + "angularVelocity": -2.7569472556880648, + "velocityX": 0.338109953564034, + "velocityY": 0.5759980652369127, + "timestamp": 1.2929376366660208 + }, + { + "x": 2.6786727556459358, + "y": 6.8400421309827735, + "heading": 3.72670670460024, + "angularVelocity": -3.521250245306635, + "velocityX": 0.17261202773019518, + "velocityY": 0.41988996114756666, + "timestamp": 1.3199145408768072 + }, + { + "x": 2.678518823316929, + "y": 6.846619952079936, + "heading": 3.614568928899329, + "angularVelocity": -4.15680668266135, + "velocityX": -0.00570607834774554, + "velocityY": 0.24383157703223057, + "timestamp": 1.3468914450875935 + }, + { + "x": 2.6741686792707893, + "y": 6.846169187557711, + "heading": 3.4948207380964975, + "angularVelocity": -4.438915224191635, + "velocityX": -0.16125438308832984, + "velocityY": -0.0167092754120811, + "timestamp": 1.3738683492983799 + }, + { + "x": 2.663571596898345, + "y": 6.842490941859769, + "heading": 3.391106479438399, + "angularVelocity": -3.844557472114035, + "velocityX": -0.3928205508549501, + "velocityY": -0.1363479541318252, + "timestamp": 1.4008452535091662 + }, + { + "x": 2.648321534336774, + "y": 6.83512618854535, + "heading": 3.3066791856345255, + "angularVelocity": -3.129613878011262, + "velocityX": -0.5653006898944218, + "velocityY": -0.2730021672197161, + "timestamp": 1.4278221577199526 + }, + { + "x": 2.630755187717048, + "y": 6.821913643765559, + "heading": 3.2403542901418882, + "angularVelocity": -2.4585806797690686, + "velocityX": -0.6511624344467275, + "velocityY": -0.4897724615306165, + "timestamp": 1.454799061930739 + }, + { + "x": 2.610177790375068, + "y": 6.803317677931451, + "heading": 3.192043949934185, + "angularVelocity": -1.7908037123240415, + "velocityX": -0.7627783077407181, + "velocityY": -0.6893291271973806, + "timestamp": 1.4817759661415253 + }, + { + "x": 2.585880089062531, + "y": 6.779956801667491, + "heading": 3.1617447456020207, + "angularVelocity": -1.1231534980960616, + "velocityX": -0.9006853092812849, + "velocityY": -0.8659583798581432, + "timestamp": 1.5087528703523116 + }, + { + "x": 2.557481302008236, + "y": 6.752192795634918, + "heading": 3.1494506971418432, + "angularVelocity": -0.4557249550980476, + "velocityX": -1.0527074134380248, + "velocityY": -1.029176877214801, + "timestamp": 1.535729774563098 + }, + { + "x": 2.5262145192081267, + "y": 6.721488912807054, + "heading": 3.1494498734965517, + "angularVelocity": -0.00003053149779678828, + "velocityX": -1.1590204181995478, + "velocityY": -1.1381544223143767, + "timestamp": 1.5627066787738844 + }, + { + "x": 2.494951526671565, + "y": 6.690781099940809, + "heading": 3.1494491983381225, + "angularVelocity": -0.000025027276073123234, + "velocityX": -1.1588799178840683, + "velocityY": -1.1383001039076166, + "timestamp": 1.5896835829846707 + }, + { + "x": 2.46368853251831, + "y": 6.660073288720487, + "heading": 3.149448523179691, + "angularVelocity": -0.000025027276154336604, + "velocityX": -1.1588799778128431, + "velocityY": -1.138300042895271, + "timestamp": 1.616660487195457 + }, + { + "x": 2.4324255384011386, + "y": 6.629365477463429, + "heading": 3.1494478480212726, + "angularVelocity": -0.000025027275679943262, + "velocityX": -1.1588799764752862, + "velocityY": -1.138300044257102, + "timestamp": 1.6436373914062434 + }, + { + "x": 2.401162544284488, + "y": 6.5986576662058365, + "heading": 3.1494471728628666, + "angularVelocity": -0.000025027275214919653, + "velocityX": -1.1588799764559639, + "velocityY": -1.1383000442768603, + "timestamp": 1.6706142956170298 + }, + { + "x": 2.3698995501679407, + "y": 6.567949854948137, + "heading": 3.1494464977044734, + "angularVelocity": -0.000025027274749328804, + "velocityX": -1.1588799764521394, + "velocityY": -1.1383000442808404, + "timestamp": 1.6975911998278161 + }, + { + "x": 2.3386365560514903, + "y": 6.537242043690337, + "heading": 3.1494458225460926, + "angularVelocity": -0.000025027274284044715, + "velocityX": -1.1588799764485511, + "velocityY": -1.1383000442845796, + "timestamp": 1.7245681040386025 + }, + { + "x": 2.307373561935169, + "y": 6.5065342324324025, + "heading": 3.1494451473877243, + "angularVelocity": -0.00002502727381983999, + "velocityX": -1.1588799764437645, + "velocityY": -1.1383000442895392, + "timestamp": 1.7515450082493889 + }, + { + "x": 2.2761105678205396, + "y": 6.475826421172744, + "heading": 3.1494444722293684, + "angularVelocity": -0.00002502727335405626, + "velocityX": -1.158879976381056, + "velocityY": -1.1383000443534677, + "timestamp": 1.7785219124601752 + }, + { + "x": 2.244847573837531, + "y": 6.445118609779082, + "heading": 3.1494437970710263, + "angularVelocity": -0.000025027272855365495, + "velocityX": -1.1588799715020328, + "velocityY": -1.138300049320807, + "timestamp": 1.8054988166709616 + }, + { + "x": 2.2135845753549397, + "y": 6.414410802966355, + "heading": 3.149443121912666, + "angularVelocity": -0.000025027273518628322, + "velocityX": -1.1588801382959264, + "velocityY": -1.1382998795112838, + "timestamp": 1.832475720881748 + }, + { + "x": 2.182332647777799, + "y": 6.3836917291800725, + "heading": 3.14944244669543, + "angularVelocity": -0.000025029455959603075, + "velocityX": -1.1584697537175366, + "velocityY": -1.1387175320883667, + "timestamp": 1.8594526250925343 + }, + { + "x": 2.153094421241092, + "y": 6.351050761951557, + "heading": 3.149440476891046, + "angularVelocity": -0.00007301817765136917, + "velocityX": -1.0838243820804343, + "velocityY": -1.2099597112211005, + "timestamp": 1.8864295293033206 + }, + { + "x": 2.1314747036692956, + "y": 6.314801767366594, + "heading": 3.1444623769659996, + "angularVelocity": -0.18453191983123554, + "velocityX": -0.801415811201077, + "velocityY": -1.3437047595123932, + "timestamp": 1.913406433514107 + }, + { + "x": 2.1175303161219627, + "y": 6.274679449481932, + "heading": 3.139999999999996, + "angularVelocity": -0.16541471664709217, + "velocityX": -0.5169009549193061, + "velocityY": -1.487283995641412, + "timestamp": 1.9403833377248934 + }, + { + "x": 2.111393213272095, "y": 6.231289386749268, + "heading": 3.139999999999996, + "angularVelocity": 1.2396255317011e-17, + "velocityX": -0.22749470442979314, + "velocityY": -1.6084151981870152, + "timestamp": 1.9673602419356797 + }, + { + "x": 2.1135515859889713, + "y": 6.18871804241084, + "heading": 3.139999999999996, + "angularVelocity": 4.342266970142451e-18, + "velocityX": 0.08225287430098827, + "velocityY": -1.6223404824025023, + "timestamp": 1.9936009385096722 + }, + { + "x": 2.1237593069102005, + "y": 6.1473322849843255, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422681619637e-18, + "velocityX": 0.3890034280317459, + "velocityY": -1.5771592537498764, + "timestamp": 2.0198416350836648 + }, + { + "x": 2.141644452417822, + "y": 6.1086399046863455, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422691347276405e-18, + "velocityX": 0.6815804396497286, + "velocityY": -1.4745180330437861, + "timestamp": 2.0460823316576575 + }, + { + "x": 2.1665552441015765, + "y": 6.0740504370872355, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422696264537505e-18, + "velocityX": 0.9493189943912999, + "velocityY": -1.3181611814907124, + "timestamp": 2.0723230282316503 + }, + { + "x": 2.1935012145011026, + "y": 6.041021760141006, + "heading": 3.139999999999997, + "angularVelocity": 4.34227316559682e-18, + "velocityX": 1.0268770999862573, + "velocityY": -1.2586814093559167, + "timestamp": 2.098563724805643 + }, + { + "x": 2.220442534619127, + "y": 6.00798928987496, + "heading": 3.139999999999997, + "angularVelocity": 4.342268965346379e-18, + "velocityX": 1.0266998835971468, + "velocityY": -1.2588259680112164, + "timestamp": 2.1248044213796358 + }, + { + "x": 2.247383856273639, + "y": 5.974956820862076, + "heading": 3.1399999999999975, + "angularVelocity": 4.342269673161453e-18, + "velocityX": 1.0266999421507437, + "velocityY": -1.2588259202547785, + "timestamp": 2.1510451179536285 + }, + { + "x": 2.274325177898134, + "y": 5.941924351824711, + "heading": 3.1399999999999975, + "angularVelocity": 4.342269691700593e-18, + "velocityX": 1.0266999410068467, + "velocityY": -1.2588259211877415, + "timestamp": 2.1772858145276213 + }, + { + "x": 2.3012664995222347, + "y": 5.908891882787024, + "heading": 3.1399999999999975, + "angularVelocity": 4.3422697108078704e-18, + "velocityX": 1.0266999409917994, + "velocityY": -1.258825921200014, + "timestamp": 2.203526511101614 + }, + { + "x": 2.328207821146328, + "y": 5.87585941374933, + "heading": 3.139999999999998, + "angularVelocity": 4.3422696965810506e-18, + "velocityX": 1.0266999409915183, + "velocityY": -1.2588259212002433, + "timestamp": 2.2297672076756068 + }, + { + "x": 2.355149142770421, + "y": 5.8428269447116365, + "heading": 3.139999999999998, + "angularVelocity": 4.342269684490725e-18, + "velocityX": 1.0266999409915205, + "velocityY": -1.2588259212002417, + "timestamp": 2.2560079042495995 + }, + { + "x": 2.3820904643945138, + "y": 5.809794475673942, + "heading": 3.139999999999998, + "angularVelocity": 4.342269693460261e-18, + "velocityX": 1.026699940991488, + "velocityY": -1.258825921200268, + "timestamp": 2.2822486008235923 + }, + { + "x": 2.4090317860186072, + "y": 5.7767620066362495, + "heading": 3.1399999999999983, + "angularVelocity": 4.342269692405428e-18, + "velocityX": 1.026699940991538, + "velocityY": -1.258825921200227, + "timestamp": 2.308489297397585 + }, + { + "x": 2.4359731076427, + "y": 5.743729537598556, + "heading": 3.1399999999999983, + "angularVelocity": 4.342269716594298e-18, + "velocityX": 1.0266999409914905, + "velocityY": -1.258825921200266, + "timestamp": 2.3347299939715778 + }, + { + "x": 2.4629144292667973, + "y": 5.710697068560865, + "heading": 3.139999999999999, + "angularVelocity": 4.342269681682682e-18, + "velocityX": 1.026699940991683, + "velocityY": -1.258825921200109, + "timestamp": 2.3609706905455705 + }, + { + "x": 2.4898557508911936, + "y": 5.677664599523419, + "heading": 3.139999999999999, + "angularVelocity": 4.3422696993715505e-18, + "velocityX": 1.0266999410030797, + "velocityY": -1.2588259211908142, + "timestamp": 2.3872113871195633 + }, + { + "x": 2.5167970725019893, + "y": 5.644632130474532, + "heading": 3.139999999999999, + "angularVelocity": 4.342269692409483e-18, + "velocityX": 1.026699940484769, + "velocityY": -1.2588259216268345, + "timestamp": 2.413452083693556 + }, + { + "x": 2.5425090771755396, + "y": 5.613106913889164, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374488911e-18, + "velocityX": 0.9798522154718704, + "velocityY": -1.2013864226688296, + "timestamp": 2.4396927802675488 + }, + { + "x": 2.563078686478754, + "y": 5.587886733807481, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374369648e-18, + "velocityX": 0.7838819844288143, + "velocityY": -0.9611093977847911, + "timestamp": 2.4659334768415415 + }, + { + "x": 2.578505895291019, + "y": 5.568971596499493, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374398685e-18, + "velocityX": 0.5879115582455986, + "velocityY": -0.7208321339584491, + "timestamp": 2.4921741734155343 + }, + { + "x": 2.5887907019054897, + "y": 5.5563615040551815, + "heading": 3.1399999999999997, + "angularVelocity": 4.342265374398075e-18, + "velocityX": 0.39194106701667647, + "velocityY": -0.48055479048560223, + "timestamp": 2.518414869989527 + }, + { + "x": 2.5939331054687496, + "y": 5.55005645751953, + "heading": 3.1399999999999997, + "angularVelocity": 4.342265374359544e-18, + "velocityX": 0.19597054326510124, + "velocityY": -0.2402774071896897, + "timestamp": 2.5446555665635198 + }, + { + "x": 2.5939331054687496, + "y": 5.55005645751953, "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, + "angularVelocity": 3.0510058962998796e-17, + "velocityX": 1.3525026943280928e-16, + "velocityY": 8.359694800915289e-17, + "timestamp": 2.5708962631375125 + }, + { + "x": 2.5906368423225947, + "y": 5.5485199070890205, + "heading": 3.101591851410996, + "angularVelocity": -1.2805791583146104, + "velocityX": -0.10990183178303778, + "velocityY": -0.05123065102809662, + "timestamp": 2.60088905845952 + }, + { + "x": 2.584621501308477, + "y": 5.544975862182894, + "heading": 3.0243371709692783, + "angularVelocity": -2.5757746022741834, + "velocityX": -0.20055953270102006, + "velocityY": -0.11816320779855842, + "timestamp": 2.6308818537815273 + }, + { + "x": 2.5759613994637824, + "y": 5.536844199254011, + "heading": 2.9135350884263085, + "angularVelocity": -3.6942899570776517, + "velocityX": -0.28873940397378167, + "velocityY": -0.2711205421688905, + "timestamp": 2.6608746491035347 + }, + { + "x": 2.56275074361348, + "y": 5.521892690270415, + "heading": 2.8248688434153846, + "angularVelocity": -2.956251461684603, + "velocityX": -0.44046097433337356, + "velocityY": -0.49850335131840534, + "timestamp": 2.690867444425542 + }, + { + "x": 2.543962903792877, + "y": 5.501410245111836, + "heading": 2.7589030524712514, + "angularVelocity": -2.199387894182165, + "velocityX": -0.6264117638620101, + "velocityY": -0.6829121773583486, + "timestamp": 2.7208602397475494 + }, + { + "x": 2.518778715441927, + "y": 5.477002144640871, + "heading": 2.7154691851313926, + "angularVelocity": -1.448143358205586, + "velocityX": -0.839674597873837, + "velocityY": -0.8137987876364632, + "timestamp": 2.750853035069557 + }, + { + "x": 2.486733800013198, + "y": 5.449761748396881, + "heading": 2.6943182531197865, + "angularVelocity": -0.705200425130341, + "velocityX": -1.0684204351295292, + "velocityY": -0.9082313252703665, + "timestamp": 2.780845830391564 + }, + { + "x": 2.4478196867061426, + "y": 5.4204460978412365, + "heading": 2.694317441639593, + "angularVelocity": -0.000027055837428819432, + "velocityX": -1.2974487002285997, + "velocityY": -0.9774230858209427, + "timestamp": 2.8108386257135716 + }, + { + "x": 2.408901839144878, + "y": 5.391135398464328, + "heading": 2.6943166423252785, + "angularVelocity": -0.000026650210696695328, + "velocityX": -1.2975732052797133, + "velocityY": -0.9772580068715555, + "timestamp": 2.840831421035579 + }, + { + "x": 2.3699840051756724, + "y": 5.361824681040097, + "heading": 2.6943158430110494, + "angularVelocity": -0.00002665020784043696, + "velocityX": -1.2975727521022344, + "velocityY": -0.9772586085935405, + "timestamp": 2.8708242163575863 + }, + { + "x": 2.3310661711945366, + "y": 5.332513963631788, + "heading": 2.6943150436964616, + "angularVelocity": -0.000026650219800690006, + "velocityX": -1.297572752499997, + "velocityY": -0.9772586080626477, + "timestamp": 2.9008170116795937 + }, + { + "x": 2.292148337213513, + "y": 5.303203246223414, + "heading": 2.694314244381513, + "angularVelocity": -0.000026650231834296552, + "velocityX": -1.2975727524962546, + "velocityY": -0.9772586080648237, + "timestamp": 2.930809807001601 + }, + { + "x": 2.25323050323266, + "y": 5.273892528814898, + "heading": 2.6943134450662036, + "angularVelocity": -0.00002665024386857193, + "velocityX": -1.297572752490578, + "velocityY": -0.9772586080695672, + "timestamp": 2.9608026023236085 + }, + { + "x": 2.214312669251978, + "y": 5.2445818114062375, + "heading": 2.694312645750533, + "angularVelocity": -0.00002665025590264624, + "velocityX": -1.2975727524848706, + "velocityY": -0.9772586080743522, + "timestamp": 2.990795397645616 + }, + { + "x": 2.1753948352714723, + "y": 5.215271093997427, + "heading": 2.694311846434502, + "angularVelocity": -0.000026650267936849635, + "velocityX": -1.2975727524790048, + "velocityY": -0.9772586080793467, + "timestamp": 3.0207881929676232 + }, + { + "x": 2.136477001291495, + "y": 5.185960376588, + "heading": 2.6943110471181098, + "angularVelocity": -0.000026650279973527825, + "velocityX": -1.2975727524613627, + "velocityY": -0.9772586080999772, + "timestamp": 3.0507809882896306 + }, + { + "x": 2.097559167357152, + "y": 5.1566496591180675, + "heading": 2.694310247801348, + "angularVelocity": -0.000026650292283470146, + "velocityX": -1.2975727509398636, + "velocityY": -0.9772586101172457, + "timestamp": 3.080773783611638 + }, + { + "x": 2.058641302557044, + "y": 5.127338982627939, + "heading": 2.694309448489961, + "angularVelocity": -0.000026650113093683626, + "velocityX": -1.2975737800459084, + "velocityY": -0.9772572437956306, + "timestamp": 3.1107665789336454 + }, + { + "x": 2.0197569370912083, + "y": 5.0979839038638834, + "heading": 2.6943086027536207, + "angularVelocity": -0.000028197983267341566, + "velocityX": -1.2964568673313885, + "velocityY": -0.9787376751348899, + "timestamp": 3.1407593742556528 + }, + { + "x": 1.9877778714785685, + "y": 5.067888184962114, + "heading": 2.678621118987198, + "angularVelocity": -0.5230417371285907, + "velocityX": -1.0662249139900737, + "velocityY": -1.0034316101155079, + "timestamp": 3.17075216957766 + }, + { + "x": 1.9640654183560038, + "y": 5.031205594348394, + "heading": 2.6596531558314034, + "angularVelocity": -0.6324173173088112, + "velocityX": -0.7906049725553251, + "velocityY": -1.223046742388833, + "timestamp": 3.2007449648996675 + }, + { + "x": 1.9481708008996033, + "y": 4.987987936601904, "heading": 2.649783348569472, - "angularVelocity": -0.11091850762536054, - "velocityX": -0.27306763581709725, - "velocityY": -1.4626438018383596, - "timestamp": 3.3248528985757186 + "angularVelocity": -0.32907260414160233, + "velocityX": -0.5299478519964009, + "velocityY": -1.4409346405525878, + "timestamp": 3.230737760221675 }, { - "x": 1.941084861755371, + "x": 1.9410848617553718, "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 + "angularVelocity": 6.681257540282107e-18, + "velocityX": -0.23625470944103766, + "velocityY": -1.6071503980304298, + "timestamp": 3.2607305555436823 }, { - "x": 1.9600538072506943, - "y": 4.786928471515363, + "x": 1.9456173734407256, + "y": 4.888006870907468, "heading": 2.649783348569472, - "angularVelocity": 4.629261284078221e-26, - "velocityX": 0.3464586614300294, - "velocityY": -1.4977484159101515, - "timestamp": 3.457245662825282 + "angularVelocity": -1.484655388526889e-19, + "velocityX": 0.14165581617647935, + "velocityY": -1.6182360167593688, + "timestamp": 3.292727206352592 }, { - "x": 1.977394685635232, - "y": 4.735788039854744, + "x": 1.9619962278910381, + "y": 4.838678856610239, "heading": 2.649783348569472, - "angularVelocity": 4.640038416252255e-26, - "velocityX": 0.5078462851879747, - "velocityY": -1.4977025768725385, - "timestamp": 3.491391582349177 + "angularVelocity": -1.4846556369937314e-19, + "velocityX": 0.5118927774045035, + "velocityY": -1.5416618005383362, + "timestamp": 3.324723857161502 }, { - "x": 2.0002427480647933, - "y": 4.684845229382865, + "x": 1.9893341653194017, + "y": 4.794473003544163, "heading": 2.649783348569472, - "angularVelocity": 4.6490332666751425e-26, - "velocityX": 0.6691300966012094, - "velocityY": -1.4919150276866968, - "timestamp": 3.525537501873072 + "angularVelocity": -1.4846558519027202e-19, + "velocityX": 0.8543999680324345, + "velocityY": -1.381577507287736, + "timestamp": 3.3567205079704117 }, { - "x": 2.0285822715254445, - "y": 4.634362832128866, + "x": 2.026135553103086, + "y": 4.757769076744815, "heading": 2.649783348569472, - "angularVelocity": 4.622795940866032e-26, - "velocityX": 0.8299534426308102, - "velocityY": -1.478431331119124, - "timestamp": 3.559683421396967 + "angularVelocity": -1.4846559448429508e-19, + "velocityX": 1.1501637469359431, + "velocityY": -1.1471177723711978, + "timestamp": 3.3887171587793214 }, { - "x": 2.0623699922411927, - "y": 4.5847082412928, + "x": 2.0637808893572873, + "y": 4.721931260837698, "heading": 2.649783348569472, - "angularVelocity": 4.634016030607817e-26, - "velocityX": 0.9895097624213414, - "velocityY": -1.454188129311252, - "timestamp": 3.5938293409208617 + "angularVelocity": -1.4846636373679086e-19, + "velocityX": 1.1765398972283652, + "velocityY": -1.120048973905845, + "timestamp": 3.4207138095882312 }, { - "x": 2.101495965027088, - "y": 4.536421557771227, + "x": 2.1014301321789435, + "y": 4.6860975489840255, "heading": 2.649783348569472, - "angularVelocity": 4.645177261974479e-26, - "velocityX": 1.1458462191511727, - "velocityY": -1.4141274915083855, - "timestamp": 3.6279752604447566 + "angularVelocity": -1.484648310808098e-19, + "velocityX": 1.1766619902345172, + "velocityY": -1.1199207088167735, + "timestamp": 3.452710460397141 }, { - "x": 2.145673252803998, - "y": 4.4903376692314145, + "x": 2.139079370280558, + "y": 4.650263832171171, "heading": 2.649783348569472, - "angularVelocity": 4.6233288536561975e-26, - "velocityX": 1.2937794147261192, - "velocityY": -1.3496162698902718, - "timestamp": 3.6621211799686515 + "angularVelocity": -1.48465589810743e-19, + "velocityX": 1.1766618427177973, + "velocityY": -1.1199208638074638, + "timestamp": 3.484707111206051 }, { - "x": 2.1941116699379357, - "y": 4.447748182132556, + "x": 2.17672860838618, + "y": 4.614430115362526, "heading": 2.649783348569472, - "angularVelocity": 4.6479494614855375e-26, - "velocityX": 1.4185711736373225, - "velocityY": -1.2472789631292271, - "timestamp": 3.6962670994925464 + "angularVelocity": -1.4846559463647802e-19, + "velocityX": 1.1766618428430395, + "velocityY": -1.1199208636758753, + "timestamp": 3.5167037620149606 }, { - "x": 2.2448201323304646, - "y": 4.410180142680831, + "x": 2.2143778464918284, + "y": 4.57859639855391, "heading": 2.649783348569472, - "angularVelocity": 4.6329904581670297e-26, - "velocityX": 1.485051891985029, - "velocityY": -1.1002204648621148, - "timestamp": 3.7304130190164413 + "angularVelocity": -1.4846559409367017e-19, + "velocityX": 1.176661842843871, + "velocityY": -1.1199208636750018, + "timestamp": 3.5487004128238704 }, { - "x": 2.294852293597042, - "y": 4.3780811675391975, + "x": 2.2520270845974713, + "y": 4.5427626817452875, "heading": 2.649783348569472, - "angularVelocity": 4.6446166642422895e-26, - "velocityX": 1.4652456856979683, - "velocityY": -0.9400530309096446, - "timestamp": 3.764558938540336 + "angularVelocity": -1.4846559515966518e-19, + "velocityX": 1.1766618428436975, + "velocityY": -1.1199208636751843, + "timestamp": 3.58069706363278 }, { - "x": 2.342149827765447, - "y": 4.350766530180341, + "x": 2.289676322703114, + "y": 4.506928964936664, "heading": 2.649783348569472, - "angularVelocity": 4.633506801188826e-26, - "velocityX": 1.3851591882100787, - "velocityY": -0.7999385501902068, - "timestamp": 3.798704858064231 + "angularVelocity": -1.4846559610313848e-19, + "velocityX": 1.1766618428436995, + "velocityY": -1.1199208636751825, + "timestamp": 3.61269371444169 }, { - "x": 2.3856944951859207, - "y": 4.3274872473595005, + "x": 2.3273255608087573, + "y": 4.471095248128042, "heading": 2.649783348569472, - "angularVelocity": 4.6381778689211406e-26, - "velocityX": 1.2752524467821442, - "velocityY": -0.6817588498253565, - "timestamp": 3.832850777588126 + "angularVelocity": -1.484655947429445e-19, + "velocityX": 1.1766618428436957, + "velocityY": -1.1199208636751863, + "timestamp": 3.6446903652505998 }, { - "x": 2.4249725857215654, - "y": 4.307695719032298, + "x": 2.3649747989143997, + "y": 4.435261531319418, "heading": 2.649783348569472, - "angularVelocity": 4.6432377599415885e-26, - "velocityX": 1.150301151156826, - "velocityY": -0.5796162060697082, - "timestamp": 3.866996697112021 + "angularVelocity": -1.4846559574023035e-19, + "velocityX": 1.1766618428436864, + "velocityY": -1.1199208636751963, + "timestamp": 3.6766870160595095 }, { - "x": 2.4596964858202144, - "y": 4.291007007021927, + "x": 2.4026240370200447, + "y": 4.399427814510799, "heading": 2.649783348569472, - "angularVelocity": 4.6331179794053255e-26, - "velocityX": 1.0169267831358226, - "velocityY": -0.4887468910800007, - "timestamp": 3.9011426166359158 + "angularVelocity": -1.484655924580371e-19, + "velocityX": 1.1766618428437539, + "velocityY": -1.1199208636751252, + "timestamp": 3.7086836668684193 }, { - "x": 2.4896890256889828, - "y": 4.277143601542489, + "x": 2.4402732751256884, + "y": 4.363594097702177, "heading": 2.649783348569472, - "angularVelocity": 4.6467164999430504e-26, - "velocityX": 0.8783638070657234, - "velocityY": -0.4060047488173824, - "timestamp": 3.9352885361598107 + "angularVelocity": -1.4846559654067578e-19, + "velocityX": 1.1766618428437225, + "velocityY": -1.1199208636751585, + "timestamp": 3.740680317677329 }, { - "x": 2.514832571254234, - "y": 4.265898475268687, + "x": 2.4779225132411047, + "y": 4.327760380902407, "heading": 2.649783348569472, - "angularVelocity": 4.632856845272505e-26, - "velocityX": 0.7363557905551701, - "velocityY": -0.3293256245723021, - "timestamp": 3.9694344556837056 + "angularVelocity": -1.4846559492892923e-19, + "velocityX": 1.1766618431491565, + "velocityY": -1.1199208633985085, + "timestamp": 3.772676968486239 }, { - "x": 2.5350443768209585, - "y": 4.257112226026017, + "x": 2.5129728702731393, + "y": 4.294400221963306, "heading": 2.649783348569472, - "angularVelocity": 4.629729705599566e-26, - "velocityX": 0.5919244773180152, - "velocityY": -0.2573147645510306, - "timestamp": 4.0035803752076005 + "angularVelocity": -1.484655203554499e-19, + "velocityX": 1.0954383082580312, + "velocityY": -1.0426140891540008, + "timestamp": 3.8046736192951487 }, { - "x": 2.550263563230007, - "y": 4.2506587632470065, + "x": 2.53926064365096, + "y": 4.2693800974206075, "heading": 2.649783348569472, - "angularVelocity": 4.62810585566047e-26, - "velocityX": 0.445710252388941, - "velocityY": -0.18899660249284694, - "timestamp": 4.037726294731495 + "angularVelocity": -1.484655203554444e-19, + "velocityX": 0.8215789063304317, + "velocityY": -0.7819607337071065, + "timestamp": 3.8366702701040585 }, { - "x": 2.5604437164785643, - "y": 4.246436059535304, + "x": 2.5567858275270727, + "y": 4.252700012844841, "heading": 2.649783348569472, - "angularVelocity": 4.6519462435401186e-26, - "velocityX": 0.29813674343821717, - "velocityY": -0.12366642253542871, - "timestamp": 4.07187221425539 + "angularVelocity": -1.4846552035544855e-19, + "velocityX": 0.5477193216495216, + "velocityY": -0.5213072041629537, + "timestamp": 3.8686669209129683 }, { "x": 2.5655484199523926, "y": 4.244359970092773, "heading": 2.649783348569472, - "angularVelocity": 4.6699948971014915e-26, - "velocityX": 0.14949673474910496, - "velocityY": -0.06080051354533503, - "timestamp": 4.106018133779285 + "angularVelocity": -1.4846552035527083e-19, + "velocityX": 0.2738596760533411, + "velocityY": -0.260653616588761, + "timestamp": 3.900663571721878 }, { "x": 2.5655484199523926, "y": 4.244359970092773, "heading": 2.649783348569472, - "angularVelocity": 1.535197914331468e-26, - "velocityX": 1.1963425727073795e-24, - "velocityY": -7.168329076335369e-24, - "timestamp": 4.14016405330318 + "angularVelocity": -4.948850678520552e-20, + "velocityX": 6.0427487543509175e-18, + "velocityY": -9.335003878092332e-18, + "timestamp": 3.932660222530788 } ], "constraints": [ @@ -1197,7 +1125,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 25 + "controlIntervalCount": 24 }, { "x": 2.6290202140808105, @@ -1206,7 +1134,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 23 + "controlIntervalCount": 36 }, { "x": 7.999455451965332, @@ -1215,7 +1143,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 32 + "controlIntervalCount": 47 }, { "x": 2.4533867835998535, @@ -1229,976 +1157,1219 @@ ], "trajectory": [ { - "x": 0.3137660622596754, + "x": 0.3137660622596741, "y": 6.865104675292969, - "heading": 3.135568639860517, - "angularVelocity": -8.5693301650697e-28, - "velocityX": -3.4362695197914885e-17, - "velocityY": -4.743570524232907e-17, + "heading": 3.135568639860519, + "angularVelocity": 1.3323616061473399e-26, + "velocityX": -9.030177506128314e-25, + "velocityY": 7.578270673418084e-24, "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": 0.3285749308509426, + "y": 6.865947459797136, + "heading": 3.135568639860519, + "angularVelocity": 3.3337145202478786e-22, + "velocityX": 0.41797046933441173, + "velocityY": 0.023787032249159984, + "timestamp": 0.03543041836149472 + }, + { + "x": 0.35819266531481686, + "y": 6.867633028650745, + "heading": 3.135568639860519, + "angularVelocity": 3.3337147019213317e-22, + "velocityX": 0.8359408619363751, + "velocityY": 0.04757406013141577, + "timestamp": 0.07086083672298944 + }, + { + "x": 0.4026192574953566, + "y": 6.87016138138964, + "heading": 3.135568639860519, + "angularVelocity": 3.3337143389947597e-22, + "velocityX": 1.2539110243423486, + "velocityY": 0.07136107491303612, + "timestamp": 0.10629125508448417 + }, + { + "x": 0.4600803298886451, + "y": 6.873431536777097, + "heading": 3.135568639860519, + "angularVelocity": 3.331660178480041e-22, + "velocityX": 1.621800561512317, + "velocityY": 0.09229796143220789, + "timestamp": 0.14172167344597889 + }, + { + "x": 0.5175414022820589, + "y": 6.8767016921645565, + "heading": 3.135568639860519, + "angularVelocity": 3.3316629368551667e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233462, + "timestamp": 0.1771520918074736 + }, + { + "x": 0.5750024746754727, + "y": 6.879971847552017, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614681891394e-22, + "velocityX": 1.6218005615158566, + "velocityY": 0.09229796143233254, + "timestamp": 0.2125825101689683 + }, + { + "x": 0.6324635470688867, + "y": 6.883242002939478, + "heading": 3.135568639860519, + "angularVelocity": 3.331683104806914e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.24801292853046303 + }, + { + "x": 0.6899246194623005, + "y": 6.886512158326939, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614878052174e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233253, + "timestamp": 0.28344334689195777 + }, + { + "x": 0.7473856918557144, + "y": 6.8897823137144, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614758461554e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233254, + "timestamp": 0.3188737652534525 + }, + { + "x": 0.8048467642491284, + "y": 6.89305246910186, + "heading": 3.135568639860519, + "angularVelocity": 3.331638534198991e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233299, + "timestamp": 0.35430418361494725 + }, + { + "x": 0.8623078366425422, + "y": 6.89632262448932, + "heading": 3.135568639860519, + "angularVelocity": 3.3317076614902997e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233253, + "timestamp": 0.389734601976442 + }, + { + "x": 0.9197689090359561, + "y": 6.899592779876781, + "heading": 3.135568639860519, + "angularVelocity": 3.331661479116866e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233396, + "timestamp": 0.42516502033793674 + }, + { + "x": 0.97722998142937, + "y": 6.902862935264242, + "heading": 3.135568639860519, + "angularVelocity": 3.33166145502926e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.4605954386994315 + }, + { + "x": 1.0346910538227838, + "y": 6.906133090651703, + "heading": 3.135568639860519, + "angularVelocity": 3.3316600659908063e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233301, + "timestamp": 0.4960258570609262 + }, + { + "x": 1.0921521262161977, + "y": 6.9094032460391634, + "heading": 3.135568639860519, + "angularVelocity": 3.331661460674642e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.5314562754224209 + }, + { + "x": 1.1496131986096116, + "y": 6.912673401426624, + "heading": 3.135568639860519, + "angularVelocity": 3.331661475383932e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.5668866937839157 + }, + { + "x": 1.2070742710030253, + "y": 6.915943556814084, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614753921174e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233254, + "timestamp": 0.6023171121454104 + }, + { + "x": 1.2645353433964392, + "y": 6.919213712201545, + "heading": 3.135568639860519, + "angularVelocity": 3.331662731932369e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.6377475305069051 + }, + { + "x": 1.3219964157898532, + "y": 6.922483867589006, + "heading": 3.135568639860519, + "angularVelocity": 3.3316627143346654e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233393, + "timestamp": 0.6731779488683999 + }, + { + "x": 1.379457488183267, + "y": 6.925754022976466, + "heading": 3.135568639860519, + "angularVelocity": 3.331661483573468e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233157, + "timestamp": 0.7086083672298946 + }, + { + "x": 1.436918560576681, + "y": 6.929024178363926, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614890219236e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233392, + "timestamp": 0.7440387855913894 + }, + { + "x": 1.494379632970095, + "y": 6.932294333751387, + "heading": 3.135568639860519, + "angularVelocity": 3.331662743669949e-22, + "velocityX": 1.6218005615158566, + "velocityY": 0.09229796143233207, + "timestamp": 0.7794692039528841 + }, + { + "x": 1.5518407053635086, + "y": 6.935564489138847, + "heading": 3.135568639860519, + "angularVelocity": 3.331638909213181e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.8148996223143788 + }, + { + "x": 1.6093017777569225, + "y": 6.938834644526308, + "heading": 3.135568639860519, + "angularVelocity": 3.3316600948450887e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233622, + "timestamp": 0.8503300406758736 + }, + { + "x": 1.666762850150336, + "y": 6.942104799913777, + "heading": 3.135568639860519, + "angularVelocity": 3.331671581625241e-22, + "velocityX": 1.621800561515844, + "velocityY": 0.09229796143255317, + "timestamp": 0.8857604590373683 + }, + { + "x": 1.7242239225437261, + "y": 6.945374955301653, + "heading": 3.135568639860519, + "angularVelocity": 3.3334679678699274e-22, + "velocityX": 1.6218005615151887, + "velocityY": 0.09229796144406777, + "timestamp": 0.9211908773988631 + }, + { + "x": 1.7816849946975708, + "y": 6.948645114898682, + "heading": 3.135568639860519, + "angularVelocity": 1.7645788213167404e-22, + "velocityX": 1.6218005547541783, + "velocityY": 0.09229808024460076, + "timestamp": 0.9566212957603578 + }, + { + "x": 1.8178857407766151, + "y": 6.94853231479887, + "heading": 3.1384797599068888, + "angularVelocity": 0.12817174361754557, + "velocityX": 1.5938582646199322, + "velocityY": -0.004966399613447973, + "timestamp": 0.9793339464913666 + }, + { + "x": 1.8540603970824046, + "y": 6.948528771707492, + "heading": 3.1414999940123267, + "angularVelocity": 0.13297585302602363, + "velocityX": 1.592709575567121, + "velocityY": -0.00015599638369279018, + "timestamp": 1.0020465972223755 + }, + { + "x": 1.8901908310793394, + "y": 6.948640868874459, + "heading": 3.1447030512216227, + "angularVelocity": 0.14102524831780386, + "velocityX": 1.5907625413183106, + "velocityY": 0.0049354506567203535, + "timestamp": 1.0247592479533845 + }, + { + "x": 1.926257669084107, + "y": 6.9488776015696425, + "heading": 3.148167194864803, + "angularVelocity": 0.15252044704983372, + "velocityX": 1.5879625162168578, + "velocityY": 0.010422944375284186, + "timestamp": 1.0474718986843934 + }, + { + "x": 1.9622398024006038, + "y": 6.94925072993569, + "heading": 3.151976954699645, + "angularVelocity": 0.16773734954856945, + "velocityX": 1.584233110553301, + "velocityY": 0.01642821749276163, + "timestamp": 1.0701845494154023 + }, + { + "x": 1.9981171224268564, + "y": 6.949774504499785, + "heading": 3.156211459419019, + "angularVelocity": 0.1864381559653407, + "velocityX": 1.5796183567984214, + "velocityY": 0.023060917472754097, + "timestamp": 1.0928972001464112 + }, + { + "x": 2.033847488660219, + "y": 6.950468453992633, + "heading": 3.161038496208879, + "angularVelocity": 0.2125263513725333, + "velocityX": 1.5731482272380108, + "velocityY": 0.03055343478252191, + "timestamp": 1.1156098508774202 + }, + { + "x": 2.0694053227389695, + "y": 6.951354952814641, + "heading": 3.166554769842392, + "angularVelocity": 0.24287229609803831, + "velocityX": 1.565551925218666, + "velocityY": 0.039031059496573456, + "timestamp": 1.138322501608429 + }, + { + "x": 2.104784911670157, + "y": 6.952454847185295, + "heading": 3.1727732170956275, + "angularVelocity": 0.27378782542301205, + "velocityX": 1.5577040896809682, + "velocityY": 0.04842650836667849, + "timestamp": 1.161035152339438 + }, + { + "x": 2.1400047240253706, + "y": 6.9537191803371154, + "heading": 3.1796054213345153, + "angularVelocity": 0.3008105183231903, + "velocityX": 1.550669394441415, + "velocityY": 0.05566647269821489, + "timestamp": 1.183747803070447 + }, + { + "x": 2.175095855891208, + "y": 6.955084615506324, + "heading": 3.18691368664007, + "angularVelocity": 0.3217706903570311, + "velocityX": 1.5450038078527182, + "velocityY": 0.060117825320333544, + "timestamp": 1.2064604538014558 + }, + { + "x": 2.2101380445057046, + "y": 6.956471002562802, + "heading": 3.194369840668752, + "angularVelocity": 0.3282819833311086, + "velocityX": 1.5428489184071512, + "velocityY": 0.06104030185188484, + "timestamp": 1.2291731045324648 + }, + { + "x": 2.2451610724837057, + "y": 6.957849512193169, + "heading": 3.2018529792356185, + "angularVelocity": 0.32947006738627316, + "velocityX": 1.5420053076493287, + "velocityY": 0.060693471963871995, + "timestamp": 1.2518857552634737 + }, + { + "x": 2.2801629678458126, + "y": 6.959221595789265, + "heading": 3.2093707720743723, + "angularVelocity": 0.3309958369804056, + "velocityX": 1.5410748739388525, + "velocityY": 0.060410544429463274, + "timestamp": 1.2745984059944826 + }, + { + "x": 2.315144049729391, + "y": 6.960586594052822, + "heading": 3.2169217131634897, + "angularVelocity": 0.3324552989672944, + "velocityX": 1.540158491312511, + "velocityY": 0.06009858909567994, + "timestamp": 1.2973110567254915 + }, + { + "x": 2.3501040543802802, + "y": 6.961944386116353, + "heading": 3.2245066187504112, + "angularVelocity": 0.3339506989629348, + "velocityX": 1.539230496031878, + "velocityY": 0.05978131216881497, + "timestamp": 1.3200237074565004 + }, + { + "x": 2.3850426980471595, + "y": 6.963294864074251, + "heading": 3.232126373384074, + "angularVelocity": 0.3354850441678861, + "velocityX": 1.5382900076554706, + "velocityY": 0.05945928433854099, + "timestamp": 1.3427363581875094 + }, + { + "x": 2.419959710539657, + "y": 6.964637896582436, + "heading": 3.239781798872735, + "angularVelocity": 0.3370555722150803, + "velocityX": 1.5373376232491214, + "velocityY": 0.05913147364836824, + "timestamp": 1.3654490089185183 + }, + { + "x": 2.454854803951486, + "y": 6.965973356645481, + "heading": 3.247473776388813, + "angularVelocity": 0.3386648968090985, + "velocityX": 1.5363725628109135, + "velocityY": 0.05879807156202029, + "timestamp": 1.3881616596495272 + }, + { + "x": 2.48972762858419, + "y": 6.967301155248667, + "heading": 3.2552034126870217, + "angularVelocity": 0.3403229499609042, + "velocityX": 1.5353921057348447, + "velocityY": 0.05846075030657316, + "timestamp": 1.4108743103805361 + }, + { + "x": 2.5245780070203394, + "y": 6.968621054178632, + "heading": 3.262971149366171, + "angularVelocity": 0.34200044596925644, + "velocityX": 1.5344038372663136, + "velocityY": 0.05811294091547401, + "timestamp": 1.433586961111545 + }, + { + "x": 2.559405532428149, + "y": 6.969932985582836, + "heading": 3.2707782920481354, + "angularVelocity": 0.3437354263236719, + "velocityX": 1.5333976566751537, + "velocityY": 0.05776214409057404, + "timestamp": 1.456299611842554 + }, + { + "x": 2.594207801112396, + "y": 6.97123839692077, + "heading": 3.2786335848605472, + "angularVelocity": 0.34585539598366793, + "velocityX": 1.5322856454060756, + "velocityY": 0.05747507648462563, + "timestamp": 1.4790122625735629 + }, + { + "x": 2.6290202140808105, + "y": 6.972513675689697, + "heading": 3.286405336491994, + "angularVelocity": 0.3421772175995684, + "velocityX": 1.5327322812606092, + "velocityY": 0.05614838990089804, + "timestamp": 1.5017249133045718 + }, + { + "x": 2.7840510289336455, + "y": 6.98699854094784, + "heading": 3.286405287600397, + "angularVelocity": -5.100684883763116e-7, + "velocityX": 1.6173808639626774, + "velocityY": 0.1511154018498607, + "timestamp": 1.5975779175703702 + }, + { + "x": 2.93908160001489, + "y": 7.001486015045406, + "heading": 3.2864052387079807, + "angularVelocity": -5.100770354356606e-7, + "velocityX": 1.6173783207811374, + "velocityY": 0.15114261893547631, + "timestamp": 1.6934309218361685 + }, + { + "x": 3.094112171101734, + "y": 7.015973489083046, + "heading": 3.286405189815564, + "angularVelocity": -5.100770390190521e-7, + "velocityX": 1.617378320839561, + "velocityY": 0.15114261831028078, + "timestamp": 1.7892839261019668 + }, + { + "x": 3.2491427421885786, + "y": 7.030460963120681, + "heading": 3.286405140923147, + "angularVelocity": -5.100770430673946e-7, + "velocityX": 1.617378320839565, + "velocityY": 0.15114261831023715, + "timestamp": 1.8851369303677652 + }, + { + "x": 3.404173313275423, + "y": 7.0449484371583155, + "heading": 3.28640509203073, + "angularVelocity": -5.100770460466358e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831023496, + "timestamp": 1.9809899346335635 + }, + { + "x": 3.5592038843622675, + "y": 7.05943591119595, + "heading": 3.286405043138312, + "angularVelocity": -5.100770488976204e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831023276, + "timestamp": 2.076842938899362 + }, + { + "x": 3.714234455449112, + "y": 7.073923385233584, + "heading": 3.2864049942458946, + "angularVelocity": -5.100770524050277e-7, + "velocityX": 1.6173783208395642, + "velocityY": 0.15114261831023054, + "timestamp": 2.1726959431651602 + }, + { + "x": 3.8692650265359565, + "y": 7.088410859271219, + "heading": 3.2864049453534765, + "angularVelocity": -5.100770565197187e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831022827, + "timestamp": 2.2685489474309586 + }, + { + "x": 4.0242955976228005, + "y": 7.102898333308853, + "heading": 3.2864048964610575, + "angularVelocity": -5.100770606946053e-7, + "velocityX": 1.6173783208395642, + "velocityY": 0.15114261831022607, + "timestamp": 2.364401951696757 + }, + { + "x": 4.179326168709644, + "y": 7.117385807346487, + "heading": 3.286404847568639, + "angularVelocity": -5.100770634020749e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.151142618310224, + "timestamp": 2.4602549559625553 + }, + { + "x": 4.334356739796489, + "y": 7.13187328138412, + "heading": 3.2864047986762195, + "angularVelocity": -5.100770666030285e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.15114261831022163, + "timestamp": 2.5561079602283536 + }, + { + "x": 4.489387310883332, + "y": 7.146360755421753, + "heading": 3.2864047497838, + "angularVelocity": -5.100770706092546e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.15114261831021944, + "timestamp": 2.651960964494152 + }, + { + "x": 4.644417881970177, + "y": 7.160848229459387, + "heading": 3.28640470089138, + "angularVelocity": -5.100770740673254e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021727, + "timestamp": 2.7478139687599503 + }, + { + "x": 4.79944845305702, + "y": 7.175335703497019, + "heading": 3.28640465199896, + "angularVelocity": -5.100770777885376e-7, + "velocityX": 1.6173783208395638, + "velocityY": 0.15114261831021514, + "timestamp": 2.8436669730257487 + }, + { + "x": 4.954479024143865, + "y": 7.1898231775346515, + "heading": 3.286404603106539, + "angularVelocity": -5.100770810299081e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021283, + "timestamp": 2.939519977291547 + }, + { + "x": 5.109509595230708, + "y": 7.2043106515722855, + "heading": 3.2864045542141187, + "angularVelocity": -5.100770846564483e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021056, + "timestamp": 3.0353729815573454 + }, + { + "x": 5.264540166317553, + "y": 7.218798125609918, + "heading": 3.286404505321697, + "angularVelocity": -5.100770889061609e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.1511426183102084, + "timestamp": 3.1312259858231437 + }, + { + "x": 5.419570737404396, + "y": 7.233285599647549, + "heading": 3.2864044564292754, + "angularVelocity": -5.100770922378371e-7, + "velocityX": 1.6173783208395631, + "velocityY": 0.15114261831020614, + "timestamp": 3.227078990088942 + }, + { + "x": 5.574601308491241, + "y": 7.247773073685181, + "heading": 3.2864044075368533, + "angularVelocity": -5.100770952675197e-7, + "velocityX": 1.6173783208395631, + "velocityY": 0.15114261831020392, + "timestamp": 3.3229319943547404 + }, + { + "x": 5.7296318795780845, + "y": 7.262260547722812, + "heading": 3.286404358644431, + "angularVelocity": -5.100770991840634e-7, + "velocityX": 1.617378320839563, + "velocityY": 0.1511426183102018, + "timestamp": 3.4187849986205388 + }, + { + "x": 5.884662450664929, + "y": 7.276748021760444, + "heading": 3.286404309752008, + "angularVelocity": -5.100771023487481e-7, + "velocityX": 1.617378320839563, + "velocityY": 0.15114261831019962, + "timestamp": 3.514638002886337 + }, + { + "x": 6.039693021751773, + "y": 7.291235495798076, + "heading": 3.286404260859585, + "angularVelocity": -5.100771061559551e-7, + "velocityX": 1.6173783208395627, + "velocityY": 0.15114261831019746, + "timestamp": 3.6104910071521354 + }, + { + "x": 6.194723592838616, + "y": 7.305722969835706, + "heading": 3.2864042119671617, + "angularVelocity": -5.100771091660107e-7, + "velocityX": 1.6173783208395627, + "velocityY": 0.1511426183101951, + "timestamp": 3.706344011417934 + }, + { + "x": 6.349754163925461, + "y": 7.320210443873337, + "heading": 3.2864041630747383, + "angularVelocity": -5.100771129976334e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.15114261831019293, + "timestamp": 3.802197015683732 + }, + { + "x": 6.504784735012304, + "y": 7.334697917910969, + "heading": 3.286404114182314, + "angularVelocity": -5.100771163404491e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.1511426183101907, + "timestamp": 3.8980500199495305 + }, + { + "x": 6.659815306099149, + "y": 7.349185391948599, + "heading": 3.28640406528989, + "angularVelocity": -5.100771202032421e-7, + "velocityX": 1.6173783208395622, + "velocityY": 0.1511426183101885, + "timestamp": 3.993903024215329 + }, + { + "x": 6.814845877185992, + "y": 7.363672865986229, + "heading": 3.2864040163974653, + "angularVelocity": -5.100771233944824e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.15114261831018627, + "timestamp": 4.089756028481127 + }, + { + "x": 6.969876448272837, + "y": 7.378160340023859, + "heading": 3.2864039675050396, + "angularVelocity": -5.100771270486632e-7, + "velocityX": 1.6173783208395622, + "velocityY": 0.15114261831018408, + "timestamp": 4.1856090327469255 + }, + { + "x": 7.12490701935968, + "y": 7.392647814061489, + "heading": 3.286403918612615, + "angularVelocity": -5.100771304657537e-7, + "velocityX": 1.617378320839562, + "velocityY": 0.15114261831018191, + "timestamp": 4.281462037012724 + }, + { + "x": 7.279937590446525, + "y": 7.407135288099118, + "heading": 3.286403869720189, + "angularVelocity": -5.10077134427039e-7, + "velocityX": 1.6173783208395618, + "velocityY": 0.1511426183101796, + "timestamp": 4.377315041278522 + }, + { + "x": 7.4349681615333685, + "y": 7.421622762136748, + "heading": 3.286403820827763, + "angularVelocity": -5.100771387503622e-7, + "velocityX": 1.6173783208395618, + "velocityY": 0.15114261831017736, + "timestamp": 4.473168045544321 + }, + { + "x": 7.589998732620213, + "y": 7.436110236174376, + "heading": 3.2864037719353365, + "angularVelocity": -5.100771410554518e-7, + "velocityX": 1.6173783208395616, + "velocityY": 0.15114261831017525, + "timestamp": 4.569021049810119 + }, + { + "x": 7.745029303707056, + "y": 7.450597710212007, + "heading": 3.28640372304291, + "angularVelocity": -5.100771448287922e-7, + "velocityX": 1.6173783208395598, + "velocityY": 0.1511426183101905, + "timestamp": 4.664874054075917 + }, + { + "x": 7.90005987484092, + "y": 7.465085183746354, + "heading": 3.2864036741504377, + "angularVelocity": -5.1007762076643e-7, + "velocityX": 1.6173783213301043, + "velocityY": 0.15114261305961757, + "timestamp": 4.760727058341716 }, { "x": 7.999455451965332, "y": 7.473754405975342, "heading": 3.153293227446569, - "angularVelocity": -0.03044253542482025, - "velocityX": 0.33021570985126986, - "velocityY": 0.03437608724320182, - "timestamp": 2.5273272332371217 + "angularVelocity": -1.3886935284235506, + "velocityX": 1.0369583915052876, + "velocityY": 0.09044288486721058, + "timestamp": 4.856580062607514 }, { "x": 7.999455451965332, "y": 7.473754405975342, "heading": 3.153293227446569, - "angularVelocity": -3.79407884851814e-18, - "velocityX": 2.546987187300363e-16, - "velocityY": -1.0256209789219734e-16, - "timestamp": 2.5976055008604364 + "angularVelocity": 5.239369635456734e-24, + "velocityX": 2.4359888438060422e-22, + "velocityY": -8.774946070326848e-22, + "timestamp": 4.952433066873312 + }, + { + "x": 7.928671024084623, + "y": 7.451618987983874, + "heading": 3.153293227446569, + "angularVelocity": 2.2670564289793535e-36, + "velocityX": -0.8934591439715088, + "velocityY": -0.2793989045646846, + "timestamp": 5.031658209240836 + }, + { + "x": 7.805841482889084, + "y": 7.413208234129291, + "heading": 3.153293227446569, + "angularVelocity": 3.3814188338755363e-34, + "velocityX": -1.5503858689926344, + "velocityY": -0.4848303544397123, + "timestamp": 5.1108833516083605 + }, + { + "x": 7.683011941693537, + "y": 7.374797480274705, + "heading": 3.153293227446569, + "angularVelocity": 6.0567834914455396e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.190108493975885 }, { - "x": 7.9777911181971035, - "y": 7.466979623755239, + "x": 7.56018240049799, + "y": 7.336386726420119, "heading": 3.153293227446569, - "angularVelocity": 4.0010921973360205e-29, - "velocityX": -0.31261406582002, - "velocityY": -0.09775939742848679, - "timestamp": 2.6669060734735086 + "angularVelocity": -6.892446902508999e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.269333636343409 }, { - "x": 7.934462450883723, - "y": 7.453430059384792, + "x": 7.437352859302442, + "y": 7.297975972565533, "heading": 3.153293227446569, - "angularVelocity": -2.051733815514594e-34, - "velocityX": -0.6252281284210741, - "velocityY": -0.1955187938503514, - "timestamp": 2.736206646086581 + "angularVelocity": -4.625140327997389e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.348558778710933 }, { - "x": 7.869469450281316, - "y": 7.433105712944096, + "x": 7.314523318106895, + "y": 7.2595652187109465, "heading": 3.153293227446569, - "angularVelocity": -1.98522229630168e-34, - "velocityX": -0.9378421873262779, - "velocityY": -0.29327818911646475, - "timestamp": 2.805507218699653 + "angularVelocity": 2.407409636834091e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.427783921078457 }, { - "x": 7.782812116686986, - "y": 7.40600658452606, + "x": 7.191693776911348, + "y": 7.22115446485636, "heading": 3.153293227446569, - "angularVelocity": -1.9190015159804115e-34, - "velocityX": -1.2504562419442953, - "velocityY": -0.39103758304190667, - "timestamp": 2.8748077913127252 + "angularVelocity": -8.071167864078362e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.507009063445981 }, { - "x": 7.6744904504495075, - "y": 7.372132674239751, + "x": 7.0688642357158, + "y": 7.182743711001775, "heading": 3.153293227446569, - "angularVelocity": -8.200637720207044e-29, - "velocityX": -1.5630702915295283, - "velocityY": -0.4887969753935167, - "timestamp": 2.9441083639257974 + "angularVelocity": 6.706721989227974e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.586234205813505 }, { - "x": 7.544504451984089, - "y": 7.331483982215012, + "x": 6.946034694520253, + "y": 7.1443329571471885, "heading": 3.153293227446569, - "angularVelocity": 6.610929004011549e-29, - "velocityX": -1.8756843351233508, - "velocityY": -0.5865563658715173, - "timestamp": 3.0134089365388697 + "angularVelocity": -3.932169261853096e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.665459348181029 }, { - "x": 7.392854121793352, - "y": 7.28406050860902, + "x": 6.823205153324706, + "y": 7.105922203292602, "heading": 3.153293227446569, - "angularVelocity": -6.483737543755527e-29, - "velocityX": -2.188298371464413, - "velocityY": -0.6843157540814643, - "timestamp": 3.082709509151942 + "angularVelocity": -5.711604766985455e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.744684490548553 }, { - "x": 7.219539460498179, - "y": 7.229862253615935, + "x": 6.700375612129158, + "y": 7.067511449438016, "heading": 3.153293227446569, - "angularVelocity": 8.073376116568391e-29, - "velocityX": -2.5009123988461828, - "velocityY": -0.7820751394896976, - "timestamp": 3.152010081765014 + "angularVelocity": 9.147104299173202e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.823909632916077 }, { - "x": 7.024560468885024, - "y": 7.168889217481695, + "x": 6.577546070933611, + "y": 7.0291006955834305, "heading": 3.153293227446569, - "angularVelocity": -3.526812750348743e-30, - "velocityX": -2.813526414879514, - "velocityY": -0.8798345213490937, - "timestamp": 3.2213106543780863 + "angularVelocity": 4.987399043745874e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.903134775283601 }, { - "x": 6.807917147982327, - "y": 7.101141400527908, + "x": 6.4547165297380635, + "y": 6.990689941728844, "heading": 3.153293227446569, - "angularVelocity": -9.438045628849687e-29, - "velocityX": -3.1261404160725776, - "velocityY": -0.9775938985677018, - "timestamp": 3.2906112269911585 + "angularVelocity": 1.0726945561042014e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.9823599176511255 }, { - "x": 6.569609499192503, - "y": 7.026618803193132, + "x": 6.331886988542516, + "y": 6.952279187874258, "heading": 3.153293227446569, - "angularVelocity": 9.790681244582267e-29, - "velocityX": -3.438754397028912, - "velocityY": -1.0753532694579622, - "timestamp": 3.3599117996042307 + "angularVelocity": 1.8749531736864053e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.06158506001865 }, { - "x": 6.309637524541267, - "y": 6.94532142611084, + "x": 6.209057447346969, + "y": 6.913868434019672, "heading": 3.153293227446569, - "angularVelocity": -1.3895984503554753e-34, - "velocityX": -3.7513683487544087, - "velocityY": -1.1731126312072742, - "timestamp": 3.429212372217303 + "angularVelocity": -8.352063684746622e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.140810202386174 }, { - "x": 6.028001227211883, - "y": 6.85724927027649, + "x": 6.0862279061514215, + "y": 6.875457680165086, "heading": 3.153293227446569, - "angularVelocity": -1.323503512813964e-34, - "velocityX": -4.063982254545725, - "velocityY": -1.2708719785922367, - "timestamp": 3.498512944830375 + "angularVelocity": 6.429027209138877e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.220035344753698 }, { - "x": 5.724700612934229, - "y": 6.762402337481905, + "x": 5.963398364955874, + "y": 6.8370469263105, "heading": 3.153293227446569, - "angularVelocity": -2.1989550257903952e-29, - "velocityX": -4.376596077655505, - "velocityY": -1.3686313001213652, - "timestamp": 3.5678135174434473 + "angularVelocity": -7.349253697576567e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.299260487121222 }, { - "x": 5.399735695078014, - "y": 6.660780631908008, + "x": 5.840568823760327, + "y": 6.798636172455914, "heading": 3.153293227446569, - "angularVelocity": 1.8024841235265752e-29, - "velocityX": -4.689209707841794, - "velocityY": -1.4663905613202457, - "timestamp": 3.6371140900565195 + "angularVelocity": -5.316547406258321e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.378485629488746 }, { - "x": 5.053106540487171, - "y": 6.552384174457959, + "x": 5.7177392825647795, + "y": 6.760225418601328, "heading": 3.153293227446569, - "angularVelocity": 1.896211807861914e-29, - "velocityX": -5.001822373477154, - "velocityY": -1.564149520888688, - "timestamp": 3.7064146626695917 + "angularVelocity": 3.261106516242251e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.45771077185627 }, { - "x": 4.7281416226309565, - "y": 6.4507624688840615, + "x": 5.594909741369232, + "y": 6.721814664746741, "heading": 3.153293227446569, - "angularVelocity": -1.4997927313419928e-29, - "velocityX": -4.689209707841795, - "velocityY": -1.4663905613202457, - "timestamp": 3.775715235282664 + "angularVelocity": -2.0479976334874182e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.536935914223794 }, { - "x": 4.424841008353304, - "y": 6.355915536089477, + "x": 5.472080200173685, + "y": 6.683403910892156, "heading": 3.153293227446569, - "angularVelocity": -4.4185947102012545e-35, - "velocityX": -4.376596077655505, - "velocityY": -1.3686313001213652, - "timestamp": 3.845015807895736 + "angularVelocity": -7.212902306224552e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.616161056591318 }, { - "x": 4.14320471102392, - "y": 6.267843380255127, + "x": 5.3492506589781375, + "y": 6.64499315703757, "heading": 3.153293227446569, - "angularVelocity": -9.264733448000688e-35, - "velocityX": -4.063982254545726, - "velocityY": -1.2708719785922367, - "timestamp": 3.9143163805088084 + "angularVelocity": 2.340395125909778e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.695386198958842 }, { - "x": 3.883232736372683, - "y": 6.186546003172835, + "x": 5.22642111778259, + "y": 6.606582403182983, "heading": 3.153293227446569, - "angularVelocity": 3.5265482976182584e-30, - "velocityX": -3.751368348754409, - "velocityY": -1.1731126312072742, - "timestamp": 3.9836169531218806 + "angularVelocity": -2.132648663400479e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.774611341326366 }, { - "x": 3.644925087582859, - "y": 6.112023405838059, + "x": 5.103591576587043, + "y": 6.568171649328397, "heading": 3.153293227446569, - "angularVelocity": 2.156768838744686e-29, - "velocityX": -3.438754397028912, - "velocityY": -1.0753532694579622, - "timestamp": 4.052917525734953 + "angularVelocity": -2.6423732915608732e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.8538364836938905 }, { - "x": 3.4282817666801613, - "y": 6.044275588884272, + "x": 4.980762035391495, + "y": 6.529760895473811, "heading": 3.153293227446569, - "angularVelocity": -2.105657187574439e-28, - "velocityX": -3.1261404160725776, - "velocityY": -0.9775938985677018, - "timestamp": 4.122218098348026 + "angularVelocity": -1.5836433593674017e-28, + "velocityX": -1.5503858689927383, + "velocityY": -0.4848303544397451, + "timestamp": 6.933061626061415 }, { - "x": 3.2333027750670063, - "y": 5.983302552750032, + "x": 4.857932494195948, + "y": 6.491350141619225, "heading": 3.153293227446569, - "angularVelocity": 1.8547117767579628e-28, - "velocityX": -2.813526414879514, - "velocityY": -0.8798345213490937, - "timestamp": 4.1915186709610985 + "angularVelocity": 3.462450644614712e-28, + "velocityX": -1.5503858689927386, + "velocityY": -0.4848303544397447, + "timestamp": 7.012286768428939 }, { - "x": 3.059988113771833, - "y": 5.929104297756947, + "x": 4.735102953000401, + "y": 6.452939387764639, "heading": 3.153293227446569, - "angularVelocity": -5.955568710068408e-35, - "velocityX": -2.5009123988461828, - "velocityY": -0.7820751394896976, - "timestamp": 4.260819243574171 + "angularVelocity": 9.425461275139336e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.091511910796463 }, { - "x": 2.908337783581096, - "y": 5.881680824150955, + "x": 4.612273411804853, + "y": 6.414528633910053, "heading": 3.153293227446569, - "angularVelocity": -2.0810713408746708e-29, - "velocityX": -2.1882983714644135, - "velocityY": -0.6843157540814642, - "timestamp": 4.330119816187244 + "angularVelocity": 2.8373243283513945e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.170737053163987 }, { - "x": 2.7783517851156776, - "y": 5.841032132126216, + "x": 4.489443870609306, + "y": 6.3761178800554665, "heading": 3.153293227446569, - "angularVelocity": 2.081061415014159e-29, - "velocityX": -1.875684335123351, - "velocityY": -0.5865563658715173, - "timestamp": 4.3994203888003165 + "angularVelocity": 4.424546575494848e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.249962195531511 }, { - "x": 2.6700301188781994, - "y": 5.807158221839908, + "x": 4.366614329413759, + "y": 6.337707126200881, "heading": 3.153293227446569, - "angularVelocity": 7.326893682940581e-29, - "velocityX": -1.5630702915295283, - "velocityY": -0.48879697539351663, - "timestamp": 4.468720961413389 + "angularVelocity": 2.6048393166300173e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.329187337899035 }, { - "x": 2.583372785283869, - "y": 5.780059093421871, + "x": 4.243784788218211, + "y": 6.299296372346295, "heading": 3.153293227446569, - "angularVelocity": -7.326900961882318e-29, - "velocityX": -1.2504562419442953, - "velocityY": -0.39103758304190656, - "timestamp": 4.538021534026462 + "angularVelocity": -8.141761105066643e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.408412480266559 }, { - "x": 2.5183797846814615, - "y": 5.759734746981175, + "x": 4.120955247022664, + "y": 6.2608856184917085, "heading": 3.153293227446569, - "angularVelocity": -2.64698564160025e-35, - "velocityX": -0.9378421873262781, - "velocityY": -0.2932781891164647, - "timestamp": 4.6073221066395345 + "angularVelocity": -1.5769317536066289e-37, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.487637622634083 }, { - "x": 2.4750511173680816, - "y": 5.746185182610728, + "x": 3.998125705827117, + "y": 6.222474864637123, "heading": 3.153293227446569, - "angularVelocity": -4.5099737006589516e-29, - "velocityX": -0.6252281284210743, - "velocityY": -0.1955187938503513, - "timestamp": 4.676622679252607 + "angularVelocity": -1.9751877406999063e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.566862765001607 + }, + { + "x": 3.8752961646315702, + "y": 6.184064110782536, + "heading": 3.153293227446569, + "angularVelocity": 6.296656188647329e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.6460879073691315 + }, + { + "x": 3.7524666234360233, + "y": 6.145653356927951, + "heading": 3.153293227446569, + "angularVelocity": 3.349857109653225e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.725313049736656 + }, + { + "x": 3.6296370822404764, + "y": 6.107242603073365, + "heading": 3.153293227446569, + "angularVelocity": 1.1073851910178502e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.80453819210418 + }, + { + "x": 3.5068075410449295, + "y": 6.068831849218778, + "heading": 3.153293227446569, + "angularVelocity": -1.0776722091283702e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.883763334471704 + }, + { + "x": 3.3839779998493826, + "y": 6.030421095364192, + "heading": 3.153293227446569, + "angularVelocity": -2.2657744543933122e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.962988476839228 + }, + { + "x": 3.2611484586538357, + "y": 5.992010341509607, + "heading": 3.153293227446569, + "angularVelocity": 6.322058172960319e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.042213619206752 + }, + { + "x": 3.138318917458289, + "y": 5.95359958765502, + "heading": 3.153293227446569, + "angularVelocity": -1.267303885279781e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.121438761574275 + }, + { + "x": 3.015489376262742, + "y": 5.915188833800434, + "heading": 3.153293227446569, + "angularVelocity": 4.539047579816208e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.200663903941798 + }, + { + "x": 2.892659835067195, + "y": 5.876778079945848, + "heading": 3.153293227446569, + "angularVelocity": -2.4352191603126546e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.279889046309322 + }, + { + "x": 2.769830293871648, + "y": 5.838367326091261, + "heading": 3.153293227446569, + "angularVelocity": 1.9777124768596482e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.359114188676845 + }, + { + "x": 2.6470007526761012, + "y": 5.799956572236676, + "heading": 3.153293227446569, + "angularVelocity": 1.6826010379363495e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.438339331044368 + }, + { + "x": 2.5241712114805623, + "y": 5.761545818382093, + "heading": 3.153293227446569, + "angularVelocity": -1.9705474475500694e-33, + "velocityX": -1.5503858689926344, + "velocityY": -0.4848303544397123, + "timestamp": 8.517564473411891 }, { "x": 2.4533867835998535, "y": 5.739410400390625, "heading": 3.153293227446569, - "angularVelocity": 1.9207053764808128e-28, - "velocityX": -0.31261406582002027, - "velocityY": -0.09775939742848665, - "timestamp": 4.74592325186568 + "angularVelocity": 2.748235734113482e-34, + "velocityX": -0.8934591439715088, + "velocityY": -0.2793989045646846, + "timestamp": 8.596789615779414 }, { "x": 2.4533867835998535, "y": 5.739410400390625, "heading": 3.153293227446569, - "angularVelocity": -4.899028012023128e-29, - "velocityX": 1.275108562409782e-22, - "velocityY": 3.987692290669026e-23, - "timestamp": 4.815223824478752 + "angularVelocity": -7.888608661057978e-31, + "velocityX": -1.038819230645035e-25, + "velocityY": -4.640517458430809e-27, + "timestamp": 8.676014758146938 } ], "constraints": [ @@ -2233,6 +2404,1329 @@ 4 ], "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 3 + ], + "type": "WptZeroVelocity" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "FrontWing2": { + "waypoints": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -1.8084842834156656e-35, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.347146156455349, + "y": 5.593360447165824, + "heading": 3.140264869371664, + "angularVelocity": -0.007271662750997594, + "velocityX": 1.1945189788836181, + "velocityY": -0.007419158267666702, + "timestamp": 0.10109525888490682 + }, + { + "x": 1.5113646664969056, + "y": 5.59234048587755, + "heading": 3.1402648691641017, + "angularVelocity": -2.053134778232203e-9, + "velocityX": 1.6243937831794193, + "velocityY": -0.010089110998122406, + "timestamp": 0.20219051776981364 + }, + { + "x": 1.6755831765384634, + "y": 5.591320524589276, + "heading": 3.14026486895654, + "angularVelocity": -2.053134775032819e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.010089110998122467, + "timestamp": 0.30328577665472045 + }, + { + "x": 1.8398016865800209, + "y": 5.5903005633010014, + "heading": 3.140264868748978, + "angularVelocity": -2.0531347455807303e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.010089110998122468, + "timestamp": 0.4043810355396273 + }, + { + "x": 2.0040201966215783, + "y": 5.589280602012727, + "heading": 3.1402648685414154, + "angularVelocity": -2.0531349299385344e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.01008911099812247, + "timestamp": 0.5054762944245341 + }, + { + "x": 2.168238706663136, + "y": 5.588260640724453, + "heading": 3.1402648683338534, + "angularVelocity": -2.053134785130435e-9, + "velocityX": 1.6243937831794293, + "velocityY": -0.010089110998122468, + "timestamp": 0.6065715533094409 + }, + { + "x": 2.3324572167046935, + "y": 5.587240679436179, + "heading": 3.1402648681262915, + "angularVelocity": -2.0531346533039746e-9, + "velocityX": 1.6243937831794288, + "velocityY": -0.010089110998122467, + "timestamp": 0.7076668121943477 + }, + { + "x": 2.49667572674625, + "y": 5.586220718147905, + "heading": 3.140264867918729, + "angularVelocity": -2.0531352108819935e-9, + "velocityX": 1.624393783179419, + "velocityY": -0.010089110998122406, + "timestamp": 0.8087620710792545 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": -0.0072716627525318345, + "velocityX": 1.1945189788836168, + "velocityY": -0.007419158267749976, + "timestamp": 0.9098573299641612 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": 0, + "velocityX": -2.8085786967275447e-37, + "velocityY": 1.7440158343416895e-35, + "timestamp": 1.010952588849068 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "FrontWing1": { + "waypoints": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -1.6112520136935844e-31, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.2701999983580183, + "y": 5.571793741558495, + "heading": 2.9123192627847865, + "angularVelocity": -2.75479375784366, + "velocityX": 0.5278042458729727, + "velocityY": -0.26883784352486495, + "timestamp": 0.08301192659671743 + }, + { + "x": 1.3598509429602765, + "y": 5.50482600027521, + "heading": 2.837562269615421, + "angularVelocity": -0.9005572600735416, + "velocityX": 1.0799766765779808, + "velocityY": -0.806724335029887, + "timestamp": 0.16602385319343485 + }, + { + "x": 1.459326562321469, + "y": 5.413786422039427, + "heading": 2.837562034760696, + "angularVelocity": -0.000002829168462845694, + "velocityX": 1.1983292454403298, + "velocityY": -1.0967047985533822, + "timestamp": 0.2490357797901523 + }, + { + "x": 1.558802177818681, + "y": 5.322746839581507, + "heading": 2.837561799906192, + "angularVelocity": -0.000002829165803381977, + "velocityX": 1.198329198893034, + "velocityY": -1.0967048494152307, + "timestamp": 0.3320477063868697 + }, + { + "x": 1.6582777933158976, + "y": 5.231707257123597, + "heading": 2.8375615650516535, + "angularVelocity": -0.0000028291662173205513, + "velocityX": 1.1983291988930937, + "velocityY": -1.0967048494150837, + "timestamp": 0.4150596329835871 + }, + { + "x": 1.7577534088131206, + "y": 5.140667674665702, + "heading": 2.837561330197081, + "angularVelocity": -0.0000028291666305712727, + "velocityX": 1.19832919889317, + "velocityY": -1.0967048494149187, + "timestamp": 0.4980715595803045 + }, + { + "x": 1.8572290243103502, + "y": 5.04962809220782, + "heading": 2.837561095342474, + "angularVelocity": -0.000002829167043337366, + "velocityX": 1.1983291988932463, + "velocityY": -1.0967048494147538, + "timestamp": 0.581083486177022 + }, + { + "x": 1.9567046398075858, + "y": 4.958588509749952, + "heading": 2.8375608604878324, + "angularVelocity": -0.000002829167457373439, + "velocityX": 1.198329198893323, + "velocityY": -1.0967048494145886, + "timestamp": 0.6640954127737394 + }, + { + "x": 2.056180255304828, + "y": 4.867548927292098, + "heading": 2.8375606256331567, + "angularVelocity": -0.00000282916787026248, + "velocityX": 1.1983291988933988, + "velocityY": -1.096704849414424, + "timestamp": 0.7471073393704568 + }, + { + "x": 2.1556558708020765, + "y": 4.776509344834258, + "heading": 2.8375603907784472, + "angularVelocity": -0.0000028291682836294088, + "velocityX": 1.1983291988934754, + "velocityY": -1.0967048494142588, + "timestamp": 0.8301192659671742 + }, + { + "x": 2.2551314862993315, + "y": 4.685469762376432, + "heading": 2.8375601559237027, + "angularVelocity": -0.000002829168697323748, + "velocityX": 1.1983291988935516, + "velocityY": -1.096704849414094, + "timestamp": 0.9131311925638916 + }, + { + "x": 2.354607101796593, + "y": 4.594430179918619, + "heading": 2.8375599210689244, + "angularVelocity": -0.000002829169110584532, + "velocityX": 1.1983291988936342, + "velocityY": -1.096704849413922, + "timestamp": 0.996143119160609 + }, + { + "x": 2.45408271880603, + "y": 4.503390599113157, + "heading": 2.837559686214013, + "angularVelocity": -0.000002829170713915412, + "velocityX": 1.1983292171099962, + "velocityY": -1.0967048295089454, + "timestamp": 1.0791550457573265 + }, + { + "x": 2.5484359060007082, + "y": 4.424785420118879, + "heading": 2.7982046114916295, + "angularVelocity": -0.47408940300319863, + "velocityX": 1.13662206218943, + "velocityY": -0.9469142834878584, + "timestamp": 1.162166972354044 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": -2.3995421391526537, + "velocityX": 0.5485627867734783, + "velocityY": -0.4243619716588108, + "timestamp": 1.2451788989507615 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": 0, + "velocityX": -4.934633813021939e-37, + "velocityY": -2.767548277117811e-37, + "timestamp": 1.328190825547479 + }, + { + "x": 2.534366599021267, + "y": 4.442118048365455, + "heading": 2.6991662321397754, + "angularVelocity": 1.173988745719896, + "velocityX": -0.6987106220563037, + "velocityY": 0.6161074104533542, + "timestamp": 1.4135001923828505 + }, + { + "x": 2.4322324251030567, + "y": 4.53578051923557, + "heading": 2.6991662757513564, + "angularVelocity": 5.112167893680353e-7, + "velocityX": -1.1972210990067154, + "velocityY": 1.097915438182342, + "timestamp": 1.498809559218222 + }, + { + "x": 2.330098253355979, + "y": 4.629442992473233, + "heading": 2.6991663193628423, + "angularVelocity": 5.112156854714172e-7, + "velocityX": -1.1972210735566082, + "velocityY": 1.0979154659348471, + "timestamp": 1.5841189260535935 + }, + { + "x": 2.227964081608902, + "y": 4.723105465710898, + "heading": 2.6991663629743283, + "angularVelocity": 5.112156767559466e-7, + "velocityX": -1.1972210735566007, + "velocityY": 1.0979154659348562, + "timestamp": 1.669428292888965 + }, + { + "x": 2.1258299098618236, + "y": 4.816767938948561, + "heading": 2.6991664065858134, + "angularVelocity": 5.112156675644069e-7, + "velocityX": -1.1972210735566096, + "velocityY": 1.0979154659348476, + "timestamp": 1.7547376597243365 + }, + { + "x": 2.023695735807304, + "y": 4.910430409670039, + "heading": 2.6991664501973878, + "angularVelocity": 5.112167177081662e-7, + "velocityX": -1.1972211006045375, + "velocityY": 1.0979154364400177, + "timestamp": 1.840047026559708 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": 1.1341858822337967, + "velocityX": -0.702995210451579, + "velocityY": 0.6160140441054905, + "timestamp": 1.9253563933950795 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": -1.527841446263418e-36, + "velocityX": 0, + "velocityY": 0, + "timestamp": 2.010665760230451 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "FrontWing3Contested5": { + "waypoints": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 38 + }, + { + "x": 7.936469078063965, + "y": 7.452759265899658, + "heading": 3.1300325241315123, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 36 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": 0, + "velocityX": -1.009927502555409e-33, + "velocityY": 9.510006388327946e-34, + "timestamp": 0 + }, + { + "x": 1.2629372157532939, + "y": 5.6118681655215035, + "heading": 3.3611684682063676, + "angularVelocity": 2.615104090958444, + "velocityX": 0.4341464636351002, + "velocityY": 0.21092108774289103, + "timestamp": 0.08419109165389853 + }, + { + "x": 1.3467908645165432, + "y": 5.672396843669079, + "heading": 3.470572560445472, + "angularVelocity": 1.2994734964223353, + "velocityX": 0.9959919406671149, + "velocityY": 0.718943975645346, + "timestamp": 0.16838218330779706 + }, + { + "x": 1.4464012333437837, + "y": 5.766107211983687, + "heading": 3.4705728197139094, + "angularVelocity": 0.000003079523404990731, + "velocityX": 1.1831461841203954, + "velocityY": 1.1130675048120657, + "timestamp": 0.25257327496169557 + }, + { + "x": 1.5460115900456228, + "y": 5.859817593187631, + "heading": 3.4705730789812166, + "angularVelocity": 0.000003079509987095674, + "velocityX": 1.1831460400980154, + "velocityY": 1.1130676579082395, + "timestamp": 0.3367643666155941 + }, + { + "x": 1.645621946747464, + "y": 5.953527974391563, + "heading": 3.470573338248568, + "angularVelocity": 0.000003079510511134596, + "velocityX": 1.1831460400980387, + "velocityY": 1.1130676579081111, + "timestamp": 0.42095545826949265 + }, + { + "x": 1.7452323034493133, + "y": 6.047238355595477, + "heading": 3.470573597515964, + "angularVelocity": 0.000003079511034719833, + "velocityX": 1.183146040098137, + "velocityY": 1.1130676579079029, + "timestamp": 0.5051465499233911 + }, + { + "x": 1.844842660151171, + "y": 6.140948736799373, + "heading": 3.470573856783404, + "angularVelocity": 0.0000030795115586857372, + "velocityX": 1.1831460400982357, + "velocityY": 1.1130676579076944, + "timestamp": 0.5893376415772896 + }, + { + "x": 1.944453016853037, + "y": 6.234659118003253, + "heading": 3.470574116050888, + "angularVelocity": 0.0000030795120819840736, + "velocityX": 1.1831460400983345, + "velocityY": 1.1130676579074863, + "timestamp": 0.6735287332311881 + }, + { + "x": 2.044063373554911, + "y": 6.328369499207114, + "heading": 3.470574375318416, + "angularVelocity": 0.0000030795126059487745, + "velocityX": 1.1831460400984328, + "velocityY": 1.1130676579072778, + "timestamp": 0.7577198248850866 + }, + { + "x": 2.1436737302567934, + "y": 6.4220798804109585, + "heading": 3.470574634585988, + "angularVelocity": 0.000003079513129853114, + "velocityX": 1.183146040098531, + "velocityY": 1.1130676579070695, + "timestamp": 0.8419109165389851 + }, + { + "x": 2.243284086958684, + "y": 6.515790261614786, + "heading": 3.470574893853604, + "angularVelocity": 0.0000030795136536386766, + "velocityX": 1.1831460400986302, + "velocityY": 1.1130676579068612, + "timestamp": 0.9261020081928836 + }, + { + "x": 2.342894443660583, + "y": 6.6095006428185945, + "heading": 3.4705751531212647, + "angularVelocity": 0.0000030795141771132826, + "velocityX": 1.1831460400987281, + "velocityY": 1.1130676579066532, + "timestamp": 1.010293099846782 + }, + { + "x": 2.442504800362493, + "y": 6.703211024022384, + "heading": 3.470575412388969, + "angularVelocity": 0.0000030795147010529834, + "velocityX": 1.1831460400988572, + "velocityY": 1.1130676579064127, + "timestamp": 1.0944841915006807 + }, + { + "x": 2.5421151619283058, + "y": 6.79692140005584, + "heading": 3.470575671657143, + "angularVelocity": 0.0000030795202773893193, + "velocityX": 1.1831460978710306, + "velocityY": 1.1130675964945436, + "timestamp": 1.1786752831545793 + }, + { + "x": 2.632005363421341, + "y": 6.868564036243049, + "heading": 3.5437695268680747, + "angularVelocity": 0.8693776713553536, + "velocityX": 1.0676925518743172, + "velocityY": 0.8509526932103911, + "timestamp": 1.2628663748084779 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": 2.207199307384385, + "velocityX": 0.46866491549483613, + "velocityY": 0.37605700085290744, + "timestamp": 1.3470574664623765 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": 0, + "velocityX": -1.5194658088032264e-34, + "velocityY": -2.7343286363371283e-33, + "timestamp": 1.431248558116275 + }, + { + "x": 2.7208277247721226, + "y": 6.912165859423971, + "heading": 3.486294740771461, + "angularVelocity": -2.694751338131124, + "velocityX": 0.5467552516805291, + "velocityY": 0.13225779416916014, + "timestamp": 1.5215356661845725 + }, + { + "x": 2.839209601133149, + "y": 6.942185820162622, + "heading": 3.3899477762404344, + "angularVelocity": -1.0671176272269702, + "velocityX": 1.311171427392232, + "velocityY": 0.33249443227201864, + "timestamp": 1.61182277425287 + }, + { + "x": 2.985101375777118, + "y": 6.957221507964457, + "heading": 3.3899472751996993, + "angularVelocity": -0.0000055494161427999685, + "velocityX": 1.615864964171956, + "velocityY": 0.16653194596133747, + "timestamp": 1.7021098823211673 + }, + { + "x": 3.130993151670898, + "y": 6.97225718364292, + "heading": 3.3899467741601925, + "angularVelocity": -0.000005549402536979108, + "velocityX": 1.6158649780145908, + "velocityY": 0.1665318116855625, + "timestamp": 1.7923969903894648 + }, + { + "x": 3.2768849275645753, + "y": 6.98729285932146, + "heading": 3.3899462731201484, + "angularVelocity": -0.00000554940849560343, + "velocityX": 1.6158649780134444, + "velocityY": 0.1665318116864077, + "timestamp": 1.8826840984577622 + }, + { + "x": 3.4227767034581484, + "y": 7.002328535000082, + "heading": 3.3899457720795656, + "angularVelocity": -0.0000055494144557147385, + "velocityX": 1.615864978012292, + "velocityY": 0.1665318116873102, + "timestamp": 1.9729712065260596 + }, + { + "x": 3.5686684793516172, + "y": 7.017364210678785, + "heading": 3.3899452710384446, + "angularVelocity": -0.0000055494204159770735, + "velocityX": 1.6158649780111394, + "velocityY": 0.1665318116882126, + "timestamp": 2.063258314594357 + }, + { + "x": 3.7145602552449817, + "y": 7.03239988635757, + "heading": 3.3899447699967857, + "angularVelocity": -0.000005549426375721003, + "velocityX": 1.6158649780099874, + "velocityY": 0.1665318116891151, + "timestamp": 2.1535454226626545 + }, + { + "x": 3.8604520311382426, + "y": 7.047435562036436, + "heading": 3.3899442689545887, + "angularVelocity": -0.000005549432334941507, + "velocityX": 1.615864978008835, + "velocityY": 0.16653181169001743, + "timestamp": 2.243832530730952 + }, + { + "x": 4.006343807031399, + "y": 7.062471237715384, + "heading": 3.389943767911854, + "angularVelocity": -0.000005549438294605419, + "velocityX": 1.6158649780076826, + "velocityY": 0.16653181169091993, + "timestamp": 2.3341196387992493 + }, + { + "x": 4.152235582924452, + "y": 7.077506913394413, + "heading": 3.3899432668685807, + "angularVelocity": -0.000005549444254929096, + "velocityX": 1.6158649780065304, + "velocityY": 0.16653181169182238, + "timestamp": 2.4244067468675468 + }, + { + "x": 4.298127358817401, + "y": 7.092542589073523, + "heading": 3.3899427658247694, + "angularVelocity": -0.000005549450214439604, + "velocityX": 1.615864978005378, + "velocityY": 0.1665318116927247, + "timestamp": 2.514693854935844 + }, + { + "x": 4.444019134710246, + "y": 7.107578264752715, + "heading": 3.38994226478042, + "angularVelocity": -0.000005549456174760312, + "velocityX": 1.6158649780042256, + "velocityY": 0.16653181169362713, + "timestamp": 2.6049809630041416 + }, + { + "x": 4.5899109106029865, + "y": 7.122613940431989, + "heading": 3.389941763735532, + "angularVelocity": -0.000005549462134716656, + "velocityX": 1.6158649780030732, + "velocityY": 0.16653181169452963, + "timestamp": 2.695268071072439 + }, + { + "x": 4.735802686495623, + "y": 7.137649616111343, + "heading": 3.389941262690107, + "angularVelocity": -0.0000055494680950242725, + "velocityX": 1.6158649780019205, + "velocityY": 0.1665318116954319, + "timestamp": 2.7855551791407365 + }, + { + "x": 4.881694462388156, + "y": 7.15268529179078, + "heading": 3.389940761644143, + "angularVelocity": -0.000005549474055578583, + "velocityX": 1.6158649780007681, + "velocityY": 0.16653181169633444, + "timestamp": 2.875842287209034 + }, + { + "x": 5.027586238280584, + "y": 7.167720967470297, + "heading": 3.3899402605976414, + "angularVelocity": -0.000005549480015574405, + "velocityX": 1.6158649779996155, + "velocityY": 0.1665318116972368, + "timestamp": 2.9661293952773313 + }, + { + "x": 5.173478014172908, + "y": 7.182756643149896, + "heading": 3.389939759550601, + "angularVelocity": -0.000005549485976020103, + "velocityX": 1.6158649779984633, + "velocityY": 0.16653181169813924, + "timestamp": 3.0564165033456288 + }, + { + "x": 5.319369790065129, + "y": 7.197792318829577, + "heading": 3.389939258503023, + "angularVelocity": -0.000005549491936617687, + "velocityX": 1.6158649779973107, + "velocityY": 0.16653181169904166, + "timestamp": 3.146703611413926 + }, + { + "x": 5.4652615659572445, + "y": 7.212827994509339, + "heading": 3.3899387574549067, + "angularVelocity": -0.00000554949789662324, + "velocityX": 1.6158649779961582, + "velocityY": 0.1665318116999441, + "timestamp": 3.2369907194822236 + }, + { + "x": 5.611153341849257, + "y": 7.2278636701891825, + "heading": 3.389938256406252, + "angularVelocity": -0.000005549503857497513, + "velocityX": 1.6158649779950054, + "velocityY": 0.16653181170084652, + "timestamp": 3.327277827550521 + }, + { + "x": 5.757045117741165, + "y": 7.242899345869108, + "heading": 3.3899377553570593, + "angularVelocity": -0.000005549509818264508, + "velocityX": 1.615864977993853, + "velocityY": 0.16653181170174894, + "timestamp": 3.4175649356188185 + }, + { + "x": 5.902936893632969, + "y": 7.257935021549114, + "heading": 3.3899372543073283, + "angularVelocity": -0.000005549515778536107, + "velocityX": 1.6158649779927006, + "velocityY": 0.1665318117026513, + "timestamp": 3.507852043687116 + }, + { + "x": 6.048828669524668, + "y": 7.272970697229202, + "heading": 3.3899367532570595, + "angularVelocity": -0.000005549521739714091, + "velocityX": 1.6158649779915477, + "velocityY": 0.16653181170355377, + "timestamp": 3.5981391517554133 + }, + { + "x": 6.194720445416264, + "y": 7.288006372909372, + "heading": 3.3899362522062524, + "angularVelocity": -0.000005549527700309559, + "velocityX": 1.6158649779903953, + "velocityY": 0.16653181170445627, + "timestamp": 3.6884262598237108 + }, + { + "x": 6.340612221307755, + "y": 7.303042048589623, + "heading": 3.389935751154907, + "angularVelocity": -0.000005549533660762894, + "velocityX": 1.6158649779892427, + "velocityY": 0.16653181170535863, + "timestamp": 3.778713367892008 + }, + { + "x": 6.486503997199144, + "y": 7.318077724269955, + "heading": 3.3899352501030235, + "angularVelocity": -0.000005549539621393899, + "velocityX": 1.6158649779880898, + "velocityY": 0.16653181170626094, + "timestamp": 3.8690004759603056 + }, + { + "x": 6.632395773090427, + "y": 7.333113399950369, + "heading": 3.3899347490506018, + "angularVelocity": -0.00000554954558299688, + "velocityX": 1.6158649779869374, + "velocityY": 0.1665318117071635, + "timestamp": 3.959287584028603 + }, + { + "x": 6.778287548981607, + "y": 7.3481490756308645, + "heading": 3.3899342479976413, + "angularVelocity": -0.000005549551543569729, + "velocityX": 1.615864977985785, + "velocityY": 0.16653181170806597, + "timestamp": 4.0495746920969005 + }, + { + "x": 6.924179324872682, + "y": 7.363184751311442, + "heading": 3.3899337469441435, + "angularVelocity": -0.000005549557505439446, + "velocityX": 1.615864977984632, + "velocityY": 0.16653181170896839, + "timestamp": 4.139861800165198 + }, + { + "x": 7.070071100763654, + "y": 7.378220426992101, + "heading": 3.389933245890107, + "angularVelocity": -0.00000554956346606953, + "velocityX": 1.6158649779834795, + "velocityY": 0.1665318117098707, + "timestamp": 4.230148908233495 + }, + { + "x": 7.215962876654522, + "y": 7.393256102672841, + "heading": 3.3899327448355323, + "angularVelocity": -0.000005549569427174084, + "velocityX": 1.6158649779823266, + "velocityY": 0.16653181171077316, + "timestamp": 4.320436016301793 + }, + { + "x": 7.361854652545285, + "y": 7.408291778353663, + "heading": 3.3899322437804194, + "angularVelocity": -0.000005549575389475234, + "velocityX": 1.615864977981174, + "velocityY": 0.16653181171167572, + "timestamp": 4.41072312437009 + }, + { + "x": 7.5077464284359445, + "y": 7.423327454034565, + "heading": 3.389931742724768, + "angularVelocity": -0.000005549581350385169, + "velocityX": 1.6158649779800212, + "velocityY": 0.1665318117125781, + "timestamp": 4.501010232438388 + }, + { + "x": 7.6536382043265, + "y": 7.43836312971555, + "heading": 3.3899312416685787, + "angularVelocity": -0.000005549587311867374, + "velocityX": 1.6158649779788683, + "velocityY": 0.166531811713483, + "timestamp": 4.591297340506685 + }, + { + "x": 7.799529980161501, + "y": 7.453398805927109, + "heading": 3.3899307406089574, + "angularVelocity": -0.000005549625325450788, + "velocityX": 1.6158649773635676, + "velocityY": 0.16653181759000288, + "timestamp": 4.6815844485749825 + }, + { + "x": 7.908273189210625, + "y": 7.465055953097054, + "heading": 3.2483837637195303, + "angularVelocity": -1.5677429471143751, + "velocityX": 1.204415684317477, + "velocityY": 0.12911197865732152, + "timestamp": 4.77187155664328 + }, + { + "x": 7.936469078063965, + "y": 7.452759265899658, + "heading": 3.1300325241315123, + "angularVelocity": -1.3108321012839574, + "velocityX": 0.3122914163117384, + "velocityY": -0.13619538226979142, + "timestamp": 4.862158664711577 + }, + { + "x": 7.897313987574637, + "y": 7.4320035366195905, + "heading": 3.223159902402127, + "angularVelocity": 0.9773255855261127, + "velocityX": -0.4109132292719665, + "velocityY": -0.21782107097140077, + "timestamp": 4.957446642075316 + }, + { + "x": 7.764258802337476, + "y": 7.410354345348057, + "heading": 3.302887545584371, + "angularVelocity": 0.8367020204228218, + "velocityX": -1.396348090475833, + "velocityY": -0.22719751085588358, + "timestamp": 5.052734619439054 + }, + { + "x": 7.611104941895525, + "y": 7.387923016344696, + "heading": 3.302889098211225, + "angularVelocity": 0.000016294047758499195, + "velocityX": -1.607273705236954, + "velocityY": -0.23540565792192894, + "timestamp": 5.148022596802792 + }, + { + "x": 7.457951081309142, + "y": 7.365491688274562, + "heading": 3.302890650804134, + "angularVelocity": 0.000016293691520657764, + "velocityX": -1.6072737067526932, + "velocityY": -0.2354056481281831, + "timestamp": 5.2433105741665305 + }, + { + "x": 7.30479722071746, + "y": 7.343060360198914, + "heading": 3.3028922033698884, + "angularVelocity": 0.000016293406553711894, + "velocityX": -1.6072737068083087, + "velocityY": -0.23540564818605267, + "timestamp": 5.338598551530269 + }, + { + "x": 7.151643360120445, + "y": 7.320629032117711, + "heading": 3.302893755908325, + "angularVelocity": 0.00001629311986139254, + "velocityX": -1.6072737068642717, + "velocityY": -0.23540564824432697, + "timestamp": 5.433886528894007 + }, + { + "x": 6.998489499518071, + "y": 7.298197704030925, + "heading": 3.302895308419307, + "angularVelocity": 0.00001629283173826444, + "velocityX": -1.6072737069205232, + "velocityY": -0.2354056483029315, + "timestamp": 5.529174506257745 + }, + { + "x": 6.845335638910315, + "y": 7.275766375938532, + "heading": 3.3028968609027314, + "angularVelocity": 0.000016292542536041963, + "velocityX": -1.6072737069769925, + "velocityY": -0.23540564836178338, + "timestamp": 5.624462483621484 + }, + { + "x": 6.692181778297163, + "y": 7.253335047840514, + "heading": 3.302898413358527, + "angularVelocity": 0.000016292252586671315, + "velocityX": -1.6072737070336132, + "velocityY": -0.23540564842080344, + "timestamp": 5.719750460985222 + }, + { + "x": 6.539027917678609, + "y": 7.230903719736863, + "heading": 3.3028999657866516, + "angularVelocity": 0.000016291962193735624, + "velocityX": -1.607273707090324, + "velocityY": -0.23540564847991918, + "timestamp": 5.81503843834896 + }, + { + "x": 6.385874057054646, + "y": 7.208472391627575, + "heading": 3.302901518187087, + "angularVelocity": 0.000016291671606816483, + "velocityX": -1.607273707147075, + "velocityY": -0.2354056485390708, + "timestamp": 5.9103264157126985 + }, + { + "x": 6.232720196425276, + "y": 7.186041063512652, + "heading": 3.3029030705598315, + "angularVelocity": 0.00001629138100746966, + "velocityX": -1.60727370720383, + "velocityY": -0.23540564859821628, + "timestamp": 6.005614393076437 + }, + { + "x": 6.079566335790499, + "y": 7.1636097353920976, + "heading": 3.3029046229048977, + "angularVelocity": 0.000016291090533089062, + "velocityX": -1.6072737072605614, + "velocityY": -0.23540564865732222, + "timestamp": 6.100902370440175 + }, + { + "x": 5.926412475150321, + "y": 7.141178407265915, + "heading": 3.302906175222303, + "angularVelocity": 0.000016290800243766995, + "velocityX": -1.6072737073172572, + "velocityY": -0.23540564871637523, + "timestamp": 6.196190347803913 + }, + { + "x": 5.773258614504743, + "y": 7.118747079134112, + "heading": 3.3029077275120664, + "angularVelocity": 0.00001629051016274816, + "velocityX": -1.6072737073739125, + "velocityY": -0.23540564877536937, + "timestamp": 6.291478325167652 + }, + { + "x": 5.62010475385377, + "y": 7.096315750996691, + "heading": 3.302909279774206, + "angularVelocity": 0.000016290220263808567, + "velocityX": -1.6072737074305328, + "velocityY": -0.23540564883431206, + "timestamp": 6.38676630253139 + }, + { + "x": 5.466950893197405, + "y": 7.073884422853658, + "heading": 3.302910832008734, + "angularVelocity": 0.00001628993049423592, + "velocityX": -1.6072737074871284, + "velocityY": -0.23540564889321514, + "timestamp": 6.482054279895128 + }, + { + "x": 5.313797032535647, + "y": 7.051453094705014, + "heading": 3.3029123842156554, + "angularVelocity": 0.000016289640769266148, + "velocityX": -1.6072737075437156, + "velocityY": -0.23540564895209914, + "timestamp": 6.5773422572588665 + }, + { + "x": 5.160643171868496, + "y": 7.02902176655076, + "heading": 3.3029139363949653, + "angularVelocity": 0.000016289351008016845, + "velocityX": -1.6072737076003119, + "velocityY": -0.23540564901098462, + "timestamp": 6.672630234622605 + }, + { + "x": 5.0074893111959495, + "y": 7.006590438390892, + "heading": 3.302915488546652, + "angularVelocity": 0.000016289061112899286, + "velocityX": -1.6072737076569361, + "velocityY": -0.23540564906989445, + "timestamp": 6.767918211986343 + }, + { + "x": 4.854335450518003, + "y": 6.984159110225406, + "heading": 3.3029170406706947, + "angularVelocity": 0.000016288771004663956, + "velocityX": -1.607273707713604, + "velocityY": -0.23540564912884793, + "timestamp": 6.863206189350081 + }, + { + "x": 4.701181589834651, + "y": 6.961727782054296, + "heading": 3.3029185927670666, + "angularVelocity": 0.00001628848060846621, + "velocityX": -1.6072737077703305, + "velocityY": -0.235405649187863, + "timestamp": 6.95849416671382 + }, + { + "x": 4.548027729145889, + "y": 6.939296453877556, + "heading": 3.3029201448357353, + "angularVelocity": 0.000016288189883493963, + "velocityX": -1.607273707827124, + "velocityY": -0.23540564924694996, + "timestamp": 7.053782144077558 + }, + { + "x": 4.3948738684517075, + "y": 6.916865125695178, + "heading": 3.3029216968766675, + "angularVelocity": 0.00001628789879930085, + "velocityX": -1.6072737078839905, + "velocityY": -0.23540564930611646, + "timestamp": 7.149070121441296 + }, + { + "x": 4.2417200077521, + "y": 6.894433797507156, + "heading": 3.302923248889828, + "angularVelocity": 0.00001628760735122893, + "velocityX": -1.6072737079409307, + "velocityY": -0.23540564936536426, + "timestamp": 7.2443580988050345 + }, + { + "x": 4.088566147047059, + "y": 6.872002469313479, + "heading": 3.3029248008751826, + "angularVelocity": 0.000016287315540072175, + "velocityX": -1.6072737079979444, + "velocityY": -0.23540564942469375, + "timestamp": 7.339646076168773 + }, + { + "x": 3.9354122863365792, + "y": 6.849571141114141, + "heading": 3.3029263528326944, + "angularVelocity": 0.000016287023346622306, + "velocityX": -1.6072737080550357, + "velocityY": -0.23540564948410944, + "timestamp": 7.434934053532511 + }, + { + "x": 3.7822584256206495, + "y": 6.827139812909132, + "heading": 3.3029279047623197, + "angularVelocity": 0.00001628673068839883, + "velocityX": -1.6072737081122201, + "velocityY": -0.23540564954363288, + "timestamp": 7.530222030896249 + }, + { + "x": 3.629104564899259, + "y": 6.804708484698436, + "heading": 3.3029294566639957, + "angularVelocity": 0.000016286437378238427, + "velocityX": -1.607273708169536, + "velocityY": -0.2354056496033085, + "timestamp": 7.625510008259988 + }, + { + "x": 3.4759507041723876, + "y": 6.7822771564820306, + "heading": 3.302931008537628, + "angularVelocity": 0.00001628614306837342, + "velocityX": -1.6072737082270525, + "velocityY": -0.2354056496632214, + "timestamp": 7.720797985623726 + }, + { + "x": 3.3227968434400057, + "y": 6.759845828259881, + "heading": 3.3029325603830677, + "angularVelocity": 0.000016285847206848686, + "velocityX": -1.6072737082848803, + "velocityY": -0.23540564972350414, + "timestamp": 7.816085962987464 + }, + { + "x": 3.1696429827020713, + "y": 6.737414500031936, + "heading": 3.3029341122001052, + "angularVelocity": 0.00001628554913981109, + "velocityX": -1.6072737083431483, + "velocityY": -0.2354056497843278, + "timestamp": 7.9113739403512024 + }, + { + "x": 3.016489122487879, + "y": 6.714983168191867, + "heading": 3.3029356639933174, + "angularVelocity": 0.000016285299102484297, + "velocityX": -1.6072737028467396, + "velocityY": -0.23540568769178183, + "timestamp": 8.006661917714942 + }, + { + "x": 2.8768170839321456, + "y": 6.685183755290752, + "heading": 3.3517957161483998, + "angularVelocity": 0.512761982223329, + "velocityX": -1.465788679956645, + "velocityY": -0.31273003925103915, + "timestamp": 8.10194989507868 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": 2.3762078534459548, + "velocityX": -0.6771226429407677, + "velocityY": -0.18518169587388544, + "timestamp": 8.19723787244242 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": 4.7042853211430965e-33, + "velocityX": -2.4138813228334137e-34, + "velocityY": 0, + "timestamp": 8.29252584980616 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, diff --git a/src/main/deploy/choreo/FrontWing1.1.traj b/src/main/deploy/choreo/FrontWing1.1.traj new file mode 100644 index 00000000..090e2a15 --- /dev/null +++ b/src/main/deploy/choreo/FrontWing1.1.traj @@ -0,0 +1,157 @@ +{ + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -1.6112520136935844e-31, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.2701999983580183, + "y": 5.571793741558495, + "heading": 2.9123192627847865, + "angularVelocity": -2.75479375784366, + "velocityX": 0.5278042458729727, + "velocityY": -0.26883784352486495, + "timestamp": 0.08301192659671743 + }, + { + "x": 1.3598509429602765, + "y": 5.50482600027521, + "heading": 2.837562269615421, + "angularVelocity": -0.9005572600735416, + "velocityX": 1.0799766765779808, + "velocityY": -0.806724335029887, + "timestamp": 0.16602385319343485 + }, + { + "x": 1.459326562321469, + "y": 5.413786422039427, + "heading": 2.837562034760696, + "angularVelocity": -0.000002829168462845694, + "velocityX": 1.1983292454403298, + "velocityY": -1.0967047985533822, + "timestamp": 0.2490357797901523 + }, + { + "x": 1.558802177818681, + "y": 5.322746839581507, + "heading": 2.837561799906192, + "angularVelocity": -0.000002829165803381977, + "velocityX": 1.198329198893034, + "velocityY": -1.0967048494152307, + "timestamp": 0.3320477063868697 + }, + { + "x": 1.6582777933158976, + "y": 5.231707257123597, + "heading": 2.8375615650516535, + "angularVelocity": -0.0000028291662173205513, + "velocityX": 1.1983291988930937, + "velocityY": -1.0967048494150837, + "timestamp": 0.4150596329835871 + }, + { + "x": 1.7577534088131206, + "y": 5.140667674665702, + "heading": 2.837561330197081, + "angularVelocity": -0.0000028291666305712727, + "velocityX": 1.19832919889317, + "velocityY": -1.0967048494149187, + "timestamp": 0.4980715595803045 + }, + { + "x": 1.8572290243103502, + "y": 5.04962809220782, + "heading": 2.837561095342474, + "angularVelocity": -0.000002829167043337366, + "velocityX": 1.1983291988932463, + "velocityY": -1.0967048494147538, + "timestamp": 0.581083486177022 + }, + { + "x": 1.9567046398075858, + "y": 4.958588509749952, + "heading": 2.8375608604878324, + "angularVelocity": -0.000002829167457373439, + "velocityX": 1.198329198893323, + "velocityY": -1.0967048494145886, + "timestamp": 0.6640954127737394 + }, + { + "x": 2.056180255304828, + "y": 4.867548927292098, + "heading": 2.8375606256331567, + "angularVelocity": -0.00000282916787026248, + "velocityX": 1.1983291988933988, + "velocityY": -1.096704849414424, + "timestamp": 0.7471073393704568 + }, + { + "x": 2.1556558708020765, + "y": 4.776509344834258, + "heading": 2.8375603907784472, + "angularVelocity": -0.0000028291682836294088, + "velocityX": 1.1983291988934754, + "velocityY": -1.0967048494142588, + "timestamp": 0.8301192659671742 + }, + { + "x": 2.2551314862993315, + "y": 4.685469762376432, + "heading": 2.8375601559237027, + "angularVelocity": -0.000002829168697323748, + "velocityX": 1.1983291988935516, + "velocityY": -1.096704849414094, + "timestamp": 0.9131311925638916 + }, + { + "x": 2.354607101796593, + "y": 4.594430179918619, + "heading": 2.8375599210689244, + "angularVelocity": -0.000002829169110584532, + "velocityX": 1.1983291988936342, + "velocityY": -1.096704849413922, + "timestamp": 0.996143119160609 + }, + { + "x": 2.45408271880603, + "y": 4.503390599113157, + "heading": 2.837559686214013, + "angularVelocity": -0.000002829170713915412, + "velocityX": 1.1983292171099962, + "velocityY": -1.0967048295089454, + "timestamp": 1.0791550457573265 + }, + { + "x": 2.5484359060007082, + "y": 4.424785420118879, + "heading": 2.7982046114916295, + "angularVelocity": -0.47408940300319863, + "velocityX": 1.13662206218943, + "velocityY": -0.9469142834878584, + "timestamp": 1.162166972354044 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": -2.3995421391526537, + "velocityX": 0.5485627867734783, + "velocityY": -0.4243619716588108, + "timestamp": 1.2451788989507615 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": 0, + "velocityX": -4.934633813021939e-37, + "velocityY": -2.767548277117811e-37, + "timestamp": 1.328190825547479 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing1.2.traj b/src/main/deploy/choreo/FrontWing1.2.traj new file mode 100644 index 00000000..d3d054b7 --- /dev/null +++ b/src/main/deploy/choreo/FrontWing1.2.traj @@ -0,0 +1,85 @@ +{ + "samples": [ + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": 0, + "velocityX": -4.934633813021939e-37, + "velocityY": -2.767548277117811e-37, + "timestamp": 0 + }, + { + "x": 2.534366599021267, + "y": 4.442118048365455, + "heading": 2.6991662321397754, + "angularVelocity": 1.173988745719896, + "velocityX": -0.6987106220563037, + "velocityY": 0.6161074104533542, + "timestamp": 0.08530936683537149 + }, + { + "x": 2.4322324251030567, + "y": 4.53578051923557, + "heading": 2.6991662757513564, + "angularVelocity": 5.112167893680353e-7, + "velocityX": -1.1972210990067154, + "velocityY": 1.097915438182342, + "timestamp": 0.17061873367074298 + }, + { + "x": 2.330098253355979, + "y": 4.629442992473233, + "heading": 2.6991663193628423, + "angularVelocity": 5.112156854714172e-7, + "velocityX": -1.1972210735566082, + "velocityY": 1.0979154659348471, + "timestamp": 0.2559281005061145 + }, + { + "x": 2.227964081608902, + "y": 4.723105465710898, + "heading": 2.6991663629743283, + "angularVelocity": 5.112156767559466e-7, + "velocityX": -1.1972210735566007, + "velocityY": 1.0979154659348562, + "timestamp": 0.34123746734148597 + }, + { + "x": 2.1258299098618236, + "y": 4.816767938948561, + "heading": 2.6991664065858134, + "angularVelocity": 5.112156675644069e-7, + "velocityX": -1.1972210735566096, + "velocityY": 1.0979154659348476, + "timestamp": 0.42654683417685746 + }, + { + "x": 2.023695735807304, + "y": 4.910430409670039, + "heading": 2.6991664501973878, + "angularVelocity": 5.112167177081662e-7, + "velocityX": -1.1972211006045375, + "velocityY": 1.0979154364400177, + "timestamp": 0.511856201012229 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": 1.1341858822337967, + "velocityX": -0.702995210451579, + "velocityY": 0.6160140441054905, + "timestamp": 0.5971655678476004 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": -1.527841446263418e-36, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0.6824749346829719 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing1.traj b/src/main/deploy/choreo/FrontWing1.traj new file mode 100644 index 00000000..e9811392 --- /dev/null +++ b/src/main/deploy/choreo/FrontWing1.traj @@ -0,0 +1,229 @@ +{ + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -1.6112520136935844e-31, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.2701999983580183, + "y": 5.571793741558495, + "heading": 2.9123192627847865, + "angularVelocity": -2.75479375784366, + "velocityX": 0.5278042458729727, + "velocityY": -0.26883784352486495, + "timestamp": 0.08301192659671743 + }, + { + "x": 1.3598509429602765, + "y": 5.50482600027521, + "heading": 2.837562269615421, + "angularVelocity": -0.9005572600735416, + "velocityX": 1.0799766765779808, + "velocityY": -0.806724335029887, + "timestamp": 0.16602385319343485 + }, + { + "x": 1.459326562321469, + "y": 5.413786422039427, + "heading": 2.837562034760696, + "angularVelocity": -0.000002829168462845694, + "velocityX": 1.1983292454403298, + "velocityY": -1.0967047985533822, + "timestamp": 0.2490357797901523 + }, + { + "x": 1.558802177818681, + "y": 5.322746839581507, + "heading": 2.837561799906192, + "angularVelocity": -0.000002829165803381977, + "velocityX": 1.198329198893034, + "velocityY": -1.0967048494152307, + "timestamp": 0.3320477063868697 + }, + { + "x": 1.6582777933158976, + "y": 5.231707257123597, + "heading": 2.8375615650516535, + "angularVelocity": -0.0000028291662173205513, + "velocityX": 1.1983291988930937, + "velocityY": -1.0967048494150837, + "timestamp": 0.4150596329835871 + }, + { + "x": 1.7577534088131206, + "y": 5.140667674665702, + "heading": 2.837561330197081, + "angularVelocity": -0.0000028291666305712727, + "velocityX": 1.19832919889317, + "velocityY": -1.0967048494149187, + "timestamp": 0.4980715595803045 + }, + { + "x": 1.8572290243103502, + "y": 5.04962809220782, + "heading": 2.837561095342474, + "angularVelocity": -0.000002829167043337366, + "velocityX": 1.1983291988932463, + "velocityY": -1.0967048494147538, + "timestamp": 0.581083486177022 + }, + { + "x": 1.9567046398075858, + "y": 4.958588509749952, + "heading": 2.8375608604878324, + "angularVelocity": -0.000002829167457373439, + "velocityX": 1.198329198893323, + "velocityY": -1.0967048494145886, + "timestamp": 0.6640954127737394 + }, + { + "x": 2.056180255304828, + "y": 4.867548927292098, + "heading": 2.8375606256331567, + "angularVelocity": -0.00000282916787026248, + "velocityX": 1.1983291988933988, + "velocityY": -1.096704849414424, + "timestamp": 0.7471073393704568 + }, + { + "x": 2.1556558708020765, + "y": 4.776509344834258, + "heading": 2.8375603907784472, + "angularVelocity": -0.0000028291682836294088, + "velocityX": 1.1983291988934754, + "velocityY": -1.0967048494142588, + "timestamp": 0.8301192659671742 + }, + { + "x": 2.2551314862993315, + "y": 4.685469762376432, + "heading": 2.8375601559237027, + "angularVelocity": -0.000002829168697323748, + "velocityX": 1.1983291988935516, + "velocityY": -1.096704849414094, + "timestamp": 0.9131311925638916 + }, + { + "x": 2.354607101796593, + "y": 4.594430179918619, + "heading": 2.8375599210689244, + "angularVelocity": -0.000002829169110584532, + "velocityX": 1.1983291988936342, + "velocityY": -1.096704849413922, + "timestamp": 0.996143119160609 + }, + { + "x": 2.45408271880603, + "y": 4.503390599113157, + "heading": 2.837559686214013, + "angularVelocity": -0.000002829170713915412, + "velocityX": 1.1983292171099962, + "velocityY": -1.0967048295089454, + "timestamp": 1.0791550457573265 + }, + { + "x": 2.5484359060007082, + "y": 4.424785420118879, + "heading": 2.7982046114916295, + "angularVelocity": -0.47408940300319863, + "velocityX": 1.13662206218943, + "velocityY": -0.9469142834878584, + "timestamp": 1.162166972354044 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": -2.3995421391526537, + "velocityX": 0.5485627867734783, + "velocityY": -0.4243619716588108, + "timestamp": 1.2451788989507615 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": 0, + "velocityX": -4.934633813021939e-37, + "velocityY": -2.767548277117811e-37, + "timestamp": 1.328190825547479 + }, + { + "x": 2.534366599021267, + "y": 4.442118048365455, + "heading": 2.6991662321397754, + "angularVelocity": 1.173988745719896, + "velocityX": -0.6987106220563037, + "velocityY": 0.6161074104533542, + "timestamp": 1.4135001923828505 + }, + { + "x": 2.4322324251030567, + "y": 4.53578051923557, + "heading": 2.6991662757513564, + "angularVelocity": 5.112167893680353e-7, + "velocityX": -1.1972210990067154, + "velocityY": 1.097915438182342, + "timestamp": 1.498809559218222 + }, + { + "x": 2.330098253355979, + "y": 4.629442992473233, + "heading": 2.6991663193628423, + "angularVelocity": 5.112156854714172e-7, + "velocityX": -1.1972210735566082, + "velocityY": 1.0979154659348471, + "timestamp": 1.5841189260535935 + }, + { + "x": 2.227964081608902, + "y": 4.723105465710898, + "heading": 2.6991663629743283, + "angularVelocity": 5.112156767559466e-7, + "velocityX": -1.1972210735566007, + "velocityY": 1.0979154659348562, + "timestamp": 1.669428292888965 + }, + { + "x": 2.1258299098618236, + "y": 4.816767938948561, + "heading": 2.6991664065858134, + "angularVelocity": 5.112156675644069e-7, + "velocityX": -1.1972210735566096, + "velocityY": 1.0979154659348476, + "timestamp": 1.7547376597243365 + }, + { + "x": 2.023695735807304, + "y": 4.910430409670039, + "heading": 2.6991664501973878, + "angularVelocity": 5.112167177081662e-7, + "velocityX": -1.1972211006045375, + "velocityY": 1.0979154364400177, + "timestamp": 1.840047026559708 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": 1.1341858822337967, + "velocityX": -0.702995210451579, + "velocityY": 0.6160140441054905, + "timestamp": 1.9253563933950795 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": -1.527841446263418e-36, + "velocityX": 0, + "velocityY": 0, + "timestamp": 2.010665760230451 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing2.1.traj b/src/main/deploy/choreo/FrontWing2.1.traj new file mode 100644 index 00000000..73d19d55 --- /dev/null +++ b/src/main/deploy/choreo/FrontWing2.1.traj @@ -0,0 +1,103 @@ +{ + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -1.8084842834156656e-35, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.347146156455349, + "y": 5.593360447165824, + "heading": 3.140264869371664, + "angularVelocity": -0.007271662750997594, + "velocityX": 1.1945189788836181, + "velocityY": -0.007419158267666702, + "timestamp": 0.10109525888490682 + }, + { + "x": 1.5113646664969056, + "y": 5.59234048587755, + "heading": 3.1402648691641017, + "angularVelocity": -2.053134778232203e-9, + "velocityX": 1.6243937831794193, + "velocityY": -0.010089110998122406, + "timestamp": 0.20219051776981364 + }, + { + "x": 1.6755831765384634, + "y": 5.591320524589276, + "heading": 3.14026486895654, + "angularVelocity": -2.053134775032819e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.010089110998122467, + "timestamp": 0.30328577665472045 + }, + { + "x": 1.8398016865800209, + "y": 5.5903005633010014, + "heading": 3.140264868748978, + "angularVelocity": -2.0531347455807303e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.010089110998122468, + "timestamp": 0.4043810355396273 + }, + { + "x": 2.0040201966215783, + "y": 5.589280602012727, + "heading": 3.1402648685414154, + "angularVelocity": -2.0531349299385344e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.01008911099812247, + "timestamp": 0.5054762944245341 + }, + { + "x": 2.168238706663136, + "y": 5.588260640724453, + "heading": 3.1402648683338534, + "angularVelocity": -2.053134785130435e-9, + "velocityX": 1.6243937831794293, + "velocityY": -0.010089110998122468, + "timestamp": 0.6065715533094409 + }, + { + "x": 2.3324572167046935, + "y": 5.587240679436179, + "heading": 3.1402648681262915, + "angularVelocity": -2.0531346533039746e-9, + "velocityX": 1.6243937831794288, + "velocityY": -0.010089110998122467, + "timestamp": 0.7076668121943477 + }, + { + "x": 2.49667572674625, + "y": 5.586220718147905, + "heading": 3.140264867918729, + "angularVelocity": -2.0531352108819935e-9, + "velocityX": 1.624393783179419, + "velocityY": -0.010089110998122406, + "timestamp": 0.8087620710792545 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": -0.0072716627525318345, + "velocityX": 1.1945189788836168, + "velocityY": -0.007419158267749976, + "timestamp": 0.9098573299641612 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": 0, + "velocityX": -2.8085786967275447e-37, + "velocityY": 1.7440158343416895e-35, + "timestamp": 1.010952588849068 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing2.traj b/src/main/deploy/choreo/FrontWing2.traj new file mode 100644 index 00000000..73d19d55 --- /dev/null +++ b/src/main/deploy/choreo/FrontWing2.traj @@ -0,0 +1,103 @@ +{ + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -1.8084842834156656e-35, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.347146156455349, + "y": 5.593360447165824, + "heading": 3.140264869371664, + "angularVelocity": -0.007271662750997594, + "velocityX": 1.1945189788836181, + "velocityY": -0.007419158267666702, + "timestamp": 0.10109525888490682 + }, + { + "x": 1.5113646664969056, + "y": 5.59234048587755, + "heading": 3.1402648691641017, + "angularVelocity": -2.053134778232203e-9, + "velocityX": 1.6243937831794193, + "velocityY": -0.010089110998122406, + "timestamp": 0.20219051776981364 + }, + { + "x": 1.6755831765384634, + "y": 5.591320524589276, + "heading": 3.14026486895654, + "angularVelocity": -2.053134775032819e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.010089110998122467, + "timestamp": 0.30328577665472045 + }, + { + "x": 1.8398016865800209, + "y": 5.5903005633010014, + "heading": 3.140264868748978, + "angularVelocity": -2.0531347455807303e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.010089110998122468, + "timestamp": 0.4043810355396273 + }, + { + "x": 2.0040201966215783, + "y": 5.589280602012727, + "heading": 3.1402648685414154, + "angularVelocity": -2.0531349299385344e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.01008911099812247, + "timestamp": 0.5054762944245341 + }, + { + "x": 2.168238706663136, + "y": 5.588260640724453, + "heading": 3.1402648683338534, + "angularVelocity": -2.053134785130435e-9, + "velocityX": 1.6243937831794293, + "velocityY": -0.010089110998122468, + "timestamp": 0.6065715533094409 + }, + { + "x": 2.3324572167046935, + "y": 5.587240679436179, + "heading": 3.1402648681262915, + "angularVelocity": -2.0531346533039746e-9, + "velocityX": 1.6243937831794288, + "velocityY": -0.010089110998122467, + "timestamp": 0.7076668121943477 + }, + { + "x": 2.49667572674625, + "y": 5.586220718147905, + "heading": 3.140264867918729, + "angularVelocity": -2.0531352108819935e-9, + "velocityX": 1.624393783179419, + "velocityY": -0.010089110998122406, + "timestamp": 0.8087620710792545 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": -0.0072716627525318345, + "velocityX": 1.1945189788836168, + "velocityY": -0.007419158267749976, + "timestamp": 0.9098573299641612 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": 0, + "velocityX": -2.8085786967275447e-37, + "velocityY": 1.7440158343416895e-35, + "timestamp": 1.010952588849068 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing3Contested5.1.traj b/src/main/deploy/choreo/FrontWing3Contested5.1.traj new file mode 100644 index 00000000..0cd984e3 --- /dev/null +++ b/src/main/deploy/choreo/FrontWing3Contested5.1.traj @@ -0,0 +1,166 @@ +{ + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": 0, + "velocityX": -1.009927502555409e-33, + "velocityY": 9.510006388327946e-34, + "timestamp": 0 + }, + { + "x": 1.2629372157532939, + "y": 5.6118681655215035, + "heading": 3.3611684682063676, + "angularVelocity": 2.615104090958444, + "velocityX": 0.4341464636351002, + "velocityY": 0.21092108774289103, + "timestamp": 0.08419109165389853 + }, + { + "x": 1.3467908645165432, + "y": 5.672396843669079, + "heading": 3.470572560445472, + "angularVelocity": 1.2994734964223353, + "velocityX": 0.9959919406671149, + "velocityY": 0.718943975645346, + "timestamp": 0.16838218330779706 + }, + { + "x": 1.4464012333437837, + "y": 5.766107211983687, + "heading": 3.4705728197139094, + "angularVelocity": 0.000003079523404990731, + "velocityX": 1.1831461841203954, + "velocityY": 1.1130675048120657, + "timestamp": 0.25257327496169557 + }, + { + "x": 1.5460115900456228, + "y": 5.859817593187631, + "heading": 3.4705730789812166, + "angularVelocity": 0.000003079509987095674, + "velocityX": 1.1831460400980154, + "velocityY": 1.1130676579082395, + "timestamp": 0.3367643666155941 + }, + { + "x": 1.645621946747464, + "y": 5.953527974391563, + "heading": 3.470573338248568, + "angularVelocity": 0.000003079510511134596, + "velocityX": 1.1831460400980387, + "velocityY": 1.1130676579081111, + "timestamp": 0.42095545826949265 + }, + { + "x": 1.7452323034493133, + "y": 6.047238355595477, + "heading": 3.470573597515964, + "angularVelocity": 0.000003079511034719833, + "velocityX": 1.183146040098137, + "velocityY": 1.1130676579079029, + "timestamp": 0.5051465499233911 + }, + { + "x": 1.844842660151171, + "y": 6.140948736799373, + "heading": 3.470573856783404, + "angularVelocity": 0.0000030795115586857372, + "velocityX": 1.1831460400982357, + "velocityY": 1.1130676579076944, + "timestamp": 0.5893376415772896 + }, + { + "x": 1.944453016853037, + "y": 6.234659118003253, + "heading": 3.470574116050888, + "angularVelocity": 0.0000030795120819840736, + "velocityX": 1.1831460400983345, + "velocityY": 1.1130676579074863, + "timestamp": 0.6735287332311881 + }, + { + "x": 2.044063373554911, + "y": 6.328369499207114, + "heading": 3.470574375318416, + "angularVelocity": 0.0000030795126059487745, + "velocityX": 1.1831460400984328, + "velocityY": 1.1130676579072778, + "timestamp": 0.7577198248850866 + }, + { + "x": 2.1436737302567934, + "y": 6.4220798804109585, + "heading": 3.470574634585988, + "angularVelocity": 0.000003079513129853114, + "velocityX": 1.183146040098531, + "velocityY": 1.1130676579070695, + "timestamp": 0.8419109165389851 + }, + { + "x": 2.243284086958684, + "y": 6.515790261614786, + "heading": 3.470574893853604, + "angularVelocity": 0.0000030795136536386766, + "velocityX": 1.1831460400986302, + "velocityY": 1.1130676579068612, + "timestamp": 0.9261020081928836 + }, + { + "x": 2.342894443660583, + "y": 6.6095006428185945, + "heading": 3.4705751531212647, + "angularVelocity": 0.0000030795141771132826, + "velocityX": 1.1831460400987281, + "velocityY": 1.1130676579066532, + "timestamp": 1.010293099846782 + }, + { + "x": 2.442504800362493, + "y": 6.703211024022384, + "heading": 3.470575412388969, + "angularVelocity": 0.0000030795147010529834, + "velocityX": 1.1831460400988572, + "velocityY": 1.1130676579064127, + "timestamp": 1.0944841915006807 + }, + { + "x": 2.5421151619283058, + "y": 6.79692140005584, + "heading": 3.470575671657143, + "angularVelocity": 0.0000030795202773893193, + "velocityX": 1.1831460978710306, + "velocityY": 1.1130675964945436, + "timestamp": 1.1786752831545793 + }, + { + "x": 2.632005363421341, + "y": 6.868564036243049, + "heading": 3.5437695268680747, + "angularVelocity": 0.8693776713553536, + "velocityX": 1.0676925518743172, + "velocityY": 0.8509526932103911, + "timestamp": 1.2628663748084779 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": 2.207199307384385, + "velocityX": 0.46866491549483613, + "velocityY": 0.37605700085290744, + "timestamp": 1.3470574664623765 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": 0, + "velocityX": -1.5194658088032264e-34, + "velocityY": -2.7343286363371283e-33, + "timestamp": 1.431248558116275 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing3Contested5.2.traj b/src/main/deploy/choreo/FrontWing3Contested5.2.traj new file mode 100644 index 00000000..17798e6e --- /dev/null +++ b/src/main/deploy/choreo/FrontWing3Contested5.2.traj @@ -0,0 +1,679 @@ +{ + "samples": [ + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": 0, + "velocityX": -1.5194658088032264e-34, + "velocityY": -2.7343286363371283e-33, + "timestamp": 0 + }, + { + "x": 2.7208277247721226, + "y": 6.912165859423971, + "heading": 3.486294740771461, + "angularVelocity": -2.694751338131124, + "velocityX": 0.5467552516805291, + "velocityY": 0.13225779416916014, + "timestamp": 0.09028710806829743 + }, + { + "x": 2.839209601133149, + "y": 6.942185820162622, + "heading": 3.3899477762404344, + "angularVelocity": -1.0671176272269702, + "velocityX": 1.311171427392232, + "velocityY": 0.33249443227201864, + "timestamp": 0.18057421613659486 + }, + { + "x": 2.985101375777118, + "y": 6.957221507964457, + "heading": 3.3899472751996993, + "angularVelocity": -0.0000055494161427999685, + "velocityX": 1.615864964171956, + "velocityY": 0.16653194596133747, + "timestamp": 0.2708613242048923 + }, + { + "x": 3.130993151670898, + "y": 6.97225718364292, + "heading": 3.3899467741601925, + "angularVelocity": -0.000005549402536979108, + "velocityX": 1.6158649780145908, + "velocityY": 0.1665318116855625, + "timestamp": 0.3611484322731897 + }, + { + "x": 3.2768849275645753, + "y": 6.98729285932146, + "heading": 3.3899462731201484, + "angularVelocity": -0.00000554940849560343, + "velocityX": 1.6158649780134444, + "velocityY": 0.1665318116864077, + "timestamp": 0.45143554034148714 + }, + { + "x": 3.4227767034581484, + "y": 7.002328535000082, + "heading": 3.3899457720795656, + "angularVelocity": -0.0000055494144557147385, + "velocityX": 1.615864978012292, + "velocityY": 0.1665318116873102, + "timestamp": 0.5417226484097846 + }, + { + "x": 3.5686684793516172, + "y": 7.017364210678785, + "heading": 3.3899452710384446, + "angularVelocity": -0.0000055494204159770735, + "velocityX": 1.6158649780111394, + "velocityY": 0.1665318116882126, + "timestamp": 0.632009756478082 + }, + { + "x": 3.7145602552449817, + "y": 7.03239988635757, + "heading": 3.3899447699967857, + "angularVelocity": -0.000005549426375721003, + "velocityX": 1.6158649780099874, + "velocityY": 0.1665318116891151, + "timestamp": 0.7222968645463794 + }, + { + "x": 3.8604520311382426, + "y": 7.047435562036436, + "heading": 3.3899442689545887, + "angularVelocity": -0.000005549432334941507, + "velocityX": 1.615864978008835, + "velocityY": 0.16653181169001743, + "timestamp": 0.8125839726146769 + }, + { + "x": 4.006343807031399, + "y": 7.062471237715384, + "heading": 3.389943767911854, + "angularVelocity": -0.000005549438294605419, + "velocityX": 1.6158649780076826, + "velocityY": 0.16653181169091993, + "timestamp": 0.9028710806829743 + }, + { + "x": 4.152235582924452, + "y": 7.077506913394413, + "heading": 3.3899432668685807, + "angularVelocity": -0.000005549444254929096, + "velocityX": 1.6158649780065304, + "velocityY": 0.16653181169182238, + "timestamp": 0.9931581887512717 + }, + { + "x": 4.298127358817401, + "y": 7.092542589073523, + "heading": 3.3899427658247694, + "angularVelocity": -0.000005549450214439604, + "velocityX": 1.615864978005378, + "velocityY": 0.1665318116927247, + "timestamp": 1.0834452968195691 + }, + { + "x": 4.444019134710246, + "y": 7.107578264752715, + "heading": 3.38994226478042, + "angularVelocity": -0.000005549456174760312, + "velocityX": 1.6158649780042256, + "velocityY": 0.16653181169362713, + "timestamp": 1.1737324048878666 + }, + { + "x": 4.5899109106029865, + "y": 7.122613940431989, + "heading": 3.389941763735532, + "angularVelocity": -0.000005549462134716656, + "velocityX": 1.6158649780030732, + "velocityY": 0.16653181169452963, + "timestamp": 1.264019512956164 + }, + { + "x": 4.735802686495623, + "y": 7.137649616111343, + "heading": 3.389941262690107, + "angularVelocity": -0.0000055494680950242725, + "velocityX": 1.6158649780019205, + "velocityY": 0.1665318116954319, + "timestamp": 1.3543066210244614 + }, + { + "x": 4.881694462388156, + "y": 7.15268529179078, + "heading": 3.389940761644143, + "angularVelocity": -0.000005549474055578583, + "velocityX": 1.6158649780007681, + "velocityY": 0.16653181169633444, + "timestamp": 1.4445937290927588 + }, + { + "x": 5.027586238280584, + "y": 7.167720967470297, + "heading": 3.3899402605976414, + "angularVelocity": -0.000005549480015574405, + "velocityX": 1.6158649779996155, + "velocityY": 0.1665318116972368, + "timestamp": 1.5348808371610563 + }, + { + "x": 5.173478014172908, + "y": 7.182756643149896, + "heading": 3.389939759550601, + "angularVelocity": -0.000005549485976020103, + "velocityX": 1.6158649779984633, + "velocityY": 0.16653181169813924, + "timestamp": 1.6251679452293537 + }, + { + "x": 5.319369790065129, + "y": 7.197792318829577, + "heading": 3.389939258503023, + "angularVelocity": -0.000005549491936617687, + "velocityX": 1.6158649779973107, + "velocityY": 0.16653181169904166, + "timestamp": 1.7154550532976511 + }, + { + "x": 5.4652615659572445, + "y": 7.212827994509339, + "heading": 3.3899387574549067, + "angularVelocity": -0.00000554949789662324, + "velocityX": 1.6158649779961582, + "velocityY": 0.1665318116999441, + "timestamp": 1.8057421613659486 + }, + { + "x": 5.611153341849257, + "y": 7.2278636701891825, + "heading": 3.389938256406252, + "angularVelocity": -0.000005549503857497513, + "velocityX": 1.6158649779950054, + "velocityY": 0.16653181170084652, + "timestamp": 1.896029269434246 + }, + { + "x": 5.757045117741165, + "y": 7.242899345869108, + "heading": 3.3899377553570593, + "angularVelocity": -0.000005549509818264508, + "velocityX": 1.615864977993853, + "velocityY": 0.16653181170174894, + "timestamp": 1.9863163775025434 + }, + { + "x": 5.902936893632969, + "y": 7.257935021549114, + "heading": 3.3899372543073283, + "angularVelocity": -0.000005549515778536107, + "velocityX": 1.6158649779927006, + "velocityY": 0.1665318117026513, + "timestamp": 2.076603485570841 + }, + { + "x": 6.048828669524668, + "y": 7.272970697229202, + "heading": 3.3899367532570595, + "angularVelocity": -0.000005549521739714091, + "velocityX": 1.6158649779915477, + "velocityY": 0.16653181170355377, + "timestamp": 2.1668905936391383 + }, + { + "x": 6.194720445416264, + "y": 7.288006372909372, + "heading": 3.3899362522062524, + "angularVelocity": -0.000005549527700309559, + "velocityX": 1.6158649779903953, + "velocityY": 0.16653181170445627, + "timestamp": 2.2571777017074357 + }, + { + "x": 6.340612221307755, + "y": 7.303042048589623, + "heading": 3.389935751154907, + "angularVelocity": -0.000005549533660762894, + "velocityX": 1.6158649779892427, + "velocityY": 0.16653181170535863, + "timestamp": 2.347464809775733 + }, + { + "x": 6.486503997199144, + "y": 7.318077724269955, + "heading": 3.3899352501030235, + "angularVelocity": -0.000005549539621393899, + "velocityX": 1.6158649779880898, + "velocityY": 0.16653181170626094, + "timestamp": 2.4377519178440306 + }, + { + "x": 6.632395773090427, + "y": 7.333113399950369, + "heading": 3.3899347490506018, + "angularVelocity": -0.00000554954558299688, + "velocityX": 1.6158649779869374, + "velocityY": 0.1665318117071635, + "timestamp": 2.528039025912328 + }, + { + "x": 6.778287548981607, + "y": 7.3481490756308645, + "heading": 3.3899342479976413, + "angularVelocity": -0.000005549551543569729, + "velocityX": 1.615864977985785, + "velocityY": 0.16653181170806597, + "timestamp": 2.6183261339806254 + }, + { + "x": 6.924179324872682, + "y": 7.363184751311442, + "heading": 3.3899337469441435, + "angularVelocity": -0.000005549557505439446, + "velocityX": 1.615864977984632, + "velocityY": 0.16653181170896839, + "timestamp": 2.708613242048923 + }, + { + "x": 7.070071100763654, + "y": 7.378220426992101, + "heading": 3.389933245890107, + "angularVelocity": -0.00000554956346606953, + "velocityX": 1.6158649779834795, + "velocityY": 0.1665318117098707, + "timestamp": 2.7989003501172203 + }, + { + "x": 7.215962876654522, + "y": 7.393256102672841, + "heading": 3.3899327448355323, + "angularVelocity": -0.000005549569427174084, + "velocityX": 1.6158649779823266, + "velocityY": 0.16653181171077316, + "timestamp": 2.8891874581855177 + }, + { + "x": 7.361854652545285, + "y": 7.408291778353663, + "heading": 3.3899322437804194, + "angularVelocity": -0.000005549575389475234, + "velocityX": 1.615864977981174, + "velocityY": 0.16653181171167572, + "timestamp": 2.979474566253815 + }, + { + "x": 7.5077464284359445, + "y": 7.423327454034565, + "heading": 3.389931742724768, + "angularVelocity": -0.000005549581350385169, + "velocityX": 1.6158649779800212, + "velocityY": 0.1665318117125781, + "timestamp": 3.0697616743221126 + }, + { + "x": 7.6536382043265, + "y": 7.43836312971555, + "heading": 3.3899312416685787, + "angularVelocity": -0.000005549587311867374, + "velocityX": 1.6158649779788683, + "velocityY": 0.166531811713483, + "timestamp": 3.16004878239041 + }, + { + "x": 7.799529980161501, + "y": 7.453398805927109, + "heading": 3.3899307406089574, + "angularVelocity": -0.000005549625325450788, + "velocityX": 1.6158649773635676, + "velocityY": 0.16653181759000288, + "timestamp": 3.2503358904587074 + }, + { + "x": 7.908273189210625, + "y": 7.465055953097054, + "heading": 3.2483837637195303, + "angularVelocity": -1.5677429471143751, + "velocityX": 1.204415684317477, + "velocityY": 0.12911197865732152, + "timestamp": 3.340622998527005 + }, + { + "x": 7.936469078063965, + "y": 7.452759265899658, + "heading": 3.1300325241315123, + "angularVelocity": -1.3108321012839574, + "velocityX": 0.3122914163117384, + "velocityY": -0.13619538226979142, + "timestamp": 3.4309101065953023 + }, + { + "x": 7.897313987574637, + "y": 7.4320035366195905, + "heading": 3.223159902402127, + "angularVelocity": 0.9773255855261127, + "velocityX": -0.4109132292719665, + "velocityY": -0.21782107097140077, + "timestamp": 3.5261980839590406 + }, + { + "x": 7.764258802337476, + "y": 7.410354345348057, + "heading": 3.302887545584371, + "angularVelocity": 0.8367020204228218, + "velocityX": -1.396348090475833, + "velocityY": -0.22719751085588358, + "timestamp": 3.621486061322779 + }, + { + "x": 7.611104941895525, + "y": 7.387923016344696, + "heading": 3.302889098211225, + "angularVelocity": 0.000016294047758499195, + "velocityX": -1.607273705236954, + "velocityY": -0.23540565792192894, + "timestamp": 3.716774038686517 + }, + { + "x": 7.457951081309142, + "y": 7.365491688274562, + "heading": 3.302890650804134, + "angularVelocity": 0.000016293691520657764, + "velocityX": -1.6072737067526932, + "velocityY": -0.2354056481281831, + "timestamp": 3.8120620160502554 + }, + { + "x": 7.30479722071746, + "y": 7.343060360198914, + "heading": 3.3028922033698884, + "angularVelocity": 0.000016293406553711894, + "velocityX": -1.6072737068083087, + "velocityY": -0.23540564818605267, + "timestamp": 3.9073499934139937 + }, + { + "x": 7.151643360120445, + "y": 7.320629032117711, + "heading": 3.302893755908325, + "angularVelocity": 0.00001629311986139254, + "velocityX": -1.6072737068642717, + "velocityY": -0.23540564824432697, + "timestamp": 4.002637970777732 + }, + { + "x": 6.998489499518071, + "y": 7.298197704030925, + "heading": 3.302895308419307, + "angularVelocity": 0.00001629283173826444, + "velocityX": -1.6072737069205232, + "velocityY": -0.2354056483029315, + "timestamp": 4.09792594814147 + }, + { + "x": 6.845335638910315, + "y": 7.275766375938532, + "heading": 3.3028968609027314, + "angularVelocity": 0.000016292542536041963, + "velocityX": -1.6072737069769925, + "velocityY": -0.23540564836178338, + "timestamp": 4.1932139255052086 + }, + { + "x": 6.692181778297163, + "y": 7.253335047840514, + "heading": 3.302898413358527, + "angularVelocity": 0.000016292252586671315, + "velocityX": -1.6072737070336132, + "velocityY": -0.23540564842080344, + "timestamp": 4.288501902868947 + }, + { + "x": 6.539027917678609, + "y": 7.230903719736863, + "heading": 3.3028999657866516, + "angularVelocity": 0.000016291962193735624, + "velocityX": -1.607273707090324, + "velocityY": -0.23540564847991918, + "timestamp": 4.383789880232685 + }, + { + "x": 6.385874057054646, + "y": 7.208472391627575, + "heading": 3.302901518187087, + "angularVelocity": 0.000016291671606816483, + "velocityX": -1.607273707147075, + "velocityY": -0.2354056485390708, + "timestamp": 4.479077857596423 + }, + { + "x": 6.232720196425276, + "y": 7.186041063512652, + "heading": 3.3029030705598315, + "angularVelocity": 0.00001629138100746966, + "velocityX": -1.60727370720383, + "velocityY": -0.23540564859821628, + "timestamp": 4.574365834960162 + }, + { + "x": 6.079566335790499, + "y": 7.1636097353920976, + "heading": 3.3029046229048977, + "angularVelocity": 0.000016291090533089062, + "velocityX": -1.6072737072605614, + "velocityY": -0.23540564865732222, + "timestamp": 4.6696538123239 + }, + { + "x": 5.926412475150321, + "y": 7.141178407265915, + "heading": 3.302906175222303, + "angularVelocity": 0.000016290800243766995, + "velocityX": -1.6072737073172572, + "velocityY": -0.23540564871637523, + "timestamp": 4.764941789687638 + }, + { + "x": 5.773258614504743, + "y": 7.118747079134112, + "heading": 3.3029077275120664, + "angularVelocity": 0.00001629051016274816, + "velocityX": -1.6072737073739125, + "velocityY": -0.23540564877536937, + "timestamp": 4.8602297670513765 + }, + { + "x": 5.62010475385377, + "y": 7.096315750996691, + "heading": 3.302909279774206, + "angularVelocity": 0.000016290220263808567, + "velocityX": -1.6072737074305328, + "velocityY": -0.23540564883431206, + "timestamp": 4.955517744415115 + }, + { + "x": 5.466950893197405, + "y": 7.073884422853658, + "heading": 3.302910832008734, + "angularVelocity": 0.00001628993049423592, + "velocityX": -1.6072737074871284, + "velocityY": -0.23540564889321514, + "timestamp": 5.050805721778853 + }, + { + "x": 5.313797032535647, + "y": 7.051453094705014, + "heading": 3.3029123842156554, + "angularVelocity": 0.000016289640769266148, + "velocityX": -1.6072737075437156, + "velocityY": -0.23540564895209914, + "timestamp": 5.146093699142591 + }, + { + "x": 5.160643171868496, + "y": 7.02902176655076, + "heading": 3.3029139363949653, + "angularVelocity": 0.000016289351008016845, + "velocityX": -1.6072737076003119, + "velocityY": -0.23540564901098462, + "timestamp": 5.24138167650633 + }, + { + "x": 5.0074893111959495, + "y": 7.006590438390892, + "heading": 3.302915488546652, + "angularVelocity": 0.000016289061112899286, + "velocityX": -1.6072737076569361, + "velocityY": -0.23540564906989445, + "timestamp": 5.336669653870068 + }, + { + "x": 4.854335450518003, + "y": 6.984159110225406, + "heading": 3.3029170406706947, + "angularVelocity": 0.000016288771004663956, + "velocityX": -1.607273707713604, + "velocityY": -0.23540564912884793, + "timestamp": 5.431957631233806 + }, + { + "x": 4.701181589834651, + "y": 6.961727782054296, + "heading": 3.3029185927670666, + "angularVelocity": 0.00001628848060846621, + "velocityX": -1.6072737077703305, + "velocityY": -0.235405649187863, + "timestamp": 5.5272456085975445 + }, + { + "x": 4.548027729145889, + "y": 6.939296453877556, + "heading": 3.3029201448357353, + "angularVelocity": 0.000016288189883493963, + "velocityX": -1.607273707827124, + "velocityY": -0.23540564924694996, + "timestamp": 5.622533585961283 + }, + { + "x": 4.3948738684517075, + "y": 6.916865125695178, + "heading": 3.3029216968766675, + "angularVelocity": 0.00001628789879930085, + "velocityX": -1.6072737078839905, + "velocityY": -0.23540564930611646, + "timestamp": 5.717821563325021 + }, + { + "x": 4.2417200077521, + "y": 6.894433797507156, + "heading": 3.302923248889828, + "angularVelocity": 0.00001628760735122893, + "velocityX": -1.6072737079409307, + "velocityY": -0.23540564936536426, + "timestamp": 5.813109540688759 + }, + { + "x": 4.088566147047059, + "y": 6.872002469313479, + "heading": 3.3029248008751826, + "angularVelocity": 0.000016287315540072175, + "velocityX": -1.6072737079979444, + "velocityY": -0.23540564942469375, + "timestamp": 5.908397518052498 + }, + { + "x": 3.9354122863365792, + "y": 6.849571141114141, + "heading": 3.3029263528326944, + "angularVelocity": 0.000016287023346622306, + "velocityX": -1.6072737080550357, + "velocityY": -0.23540564948410944, + "timestamp": 6.003685495416236 + }, + { + "x": 3.7822584256206495, + "y": 6.827139812909132, + "heading": 3.3029279047623197, + "angularVelocity": 0.00001628673068839883, + "velocityX": -1.6072737081122201, + "velocityY": -0.23540564954363288, + "timestamp": 6.098973472779974 + }, + { + "x": 3.629104564899259, + "y": 6.804708484698436, + "heading": 3.3029294566639957, + "angularVelocity": 0.000016286437378238427, + "velocityX": -1.607273708169536, + "velocityY": -0.2354056496033085, + "timestamp": 6.1942614501437125 + }, + { + "x": 3.4759507041723876, + "y": 6.7822771564820306, + "heading": 3.302931008537628, + "angularVelocity": 0.00001628614306837342, + "velocityX": -1.6072737082270525, + "velocityY": -0.2354056496632214, + "timestamp": 6.289549427507451 + }, + { + "x": 3.3227968434400057, + "y": 6.759845828259881, + "heading": 3.3029325603830677, + "angularVelocity": 0.000016285847206848686, + "velocityX": -1.6072737082848803, + "velocityY": -0.23540564972350414, + "timestamp": 6.384837404871189 + }, + { + "x": 3.1696429827020713, + "y": 6.737414500031936, + "heading": 3.3029341122001052, + "angularVelocity": 0.00001628554913981109, + "velocityX": -1.6072737083431483, + "velocityY": -0.2354056497843278, + "timestamp": 6.480125382234927 + }, + { + "x": 3.016489122487879, + "y": 6.714983168191867, + "heading": 3.3029356639933174, + "angularVelocity": 0.000016285299102484297, + "velocityX": -1.6072737028467396, + "velocityY": -0.23540568769178183, + "timestamp": 6.575413359598667 + }, + { + "x": 2.8768170839321456, + "y": 6.685183755290752, + "heading": 3.3517957161483998, + "angularVelocity": 0.512761982223329, + "velocityX": -1.465788679956645, + "velocityY": -0.31273003925103915, + "timestamp": 6.670701336962406 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": 2.3762078534459548, + "velocityX": -0.6771226429407677, + "velocityY": -0.18518169587388544, + "timestamp": 6.765989314326145 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": 4.7042853211430965e-33, + "velocityX": -2.4138813228334137e-34, + "velocityY": 0, + "timestamp": 6.861277291689884 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing3Contested5.traj b/src/main/deploy/choreo/FrontWing3Contested5.traj new file mode 100644 index 00000000..c412d812 --- /dev/null +++ b/src/main/deploy/choreo/FrontWing3Contested5.traj @@ -0,0 +1,832 @@ +{ + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": 0, + "velocityX": -1.009927502555409e-33, + "velocityY": 9.510006388327946e-34, + "timestamp": 0 + }, + { + "x": 1.2629372157532939, + "y": 5.6118681655215035, + "heading": 3.3611684682063676, + "angularVelocity": 2.615104090958444, + "velocityX": 0.4341464636351002, + "velocityY": 0.21092108774289103, + "timestamp": 0.08419109165389853 + }, + { + "x": 1.3467908645165432, + "y": 5.672396843669079, + "heading": 3.470572560445472, + "angularVelocity": 1.2994734964223353, + "velocityX": 0.9959919406671149, + "velocityY": 0.718943975645346, + "timestamp": 0.16838218330779706 + }, + { + "x": 1.4464012333437837, + "y": 5.766107211983687, + "heading": 3.4705728197139094, + "angularVelocity": 0.000003079523404990731, + "velocityX": 1.1831461841203954, + "velocityY": 1.1130675048120657, + "timestamp": 0.25257327496169557 + }, + { + "x": 1.5460115900456228, + "y": 5.859817593187631, + "heading": 3.4705730789812166, + "angularVelocity": 0.000003079509987095674, + "velocityX": 1.1831460400980154, + "velocityY": 1.1130676579082395, + "timestamp": 0.3367643666155941 + }, + { + "x": 1.645621946747464, + "y": 5.953527974391563, + "heading": 3.470573338248568, + "angularVelocity": 0.000003079510511134596, + "velocityX": 1.1831460400980387, + "velocityY": 1.1130676579081111, + "timestamp": 0.42095545826949265 + }, + { + "x": 1.7452323034493133, + "y": 6.047238355595477, + "heading": 3.470573597515964, + "angularVelocity": 0.000003079511034719833, + "velocityX": 1.183146040098137, + "velocityY": 1.1130676579079029, + "timestamp": 0.5051465499233911 + }, + { + "x": 1.844842660151171, + "y": 6.140948736799373, + "heading": 3.470573856783404, + "angularVelocity": 0.0000030795115586857372, + "velocityX": 1.1831460400982357, + "velocityY": 1.1130676579076944, + "timestamp": 0.5893376415772896 + }, + { + "x": 1.944453016853037, + "y": 6.234659118003253, + "heading": 3.470574116050888, + "angularVelocity": 0.0000030795120819840736, + "velocityX": 1.1831460400983345, + "velocityY": 1.1130676579074863, + "timestamp": 0.6735287332311881 + }, + { + "x": 2.044063373554911, + "y": 6.328369499207114, + "heading": 3.470574375318416, + "angularVelocity": 0.0000030795126059487745, + "velocityX": 1.1831460400984328, + "velocityY": 1.1130676579072778, + "timestamp": 0.7577198248850866 + }, + { + "x": 2.1436737302567934, + "y": 6.4220798804109585, + "heading": 3.470574634585988, + "angularVelocity": 0.000003079513129853114, + "velocityX": 1.183146040098531, + "velocityY": 1.1130676579070695, + "timestamp": 0.8419109165389851 + }, + { + "x": 2.243284086958684, + "y": 6.515790261614786, + "heading": 3.470574893853604, + "angularVelocity": 0.0000030795136536386766, + "velocityX": 1.1831460400986302, + "velocityY": 1.1130676579068612, + "timestamp": 0.9261020081928836 + }, + { + "x": 2.342894443660583, + "y": 6.6095006428185945, + "heading": 3.4705751531212647, + "angularVelocity": 0.0000030795141771132826, + "velocityX": 1.1831460400987281, + "velocityY": 1.1130676579066532, + "timestamp": 1.010293099846782 + }, + { + "x": 2.442504800362493, + "y": 6.703211024022384, + "heading": 3.470575412388969, + "angularVelocity": 0.0000030795147010529834, + "velocityX": 1.1831460400988572, + "velocityY": 1.1130676579064127, + "timestamp": 1.0944841915006807 + }, + { + "x": 2.5421151619283058, + "y": 6.79692140005584, + "heading": 3.470575671657143, + "angularVelocity": 0.0000030795202773893193, + "velocityX": 1.1831460978710306, + "velocityY": 1.1130675964945436, + "timestamp": 1.1786752831545793 + }, + { + "x": 2.632005363421341, + "y": 6.868564036243049, + "heading": 3.5437695268680747, + "angularVelocity": 0.8693776713553536, + "velocityX": 1.0676925518743172, + "velocityY": 0.8509526932103911, + "timestamp": 1.2628663748084779 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": 2.207199307384385, + "velocityX": 0.46866491549483613, + "velocityY": 0.37605700085290744, + "timestamp": 1.3470574664623765 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": 0, + "velocityX": -1.5194658088032264e-34, + "velocityY": -2.7343286363371283e-33, + "timestamp": 1.431248558116275 + }, + { + "x": 2.7208277247721226, + "y": 6.912165859423971, + "heading": 3.486294740771461, + "angularVelocity": -2.694751338131124, + "velocityX": 0.5467552516805291, + "velocityY": 0.13225779416916014, + "timestamp": 1.5215356661845725 + }, + { + "x": 2.839209601133149, + "y": 6.942185820162622, + "heading": 3.3899477762404344, + "angularVelocity": -1.0671176272269702, + "velocityX": 1.311171427392232, + "velocityY": 0.33249443227201864, + "timestamp": 1.61182277425287 + }, + { + "x": 2.985101375777118, + "y": 6.957221507964457, + "heading": 3.3899472751996993, + "angularVelocity": -0.0000055494161427999685, + "velocityX": 1.615864964171956, + "velocityY": 0.16653194596133747, + "timestamp": 1.7021098823211673 + }, + { + "x": 3.130993151670898, + "y": 6.97225718364292, + "heading": 3.3899467741601925, + "angularVelocity": -0.000005549402536979108, + "velocityX": 1.6158649780145908, + "velocityY": 0.1665318116855625, + "timestamp": 1.7923969903894648 + }, + { + "x": 3.2768849275645753, + "y": 6.98729285932146, + "heading": 3.3899462731201484, + "angularVelocity": -0.00000554940849560343, + "velocityX": 1.6158649780134444, + "velocityY": 0.1665318116864077, + "timestamp": 1.8826840984577622 + }, + { + "x": 3.4227767034581484, + "y": 7.002328535000082, + "heading": 3.3899457720795656, + "angularVelocity": -0.0000055494144557147385, + "velocityX": 1.615864978012292, + "velocityY": 0.1665318116873102, + "timestamp": 1.9729712065260596 + }, + { + "x": 3.5686684793516172, + "y": 7.017364210678785, + "heading": 3.3899452710384446, + "angularVelocity": -0.0000055494204159770735, + "velocityX": 1.6158649780111394, + "velocityY": 0.1665318116882126, + "timestamp": 2.063258314594357 + }, + { + "x": 3.7145602552449817, + "y": 7.03239988635757, + "heading": 3.3899447699967857, + "angularVelocity": -0.000005549426375721003, + "velocityX": 1.6158649780099874, + "velocityY": 0.1665318116891151, + "timestamp": 2.1535454226626545 + }, + { + "x": 3.8604520311382426, + "y": 7.047435562036436, + "heading": 3.3899442689545887, + "angularVelocity": -0.000005549432334941507, + "velocityX": 1.615864978008835, + "velocityY": 0.16653181169001743, + "timestamp": 2.243832530730952 + }, + { + "x": 4.006343807031399, + "y": 7.062471237715384, + "heading": 3.389943767911854, + "angularVelocity": -0.000005549438294605419, + "velocityX": 1.6158649780076826, + "velocityY": 0.16653181169091993, + "timestamp": 2.3341196387992493 + }, + { + "x": 4.152235582924452, + "y": 7.077506913394413, + "heading": 3.3899432668685807, + "angularVelocity": -0.000005549444254929096, + "velocityX": 1.6158649780065304, + "velocityY": 0.16653181169182238, + "timestamp": 2.4244067468675468 + }, + { + "x": 4.298127358817401, + "y": 7.092542589073523, + "heading": 3.3899427658247694, + "angularVelocity": -0.000005549450214439604, + "velocityX": 1.615864978005378, + "velocityY": 0.1665318116927247, + "timestamp": 2.514693854935844 + }, + { + "x": 4.444019134710246, + "y": 7.107578264752715, + "heading": 3.38994226478042, + "angularVelocity": -0.000005549456174760312, + "velocityX": 1.6158649780042256, + "velocityY": 0.16653181169362713, + "timestamp": 2.6049809630041416 + }, + { + "x": 4.5899109106029865, + "y": 7.122613940431989, + "heading": 3.389941763735532, + "angularVelocity": -0.000005549462134716656, + "velocityX": 1.6158649780030732, + "velocityY": 0.16653181169452963, + "timestamp": 2.695268071072439 + }, + { + "x": 4.735802686495623, + "y": 7.137649616111343, + "heading": 3.389941262690107, + "angularVelocity": -0.0000055494680950242725, + "velocityX": 1.6158649780019205, + "velocityY": 0.1665318116954319, + "timestamp": 2.7855551791407365 + }, + { + "x": 4.881694462388156, + "y": 7.15268529179078, + "heading": 3.389940761644143, + "angularVelocity": -0.000005549474055578583, + "velocityX": 1.6158649780007681, + "velocityY": 0.16653181169633444, + "timestamp": 2.875842287209034 + }, + { + "x": 5.027586238280584, + "y": 7.167720967470297, + "heading": 3.3899402605976414, + "angularVelocity": -0.000005549480015574405, + "velocityX": 1.6158649779996155, + "velocityY": 0.1665318116972368, + "timestamp": 2.9661293952773313 + }, + { + "x": 5.173478014172908, + "y": 7.182756643149896, + "heading": 3.389939759550601, + "angularVelocity": -0.000005549485976020103, + "velocityX": 1.6158649779984633, + "velocityY": 0.16653181169813924, + "timestamp": 3.0564165033456288 + }, + { + "x": 5.319369790065129, + "y": 7.197792318829577, + "heading": 3.389939258503023, + "angularVelocity": -0.000005549491936617687, + "velocityX": 1.6158649779973107, + "velocityY": 0.16653181169904166, + "timestamp": 3.146703611413926 + }, + { + "x": 5.4652615659572445, + "y": 7.212827994509339, + "heading": 3.3899387574549067, + "angularVelocity": -0.00000554949789662324, + "velocityX": 1.6158649779961582, + "velocityY": 0.1665318116999441, + "timestamp": 3.2369907194822236 + }, + { + "x": 5.611153341849257, + "y": 7.2278636701891825, + "heading": 3.389938256406252, + "angularVelocity": -0.000005549503857497513, + "velocityX": 1.6158649779950054, + "velocityY": 0.16653181170084652, + "timestamp": 3.327277827550521 + }, + { + "x": 5.757045117741165, + "y": 7.242899345869108, + "heading": 3.3899377553570593, + "angularVelocity": -0.000005549509818264508, + "velocityX": 1.615864977993853, + "velocityY": 0.16653181170174894, + "timestamp": 3.4175649356188185 + }, + { + "x": 5.902936893632969, + "y": 7.257935021549114, + "heading": 3.3899372543073283, + "angularVelocity": -0.000005549515778536107, + "velocityX": 1.6158649779927006, + "velocityY": 0.1665318117026513, + "timestamp": 3.507852043687116 + }, + { + "x": 6.048828669524668, + "y": 7.272970697229202, + "heading": 3.3899367532570595, + "angularVelocity": -0.000005549521739714091, + "velocityX": 1.6158649779915477, + "velocityY": 0.16653181170355377, + "timestamp": 3.5981391517554133 + }, + { + "x": 6.194720445416264, + "y": 7.288006372909372, + "heading": 3.3899362522062524, + "angularVelocity": -0.000005549527700309559, + "velocityX": 1.6158649779903953, + "velocityY": 0.16653181170445627, + "timestamp": 3.6884262598237108 + }, + { + "x": 6.340612221307755, + "y": 7.303042048589623, + "heading": 3.389935751154907, + "angularVelocity": -0.000005549533660762894, + "velocityX": 1.6158649779892427, + "velocityY": 0.16653181170535863, + "timestamp": 3.778713367892008 + }, + { + "x": 6.486503997199144, + "y": 7.318077724269955, + "heading": 3.3899352501030235, + "angularVelocity": -0.000005549539621393899, + "velocityX": 1.6158649779880898, + "velocityY": 0.16653181170626094, + "timestamp": 3.8690004759603056 + }, + { + "x": 6.632395773090427, + "y": 7.333113399950369, + "heading": 3.3899347490506018, + "angularVelocity": -0.00000554954558299688, + "velocityX": 1.6158649779869374, + "velocityY": 0.1665318117071635, + "timestamp": 3.959287584028603 + }, + { + "x": 6.778287548981607, + "y": 7.3481490756308645, + "heading": 3.3899342479976413, + "angularVelocity": -0.000005549551543569729, + "velocityX": 1.615864977985785, + "velocityY": 0.16653181170806597, + "timestamp": 4.0495746920969005 + }, + { + "x": 6.924179324872682, + "y": 7.363184751311442, + "heading": 3.3899337469441435, + "angularVelocity": -0.000005549557505439446, + "velocityX": 1.615864977984632, + "velocityY": 0.16653181170896839, + "timestamp": 4.139861800165198 + }, + { + "x": 7.070071100763654, + "y": 7.378220426992101, + "heading": 3.389933245890107, + "angularVelocity": -0.00000554956346606953, + "velocityX": 1.6158649779834795, + "velocityY": 0.1665318117098707, + "timestamp": 4.230148908233495 + }, + { + "x": 7.215962876654522, + "y": 7.393256102672841, + "heading": 3.3899327448355323, + "angularVelocity": -0.000005549569427174084, + "velocityX": 1.6158649779823266, + "velocityY": 0.16653181171077316, + "timestamp": 4.320436016301793 + }, + { + "x": 7.361854652545285, + "y": 7.408291778353663, + "heading": 3.3899322437804194, + "angularVelocity": -0.000005549575389475234, + "velocityX": 1.615864977981174, + "velocityY": 0.16653181171167572, + "timestamp": 4.41072312437009 + }, + { + "x": 7.5077464284359445, + "y": 7.423327454034565, + "heading": 3.389931742724768, + "angularVelocity": -0.000005549581350385169, + "velocityX": 1.6158649779800212, + "velocityY": 0.1665318117125781, + "timestamp": 4.501010232438388 + }, + { + "x": 7.6536382043265, + "y": 7.43836312971555, + "heading": 3.3899312416685787, + "angularVelocity": -0.000005549587311867374, + "velocityX": 1.6158649779788683, + "velocityY": 0.166531811713483, + "timestamp": 4.591297340506685 + }, + { + "x": 7.799529980161501, + "y": 7.453398805927109, + "heading": 3.3899307406089574, + "angularVelocity": -0.000005549625325450788, + "velocityX": 1.6158649773635676, + "velocityY": 0.16653181759000288, + "timestamp": 4.6815844485749825 + }, + { + "x": 7.908273189210625, + "y": 7.465055953097054, + "heading": 3.2483837637195303, + "angularVelocity": -1.5677429471143751, + "velocityX": 1.204415684317477, + "velocityY": 0.12911197865732152, + "timestamp": 4.77187155664328 + }, + { + "x": 7.936469078063965, + "y": 7.452759265899658, + "heading": 3.1300325241315123, + "angularVelocity": -1.3108321012839574, + "velocityX": 0.3122914163117384, + "velocityY": -0.13619538226979142, + "timestamp": 4.862158664711577 + }, + { + "x": 7.897313987574637, + "y": 7.4320035366195905, + "heading": 3.223159902402127, + "angularVelocity": 0.9773255855261127, + "velocityX": -0.4109132292719665, + "velocityY": -0.21782107097140077, + "timestamp": 4.957446642075316 + }, + { + "x": 7.764258802337476, + "y": 7.410354345348057, + "heading": 3.302887545584371, + "angularVelocity": 0.8367020204228218, + "velocityX": -1.396348090475833, + "velocityY": -0.22719751085588358, + "timestamp": 5.052734619439054 + }, + { + "x": 7.611104941895525, + "y": 7.387923016344696, + "heading": 3.302889098211225, + "angularVelocity": 0.000016294047758499195, + "velocityX": -1.607273705236954, + "velocityY": -0.23540565792192894, + "timestamp": 5.148022596802792 + }, + { + "x": 7.457951081309142, + "y": 7.365491688274562, + "heading": 3.302890650804134, + "angularVelocity": 0.000016293691520657764, + "velocityX": -1.6072737067526932, + "velocityY": -0.2354056481281831, + "timestamp": 5.2433105741665305 + }, + { + "x": 7.30479722071746, + "y": 7.343060360198914, + "heading": 3.3028922033698884, + "angularVelocity": 0.000016293406553711894, + "velocityX": -1.6072737068083087, + "velocityY": -0.23540564818605267, + "timestamp": 5.338598551530269 + }, + { + "x": 7.151643360120445, + "y": 7.320629032117711, + "heading": 3.302893755908325, + "angularVelocity": 0.00001629311986139254, + "velocityX": -1.6072737068642717, + "velocityY": -0.23540564824432697, + "timestamp": 5.433886528894007 + }, + { + "x": 6.998489499518071, + "y": 7.298197704030925, + "heading": 3.302895308419307, + "angularVelocity": 0.00001629283173826444, + "velocityX": -1.6072737069205232, + "velocityY": -0.2354056483029315, + "timestamp": 5.529174506257745 + }, + { + "x": 6.845335638910315, + "y": 7.275766375938532, + "heading": 3.3028968609027314, + "angularVelocity": 0.000016292542536041963, + "velocityX": -1.6072737069769925, + "velocityY": -0.23540564836178338, + "timestamp": 5.624462483621484 + }, + { + "x": 6.692181778297163, + "y": 7.253335047840514, + "heading": 3.302898413358527, + "angularVelocity": 0.000016292252586671315, + "velocityX": -1.6072737070336132, + "velocityY": -0.23540564842080344, + "timestamp": 5.719750460985222 + }, + { + "x": 6.539027917678609, + "y": 7.230903719736863, + "heading": 3.3028999657866516, + "angularVelocity": 0.000016291962193735624, + "velocityX": -1.607273707090324, + "velocityY": -0.23540564847991918, + "timestamp": 5.81503843834896 + }, + { + "x": 6.385874057054646, + "y": 7.208472391627575, + "heading": 3.302901518187087, + "angularVelocity": 0.000016291671606816483, + "velocityX": -1.607273707147075, + "velocityY": -0.2354056485390708, + "timestamp": 5.9103264157126985 + }, + { + "x": 6.232720196425276, + "y": 7.186041063512652, + "heading": 3.3029030705598315, + "angularVelocity": 0.00001629138100746966, + "velocityX": -1.60727370720383, + "velocityY": -0.23540564859821628, + "timestamp": 6.005614393076437 + }, + { + "x": 6.079566335790499, + "y": 7.1636097353920976, + "heading": 3.3029046229048977, + "angularVelocity": 0.000016291090533089062, + "velocityX": -1.6072737072605614, + "velocityY": -0.23540564865732222, + "timestamp": 6.100902370440175 + }, + { + "x": 5.926412475150321, + "y": 7.141178407265915, + "heading": 3.302906175222303, + "angularVelocity": 0.000016290800243766995, + "velocityX": -1.6072737073172572, + "velocityY": -0.23540564871637523, + "timestamp": 6.196190347803913 + }, + { + "x": 5.773258614504743, + "y": 7.118747079134112, + "heading": 3.3029077275120664, + "angularVelocity": 0.00001629051016274816, + "velocityX": -1.6072737073739125, + "velocityY": -0.23540564877536937, + "timestamp": 6.291478325167652 + }, + { + "x": 5.62010475385377, + "y": 7.096315750996691, + "heading": 3.302909279774206, + "angularVelocity": 0.000016290220263808567, + "velocityX": -1.6072737074305328, + "velocityY": -0.23540564883431206, + "timestamp": 6.38676630253139 + }, + { + "x": 5.466950893197405, + "y": 7.073884422853658, + "heading": 3.302910832008734, + "angularVelocity": 0.00001628993049423592, + "velocityX": -1.6072737074871284, + "velocityY": -0.23540564889321514, + "timestamp": 6.482054279895128 + }, + { + "x": 5.313797032535647, + "y": 7.051453094705014, + "heading": 3.3029123842156554, + "angularVelocity": 0.000016289640769266148, + "velocityX": -1.6072737075437156, + "velocityY": -0.23540564895209914, + "timestamp": 6.5773422572588665 + }, + { + "x": 5.160643171868496, + "y": 7.02902176655076, + "heading": 3.3029139363949653, + "angularVelocity": 0.000016289351008016845, + "velocityX": -1.6072737076003119, + "velocityY": -0.23540564901098462, + "timestamp": 6.672630234622605 + }, + { + "x": 5.0074893111959495, + "y": 7.006590438390892, + "heading": 3.302915488546652, + "angularVelocity": 0.000016289061112899286, + "velocityX": -1.6072737076569361, + "velocityY": -0.23540564906989445, + "timestamp": 6.767918211986343 + }, + { + "x": 4.854335450518003, + "y": 6.984159110225406, + "heading": 3.3029170406706947, + "angularVelocity": 0.000016288771004663956, + "velocityX": -1.607273707713604, + "velocityY": -0.23540564912884793, + "timestamp": 6.863206189350081 + }, + { + "x": 4.701181589834651, + "y": 6.961727782054296, + "heading": 3.3029185927670666, + "angularVelocity": 0.00001628848060846621, + "velocityX": -1.6072737077703305, + "velocityY": -0.235405649187863, + "timestamp": 6.95849416671382 + }, + { + "x": 4.548027729145889, + "y": 6.939296453877556, + "heading": 3.3029201448357353, + "angularVelocity": 0.000016288189883493963, + "velocityX": -1.607273707827124, + "velocityY": -0.23540564924694996, + "timestamp": 7.053782144077558 + }, + { + "x": 4.3948738684517075, + "y": 6.916865125695178, + "heading": 3.3029216968766675, + "angularVelocity": 0.00001628789879930085, + "velocityX": -1.6072737078839905, + "velocityY": -0.23540564930611646, + "timestamp": 7.149070121441296 + }, + { + "x": 4.2417200077521, + "y": 6.894433797507156, + "heading": 3.302923248889828, + "angularVelocity": 0.00001628760735122893, + "velocityX": -1.6072737079409307, + "velocityY": -0.23540564936536426, + "timestamp": 7.2443580988050345 + }, + { + "x": 4.088566147047059, + "y": 6.872002469313479, + "heading": 3.3029248008751826, + "angularVelocity": 0.000016287315540072175, + "velocityX": -1.6072737079979444, + "velocityY": -0.23540564942469375, + "timestamp": 7.339646076168773 + }, + { + "x": 3.9354122863365792, + "y": 6.849571141114141, + "heading": 3.3029263528326944, + "angularVelocity": 0.000016287023346622306, + "velocityX": -1.6072737080550357, + "velocityY": -0.23540564948410944, + "timestamp": 7.434934053532511 + }, + { + "x": 3.7822584256206495, + "y": 6.827139812909132, + "heading": 3.3029279047623197, + "angularVelocity": 0.00001628673068839883, + "velocityX": -1.6072737081122201, + "velocityY": -0.23540564954363288, + "timestamp": 7.530222030896249 + }, + { + "x": 3.629104564899259, + "y": 6.804708484698436, + "heading": 3.3029294566639957, + "angularVelocity": 0.000016286437378238427, + "velocityX": -1.607273708169536, + "velocityY": -0.2354056496033085, + "timestamp": 7.625510008259988 + }, + { + "x": 3.4759507041723876, + "y": 6.7822771564820306, + "heading": 3.302931008537628, + "angularVelocity": 0.00001628614306837342, + "velocityX": -1.6072737082270525, + "velocityY": -0.2354056496632214, + "timestamp": 7.720797985623726 + }, + { + "x": 3.3227968434400057, + "y": 6.759845828259881, + "heading": 3.3029325603830677, + "angularVelocity": 0.000016285847206848686, + "velocityX": -1.6072737082848803, + "velocityY": -0.23540564972350414, + "timestamp": 7.816085962987464 + }, + { + "x": 3.1696429827020713, + "y": 6.737414500031936, + "heading": 3.3029341122001052, + "angularVelocity": 0.00001628554913981109, + "velocityX": -1.6072737083431483, + "velocityY": -0.2354056497843278, + "timestamp": 7.9113739403512024 + }, + { + "x": 3.016489122487879, + "y": 6.714983168191867, + "heading": 3.3029356639933174, + "angularVelocity": 0.000016285299102484297, + "velocityX": -1.6072737028467396, + "velocityY": -0.23540568769178183, + "timestamp": 8.006661917714942 + }, + { + "x": 2.8768170839321456, + "y": 6.685183755290752, + "heading": 3.3517957161483998, + "angularVelocity": 0.512761982223329, + "velocityX": -1.465788679956645, + "velocityY": -0.31273003925103915, + "timestamp": 8.10194989507868 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": 2.3762078534459548, + "velocityX": -0.6771226429407677, + "velocityY": -0.18518169587388544, + "timestamp": 8.19723787244242 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": 4.7042853211430965e-33, + "velocityX": -2.4138813228334137e-34, + "velocityY": 0, + "timestamp": 8.29252584980616 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/NewPath.1.traj b/src/main/deploy/choreo/NewPath.1.traj new file mode 100644 index 00000000..38751a08 --- /dev/null +++ b/src/main/deploy/choreo/NewPath.1.traj @@ -0,0 +1,670 @@ +{ + "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 new file mode 100644 index 00000000..38751a08 --- /dev/null +++ b/src/main/deploy/choreo/NewPath.traj @@ -0,0 +1,670 @@ +{ + "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 index 35cbe979..f107b798 100644 --- a/src/main/deploy/choreo/Test1.1.traj +++ b/src/main/deploy/choreo/Test1.1.traj @@ -4,631 +4,595 @@ "x": 1.4869295358657837, "y": 5.564249038696289, "heading": 3.141, - "angularVelocity": 8.029589971503427e-22, - "velocityX": 1.7351458241819956e-21, - "velocityY": 6.201477276822387e-22, + "angularVelocity": 3.7405663636389993e-19, + "velocityX": -1.1249906625590385e-18, + "velocityY": 2.3803295269896554e-19, "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, + "x": 1.5092825889004946, + "y": 5.579306288055266, + "heading": 3.4062283948642453, + "angularVelocity": 3.352121601432042, + "velocityX": 0.282511802606821, + "velocityY": 0.19030289294706518, + "timestamp": 0.07912254577845217 + }, + { + "x": 1.5673643497428493, + "y": 5.60987650843196, + "heading": 3.626081158643754, + "angularVelocity": 2.77863612218835, + "velocityX": 0.7340734587204384, + "velocityY": 0.38636547997701703, + "timestamp": 0.15824509155690433 + }, + { + "x": 1.639918967505268, + "y": 5.662681063987355, + "heading": 3.7707265189248944, + "angularVelocity": 1.828118128126927, + "velocityX": 0.9169904361466855, + "velocityY": 0.6673768523986875, + "timestamp": 0.23736763733535649 + }, + { + "x": 1.719103738696596, + "y": 5.732106202864848, + "heading": 3.861411095653706, + "angularVelocity": 1.1461281463659418, + "velocityX": 1.000786443513203, + "velocityY": 0.8774381333973873, + "timestamp": 0.31649018311380867 + }, + { + "x": 1.8011465793982657, + "y": 5.8124573886259565, + "heading": 3.9165727578512475, + "angularVelocity": 0.6971674338183805, + "velocityX": 1.0369085055907419, + "velocityY": 1.0155283171258929, + "timestamp": 0.39561272889226085 + }, + { + "x": 1.8844011608490543, + "y": 5.899562457377884, + "heading": 3.9495869202931786, + "angularVelocity": 0.41725354154267424, + "velocityX": 1.0522232396807132, + "velocityY": 1.1008880957373124, + "timestamp": 0.474735274670713 + }, + { + "x": 1.968168332271066, + "y": 5.990724119399961, + "heading": 3.96923386729424, + "angularVelocity": 0.24831034956931133, + "velocityX": 1.0587016709063517, + "velocityY": 1.1521578473642025, + "timestamp": 0.5538578204491652 + }, + { + "x": 2.05213613980173, + "y": 6.084290626509761, + "heading": 3.9810005346780404, + "angularVelocity": 0.1487144690307097, + "velocityX": 1.0612374349756033, + "velocityY": 1.1825517769839073, + "timestamp": 0.6329803662276173 + }, + { + "x": 2.1361397889017213, + "y": 6.179275610733387, + "heading": 3.988261371091814, + "angularVelocity": 0.0917669716303672, + "velocityX": 1.0616904230458737, + "velocityY": 1.2004793739775566, + "timestamp": 0.7121029120060695 + }, + { + "x": 2.2200564987148903, + "y": 6.2750981229467735, + "heading": 3.9931304198236632, + "angularVelocity": 0.061538069635463585, + "velocityX": 1.0605916302053886, + "velocityY": 1.2110645742073904, + "timestamp": 0.7912254577845216 + }, + { + "x": 2.3037490732526433, + "y": 6.3714204686821265, + "heading": 3.997036412999259, + "angularVelocity": 0.049366373859271756, + "velocityX": 1.0577588690345867, + "velocityY": 1.217381781484405, + "timestamp": 0.8703480035629737 + }, + { + "x": 2.3870186605553965, + "y": 6.4680502574631, + "heading": 4.001128510533661, + "angularVelocity": 0.05171847662561717, + "velocityX": 1.0524128929813872, + "velocityY": 1.221267438127472, + "timestamp": 0.9494705493414258 + }, + { + "x": 2.4696215654523987, + "y": 6.564870888609008, + "heading": 4.006432047635611, + "angularVelocity": 0.06702940419537926, + "velocityX": 1.0439869456209716, + "velocityY": 1.2236794227654333, + "timestamp": 1.028593095119878 + }, + { + "x": 2.5522006651677756, + "y": 6.661687676467445, + "heading": 4.0117852212202445, + "angularVelocity": 0.06765674097018534, + "velocityX": 1.043686080913052, + "velocityY": 1.223630848905317, + "timestamp": 1.1077156408983302 + }, + { + "x": 2.626171983754375, + "y": 6.756004010665758, + "heading": 4.047305777483365, + "angularVelocity": 0.44893090728627943, + "velocityX": 0.9348955832857486, + "velocityY": 1.1920285586159498, + "timestamp": 1.1868381866767823 + }, + { + "x": 2.664895057678222, "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, + "angularVelocity": -1.9113735382816162, + "velocityX": 0.4894063195625013, + "velocityY": 0.7225771600506798, + "timestamp": 1.2659607324552344 + }, + { + "x": 2.67401621750823, + "y": 6.8287147997218245, + "heading": 3.8216991351700837, + "angularVelocity": -2.7569472556880648, + "velocityX": 0.338109953564034, + "velocityY": 0.5759980652369127, + "timestamp": 1.2929376366660208 + }, + { + "x": 2.6786727556459358, + "y": 6.8400421309827735, + "heading": 3.72670670460024, + "angularVelocity": -3.521250245306635, + "velocityX": 0.17261202773019518, + "velocityY": 0.41988996114756666, + "timestamp": 1.3199145408768072 + }, + { + "x": 2.678518823316929, + "y": 6.846619952079936, + "heading": 3.614568928899329, + "angularVelocity": -4.15680668266135, + "velocityX": -0.00570607834774554, + "velocityY": 0.24383157703223057, + "timestamp": 1.3468914450875935 + }, + { + "x": 2.6741686792707893, + "y": 6.846169187557711, + "heading": 3.4948207380964975, + "angularVelocity": -4.438915224191635, + "velocityX": -0.16125438308832984, + "velocityY": -0.0167092754120811, + "timestamp": 1.3738683492983799 + }, + { + "x": 2.663571596898345, + "y": 6.842490941859769, + "heading": 3.391106479438399, + "angularVelocity": -3.844557472114035, + "velocityX": -0.3928205508549501, + "velocityY": -0.1363479541318252, + "timestamp": 1.4008452535091662 + }, + { + "x": 2.648321534336774, + "y": 6.83512618854535, + "heading": 3.3066791856345255, + "angularVelocity": -3.129613878011262, + "velocityX": -0.5653006898944218, + "velocityY": -0.2730021672197161, + "timestamp": 1.4278221577199526 + }, + { + "x": 2.630755187717048, + "y": 6.821913643765559, + "heading": 3.2403542901418882, + "angularVelocity": -2.4585806797690686, + "velocityX": -0.6511624344467275, + "velocityY": -0.4897724615306165, + "timestamp": 1.454799061930739 + }, + { + "x": 2.610177790375068, + "y": 6.803317677931451, + "heading": 3.192043949934185, + "angularVelocity": -1.7908037123240415, + "velocityX": -0.7627783077407181, + "velocityY": -0.6893291271973806, + "timestamp": 1.4817759661415253 + }, + { + "x": 2.585880089062531, + "y": 6.779956801667491, + "heading": 3.1617447456020207, + "angularVelocity": -1.1231534980960616, + "velocityX": -0.9006853092812849, + "velocityY": -0.8659583798581432, + "timestamp": 1.5087528703523116 + }, + { + "x": 2.557481302008236, + "y": 6.752192795634918, + "heading": 3.1494506971418432, + "angularVelocity": -0.4557249550980476, + "velocityX": -1.0527074134380248, + "velocityY": -1.029176877214801, + "timestamp": 1.535729774563098 + }, + { + "x": 2.5262145192081267, + "y": 6.721488912807054, + "heading": 3.1494498734965517, + "angularVelocity": -0.00003053149779678828, + "velocityX": -1.1590204181995478, + "velocityY": -1.1381544223143767, + "timestamp": 1.5627066787738844 + }, + { + "x": 2.494951526671565, + "y": 6.690781099940809, + "heading": 3.1494491983381225, + "angularVelocity": -0.000025027276073123234, + "velocityX": -1.1588799178840683, + "velocityY": -1.1383001039076166, + "timestamp": 1.5896835829846707 + }, + { + "x": 2.46368853251831, + "y": 6.660073288720487, + "heading": 3.149448523179691, + "angularVelocity": -0.000025027276154336604, + "velocityX": -1.1588799778128431, + "velocityY": -1.138300042895271, + "timestamp": 1.616660487195457 + }, + { + "x": 2.4324255384011386, + "y": 6.629365477463429, + "heading": 3.1494478480212726, + "angularVelocity": -0.000025027275679943262, + "velocityX": -1.1588799764752862, + "velocityY": -1.138300044257102, + "timestamp": 1.6436373914062434 + }, + { + "x": 2.401162544284488, + "y": 6.5986576662058365, + "heading": 3.1494471728628666, + "angularVelocity": -0.000025027275214919653, + "velocityX": -1.1588799764559639, + "velocityY": -1.1383000442768603, + "timestamp": 1.6706142956170298 + }, + { + "x": 2.3698995501679407, + "y": 6.567949854948137, + "heading": 3.1494464977044734, + "angularVelocity": -0.000025027274749328804, + "velocityX": -1.1588799764521394, + "velocityY": -1.1383000442808404, + "timestamp": 1.6975911998278161 + }, + { + "x": 2.3386365560514903, + "y": 6.537242043690337, + "heading": 3.1494458225460926, + "angularVelocity": -0.000025027274284044715, + "velocityX": -1.1588799764485511, + "velocityY": -1.1383000442845796, + "timestamp": 1.7245681040386025 + }, + { + "x": 2.307373561935169, + "y": 6.5065342324324025, + "heading": 3.1494451473877243, + "angularVelocity": -0.00002502727381983999, + "velocityX": -1.1588799764437645, + "velocityY": -1.1383000442895392, + "timestamp": 1.7515450082493889 + }, + { + "x": 2.2761105678205396, + "y": 6.475826421172744, + "heading": 3.1494444722293684, + "angularVelocity": -0.00002502727335405626, + "velocityX": -1.158879976381056, + "velocityY": -1.1383000443534677, + "timestamp": 1.7785219124601752 + }, + { + "x": 2.244847573837531, + "y": 6.445118609779082, + "heading": 3.1494437970710263, + "angularVelocity": -0.000025027272855365495, + "velocityX": -1.1588799715020328, + "velocityY": -1.138300049320807, + "timestamp": 1.8054988166709616 + }, + { + "x": 2.2135845753549397, + "y": 6.414410802966355, + "heading": 3.149443121912666, + "angularVelocity": -0.000025027273518628322, + "velocityX": -1.1588801382959264, + "velocityY": -1.1382998795112838, + "timestamp": 1.832475720881748 + }, + { + "x": 2.182332647777799, + "y": 6.3836917291800725, + "heading": 3.14944244669543, + "angularVelocity": -0.000025029455959603075, + "velocityX": -1.1584697537175366, + "velocityY": -1.1387175320883667, + "timestamp": 1.8594526250925343 + }, + { + "x": 2.153094421241092, + "y": 6.351050761951557, + "heading": 3.149440476891046, + "angularVelocity": -0.00007301817765136917, + "velocityX": -1.0838243820804343, + "velocityY": -1.2099597112211005, + "timestamp": 1.8864295293033206 + }, + { + "x": 2.1314747036692956, + "y": 6.314801767366594, + "heading": 3.1444623769659996, + "angularVelocity": -0.18453191983123554, + "velocityX": -0.801415811201077, + "velocityY": -1.3437047595123932, + "timestamp": 1.913406433514107 + }, + { + "x": 2.1175303161219627, + "y": 6.274679449481932, + "heading": 3.139999999999996, + "angularVelocity": -0.16541471664709217, + "velocityX": -0.5169009549193061, + "velocityY": -1.487283995641412, + "timestamp": 1.9403833377248934 + }, + { + "x": 2.111393213272095, "y": 6.231289386749268, + "heading": 3.139999999999996, + "angularVelocity": 1.2396255317011e-17, + "velocityX": -0.22749470442979314, + "velocityY": -1.6084151981870152, + "timestamp": 1.9673602419356797 + }, + { + "x": 2.1135515859889713, + "y": 6.18871804241084, + "heading": 3.139999999999996, + "angularVelocity": 4.342266970142451e-18, + "velocityX": 0.08225287430098827, + "velocityY": -1.6223404824025023, + "timestamp": 1.9936009385096722 + }, + { + "x": 2.1237593069102005, + "y": 6.1473322849843255, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422681619637e-18, + "velocityX": 0.3890034280317459, + "velocityY": -1.5771592537498764, + "timestamp": 2.0198416350836648 + }, + { + "x": 2.141644452417822, + "y": 6.1086399046863455, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422691347276405e-18, + "velocityX": 0.6815804396497286, + "velocityY": -1.4745180330437861, + "timestamp": 2.0460823316576575 + }, + { + "x": 2.1665552441015765, + "y": 6.0740504370872355, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422696264537505e-18, + "velocityX": 0.9493189943912999, + "velocityY": -1.3181611814907124, + "timestamp": 2.0723230282316503 + }, + { + "x": 2.1935012145011026, + "y": 6.041021760141006, + "heading": 3.139999999999997, + "angularVelocity": 4.34227316559682e-18, + "velocityX": 1.0268770999862573, + "velocityY": -1.2586814093559167, + "timestamp": 2.098563724805643 + }, + { + "x": 2.220442534619127, + "y": 6.00798928987496, + "heading": 3.139999999999997, + "angularVelocity": 4.342268965346379e-18, + "velocityX": 1.0266998835971468, + "velocityY": -1.2588259680112164, + "timestamp": 2.1248044213796358 + }, + { + "x": 2.247383856273639, + "y": 5.974956820862076, + "heading": 3.1399999999999975, + "angularVelocity": 4.342269673161453e-18, + "velocityX": 1.0266999421507437, + "velocityY": -1.2588259202547785, + "timestamp": 2.1510451179536285 + }, + { + "x": 2.274325177898134, + "y": 5.941924351824711, + "heading": 3.1399999999999975, + "angularVelocity": 4.342269691700593e-18, + "velocityX": 1.0266999410068467, + "velocityY": -1.2588259211877415, + "timestamp": 2.1772858145276213 + }, + { + "x": 2.3012664995222347, + "y": 5.908891882787024, + "heading": 3.1399999999999975, + "angularVelocity": 4.3422697108078704e-18, + "velocityX": 1.0266999409917994, + "velocityY": -1.258825921200014, + "timestamp": 2.203526511101614 + }, + { + "x": 2.328207821146328, + "y": 5.87585941374933, + "heading": 3.139999999999998, + "angularVelocity": 4.3422696965810506e-18, + "velocityX": 1.0266999409915183, + "velocityY": -1.2588259212002433, + "timestamp": 2.2297672076756068 + }, + { + "x": 2.355149142770421, + "y": 5.8428269447116365, + "heading": 3.139999999999998, + "angularVelocity": 4.342269684490725e-18, + "velocityX": 1.0266999409915205, + "velocityY": -1.2588259212002417, + "timestamp": 2.2560079042495995 + }, + { + "x": 2.3820904643945138, + "y": 5.809794475673942, + "heading": 3.139999999999998, + "angularVelocity": 4.342269693460261e-18, + "velocityX": 1.026699940991488, + "velocityY": -1.258825921200268, + "timestamp": 2.2822486008235923 + }, + { + "x": 2.4090317860186072, + "y": 5.7767620066362495, + "heading": 3.1399999999999983, + "angularVelocity": 4.342269692405428e-18, + "velocityX": 1.026699940991538, + "velocityY": -1.258825921200227, + "timestamp": 2.308489297397585 + }, + { + "x": 2.4359731076427, + "y": 5.743729537598556, + "heading": 3.1399999999999983, + "angularVelocity": 4.342269716594298e-18, + "velocityX": 1.0266999409914905, + "velocityY": -1.258825921200266, + "timestamp": 2.3347299939715778 + }, + { + "x": 2.4629144292667973, + "y": 5.710697068560865, + "heading": 3.139999999999999, + "angularVelocity": 4.342269681682682e-18, + "velocityX": 1.026699940991683, + "velocityY": -1.258825921200109, + "timestamp": 2.3609706905455705 + }, + { + "x": 2.4898557508911936, + "y": 5.677664599523419, + "heading": 3.139999999999999, + "angularVelocity": 4.3422696993715505e-18, + "velocityX": 1.0266999410030797, + "velocityY": -1.2588259211908142, + "timestamp": 2.3872113871195633 + }, + { + "x": 2.5167970725019893, + "y": 5.644632130474532, + "heading": 3.139999999999999, + "angularVelocity": 4.342269692409483e-18, + "velocityX": 1.026699940484769, + "velocityY": -1.2588259216268345, + "timestamp": 2.413452083693556 + }, + { + "x": 2.5425090771755396, + "y": 5.613106913889164, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374488911e-18, + "velocityX": 0.9798522154718704, + "velocityY": -1.2013864226688296, + "timestamp": 2.4396927802675488 + }, + { + "x": 2.563078686478754, + "y": 5.587886733807481, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374369648e-18, + "velocityX": 0.7838819844288143, + "velocityY": -0.9611093977847911, + "timestamp": 2.4659334768415415 + }, + { + "x": 2.578505895291019, + "y": 5.568971596499493, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374398685e-18, + "velocityX": 0.5879115582455986, + "velocityY": -0.7208321339584491, + "timestamp": 2.4921741734155343 + }, + { + "x": 2.5887907019054897, + "y": 5.5563615040551815, + "heading": 3.1399999999999997, + "angularVelocity": 4.342265374398075e-18, + "velocityX": 0.39194106701667647, + "velocityY": -0.48055479048560223, + "timestamp": 2.518414869989527 + }, + { + "x": 2.5939331054687496, + "y": 5.55005645751953, + "heading": 3.1399999999999997, + "angularVelocity": 4.342265374359544e-18, + "velocityX": 0.19597054326510124, + "velocityY": -0.2402774071896897, + "timestamp": 2.5446555665635198 + }, + { + "x": 2.5939331054687496, + "y": 5.55005645751953, "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 + "angularVelocity": 3.0510058962998796e-17, + "velocityX": 1.3525026943280928e-16, + "velocityY": 8.359694800915289e-17, + "timestamp": 2.5708962631375125 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Test1.2.traj b/src/main/deploy/choreo/Test1.2.traj index 714f9e0e..523e0fc4 100644 --- a/src/main/deploy/choreo/Test1.2.traj +++ b/src/main/deploy/choreo/Test1.2.traj @@ -1,445 +1,409 @@ { "samples": [ { - "x": 2.59393310546875, - "y": 5.550056457519531, + "x": 2.5939331054687496, + "y": 5.55005645751953, "heading": 3.14, - "angularVelocity": 4.554749859713047e-24, - "velocityX": -4.596278509340535e-25, - "velocityY": -1.5195442763313795e-23, + "angularVelocity": 3.0510058962998796e-17, + "velocityX": 1.3525026943280928e-16, + "velocityY": 8.359694800915289e-17, "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, + "x": 2.5906368423225947, + "y": 5.5485199070890205, + "heading": 3.101591851410996, + "angularVelocity": -1.2805791583146104, + "velocityX": -0.10990183178303778, + "velocityY": -0.05123065102809662, + "timestamp": 0.029992795322007382 + }, + { + "x": 2.584621501308477, + "y": 5.544975862182894, + "heading": 3.0243371709692783, + "angularVelocity": -2.5757746022741834, + "velocityX": -0.20055953270102006, + "velocityY": -0.11816320779855842, + "timestamp": 0.059985590644014763 + }, + { + "x": 2.5759613994637824, + "y": 5.536844199254011, + "heading": 2.9135350884263085, + "angularVelocity": -3.6942899570776517, + "velocityX": -0.28873940397378167, + "velocityY": -0.2711205421688905, + "timestamp": 0.08997838596602215 + }, + { + "x": 2.56275074361348, + "y": 5.521892690270415, + "heading": 2.8248688434153846, + "angularVelocity": -2.956251461684603, + "velocityX": -0.44046097433337356, + "velocityY": -0.49850335131840534, + "timestamp": 0.11997118128802953 + }, + { + "x": 2.543962903792877, + "y": 5.501410245111836, + "heading": 2.7589030524712514, + "angularVelocity": -2.199387894182165, + "velocityX": -0.6264117638620101, + "velocityY": -0.6829121773583486, + "timestamp": 0.1499639766100369 + }, + { + "x": 2.518778715441927, + "y": 5.477002144640871, + "heading": 2.7154691851313926, + "angularVelocity": -1.448143358205586, + "velocityX": -0.839674597873837, + "velocityY": -0.8137987876364632, + "timestamp": 0.1799567719320443 + }, + { + "x": 2.486733800013198, + "y": 5.449761748396881, + "heading": 2.6943182531197865, + "angularVelocity": -0.705200425130341, + "velocityX": -1.0684204351295292, + "velocityY": -0.9082313252703665, + "timestamp": 0.20994956725405167 + }, + { + "x": 2.4478196867061426, + "y": 5.4204460978412365, + "heading": 2.694317441639593, + "angularVelocity": -0.000027055837428819432, + "velocityX": -1.2974487002285997, + "velocityY": -0.9774230858209427, + "timestamp": 0.23994236257605905 + }, + { + "x": 2.408901839144878, + "y": 5.391135398464328, + "heading": 2.6943166423252785, + "angularVelocity": -0.000026650210696695328, + "velocityX": -1.2975732052797133, + "velocityY": -0.9772580068715555, + "timestamp": 0.26993515789806644 + }, + { + "x": 2.3699840051756724, + "y": 5.361824681040097, + "heading": 2.6943158430110494, + "angularVelocity": -0.00002665020784043696, + "velocityX": -1.2975727521022344, + "velocityY": -0.9772586085935405, + "timestamp": 0.2999279532200738 + }, + { + "x": 2.3310661711945366, + "y": 5.332513963631788, + "heading": 2.6943150436964616, + "angularVelocity": -0.000026650219800690006, + "velocityX": -1.297572752499997, + "velocityY": -0.9772586080626477, + "timestamp": 0.3299207485420812 + }, + { + "x": 2.292148337213513, + "y": 5.303203246223414, + "heading": 2.694314244381513, + "angularVelocity": -0.000026650231834296552, + "velocityX": -1.2975727524962546, + "velocityY": -0.9772586080648237, + "timestamp": 0.3599135438640886 + }, + { + "x": 2.25323050323266, + "y": 5.273892528814898, + "heading": 2.6943134450662036, + "angularVelocity": -0.00002665024386857193, + "velocityX": -1.297572752490578, + "velocityY": -0.9772586080695672, + "timestamp": 0.38990633918609596 + }, + { + "x": 2.214312669251978, + "y": 5.2445818114062375, + "heading": 2.694312645750533, + "angularVelocity": -0.00002665025590264624, + "velocityX": -1.2975727524848706, + "velocityY": -0.9772586080743522, + "timestamp": 0.41989913450810334 + }, + { + "x": 2.1753948352714723, + "y": 5.215271093997427, + "heading": 2.694311846434502, + "angularVelocity": -0.000026650267936849635, + "velocityX": -1.2975727524790048, + "velocityY": -0.9772586080793467, + "timestamp": 0.4498919298301107 + }, + { + "x": 2.136477001291495, + "y": 5.185960376588, + "heading": 2.6943110471181098, + "angularVelocity": -0.000026650279973527825, + "velocityX": -1.2975727524613627, + "velocityY": -0.9772586080999772, + "timestamp": 0.4798847251521181 + }, + { + "x": 2.097559167357152, + "y": 5.1566496591180675, + "heading": 2.694310247801348, + "angularVelocity": -0.000026650292283470146, + "velocityX": -1.2975727509398636, + "velocityY": -0.9772586101172457, + "timestamp": 0.5098775204741255 + }, + { + "x": 2.058641302557044, + "y": 5.127338982627939, + "heading": 2.694309448489961, + "angularVelocity": -0.000026650113093683626, + "velocityX": -1.2975737800459084, + "velocityY": -0.9772572437956306, + "timestamp": 0.5398703157961329 + }, + { + "x": 2.0197569370912083, + "y": 5.0979839038638834, + "heading": 2.6943086027536207, + "angularVelocity": -0.000028197983267341566, + "velocityX": -1.2964568673313885, + "velocityY": -0.9787376751348899, + "timestamp": 0.5698631111181403 + }, + { + "x": 1.9877778714785685, + "y": 5.067888184962114, + "heading": 2.678621118987198, + "angularVelocity": -0.5230417371285907, + "velocityX": -1.0662249139900737, + "velocityY": -1.0034316101155079, + "timestamp": 0.5998559064401476 + }, + { + "x": 1.9640654183560038, + "y": 5.031205594348394, + "heading": 2.6596531558314034, + "angularVelocity": -0.6324173173088112, + "velocityX": -0.7906049725553251, + "velocityY": -1.223046742388833, + "timestamp": 0.629848701762155 + }, + { + "x": 1.9481708008996033, + "y": 4.987987936601904, "heading": 2.649783348569472, - "angularVelocity": -0.11091850762536054, - "velocityX": -0.27306763581709725, - "velocityY": -1.4626438018383596, - "timestamp": 0.7189201362690909 + "angularVelocity": -0.32907260414160233, + "velocityX": -0.5299478519964009, + "velocityY": -1.4409346405525878, + "timestamp": 0.6598414970841624 }, { - "x": 1.941084861755371, + "x": 1.9410848617553718, "y": 4.939785003662109, "heading": 2.649783348569472, - "angularVelocity": 6.132292113202646e-25, - "velocityX": -0.13709188064779135, - "velocityY": -1.4746976869375825, - "timestamp": 0.7488751419469697 + "angularVelocity": 6.681257540282107e-18, + "velocityX": -0.23625470944103766, + "velocityY": -1.6071503980304298, + "timestamp": 0.6898342924061698 }, { - "x": 1.9419022015392158, - "y": 4.889062683793002, + "x": 1.9456173734407256, + "y": 4.888006870907468, "heading": 2.649783348569472, - "angularVelocity": 4.614715107153485e-26, - "velocityX": 0.023936675164732843, - "velocityY": -1.4854577230996109, - "timestamp": 0.7830210614708646 + "angularVelocity": -1.484655388526889e-19, + "velocityX": 0.14165581617647935, + "velocityY": -1.6182360167593688, + "timestamp": 0.7218309432150796 }, { - "x": 1.948223657679148, - "y": 4.838070468392074, + "x": 1.9619962278910381, + "y": 4.838678856610239, "heading": 2.649783348569472, - "angularVelocity": 4.653945144562686e-26, - "velocityX": 0.18513064600613907, - "velocityY": -1.4933619042018718, - "timestamp": 0.8171669809947595 + "angularVelocity": -1.4846556369937314e-19, + "velocityX": 0.5118927774045035, + "velocityY": -1.5416618005383362, + "timestamp": 0.7538275940239894 }, { - "x": 1.9600538072506943, - "y": 4.786928471515363, + "x": 1.9893341653194017, + "y": 4.794473003544163, "heading": 2.649783348569472, - "angularVelocity": 4.629261284078221e-26, - "velocityX": 0.3464586614300294, - "velocityY": -1.4977484159101515, - "timestamp": 0.8513129005186544 + "angularVelocity": -1.4846558519027202e-19, + "velocityX": 0.8543999680324345, + "velocityY": -1.381577507287736, + "timestamp": 0.7858242448328991 }, { - "x": 1.977394685635232, - "y": 4.735788039854744, + "x": 2.026135553103086, + "y": 4.757769076744815, "heading": 2.649783348569472, - "angularVelocity": 4.640038416252255e-26, - "velocityX": 0.5078462851879747, - "velocityY": -1.4977025768725385, - "timestamp": 0.8854588200425493 + "angularVelocity": -1.4846559448429508e-19, + "velocityX": 1.1501637469359431, + "velocityY": -1.1471177723711978, + "timestamp": 0.8178208956418089 }, { - "x": 2.0002427480647933, - "y": 4.684845229382865, + "x": 2.0637808893572873, + "y": 4.721931260837698, "heading": 2.649783348569472, - "angularVelocity": 4.6490332666751425e-26, - "velocityX": 0.6691300966012094, - "velocityY": -1.4919150276866968, - "timestamp": 0.9196047395664442 + "angularVelocity": -1.4846636373679086e-19, + "velocityX": 1.1765398972283652, + "velocityY": -1.120048973905845, + "timestamp": 0.8498175464507187 }, { - "x": 2.0285822715254445, - "y": 4.634362832128866, + "x": 2.1014301321789435, + "y": 4.6860975489840255, "heading": 2.649783348569472, - "angularVelocity": 4.622795940866032e-26, - "velocityX": 0.8299534426308102, - "velocityY": -1.478431331119124, - "timestamp": 0.9537506590903391 + "angularVelocity": -1.484648310808098e-19, + "velocityX": 1.1766619902345172, + "velocityY": -1.1199207088167735, + "timestamp": 0.8818141972596285 }, { - "x": 2.0623699922411927, - "y": 4.5847082412928, + "x": 2.139079370280558, + "y": 4.650263832171171, "heading": 2.649783348569472, - "angularVelocity": 4.634016030607817e-26, - "velocityX": 0.9895097624213414, - "velocityY": -1.454188129311252, - "timestamp": 0.987896578614234 + "angularVelocity": -1.48465589810743e-19, + "velocityX": 1.1766618427177973, + "velocityY": -1.1199208638074638, + "timestamp": 0.9138108480685383 }, { - "x": 2.101495965027088, - "y": 4.536421557771227, + "x": 2.17672860838618, + "y": 4.614430115362526, "heading": 2.649783348569472, - "angularVelocity": 4.645177261974479e-26, - "velocityX": 1.1458462191511727, - "velocityY": -1.4141274915083855, - "timestamp": 1.0220424981381289 + "angularVelocity": -1.4846559463647802e-19, + "velocityX": 1.1766618428430395, + "velocityY": -1.1199208636758753, + "timestamp": 0.9458074988774481 }, { - "x": 2.145673252803998, - "y": 4.4903376692314145, + "x": 2.2143778464918284, + "y": 4.57859639855391, "heading": 2.649783348569472, - "angularVelocity": 4.6233288536561975e-26, - "velocityX": 1.2937794147261192, - "velocityY": -1.3496162698902718, - "timestamp": 1.0561884176620238 + "angularVelocity": -1.4846559409367017e-19, + "velocityX": 1.176661842843871, + "velocityY": -1.1199208636750018, + "timestamp": 0.9778041496863579 }, { - "x": 2.1941116699379357, - "y": 4.447748182132556, + "x": 2.2520270845974713, + "y": 4.5427626817452875, "heading": 2.649783348569472, - "angularVelocity": 4.6479494614855375e-26, - "velocityX": 1.4185711736373225, - "velocityY": -1.2472789631292271, - "timestamp": 1.0903343371859187 + "angularVelocity": -1.4846559515966518e-19, + "velocityX": 1.1766618428436975, + "velocityY": -1.1199208636751843, + "timestamp": 1.0098008004952677 }, { - "x": 2.2448201323304646, - "y": 4.410180142680831, + "x": 2.289676322703114, + "y": 4.506928964936664, "heading": 2.649783348569472, - "angularVelocity": 4.6329904581670297e-26, - "velocityX": 1.485051891985029, - "velocityY": -1.1002204648621148, - "timestamp": 1.1244802567098136 + "angularVelocity": -1.4846559610313848e-19, + "velocityX": 1.1766618428436995, + "velocityY": -1.1199208636751825, + "timestamp": 1.0417974513041774 }, { - "x": 2.294852293597042, - "y": 4.3780811675391975, + "x": 2.3273255608087573, + "y": 4.471095248128042, "heading": 2.649783348569472, - "angularVelocity": 4.6446166642422895e-26, - "velocityX": 1.4652456856979683, - "velocityY": -0.9400530309096446, - "timestamp": 1.1586261762337084 + "angularVelocity": -1.484655947429445e-19, + "velocityX": 1.1766618428436957, + "velocityY": -1.1199208636751863, + "timestamp": 1.0737941021130872 }, { - "x": 2.342149827765447, - "y": 4.350766530180341, + "x": 2.3649747989143997, + "y": 4.435261531319418, "heading": 2.649783348569472, - "angularVelocity": 4.633506801188826e-26, - "velocityX": 1.3851591882100787, - "velocityY": -0.7999385501902068, - "timestamp": 1.1927720957576033 + "angularVelocity": -1.4846559574023035e-19, + "velocityX": 1.1766618428436864, + "velocityY": -1.1199208636751963, + "timestamp": 1.105790752921997 }, { - "x": 2.3856944951859207, - "y": 4.3274872473595005, + "x": 2.4026240370200447, + "y": 4.399427814510799, "heading": 2.649783348569472, - "angularVelocity": 4.6381778689211406e-26, - "velocityX": 1.2752524467821442, - "velocityY": -0.6817588498253565, - "timestamp": 1.2269180152814982 + "angularVelocity": -1.484655924580371e-19, + "velocityX": 1.1766618428437539, + "velocityY": -1.1199208636751252, + "timestamp": 1.1377874037309068 }, { - "x": 2.4249725857215654, - "y": 4.307695719032298, + "x": 2.4402732751256884, + "y": 4.363594097702177, "heading": 2.649783348569472, - "angularVelocity": 4.6432377599415885e-26, - "velocityX": 1.150301151156826, - "velocityY": -0.5796162060697082, - "timestamp": 1.2610639348053931 + "angularVelocity": -1.4846559654067578e-19, + "velocityX": 1.1766618428437225, + "velocityY": -1.1199208636751585, + "timestamp": 1.1697840545398166 }, { - "x": 2.4596964858202144, - "y": 4.291007007021927, + "x": 2.4779225132411047, + "y": 4.327760380902407, "heading": 2.649783348569472, - "angularVelocity": 4.6331179794053255e-26, - "velocityX": 1.0169267831358226, - "velocityY": -0.4887468910800007, - "timestamp": 1.295209854329288 + "angularVelocity": -1.4846559492892923e-19, + "velocityX": 1.1766618431491565, + "velocityY": -1.1199208633985085, + "timestamp": 1.2017807053487264 }, { - "x": 2.4896890256889828, - "y": 4.277143601542489, + "x": 2.5129728702731393, + "y": 4.294400221963306, "heading": 2.649783348569472, - "angularVelocity": 4.6467164999430504e-26, - "velocityX": 0.8783638070657234, - "velocityY": -0.4060047488173824, - "timestamp": 1.329355773853183 + "angularVelocity": -1.484655203554499e-19, + "velocityX": 1.0954383082580312, + "velocityY": -1.0426140891540008, + "timestamp": 1.2337773561576362 }, { - "x": 2.514832571254234, - "y": 4.265898475268687, + "x": 2.53926064365096, + "y": 4.2693800974206075, "heading": 2.649783348569472, - "angularVelocity": 4.632856845272505e-26, - "velocityX": 0.7363557905551701, - "velocityY": -0.3293256245723021, - "timestamp": 1.3635016933770778 + "angularVelocity": -1.484655203554444e-19, + "velocityX": 0.8215789063304317, + "velocityY": -0.7819607337071065, + "timestamp": 1.265774006966546 }, { - "x": 2.5350443768209585, - "y": 4.257112226026017, + "x": 2.5567858275270727, + "y": 4.252700012844841, "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 + "angularVelocity": -1.4846552035544855e-19, + "velocityX": 0.5477193216495216, + "velocityY": -0.5213072041629537, + "timestamp": 1.2977706577754557 }, { "x": 2.5655484199523926, "y": 4.244359970092773, "heading": 2.649783348569472, - "angularVelocity": 4.6699948971014915e-26, - "velocityX": 0.14949673474910496, - "velocityY": -0.06080051354533503, - "timestamp": 1.5000853714726574 + "angularVelocity": -1.4846552035527083e-19, + "velocityX": 0.2738596760533411, + "velocityY": -0.260653616588761, + "timestamp": 1.3297673085843655 }, { "x": 2.5655484199523926, "y": 4.244359970092773, "heading": 2.649783348569472, - "angularVelocity": 1.535197914331468e-26, - "velocityX": 1.1963425727073795e-24, - "velocityY": -7.168329076335369e-24, - "timestamp": 1.5342312909965523 + "angularVelocity": -4.948850678520552e-20, + "velocityX": 6.0427487543509175e-18, + "velocityY": -9.335003878092332e-18, + "timestamp": 1.3617639593932753 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Test1.traj b/src/main/deploy/choreo/Test1.traj index e709ba2f..a894f9a7 100644 --- a/src/main/deploy/choreo/Test1.traj +++ b/src/main/deploy/choreo/Test1.traj @@ -4,1063 +4,991 @@ "x": 1.4869295358657837, "y": 5.564249038696289, "heading": 3.141, - "angularVelocity": 8.029589971503427e-22, - "velocityX": 1.7351458241819956e-21, - "velocityY": 6.201477276822387e-22, + "angularVelocity": 3.7405663636389993e-19, + "velocityX": -1.1249906625590385e-18, + "velocityY": 2.3803295269896554e-19, "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, + "x": 1.5092825889004946, + "y": 5.579306288055266, + "heading": 3.4062283948642453, + "angularVelocity": 3.352121601432042, + "velocityX": 0.282511802606821, + "velocityY": 0.19030289294706518, + "timestamp": 0.07912254577845217 + }, + { + "x": 1.5673643497428493, + "y": 5.60987650843196, + "heading": 3.626081158643754, + "angularVelocity": 2.77863612218835, + "velocityX": 0.7340734587204384, + "velocityY": 0.38636547997701703, + "timestamp": 0.15824509155690433 + }, + { + "x": 1.639918967505268, + "y": 5.662681063987355, + "heading": 3.7707265189248944, + "angularVelocity": 1.828118128126927, + "velocityX": 0.9169904361466855, + "velocityY": 0.6673768523986875, + "timestamp": 0.23736763733535649 + }, + { + "x": 1.719103738696596, + "y": 5.732106202864848, + "heading": 3.861411095653706, + "angularVelocity": 1.1461281463659418, + "velocityX": 1.000786443513203, + "velocityY": 0.8774381333973873, + "timestamp": 0.31649018311380867 + }, + { + "x": 1.8011465793982657, + "y": 5.8124573886259565, + "heading": 3.9165727578512475, + "angularVelocity": 0.6971674338183805, + "velocityX": 1.0369085055907419, + "velocityY": 1.0155283171258929, + "timestamp": 0.39561272889226085 + }, + { + "x": 1.8844011608490543, + "y": 5.899562457377884, + "heading": 3.9495869202931786, + "angularVelocity": 0.41725354154267424, + "velocityX": 1.0522232396807132, + "velocityY": 1.1008880957373124, + "timestamp": 0.474735274670713 + }, + { + "x": 1.968168332271066, + "y": 5.990724119399961, + "heading": 3.96923386729424, + "angularVelocity": 0.24831034956931133, + "velocityX": 1.0587016709063517, + "velocityY": 1.1521578473642025, + "timestamp": 0.5538578204491652 + }, + { + "x": 2.05213613980173, + "y": 6.084290626509761, + "heading": 3.9810005346780404, + "angularVelocity": 0.1487144690307097, + "velocityX": 1.0612374349756033, + "velocityY": 1.1825517769839073, + "timestamp": 0.6329803662276173 + }, + { + "x": 2.1361397889017213, + "y": 6.179275610733387, + "heading": 3.988261371091814, + "angularVelocity": 0.0917669716303672, + "velocityX": 1.0616904230458737, + "velocityY": 1.2004793739775566, + "timestamp": 0.7121029120060695 + }, + { + "x": 2.2200564987148903, + "y": 6.2750981229467735, + "heading": 3.9931304198236632, + "angularVelocity": 0.061538069635463585, + "velocityX": 1.0605916302053886, + "velocityY": 1.2110645742073904, + "timestamp": 0.7912254577845216 + }, + { + "x": 2.3037490732526433, + "y": 6.3714204686821265, + "heading": 3.997036412999259, + "angularVelocity": 0.049366373859271756, + "velocityX": 1.0577588690345867, + "velocityY": 1.217381781484405, + "timestamp": 0.8703480035629737 + }, + { + "x": 2.3870186605553965, + "y": 6.4680502574631, + "heading": 4.001128510533661, + "angularVelocity": 0.05171847662561717, + "velocityX": 1.0524128929813872, + "velocityY": 1.221267438127472, + "timestamp": 0.9494705493414258 + }, + { + "x": 2.4696215654523987, + "y": 6.564870888609008, + "heading": 4.006432047635611, + "angularVelocity": 0.06702940419537926, + "velocityX": 1.0439869456209716, + "velocityY": 1.2236794227654333, + "timestamp": 1.028593095119878 + }, + { + "x": 2.5522006651677756, + "y": 6.661687676467445, + "heading": 4.0117852212202445, + "angularVelocity": 0.06765674097018534, + "velocityX": 1.043686080913052, + "velocityY": 1.223630848905317, + "timestamp": 1.1077156408983302 + }, + { + "x": 2.626171983754375, + "y": 6.756004010665758, + "heading": 4.047305777483365, + "angularVelocity": 0.44893090728627943, + "velocityX": 0.9348955832857486, + "velocityY": 1.1920285586159498, + "timestamp": 1.1868381866767823 + }, + { + "x": 2.664895057678222, "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, + "angularVelocity": -1.9113735382816162, + "velocityX": 0.4894063195625013, + "velocityY": 0.7225771600506798, + "timestamp": 1.2659607324552344 + }, + { + "x": 2.67401621750823, + "y": 6.8287147997218245, + "heading": 3.8216991351700837, + "angularVelocity": -2.7569472556880648, + "velocityX": 0.338109953564034, + "velocityY": 0.5759980652369127, + "timestamp": 1.2929376366660208 + }, + { + "x": 2.6786727556459358, + "y": 6.8400421309827735, + "heading": 3.72670670460024, + "angularVelocity": -3.521250245306635, + "velocityX": 0.17261202773019518, + "velocityY": 0.41988996114756666, + "timestamp": 1.3199145408768072 + }, + { + "x": 2.678518823316929, + "y": 6.846619952079936, + "heading": 3.614568928899329, + "angularVelocity": -4.15680668266135, + "velocityX": -0.00570607834774554, + "velocityY": 0.24383157703223057, + "timestamp": 1.3468914450875935 + }, + { + "x": 2.6741686792707893, + "y": 6.846169187557711, + "heading": 3.4948207380964975, + "angularVelocity": -4.438915224191635, + "velocityX": -0.16125438308832984, + "velocityY": -0.0167092754120811, + "timestamp": 1.3738683492983799 + }, + { + "x": 2.663571596898345, + "y": 6.842490941859769, + "heading": 3.391106479438399, + "angularVelocity": -3.844557472114035, + "velocityX": -0.3928205508549501, + "velocityY": -0.1363479541318252, + "timestamp": 1.4008452535091662 + }, + { + "x": 2.648321534336774, + "y": 6.83512618854535, + "heading": 3.3066791856345255, + "angularVelocity": -3.129613878011262, + "velocityX": -0.5653006898944218, + "velocityY": -0.2730021672197161, + "timestamp": 1.4278221577199526 + }, + { + "x": 2.630755187717048, + "y": 6.821913643765559, + "heading": 3.2403542901418882, + "angularVelocity": -2.4585806797690686, + "velocityX": -0.6511624344467275, + "velocityY": -0.4897724615306165, + "timestamp": 1.454799061930739 + }, + { + "x": 2.610177790375068, + "y": 6.803317677931451, + "heading": 3.192043949934185, + "angularVelocity": -1.7908037123240415, + "velocityX": -0.7627783077407181, + "velocityY": -0.6893291271973806, + "timestamp": 1.4817759661415253 + }, + { + "x": 2.585880089062531, + "y": 6.779956801667491, + "heading": 3.1617447456020207, + "angularVelocity": -1.1231534980960616, + "velocityX": -0.9006853092812849, + "velocityY": -0.8659583798581432, + "timestamp": 1.5087528703523116 + }, + { + "x": 2.557481302008236, + "y": 6.752192795634918, + "heading": 3.1494506971418432, + "angularVelocity": -0.4557249550980476, + "velocityX": -1.0527074134380248, + "velocityY": -1.029176877214801, + "timestamp": 1.535729774563098 + }, + { + "x": 2.5262145192081267, + "y": 6.721488912807054, + "heading": 3.1494498734965517, + "angularVelocity": -0.00003053149779678828, + "velocityX": -1.1590204181995478, + "velocityY": -1.1381544223143767, + "timestamp": 1.5627066787738844 + }, + { + "x": 2.494951526671565, + "y": 6.690781099940809, + "heading": 3.1494491983381225, + "angularVelocity": -0.000025027276073123234, + "velocityX": -1.1588799178840683, + "velocityY": -1.1383001039076166, + "timestamp": 1.5896835829846707 + }, + { + "x": 2.46368853251831, + "y": 6.660073288720487, + "heading": 3.149448523179691, + "angularVelocity": -0.000025027276154336604, + "velocityX": -1.1588799778128431, + "velocityY": -1.138300042895271, + "timestamp": 1.616660487195457 + }, + { + "x": 2.4324255384011386, + "y": 6.629365477463429, + "heading": 3.1494478480212726, + "angularVelocity": -0.000025027275679943262, + "velocityX": -1.1588799764752862, + "velocityY": -1.138300044257102, + "timestamp": 1.6436373914062434 + }, + { + "x": 2.401162544284488, + "y": 6.5986576662058365, + "heading": 3.1494471728628666, + "angularVelocity": -0.000025027275214919653, + "velocityX": -1.1588799764559639, + "velocityY": -1.1383000442768603, + "timestamp": 1.6706142956170298 + }, + { + "x": 2.3698995501679407, + "y": 6.567949854948137, + "heading": 3.1494464977044734, + "angularVelocity": -0.000025027274749328804, + "velocityX": -1.1588799764521394, + "velocityY": -1.1383000442808404, + "timestamp": 1.6975911998278161 + }, + { + "x": 2.3386365560514903, + "y": 6.537242043690337, + "heading": 3.1494458225460926, + "angularVelocity": -0.000025027274284044715, + "velocityX": -1.1588799764485511, + "velocityY": -1.1383000442845796, + "timestamp": 1.7245681040386025 + }, + { + "x": 2.307373561935169, + "y": 6.5065342324324025, + "heading": 3.1494451473877243, + "angularVelocity": -0.00002502727381983999, + "velocityX": -1.1588799764437645, + "velocityY": -1.1383000442895392, + "timestamp": 1.7515450082493889 + }, + { + "x": 2.2761105678205396, + "y": 6.475826421172744, + "heading": 3.1494444722293684, + "angularVelocity": -0.00002502727335405626, + "velocityX": -1.158879976381056, + "velocityY": -1.1383000443534677, + "timestamp": 1.7785219124601752 + }, + { + "x": 2.244847573837531, + "y": 6.445118609779082, + "heading": 3.1494437970710263, + "angularVelocity": -0.000025027272855365495, + "velocityX": -1.1588799715020328, + "velocityY": -1.138300049320807, + "timestamp": 1.8054988166709616 + }, + { + "x": 2.2135845753549397, + "y": 6.414410802966355, + "heading": 3.149443121912666, + "angularVelocity": -0.000025027273518628322, + "velocityX": -1.1588801382959264, + "velocityY": -1.1382998795112838, + "timestamp": 1.832475720881748 + }, + { + "x": 2.182332647777799, + "y": 6.3836917291800725, + "heading": 3.14944244669543, + "angularVelocity": -0.000025029455959603075, + "velocityX": -1.1584697537175366, + "velocityY": -1.1387175320883667, + "timestamp": 1.8594526250925343 + }, + { + "x": 2.153094421241092, + "y": 6.351050761951557, + "heading": 3.149440476891046, + "angularVelocity": -0.00007301817765136917, + "velocityX": -1.0838243820804343, + "velocityY": -1.2099597112211005, + "timestamp": 1.8864295293033206 + }, + { + "x": 2.1314747036692956, + "y": 6.314801767366594, + "heading": 3.1444623769659996, + "angularVelocity": -0.18453191983123554, + "velocityX": -0.801415811201077, + "velocityY": -1.3437047595123932, + "timestamp": 1.913406433514107 + }, + { + "x": 2.1175303161219627, + "y": 6.274679449481932, + "heading": 3.139999999999996, + "angularVelocity": -0.16541471664709217, + "velocityX": -0.5169009549193061, + "velocityY": -1.487283995641412, + "timestamp": 1.9403833377248934 + }, + { + "x": 2.111393213272095, "y": 6.231289386749268, + "heading": 3.139999999999996, + "angularVelocity": 1.2396255317011e-17, + "velocityX": -0.22749470442979314, + "velocityY": -1.6084151981870152, + "timestamp": 1.9673602419356797 + }, + { + "x": 2.1135515859889713, + "y": 6.18871804241084, + "heading": 3.139999999999996, + "angularVelocity": 4.342266970142451e-18, + "velocityX": 0.08225287430098827, + "velocityY": -1.6223404824025023, + "timestamp": 1.9936009385096722 + }, + { + "x": 2.1237593069102005, + "y": 6.1473322849843255, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422681619637e-18, + "velocityX": 0.3890034280317459, + "velocityY": -1.5771592537498764, + "timestamp": 2.0198416350836648 + }, + { + "x": 2.141644452417822, + "y": 6.1086399046863455, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422691347276405e-18, + "velocityX": 0.6815804396497286, + "velocityY": -1.4745180330437861, + "timestamp": 2.0460823316576575 + }, + { + "x": 2.1665552441015765, + "y": 6.0740504370872355, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422696264537505e-18, + "velocityX": 0.9493189943912999, + "velocityY": -1.3181611814907124, + "timestamp": 2.0723230282316503 + }, + { + "x": 2.1935012145011026, + "y": 6.041021760141006, + "heading": 3.139999999999997, + "angularVelocity": 4.34227316559682e-18, + "velocityX": 1.0268770999862573, + "velocityY": -1.2586814093559167, + "timestamp": 2.098563724805643 + }, + { + "x": 2.220442534619127, + "y": 6.00798928987496, + "heading": 3.139999999999997, + "angularVelocity": 4.342268965346379e-18, + "velocityX": 1.0266998835971468, + "velocityY": -1.2588259680112164, + "timestamp": 2.1248044213796358 + }, + { + "x": 2.247383856273639, + "y": 5.974956820862076, + "heading": 3.1399999999999975, + "angularVelocity": 4.342269673161453e-18, + "velocityX": 1.0266999421507437, + "velocityY": -1.2588259202547785, + "timestamp": 2.1510451179536285 + }, + { + "x": 2.274325177898134, + "y": 5.941924351824711, + "heading": 3.1399999999999975, + "angularVelocity": 4.342269691700593e-18, + "velocityX": 1.0266999410068467, + "velocityY": -1.2588259211877415, + "timestamp": 2.1772858145276213 + }, + { + "x": 2.3012664995222347, + "y": 5.908891882787024, + "heading": 3.1399999999999975, + "angularVelocity": 4.3422697108078704e-18, + "velocityX": 1.0266999409917994, + "velocityY": -1.258825921200014, + "timestamp": 2.203526511101614 + }, + { + "x": 2.328207821146328, + "y": 5.87585941374933, + "heading": 3.139999999999998, + "angularVelocity": 4.3422696965810506e-18, + "velocityX": 1.0266999409915183, + "velocityY": -1.2588259212002433, + "timestamp": 2.2297672076756068 + }, + { + "x": 2.355149142770421, + "y": 5.8428269447116365, + "heading": 3.139999999999998, + "angularVelocity": 4.342269684490725e-18, + "velocityX": 1.0266999409915205, + "velocityY": -1.2588259212002417, + "timestamp": 2.2560079042495995 + }, + { + "x": 2.3820904643945138, + "y": 5.809794475673942, + "heading": 3.139999999999998, + "angularVelocity": 4.342269693460261e-18, + "velocityX": 1.026699940991488, + "velocityY": -1.258825921200268, + "timestamp": 2.2822486008235923 + }, + { + "x": 2.4090317860186072, + "y": 5.7767620066362495, + "heading": 3.1399999999999983, + "angularVelocity": 4.342269692405428e-18, + "velocityX": 1.026699940991538, + "velocityY": -1.258825921200227, + "timestamp": 2.308489297397585 + }, + { + "x": 2.4359731076427, + "y": 5.743729537598556, + "heading": 3.1399999999999983, + "angularVelocity": 4.342269716594298e-18, + "velocityX": 1.0266999409914905, + "velocityY": -1.258825921200266, + "timestamp": 2.3347299939715778 + }, + { + "x": 2.4629144292667973, + "y": 5.710697068560865, + "heading": 3.139999999999999, + "angularVelocity": 4.342269681682682e-18, + "velocityX": 1.026699940991683, + "velocityY": -1.258825921200109, + "timestamp": 2.3609706905455705 + }, + { + "x": 2.4898557508911936, + "y": 5.677664599523419, + "heading": 3.139999999999999, + "angularVelocity": 4.3422696993715505e-18, + "velocityX": 1.0266999410030797, + "velocityY": -1.2588259211908142, + "timestamp": 2.3872113871195633 + }, + { + "x": 2.5167970725019893, + "y": 5.644632130474532, + "heading": 3.139999999999999, + "angularVelocity": 4.342269692409483e-18, + "velocityX": 1.026699940484769, + "velocityY": -1.2588259216268345, + "timestamp": 2.413452083693556 + }, + { + "x": 2.5425090771755396, + "y": 5.613106913889164, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374488911e-18, + "velocityX": 0.9798522154718704, + "velocityY": -1.2013864226688296, + "timestamp": 2.4396927802675488 + }, + { + "x": 2.563078686478754, + "y": 5.587886733807481, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374369648e-18, + "velocityX": 0.7838819844288143, + "velocityY": -0.9611093977847911, + "timestamp": 2.4659334768415415 + }, + { + "x": 2.578505895291019, + "y": 5.568971596499493, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374398685e-18, + "velocityX": 0.5879115582455986, + "velocityY": -0.7208321339584491, + "timestamp": 2.4921741734155343 + }, + { + "x": 2.5887907019054897, + "y": 5.5563615040551815, + "heading": 3.1399999999999997, + "angularVelocity": 4.342265374398075e-18, + "velocityX": 0.39194106701667647, + "velocityY": -0.48055479048560223, + "timestamp": 2.518414869989527 + }, + { + "x": 2.5939331054687496, + "y": 5.55005645751953, + "heading": 3.1399999999999997, + "angularVelocity": 4.342265374359544e-18, + "velocityX": 0.19597054326510124, + "velocityY": -0.2402774071896897, + "timestamp": 2.5446555665635198 + }, + { + "x": 2.5939331054687496, + "y": 5.55005645751953, "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, + "angularVelocity": 3.0510058962998796e-17, + "velocityX": 1.3525026943280928e-16, + "velocityY": 8.359694800915289e-17, + "timestamp": 2.5708962631375125 + }, + { + "x": 2.5906368423225947, + "y": 5.5485199070890205, + "heading": 3.101591851410996, + "angularVelocity": -1.2805791583146104, + "velocityX": -0.10990183178303778, + "velocityY": -0.05123065102809662, + "timestamp": 2.60088905845952 + }, + { + "x": 2.584621501308477, + "y": 5.544975862182894, + "heading": 3.0243371709692783, + "angularVelocity": -2.5757746022741834, + "velocityX": -0.20055953270102006, + "velocityY": -0.11816320779855842, + "timestamp": 2.6308818537815273 + }, + { + "x": 2.5759613994637824, + "y": 5.536844199254011, + "heading": 2.9135350884263085, + "angularVelocity": -3.6942899570776517, + "velocityX": -0.28873940397378167, + "velocityY": -0.2711205421688905, + "timestamp": 2.6608746491035347 + }, + { + "x": 2.56275074361348, + "y": 5.521892690270415, + "heading": 2.8248688434153846, + "angularVelocity": -2.956251461684603, + "velocityX": -0.44046097433337356, + "velocityY": -0.49850335131840534, + "timestamp": 2.690867444425542 + }, + { + "x": 2.543962903792877, + "y": 5.501410245111836, + "heading": 2.7589030524712514, + "angularVelocity": -2.199387894182165, + "velocityX": -0.6264117638620101, + "velocityY": -0.6829121773583486, + "timestamp": 2.7208602397475494 + }, + { + "x": 2.518778715441927, + "y": 5.477002144640871, + "heading": 2.7154691851313926, + "angularVelocity": -1.448143358205586, + "velocityX": -0.839674597873837, + "velocityY": -0.8137987876364632, + "timestamp": 2.750853035069557 + }, + { + "x": 2.486733800013198, + "y": 5.449761748396881, + "heading": 2.6943182531197865, + "angularVelocity": -0.705200425130341, + "velocityX": -1.0684204351295292, + "velocityY": -0.9082313252703665, + "timestamp": 2.780845830391564 + }, + { + "x": 2.4478196867061426, + "y": 5.4204460978412365, + "heading": 2.694317441639593, + "angularVelocity": -0.000027055837428819432, + "velocityX": -1.2974487002285997, + "velocityY": -0.9774230858209427, + "timestamp": 2.8108386257135716 + }, + { + "x": 2.408901839144878, + "y": 5.391135398464328, + "heading": 2.6943166423252785, + "angularVelocity": -0.000026650210696695328, + "velocityX": -1.2975732052797133, + "velocityY": -0.9772580068715555, + "timestamp": 2.840831421035579 + }, + { + "x": 2.3699840051756724, + "y": 5.361824681040097, + "heading": 2.6943158430110494, + "angularVelocity": -0.00002665020784043696, + "velocityX": -1.2975727521022344, + "velocityY": -0.9772586085935405, + "timestamp": 2.8708242163575863 + }, + { + "x": 2.3310661711945366, + "y": 5.332513963631788, + "heading": 2.6943150436964616, + "angularVelocity": -0.000026650219800690006, + "velocityX": -1.297572752499997, + "velocityY": -0.9772586080626477, + "timestamp": 2.9008170116795937 + }, + { + "x": 2.292148337213513, + "y": 5.303203246223414, + "heading": 2.694314244381513, + "angularVelocity": -0.000026650231834296552, + "velocityX": -1.2975727524962546, + "velocityY": -0.9772586080648237, + "timestamp": 2.930809807001601 + }, + { + "x": 2.25323050323266, + "y": 5.273892528814898, + "heading": 2.6943134450662036, + "angularVelocity": -0.00002665024386857193, + "velocityX": -1.297572752490578, + "velocityY": -0.9772586080695672, + "timestamp": 2.9608026023236085 + }, + { + "x": 2.214312669251978, + "y": 5.2445818114062375, + "heading": 2.694312645750533, + "angularVelocity": -0.00002665025590264624, + "velocityX": -1.2975727524848706, + "velocityY": -0.9772586080743522, + "timestamp": 2.990795397645616 + }, + { + "x": 2.1753948352714723, + "y": 5.215271093997427, + "heading": 2.694311846434502, + "angularVelocity": -0.000026650267936849635, + "velocityX": -1.2975727524790048, + "velocityY": -0.9772586080793467, + "timestamp": 3.0207881929676232 + }, + { + "x": 2.136477001291495, + "y": 5.185960376588, + "heading": 2.6943110471181098, + "angularVelocity": -0.000026650279973527825, + "velocityX": -1.2975727524613627, + "velocityY": -0.9772586080999772, + "timestamp": 3.0507809882896306 + }, + { + "x": 2.097559167357152, + "y": 5.1566496591180675, + "heading": 2.694310247801348, + "angularVelocity": -0.000026650292283470146, + "velocityX": -1.2975727509398636, + "velocityY": -0.9772586101172457, + "timestamp": 3.080773783611638 + }, + { + "x": 2.058641302557044, + "y": 5.127338982627939, + "heading": 2.694309448489961, + "angularVelocity": -0.000026650113093683626, + "velocityX": -1.2975737800459084, + "velocityY": -0.9772572437956306, + "timestamp": 3.1107665789336454 + }, + { + "x": 2.0197569370912083, + "y": 5.0979839038638834, + "heading": 2.6943086027536207, + "angularVelocity": -0.000028197983267341566, + "velocityX": -1.2964568673313885, + "velocityY": -0.9787376751348899, + "timestamp": 3.1407593742556528 + }, + { + "x": 1.9877778714785685, + "y": 5.067888184962114, + "heading": 2.678621118987198, + "angularVelocity": -0.5230417371285907, + "velocityX": -1.0662249139900737, + "velocityY": -1.0034316101155079, + "timestamp": 3.17075216957766 + }, + { + "x": 1.9640654183560038, + "y": 5.031205594348394, + "heading": 2.6596531558314034, + "angularVelocity": -0.6324173173088112, + "velocityX": -0.7906049725553251, + "velocityY": -1.223046742388833, + "timestamp": 3.2007449648996675 + }, + { + "x": 1.9481708008996033, + "y": 4.987987936601904, "heading": 2.649783348569472, - "angularVelocity": -0.11091850762536054, - "velocityX": -0.27306763581709725, - "velocityY": -1.4626438018383596, - "timestamp": 3.3248528985757186 + "angularVelocity": -0.32907260414160233, + "velocityX": -0.5299478519964009, + "velocityY": -1.4409346405525878, + "timestamp": 3.230737760221675 }, { - "x": 1.941084861755371, + "x": 1.9410848617553718, "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 + "angularVelocity": 6.681257540282107e-18, + "velocityX": -0.23625470944103766, + "velocityY": -1.6071503980304298, + "timestamp": 3.2607305555436823 }, { - "x": 1.9600538072506943, - "y": 4.786928471515363, + "x": 1.9456173734407256, + "y": 4.888006870907468, "heading": 2.649783348569472, - "angularVelocity": 4.629261284078221e-26, - "velocityX": 0.3464586614300294, - "velocityY": -1.4977484159101515, - "timestamp": 3.457245662825282 + "angularVelocity": -1.484655388526889e-19, + "velocityX": 0.14165581617647935, + "velocityY": -1.6182360167593688, + "timestamp": 3.292727206352592 }, { - "x": 1.977394685635232, - "y": 4.735788039854744, + "x": 1.9619962278910381, + "y": 4.838678856610239, "heading": 2.649783348569472, - "angularVelocity": 4.640038416252255e-26, - "velocityX": 0.5078462851879747, - "velocityY": -1.4977025768725385, - "timestamp": 3.491391582349177 + "angularVelocity": -1.4846556369937314e-19, + "velocityX": 0.5118927774045035, + "velocityY": -1.5416618005383362, + "timestamp": 3.324723857161502 }, { - "x": 2.0002427480647933, - "y": 4.684845229382865, + "x": 1.9893341653194017, + "y": 4.794473003544163, "heading": 2.649783348569472, - "angularVelocity": 4.6490332666751425e-26, - "velocityX": 0.6691300966012094, - "velocityY": -1.4919150276866968, - "timestamp": 3.525537501873072 + "angularVelocity": -1.4846558519027202e-19, + "velocityX": 0.8543999680324345, + "velocityY": -1.381577507287736, + "timestamp": 3.3567205079704117 }, { - "x": 2.0285822715254445, - "y": 4.634362832128866, + "x": 2.026135553103086, + "y": 4.757769076744815, "heading": 2.649783348569472, - "angularVelocity": 4.622795940866032e-26, - "velocityX": 0.8299534426308102, - "velocityY": -1.478431331119124, - "timestamp": 3.559683421396967 + "angularVelocity": -1.4846559448429508e-19, + "velocityX": 1.1501637469359431, + "velocityY": -1.1471177723711978, + "timestamp": 3.3887171587793214 }, { - "x": 2.0623699922411927, - "y": 4.5847082412928, + "x": 2.0637808893572873, + "y": 4.721931260837698, "heading": 2.649783348569472, - "angularVelocity": 4.634016030607817e-26, - "velocityX": 0.9895097624213414, - "velocityY": -1.454188129311252, - "timestamp": 3.5938293409208617 + "angularVelocity": -1.4846636373679086e-19, + "velocityX": 1.1765398972283652, + "velocityY": -1.120048973905845, + "timestamp": 3.4207138095882312 }, { - "x": 2.101495965027088, - "y": 4.536421557771227, + "x": 2.1014301321789435, + "y": 4.6860975489840255, "heading": 2.649783348569472, - "angularVelocity": 4.645177261974479e-26, - "velocityX": 1.1458462191511727, - "velocityY": -1.4141274915083855, - "timestamp": 3.6279752604447566 + "angularVelocity": -1.484648310808098e-19, + "velocityX": 1.1766619902345172, + "velocityY": -1.1199207088167735, + "timestamp": 3.452710460397141 }, { - "x": 2.145673252803998, - "y": 4.4903376692314145, + "x": 2.139079370280558, + "y": 4.650263832171171, "heading": 2.649783348569472, - "angularVelocity": 4.6233288536561975e-26, - "velocityX": 1.2937794147261192, - "velocityY": -1.3496162698902718, - "timestamp": 3.6621211799686515 + "angularVelocity": -1.48465589810743e-19, + "velocityX": 1.1766618427177973, + "velocityY": -1.1199208638074638, + "timestamp": 3.484707111206051 }, { - "x": 2.1941116699379357, - "y": 4.447748182132556, + "x": 2.17672860838618, + "y": 4.614430115362526, "heading": 2.649783348569472, - "angularVelocity": 4.6479494614855375e-26, - "velocityX": 1.4185711736373225, - "velocityY": -1.2472789631292271, - "timestamp": 3.6962670994925464 + "angularVelocity": -1.4846559463647802e-19, + "velocityX": 1.1766618428430395, + "velocityY": -1.1199208636758753, + "timestamp": 3.5167037620149606 }, { - "x": 2.2448201323304646, - "y": 4.410180142680831, + "x": 2.2143778464918284, + "y": 4.57859639855391, "heading": 2.649783348569472, - "angularVelocity": 4.6329904581670297e-26, - "velocityX": 1.485051891985029, - "velocityY": -1.1002204648621148, - "timestamp": 3.7304130190164413 + "angularVelocity": -1.4846559409367017e-19, + "velocityX": 1.176661842843871, + "velocityY": -1.1199208636750018, + "timestamp": 3.5487004128238704 }, { - "x": 2.294852293597042, - "y": 4.3780811675391975, + "x": 2.2520270845974713, + "y": 4.5427626817452875, "heading": 2.649783348569472, - "angularVelocity": 4.6446166642422895e-26, - "velocityX": 1.4652456856979683, - "velocityY": -0.9400530309096446, - "timestamp": 3.764558938540336 + "angularVelocity": -1.4846559515966518e-19, + "velocityX": 1.1766618428436975, + "velocityY": -1.1199208636751843, + "timestamp": 3.58069706363278 }, { - "x": 2.342149827765447, - "y": 4.350766530180341, + "x": 2.289676322703114, + "y": 4.506928964936664, "heading": 2.649783348569472, - "angularVelocity": 4.633506801188826e-26, - "velocityX": 1.3851591882100787, - "velocityY": -0.7999385501902068, - "timestamp": 3.798704858064231 + "angularVelocity": -1.4846559610313848e-19, + "velocityX": 1.1766618428436995, + "velocityY": -1.1199208636751825, + "timestamp": 3.61269371444169 }, { - "x": 2.3856944951859207, - "y": 4.3274872473595005, + "x": 2.3273255608087573, + "y": 4.471095248128042, "heading": 2.649783348569472, - "angularVelocity": 4.6381778689211406e-26, - "velocityX": 1.2752524467821442, - "velocityY": -0.6817588498253565, - "timestamp": 3.832850777588126 + "angularVelocity": -1.484655947429445e-19, + "velocityX": 1.1766618428436957, + "velocityY": -1.1199208636751863, + "timestamp": 3.6446903652505998 }, { - "x": 2.4249725857215654, - "y": 4.307695719032298, + "x": 2.3649747989143997, + "y": 4.435261531319418, "heading": 2.649783348569472, - "angularVelocity": 4.6432377599415885e-26, - "velocityX": 1.150301151156826, - "velocityY": -0.5796162060697082, - "timestamp": 3.866996697112021 + "angularVelocity": -1.4846559574023035e-19, + "velocityX": 1.1766618428436864, + "velocityY": -1.1199208636751963, + "timestamp": 3.6766870160595095 }, { - "x": 2.4596964858202144, - "y": 4.291007007021927, + "x": 2.4026240370200447, + "y": 4.399427814510799, "heading": 2.649783348569472, - "angularVelocity": 4.6331179794053255e-26, - "velocityX": 1.0169267831358226, - "velocityY": -0.4887468910800007, - "timestamp": 3.9011426166359158 + "angularVelocity": -1.484655924580371e-19, + "velocityX": 1.1766618428437539, + "velocityY": -1.1199208636751252, + "timestamp": 3.7086836668684193 }, { - "x": 2.4896890256889828, - "y": 4.277143601542489, + "x": 2.4402732751256884, + "y": 4.363594097702177, "heading": 2.649783348569472, - "angularVelocity": 4.6467164999430504e-26, - "velocityX": 0.8783638070657234, - "velocityY": -0.4060047488173824, - "timestamp": 3.9352885361598107 + "angularVelocity": -1.4846559654067578e-19, + "velocityX": 1.1766618428437225, + "velocityY": -1.1199208636751585, + "timestamp": 3.740680317677329 }, { - "x": 2.514832571254234, - "y": 4.265898475268687, + "x": 2.4779225132411047, + "y": 4.327760380902407, "heading": 2.649783348569472, - "angularVelocity": 4.632856845272505e-26, - "velocityX": 0.7363557905551701, - "velocityY": -0.3293256245723021, - "timestamp": 3.9694344556837056 + "angularVelocity": -1.4846559492892923e-19, + "velocityX": 1.1766618431491565, + "velocityY": -1.1199208633985085, + "timestamp": 3.772676968486239 }, { - "x": 2.5350443768209585, - "y": 4.257112226026017, + "x": 2.5129728702731393, + "y": 4.294400221963306, "heading": 2.649783348569472, - "angularVelocity": 4.629729705599566e-26, - "velocityX": 0.5919244773180152, - "velocityY": -0.2573147645510306, - "timestamp": 4.0035803752076005 + "angularVelocity": -1.484655203554499e-19, + "velocityX": 1.0954383082580312, + "velocityY": -1.0426140891540008, + "timestamp": 3.8046736192951487 }, { - "x": 2.550263563230007, - "y": 4.2506587632470065, + "x": 2.53926064365096, + "y": 4.2693800974206075, "heading": 2.649783348569472, - "angularVelocity": 4.62810585566047e-26, - "velocityX": 0.445710252388941, - "velocityY": -0.18899660249284694, - "timestamp": 4.037726294731495 + "angularVelocity": -1.484655203554444e-19, + "velocityX": 0.8215789063304317, + "velocityY": -0.7819607337071065, + "timestamp": 3.8366702701040585 }, { - "x": 2.5604437164785643, - "y": 4.246436059535304, + "x": 2.5567858275270727, + "y": 4.252700012844841, "heading": 2.649783348569472, - "angularVelocity": 4.6519462435401186e-26, - "velocityX": 0.29813674343821717, - "velocityY": -0.12366642253542871, - "timestamp": 4.07187221425539 + "angularVelocity": -1.4846552035544855e-19, + "velocityX": 0.5477193216495216, + "velocityY": -0.5213072041629537, + "timestamp": 3.8686669209129683 }, { "x": 2.5655484199523926, "y": 4.244359970092773, "heading": 2.649783348569472, - "angularVelocity": 4.6699948971014915e-26, - "velocityX": 0.14949673474910496, - "velocityY": -0.06080051354533503, - "timestamp": 4.106018133779285 + "angularVelocity": -1.4846552035527083e-19, + "velocityX": 0.2738596760533411, + "velocityY": -0.260653616588761, + "timestamp": 3.900663571721878 }, { "x": 2.5655484199523926, "y": 4.244359970092773, "heading": 2.649783348569472, - "angularVelocity": 1.535197914331468e-26, - "velocityX": 1.1963425727073795e-24, - "velocityY": -7.168329076335369e-24, - "timestamp": 4.14016405330318 + "angularVelocity": -4.948850678520552e-20, + "velocityX": 6.0427487543509175e-18, + "velocityY": -9.335003878092332e-18, + "timestamp": 3.932660222530788 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Test2.1.traj b/src/main/deploy/choreo/Test2.1.traj index 6858b3e4..60dd1202 100644 --- a/src/main/deploy/choreo/Test2.1.traj +++ b/src/main/deploy/choreo/Test2.1.traj @@ -1,688 +1,796 @@ { "samples": [ { - "x": 0.3137660622596754, + "x": 0.3137660622596741, "y": 6.865104675292969, - "heading": 3.135568639860517, - "angularVelocity": -8.5693301650697e-28, - "velocityX": -3.4362695197914885e-17, - "velocityY": -4.743570524232907e-17, + "heading": 3.135568639860519, + "angularVelocity": 1.3323616061473399e-26, + "velocityX": -9.030177506128314e-25, + "velocityY": 7.578270673418084e-24, "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": 0.3285749308509426, + "y": 6.865947459797136, + "heading": 3.135568639860519, + "angularVelocity": 3.3337145202478786e-22, + "velocityX": 0.41797046933441173, + "velocityY": 0.023787032249159984, + "timestamp": 0.03543041836149472 + }, + { + "x": 0.35819266531481686, + "y": 6.867633028650745, + "heading": 3.135568639860519, + "angularVelocity": 3.3337147019213317e-22, + "velocityX": 0.8359408619363751, + "velocityY": 0.04757406013141577, + "timestamp": 0.07086083672298944 + }, + { + "x": 0.4026192574953566, + "y": 6.87016138138964, + "heading": 3.135568639860519, + "angularVelocity": 3.3337143389947597e-22, + "velocityX": 1.2539110243423486, + "velocityY": 0.07136107491303612, + "timestamp": 0.10629125508448417 + }, + { + "x": 0.4600803298886451, + "y": 6.873431536777097, + "heading": 3.135568639860519, + "angularVelocity": 3.331660178480041e-22, + "velocityX": 1.621800561512317, + "velocityY": 0.09229796143220789, + "timestamp": 0.14172167344597889 + }, + { + "x": 0.5175414022820589, + "y": 6.8767016921645565, + "heading": 3.135568639860519, + "angularVelocity": 3.3316629368551667e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233462, + "timestamp": 0.1771520918074736 + }, + { + "x": 0.5750024746754727, + "y": 6.879971847552017, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614681891394e-22, + "velocityX": 1.6218005615158566, + "velocityY": 0.09229796143233254, + "timestamp": 0.2125825101689683 + }, + { + "x": 0.6324635470688867, + "y": 6.883242002939478, + "heading": 3.135568639860519, + "angularVelocity": 3.331683104806914e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.24801292853046303 + }, + { + "x": 0.6899246194623005, + "y": 6.886512158326939, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614878052174e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233253, + "timestamp": 0.28344334689195777 + }, + { + "x": 0.7473856918557144, + "y": 6.8897823137144, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614758461554e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233254, + "timestamp": 0.3188737652534525 + }, + { + "x": 0.8048467642491284, + "y": 6.89305246910186, + "heading": 3.135568639860519, + "angularVelocity": 3.331638534198991e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233299, + "timestamp": 0.35430418361494725 + }, + { + "x": 0.8623078366425422, + "y": 6.89632262448932, + "heading": 3.135568639860519, + "angularVelocity": 3.3317076614902997e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233253, + "timestamp": 0.389734601976442 + }, + { + "x": 0.9197689090359561, + "y": 6.899592779876781, + "heading": 3.135568639860519, + "angularVelocity": 3.331661479116866e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233396, + "timestamp": 0.42516502033793674 + }, + { + "x": 0.97722998142937, + "y": 6.902862935264242, + "heading": 3.135568639860519, + "angularVelocity": 3.33166145502926e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.4605954386994315 + }, + { + "x": 1.0346910538227838, + "y": 6.906133090651703, + "heading": 3.135568639860519, + "angularVelocity": 3.3316600659908063e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233301, + "timestamp": 0.4960258570609262 + }, + { + "x": 1.0921521262161977, + "y": 6.9094032460391634, + "heading": 3.135568639860519, + "angularVelocity": 3.331661460674642e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.5314562754224209 + }, + { + "x": 1.1496131986096116, + "y": 6.912673401426624, + "heading": 3.135568639860519, + "angularVelocity": 3.331661475383932e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.5668866937839157 + }, + { + "x": 1.2070742710030253, + "y": 6.915943556814084, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614753921174e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233254, + "timestamp": 0.6023171121454104 + }, + { + "x": 1.2645353433964392, + "y": 6.919213712201545, + "heading": 3.135568639860519, + "angularVelocity": 3.331662731932369e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.6377475305069051 + }, + { + "x": 1.3219964157898532, + "y": 6.922483867589006, + "heading": 3.135568639860519, + "angularVelocity": 3.3316627143346654e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233393, + "timestamp": 0.6731779488683999 + }, + { + "x": 1.379457488183267, + "y": 6.925754022976466, + "heading": 3.135568639860519, + "angularVelocity": 3.331661483573468e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233157, + "timestamp": 0.7086083672298946 + }, + { + "x": 1.436918560576681, + "y": 6.929024178363926, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614890219236e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233392, + "timestamp": 0.7440387855913894 + }, + { + "x": 1.494379632970095, + "y": 6.932294333751387, + "heading": 3.135568639860519, + "angularVelocity": 3.331662743669949e-22, + "velocityX": 1.6218005615158566, + "velocityY": 0.09229796143233207, + "timestamp": 0.7794692039528841 + }, + { + "x": 1.5518407053635086, + "y": 6.935564489138847, + "heading": 3.135568639860519, + "angularVelocity": 3.331638909213181e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.8148996223143788 + }, + { + "x": 1.6093017777569225, + "y": 6.938834644526308, + "heading": 3.135568639860519, + "angularVelocity": 3.3316600948450887e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233622, + "timestamp": 0.8503300406758736 + }, + { + "x": 1.666762850150336, + "y": 6.942104799913777, + "heading": 3.135568639860519, + "angularVelocity": 3.331671581625241e-22, + "velocityX": 1.621800561515844, + "velocityY": 0.09229796143255317, + "timestamp": 0.8857604590373683 + }, + { + "x": 1.7242239225437261, + "y": 6.945374955301653, + "heading": 3.135568639860519, + "angularVelocity": 3.3334679678699274e-22, + "velocityX": 1.6218005615151887, + "velocityY": 0.09229796144406777, + "timestamp": 0.9211908773988631 + }, + { + "x": 1.7816849946975708, + "y": 6.948645114898682, + "heading": 3.135568639860519, + "angularVelocity": 1.7645788213167404e-22, + "velocityX": 1.6218005547541783, + "velocityY": 0.09229808024460076, + "timestamp": 0.9566212957603578 + }, + { + "x": 1.8178857407766151, + "y": 6.94853231479887, + "heading": 3.1384797599068888, + "angularVelocity": 0.12817174361754557, + "velocityX": 1.5938582646199322, + "velocityY": -0.004966399613447973, + "timestamp": 0.9793339464913666 + }, + { + "x": 1.8540603970824046, + "y": 6.948528771707492, + "heading": 3.1414999940123267, + "angularVelocity": 0.13297585302602363, + "velocityX": 1.592709575567121, + "velocityY": -0.00015599638369279018, + "timestamp": 1.0020465972223755 + }, + { + "x": 1.8901908310793394, + "y": 6.948640868874459, + "heading": 3.1447030512216227, + "angularVelocity": 0.14102524831780386, + "velocityX": 1.5907625413183106, + "velocityY": 0.0049354506567203535, + "timestamp": 1.0247592479533845 + }, + { + "x": 1.926257669084107, + "y": 6.9488776015696425, + "heading": 3.148167194864803, + "angularVelocity": 0.15252044704983372, + "velocityX": 1.5879625162168578, + "velocityY": 0.010422944375284186, + "timestamp": 1.0474718986843934 + }, + { + "x": 1.9622398024006038, + "y": 6.94925072993569, + "heading": 3.151976954699645, + "angularVelocity": 0.16773734954856945, + "velocityX": 1.584233110553301, + "velocityY": 0.01642821749276163, + "timestamp": 1.0701845494154023 + }, + { + "x": 1.9981171224268564, + "y": 6.949774504499785, + "heading": 3.156211459419019, + "angularVelocity": 0.1864381559653407, + "velocityX": 1.5796183567984214, + "velocityY": 0.023060917472754097, + "timestamp": 1.0928972001464112 + }, + { + "x": 2.033847488660219, + "y": 6.950468453992633, + "heading": 3.161038496208879, + "angularVelocity": 0.2125263513725333, + "velocityX": 1.5731482272380108, + "velocityY": 0.03055343478252191, + "timestamp": 1.1156098508774202 + }, + { + "x": 2.0694053227389695, + "y": 6.951354952814641, + "heading": 3.166554769842392, + "angularVelocity": 0.24287229609803831, + "velocityX": 1.565551925218666, + "velocityY": 0.039031059496573456, + "timestamp": 1.138322501608429 + }, + { + "x": 2.104784911670157, + "y": 6.952454847185295, + "heading": 3.1727732170956275, + "angularVelocity": 0.27378782542301205, + "velocityX": 1.5577040896809682, + "velocityY": 0.04842650836667849, + "timestamp": 1.161035152339438 + }, + { + "x": 2.1400047240253706, + "y": 6.9537191803371154, + "heading": 3.1796054213345153, + "angularVelocity": 0.3008105183231903, + "velocityX": 1.550669394441415, + "velocityY": 0.05566647269821489, + "timestamp": 1.183747803070447 + }, + { + "x": 2.175095855891208, + "y": 6.955084615506324, + "heading": 3.18691368664007, + "angularVelocity": 0.3217706903570311, + "velocityX": 1.5450038078527182, + "velocityY": 0.060117825320333544, + "timestamp": 1.2064604538014558 + }, + { + "x": 2.2101380445057046, + "y": 6.956471002562802, + "heading": 3.194369840668752, + "angularVelocity": 0.3282819833311086, + "velocityX": 1.5428489184071512, + "velocityY": 0.06104030185188484, + "timestamp": 1.2291731045324648 + }, + { + "x": 2.2451610724837057, + "y": 6.957849512193169, + "heading": 3.2018529792356185, + "angularVelocity": 0.32947006738627316, + "velocityX": 1.5420053076493287, + "velocityY": 0.060693471963871995, + "timestamp": 1.2518857552634737 + }, + { + "x": 2.2801629678458126, + "y": 6.959221595789265, + "heading": 3.2093707720743723, + "angularVelocity": 0.3309958369804056, + "velocityX": 1.5410748739388525, + "velocityY": 0.060410544429463274, + "timestamp": 1.2745984059944826 + }, + { + "x": 2.315144049729391, + "y": 6.960586594052822, + "heading": 3.2169217131634897, + "angularVelocity": 0.3324552989672944, + "velocityX": 1.540158491312511, + "velocityY": 0.06009858909567994, + "timestamp": 1.2973110567254915 + }, + { + "x": 2.3501040543802802, + "y": 6.961944386116353, + "heading": 3.2245066187504112, + "angularVelocity": 0.3339506989629348, + "velocityX": 1.539230496031878, + "velocityY": 0.05978131216881497, + "timestamp": 1.3200237074565004 + }, + { + "x": 2.3850426980471595, + "y": 6.963294864074251, + "heading": 3.232126373384074, + "angularVelocity": 0.3354850441678861, + "velocityX": 1.5382900076554706, + "velocityY": 0.05945928433854099, + "timestamp": 1.3427363581875094 + }, + { + "x": 2.419959710539657, + "y": 6.964637896582436, + "heading": 3.239781798872735, + "angularVelocity": 0.3370555722150803, + "velocityX": 1.5373376232491214, + "velocityY": 0.05913147364836824, + "timestamp": 1.3654490089185183 + }, + { + "x": 2.454854803951486, + "y": 6.965973356645481, + "heading": 3.247473776388813, + "angularVelocity": 0.3386648968090985, + "velocityX": 1.5363725628109135, + "velocityY": 0.05879807156202029, + "timestamp": 1.3881616596495272 + }, + { + "x": 2.48972762858419, + "y": 6.967301155248667, + "heading": 3.2552034126870217, + "angularVelocity": 0.3403229499609042, + "velocityX": 1.5353921057348447, + "velocityY": 0.05846075030657316, + "timestamp": 1.4108743103805361 + }, + { + "x": 2.5245780070203394, + "y": 6.968621054178632, + "heading": 3.262971149366171, + "angularVelocity": 0.34200044596925644, + "velocityX": 1.5344038372663136, + "velocityY": 0.05811294091547401, + "timestamp": 1.433586961111545 + }, + { + "x": 2.559405532428149, + "y": 6.969932985582836, + "heading": 3.2707782920481354, + "angularVelocity": 0.3437354263236719, + "velocityX": 1.5333976566751537, + "velocityY": 0.05776214409057404, + "timestamp": 1.456299611842554 + }, + { + "x": 2.594207801112396, + "y": 6.97123839692077, + "heading": 3.2786335848605472, + "angularVelocity": 0.34585539598366793, + "velocityX": 1.5322856454060756, + "velocityY": 0.05747507648462563, + "timestamp": 1.4790122625735629 + }, + { + "x": 2.6290202140808105, + "y": 6.972513675689697, + "heading": 3.286405336491994, + "angularVelocity": 0.3421772175995684, + "velocityX": 1.5327322812606092, + "velocityY": 0.05614838990089804, + "timestamp": 1.5017249133045718 + }, + { + "x": 2.7840510289336455, + "y": 6.98699854094784, + "heading": 3.286405287600397, + "angularVelocity": -5.100684883763116e-7, + "velocityX": 1.6173808639626774, + "velocityY": 0.1511154018498607, + "timestamp": 1.5975779175703702 + }, + { + "x": 2.93908160001489, + "y": 7.001486015045406, + "heading": 3.2864052387079807, + "angularVelocity": -5.100770354356606e-7, + "velocityX": 1.6173783207811374, + "velocityY": 0.15114261893547631, + "timestamp": 1.6934309218361685 + }, + { + "x": 3.094112171101734, + "y": 7.015973489083046, + "heading": 3.286405189815564, + "angularVelocity": -5.100770390190521e-7, + "velocityX": 1.617378320839561, + "velocityY": 0.15114261831028078, + "timestamp": 1.7892839261019668 + }, + { + "x": 3.2491427421885786, + "y": 7.030460963120681, + "heading": 3.286405140923147, + "angularVelocity": -5.100770430673946e-7, + "velocityX": 1.617378320839565, + "velocityY": 0.15114261831023715, + "timestamp": 1.8851369303677652 + }, + { + "x": 3.404173313275423, + "y": 7.0449484371583155, + "heading": 3.28640509203073, + "angularVelocity": -5.100770460466358e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831023496, + "timestamp": 1.9809899346335635 + }, + { + "x": 3.5592038843622675, + "y": 7.05943591119595, + "heading": 3.286405043138312, + "angularVelocity": -5.100770488976204e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831023276, + "timestamp": 2.076842938899362 + }, + { + "x": 3.714234455449112, + "y": 7.073923385233584, + "heading": 3.2864049942458946, + "angularVelocity": -5.100770524050277e-7, + "velocityX": 1.6173783208395642, + "velocityY": 0.15114261831023054, + "timestamp": 2.1726959431651602 + }, + { + "x": 3.8692650265359565, + "y": 7.088410859271219, + "heading": 3.2864049453534765, + "angularVelocity": -5.100770565197187e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831022827, + "timestamp": 2.2685489474309586 + }, + { + "x": 4.0242955976228005, + "y": 7.102898333308853, + "heading": 3.2864048964610575, + "angularVelocity": -5.100770606946053e-7, + "velocityX": 1.6173783208395642, + "velocityY": 0.15114261831022607, + "timestamp": 2.364401951696757 + }, + { + "x": 4.179326168709644, + "y": 7.117385807346487, + "heading": 3.286404847568639, + "angularVelocity": -5.100770634020749e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.151142618310224, + "timestamp": 2.4602549559625553 + }, + { + "x": 4.334356739796489, + "y": 7.13187328138412, + "heading": 3.2864047986762195, + "angularVelocity": -5.100770666030285e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.15114261831022163, + "timestamp": 2.5561079602283536 + }, + { + "x": 4.489387310883332, + "y": 7.146360755421753, + "heading": 3.2864047497838, + "angularVelocity": -5.100770706092546e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.15114261831021944, + "timestamp": 2.651960964494152 + }, + { + "x": 4.644417881970177, + "y": 7.160848229459387, + "heading": 3.28640470089138, + "angularVelocity": -5.100770740673254e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021727, + "timestamp": 2.7478139687599503 + }, + { + "x": 4.79944845305702, + "y": 7.175335703497019, + "heading": 3.28640465199896, + "angularVelocity": -5.100770777885376e-7, + "velocityX": 1.6173783208395638, + "velocityY": 0.15114261831021514, + "timestamp": 2.8436669730257487 + }, + { + "x": 4.954479024143865, + "y": 7.1898231775346515, + "heading": 3.286404603106539, + "angularVelocity": -5.100770810299081e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021283, + "timestamp": 2.939519977291547 + }, + { + "x": 5.109509595230708, + "y": 7.2043106515722855, + "heading": 3.2864045542141187, + "angularVelocity": -5.100770846564483e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021056, + "timestamp": 3.0353729815573454 + }, + { + "x": 5.264540166317553, + "y": 7.218798125609918, + "heading": 3.286404505321697, + "angularVelocity": -5.100770889061609e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.1511426183102084, + "timestamp": 3.1312259858231437 + }, + { + "x": 5.419570737404396, + "y": 7.233285599647549, + "heading": 3.2864044564292754, + "angularVelocity": -5.100770922378371e-7, + "velocityX": 1.6173783208395631, + "velocityY": 0.15114261831020614, + "timestamp": 3.227078990088942 + }, + { + "x": 5.574601308491241, + "y": 7.247773073685181, + "heading": 3.2864044075368533, + "angularVelocity": -5.100770952675197e-7, + "velocityX": 1.6173783208395631, + "velocityY": 0.15114261831020392, + "timestamp": 3.3229319943547404 + }, + { + "x": 5.7296318795780845, + "y": 7.262260547722812, + "heading": 3.286404358644431, + "angularVelocity": -5.100770991840634e-7, + "velocityX": 1.617378320839563, + "velocityY": 0.1511426183102018, + "timestamp": 3.4187849986205388 + }, + { + "x": 5.884662450664929, + "y": 7.276748021760444, + "heading": 3.286404309752008, + "angularVelocity": -5.100771023487481e-7, + "velocityX": 1.617378320839563, + "velocityY": 0.15114261831019962, + "timestamp": 3.514638002886337 + }, + { + "x": 6.039693021751773, + "y": 7.291235495798076, + "heading": 3.286404260859585, + "angularVelocity": -5.100771061559551e-7, + "velocityX": 1.6173783208395627, + "velocityY": 0.15114261831019746, + "timestamp": 3.6104910071521354 + }, + { + "x": 6.194723592838616, + "y": 7.305722969835706, + "heading": 3.2864042119671617, + "angularVelocity": -5.100771091660107e-7, + "velocityX": 1.6173783208395627, + "velocityY": 0.1511426183101951, + "timestamp": 3.706344011417934 + }, + { + "x": 6.349754163925461, + "y": 7.320210443873337, + "heading": 3.2864041630747383, + "angularVelocity": -5.100771129976334e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.15114261831019293, + "timestamp": 3.802197015683732 + }, + { + "x": 6.504784735012304, + "y": 7.334697917910969, + "heading": 3.286404114182314, + "angularVelocity": -5.100771163404491e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.1511426183101907, + "timestamp": 3.8980500199495305 + }, + { + "x": 6.659815306099149, + "y": 7.349185391948599, + "heading": 3.28640406528989, + "angularVelocity": -5.100771202032421e-7, + "velocityX": 1.6173783208395622, + "velocityY": 0.1511426183101885, + "timestamp": 3.993903024215329 + }, + { + "x": 6.814845877185992, + "y": 7.363672865986229, + "heading": 3.2864040163974653, + "angularVelocity": -5.100771233944824e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.15114261831018627, + "timestamp": 4.089756028481127 + }, + { + "x": 6.969876448272837, + "y": 7.378160340023859, + "heading": 3.2864039675050396, + "angularVelocity": -5.100771270486632e-7, + "velocityX": 1.6173783208395622, + "velocityY": 0.15114261831018408, + "timestamp": 4.1856090327469255 + }, + { + "x": 7.12490701935968, + "y": 7.392647814061489, + "heading": 3.286403918612615, + "angularVelocity": -5.100771304657537e-7, + "velocityX": 1.617378320839562, + "velocityY": 0.15114261831018191, + "timestamp": 4.281462037012724 + }, + { + "x": 7.279937590446525, + "y": 7.407135288099118, + "heading": 3.286403869720189, + "angularVelocity": -5.10077134427039e-7, + "velocityX": 1.6173783208395618, + "velocityY": 0.1511426183101796, + "timestamp": 4.377315041278522 + }, + { + "x": 7.4349681615333685, + "y": 7.421622762136748, + "heading": 3.286403820827763, + "angularVelocity": -5.100771387503622e-7, + "velocityX": 1.6173783208395618, + "velocityY": 0.15114261831017736, + "timestamp": 4.473168045544321 + }, + { + "x": 7.589998732620213, + "y": 7.436110236174376, + "heading": 3.2864037719353365, + "angularVelocity": -5.100771410554518e-7, + "velocityX": 1.6173783208395616, + "velocityY": 0.15114261831017525, + "timestamp": 4.569021049810119 + }, + { + "x": 7.745029303707056, + "y": 7.450597710212007, + "heading": 3.28640372304291, + "angularVelocity": -5.100771448287922e-7, + "velocityX": 1.6173783208395598, + "velocityY": 0.1511426183101905, + "timestamp": 4.664874054075917 + }, + { + "x": 7.90005987484092, + "y": 7.465085183746354, + "heading": 3.2864036741504377, + "angularVelocity": -5.1007762076643e-7, + "velocityX": 1.6173783213301043, + "velocityY": 0.15114261305961757, + "timestamp": 4.760727058341716 }, { "x": 7.999455451965332, "y": 7.473754405975342, "heading": 3.153293227446569, - "angularVelocity": -0.03044253542482025, - "velocityX": 0.33021570985126986, - "velocityY": 0.03437608724320182, - "timestamp": 2.5273272332371217 + "angularVelocity": -1.3886935284235506, + "velocityX": 1.0369583915052876, + "velocityY": 0.09044288486721058, + "timestamp": 4.856580062607514 }, { "x": 7.999455451965332, "y": 7.473754405975342, "heading": 3.153293227446569, - "angularVelocity": -3.79407884851814e-18, - "velocityX": 2.546987187300363e-16, - "velocityY": -1.0256209789219734e-16, - "timestamp": 2.5976055008604364 + "angularVelocity": 5.239369635456734e-24, + "velocityX": 2.4359888438060422e-22, + "velocityY": -8.774946070326848e-22, + "timestamp": 4.952433066873312 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Test2.2.traj b/src/main/deploy/choreo/Test2.2.traj index a0cb7c71..2c12c192 100644 --- a/src/main/deploy/choreo/Test2.2.traj +++ b/src/main/deploy/choreo/Test2.2.traj @@ -4,298 +4,433 @@ "x": 7.999455451965332, "y": 7.473754405975342, "heading": 3.153293227446569, - "angularVelocity": -3.79407884851814e-18, - "velocityX": 2.546987187300363e-16, - "velocityY": -1.0256209789219734e-16, + "angularVelocity": 5.239369635456734e-24, + "velocityX": 2.4359888438060422e-22, + "velocityY": -8.774946070326848e-22, "timestamp": 0 }, { - "x": 7.9777911181971035, - "y": 7.466979623755239, + "x": 7.928671024084623, + "y": 7.451618987983874, "heading": 3.153293227446569, - "angularVelocity": 4.0010921973360205e-29, - "velocityX": -0.31261406582002, - "velocityY": -0.09775939742848679, - "timestamp": 0.06930057261307221 + "angularVelocity": 2.2670564289793535e-36, + "velocityX": -0.8934591439715088, + "velocityY": -0.2793989045646846, + "timestamp": 0.07922514236752409 }, { - "x": 7.934462450883723, - "y": 7.453430059384792, + "x": 7.805841482889084, + "y": 7.413208234129291, "heading": 3.153293227446569, - "angularVelocity": -2.051733815514594e-34, - "velocityX": -0.6252281284210741, - "velocityY": -0.1955187938503514, - "timestamp": 0.13860114522614442 + "angularVelocity": 3.3814188338755363e-34, + "velocityX": -1.5503858689926344, + "velocityY": -0.4848303544397123, + "timestamp": 0.15845028473504819 }, { - "x": 7.869469450281316, - "y": 7.433105712944096, + "x": 7.683011941693537, + "y": 7.374797480274705, "heading": 3.153293227446569, - "angularVelocity": -1.98522229630168e-34, - "velocityX": -0.9378421873262779, - "velocityY": -0.29327818911646475, - "timestamp": 0.20790171783921663 + "angularVelocity": 6.0567834914455396e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.23767542710257228 }, { - "x": 7.782812116686986, - "y": 7.40600658452606, + "x": 7.56018240049799, + "y": 7.336386726420119, "heading": 3.153293227446569, - "angularVelocity": -1.9190015159804115e-34, - "velocityX": -1.2504562419442953, - "velocityY": -0.39103758304190667, - "timestamp": 0.27720229045228884 + "angularVelocity": -6.892446902508999e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.31690056947009637 }, { - "x": 7.6744904504495075, - "y": 7.372132674239751, + "x": 7.437352859302442, + "y": 7.297975972565533, "heading": 3.153293227446569, - "angularVelocity": -8.200637720207044e-29, - "velocityX": -1.5630702915295283, - "velocityY": -0.4887969753935167, - "timestamp": 0.34650286306536104 + "angularVelocity": -4.625140327997389e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.39612571183762046 }, { - "x": 7.544504451984089, - "y": 7.331483982215012, + "x": 7.314523318106895, + "y": 7.2595652187109465, "heading": 3.153293227446569, - "angularVelocity": 6.610929004011549e-29, - "velocityX": -1.8756843351233508, - "velocityY": -0.5865563658715173, - "timestamp": 0.41580343567843325 + "angularVelocity": 2.407409636834091e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.47535085420514456 }, { - "x": 7.392854121793352, - "y": 7.28406050860902, + "x": 7.191693776911348, + "y": 7.22115446485636, "heading": 3.153293227446569, - "angularVelocity": -6.483737543755527e-29, - "velocityX": -2.188298371464413, - "velocityY": -0.6843157540814643, - "timestamp": 0.48510400829150546 + "angularVelocity": -8.071167864078362e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.5545759965726687 }, { - "x": 7.219539460498179, - "y": 7.229862253615935, + "x": 7.0688642357158, + "y": 7.182743711001775, "heading": 3.153293227446569, - "angularVelocity": 8.073376116568391e-29, - "velocityX": -2.5009123988461828, - "velocityY": -0.7820751394896976, - "timestamp": 0.5544045809045777 + "angularVelocity": 6.706721989227974e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.6338011389401927 }, { - "x": 7.024560468885024, - "y": 7.168889217481695, + "x": 6.946034694520253, + "y": 7.1443329571471885, "heading": 3.153293227446569, - "angularVelocity": -3.526812750348743e-30, - "velocityX": -2.813526414879514, - "velocityY": -0.8798345213490937, - "timestamp": 0.6237051535176499 + "angularVelocity": -3.932169261853096e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.7130262813077168 }, { - "x": 6.807917147982327, - "y": 7.101141400527908, + "x": 6.823205153324706, + "y": 7.105922203292602, "heading": 3.153293227446569, - "angularVelocity": -9.438045628849687e-29, - "velocityX": -3.1261404160725776, - "velocityY": -0.9775938985677018, - "timestamp": 0.6930057261307221 + "angularVelocity": -5.711604766985455e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.7922514236752409 }, { - "x": 6.569609499192503, - "y": 7.026618803193132, + "x": 6.700375612129158, + "y": 7.067511449438016, "heading": 3.153293227446569, - "angularVelocity": 9.790681244582267e-29, - "velocityX": -3.438754397028912, - "velocityY": -1.0753532694579622, - "timestamp": 0.7623062987437943 + "angularVelocity": 9.147104299173202e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.871476566042765 }, { - "x": 6.309637524541267, - "y": 6.94532142611084, + "x": 6.577546070933611, + "y": 7.0291006955834305, "heading": 3.153293227446569, - "angularVelocity": -1.3895984503554753e-34, - "velocityX": -3.7513683487544087, - "velocityY": -1.1731126312072742, - "timestamp": 0.8316068713568665 + "angularVelocity": 4.987399043745874e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.9507017084102891 }, { - "x": 6.028001227211883, - "y": 6.85724927027649, + "x": 6.4547165297380635, + "y": 6.990689941728844, "heading": 3.153293227446569, - "angularVelocity": -1.323503512813964e-34, - "velocityX": -4.063982254545725, - "velocityY": -1.2708719785922367, - "timestamp": 0.9009074439699387 + "angularVelocity": 1.0726945561042014e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.0299268507778132 }, { - "x": 5.724700612934229, - "y": 6.762402337481905, + "x": 6.331886988542516, + "y": 6.952279187874258, "heading": 3.153293227446569, - "angularVelocity": -2.1989550257903952e-29, - "velocityX": -4.376596077655505, - "velocityY": -1.3686313001213652, - "timestamp": 0.9702080165830109 + "angularVelocity": 1.8749531736864053e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.1091519931453373 }, { - "x": 5.399735695078014, - "y": 6.660780631908008, + "x": 6.209057447346969, + "y": 6.913868434019672, "heading": 3.153293227446569, - "angularVelocity": 1.8024841235265752e-29, - "velocityX": -4.689209707841794, - "velocityY": -1.4663905613202457, - "timestamp": 1.0395085891960831 + "angularVelocity": -8.352063684746622e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.1883771355128614 }, { - "x": 5.053106540487171, - "y": 6.552384174457959, + "x": 6.0862279061514215, + "y": 6.875457680165086, "heading": 3.153293227446569, - "angularVelocity": 1.896211807861914e-29, - "velocityX": -5.001822373477154, - "velocityY": -1.564149520888688, - "timestamp": 1.1088091618091553 + "angularVelocity": 6.429027209138877e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.2676022778803855 }, { - "x": 4.7281416226309565, - "y": 6.4507624688840615, + "x": 5.963398364955874, + "y": 6.8370469263105, "heading": 3.153293227446569, - "angularVelocity": -1.4997927313419928e-29, - "velocityX": -4.689209707841795, - "velocityY": -1.4663905613202457, - "timestamp": 1.1781097344222276 + "angularVelocity": -7.349253697576567e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.3468274202479096 }, { - "x": 4.424841008353304, - "y": 6.355915536089477, + "x": 5.840568823760327, + "y": 6.798636172455914, "heading": 3.153293227446569, - "angularVelocity": -4.4185947102012545e-35, - "velocityX": -4.376596077655505, - "velocityY": -1.3686313001213652, - "timestamp": 1.2474103070352998 + "angularVelocity": -5.316547406258321e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.4260525626154337 }, { - "x": 4.14320471102392, - "y": 6.267843380255127, + "x": 5.7177392825647795, + "y": 6.760225418601328, "heading": 3.153293227446569, - "angularVelocity": -9.264733448000688e-35, - "velocityX": -4.063982254545726, - "velocityY": -1.2708719785922367, - "timestamp": 1.316710879648372 + "angularVelocity": 3.261106516242251e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.5052777049829578 }, { - "x": 3.883232736372683, - "y": 6.186546003172835, + "x": 5.594909741369232, + "y": 6.721814664746741, "heading": 3.153293227446569, - "angularVelocity": 3.5265482976182584e-30, - "velocityX": -3.751368348754409, - "velocityY": -1.1731126312072742, - "timestamp": 1.3860114522614442 + "angularVelocity": -2.0479976334874182e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.5845028473504819 }, { - "x": 3.644925087582859, - "y": 6.112023405838059, + "x": 5.472080200173685, + "y": 6.683403910892156, "heading": 3.153293227446569, - "angularVelocity": 2.156768838744686e-29, - "velocityX": -3.438754397028912, - "velocityY": -1.0753532694579622, - "timestamp": 1.4553120248745168 + "angularVelocity": -7.212902306224552e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.663727989718006 }, { - "x": 3.4282817666801613, - "y": 6.044275588884272, + "x": 5.3492506589781375, + "y": 6.64499315703757, "heading": 3.153293227446569, - "angularVelocity": -2.105657187574439e-28, - "velocityX": -3.1261404160725776, - "velocityY": -0.9775938985677018, - "timestamp": 1.5246125974875895 + "angularVelocity": 2.340395125909778e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.74295313208553 }, { - "x": 3.2333027750670063, - "y": 5.983302552750032, + "x": 5.22642111778259, + "y": 6.606582403182983, "heading": 3.153293227446569, - "angularVelocity": 1.8547117767579628e-28, - "velocityX": -2.813526414879514, - "velocityY": -0.8798345213490937, - "timestamp": 1.5939131701006621 + "angularVelocity": -2.132648663400479e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.8221782744530541 }, { - "x": 3.059988113771833, - "y": 5.929104297756947, + "x": 5.103591576587043, + "y": 6.568171649328397, "heading": 3.153293227446569, - "angularVelocity": -5.955568710068408e-35, - "velocityX": -2.5009123988461828, - "velocityY": -0.7820751394896976, - "timestamp": 1.6632137427137348 + "angularVelocity": -2.6423732915608732e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.9014034168205782 }, { - "x": 2.908337783581096, - "y": 5.881680824150955, + "x": 4.980762035391495, + "y": 6.529760895473811, "heading": 3.153293227446569, - "angularVelocity": -2.0810713408746708e-29, - "velocityX": -2.1882983714644135, - "velocityY": -0.6843157540814642, - "timestamp": 1.7325143153268074 + "angularVelocity": -1.5836433593674017e-28, + "velocityX": -1.5503858689927383, + "velocityY": -0.4848303544397451, + "timestamp": 1.9806285591881023 }, { - "x": 2.7783517851156776, - "y": 5.841032132126216, + "x": 4.857932494195948, + "y": 6.491350141619225, "heading": 3.153293227446569, - "angularVelocity": 2.081061415014159e-29, - "velocityX": -1.875684335123351, - "velocityY": -0.5865563658715173, - "timestamp": 1.80181488793988 + "angularVelocity": 3.462450644614712e-28, + "velocityX": -1.5503858689927386, + "velocityY": -0.4848303544397447, + "timestamp": 2.0598537015556264 }, { - "x": 2.6700301188781994, - "y": 5.807158221839908, + "x": 4.735102953000401, + "y": 6.452939387764639, "heading": 3.153293227446569, - "angularVelocity": 7.326893682940581e-29, - "velocityX": -1.5630702915295283, - "velocityY": -0.48879697539351663, - "timestamp": 1.8711154605529527 + "angularVelocity": 9.425461275139336e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.1390788439231505 }, { - "x": 2.583372785283869, - "y": 5.780059093421871, + "x": 4.612273411804853, + "y": 6.414528633910053, "heading": 3.153293227446569, - "angularVelocity": -7.326900961882318e-29, - "velocityX": -1.2504562419442953, - "velocityY": -0.39103758304190656, - "timestamp": 1.9404160331660254 + "angularVelocity": 2.8373243283513945e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.2183039862906746 }, { - "x": 2.5183797846814615, - "y": 5.759734746981175, + "x": 4.489443870609306, + "y": 6.3761178800554665, "heading": 3.153293227446569, - "angularVelocity": -2.64698564160025e-35, - "velocityX": -0.9378421873262781, - "velocityY": -0.2932781891164647, - "timestamp": 2.009716605779098 + "angularVelocity": 4.424546575494848e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.2975291286581987 }, { - "x": 2.4750511173680816, - "y": 5.746185182610728, + "x": 4.366614329413759, + "y": 6.337707126200881, "heading": 3.153293227446569, - "angularVelocity": -4.5099737006589516e-29, - "velocityX": -0.6252281284210743, - "velocityY": -0.1955187938503513, - "timestamp": 2.0790171783921707 + "angularVelocity": 2.6048393166300173e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.376754271025723 + }, + { + "x": 4.243784788218211, + "y": 6.299296372346295, + "heading": 3.153293227446569, + "angularVelocity": -8.141761105066643e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.455979413393247 + }, + { + "x": 4.120955247022664, + "y": 6.2608856184917085, + "heading": 3.153293227446569, + "angularVelocity": -1.5769317536066289e-37, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.535204555760771 + }, + { + "x": 3.998125705827117, + "y": 6.222474864637123, + "heading": 3.153293227446569, + "angularVelocity": -1.9751877406999063e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.614429698128295 + }, + { + "x": 3.8752961646315702, + "y": 6.184064110782536, + "heading": 3.153293227446569, + "angularVelocity": 6.296656188647329e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.693654840495819 + }, + { + "x": 3.7524666234360233, + "y": 6.145653356927951, + "heading": 3.153293227446569, + "angularVelocity": 3.349857109653225e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.7728799828633433 + }, + { + "x": 3.6296370822404764, + "y": 6.107242603073365, + "heading": 3.153293227446569, + "angularVelocity": 1.1073851910178502e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.8521051252308673 + }, + { + "x": 3.5068075410449295, + "y": 6.068831849218778, + "heading": 3.153293227446569, + "angularVelocity": -1.0776722091283702e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.9313302675983914 + }, + { + "x": 3.3839779998493826, + "y": 6.030421095364192, + "heading": 3.153293227446569, + "angularVelocity": -2.2657744543933122e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 3.0105554099659155 + }, + { + "x": 3.2611484586538357, + "y": 5.992010341509607, + "heading": 3.153293227446569, + "angularVelocity": 6.322058172960319e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 3.0897805523334396 + }, + { + "x": 3.138318917458289, + "y": 5.95359958765502, + "heading": 3.153293227446569, + "angularVelocity": -1.267303885279781e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 3.169005694700963 + }, + { + "x": 3.015489376262742, + "y": 5.915188833800434, + "heading": 3.153293227446569, + "angularVelocity": 4.539047579816208e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 3.248230837068486 + }, + { + "x": 2.892659835067195, + "y": 5.876778079945848, + "heading": 3.153293227446569, + "angularVelocity": -2.4352191603126546e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 3.3274559794360092 + }, + { + "x": 2.769830293871648, + "y": 5.838367326091261, + "heading": 3.153293227446569, + "angularVelocity": 1.9777124768596482e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 3.4066811218035324 + }, + { + "x": 2.6470007526761012, + "y": 5.799956572236676, + "heading": 3.153293227446569, + "angularVelocity": 1.6826010379363495e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 3.4859062641710556 + }, + { + "x": 2.5241712114805623, + "y": 5.761545818382093, + "heading": 3.153293227446569, + "angularVelocity": -1.9705474475500694e-33, + "velocityX": -1.5503858689926344, + "velocityY": -0.4848303544397123, + "timestamp": 3.565131406538579 }, { "x": 2.4533867835998535, "y": 5.739410400390625, "heading": 3.153293227446569, - "angularVelocity": 1.9207053764808128e-28, - "velocityX": -0.31261406582002027, - "velocityY": -0.09775939742848665, - "timestamp": 2.1483177510052434 + "angularVelocity": 2.748235734113482e-34, + "velocityX": -0.8934591439715088, + "velocityY": -0.2793989045646846, + "timestamp": 3.644356548906102 }, { "x": 2.4533867835998535, "y": 5.739410400390625, "heading": 3.153293227446569, - "angularVelocity": -4.899028012023128e-29, - "velocityX": 1.275108562409782e-22, - "velocityY": 3.987692290669026e-23, - "timestamp": 2.217618323618316 + "angularVelocity": -7.888608661057978e-31, + "velocityX": -1.038819230645035e-25, + "velocityY": -4.640517458430809e-27, + "timestamp": 3.7235816912736253 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Test2.traj b/src/main/deploy/choreo/Test2.traj index 68c886e7..027e49e3 100644 --- a/src/main/deploy/choreo/Test2.traj +++ b/src/main/deploy/choreo/Test2.traj @@ -1,976 +1,1219 @@ { "samples": [ { - "x": 0.3137660622596754, + "x": 0.3137660622596741, "y": 6.865104675292969, - "heading": 3.135568639860517, - "angularVelocity": -8.5693301650697e-28, - "velocityX": -3.4362695197914885e-17, - "velocityY": -4.743570524232907e-17, + "heading": 3.135568639860519, + "angularVelocity": 1.3323616061473399e-26, + "velocityX": -9.030177506128314e-25, + "velocityY": 7.578270673418084e-24, "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": 0.3285749308509426, + "y": 6.865947459797136, + "heading": 3.135568639860519, + "angularVelocity": 3.3337145202478786e-22, + "velocityX": 0.41797046933441173, + "velocityY": 0.023787032249159984, + "timestamp": 0.03543041836149472 + }, + { + "x": 0.35819266531481686, + "y": 6.867633028650745, + "heading": 3.135568639860519, + "angularVelocity": 3.3337147019213317e-22, + "velocityX": 0.8359408619363751, + "velocityY": 0.04757406013141577, + "timestamp": 0.07086083672298944 + }, + { + "x": 0.4026192574953566, + "y": 6.87016138138964, + "heading": 3.135568639860519, + "angularVelocity": 3.3337143389947597e-22, + "velocityX": 1.2539110243423486, + "velocityY": 0.07136107491303612, + "timestamp": 0.10629125508448417 + }, + { + "x": 0.4600803298886451, + "y": 6.873431536777097, + "heading": 3.135568639860519, + "angularVelocity": 3.331660178480041e-22, + "velocityX": 1.621800561512317, + "velocityY": 0.09229796143220789, + "timestamp": 0.14172167344597889 + }, + { + "x": 0.5175414022820589, + "y": 6.8767016921645565, + "heading": 3.135568639860519, + "angularVelocity": 3.3316629368551667e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233462, + "timestamp": 0.1771520918074736 + }, + { + "x": 0.5750024746754727, + "y": 6.879971847552017, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614681891394e-22, + "velocityX": 1.6218005615158566, + "velocityY": 0.09229796143233254, + "timestamp": 0.2125825101689683 + }, + { + "x": 0.6324635470688867, + "y": 6.883242002939478, + "heading": 3.135568639860519, + "angularVelocity": 3.331683104806914e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.24801292853046303 + }, + { + "x": 0.6899246194623005, + "y": 6.886512158326939, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614878052174e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233253, + "timestamp": 0.28344334689195777 + }, + { + "x": 0.7473856918557144, + "y": 6.8897823137144, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614758461554e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233254, + "timestamp": 0.3188737652534525 + }, + { + "x": 0.8048467642491284, + "y": 6.89305246910186, + "heading": 3.135568639860519, + "angularVelocity": 3.331638534198991e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233299, + "timestamp": 0.35430418361494725 + }, + { + "x": 0.8623078366425422, + "y": 6.89632262448932, + "heading": 3.135568639860519, + "angularVelocity": 3.3317076614902997e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233253, + "timestamp": 0.389734601976442 + }, + { + "x": 0.9197689090359561, + "y": 6.899592779876781, + "heading": 3.135568639860519, + "angularVelocity": 3.331661479116866e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233396, + "timestamp": 0.42516502033793674 + }, + { + "x": 0.97722998142937, + "y": 6.902862935264242, + "heading": 3.135568639860519, + "angularVelocity": 3.33166145502926e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.4605954386994315 + }, + { + "x": 1.0346910538227838, + "y": 6.906133090651703, + "heading": 3.135568639860519, + "angularVelocity": 3.3316600659908063e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233301, + "timestamp": 0.4960258570609262 + }, + { + "x": 1.0921521262161977, + "y": 6.9094032460391634, + "heading": 3.135568639860519, + "angularVelocity": 3.331661460674642e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.5314562754224209 + }, + { + "x": 1.1496131986096116, + "y": 6.912673401426624, + "heading": 3.135568639860519, + "angularVelocity": 3.331661475383932e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.5668866937839157 + }, + { + "x": 1.2070742710030253, + "y": 6.915943556814084, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614753921174e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233254, + "timestamp": 0.6023171121454104 + }, + { + "x": 1.2645353433964392, + "y": 6.919213712201545, + "heading": 3.135568639860519, + "angularVelocity": 3.331662731932369e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.6377475305069051 + }, + { + "x": 1.3219964157898532, + "y": 6.922483867589006, + "heading": 3.135568639860519, + "angularVelocity": 3.3316627143346654e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233393, + "timestamp": 0.6731779488683999 + }, + { + "x": 1.379457488183267, + "y": 6.925754022976466, + "heading": 3.135568639860519, + "angularVelocity": 3.331661483573468e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233157, + "timestamp": 0.7086083672298946 + }, + { + "x": 1.436918560576681, + "y": 6.929024178363926, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614890219236e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233392, + "timestamp": 0.7440387855913894 + }, + { + "x": 1.494379632970095, + "y": 6.932294333751387, + "heading": 3.135568639860519, + "angularVelocity": 3.331662743669949e-22, + "velocityX": 1.6218005615158566, + "velocityY": 0.09229796143233207, + "timestamp": 0.7794692039528841 + }, + { + "x": 1.5518407053635086, + "y": 6.935564489138847, + "heading": 3.135568639860519, + "angularVelocity": 3.331638909213181e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.8148996223143788 + }, + { + "x": 1.6093017777569225, + "y": 6.938834644526308, + "heading": 3.135568639860519, + "angularVelocity": 3.3316600948450887e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233622, + "timestamp": 0.8503300406758736 + }, + { + "x": 1.666762850150336, + "y": 6.942104799913777, + "heading": 3.135568639860519, + "angularVelocity": 3.331671581625241e-22, + "velocityX": 1.621800561515844, + "velocityY": 0.09229796143255317, + "timestamp": 0.8857604590373683 + }, + { + "x": 1.7242239225437261, + "y": 6.945374955301653, + "heading": 3.135568639860519, + "angularVelocity": 3.3334679678699274e-22, + "velocityX": 1.6218005615151887, + "velocityY": 0.09229796144406777, + "timestamp": 0.9211908773988631 + }, + { + "x": 1.7816849946975708, + "y": 6.948645114898682, + "heading": 3.135568639860519, + "angularVelocity": 1.7645788213167404e-22, + "velocityX": 1.6218005547541783, + "velocityY": 0.09229808024460076, + "timestamp": 0.9566212957603578 + }, + { + "x": 1.8178857407766151, + "y": 6.94853231479887, + "heading": 3.1384797599068888, + "angularVelocity": 0.12817174361754557, + "velocityX": 1.5938582646199322, + "velocityY": -0.004966399613447973, + "timestamp": 0.9793339464913666 + }, + { + "x": 1.8540603970824046, + "y": 6.948528771707492, + "heading": 3.1414999940123267, + "angularVelocity": 0.13297585302602363, + "velocityX": 1.592709575567121, + "velocityY": -0.00015599638369279018, + "timestamp": 1.0020465972223755 + }, + { + "x": 1.8901908310793394, + "y": 6.948640868874459, + "heading": 3.1447030512216227, + "angularVelocity": 0.14102524831780386, + "velocityX": 1.5907625413183106, + "velocityY": 0.0049354506567203535, + "timestamp": 1.0247592479533845 + }, + { + "x": 1.926257669084107, + "y": 6.9488776015696425, + "heading": 3.148167194864803, + "angularVelocity": 0.15252044704983372, + "velocityX": 1.5879625162168578, + "velocityY": 0.010422944375284186, + "timestamp": 1.0474718986843934 + }, + { + "x": 1.9622398024006038, + "y": 6.94925072993569, + "heading": 3.151976954699645, + "angularVelocity": 0.16773734954856945, + "velocityX": 1.584233110553301, + "velocityY": 0.01642821749276163, + "timestamp": 1.0701845494154023 + }, + { + "x": 1.9981171224268564, + "y": 6.949774504499785, + "heading": 3.156211459419019, + "angularVelocity": 0.1864381559653407, + "velocityX": 1.5796183567984214, + "velocityY": 0.023060917472754097, + "timestamp": 1.0928972001464112 + }, + { + "x": 2.033847488660219, + "y": 6.950468453992633, + "heading": 3.161038496208879, + "angularVelocity": 0.2125263513725333, + "velocityX": 1.5731482272380108, + "velocityY": 0.03055343478252191, + "timestamp": 1.1156098508774202 + }, + { + "x": 2.0694053227389695, + "y": 6.951354952814641, + "heading": 3.166554769842392, + "angularVelocity": 0.24287229609803831, + "velocityX": 1.565551925218666, + "velocityY": 0.039031059496573456, + "timestamp": 1.138322501608429 + }, + { + "x": 2.104784911670157, + "y": 6.952454847185295, + "heading": 3.1727732170956275, + "angularVelocity": 0.27378782542301205, + "velocityX": 1.5577040896809682, + "velocityY": 0.04842650836667849, + "timestamp": 1.161035152339438 + }, + { + "x": 2.1400047240253706, + "y": 6.9537191803371154, + "heading": 3.1796054213345153, + "angularVelocity": 0.3008105183231903, + "velocityX": 1.550669394441415, + "velocityY": 0.05566647269821489, + "timestamp": 1.183747803070447 + }, + { + "x": 2.175095855891208, + "y": 6.955084615506324, + "heading": 3.18691368664007, + "angularVelocity": 0.3217706903570311, + "velocityX": 1.5450038078527182, + "velocityY": 0.060117825320333544, + "timestamp": 1.2064604538014558 + }, + { + "x": 2.2101380445057046, + "y": 6.956471002562802, + "heading": 3.194369840668752, + "angularVelocity": 0.3282819833311086, + "velocityX": 1.5428489184071512, + "velocityY": 0.06104030185188484, + "timestamp": 1.2291731045324648 + }, + { + "x": 2.2451610724837057, + "y": 6.957849512193169, + "heading": 3.2018529792356185, + "angularVelocity": 0.32947006738627316, + "velocityX": 1.5420053076493287, + "velocityY": 0.060693471963871995, + "timestamp": 1.2518857552634737 + }, + { + "x": 2.2801629678458126, + "y": 6.959221595789265, + "heading": 3.2093707720743723, + "angularVelocity": 0.3309958369804056, + "velocityX": 1.5410748739388525, + "velocityY": 0.060410544429463274, + "timestamp": 1.2745984059944826 + }, + { + "x": 2.315144049729391, + "y": 6.960586594052822, + "heading": 3.2169217131634897, + "angularVelocity": 0.3324552989672944, + "velocityX": 1.540158491312511, + "velocityY": 0.06009858909567994, + "timestamp": 1.2973110567254915 + }, + { + "x": 2.3501040543802802, + "y": 6.961944386116353, + "heading": 3.2245066187504112, + "angularVelocity": 0.3339506989629348, + "velocityX": 1.539230496031878, + "velocityY": 0.05978131216881497, + "timestamp": 1.3200237074565004 + }, + { + "x": 2.3850426980471595, + "y": 6.963294864074251, + "heading": 3.232126373384074, + "angularVelocity": 0.3354850441678861, + "velocityX": 1.5382900076554706, + "velocityY": 0.05945928433854099, + "timestamp": 1.3427363581875094 + }, + { + "x": 2.419959710539657, + "y": 6.964637896582436, + "heading": 3.239781798872735, + "angularVelocity": 0.3370555722150803, + "velocityX": 1.5373376232491214, + "velocityY": 0.05913147364836824, + "timestamp": 1.3654490089185183 + }, + { + "x": 2.454854803951486, + "y": 6.965973356645481, + "heading": 3.247473776388813, + "angularVelocity": 0.3386648968090985, + "velocityX": 1.5363725628109135, + "velocityY": 0.05879807156202029, + "timestamp": 1.3881616596495272 + }, + { + "x": 2.48972762858419, + "y": 6.967301155248667, + "heading": 3.2552034126870217, + "angularVelocity": 0.3403229499609042, + "velocityX": 1.5353921057348447, + "velocityY": 0.05846075030657316, + "timestamp": 1.4108743103805361 + }, + { + "x": 2.5245780070203394, + "y": 6.968621054178632, + "heading": 3.262971149366171, + "angularVelocity": 0.34200044596925644, + "velocityX": 1.5344038372663136, + "velocityY": 0.05811294091547401, + "timestamp": 1.433586961111545 + }, + { + "x": 2.559405532428149, + "y": 6.969932985582836, + "heading": 3.2707782920481354, + "angularVelocity": 0.3437354263236719, + "velocityX": 1.5333976566751537, + "velocityY": 0.05776214409057404, + "timestamp": 1.456299611842554 + }, + { + "x": 2.594207801112396, + "y": 6.97123839692077, + "heading": 3.2786335848605472, + "angularVelocity": 0.34585539598366793, + "velocityX": 1.5322856454060756, + "velocityY": 0.05747507648462563, + "timestamp": 1.4790122625735629 + }, + { + "x": 2.6290202140808105, + "y": 6.972513675689697, + "heading": 3.286405336491994, + "angularVelocity": 0.3421772175995684, + "velocityX": 1.5327322812606092, + "velocityY": 0.05614838990089804, + "timestamp": 1.5017249133045718 + }, + { + "x": 2.7840510289336455, + "y": 6.98699854094784, + "heading": 3.286405287600397, + "angularVelocity": -5.100684883763116e-7, + "velocityX": 1.6173808639626774, + "velocityY": 0.1511154018498607, + "timestamp": 1.5975779175703702 + }, + { + "x": 2.93908160001489, + "y": 7.001486015045406, + "heading": 3.2864052387079807, + "angularVelocity": -5.100770354356606e-7, + "velocityX": 1.6173783207811374, + "velocityY": 0.15114261893547631, + "timestamp": 1.6934309218361685 + }, + { + "x": 3.094112171101734, + "y": 7.015973489083046, + "heading": 3.286405189815564, + "angularVelocity": -5.100770390190521e-7, + "velocityX": 1.617378320839561, + "velocityY": 0.15114261831028078, + "timestamp": 1.7892839261019668 + }, + { + "x": 3.2491427421885786, + "y": 7.030460963120681, + "heading": 3.286405140923147, + "angularVelocity": -5.100770430673946e-7, + "velocityX": 1.617378320839565, + "velocityY": 0.15114261831023715, + "timestamp": 1.8851369303677652 + }, + { + "x": 3.404173313275423, + "y": 7.0449484371583155, + "heading": 3.28640509203073, + "angularVelocity": -5.100770460466358e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831023496, + "timestamp": 1.9809899346335635 + }, + { + "x": 3.5592038843622675, + "y": 7.05943591119595, + "heading": 3.286405043138312, + "angularVelocity": -5.100770488976204e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831023276, + "timestamp": 2.076842938899362 + }, + { + "x": 3.714234455449112, + "y": 7.073923385233584, + "heading": 3.2864049942458946, + "angularVelocity": -5.100770524050277e-7, + "velocityX": 1.6173783208395642, + "velocityY": 0.15114261831023054, + "timestamp": 2.1726959431651602 + }, + { + "x": 3.8692650265359565, + "y": 7.088410859271219, + "heading": 3.2864049453534765, + "angularVelocity": -5.100770565197187e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831022827, + "timestamp": 2.2685489474309586 + }, + { + "x": 4.0242955976228005, + "y": 7.102898333308853, + "heading": 3.2864048964610575, + "angularVelocity": -5.100770606946053e-7, + "velocityX": 1.6173783208395642, + "velocityY": 0.15114261831022607, + "timestamp": 2.364401951696757 + }, + { + "x": 4.179326168709644, + "y": 7.117385807346487, + "heading": 3.286404847568639, + "angularVelocity": -5.100770634020749e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.151142618310224, + "timestamp": 2.4602549559625553 + }, + { + "x": 4.334356739796489, + "y": 7.13187328138412, + "heading": 3.2864047986762195, + "angularVelocity": -5.100770666030285e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.15114261831022163, + "timestamp": 2.5561079602283536 + }, + { + "x": 4.489387310883332, + "y": 7.146360755421753, + "heading": 3.2864047497838, + "angularVelocity": -5.100770706092546e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.15114261831021944, + "timestamp": 2.651960964494152 + }, + { + "x": 4.644417881970177, + "y": 7.160848229459387, + "heading": 3.28640470089138, + "angularVelocity": -5.100770740673254e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021727, + "timestamp": 2.7478139687599503 + }, + { + "x": 4.79944845305702, + "y": 7.175335703497019, + "heading": 3.28640465199896, + "angularVelocity": -5.100770777885376e-7, + "velocityX": 1.6173783208395638, + "velocityY": 0.15114261831021514, + "timestamp": 2.8436669730257487 + }, + { + "x": 4.954479024143865, + "y": 7.1898231775346515, + "heading": 3.286404603106539, + "angularVelocity": -5.100770810299081e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021283, + "timestamp": 2.939519977291547 + }, + { + "x": 5.109509595230708, + "y": 7.2043106515722855, + "heading": 3.2864045542141187, + "angularVelocity": -5.100770846564483e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021056, + "timestamp": 3.0353729815573454 + }, + { + "x": 5.264540166317553, + "y": 7.218798125609918, + "heading": 3.286404505321697, + "angularVelocity": -5.100770889061609e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.1511426183102084, + "timestamp": 3.1312259858231437 + }, + { + "x": 5.419570737404396, + "y": 7.233285599647549, + "heading": 3.2864044564292754, + "angularVelocity": -5.100770922378371e-7, + "velocityX": 1.6173783208395631, + "velocityY": 0.15114261831020614, + "timestamp": 3.227078990088942 + }, + { + "x": 5.574601308491241, + "y": 7.247773073685181, + "heading": 3.2864044075368533, + "angularVelocity": -5.100770952675197e-7, + "velocityX": 1.6173783208395631, + "velocityY": 0.15114261831020392, + "timestamp": 3.3229319943547404 + }, + { + "x": 5.7296318795780845, + "y": 7.262260547722812, + "heading": 3.286404358644431, + "angularVelocity": -5.100770991840634e-7, + "velocityX": 1.617378320839563, + "velocityY": 0.1511426183102018, + "timestamp": 3.4187849986205388 + }, + { + "x": 5.884662450664929, + "y": 7.276748021760444, + "heading": 3.286404309752008, + "angularVelocity": -5.100771023487481e-7, + "velocityX": 1.617378320839563, + "velocityY": 0.15114261831019962, + "timestamp": 3.514638002886337 + }, + { + "x": 6.039693021751773, + "y": 7.291235495798076, + "heading": 3.286404260859585, + "angularVelocity": -5.100771061559551e-7, + "velocityX": 1.6173783208395627, + "velocityY": 0.15114261831019746, + "timestamp": 3.6104910071521354 + }, + { + "x": 6.194723592838616, + "y": 7.305722969835706, + "heading": 3.2864042119671617, + "angularVelocity": -5.100771091660107e-7, + "velocityX": 1.6173783208395627, + "velocityY": 0.1511426183101951, + "timestamp": 3.706344011417934 + }, + { + "x": 6.349754163925461, + "y": 7.320210443873337, + "heading": 3.2864041630747383, + "angularVelocity": -5.100771129976334e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.15114261831019293, + "timestamp": 3.802197015683732 + }, + { + "x": 6.504784735012304, + "y": 7.334697917910969, + "heading": 3.286404114182314, + "angularVelocity": -5.100771163404491e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.1511426183101907, + "timestamp": 3.8980500199495305 + }, + { + "x": 6.659815306099149, + "y": 7.349185391948599, + "heading": 3.28640406528989, + "angularVelocity": -5.100771202032421e-7, + "velocityX": 1.6173783208395622, + "velocityY": 0.1511426183101885, + "timestamp": 3.993903024215329 + }, + { + "x": 6.814845877185992, + "y": 7.363672865986229, + "heading": 3.2864040163974653, + "angularVelocity": -5.100771233944824e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.15114261831018627, + "timestamp": 4.089756028481127 + }, + { + "x": 6.969876448272837, + "y": 7.378160340023859, + "heading": 3.2864039675050396, + "angularVelocity": -5.100771270486632e-7, + "velocityX": 1.6173783208395622, + "velocityY": 0.15114261831018408, + "timestamp": 4.1856090327469255 + }, + { + "x": 7.12490701935968, + "y": 7.392647814061489, + "heading": 3.286403918612615, + "angularVelocity": -5.100771304657537e-7, + "velocityX": 1.617378320839562, + "velocityY": 0.15114261831018191, + "timestamp": 4.281462037012724 + }, + { + "x": 7.279937590446525, + "y": 7.407135288099118, + "heading": 3.286403869720189, + "angularVelocity": -5.10077134427039e-7, + "velocityX": 1.6173783208395618, + "velocityY": 0.1511426183101796, + "timestamp": 4.377315041278522 + }, + { + "x": 7.4349681615333685, + "y": 7.421622762136748, + "heading": 3.286403820827763, + "angularVelocity": -5.100771387503622e-7, + "velocityX": 1.6173783208395618, + "velocityY": 0.15114261831017736, + "timestamp": 4.473168045544321 + }, + { + "x": 7.589998732620213, + "y": 7.436110236174376, + "heading": 3.2864037719353365, + "angularVelocity": -5.100771410554518e-7, + "velocityX": 1.6173783208395616, + "velocityY": 0.15114261831017525, + "timestamp": 4.569021049810119 + }, + { + "x": 7.745029303707056, + "y": 7.450597710212007, + "heading": 3.28640372304291, + "angularVelocity": -5.100771448287922e-7, + "velocityX": 1.6173783208395598, + "velocityY": 0.1511426183101905, + "timestamp": 4.664874054075917 + }, + { + "x": 7.90005987484092, + "y": 7.465085183746354, + "heading": 3.2864036741504377, + "angularVelocity": -5.1007762076643e-7, + "velocityX": 1.6173783213301043, + "velocityY": 0.15114261305961757, + "timestamp": 4.760727058341716 }, { "x": 7.999455451965332, "y": 7.473754405975342, "heading": 3.153293227446569, - "angularVelocity": -0.03044253542482025, - "velocityX": 0.33021570985126986, - "velocityY": 0.03437608724320182, - "timestamp": 2.5273272332371217 + "angularVelocity": -1.3886935284235506, + "velocityX": 1.0369583915052876, + "velocityY": 0.09044288486721058, + "timestamp": 4.856580062607514 }, { "x": 7.999455451965332, "y": 7.473754405975342, "heading": 3.153293227446569, - "angularVelocity": -3.79407884851814e-18, - "velocityX": 2.546987187300363e-16, - "velocityY": -1.0256209789219734e-16, - "timestamp": 2.5976055008604364 + "angularVelocity": 5.239369635456734e-24, + "velocityX": 2.4359888438060422e-22, + "velocityY": -8.774946070326848e-22, + "timestamp": 4.952433066873312 }, { - "x": 7.9777911181971035, - "y": 7.466979623755239, + "x": 7.928671024084623, + "y": 7.451618987983874, "heading": 3.153293227446569, - "angularVelocity": 4.0010921973360205e-29, - "velocityX": -0.31261406582002, - "velocityY": -0.09775939742848679, - "timestamp": 2.6669060734735086 + "angularVelocity": 2.2670564289793535e-36, + "velocityX": -0.8934591439715088, + "velocityY": -0.2793989045646846, + "timestamp": 5.031658209240836 }, { - "x": 7.934462450883723, - "y": 7.453430059384792, + "x": 7.805841482889084, + "y": 7.413208234129291, "heading": 3.153293227446569, - "angularVelocity": -2.051733815514594e-34, - "velocityX": -0.6252281284210741, - "velocityY": -0.1955187938503514, - "timestamp": 2.736206646086581 + "angularVelocity": 3.3814188338755363e-34, + "velocityX": -1.5503858689926344, + "velocityY": -0.4848303544397123, + "timestamp": 5.1108833516083605 }, { - "x": 7.869469450281316, - "y": 7.433105712944096, + "x": 7.683011941693537, + "y": 7.374797480274705, "heading": 3.153293227446569, - "angularVelocity": -1.98522229630168e-34, - "velocityX": -0.9378421873262779, - "velocityY": -0.29327818911646475, - "timestamp": 2.805507218699653 + "angularVelocity": 6.0567834914455396e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.190108493975885 }, { - "x": 7.782812116686986, - "y": 7.40600658452606, + "x": 7.56018240049799, + "y": 7.336386726420119, "heading": 3.153293227446569, - "angularVelocity": -1.9190015159804115e-34, - "velocityX": -1.2504562419442953, - "velocityY": -0.39103758304190667, - "timestamp": 2.8748077913127252 + "angularVelocity": -6.892446902508999e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.269333636343409 }, { - "x": 7.6744904504495075, - "y": 7.372132674239751, + "x": 7.437352859302442, + "y": 7.297975972565533, "heading": 3.153293227446569, - "angularVelocity": -8.200637720207044e-29, - "velocityX": -1.5630702915295283, - "velocityY": -0.4887969753935167, - "timestamp": 2.9441083639257974 + "angularVelocity": -4.625140327997389e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.348558778710933 }, { - "x": 7.544504451984089, - "y": 7.331483982215012, + "x": 7.314523318106895, + "y": 7.2595652187109465, "heading": 3.153293227446569, - "angularVelocity": 6.610929004011549e-29, - "velocityX": -1.8756843351233508, - "velocityY": -0.5865563658715173, - "timestamp": 3.0134089365388697 + "angularVelocity": 2.407409636834091e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.427783921078457 }, { - "x": 7.392854121793352, - "y": 7.28406050860902, + "x": 7.191693776911348, + "y": 7.22115446485636, "heading": 3.153293227446569, - "angularVelocity": -6.483737543755527e-29, - "velocityX": -2.188298371464413, - "velocityY": -0.6843157540814643, - "timestamp": 3.082709509151942 + "angularVelocity": -8.071167864078362e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.507009063445981 }, { - "x": 7.219539460498179, - "y": 7.229862253615935, + "x": 7.0688642357158, + "y": 7.182743711001775, "heading": 3.153293227446569, - "angularVelocity": 8.073376116568391e-29, - "velocityX": -2.5009123988461828, - "velocityY": -0.7820751394896976, - "timestamp": 3.152010081765014 + "angularVelocity": 6.706721989227974e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.586234205813505 }, { - "x": 7.024560468885024, - "y": 7.168889217481695, + "x": 6.946034694520253, + "y": 7.1443329571471885, "heading": 3.153293227446569, - "angularVelocity": -3.526812750348743e-30, - "velocityX": -2.813526414879514, - "velocityY": -0.8798345213490937, - "timestamp": 3.2213106543780863 + "angularVelocity": -3.932169261853096e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.665459348181029 }, { - "x": 6.807917147982327, - "y": 7.101141400527908, + "x": 6.823205153324706, + "y": 7.105922203292602, "heading": 3.153293227446569, - "angularVelocity": -9.438045628849687e-29, - "velocityX": -3.1261404160725776, - "velocityY": -0.9775938985677018, - "timestamp": 3.2906112269911585 + "angularVelocity": -5.711604766985455e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.744684490548553 }, { - "x": 6.569609499192503, - "y": 7.026618803193132, + "x": 6.700375612129158, + "y": 7.067511449438016, "heading": 3.153293227446569, - "angularVelocity": 9.790681244582267e-29, - "velocityX": -3.438754397028912, - "velocityY": -1.0753532694579622, - "timestamp": 3.3599117996042307 + "angularVelocity": 9.147104299173202e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.823909632916077 }, { - "x": 6.309637524541267, - "y": 6.94532142611084, + "x": 6.577546070933611, + "y": 7.0291006955834305, "heading": 3.153293227446569, - "angularVelocity": -1.3895984503554753e-34, - "velocityX": -3.7513683487544087, - "velocityY": -1.1731126312072742, - "timestamp": 3.429212372217303 + "angularVelocity": 4.987399043745874e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.903134775283601 }, { - "x": 6.028001227211883, - "y": 6.85724927027649, + "x": 6.4547165297380635, + "y": 6.990689941728844, "heading": 3.153293227446569, - "angularVelocity": -1.323503512813964e-34, - "velocityX": -4.063982254545725, - "velocityY": -1.2708719785922367, - "timestamp": 3.498512944830375 + "angularVelocity": 1.0726945561042014e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.9823599176511255 }, { - "x": 5.724700612934229, - "y": 6.762402337481905, + "x": 6.331886988542516, + "y": 6.952279187874258, "heading": 3.153293227446569, - "angularVelocity": -2.1989550257903952e-29, - "velocityX": -4.376596077655505, - "velocityY": -1.3686313001213652, - "timestamp": 3.5678135174434473 + "angularVelocity": 1.8749531736864053e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.06158506001865 }, { - "x": 5.399735695078014, - "y": 6.660780631908008, + "x": 6.209057447346969, + "y": 6.913868434019672, "heading": 3.153293227446569, - "angularVelocity": 1.8024841235265752e-29, - "velocityX": -4.689209707841794, - "velocityY": -1.4663905613202457, - "timestamp": 3.6371140900565195 + "angularVelocity": -8.352063684746622e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.140810202386174 }, { - "x": 5.053106540487171, - "y": 6.552384174457959, + "x": 6.0862279061514215, + "y": 6.875457680165086, "heading": 3.153293227446569, - "angularVelocity": 1.896211807861914e-29, - "velocityX": -5.001822373477154, - "velocityY": -1.564149520888688, - "timestamp": 3.7064146626695917 + "angularVelocity": 6.429027209138877e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.220035344753698 }, { - "x": 4.7281416226309565, - "y": 6.4507624688840615, + "x": 5.963398364955874, + "y": 6.8370469263105, "heading": 3.153293227446569, - "angularVelocity": -1.4997927313419928e-29, - "velocityX": -4.689209707841795, - "velocityY": -1.4663905613202457, - "timestamp": 3.775715235282664 + "angularVelocity": -7.349253697576567e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.299260487121222 }, { - "x": 4.424841008353304, - "y": 6.355915536089477, + "x": 5.840568823760327, + "y": 6.798636172455914, "heading": 3.153293227446569, - "angularVelocity": -4.4185947102012545e-35, - "velocityX": -4.376596077655505, - "velocityY": -1.3686313001213652, - "timestamp": 3.845015807895736 + "angularVelocity": -5.316547406258321e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.378485629488746 }, { - "x": 4.14320471102392, - "y": 6.267843380255127, + "x": 5.7177392825647795, + "y": 6.760225418601328, "heading": 3.153293227446569, - "angularVelocity": -9.264733448000688e-35, - "velocityX": -4.063982254545726, - "velocityY": -1.2708719785922367, - "timestamp": 3.9143163805088084 + "angularVelocity": 3.261106516242251e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.45771077185627 }, { - "x": 3.883232736372683, - "y": 6.186546003172835, + "x": 5.594909741369232, + "y": 6.721814664746741, "heading": 3.153293227446569, - "angularVelocity": 3.5265482976182584e-30, - "velocityX": -3.751368348754409, - "velocityY": -1.1731126312072742, - "timestamp": 3.9836169531218806 + "angularVelocity": -2.0479976334874182e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.536935914223794 }, { - "x": 3.644925087582859, - "y": 6.112023405838059, + "x": 5.472080200173685, + "y": 6.683403910892156, "heading": 3.153293227446569, - "angularVelocity": 2.156768838744686e-29, - "velocityX": -3.438754397028912, - "velocityY": -1.0753532694579622, - "timestamp": 4.052917525734953 + "angularVelocity": -7.212902306224552e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.616161056591318 }, { - "x": 3.4282817666801613, - "y": 6.044275588884272, + "x": 5.3492506589781375, + "y": 6.64499315703757, "heading": 3.153293227446569, - "angularVelocity": -2.105657187574439e-28, - "velocityX": -3.1261404160725776, - "velocityY": -0.9775938985677018, - "timestamp": 4.122218098348026 + "angularVelocity": 2.340395125909778e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.695386198958842 }, { - "x": 3.2333027750670063, - "y": 5.983302552750032, + "x": 5.22642111778259, + "y": 6.606582403182983, "heading": 3.153293227446569, - "angularVelocity": 1.8547117767579628e-28, - "velocityX": -2.813526414879514, - "velocityY": -0.8798345213490937, - "timestamp": 4.1915186709610985 + "angularVelocity": -2.132648663400479e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.774611341326366 }, { - "x": 3.059988113771833, - "y": 5.929104297756947, + "x": 5.103591576587043, + "y": 6.568171649328397, "heading": 3.153293227446569, - "angularVelocity": -5.955568710068408e-35, - "velocityX": -2.5009123988461828, - "velocityY": -0.7820751394896976, - "timestamp": 4.260819243574171 + "angularVelocity": -2.6423732915608732e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.8538364836938905 }, { - "x": 2.908337783581096, - "y": 5.881680824150955, + "x": 4.980762035391495, + "y": 6.529760895473811, "heading": 3.153293227446569, - "angularVelocity": -2.0810713408746708e-29, - "velocityX": -2.1882983714644135, - "velocityY": -0.6843157540814642, - "timestamp": 4.330119816187244 + "angularVelocity": -1.5836433593674017e-28, + "velocityX": -1.5503858689927383, + "velocityY": -0.4848303544397451, + "timestamp": 6.933061626061415 }, { - "x": 2.7783517851156776, - "y": 5.841032132126216, + "x": 4.857932494195948, + "y": 6.491350141619225, "heading": 3.153293227446569, - "angularVelocity": 2.081061415014159e-29, - "velocityX": -1.875684335123351, - "velocityY": -0.5865563658715173, - "timestamp": 4.3994203888003165 + "angularVelocity": 3.462450644614712e-28, + "velocityX": -1.5503858689927386, + "velocityY": -0.4848303544397447, + "timestamp": 7.012286768428939 }, { - "x": 2.6700301188781994, - "y": 5.807158221839908, + "x": 4.735102953000401, + "y": 6.452939387764639, "heading": 3.153293227446569, - "angularVelocity": 7.326893682940581e-29, - "velocityX": -1.5630702915295283, - "velocityY": -0.48879697539351663, - "timestamp": 4.468720961413389 + "angularVelocity": 9.425461275139336e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.091511910796463 }, { - "x": 2.583372785283869, - "y": 5.780059093421871, + "x": 4.612273411804853, + "y": 6.414528633910053, "heading": 3.153293227446569, - "angularVelocity": -7.326900961882318e-29, - "velocityX": -1.2504562419442953, - "velocityY": -0.39103758304190656, - "timestamp": 4.538021534026462 + "angularVelocity": 2.8373243283513945e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.170737053163987 }, { - "x": 2.5183797846814615, - "y": 5.759734746981175, + "x": 4.489443870609306, + "y": 6.3761178800554665, "heading": 3.153293227446569, - "angularVelocity": -2.64698564160025e-35, - "velocityX": -0.9378421873262781, - "velocityY": -0.2932781891164647, - "timestamp": 4.6073221066395345 + "angularVelocity": 4.424546575494848e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.249962195531511 }, { - "x": 2.4750511173680816, - "y": 5.746185182610728, + "x": 4.366614329413759, + "y": 6.337707126200881, "heading": 3.153293227446569, - "angularVelocity": -4.5099737006589516e-29, - "velocityX": -0.6252281284210743, - "velocityY": -0.1955187938503513, - "timestamp": 4.676622679252607 + "angularVelocity": 2.6048393166300173e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.329187337899035 + }, + { + "x": 4.243784788218211, + "y": 6.299296372346295, + "heading": 3.153293227446569, + "angularVelocity": -8.141761105066643e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.408412480266559 + }, + { + "x": 4.120955247022664, + "y": 6.2608856184917085, + "heading": 3.153293227446569, + "angularVelocity": -1.5769317536066289e-37, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.487637622634083 + }, + { + "x": 3.998125705827117, + "y": 6.222474864637123, + "heading": 3.153293227446569, + "angularVelocity": -1.9751877406999063e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.566862765001607 + }, + { + "x": 3.8752961646315702, + "y": 6.184064110782536, + "heading": 3.153293227446569, + "angularVelocity": 6.296656188647329e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.6460879073691315 + }, + { + "x": 3.7524666234360233, + "y": 6.145653356927951, + "heading": 3.153293227446569, + "angularVelocity": 3.349857109653225e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.725313049736656 + }, + { + "x": 3.6296370822404764, + "y": 6.107242603073365, + "heading": 3.153293227446569, + "angularVelocity": 1.1073851910178502e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.80453819210418 + }, + { + "x": 3.5068075410449295, + "y": 6.068831849218778, + "heading": 3.153293227446569, + "angularVelocity": -1.0776722091283702e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.883763334471704 + }, + { + "x": 3.3839779998493826, + "y": 6.030421095364192, + "heading": 3.153293227446569, + "angularVelocity": -2.2657744543933122e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.962988476839228 + }, + { + "x": 3.2611484586538357, + "y": 5.992010341509607, + "heading": 3.153293227446569, + "angularVelocity": 6.322058172960319e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.042213619206752 + }, + { + "x": 3.138318917458289, + "y": 5.95359958765502, + "heading": 3.153293227446569, + "angularVelocity": -1.267303885279781e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.121438761574275 + }, + { + "x": 3.015489376262742, + "y": 5.915188833800434, + "heading": 3.153293227446569, + "angularVelocity": 4.539047579816208e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.200663903941798 + }, + { + "x": 2.892659835067195, + "y": 5.876778079945848, + "heading": 3.153293227446569, + "angularVelocity": -2.4352191603126546e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.279889046309322 + }, + { + "x": 2.769830293871648, + "y": 5.838367326091261, + "heading": 3.153293227446569, + "angularVelocity": 1.9777124768596482e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.359114188676845 + }, + { + "x": 2.6470007526761012, + "y": 5.799956572236676, + "heading": 3.153293227446569, + "angularVelocity": 1.6826010379363495e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.438339331044368 + }, + { + "x": 2.5241712114805623, + "y": 5.761545818382093, + "heading": 3.153293227446569, + "angularVelocity": -1.9705474475500694e-33, + "velocityX": -1.5503858689926344, + "velocityY": -0.4848303544397123, + "timestamp": 8.517564473411891 }, { "x": 2.4533867835998535, "y": 5.739410400390625, "heading": 3.153293227446569, - "angularVelocity": 1.9207053764808128e-28, - "velocityX": -0.31261406582002027, - "velocityY": -0.09775939742848665, - "timestamp": 4.74592325186568 + "angularVelocity": 2.748235734113482e-34, + "velocityX": -0.8934591439715088, + "velocityY": -0.2793989045646846, + "timestamp": 8.596789615779414 }, { "x": 2.4533867835998535, "y": 5.739410400390625, "heading": 3.153293227446569, - "angularVelocity": -4.899028012023128e-29, - "velocityX": 1.275108562409782e-22, - "velocityY": 3.987692290669026e-23, - "timestamp": 4.815223824478752 + "angularVelocity": -7.888608661057978e-31, + "velocityX": -1.038819230645035e-25, + "velocityY": -4.640517458430809e-27, + "timestamp": 8.676014758146938 } ] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FrontWing1.auto b/src/main/deploy/pathplanner/autos/FrontWing1.auto new file mode 100644 index 00000000..ac1f7ee3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/FrontWing1.auto @@ -0,0 +1,69 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.2263859510421753, + "y": 5.594110488891602 + }, + "rotation": 179.96604345059157 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontWing1.1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "FrontWing1.2" + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FrontWing2.auto b/src/main/deploy/pathplanner/autos/FrontWing2.auto new file mode 100644 index 00000000..e02712a3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/FrontWing2.auto @@ -0,0 +1,50 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.2263859510421753, + "y": 5.594110488891602 + }, + "rotation": 179.96604345059157 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontWing2.1" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FrontWing3.auto b/src/main/deploy/pathplanner/autos/FrontWing3.auto new file mode 100644 index 00000000..62bec56a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/FrontWing3.auto @@ -0,0 +1,50 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.2263859510421753, + "y": 5.594110488891602 + }, + "rotation": 179.96604345059157 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontWing3Contested5.1" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FrontWing3Contested5.auto b/src/main/deploy/pathplanner/autos/FrontWing3Contested5.auto new file mode 100644 index 00000000..1fd0c9df --- /dev/null +++ b/src/main/deploy/pathplanner/autos/FrontWing3Contested5.auto @@ -0,0 +1,88 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.2263859510421753, + "y": 5.594110488891602 + }, + "rotation": 179.96604345059157 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontWing3Contested5.1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontWing3Contested5.2" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PathPlannerTest.auto b/src/main/deploy/pathplanner/autos/PathPlannerTest.auto new file mode 100644 index 00000000..e2736b56 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PathPlannerTest.auto @@ -0,0 +1,19 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "OnePieceAmpStart" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/AmpLineupPath.path b/src/main/deploy/pathplanner/paths/AmpLineupPath.path new file mode 100644 index 00000000..c8b2a305 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/AmpLineupPath.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.8725109148331507, + "y": 7.096994083000375 + }, + "prevControl": null, + "nextControl": { + "x": 1.8816898899058623, + "y": 7.409079235472567 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8725109148331507, + "y": 7.583479761854086 + }, + "prevControl": { + "x": 1.8908688649785739, + "y": 7.115352033145799 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 90.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/OnePieceAmpStart.path b/src/main/deploy/pathplanner/paths/OnePieceAmpStart.path new file mode 100644 index 00000000..3450caf8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/OnePieceAmpStart.path @@ -0,0 +1,84 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.5232015791445568, + "y": 6.977667407055125 + }, + "prevControl": null, + "nextControl": { + "x": 1.5232015791445608, + "y": 6.977667407055125 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.4691442945593995, + "y": 6.977667407055125 + }, + "prevControl": { + "x": 1.4691442945593995, + "y": 6.977667407055125 + }, + "nextControl": { + "x": 3.4691442945593995, + "y": 6.977667407055125 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.765412911513948, + "y": 7.42743718561799 + }, + "prevControl": { + "x": 5.520899364773203, + "y": 7.193373321263846 + }, + "nextControl": { + "x": 10.009926458254693, + "y": 7.661501049972133 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.4691442945593995, + "y": 5.527389345566705 + }, + "prevControl": { + "x": 5.945808174080279, + "y": 6.5485503224058625 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ 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 500e5429..ecad226e 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.SIM; + public static final Mode currentMode = Mode.REAL; public enum Mode { /** Running on a real robot. */ @@ -87,35 +87,35 @@ private DriveConstants() { public static final Transform3d LEFT_CAMERA_TRANSFORMATION = new Transform3d( new Translation3d( Units.inchesToMeters(11.0351), // 11.0351 - Units.inchesToMeters(10.023204), // 10.023204 - Units.inchesToMeters(7.1374)), // 4.1374 + Units.inchesToMeters(10.023204 - 2.0), // 10.023204 + Units.inchesToMeters(7.1374 - 3.5)), // 4.1374 new Rotation3d( - Units.degreesToRadians(0.0), - Units.degreesToRadians(-30.0), // -120.0 + 91.0 - Units.degreesToRadians(-14.7)) // 165.3224 + 180 + Units.degreesToRadians(0.0 + 4.0), + Units.degreesToRadians(-30.0 - 1.0), // -120.0 + 91.0 + Units.degreesToRadians(-14.7 - 6.5)) // 165.3224 + 180 ); public static final Transform3d RIGHT_CAMERA_TRANSFORMATION = new Transform3d( new Translation3d( Units.inchesToMeters(11.0351), //11.0351 Units.inchesToMeters(-10.023204), //-10.023204 - Units.inchesToMeters(7.1374)), // 7.1374 + Units.inchesToMeters(7.1374 - 3.5)), // 7.1374 new Rotation3d( - Units.degreesToRadians(0.0), - Units.degreesToRadians(-30.0), // -30.0 - 1 - Units.degreesToRadians(14.7)) // 165.3224) + Units.degreesToRadians(0.0 + 7.0), + Units.degreesToRadians(-30.0 + 0.85), // -30.0 - 1 + Units.degreesToRadians(14.7 - 12.5)) // 165.3224) ); public static final PathConstraints DEFAULT_CONSTRAINTS = new PathConstraints( - MAX_LINEAR_SPEED * 0.25, - MAX_LINEAR_SPEED * 0.25, - MAX_ANGULAR_SPEED * 0.25, - MAX_ANGULAR_SPEED * 0.25 + MAX_LINEAR_SPEED, + MAX_LINEAR_SPEED * 0.5, + MAX_ANGULAR_SPEED, + MAX_ANGULAR_SPEED * 0.5 ); public static final HolonomicPathFollowerConfig HOLONOMIC_CONFIG = new HolonomicPathFollowerConfig( - new PIDConstants(7.5, 0.75), new PIDConstants(7.5, 0.75), - DriveConstants.MAX_LINEAR_SPEED * 0.25, DriveConstants.DRIVE_BASE_RADIUS, new ReplanningConfig()); + new PIDConstants(4.5, 0.75), new PIDConstants(4.5, 0.75), + DriveConstants.MAX_LINEAR_SPEED * 0.5, DriveConstants.DRIVE_BASE_RADIUS, new ReplanningConfig()); public static final ModuleConstants FL_MOD_CONSTANTS = new ModuleConstants( 0, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3f35eda3..63182ded 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -58,7 +58,7 @@ public void robotInit() { // DriverStation.startDataLog(DataLogManager.getLog()); SignalLogger.setPath(logPath); - SignalLogger.start(); +// SignalLogger.start(); // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d216dec6..afdd483c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ScheduleCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -183,8 +184,7 @@ 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()) - .debounce(0.1, Debouncer.DebounceType.kBoth); + .and(controller.b().negate()); Trigger spinUpTrigger = controller.x().and(controller.y().negate()) .and(controller.a().negate()) // make sure we don't amp @@ -192,8 +192,7 @@ private void configureButtonBindings() { Trigger shootTrigger = controller.x().and(controller.y()) .and(controller.a().negate()) // make sure we don't amp - .and(controller.b().negate()) - .debounce(0.1, Debouncer.DebounceType.kBoth); + .and(controller.b().negate()); Trigger ampLineupTrigger = controller.b().and(controller.a().negate()) .debounce(0.1, Debouncer.DebounceType.kBoth); @@ -225,12 +224,16 @@ private void configureButtonBindings() { () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) ))); - ampLineupTrigger.whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)); + ampLineupTrigger.whileTrue(new ScheduleCommand(m_driveSubsystem.pathfollowFactory(FieldConstants.AMP_LINEUP)) + .andThen(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP))); ampDepositeTrigger.whileTrue(Commands.runEnd(() -> m_shooter.setKickerPower(-0.5), () -> m_shooter.setKickerPower(0.0), m_shooter) .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP))); + controller.rightBumper().whileTrue(m_climber.setClimberPowerFactory(0.25)); + controller.leftBumper().whileTrue(m_climber.setClimberPowerFactory(-0.25)); + controller.pov(0).onTrue(Commands.runOnce(SignalLogger::stop)); controller.pov(180).whileTrue(m_driveSubsystem.pathfollowFactory( FieldConstants.AMP_LINEUP @@ -258,14 +261,14 @@ private void configureButtonBindings() { * Use this method to configure any named commands needed for PathPlanner autos */ private void configureNamedCommands() { - NamedCommands.registerCommand("Run Intake", m_shooter.intakeCommand(0.75, 0.5, 0.1) + NamedCommands.registerCommand("Intake", m_shooter.intakeCommand(0.75, 0.5, 0.1) .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.INTAKE))); NamedCommands.registerCommand("AimAndShoot", new ShooterAutoCommand(m_armSubsystem, m_shooter) .raceWith(DriveCommands.alignmentDrive( m_driveSubsystem, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), + () -> 0.0, + () -> 0.0, () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) ))); } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 11ec5791..b4ae68db 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -42,7 +42,7 @@ public static Command joystickDrive( () -> { double xInput = setSensitivity(xSupplier.getAsDouble(), 0.25); double yInput = setSensitivity(ySupplier.getAsDouble(), 0.25); - double omegaInput = setSensitivity(omegaSupplier.getAsDouble(), 0.0); + double omegaInput = setSensitivity(omegaSupplier.getAsDouble(), 0.0) * 0.85; // Apply deadband double linearMagnitude = diff --git a/src/main/java/frc/robot/commands/auto/AutoFactory.java b/src/main/java/frc/robot/commands/auto/AutoFactory.java new file mode 100644 index 00000000..5cc21fc9 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AutoFactory.java @@ -0,0 +1,14 @@ +package frc.robot.commands.auto; + +public class AutoFactory { + public enum AUTOS { + ONE_PIECE_AMP_START("OneAmpStart"), + THREE_PIECE_CENTER_START("ThreeCenterStart"); + + AUTOS(String name) { + this.name = name; + } + + public final String name; + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java b/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java index 5176a41a..343a5b1d 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java @@ -21,6 +21,7 @@ import lib.properties.phoenix6.PidPropertyPublic; import frc.robot.Constants.ArmConstants; +import org.littletonrobotics.junction.Logger; public class ArmIOKraken implements ArmIO { // Physical devices @@ -84,7 +85,7 @@ public ArmIOKraken() { armConfig.Feedback.SensorToMechanismRatio = ArmConstants.ARM_SENSOR_MECHANISM_RATIO; m_armMaster = TalonFXFactory.createTalon(ArmConstants.ARM_MASTER_ID, armConfig); - m_armFollower = TalonFXFactory.createTalon(ArmConstants.ARM_MASTER_ID, armConfig); + m_armFollower = TalonFXFactory.createTalon(ArmConstants.ARM_FOLLOWER_ID, armConfig); // Wrist Configuration TalonFXConfiguration wristConfig = new TalonFXConfiguration(); @@ -242,6 +243,9 @@ public void updateInputs(ArmIOInputsAutoLogged inputs) { m_armMaxVelDegS.updateIfChanged(); m_wristMaxVelDegS.updateIfChanged(); m_accelTimeSecs.updateIfChanged(); + + Logger.recordOutput("Arm Prev Vel Mult", m_prevArmVelocityMult); + Logger.recordOutput("Wrist Prev Vel Mult", m_prevWristVelocityMult); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 1405fb6a..4600f977 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -145,8 +145,8 @@ public DriveSubsystem( Units.inchesToMeters(0.5), Units.degreesToRadians(0.75)), VecBuilder.fill( - Units.inchesToMeters(4.0), - Units.inchesToMeters(4.0), + Units.inchesToMeters(2.0), + Units.inchesToMeters(2.0), Units.degreesToRadians(25.0)) ); @@ -239,9 +239,10 @@ public void periodic() { } // 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))) { + if ((kinematics.toChassisSpeeds(getModuleStates()).vxMetersPerSecond <= Units.inchesToMeters(30)) + && (kinematics.toChassisSpeeds(getModuleStates()).vyMetersPerSecond <= Units.inchesToMeters(30)) + && (kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond <= Units.degreesToRadians(60)) + || DriverStation.isTeleop()) { for (VisionSubsystem camera : m_cameras) { camera.updateInputs(); camera.getPose(m_wpiPoseEstimator.getEstimatedPosition()).ifPresent( @@ -255,12 +256,6 @@ public void periodic() { 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)); } /** @@ -352,7 +347,7 @@ private SwerveModulePosition[] getModulePositions() { } /** Returns the robot pose with vision updates */ - @AutoLogOutput(key = "Odometry/RobotVision") + @AutoLogOutput(key = "Odometry/RobotPose") public Pose2d getVisionPose() { return m_wpiPoseEstimator.getEstimatedPosition(); } @@ -380,7 +375,7 @@ public double getMaxAngularSpeedRadPerSec() { public Command pathfollowFactory(Pose2d pose) { return AutoBuilder.pathfindToPoseFlipped( - pose, DriveConstants.DEFAULT_CONSTRAINTS); + pose, DriveConstants.DEFAULT_CONSTRAINTS).withInterruptBehavior(Command.InterruptionBehavior.kCancelSelf); } /** Returns an array of module translations. */ diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 49890e33..5c7e9b95 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -27,8 +27,8 @@ public class ShooterSubsystem extends SubsystemBase { private final DataLogTable m_logTable = DataLogUtil.getTable("Shooter"); - private double m_leftSpeedSetpoint = 0.0; - private double m_rightSpeedSetpoint = 0.0; + private double m_leftSpeedSetpoint = 3800.0; + private double m_rightSpeedSetpoint = 3800.0; private final GosDoubleProperty m_leftPower = new GosDoubleProperty(false, "Shooter/Left RPM", 3600); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 3c0aaede..dd6dc86b 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import lib.logger.DataLogUtil; +import lib.utils.FieldConstants; import lib.utils.PoseEstimator; import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; @@ -32,11 +33,11 @@ public class VisionSubsystem { private AprilTagFieldLayout m_aprilTagFieldLayout; private final String m_name; - private final double xyStdDevCoefficient = Units.inchesToMeters(8.0); - private final double thetaStdDevCoefficient = Units.degreesToRadians(24.0); + private final double xyStdDevCoefficient = Units.inchesToMeters(4.0); + private final double thetaStdDevCoefficient = Units.degreesToRadians(12.0); - private final double xyStdDevMultiTagCoefficient = Units.inchesToMeters(4.0); - private final double thetaStdDevMultiTagCoefficient = Units.degreesToRadians(12.0); + private final double xyStdDevMultiTagCoefficient = Units.inchesToMeters(2.0); + private final double thetaStdDevMultiTagCoefficient = Units.degreesToRadians(6.0); private final PhotonVisionIOInputsAutoLogged inputs = new PhotonVisionIOInputsAutoLogged(); @@ -47,12 +48,13 @@ public VisionSubsystem(String camName, Transform3d camPose) { try { m_aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); m_photonPoseEstimator = new PhotonPoseEstimator( - m_aprilTagFieldLayout, + FieldConstants.APRIL_TAG_FIELD_LAYOUT, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, m_camera, robotToCam); - m_photonPoseEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE); + m_photonPoseEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY); + } catch (IOException e){ throw new IllegalStateException(e); } @@ -103,6 +105,11 @@ public Optional getPose(Pose2d prevEstima double xyStdDev = xyStdDevCoefficient * Math.pow(avgDist, 2.0); double thetaStdDev = thetaStdDevCoefficient * Math.pow(avgDist, 2.0); + if (camResult.getTargets().size() > 1.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, diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index b36cc3ed..26ac4737 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -14,24 +14,25 @@ public class AimbotUtils { static { // angle measurements, meters -> degrees - m_angleLerpTable.put(Units.inchesToMeters(18.0), 57.0); - m_angleLerpTable.put(Units.inchesToMeters(46.0), 45.0); - m_angleLerpTable.put(Units.inchesToMeters(97.0), 33.5); - m_angleLerpTable.put(Units.inchesToMeters(116.0), 29.75); - m_angleLerpTable.put(Units.inchesToMeters(155.0), 24.65); + m_angleLerpTable.put(Units.inchesToMeters(18.0), 57.0 + 3.0); + m_angleLerpTable.put(Units.inchesToMeters(46.0), 45.0 + 3.0); + m_angleLerpTable.put(Units.inchesToMeters(97.0), 33.5 + 2.5); + m_angleLerpTable.put(Units.inchesToMeters(116.0), 29.75 + 2.5); + m_angleLerpTable.put(Units.inchesToMeters(155.0), 24.65 + 1.5); + // m_angleLerpTable.put(Units.inchesToMeters(229.0), 0.0); //flywheel measurements, meters -> RPM m_leftSpeedLerpTable.put(Units.inchesToMeters(18.0), 3600.0); - m_leftSpeedLerpTable.put(Units.inchesToMeters(46.0), 3600.0); - m_leftSpeedLerpTable.put(Units.inchesToMeters(97.0), 3600.0); - m_leftSpeedLerpTable.put(Units.inchesToMeters(116.0), 3800.0); + m_leftSpeedLerpTable.put(Units.inchesToMeters(46.0), 3700.0); + m_leftSpeedLerpTable.put(Units.inchesToMeters(97.0), 3800.0); + m_leftSpeedLerpTable.put(Units.inchesToMeters(116.0), 4000.0); m_leftSpeedLerpTable.put(Units.inchesToMeters(155.0), 5000.0); // m_leftSpeedLerpTable.put(Units.inchesToMeters(229.0), 0.0); m_rightSpeedLerpTable.put(Units.inchesToMeters(18.0), 3600.0); m_rightSpeedLerpTable.put(Units.inchesToMeters(46.0), 3600.0); - m_rightSpeedLerpTable.put(Units.inchesToMeters(97.0), 3600.0); + m_rightSpeedLerpTable.put(Units.inchesToMeters(97.0), 3700.0); m_rightSpeedLerpTable.put(Units.inchesToMeters(116.0), 3800.0); m_rightSpeedLerpTable.put(Units.inchesToMeters(155.0), 4600.0); // m_rightSpeedLerpTable.put(Units.inchesToMeters(229.0), 0.0); diff --git a/src/main/java/lib/utils/FieldConstants.java b/src/main/java/lib/utils/FieldConstants.java index 4bee1313..6a552e45 100644 --- a/src/main/java/lib/utils/FieldConstants.java +++ b/src/main/java/lib/utils/FieldConstants.java @@ -32,7 +32,7 @@ private FieldConstants() {} public static final Pose2d AMP_LINEUP = AllianceFlipUtil.apply(new Pose2d( AMP_CENTER.getX(), - AMP_CENTER.getY() - Units.inchesToMeters(20), + AMP_CENTER.getY() - Units.inchesToMeters(10), Rotation2d.fromDegrees(90) ));