diff --git a/Robot/Chreo_Auto_Project.chor b/Robot/Chreo_Auto_Project.chor index 14a03bc..f8bcb1e 100644 --- a/Robot/Chreo_Auto_Project.chor +++ b/Robot/Chreo_Auto_Project.chor @@ -13752,6 +13752,2896 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], + "isTrajectoryStale": true + }, + "New Path (1)": { + "waypoints": [ + { + "x": 1.373390793800354, + "y": 5.5500569343566895, + "heading": -3.14, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 2.4236249923706055, + "y": 5.564249038696289, + "heading": -3.135640447743494, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 27 + }, + { + "x": 3.644167423248291, + "y": 5.975827217102051, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": false, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 5.446596145629883, + "y": 6.288059234619141, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 32 + }, + { + "x": 7.759950160980225, + "y": 5.919057846069336, + "heading": -3.4633432079864352, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 5.262095928192139, + "y": 6.671252727508545, + "heading": -2.9346533643249577, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 7.774142265319824, + "y": 7.4376397132873535, + "heading": -2.9551058096712683, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 4.907287120819092, + "y": 7.324100971221924, + "heading": -2.8357137696574513, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.373390793800354, + "y": 5.5500569343566895, + "heading": -3.14, + "angularVelocity": -7.792493373112414e-25, + "velocityX": -1.4675606988248466e-25, + "velocityY": 4.296548304526963e-25, + "timestamp": 0 + }, + { + "x": 1.4008162818871832, + "y": 5.548332467221767, + "heading": -3.137847865481668, + "angularVelocity": 0.027272567199688357, + "velocityX": 0.34754494222415555, + "velocityY": -0.021853023321830287, + "timestamp": 0.07891206216760503 + }, + { + "x": 1.4556672566846394, + "y": 5.544883518010237, + "heading": -3.133543459938688, + "angularVelocity": 0.054546864252994244, + "velocityX": 0.6950898670086171, + "velocityY": -0.04370623598969305, + "timestamp": 0.15782412433521006 + }, + { + "x": 1.5379437150886541, + "y": 5.539710064387518, + "heading": -3.127086401796681, + "angularVelocity": 0.08182599674422604, + "velocityX": 1.0426347524572848, + "velocityY": -0.06555973168882138, + "timestamp": 0.23673618650281508 + }, + { + "x": 1.6476456518138687, + "y": 5.532812078143502, + "heading": -3.1184760134609104, + "angularVelocity": 0.10911371594221948, + "velocityX": 1.3901795709281257, + "velocityY": -0.08741358487583052, + "timestamp": 0.31564824867042013 + }, + { + "x": 1.7847730583095482, + "y": 5.524189529642618, + "heading": -3.10771125358726, + "angularVelocity": 0.13641463140053903, + "velocityX": 1.7377242810411937, + "velocityY": -0.10926781361474407, + "timestamp": 0.3945603108380252 + }, + { + "x": 1.949325918220542, + "y": 5.513842409726839, + "heading": -3.0947906414967634, + "angularVelocity": 0.16373431051711962, + "velocityX": 2.0852687838963337, + "velocityY": -0.1311221584071907, + "timestamp": 0.4734723730056302 + }, + { + "x": 2.1090388863997416, + "y": 5.523857134967481, + "heading": -3.1017736522087302, + "angularVelocity": -0.08849104332281946, + "velocityX": 2.0239360598634204, + "velocityY": 0.1269099420994908, + "timestamp": 0.5523844351732352 + }, + { + "x": 2.2413264008507308, + "y": 5.535596537344446, + "heading": -3.110910463319114, + "angularVelocity": -0.11578472111117445, + "velocityX": 1.6763915530431484, + "velocityY": 0.14876562662918416, + "timestamp": 0.6312964973408403 + }, + { + "x": 2.3461884403926034, + "y": 5.54906052840806, + "heading": -3.122199828492689, + "angularVelocity": -0.14306260492340278, + "velocityX": 1.3288467778113737, + "velocityY": 0.17062019029507766, + "timestamp": 0.7102085595084453 + }, + { + "x": 2.4236249923706055, + "y": 5.564249038696289, + "heading": -3.135640447743494, + "angularVelocity": -0.1703240148794674, + "velocityX": 0.9813018422143182, + "velocityY": 0.1924738737148937, + "timestamp": 0.7891206216760503 + }, + { + "x": 2.4236249924844095, + "y": 5.564249038718611, + "heading": -3.135640447763247, + "angularVelocity": -0.17032401487979337, + "velocityX": 0.9813018422127583, + "velocityY": 0.19247387371505806, + "timestamp": 0.7891206217920228 + }, + { + "x": 2.4236249925982136, + "y": 5.5642490387409325, + "heading": -3.135640447783, + "angularVelocity": -0.17032401488011933, + "velocityX": 0.9813018422113264, + "velocityY": 0.19247387371524752, + "timestamp": 0.7891206219079953 + }, + { + "x": 2.4236249927120177, + "y": 5.564249038763254, + "heading": -3.135640447802753, + "angularVelocity": -0.17032401488044527, + "velocityX": 0.9813018422100225, + "velocityY": 0.19247387371546204, + "timestamp": 0.7891206220239677 + }, + { + "x": 2.4236249928258218, + "y": 5.564249038785576, + "heading": -3.135640447822506, + "angularVelocity": -0.17032401488077123, + "velocityX": 0.9813018422088465, + "velocityY": 0.19247387371570168, + "timestamp": 0.7891206221399402 + }, + { + "x": 2.423624992939626, + "y": 5.564249038807898, + "heading": -3.1356404478422593, + "angularVelocity": -0.1703240148810972, + "velocityX": 0.9813018422077986, + "velocityY": 0.19247387371596644, + "timestamp": 0.7891206222559126 + }, + { + "x": 2.42362499305343, + "y": 5.564249038830219, + "heading": -3.1356404478620123, + "angularVelocity": -0.17032401488142315, + "velocityX": 0.9813018422068785, + "velocityY": 0.1924738737162563, + "timestamp": 0.7891206223718851 + }, + { + "x": 2.423624993167234, + "y": 5.564249038852541, + "heading": -3.1356404478817654, + "angularVelocity": -0.17032401488174911, + "velocityX": 0.9813018422060865, + "velocityY": 0.19247387371657124, + "timestamp": 0.7891206224878575 + }, + { + "x": 2.423624993281038, + "y": 5.564249038874863, + "heading": -3.1356404479015185, + "angularVelocity": -0.17032401488207508, + "velocityX": 0.9813018422054224, + "velocityY": 0.19247387371691124, + "timestamp": 0.78912062260383 + }, + { + "x": 2.423624993394842, + "y": 5.564249038897184, + "heading": -3.1356404479212716, + "angularVelocity": -0.170324014882401, + "velocityX": 0.9813018422048863, + "velocityY": 0.1924738737172764, + "timestamp": 0.7891206227198024 + }, + { + "x": 2.4236249935086462, + "y": 5.564249038919506, + "heading": -3.1356404479410247, + "angularVelocity": -0.17032401488272697, + "velocityX": 0.981301842204478, + "velocityY": 0.19247387371766664, + "timestamp": 0.7891206228357749 + }, + { + "x": 2.4236249936224503, + "y": 5.564249038941828, + "heading": -3.135640447960778, + "angularVelocity": -0.17032401488305293, + "velocityX": 0.9813018422041978, + "velocityY": 0.19247387371808197, + "timestamp": 0.7891206229517473 + }, + { + "x": 2.4236249937362544, + "y": 5.5642490389641495, + "heading": -3.135640447980531, + "angularVelocity": -0.1703240148833789, + "velocityX": 0.9813018422040455, + "velocityY": 0.1924738737185224, + "timestamp": 0.7891206230677198 + }, + { + "x": 2.4236249938500585, + "y": 5.564249038986471, + "heading": -3.135640448000284, + "angularVelocity": -0.17032401488370486, + "velocityX": 0.9813018422040212, + "velocityY": 0.19247387371898791, + "timestamp": 0.7891206231836922 + }, + { + "x": 2.4236249939638626, + "y": 5.564249039008793, + "heading": -3.135640448020037, + "angularVelocity": -0.1703240148840308, + "velocityX": 0.9813018422041248, + "velocityY": 0.19247387371947852, + "timestamp": 0.7891206232996647 + }, + { + "x": 2.4236249940776666, + "y": 5.564249039031115, + "heading": -3.13564044803979, + "angularVelocity": -0.17032401488435675, + "velocityX": 0.9813018422043563, + "velocityY": 0.19247387371999425, + "timestamp": 0.7891206234156372 + }, + { + "x": 2.4236249941914707, + "y": 5.564249039053436, + "heading": -3.1356404480595432, + "angularVelocity": -0.1703240148846827, + "velocityX": 0.9813018422047158, + "velocityY": 0.19247387372053507, + "timestamp": 0.7891206235316096 + }, + { + "x": 2.423624994305275, + "y": 5.564249039075758, + "heading": -3.1356404480792963, + "angularVelocity": -0.17032401488500867, + "velocityX": 0.9813018422052033, + "velocityY": 0.19247387372110097, + "timestamp": 0.7891206236475821 + }, + { + "x": 2.423624994419079, + "y": 5.56424903909808, + "heading": -3.1356404480990494, + "angularVelocity": -0.17032401488533463, + "velocityX": 0.9813018422058187, + "velocityY": 0.192473873721692, + "timestamp": 0.7891206237635545 + }, + { + "x": 2.423624994532883, + "y": 5.564249039120401, + "heading": -3.1356404481188025, + "angularVelocity": -0.1703240148856606, + "velocityX": 0.9813018422065621, + "velocityY": 0.19247387372230812, + "timestamp": 0.789120623879527 + }, + { + "x": 2.423624994646687, + "y": 5.564249039142723, + "heading": -3.1356404481385556, + "angularVelocity": -0.17032401488598653, + "velocityX": 0.9813018422074334, + "velocityY": 0.19247387372294933, + "timestamp": 0.7891206239954994 + }, + { + "x": 2.423624994760491, + "y": 5.564249039165045, + "heading": -3.1356404481583087, + "angularVelocity": -0.1703240148863125, + "velocityX": 0.9813018422084328, + "velocityY": 0.19247387372361563, + "timestamp": 0.7891206241114719 + }, + { + "x": 2.423624994874295, + "y": 5.5642490391873665, + "heading": -3.1356404481780618, + "angularVelocity": -0.17032401488663845, + "velocityX": 0.98130184220956, + "velocityY": 0.192473873724307, + "timestamp": 0.7891206242274443 + }, + { + "x": 2.4236249949880992, + "y": 5.564249039209688, + "heading": -3.135640448197815, + "angularVelocity": -0.17032401488696441, + "velocityX": 0.9813018422108152, + "velocityY": 0.1924738737250235, + "timestamp": 0.7891206243434168 + }, + { + "x": 2.4236249951019033, + "y": 5.56424903923201, + "heading": -3.135640448217568, + "angularVelocity": -0.17032401488729038, + "velocityX": 0.9813018422121984, + "velocityY": 0.1924738737257651, + "timestamp": 0.7891206244593892 + }, + { + "x": 2.4236249952157074, + "y": 5.564249039254332, + "heading": -3.135640448237321, + "angularVelocity": -0.1703240148876163, + "velocityX": 0.9813018422137095, + "velocityY": 0.1924738737265318, + "timestamp": 0.7891206245753617 + }, + { + "x": 2.4236249953295115, + "y": 5.564249039276653, + "heading": -3.135640448257074, + "angularVelocity": -0.17032401488794227, + "velocityX": 0.9813018422153487, + "velocityY": 0.19247387372732358, + "timestamp": 0.7891206246913341 + }, + { + "x": 2.4236249954433156, + "y": 5.564249039298975, + "heading": -3.135640448276827, + "angularVelocity": -0.17032401488826823, + "velocityX": 0.9813018422171157, + "velocityY": 0.19247387372814045, + "timestamp": 0.7891206248073066 + }, + { + "x": 2.5202131403633516, + "y": 5.5885147910320585, + "heading": -3.150117025303126, + "angularVelocity": -0.19273984769703859, + "velocityX": 1.2859658956262894, + "velocityY": 0.3230720414634121, + "timestamp": 0.8642300388660642 + }, + { + "x": 2.6397927701444233, + "y": 5.62233325234703, + "heading": -3.166259274426774, + "angularVelocity": -0.21491645655789715, + "velocityX": 1.5920724622818327, + "velocityY": 0.4502559597724382, + "timestamp": 0.9393394529248218 + }, + { + "x": 2.782492954308911, + "y": 5.665387996339145, + "heading": -3.184044486118167, + "angularVelocity": -0.23679071277909178, + "velocityX": 1.8998974489782852, + "velocityY": 0.5732269986613667, + "timestamp": 1.0144488669835794 + }, + { + "x": 2.948469657430915, + "y": 5.717279062041011, + "heading": -3.2034430209640345, + "angularVelocity": -0.25827035251122976, + "velocityX": 2.2097989340212596, + "velocityY": 0.6908729931147001, + "timestamp": 1.089558281042337 + }, + { + "x": 3.13791427133203, + "y": 5.777485440384115, + "heading": -3.2244148248769338, + "angularVelocity": -0.27921671571679574, + "velocityX": 2.5222485926053704, + "velocityY": 0.8015823195745534, + "timestamp": 1.1646676951010946 + }, + { + "x": 3.3510652321882093, + "y": 5.845301698736286, + "heading": -3.2469035661389745, + "angularVelocity": -0.29941308348441903, + "velocityX": 2.837872769043742, + "velocityY": 0.9028995792607077, + "timestamp": 1.2397771091598522 + }, + { + "x": 3.5882224140431322, + "y": 5.919722646911708, + "heading": -3.2708260554869257, + "angularVelocity": -0.3185018768650853, + "velocityX": 3.157489441595122, + "velocityY": 0.9908338269980898, + "timestamp": 1.3148865232186098 + }, + { + "x": 3.849756972147724, + "y": 5.999212365002834, + "heading": -3.296051340937575, + "angularVelocity": -0.33584718729021185, + "velocityX": 3.482047641857447, + "velocityY": 1.0583189748883177, + "timestamp": 1.3899959372773674 + }, + { + "x": 4.136067397800634, + "y": 6.081181645403884, + "heading": -3.322354225031387, + "angularVelocity": -0.3501942389436787, + "velocityX": 3.811911319517566, + "velocityY": 1.091331645017572, + "timestamp": 1.465105351336125 + }, + { + "x": 4.447151871291085, + "y": 6.160640800595462, + "heading": -3.34929544790659, + "angularVelocity": -0.3586930242076843, + "velocityX": 4.141750769711674, + "velocityY": 1.0579120631858112, + "timestamp": 1.5402147653948826 + }, + { + "x": 4.779482954645891, + "y": 6.227113094753417, + "heading": -3.37586605695639, + "angularVelocity": -0.3537587049875555, + "velocityX": 4.424626227210678, + "velocityY": 0.8850061605587058, + "timestamp": 1.6153241794536402 + }, + { + "x": 5.117259626240433, + "y": 6.269293967456475, + "heading": -3.40016260033537, + "angularVelocity": -0.32348199867427174, + "velocityX": 4.497128300459125, + "velocityY": 0.5615923547221425, + "timestamp": 1.6904335935123977 + }, + { + "x": 5.446596145629883, + "y": 6.288059234619141, + "heading": -3.421549469666226, + "angularVelocity": -0.2847428594520122, + "velocityX": 4.384756871246697, + "velocityY": 0.24983908339353045, + "timestamp": 1.7655430075711553 + }, + { + "x": 5.583978781229968, + "y": 6.292316694982243, + "heading": -3.430131520120695, + "angularVelocity": -0.2689620281221212, + "velocityX": 4.305580874384169, + "velocityY": 0.1334290890020681, + "timestamp": 1.7974510449784737 + }, + { + "x": 5.718326508347156, + "y": 6.293262578004624, + "heading": -3.4381858559860787, + "angularVelocity": -0.25242341804249496, + "velocityX": 4.210466642062215, + "velocityY": 0.02964403640080957, + "timestamp": 1.8293590823857921 + }, + { + "x": 5.849241399946558, + "y": 6.291311612489797, + "heading": -3.4457039588897924, + "angularVelocity": -0.23561784160343793, + "velocityX": 4.102881350182191, + "velocityY": -0.061143388103806366, + "timestamp": 1.8612671197931105 + }, + { + "x": 5.976424364300466, + "y": 6.28686095808839, + "heading": -3.452687518488142, + "angularVelocity": -0.21886521910455456, + "velocityX": 3.9859225037995363, + "velocityY": -0.13948380292379126, + "timestamp": 1.893175157200429 + }, + { + "x": 6.0996561002514555, + "y": 6.280274035067278, + "heading": -3.459143915120258, + "angularVelocity": -0.20234389692156543, + "velocityX": 3.8620907446575, + "velocityY": -0.20643460257448376, + "timestamp": 1.9250831946077473 + }, + { + "x": 6.21877796918791, + "y": 6.271874588037581, + "heading": -3.4650831978385432, + "angularVelocity": -0.18613751270466306, + "velocityX": 3.7332872409486546, + "velocityY": -0.26323922472808714, + "timestamp": 1.9569912320150658 + }, + { + "x": 6.33367594677285, + "y": 6.2619468977702875, + "heading": -3.4705163067965152, + "angularVelocity": -0.1702739936215994, + "velocityX": 3.6009102069871273, + "velocityY": -0.3111344687409986, + "timestamp": 1.9888992694223842 + }, + { + "x": 6.444268295780197, + "y": 6.250738999348943, + "heading": -3.475454120537259, + "angularVelocity": -0.15475140879743604, + "velocityX": 3.465971522961229, + "velocityY": -0.3512562768518353, + "timestamp": 2.0208073068297026 + }, + { + "x": 6.5504965143251574, + "y": 6.238466994593223, + "heading": -3.4799069914164837, + "angularVelocity": -0.13955326748499272, + "velocityX": 3.3291993860015867, + "velocityY": -0.3846054396597071, + "timestamp": 2.052715344237021 + }, + { + "x": 6.6523188314701445, + "y": 6.225319459420252, + "heading": -3.483884551234072, + "angularVelocity": -0.1246569874170926, + "velocityX": 3.191118145098868, + "velocityY": -0.4120446207686626, + "timestamp": 2.08462338164434 + }, + { + "x": 6.749705571819693, + "y": 6.211461495939882, + "heading": -3.487395659176523, + "angularVelocity": -0.11003835483926433, + "velocityX": 3.0521068753420764, + "velocityY": -0.4343094908492185, + "timestamp": 2.1165314190516584 + }, + { + "x": 6.842635853833305, + "y": 6.197038271774381, + "heading": -3.4904484207431485, + "angularVelocity": -0.0956737491452672, + "velocityX": 2.912441176726728, + "velocityY": -0.4520247980589766, + "timestamp": 2.148439456458977 + }, + { + "x": 6.931095228941555, + "y": 6.182178030589835, + "heading": -3.4930502391028706, + "angularVelocity": -0.08154115925428285, + "velocityX": 2.7723226589912517, + "velocityY": -0.4657209403025567, + "timestamp": 2.1803474938662957 + }, + { + "x": 7.015073982675889, + "y": 6.1669946184502, + "heading": -3.4952078785818306, + "angularVelocity": -0.06762056379139442, + "velocityX": 2.6318996891696274, + "velocityY": -0.4758491393818042, + "timestamp": 2.2122555312736143 + }, + { + "x": 7.0945659029317385, + "y": 6.151589590913828, + "heading": -3.4969275299383775, + "angularVelocity": -0.05389398710408752, + "velocityX": 2.4912820315804582, + "velocityY": -0.4827945805541645, + "timestamp": 2.244163568680933 + }, + { + "x": 7.169567379757041, + "y": 6.136053967419142, + "heading": -3.498214872438713, + "angularVelocity": -0.040345398994690515, + "velocityX": 2.350551237855201, + "velocityY": -0.4868874665140758, + "timestamp": 2.2760716060882515 + }, + { + "x": 7.240076742171475, + "y": 6.120469693395241, + "heading": -3.499075130588546, + "angularVelocity": -0.026960547239283258, + "velocityX": 2.2097680754962985, + "velocityY": -0.48841217731324, + "timestamp": 2.30797964349557 + }, + { + "x": 7.306093765850792, + "y": 6.104910861957847, + "heading": -3.4995131248526885, + "angularVelocity": -0.013726769169506972, + "velocityX": 2.068977882800654, + "velocityY": -0.48761480497154164, + "timestamp": 2.339887680902889 + }, + { + "x": 7.367619305030031, + "y": 6.089444738394438, + "heading": -3.4995333164334084, + "angularVelocity": -0.000632805473504129, + "velocityX": 1.9282144618875081, + "velocityY": -0.48470933407710726, + "timestamp": 2.3717957183102074 + }, + { + "x": 7.424655015480076, + "y": 6.074132622864405, + "heading": -3.4991398465215826, + "angularVelocity": 0.012331373026894028, + "velocityX": 1.787502932943278, + "velocityY": -0.4798827121382698, + "timestamp": 2.403703755717526 + }, + { + "x": 7.477203144806779, + "y": 6.059030580127337, + "heading": -3.498336570568695, + "angularVelocity": 0.025174721423108552, + "velocityX": 1.6468618441148708, + "velocityY": -0.473299016930574, + "timestamp": 2.4356117931248447 + }, + { + "x": 7.525266372907712, + "y": 6.044190059652854, + "heading": -3.4971270881539924, + "angularVelocity": 0.03790525876796509, + "velocityX": 1.5063047434534294, + "velocityY": -0.46510289194657173, + "timestamp": 2.4675198305321633 + }, + { + "x": 7.568847690075842, + "y": 6.029658425029338, + "heading": -3.4955147689946737, + "angularVelocity": 0.05053018895323732, + "velocityX": 1.365841358771074, + "velocityY": -0.4554223889740649, + "timestamp": 2.499427867939482 + }, + { + "x": 7.607950303556081, + "y": 6.015479408015207, + "heading": -3.4935027755974737, + "angularVelocity": 0.0630560059685356, + "velocityX": 1.225478489356149, + "velocityY": -0.44437132980415467, + "timestamp": 2.5313359053468005 + }, + { + "x": 7.642577565744241, + "y": 6.001693499708621, + "heading": -3.4910940829929844, + "angularVelocity": 0.07548858532857207, + "velocityX": 1.0852206842473227, + "velocityY": -0.43205127694331946, + "timestamp": 2.563243942754119 + }, + { + "x": 7.672732918945103, + "y": 5.988338289012847, + "heading": -3.4882914959374425, + "angularVelocity": 0.08783326344287469, + "velocityX": 0.9450707611977816, + "velocityY": -0.41855318537111263, + "timestamp": 2.5951519801614378 + }, + { + "x": 7.698419852868165, + "y": 5.975448756730407, + "heading": -3.4850976639142544, + "angularVelocity": 0.10009490657219974, + "velocityX": 0.8050302058744196, + "velocityY": -0.40395879313730104, + "timestamp": 2.6270600175687564 + }, + { + "x": 7.71964187196861, + "y": 5.963057532136706, + "heading": -3.4815150942206845, + "angularVelocity": 0.11227797083967467, + "velocityX": 0.6650994804079722, + "velocityY": -0.38834179725697887, + "timestamp": 2.658968054976075 + }, + { + "x": 7.736402470430254, + "y": 5.951195117688826, + "heading": -3.477546163384214, + "angularVelocity": 0.12438655457888602, + "velocityX": 0.5252782628930857, + "velocityY": -0.3717688523569133, + "timestamp": 2.6908760923833936 + }, + { + "x": 7.74870511310215, + "y": 5.939890086558444, + "heading": -3.473193127117835, + "angularVelocity": 0.13642444412392482, + "velocityX": 0.38556563397643184, + "velocityY": -0.3543004223691105, + "timestamp": 2.7227841297907123 + }, + { + "x": 7.756553221087926, + "y": 5.929169256892984, + "heading": -3.4684581289933885, + "angularVelocity": 0.14839515398588157, + "velocityX": 0.24596022267342674, + "velocityY": -0.33599150986958115, + "timestamp": 2.754692167198031 + }, + { + "x": 7.759950160980225, + "y": 5.919057846069336, + "heading": -3.4633432079864352, + "angularVelocity": 0.16030196221909535, + "velocityX": 0.10646032060622376, + "velocityY": -0.31689228311261575, + "timestamp": 2.7866002046053495 + }, + { + "x": 7.740992810070082, + "y": 5.897866293760295, + "heading": -3.4483259795142858, + "angularVelocity": 0.18966965117669482, + "velocityX": -0.23943393689649056, + "velocityY": -0.26765220638433596, + "timestamp": 2.865775910254534 + }, + { + "x": 7.6946750115513405, + "y": 5.880751239353706, + "heading": -3.430977462853066, + "angularVelocity": 0.21911414011373867, + "velocityX": -0.5850001353188997, + "velocityY": -0.2161654798812024, + "timestamp": 2.944951615903719 + }, + { + "x": 7.621031262251048, + "y": 5.867937342014943, + "heading": -3.4112891061492308, + "angularVelocity": 0.2486666401316515, + "velocityX": -0.9301306340937104, + "velocityY": -0.16184127736782786, + "timestamp": 3.0241273215529034 + }, + { + "x": 7.520109336525543, + "y": 5.85971721387783, + "heading": -3.389248427591031, + "angularVelocity": 0.2783767871404717, + "velocityX": -1.2746577362085507, + "velocityY": -0.10382134355121005, + "timestamp": 3.103303027202088 + }, + { + "x": 7.39197917102874, + "y": 5.8564875757937624, + "heading": -3.3648366324448036, + "angularVelocity": 0.308324314208095, + "velocityX": -1.6183015288114875, + "velocityY": -0.04079077107790146, + "timestamp": 3.1824787328512727 + }, + { + "x": 7.236751317240548, + "y": 5.858816377779796, + "heading": -3.338024361207094, + "angularVelocity": 0.3386426558231255, + "velocityX": -1.9605490410907365, + "velocityY": 0.029413087852372996, + "timestamp": 3.2616544385004573 + }, + { + "x": 7.054621391051907, + "y": 5.867582559401864, + "heading": -3.3087631347222772, + "angularVelocity": 0.3695732958095547, + "velocityX": -2.3003258978913266, + "velocityY": 0.11071807381053804, + "timestamp": 3.340830144149642 + }, + { + "x": 6.846004001197015, + "y": 5.884320000838926, + "heading": -3.2769649631981674, + "angularVelocity": 0.40161525891544314, + "velocityX": -2.63486618962696, + "velocityY": 0.21139617638803204, + "timestamp": 3.4200058497988266 + }, + { + "x": 6.612122032954981, + "y": 5.912316015175021, + "heading": -3.242440238754101, + "angularVelocity": 0.43605199550781415, + "velocityX": -2.953961272897621, + "velocityY": 0.3535934931876858, + "timestamp": 3.4991815554480112 + }, + { + "x": 6.360418430236016, + "y": 5.961461610916641, + "heading": -3.204725258846095, + "angularVelocity": 0.4763453587027727, + "velocityX": -3.1790509557846622, + "velocityY": 0.6207156013155894, + "timestamp": 3.578357261097196 + }, + { + "x": 6.128768619413155, + "y": 6.029673284245218, + "heading": -3.1672334777487867, + "angularVelocity": 0.4735263271719758, + "velocityX": -2.9257688191534936, + "velocityY": 0.861522771022888, + "timestamp": 3.6575329667463805 + }, + { + "x": 5.923854263987644, + "y": 6.105004809926703, + "heading": -3.1312739726613037, + "angularVelocity": 0.45417347142839426, + "velocityX": -2.5880963579087672, + "velocityY": 0.9514474808126816, + "timestamp": 3.736708672395565 + }, + { + "x": 5.746388049675664, + "y": 6.183799530006089, + "heading": -3.0971258429522432, + "angularVelocity": 0.4312955524560246, + "velocityX": -2.241422578515493, + "velocityY": 0.9951881001037627, + "timestamp": 3.81588437804475 + }, + { + "x": 5.596527243008144, + "y": 6.264409866427464, + "heading": -3.0649099526354786, + "angularVelocity": 0.4068911044444387, + "velocityX": -1.8927625013098104, + "velocityY": 1.0181195830264693, + "timestamp": 3.8950600836939344 + }, + { + "x": 5.47431680968178, + "y": 6.34590840516807, + "heading": -3.034696195300985, + "angularVelocity": 0.38160389082436225, + "velocityX": -1.5435345011998953, + "velocityY": 1.029337699896412, + "timestamp": 3.974235789343119 + }, + { + "x": 5.37976915844747, + "y": 6.427702267182384, + "heading": -3.0065318546626574, + "angularVelocity": 0.35571947742555177, + "velocityX": -1.1941497768676173, + "velocityY": 1.0330676732674617, + "timestamp": 4.053411494992304 + }, + { + "x": 5.312885421758628, + "y": 6.509380270776351, + "heading": -2.9804521557080004, + "angularVelocity": 0.3293901676129292, + "velocityX": -0.8447507494937071, + "velocityY": 1.0316043655596845, + "timestamp": 4.132587200641488 + }, + { + "x": 5.273662535323598, + "y": 6.590640683149381, + "heading": -2.9564850016577298, + "angularVelocity": 0.30270843630324745, + "velocityX": -0.49539042454286053, + "velocityY": 1.026330131278943, + "timestamp": 4.211762906290673 + }, + { + "x": 5.262095928192139, + "y": 6.671252727508545, + "heading": -2.9346533643249577, + "angularVelocity": 0.2757365678495407, + "velocityX": -0.14608783131922698, + "velocityY": 1.0181411545145314, + "timestamp": 4.290938611939858 + }, + { + "x": 5.286450146462021, + "y": 6.764484077147902, + "heading": -2.9120502053248636, + "angularVelocity": 0.24386251496431138, + "velocityX": 0.26275446353576026, + "velocityY": 1.0058607912494006, + "timestamp": 4.383626735833773 + }, + { + "x": 5.34869261123642, + "y": 6.856387459090783, + "heading": -2.8924677533563106, + "angularVelocity": 0.2112725033788139, + "velocityX": 0.671525780860962, + "velocityY": 0.9915335221162508, + "timestamp": 4.4763148597276885 + }, + { + "x": 5.448813085425695, + "y": 6.946710159976592, + "heading": -2.8759905639386316, + "angularVelocity": 0.17777023339621403, + "velocityX": 1.080186651570012, + "velocityY": 0.9744797617134475, + "timestamp": 4.569002983621604 + }, + { + "x": 5.586794422356268, + "y": 7.035098884885215, + "heading": -2.8627324007346533, + "angularVelocity": 0.1430405821910139, + "velocityX": 1.4886625290689568, + "velocityY": 0.9536143488003591, + "timestamp": 4.661691107515519 + }, + { + "x": 5.762604761098077, + "y": 7.121024926575688, + "heading": -2.8528585427257678, + "angularVelocity": 0.10652775775444928, + "velocityX": 1.8967946631763621, + "velocityY": 0.9270447828765889, + "timestamp": 4.754379231409435 + }, + { + "x": 5.976174763029016, + "y": 7.2036110783097, + "heading": -2.8466380444560433, + "angularVelocity": 0.06711213916514423, + "velocityX": 2.3041787119931025, + "velocityY": 0.8910111486185037, + "timestamp": 4.84706735530335 + }, + { + "x": 6.227306274033663, + "y": 7.2811226036526095, + "heading": -2.8445996856753095, + "angularVelocity": 0.021991585276523277, + "velocityX": 2.709424902073476, + "velocityY": 0.8362616707144066, + "timestamp": 4.939755479197266 + }, + { + "x": 6.514972208251902, + "y": 7.348599798549223, + "heading": -2.8482807276605207, + "angularVelocity": -0.039714278707639744, + "velocityX": 3.103589997651488, + "velocityY": 0.7280025969005842, + "timestamp": 5.032443603091181 + }, + { + "x": 6.803286438858483, + "y": 7.378323009778229, + "heading": -2.8654797905272513, + "angularVelocity": -0.1855584312658597, + "velocityX": 3.110584382272814, + "velocityY": 0.3206798236959108, + "timestamp": 5.125131726985097 + }, + { + "x": 7.05515266695702, + "y": 7.39757835383594, + "heading": -2.8830786743462165, + "angularVelocity": -0.18987204702845675, + "velocityX": 2.717351668341086, + "velocityY": 0.2077433790735653, + "timestamp": 5.217819850879012 + }, + { + "x": 7.269495335698046, + "y": 7.411362471913543, + "heading": -2.899617904375188, + "angularVelocity": -0.17843958140636978, + "velocityX": 2.312514912766473, + "velocityY": 0.1487150402718173, + "timestamp": 5.310507974772928 + }, + { + "x": 7.446102948393076, + "y": 7.421417571603764, + "heading": -2.9145959892958864, + "angularVelocity": -0.1615965917903412, + "velocityX": 1.905396347186422, + "velocityY": 0.10848315045980522, + "timestamp": 5.403196098666843 + }, + { + "x": 7.584899849507656, + "y": 7.428623463558208, + "heading": -2.927761388273211, + "angularVelocity": -0.14203976112833264, + "velocityX": 1.4974615439777093, + "velocityY": 0.07774342226077013, + "timestamp": 5.4958842225607585 + }, + { + "x": 7.685850480593157, + "y": 7.433510011721959, + "heading": -2.938963494323689, + "angularVelocity": -0.12085805149426576, + "velocityX": 1.0891431053350733, + "velocityY": 0.05272032660132098, + "timestamp": 5.588572346454674 + }, + { + "x": 7.748935268502705, + "y": 7.436431089750518, + "heading": -2.948102141289854, + "angularVelocity": -0.09859566233776026, + "velocityX": 0.6806134945804951, + "velocityY": 0.03151512735226398, + "timestamp": 5.681260470348589 + }, + { + "x": 7.774142265319824, + "y": 7.4376397132873535, + "heading": -2.9551058096712683, + "angularVelocity": -0.07556165868057244, + "velocityX": 0.27195497932366575, + "velocityY": 0.013039680663066213, + "timestamp": 5.773948594242505 + }, + { + "x": 7.7581383251743805, + "y": 7.437204402709091, + "heading": -2.960077030928164, + "angularVelocity": -0.05046428755227366, + "velocityX": -0.16246056969378117, + "velocityY": -0.004418962074057982, + "timestamp": 5.872458282935643 + }, + { + "x": 7.699340240644664, + "y": 7.4350492376719135, + "heading": -2.9625771572415784, + "angularVelocity": -0.025379496642227335, + "velocityX": -0.5968761581703449, + "velocityY": -0.021877696151197678, + "timestamp": 5.970967971628781 + }, + { + "x": 7.597748009323179, + "y": 7.431174217705324, + "heading": -2.962606855741692, + "angularVelocity": -0.0003014779612800365, + "velocityX": -1.0312917710860698, + "velocityY": -0.03933643500447566, + "timestamp": 6.069477660321919 + }, + { + "x": 7.453361630377404, + "y": 7.4255793508521934, + "heading": -2.9601662281215004, + "angularVelocity": 0.02477550840500792, + "velocityX": -1.4657073924529878, + "velocityY": -0.05679509221229112, + "timestamp": 6.167987349015057 + }, + { + "x": 7.266181104848686, + "y": 7.4182646536312635, + "heading": -2.9552547893870527, + "angularVelocity": 0.04985741808348525, + "velocityX": -1.9001230032488765, + "velocityY": -0.07425358173362791, + "timestamp": 6.266497037708195 + }, + { + "x": 7.036206436366714, + "y": 7.409230150978378, + "heading": -2.947871418759817, + "angularVelocity": 0.07495070510511129, + "velocityX": -2.334538577198753, + "velocityY": -0.09171181812408385, + "timestamp": 6.365006726401333 + }, + { + "x": 6.763437633128491, + "y": 7.398475876181393, + "heading": -2.938014282817138, + "angularVelocity": 0.10006260372402688, + "velocityX": -2.7689540679385254, + "velocityY": -0.10916971660002103, + "timestamp": 6.463516415094471 + }, + { + "x": 6.447874717087643, + "y": 7.386001870945065, + "heading": -2.9256807297504217, + "angularVelocity": 0.12520142160976788, + "velocityX": -3.203369335820747, + "velocityY": -0.12662719171903017, + "timestamp": 6.562026103787609 + }, + { + "x": 6.1055219219448285, + "y": 7.372245899357551, + "heading": -2.905707588465526, + "angularVelocity": 0.20275306469714785, + "velocityX": -3.475321054046335, + "velocityY": -0.13964079848394192, + "timestamp": 6.660535792480747 + }, + { + "x": 5.8059632213622585, + "y": 7.360209533615462, + "heading": -2.88822069052682, + "angularVelocity": 0.17751449802240743, + "velocityX": -3.040905971347722, + "velocityY": -0.12218458815337546, + "timestamp": 6.759045481173885 + }, + { + "x": 5.549198623839576, + "y": 7.349892726247163, + "heading": -2.873224621784891, + "angularVelocity": 0.15222937906790351, + "velocityX": -2.6064908023668067, + "velocityY": -0.10472885972095358, + "timestamp": 6.857555169867023 + }, + { + "x": 5.33522812761357, + "y": 7.341295437788649, + "heading": -2.860723300556775, + "angularVelocity": 0.1269044841574769, + "velocityX": -2.172075651284741, + "velocityY": -0.08727353189893447, + "timestamp": 6.956064858560161 + }, + { + "x": 5.164051729917877, + "y": 7.334417637055141, + "heading": -2.85071984383666, + "angularVelocity": 0.10154794774832755, + "velocityX": -1.7376605282848332, + "velocityY": -0.06981852064248911, + "timestamp": 7.054574547253299 + }, + { + "x": 5.0356694290546695, + "y": 7.3292593010381655, + "heading": -2.8432164608319708, + "angularVelocity": 0.07616898504331324, + "velocityX": -1.303245422520044, + "velocityY": -0.05236374295166474, + "timestamp": 7.153084235946437 + }, + { + "x": 4.950081225204065, + "y": 7.325820414771508, + "heading": -2.8382143743786914, + "angularVelocity": 0.0507776089807905, + "velocityX": -0.8688303149268349, + "velocityY": -0.03490911718713825, + "timestamp": 7.251593924639575 + }, + { + "x": 4.907287120819092, + "y": 7.324100971221924, + "heading": -2.8357137696574513, + "angularVelocity": 0.025384353096777085, + "velocityX": -0.43441518243224797, + "velocityY": -0.01745456281909801, + "timestamp": 7.350103613332713 + }, + { + "x": 4.907287120819092, + "y": 7.324100971221924, + "heading": -2.8357137696574513, + "angularVelocity": 3.103332055729446e-26, + "velocityX": -6.947696608697022e-27, + "velocityY": 2.520053434961675e-27, + "timestamp": 7.448613302025851 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.373390793800354, + "y": 5.5500569343566895, + "heading": -3.14, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 0.7891206216760503, + "isStopPoint": false, + "x": 2.4236249923706055, + "y": 5.564249038696289, + "heading": -3.135640447743494, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 27 + }, + { + "timestamp": 0.7891206248073066, + "isStopPoint": false, + "x": 3.644167423248291, + "y": 5.975827217102051, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": false, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "timestamp": 1.7655430075711553, + "isStopPoint": false, + "x": 5.446596145629883, + "y": 6.288059234619141, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 32 + }, + { + "timestamp": 2.7866002046053495, + "isStopPoint": false, + "x": 7.759950160980225, + "y": 5.919057846069336, + "heading": -3.4633432079864352, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "timestamp": 4.290938611939858, + "isStopPoint": false, + "x": 5.262095928192139, + "y": 6.671252727508545, + "heading": -2.9346533643249577, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "timestamp": 5.773948594242505, + "isStopPoint": false, + "x": 7.774142265319824, + "y": 7.4376397132873535, + "heading": -2.9551058096712683, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 7.448613302025851, + "isStopPoint": true, + "x": 4.907287120819092, + "y": 7.324100971221924, + "heading": -2.8357137696574513, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1, + 1 + ], + "type": "MaxVelocity", + "velocity": 1 + }, + { + "scope": [ + 1, + 2 + ], + "type": "MaxVelocity", + "velocity": 1 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "New Path (2)": { + "waypoints": [ + { + "x": 1.302428960800171, + "y": 5.521671772003174, + "heading": 3.119698237349717, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 2.3668556213378906, + "y": 5.5500569343566895, + "heading": 3.1232461040248376, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 30 + }, + { + "x": 4.453131675720215, + "y": 5.067516803741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 5.801405429840088, + "y": 4.017282485961914, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 30 + }, + { + "x": 7.845104217529297, + "y": 4.088244438171387, + "heading": 3.163567121665249, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 30 + }, + { + "x": 6.8658318519592285, + "y": 6.004211902618408, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 30 + }, + { + "x": 4.992441177368164, + "y": 6.5009446144104, + "heading": 3.309955326051445, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.302428960800171, + "y": 5.521671772003174, + "heading": 3.119698237349717, + "angularVelocity": -1.0799521907829878e-24, + "velocityX": -5.729727943881475e-23, + "velocityY": 3.806127884713555e-22, + "timestamp": 0 + }, + { + "x": 1.3217771089315806, + "y": 5.522383365283616, + "heading": 3.1197999674258803, + "angularVelocity": 0.0015360380238209192, + "velocityX": 0.29214065634450126, + "velocityY": 0.010744456088856187, + "timestamp": 0.0662288788336051 + }, + { + "x": 1.3604749484171008, + "y": 5.523763370471075, + "heading": 3.1199951995851856, + "angularVelocity": 0.002947840318953084, + "velocityX": 0.5843046140452638, + "velocityY": 0.020836909997010808, + "timestamp": 0.1324577576672102 + }, + { + "x": 1.4185241489686518, + "y": 5.525761671584798, + "heading": 3.1202743864187497, + "angularVelocity": 0.00421548482295459, + "velocityX": 0.8764937829824225, + "velocityY": 0.030172655024753685, + "timestamp": 0.1986866365008153 + }, + { + "x": 1.4959265053667201, + "y": 5.528319403030693, + "heading": 3.1206263164550307, + "angularVelocity": 0.005313845598465322, + "velocityX": 1.1687100515854367, + "velocityY": 0.03861957941822778, + "timestamp": 0.2649155153344204 + }, + { + "x": 1.5926839210326802, + "y": 5.53136644125991, + "heading": 3.121037637902798, + "angularVelocity": 0.006210605630214725, + "velocityX": 1.4609550602397396, + "velocityY": 0.04600769759175801, + "timestamp": 0.33114439416802555 + }, + { + "x": 1.7087983602256251, + "y": 5.5348178374547645, + "heading": 3.121492181382272, + "angularVelocity": 0.006863221716555551, + "velocityX": 1.7532297275433801, + "velocityY": 0.052113160537196716, + "timestamp": 0.39737327300163067 + }, + { + "x": 1.844271734223262, + "y": 5.538568567278952, + "heading": 3.1219699631830324, + "angularVelocity": 0.007214100694065514, + "velocityX": 2.0455332535222754, + "velocityY": 0.05663284491967778, + "timestamp": 0.4636021518352358 + }, + { + "x": 1.9991056443900284, + "y": 5.542485474471211, + "heading": 3.1224456556980607, + "angularVelocity": 0.007182554248321628, + "velocityX": 2.3378609587484425, + "velocityY": 0.05914198248923271, + "timestamp": 0.5298310306688409 + }, + { + "x": 2.1733007984970207, + "y": 5.546394249267978, + "heading": 3.1228861145568447, + "angularVelocity": 0.006650555868398608, + "velocityX": 2.6301993507189527, + "velocityY": 0.059019190202322734, + "timestamp": 0.5960599095024459 + }, + { + "x": 2.36685562133789, + "y": 5.5500569343566895, + "heading": 3.1232461040248376, + "angularVelocity": 0.005435536194070859, + "velocityX": 2.922513958407195, + "velocityY": 0.055303443954021286, + "timestamp": 0.662288788336051 + }, + { + "x": 2.4263388144591955, + "y": 5.551097384925455, + "heading": 3.123340540157932, + "angularVelocity": 0.004778249196920622, + "velocityX": 3.009711542072027, + "velocityY": 0.0526443844294007, + "timestamp": 0.6820525405394778 + }, + { + "x": 2.4875382986996155, + "y": 5.5519733808753085, + "heading": 3.1234216636032834, + "angularVelocity": 0.00410465808903763, + "velocityX": 3.096551890071197, + "velocityY": 0.044323362327003256, + "timestamp": 0.7018162927429047 + }, + { + "x": 2.550436955892463, + "y": 5.552556955429702, + "heading": 3.123489130605961, + "angularVelocity": 0.003413673779285827, + "velocityX": 3.1825260985584145, + "velocityY": 0.02952751827621247, + "timestamp": 0.7215800449463315 + }, + { + "x": 2.615003335084898, + "y": 5.552703036227377, + "heading": 3.1235425841754783, + "angularVelocity": 0.0027046265793564048, + "velocityX": 3.26690896181343, + "velocityY": 0.007391349384069019, + "timestamp": 0.7413437971497584 + }, + { + "x": 2.6811863409244094, + "y": 5.55224977213001, + "heading": 3.123581673782466, + "angularVelocity": 0.0019778434067488013, + "velocityX": 3.3487065187973872, + "velocityY": -0.022934111534127874, + "timestamp": 0.7611075493531853 + }, + { + "x": 2.7489093580691275, + "y": 5.55102122741091, + "heading": 3.123606094755385, + "angularVelocity": 0.001235644561187388, + "velocityX": 3.426627517267461, + "velocityY": -0.062161511966688114, + "timestamp": 0.7808713015566121 + }, + { + "x": 2.8180650275496353, + "y": 5.54883341134946, + "heading": 3.1236156557026797, + "angularVelocity": 0.00048376174709457927, + "velocityX": 3.4991164010099673, + "velocityY": -0.11069841591471039, + "timestamp": 0.800635053760039 + }, + { + "x": 2.8885126788941395, + "y": 5.54550389601763, + "heading": 3.1236103737324172, + "angularVelocity": -0.0002672554385466297, + "velocityX": 3.564487685303465, + "velocityY": -0.1684657496997351, + "timestamp": 0.8203988059634658 + }, + { + "x": 2.9600805496923224, + "y": 5.540863730529652, + "heading": 3.1235905765119973, + "angularVelocity": -0.0010016934141146043, + "velocityX": 3.6211681902070074, + "velocityY": -0.23478160625658828, + "timestamp": 0.8401625581668927 + }, + { + "x": 3.032573643140278, + "y": 5.534768690288578, + "heading": 3.123556968357328, + "angularVelocity": -0.0017004946390557507, + "velocityX": 3.6679823093201, + "velocityY": -0.3083948927480261, + "timestamp": 0.8599263103703195 + }, + { + "x": 3.105785678320405, + "y": 5.527106543467365, + "heading": 3.1235106204259195, + "angularVelocity": -0.0023450977795856365, + "velocityX": 3.704359092674332, + "velocityY": -0.38768684925548386, + "timestamp": 0.8796900625737464 + }, + { + "x": 3.1795117584951056, + "y": 5.517798656367154, + "heading": 3.1234528819071974, + "angularVelocity": -0.0029214350659676155, + "velocityX": 3.7303685765660113, + "velocityY": -0.4709574884568884, + "timestamp": 0.8994538147771732 + }, + { + "x": 3.2535585488164687, + "y": 5.506796705703182, + "heading": 3.1233852496472454, + "angularVelocity": -0.003422035413907008, + "velocityX": 3.7465957657839812, + "velocityY": -0.556673173733965, + "timestamp": 0.9192175669806001 + }, + { + "x": 3.3277504743592434, + "y": 5.494076719862022, + "heading": 3.123309243507978, + "angularVelocity": -0.003845734275821943, + "velocityX": 3.7539392712032984, + "velocityY": -0.6436017670244403, + "timestamp": 0.9389813191840269 + }, + { + "x": 3.401932191914863, + "y": 5.479632608953573, + "heading": 3.1232263159056575, + "angularVelocity": -0.004195944245161604, + "velocityX": 3.7534227707407335, + "velocityY": -0.7308384946226822, + "timestamp": 0.9587450713874538 + }, + { + "x": 3.4759684441746432, + "y": 5.463470489536251, + "heading": 3.1231378004849635, + "angularVelocity": -0.004478674888388461, + "velocityX": 3.7460625643213086, + "velocityY": -0.8177657385584958, + "timestamp": 0.9785088235908806 + }, + { + "x": 3.5497424561199664, + "y": 5.44560427359136, + "heading": 3.1230448916469586, + "angularVelocity": -0.004700971609475635, + "velocityX": 3.732793812934527, + "velocityY": -0.9039890685228098, + "timestamp": 0.9982725757943075 + }, + { + "x": 3.6231537261374727, + "y": 5.426052491121333, + "heading": 3.1229486436831992, + "angularVelocity": -0.004869923624246792, + "velocityX": 3.7144399131242594, + "velocityY": -0.9892748233625418, + "timestamp": 1.0180363279977345 + }, + { + "x": 3.6961157220105543, + "y": 5.404836115617309, + "heading": 3.1228499803373504, + "angularVelocity": -0.004992136353107221, + "velocityX": 3.6917076839503324, + "velocityY": -1.0734993682194114, + "timestamp": 1.0378000802011613 + }, + { + "x": 3.7685537383740204, + "y": 5.381977131361945, + "heading": 3.122749708770004, + "angularVelocity": -0.0050735086290428135, + "velocityX": 3.6651955366504962, + "velocityY": -1.1566115593880046, + "timestamp": 1.0575638324045882 + }, + { + "x": 3.8404030175360155, + "y": 5.357497620278821, + "heading": 3.1226485344894, + "angularVelocity": -0.005119183825147258, + "velocityX": 3.6354068004119586, + "velocityY": -1.2386064564642996, + "timestamp": 1.077327584608015 + }, + { + "x": 3.9116071530358583, + "y": 5.331419200542643, + "heading": 3.12254707551822, + "angularVelocity": -0.005133588507666248, + "velocityX": 3.6027640281533504, + "velocityY": -1.319507524065004, + "timestamp": 1.0970913368114419 + }, + { + "x": 3.982116756748269, + "y": 5.303762698026613, + "heading": 3.1224458750644937, + "angularVelocity": -0.005120508124394676, + "velocityX": 3.567622331360009, + "velocityY": -1.399354850807865, + "timestamp": 1.1168550890148687 + }, + { + "x": 4.051888355776174, + "y": 5.2745479692393795, + "heading": 3.122345412497445, + "angularVelocity": -0.005083172770798156, + "velocityX": 3.5302810068528747, + "velocityY": -1.478197484289892, + "timestamp": 1.1366188412182956 + }, + { + "x": 4.120883482732203, + "y": 5.24379382125935, + "heading": 3.122246112688224, + "angularVelocity": -0.005024339922835519, + "velocityX": 3.4909933218078715, + "velocityY": -1.5560885232459687, + "timestamp": 1.1563825934217224 + }, + { + "x": 4.1890679256640215, + "y": 5.211517992630351, + "heading": 3.122148353884756, + "angularVelocity": -0.0049463686076292, + "velocityX": 3.44997459136313, + "velocityY": -1.6330820330464426, + "timestamp": 1.1761463456251493 + }, + { + "x": 4.256411108437102, + "y": 5.177737171593987, + "heading": 3.1220524743221056, + "angularVelocity": -0.004851283382997846, + "velocityX": 3.407408779462607, + "velocityY": -1.7092311565466005, + "timestamp": 1.1959100978285762 + }, + { + "x": 4.3228855772355175, + "y": 5.14246703628049, + "heading": 3.1219587777652085, + "angularVelocity": -0.004740828357538679, + "velocityX": 3.363453868182489, + "velocityY": -1.7845870030379125, + "timestamp": 1.215673850032003 + }, + { + "x": 4.388466573286225, + "y": 5.105722306923142, + "heading": 3.1218675381607466, + "angularVelocity": -0.0046165122656358646, + "velocityX": 3.3182462204386316, + "velocityY": -1.8591980398831336, + "timestamp": 1.2354376022354299 + }, + { + "x": 4.453131675720215, + "y": 5.067516803741455, + "heading": 3.1217790035501856, + "angularVelocity": -0.004479645851134587, + "velocityX": 3.2719041287503026, + "velocityY": -1.9331098056907883, + "timestamp": 1.2552013544388567 + }, + { + "x": 4.5756878518534965, + "y": 4.98749687604741, + "heading": 3.1216174796705585, + "angularVelocity": -0.004190461413754277, + "velocityX": 3.179510845639871, + "velocityY": -2.0759804686941523, + "timestamp": 1.293746961635431 + }, + { + "x": 4.6944682593112, + "y": 4.902114649138773, + "heading": 3.12147264475533, + "angularVelocity": -0.003757494712439645, + "velocityX": 3.081554970763572, + "velocityY": -2.2150961709646055, + "timestamp": 1.332292568832005 + }, + { + "x": 4.8091447997962735, + "y": 4.811616865702052, + "heading": 3.121353020228485, + "angularVelocity": -0.0031034542077534163, + "velocityX": 2.975087145475454, + "velocityY": -2.347810555304065, + "timestamp": 1.3708381760285793 + }, + { + "x": 4.919165948005501, + "y": 4.716499788885737, + "heading": 3.1212730435084532, + "angularVelocity": -0.0020748595196256386, + "velocityX": 2.8543109373823983, + "velocityY": -2.4676502391371975, + "timestamp": 1.4093837832251535 + }, + { + "x": 5.023524570482215, + "y": 4.6180742282694816, + "heading": 3.121259950874307, + "angularVelocity": -0.00033966605012470416, + "velocityX": 2.7074063704460154, + "velocityY": -2.5534832053444214, + "timestamp": 1.4479293904217276 + }, + { + "x": 5.1214255127288455, + "y": 4.520792255143304, + "heading": 3.1213609572395313, + "angularVelocity": 0.0026204377767055976, + "velocityX": 2.539872877014423, + "velocityY": -2.5238147794653165, + "timestamp": 1.4864749976183018 + }, + { + "x": 5.216508916211957, + "y": 4.429432365379414, + "heading": 3.1215812446063884, + "angularVelocity": 0.005714979809078672, + "velocityX": 2.466776641971333, + "velocityY": -2.3701764327637718, + "timestamp": 1.525020604814876 + }, + { + "x": 5.311195347524098, + "y": 4.3446186668561, + "heading": 3.1218832424332517, + "angularVelocity": 0.00783481825368405, + "velocityX": 2.456477876435081, + "velocityY": -2.200346672210494, + "timestamp": 1.5635662120114502 + }, + { + "x": 5.406488770050426, + "y": 4.2663350523445525, + "heading": 3.1222460060108177, + "angularVelocity": 0.009411281957914153, + "velocityX": 2.4722252276466263, + "velocityY": -2.0309347862213687, + "timestamp": 1.6021118192080244 + }, + { + "x": 5.502899923549765, + "y": 4.194513729448243, + "heading": 3.122658002124391, + "angularVelocity": 0.010688536088486314, + "velocityX": 2.501222850315031, + "velocityY": -1.863281658271366, + "timestamp": 1.6406574264045986 + }, + { + "x": 5.600732691860309, + "y": 4.12909471859619, + "heading": 3.1231121352270255, + "angularVelocity": 0.011781708362225283, + "velocityX": 2.5381042205826043, + "velocityY": -1.6971845979343585, + "timestamp": 1.6792030336011727 + }, + { + "x": 5.700187347441565, + "y": 4.070030241682816, + "heading": 3.1236036443195165, + "angularVelocity": 0.012751364636292445, + "velocityX": 2.580181318043823, + "velocityY": -1.532327059012421, + "timestamp": 1.717748640797747 + }, + { + "x": 5.801405429840088, + "y": 4.017282485961914, + "heading": 3.124129127034147, + "angularVelocity": 0.013632752286159615, + "velocityX": 2.6259304175008134, + "velocityY": -1.3684505072627247, + "timestamp": 1.756294247994321 + }, + { + "x": 5.881714978660041, + "y": 3.979872135421584, + "heading": 3.124559351404669, + "angularVelocity": 0.014270430872906349, + "velocityX": 2.663846921269128, + "velocityY": -1.240891632125537, + "timestamp": 1.7864422073904604 + }, + { + "x": 5.963108059198022, + "y": 3.9463246319250165, + "heading": 3.1250109743561647, + "angularVelocity": 0.014980216258125125, + "velocityX": 2.699787387547169, + "velocityY": -1.11276199678257, + "timestamp": 1.8165901667865998 + }, + { + "x": 6.045516764082525, + "y": 3.91665838172662, + "heading": 3.1254864627529404, + "angularVelocity": 0.015771826893091, + "velocityX": 2.7334753839112973, + "velocityY": -0.9840218307510211, + "timestamp": 1.8467381261827391 + }, + { + "x": 6.128863052724697, + "y": 3.8908929706985393, + "heading": 3.12598863599481, + "angularVelocity": 0.016656956289182748, + "velocityX": 2.7645747941681287, + "velocityY": -0.8546320064163314, + "timestamp": 1.8768860855788785 + }, + { + "x": 6.213056423203702, + "y": 3.8690490451787327, + "heading": 3.1265207413378744, + "angularVelocity": 0.017649796328583447, + "velocityX": 2.792672279165528, + "velocityY": -0.7245573484022891, + "timestamp": 1.9070340449750178 + }, + { + "x": 6.297990860537032, + "y": 3.851148011868822, + "heading": 3.1270865497545324, + "angularVelocity": 0.018767718545174807, + "velocityX": 2.8172532746679417, + "velocityY": -0.5937726356432314, + "timestamp": 1.9371820043711572 + }, + { + "x": 6.383540783527209, + "y": 3.837211412188912, + "heading": 3.127690478291489, + "angularVelocity": 0.020032153056224314, + "velocityX": 2.83766877439583, + "velocityY": -0.46227339956199653, + "timestamp": 1.9673299637672965 + }, + { + "x": 6.469555594214739, + "y": 3.8272597097732, + "heading": 3.1283377457202124, + "angularVelocity": 0.021469692864407197, + "velocityX": 2.8530889788362965, + "velocityY": -0.3300953900377929, + "timestamp": 1.9974779231634359 + }, + { + "x": 6.555852273967605, + "y": 3.8213100114054206, + "heading": 3.1290345673449957, + "angularVelocity": 0.023113392705193338, + "velocityX": 2.8624385026840957, + "velocityY": -0.1973499529305097, + "timestamp": 2.027625882559575 + }, + { + "x": 6.642205282475191, + "y": 3.8193718287910654, + "heading": 3.1297883882708835, + "angularVelocity": 0.02500404475084348, + "velocityX": 2.864306912879999, + "velocityY": -0.06428901501716788, + "timestamp": 2.0577738419557146 + }, + { + "x": 6.7283328946409515, + "y": 3.821439211302739, + "heading": 3.13060813228516, + "angularVelocity": 0.02719069650803073, + "velocityX": 2.856830574635499, + "velocityY": 0.06857454212766521, + "timestamp": 2.087921801351854 + }, + { + "x": 6.813879408595297, + "y": 3.827476175013503, + "heading": 3.131504382371298, + "angularVelocity": 0.029728383084265037, + "velocityX": 2.837555697560754, + "velocityY": 0.20024452174156598, + "timestamp": 2.1180697607479932 + }, + { + "x": 6.898394452961866, + "y": 3.837390144910791, + "heading": 3.1324892640321416, + "angularVelocity": 0.032668269447444956, + "velocityX": 2.8033421186507304, + "velocityY": 0.3288438121804522, + "timestamp": 2.1482177201441326 + }, + { + "x": 6.981316545653192, + "y": 3.850986156064535, + "heading": 3.1335755392972953, + "angularVelocity": 0.03603146902516665, + "velocityX": 2.7505043244135474, + "velocityY": 0.45097616641626476, + "timestamp": 2.178365679540272 + }, + { + "x": 7.061981725860651, + "y": 3.867898952893054, + "heading": 3.1347742460736483, + "angularVelocity": 0.03976079311379567, + "velocityX": 2.675643122226997, + "velocityY": 0.5609930876676483, + "timestamp": 2.2085136389364113 + }, + { + "x": 7.139693035364971, + "y": 3.8875263790502017, + "heading": 3.1360911020047273, + "angularVelocity": 0.0436797699564237, + "velocityX": 2.577663996531448, + "velocityY": 0.651036638972683, + "timestamp": 2.2386615983325506 + }, + { + "x": 7.213860796163365, + "y": 3.9090347061974744, + "heading": 3.1375244322919986, + "angularVelocity": 0.04754319416573168, + "velocityX": 2.4601254043049017, + "velocityY": 0.7134256373592958, + "timestamp": 2.26880955772869 + }, + { + "x": 7.2841296289302875, + "y": 3.9314880045121248, + "heading": 3.1390672357385005, + "angularVelocity": 0.05117439048625725, + "velocityX": 2.330798971950365, + "velocityY": 0.7447700860817068, + "timestamp": 2.2989575171248293 + }, + { + "x": 7.350387407544562, + "y": 3.954020046909829, + "heading": 3.1407107850332614, + "angularVelocity": 0.05451610416363438, + "velocityX": 2.1977533452150904, + "velocityY": 0.7473820069092071, + "timestamp": 2.3291054765209687 + }, + { + "x": 7.412682228157059, + "y": 3.9759276926483125, + "heading": 3.142446557806337, + "angularVelocity": 0.05757513303862974, + "velocityX": 2.0663030553396227, + "velocityY": 0.7266709315420209, + "timestamp": 2.359253435917108 + }, + { + "x": 7.4711363711768914, + "y": 3.9966759270340453, + "heading": 3.1442667998509624, + "angularVelocity": 0.0603769568847877, + "velocityX": 1.9389087749440745, + "velocityY": 0.6882135574452845, + "timestamp": 2.3894013953132474 + }, + { + "x": 7.525893877095674, + "y": 4.0158653092903895, + "heading": 3.1461646469014637, + "angularVelocity": 0.06295109481751612, + "velocityX": 1.8162922803257737, + "velocityY": 0.6365068363068591, + "timestamp": 2.4195493547093867 + }, + { + "x": 7.5770959995446026, + "y": 4.033196068837796, + "heading": 3.1481341124991036, + "angularVelocity": 0.06532666346539286, + "velocityX": 1.6983611320468093, + "velocityY": 0.5748568027335785, + "timestamp": 2.449697314105526 + }, + { + "x": 7.624871879535836, + "y": 4.04843968713685, + "heading": 3.1501700145363154, + "angularVelocity": 0.06753034294826869, + "velocityX": 1.5847135576728835, + "velocityY": 0.5056268684309618, + "timestamp": 2.4798452735016654 + }, + { + "x": 7.669336187729304, + "y": 4.061418611160472, + "heading": 3.152267875920012, + "angularVelocity": 0.06958551841372541, + "velocityX": 1.4748695793706585, + "velocityY": 0.43050754623493903, + "timestamp": 2.5099932328978047 + }, + { + "x": 7.710589606461839, + "y": 4.071992212309712, + "heading": 3.1544238219627907, + "angularVelocity": 0.07151217150223022, + "velocityX": 1.3683652080882895, + "velocityY": 0.35072360985710144, + "timestamp": 2.540141192293944 + }, + { + "x": 7.748720297763379, + "y": 4.080047084432328, + "heading": 3.156634486140592, + "angularVelocity": 0.07332715786014068, + "velocityX": 1.2647851484907588, + "velocityY": 0.2671780207998906, + "timestamp": 2.5702891516900834 + }, + { + "x": 7.783805577887537, + "y": 4.085490267375178, + "heading": 3.158896928505875, + "angularVelocity": 0.07504462691998749, + "velocityX": 1.1637696489882914, + "velocityY": 0.1805489675545591, + "timestamp": 2.6004371110862228 + }, + { + "x": 7.815913495819532, + "y": 4.088244438171387, + "heading": 3.161208567177335, + "angularVelocity": 0.07667645564617116, + "velocityX": 1.0650113166898847, + "velocityY": 0.09135513153707456, + "timestamp": 2.630585070482362 + }, + { + "x": 7.845104217529297, + "y": 4.088244438171387, + "heading": 3.163567121665249, + "angularVelocity": 0.07823264111918607, + "velocityX": 0.968248674021464, + "velocityY": 9.949308093023297e-24, + "timestamp": 2.6607330298785015 + }, + { + "x": 7.878491824221837, + "y": 4.093585686533033, + "heading": 3.1666157231902687, + "angularVelocity": 0.07980374508976079, + "velocityX": 0.8739928887988145, + "velocityY": 0.1398187395813148, + "timestamp": 2.6989342636876534 + }, + { + "x": 7.908226247157587, + "y": 4.10423241668243, + "heading": 3.1697235484523434, + "angularVelocity": 0.08135405462559406, + "velocityX": 0.7783628948818408, + "velocityY": 0.278701211656863, + "timestamp": 2.7371354974968054 + }, + { + "x": 7.934251131583176, + "y": 4.120145380644015, + "heading": 3.1728897318487155, + "angularVelocity": 0.08288170513523152, + "velocityX": 0.6812576932882871, + "velocityY": 0.4165562830007863, + "timestamp": 2.7753367313059574 + }, + { + "x": 7.956505849641918, + "y": 4.141281346281812, + "heading": 3.176113327242148, + "angularVelocity": 0.0843845884543132, + "velocityX": 0.582565426287611, + "velocityY": 0.5532796595887517, + "timestamp": 2.8135379651151093 + }, + { + "x": 7.974925029508088, + "y": 4.167592498355179, + "heading": 3.179393297087888, + "angularVelocity": 0.08586031179323926, + "velocityX": 0.4821619102197233, + "velocityY": 0.6887513687336226, + "timestamp": 2.8517391989242613 + }, + { + "x": 7.989438020301411, + "y": 4.199025720545251, + "heading": 3.1827284996727405, + "angularVelocity": 0.08730614831746564, + "velocityX": 0.37990895440256806, + "velocityY": 0.8228326432362902, + "timestamp": 2.8899404327334133 + }, + { + "x": 7.999968283780463, + "y": 4.235521729180706, + "heading": 3.1861176740734445, + "angularVelocity": 0.0887189774454874, + "velocityX": 0.27565244441213116, + "velocityY": 0.9553620392939309, + "timestamp": 2.9281416665425652 + }, + { + "x": 8.00643270325469, + "y": 4.27701402090998, + "heading": 3.1895594223515835, + "angularVelocity": 0.09009521250892577, + "velocityX": 0.16922017510021273, + "velocityY": 1.0861505661456612, + "timestamp": 2.966342900351717 + }, + { + "x": 8.008740800208267, + "y": 4.323427585324584, + "heading": 3.1930521883927083, + "angularVelocity": 0.09143071290771301, + "velocityX": 0.060419434751950635, + "velocityY": 1.2149755331589407, + "timestamp": 3.004544134160869 + }, + { + "x": 8.006793850403897, + "y": 4.3746773185892955, + "heading": 3.1965942326665227, + "angularVelocity": 0.09272067733493149, + "velocityX": -0.05096562624382283, + "velocityY": 1.341572723036849, + "timestamp": 3.042745367970021 + }, + { + "x": 8.000483894798665, + "y": 4.430666054241619, + "heading": 3.200183602033556, + "angularVelocity": 0.09395951410798249, + "velocityX": -0.16517674891749978, + "velocityY": 1.4656263703951196, + "timestamp": 3.080946601779173 + }, + { + "x": 7.989692648311953, + "y": 4.49128210092684, + "heading": 3.203818093558477, + "angularVelocity": 0.09514068427942153, + "velocityX": -0.2824842396615403, + "velocityY": 1.5867562547338276, + "timestamp": 3.119147835588325 + }, + { + "x": 7.974290324535485, + "y": 4.556396142126618, + "heading": 3.207495211129814, + "angularVelocity": 0.09625651332906135, + "velocityX": -0.4031891706277398, + "velocityY": 1.7045009992367957, + "timestamp": 3.157349069397477 + }, + { + "x": 7.954134422332773, + "y": 4.625857308190122, + "heading": 3.2112121135725333, + "angularVelocity": 0.09729796847109581, + "velocityX": -0.5276243773541068, + "velocityY": 1.8182964039989622, + "timestamp": 3.195550303206629 + }, + { + "x": 7.929068570159579, + "y": 4.699488175513398, + "heading": 3.2149655529585908, + "angularVelocity": 0.09825440206483436, + "velocityX": -0.6561529477927303, + "velocityY": 1.9274473618083952, + "timestamp": 3.233751537015781 + }, + { + "x": 7.898921611054975, + "y": 4.7770783843465825, + "heading": 3.2187518021293573, + "angularVelocity": 0.09911326921224117, + "velocityX": -0.7891619222356322, + "velocityY": 2.031091697739763, + "timestamp": 3.271952770824933 + }, + { + "x": 7.863507259665322, + "y": 4.858376507302923, + "heading": 3.2225665713147067, + "angularVelocity": 0.09985984233932846, + "velocityX": -0.9270473191148296, + "velocityY": 2.1281543774867226, + "timestamp": 3.310154004634085 + }, + { + "x": 7.82262490744023, + "y": 4.943079777954261, + "heading": 3.226404915584542, + "angularVelocity": 0.10047697121541134, + "velocityX": -1.0701840791146968, + "velocityY": 2.217291490492266, + "timestamp": 3.348355238443237 + }, + { + "x": 7.776062535619055, + "y": 5.030821379568998, + "heading": 3.2302611382194035, + "angularVelocity": 0.1009449761263412, + "velocityX": -1.2188708891915252, + "velocityY": 2.2968263813960155, + "timestamp": 3.386556472252389 + }, + { + "x": 7.723603242812097, + "y": 5.121155356689003, + "heading": 3.23412870026526, + "angularVelocity": 0.10124180975877002, + "velocityX": -1.373235562731729, + "velocityY": 2.364687422696869, + "timestamp": 3.424757706061541 + }, + { + "x": 7.665037532882504, + "y": 5.213540129232238, + "heading": 3.2380001528589366, + "angularVelocity": 0.10134365327094359, + "velocityX": -1.5330842512097755, + "velocityY": 2.4183714328384474, + "timestamp": 3.462958939870693 + }, + { + "x": 7.600183883971553, + "y": 5.307323440188604, + "heading": 3.2418671133682277, + "angularVelocity": 0.10122606323685251, + "velocityX": -1.6976846673317019, + "velocityY": 2.4549812036148855, + "timestamp": 3.501160173679845 + }, + { + "x": 7.528919309283333, + "y": 5.401734523083348, + "heading": 3.2457203037789037, + "angularVelocity": 0.10086560109356, + "velocityX": -1.8655045291010028, + "velocityY": 2.4714144932178943, + "timestamp": 3.5393614074889967 + }, + { + "x": 7.451218127874509, + "y": 5.495892378933352, + "heading": 3.249549659566478, + "angularVelocity": 0.10024167823230529, + "velocityX": -2.0339966451609546, + "velocityY": 2.4647857270894256, + "timestamp": 3.5775626412981487 + }, + { + "x": 7.36719026389099, + "y": 5.588839011710391, + "heading": 3.253344517033594, + "angularVelocity": 0.09933861000601822, + "velocityX": -2.1996112587177326, + "velocityY": 2.433079340876453, + "timestamp": 3.6157638751073007 + }, + { + "x": 7.277103414955576, + "y": 5.6795987285633585, + "heading": 3.257093918809728, + "angularVelocity": 0.0981487088837405, + "velocityX": -2.3582183074367444, + "velocityY": 2.375832081926742, + "timestamp": 3.6539651089164527 + }, + { + "x": 7.181374686983581, + "y": 5.767249906226262, + "heading": 3.260787096257733, + "angularVelocity": 0.09667691537021512, + "velocityX": -2.5059067057949522, + "velocityY": 2.2944593386903587, + "timestamp": 3.6921663427256046 + }, + { + "x": 7.080531109805766, + "y": 5.8509854681858595, + "heading": 3.2644140920664264, + "angularVelocity": 0.09494446767906256, + "velocityX": -2.6397989573220397, + "velocityY": 2.1919596204125034, + "timestamp": 3.7303675765347566 + }, + { + "x": 6.97515470473837, + "y": 5.930144227513004, + "heading": 3.267966327996027, + "angularVelocity": 0.09298746598989815, + "velocityX": -2.758455540829955, + "velocityY": 2.0721519028053215, + "timestamp": 3.7685688103439086 + }, + { + "x": 6.8658318519592285, + "y": 6.004211902618408, + "heading": 3.271436911327556, + "angularVelocity": 0.09085003245884067, + "velocityX": -2.8617623536795467, + "velocityY": 1.9388817511872807, + "timestamp": 3.8067700441530605 + }, + { + "x": 6.76937026572913, + "y": 6.063823537641736, + "heading": 3.274355520630392, + "angularVelocity": 0.08889560431653577, + "velocityX": -2.9380468954592764, + "velocityY": 1.8156634786800647, + "timestamp": 3.839601918409111 + }, + { + "x": 6.670811763453878, + "y": 6.119164152757477, + "heading": 3.2771960124627224, + "angularVelocity": 0.08651628628259647, + "velocityX": -3.00191519700062, + "velocityY": 1.6855758731331847, + "timestamp": 3.8724337926651615 + }, + { + "x": 6.570666307865554, + "y": 6.170019228878687, + "heading": 3.2799422951123653, + "angularVelocity": 0.08364684355894975, + "velocityX": -3.0502509484322142, + "velocityY": 1.5489544009762712, + "timestamp": 3.905265666921212 + }, + { + "x": 6.46955818993848, + "y": 6.216214742131844, + "heading": 3.2825768229193018, + "angularVelocity": 0.08024299150240671, + "velocityX": -3.079571916563354, + "velocityY": 1.4070324737748963, + "timestamp": 3.9380975411772625 + }, + { + "x": 6.368220806913084, + "y": 6.257657843766639, + "heading": 3.285082327525109, + "angularVelocity": 0.07631317622219781, + "velocityX": -3.086554920230331, + "velocityY": 1.2622825401799327, + "timestamp": 3.970929415433313 + }, + { + "x": 6.267462404278832, + "y": 6.2943783920802225, + "heading": 3.2874445528955567, + "angularVelocity": 0.07194914771010016, + "velocityX": -3.068920216020964, + "velocityY": 1.118442036759947, + "timestamp": 4.003761289689364 + }, + { + "x": 6.1681022663365965, + "y": 6.326551148740925, + "heading": 3.289654959048111, + "angularVelocity": 0.06732500664797306, + "velocityX": -3.026331581539983, + "velocityY": 0.9799244602909629, + "timestamp": 4.0365931639454145 + }, + { + "x": 6.070897503286478, + "y": 6.354482225299535, + "heading": 3.2917117995402965, + "angularVelocity": 0.06264767208064463, + "velocityX": -2.960682728376521, + "velocityY": 0.8507304925932898, + "timestamp": 4.069425038201465 + }, + { + "x": 5.976489922312775, + "y": 6.378564495957317, + "heading": 3.2936190060486643, + "angularVelocity": 0.058090089328853824, + "velocityX": -2.875485579575279, + "velocityY": 0.7335027683758681, + "timestamp": 4.1022569124575154 + }, + { + "x": 5.885387749283588, + "y": 6.399224497871891, + "heading": 3.295383909871142, + "angularVelocity": 0.05375580476197676, + "velocityX": -2.774808782425772, + "velocityY": 0.6292666009089228, + "timestamp": 4.135088786713566 + }, + { + "x": 5.79797544997618, + "y": 6.4168807330266695, + "heading": 3.2970151433758272, + "angularVelocity": 0.049684446643635256, + "velocityX": -2.6624218473088823, + "velocityY": 0.5377772531985251, + "timestamp": 4.167920660969616 + }, + { + "x": 5.7145364982554065, + "y": 6.431919635509067, + "heading": 3.298521304612775, + "angularVelocity": 0.04587496970782373, + "velocityX": -2.541400806729675, + "velocityY": 0.4580579946521674, + "timestamp": 4.200752535225667 + }, + { + "x": 5.635277634199153, + "y": 6.444685893070414, + "heading": 3.2999103137042596, + "angularVelocity": 0.04230672549035185, + "velocityX": -2.4140828342033305, + "velocityY": 0.3888373067520809, + "timestamp": 4.233584409481717 + }, + { + "x": 5.5603494135057785, + "y": 6.455481443626585, + "heading": 3.301189197207777, + "angularVelocity": 0.038952497610812056, + "velocityX": -2.2821792051535352, + "velocityY": 0.3288131061900009, + "timestamp": 4.266416283737768 + }, + { + "x": 5.489861905204234, + "y": 6.4645686834525575, + "heading": 3.302364085244027, + "angularVelocity": 0.03578498221232499, + "velocityX": -2.146923070910393, + "velocityY": 0.27678102550899025, + "timestamp": 4.299248157993818 + }, + { + "x": 5.423896104976939, + "y": 6.472175218090503, + "heading": 3.3034402954817073, + "angularVelocity": 0.03277943346417197, + "velocityX": -2.009199953461021, + "velocityY": 0.23168140139134377, + "timestamp": 4.332080032249869 + }, + { + "x": 5.3625120552036805, + "y": 6.478498802228473, + "heading": 3.304422441816652, + "angularVelocity": 0.029914415707270804, + "velocityX": -1.8696480528201007, + "velocityY": 0.19260503036328994, + "timestamp": 4.364911906505919 + }, + { + "x": 5.305754584196663, + "y": 6.483711879532544, + "heading": 3.305314540782059, + "angularVelocity": 0.027171734347235434, + "velocityX": -1.728730762196986, + "velocityY": 0.15878098409538036, + "timestamp": 4.39774378076197 + }, + { + "x": 5.253657370247068, + "y": 6.487965524914462, + "heading": 3.3061201058429943, + "angularVelocity": 0.024536066831048555, + "velocityX": -1.5867876912325205, + "velocityY": 0.12955840865933993, + "timestamp": 4.43057565501802 + }, + { + "x": 5.206245838192873, + "y": 6.491392772869967, + "heading": 3.3068422273399647, + "angularVelocity": 0.021994525543636716, + "velocityX": -1.444070225307262, + "velocityY": 0.1043878253424375, + "timestamp": 4.463407529274071 + }, + { + "x": 5.163539242392539, + "y": 6.494111389192325, + "heading": 3.3074836389039146, + "angularVelocity": 0.019536245751546197, + "velocityX": -1.3007663061594368, + "velocityY": 0.08280417685432861, + "timestamp": 4.496239403530121 + }, + { + "x": 5.125552179765353, + "y": 6.496226165889178, + "heading": 3.308046772172593, + "angularVelocity": 0.01715202928369231, + "velocityX": -1.1570177910323254, + "velocityY": 0.06441230495586951, + "timestamp": 4.529071277786172 + }, + { + "x": 5.092295700352608, + "y": 6.497830819141238, + "heading": 3.308533801781415, + "angularVelocity": 0.01483404831001049, + "velocityX": -1.0129327114676103, + "velocityY": 0.04887485982510178, + "timestamp": 4.561903152042222 + }, + { + "x": 5.063778130929228, + "y": 6.499009561458986, + "heading": 3.308946682418035, + "angularVelocity": 0.012575603616169764, + "velocityX": -0.8685940132743156, + "velocityY": 0.035902376713398354, + "timestamp": 4.594735026298273 + }, + { + "x": 5.040005691939611, + "y": 6.499838408157472, + "heading": 3.3092871794520544, + "angularVelocity": 0.010370928913894002, + "velocityX": -0.7240658515020671, + "velocityY": 0.025245183751066175, + "timestamp": 4.627566900554323 + }, + { + "x": 5.020982964030012, + "y": 6.50038626757828, + "heading": 3.3095568943744427, + "angularVelocity": 0.008215032753991694, + "velocityX": -0.5793981714611864, + "velocityY": 0.01668681527396481, + "timestamp": 4.660398774810374 + }, + { + "x": 5.006713244009331, + "y": 6.500715855130694, + "heading": 3.3097572860387627, + "angularVelocity": 0.00610357065688574, + "velocityX": -0.4346300765345807, + "velocityY": 0.010038645672288071, + "timestamp": 4.693230649066424 + }, + { + "x": 4.997198818717405, + "y": 6.500884463422624, + "heading": 3.30988968849574, + "angularVelocity": 0.004032741352049697, + "velocityX": -0.2897923285684355, + "velocityY": 0.005135506143067766, + "timestamp": 4.726062523322475 + }, + { + "x": 4.992441177368164, + "y": 6.5009446144104, + "heading": 3.309955326051445, + "angularVelocity": 0.001999202214064957, + "velocityX": -0.14490922181708896, + "velocityY": 0.0018320911961180106, + "timestamp": 4.758894397578525 + }, + { + "x": 4.992441177368164, + "y": 6.5009446144104, + "heading": 3.309955326051445, + "angularVelocity": 5.207192621874317e-25, + "velocityX": 4.1285632704076663e-23, + "velocityY": -5.50747176857669e-23, + "timestamp": 4.791726271834576 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.302428960800171, + "y": 5.521671772003174, + "heading": 3.119698237349717, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 0.662288788336051, + "isStopPoint": false, + "x": 2.3668556213378906, + "y": 5.5500569343566895, + "heading": 3.1232461040248376, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 30 + }, + { + "timestamp": 1.2552013544388567, + "isStopPoint": false, + "x": 4.453131675720215, + "y": 5.067516803741455, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "timestamp": 1.756294247994321, + "isStopPoint": false, + "x": 5.801405429840088, + "y": 4.017282485961914, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 30 + }, + { + "timestamp": 2.6607330298785015, + "isStopPoint": false, + "x": 7.845104217529297, + "y": 4.088244438171387, + "heading": 3.163567121665249, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 30 + }, + { + "timestamp": 3.8067700441530605, + "isStopPoint": false, + "x": 6.8658318519592285, + "y": 6.004211902618408, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 30 + }, + { + "timestamp": 4.791726271834576, + "isStopPoint": true, + "x": 4.992441177368164, + "y": 6.5009446144104, + "heading": 3.309955326051445, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "WptVelocityDirection", + "direction": 0 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], "isTrajectoryStale": false } }, diff --git a/Robot/src/main/deploy/choreo/New Path (1).1.traj b/Robot/src/main/deploy/choreo/New Path (1).1.traj new file mode 100644 index 0000000..3f21b2e --- /dev/null +++ b/Robot/src/main/deploy/choreo/New Path (1).1.traj @@ -0,0 +1,1220 @@ +{ + "samples": [ + { + "x": 1.373390793800354, + "y": 5.5500569343566895, + "heading": -3.14, + "angularVelocity": -7.792493373112414e-25, + "velocityX": -1.4675606988248466e-25, + "velocityY": 4.296548304526963e-25, + "timestamp": 0 + }, + { + "x": 1.4008162818871832, + "y": 5.548332467221767, + "heading": -3.137847865481668, + "angularVelocity": 0.027272567199688357, + "velocityX": 0.34754494222415555, + "velocityY": -0.021853023321830287, + "timestamp": 0.07891206216760503 + }, + { + "x": 1.4556672566846394, + "y": 5.544883518010237, + "heading": -3.133543459938688, + "angularVelocity": 0.054546864252994244, + "velocityX": 0.6950898670086171, + "velocityY": -0.04370623598969305, + "timestamp": 0.15782412433521006 + }, + { + "x": 1.5379437150886541, + "y": 5.539710064387518, + "heading": -3.127086401796681, + "angularVelocity": 0.08182599674422604, + "velocityX": 1.0426347524572848, + "velocityY": -0.06555973168882138, + "timestamp": 0.23673618650281508 + }, + { + "x": 1.6476456518138687, + "y": 5.532812078143502, + "heading": -3.1184760134609104, + "angularVelocity": 0.10911371594221948, + "velocityX": 1.3901795709281257, + "velocityY": -0.08741358487583052, + "timestamp": 0.31564824867042013 + }, + { + "x": 1.7847730583095482, + "y": 5.524189529642618, + "heading": -3.10771125358726, + "angularVelocity": 0.13641463140053903, + "velocityX": 1.7377242810411937, + "velocityY": -0.10926781361474407, + "timestamp": 0.3945603108380252 + }, + { + "x": 1.949325918220542, + "y": 5.513842409726839, + "heading": -3.0947906414967634, + "angularVelocity": 0.16373431051711962, + "velocityX": 2.0852687838963337, + "velocityY": -0.1311221584071907, + "timestamp": 0.4734723730056302 + }, + { + "x": 2.1090388863997416, + "y": 5.523857134967481, + "heading": -3.1017736522087302, + "angularVelocity": -0.08849104332281946, + "velocityX": 2.0239360598634204, + "velocityY": 0.1269099420994908, + "timestamp": 0.5523844351732352 + }, + { + "x": 2.2413264008507308, + "y": 5.535596537344446, + "heading": -3.110910463319114, + "angularVelocity": -0.11578472111117445, + "velocityX": 1.6763915530431484, + "velocityY": 0.14876562662918416, + "timestamp": 0.6312964973408403 + }, + { + "x": 2.3461884403926034, + "y": 5.54906052840806, + "heading": -3.122199828492689, + "angularVelocity": -0.14306260492340278, + "velocityX": 1.3288467778113737, + "velocityY": 0.17062019029507766, + "timestamp": 0.7102085595084453 + }, + { + "x": 2.4236249923706055, + "y": 5.564249038696289, + "heading": -3.135640447743494, + "angularVelocity": -0.1703240148794674, + "velocityX": 0.9813018422143182, + "velocityY": 0.1924738737148937, + "timestamp": 0.7891206216760503 + }, + { + "x": 2.4236249924844095, + "y": 5.564249038718611, + "heading": -3.135640447763247, + "angularVelocity": -0.17032401487979337, + "velocityX": 0.9813018422127583, + "velocityY": 0.19247387371505806, + "timestamp": 0.7891206217920228 + }, + { + "x": 2.4236249925982136, + "y": 5.5642490387409325, + "heading": -3.135640447783, + "angularVelocity": -0.17032401488011933, + "velocityX": 0.9813018422113264, + "velocityY": 0.19247387371524752, + "timestamp": 0.7891206219079953 + }, + { + "x": 2.4236249927120177, + "y": 5.564249038763254, + "heading": -3.135640447802753, + "angularVelocity": -0.17032401488044527, + "velocityX": 0.9813018422100225, + "velocityY": 0.19247387371546204, + "timestamp": 0.7891206220239677 + }, + { + "x": 2.4236249928258218, + "y": 5.564249038785576, + "heading": -3.135640447822506, + "angularVelocity": -0.17032401488077123, + "velocityX": 0.9813018422088465, + "velocityY": 0.19247387371570168, + "timestamp": 0.7891206221399402 + }, + { + "x": 2.423624992939626, + "y": 5.564249038807898, + "heading": -3.1356404478422593, + "angularVelocity": -0.1703240148810972, + "velocityX": 0.9813018422077986, + "velocityY": 0.19247387371596644, + "timestamp": 0.7891206222559126 + }, + { + "x": 2.42362499305343, + "y": 5.564249038830219, + "heading": -3.1356404478620123, + "angularVelocity": -0.17032401488142315, + "velocityX": 0.9813018422068785, + "velocityY": 0.1924738737162563, + "timestamp": 0.7891206223718851 + }, + { + "x": 2.423624993167234, + "y": 5.564249038852541, + "heading": -3.1356404478817654, + "angularVelocity": -0.17032401488174911, + "velocityX": 0.9813018422060865, + "velocityY": 0.19247387371657124, + "timestamp": 0.7891206224878575 + }, + { + "x": 2.423624993281038, + "y": 5.564249038874863, + "heading": -3.1356404479015185, + "angularVelocity": -0.17032401488207508, + "velocityX": 0.9813018422054224, + "velocityY": 0.19247387371691124, + "timestamp": 0.78912062260383 + }, + { + "x": 2.423624993394842, + "y": 5.564249038897184, + "heading": -3.1356404479212716, + "angularVelocity": -0.170324014882401, + "velocityX": 0.9813018422048863, + "velocityY": 0.1924738737172764, + "timestamp": 0.7891206227198024 + }, + { + "x": 2.4236249935086462, + "y": 5.564249038919506, + "heading": -3.1356404479410247, + "angularVelocity": -0.17032401488272697, + "velocityX": 0.981301842204478, + "velocityY": 0.19247387371766664, + "timestamp": 0.7891206228357749 + }, + { + "x": 2.4236249936224503, + "y": 5.564249038941828, + "heading": -3.135640447960778, + "angularVelocity": -0.17032401488305293, + "velocityX": 0.9813018422041978, + "velocityY": 0.19247387371808197, + "timestamp": 0.7891206229517473 + }, + { + "x": 2.4236249937362544, + "y": 5.5642490389641495, + "heading": -3.135640447980531, + "angularVelocity": -0.1703240148833789, + "velocityX": 0.9813018422040455, + "velocityY": 0.1924738737185224, + "timestamp": 0.7891206230677198 + }, + { + "x": 2.4236249938500585, + "y": 5.564249038986471, + "heading": -3.135640448000284, + "angularVelocity": -0.17032401488370486, + "velocityX": 0.9813018422040212, + "velocityY": 0.19247387371898791, + "timestamp": 0.7891206231836922 + }, + { + "x": 2.4236249939638626, + "y": 5.564249039008793, + "heading": -3.135640448020037, + "angularVelocity": -0.1703240148840308, + "velocityX": 0.9813018422041248, + "velocityY": 0.19247387371947852, + "timestamp": 0.7891206232996647 + }, + { + "x": 2.4236249940776666, + "y": 5.564249039031115, + "heading": -3.13564044803979, + "angularVelocity": -0.17032401488435675, + "velocityX": 0.9813018422043563, + "velocityY": 0.19247387371999425, + "timestamp": 0.7891206234156372 + }, + { + "x": 2.4236249941914707, + "y": 5.564249039053436, + "heading": -3.1356404480595432, + "angularVelocity": -0.1703240148846827, + "velocityX": 0.9813018422047158, + "velocityY": 0.19247387372053507, + "timestamp": 0.7891206235316096 + }, + { + "x": 2.423624994305275, + "y": 5.564249039075758, + "heading": -3.1356404480792963, + "angularVelocity": -0.17032401488500867, + "velocityX": 0.9813018422052033, + "velocityY": 0.19247387372110097, + "timestamp": 0.7891206236475821 + }, + { + "x": 2.423624994419079, + "y": 5.56424903909808, + "heading": -3.1356404480990494, + "angularVelocity": -0.17032401488533463, + "velocityX": 0.9813018422058187, + "velocityY": 0.192473873721692, + "timestamp": 0.7891206237635545 + }, + { + "x": 2.423624994532883, + "y": 5.564249039120401, + "heading": -3.1356404481188025, + "angularVelocity": -0.1703240148856606, + "velocityX": 0.9813018422065621, + "velocityY": 0.19247387372230812, + "timestamp": 0.789120623879527 + }, + { + "x": 2.423624994646687, + "y": 5.564249039142723, + "heading": -3.1356404481385556, + "angularVelocity": -0.17032401488598653, + "velocityX": 0.9813018422074334, + "velocityY": 0.19247387372294933, + "timestamp": 0.7891206239954994 + }, + { + "x": 2.423624994760491, + "y": 5.564249039165045, + "heading": -3.1356404481583087, + "angularVelocity": -0.1703240148863125, + "velocityX": 0.9813018422084328, + "velocityY": 0.19247387372361563, + "timestamp": 0.7891206241114719 + }, + { + "x": 2.423624994874295, + "y": 5.5642490391873665, + "heading": -3.1356404481780618, + "angularVelocity": -0.17032401488663845, + "velocityX": 0.98130184220956, + "velocityY": 0.192473873724307, + "timestamp": 0.7891206242274443 + }, + { + "x": 2.4236249949880992, + "y": 5.564249039209688, + "heading": -3.135640448197815, + "angularVelocity": -0.17032401488696441, + "velocityX": 0.9813018422108152, + "velocityY": 0.1924738737250235, + "timestamp": 0.7891206243434168 + }, + { + "x": 2.4236249951019033, + "y": 5.56424903923201, + "heading": -3.135640448217568, + "angularVelocity": -0.17032401488729038, + "velocityX": 0.9813018422121984, + "velocityY": 0.1924738737257651, + "timestamp": 0.7891206244593892 + }, + { + "x": 2.4236249952157074, + "y": 5.564249039254332, + "heading": -3.135640448237321, + "angularVelocity": -0.1703240148876163, + "velocityX": 0.9813018422137095, + "velocityY": 0.1924738737265318, + "timestamp": 0.7891206245753617 + }, + { + "x": 2.4236249953295115, + "y": 5.564249039276653, + "heading": -3.135640448257074, + "angularVelocity": -0.17032401488794227, + "velocityX": 0.9813018422153487, + "velocityY": 0.19247387372732358, + "timestamp": 0.7891206246913341 + }, + { + "x": 2.4236249954433156, + "y": 5.564249039298975, + "heading": -3.135640448276827, + "angularVelocity": -0.17032401488826823, + "velocityX": 0.9813018422171157, + "velocityY": 0.19247387372814045, + "timestamp": 0.7891206248073066 + }, + { + "x": 2.5202131403633516, + "y": 5.5885147910320585, + "heading": -3.150117025303126, + "angularVelocity": -0.19273984769703859, + "velocityX": 1.2859658956262894, + "velocityY": 0.3230720414634121, + "timestamp": 0.8642300388660642 + }, + { + "x": 2.6397927701444233, + "y": 5.62233325234703, + "heading": -3.166259274426774, + "angularVelocity": -0.21491645655789715, + "velocityX": 1.5920724622818327, + "velocityY": 0.4502559597724382, + "timestamp": 0.9393394529248218 + }, + { + "x": 2.782492954308911, + "y": 5.665387996339145, + "heading": -3.184044486118167, + "angularVelocity": -0.23679071277909178, + "velocityX": 1.8998974489782852, + "velocityY": 0.5732269986613667, + "timestamp": 1.0144488669835794 + }, + { + "x": 2.948469657430915, + "y": 5.717279062041011, + "heading": -3.2034430209640345, + "angularVelocity": -0.25827035251122976, + "velocityX": 2.2097989340212596, + "velocityY": 0.6908729931147001, + "timestamp": 1.089558281042337 + }, + { + "x": 3.13791427133203, + "y": 5.777485440384115, + "heading": -3.2244148248769338, + "angularVelocity": -0.27921671571679574, + "velocityX": 2.5222485926053704, + "velocityY": 0.8015823195745534, + "timestamp": 1.1646676951010946 + }, + { + "x": 3.3510652321882093, + "y": 5.845301698736286, + "heading": -3.2469035661389745, + "angularVelocity": -0.29941308348441903, + "velocityX": 2.837872769043742, + "velocityY": 0.9028995792607077, + "timestamp": 1.2397771091598522 + }, + { + "x": 3.5882224140431322, + "y": 5.919722646911708, + "heading": -3.2708260554869257, + "angularVelocity": -0.3185018768650853, + "velocityX": 3.157489441595122, + "velocityY": 0.9908338269980898, + "timestamp": 1.3148865232186098 + }, + { + "x": 3.849756972147724, + "y": 5.999212365002834, + "heading": -3.296051340937575, + "angularVelocity": -0.33584718729021185, + "velocityX": 3.482047641857447, + "velocityY": 1.0583189748883177, + "timestamp": 1.3899959372773674 + }, + { + "x": 4.136067397800634, + "y": 6.081181645403884, + "heading": -3.322354225031387, + "angularVelocity": -0.3501942389436787, + "velocityX": 3.811911319517566, + "velocityY": 1.091331645017572, + "timestamp": 1.465105351336125 + }, + { + "x": 4.447151871291085, + "y": 6.160640800595462, + "heading": -3.34929544790659, + "angularVelocity": -0.3586930242076843, + "velocityX": 4.141750769711674, + "velocityY": 1.0579120631858112, + "timestamp": 1.5402147653948826 + }, + { + "x": 4.779482954645891, + "y": 6.227113094753417, + "heading": -3.37586605695639, + "angularVelocity": -0.3537587049875555, + "velocityX": 4.424626227210678, + "velocityY": 0.8850061605587058, + "timestamp": 1.6153241794536402 + }, + { + "x": 5.117259626240433, + "y": 6.269293967456475, + "heading": -3.40016260033537, + "angularVelocity": -0.32348199867427174, + "velocityX": 4.497128300459125, + "velocityY": 0.5615923547221425, + "timestamp": 1.6904335935123977 + }, + { + "x": 5.446596145629883, + "y": 6.288059234619141, + "heading": -3.421549469666226, + "angularVelocity": -0.2847428594520122, + "velocityX": 4.384756871246697, + "velocityY": 0.24983908339353045, + "timestamp": 1.7655430075711553 + }, + { + "x": 5.583978781229968, + "y": 6.292316694982243, + "heading": -3.430131520120695, + "angularVelocity": -0.2689620281221212, + "velocityX": 4.305580874384169, + "velocityY": 0.1334290890020681, + "timestamp": 1.7974510449784737 + }, + { + "x": 5.718326508347156, + "y": 6.293262578004624, + "heading": -3.4381858559860787, + "angularVelocity": -0.25242341804249496, + "velocityX": 4.210466642062215, + "velocityY": 0.02964403640080957, + "timestamp": 1.8293590823857921 + }, + { + "x": 5.849241399946558, + "y": 6.291311612489797, + "heading": -3.4457039588897924, + "angularVelocity": -0.23561784160343793, + "velocityX": 4.102881350182191, + "velocityY": -0.061143388103806366, + "timestamp": 1.8612671197931105 + }, + { + "x": 5.976424364300466, + "y": 6.28686095808839, + "heading": -3.452687518488142, + "angularVelocity": -0.21886521910455456, + "velocityX": 3.9859225037995363, + "velocityY": -0.13948380292379126, + "timestamp": 1.893175157200429 + }, + { + "x": 6.0996561002514555, + "y": 6.280274035067278, + "heading": -3.459143915120258, + "angularVelocity": -0.20234389692156543, + "velocityX": 3.8620907446575, + "velocityY": -0.20643460257448376, + "timestamp": 1.9250831946077473 + }, + { + "x": 6.21877796918791, + "y": 6.271874588037581, + "heading": -3.4650831978385432, + "angularVelocity": -0.18613751270466306, + "velocityX": 3.7332872409486546, + "velocityY": -0.26323922472808714, + "timestamp": 1.9569912320150658 + }, + { + "x": 6.33367594677285, + "y": 6.2619468977702875, + "heading": -3.4705163067965152, + "angularVelocity": -0.1702739936215994, + "velocityX": 3.6009102069871273, + "velocityY": -0.3111344687409986, + "timestamp": 1.9888992694223842 + }, + { + "x": 6.444268295780197, + "y": 6.250738999348943, + "heading": -3.475454120537259, + "angularVelocity": -0.15475140879743604, + "velocityX": 3.465971522961229, + "velocityY": -0.3512562768518353, + "timestamp": 2.0208073068297026 + }, + { + "x": 6.5504965143251574, + "y": 6.238466994593223, + "heading": -3.4799069914164837, + "angularVelocity": -0.13955326748499272, + "velocityX": 3.3291993860015867, + "velocityY": -0.3846054396597071, + "timestamp": 2.052715344237021 + }, + { + "x": 6.6523188314701445, + "y": 6.225319459420252, + "heading": -3.483884551234072, + "angularVelocity": -0.1246569874170926, + "velocityX": 3.191118145098868, + "velocityY": -0.4120446207686626, + "timestamp": 2.08462338164434 + }, + { + "x": 6.749705571819693, + "y": 6.211461495939882, + "heading": -3.487395659176523, + "angularVelocity": -0.11003835483926433, + "velocityX": 3.0521068753420764, + "velocityY": -0.4343094908492185, + "timestamp": 2.1165314190516584 + }, + { + "x": 6.842635853833305, + "y": 6.197038271774381, + "heading": -3.4904484207431485, + "angularVelocity": -0.0956737491452672, + "velocityX": 2.912441176726728, + "velocityY": -0.4520247980589766, + "timestamp": 2.148439456458977 + }, + { + "x": 6.931095228941555, + "y": 6.182178030589835, + "heading": -3.4930502391028706, + "angularVelocity": -0.08154115925428285, + "velocityX": 2.7723226589912517, + "velocityY": -0.4657209403025567, + "timestamp": 2.1803474938662957 + }, + { + "x": 7.015073982675889, + "y": 6.1669946184502, + "heading": -3.4952078785818306, + "angularVelocity": -0.06762056379139442, + "velocityX": 2.6318996891696274, + "velocityY": -0.4758491393818042, + "timestamp": 2.2122555312736143 + }, + { + "x": 7.0945659029317385, + "y": 6.151589590913828, + "heading": -3.4969275299383775, + "angularVelocity": -0.05389398710408752, + "velocityX": 2.4912820315804582, + "velocityY": -0.4827945805541645, + "timestamp": 2.244163568680933 + }, + { + "x": 7.169567379757041, + "y": 6.136053967419142, + "heading": -3.498214872438713, + "angularVelocity": -0.040345398994690515, + "velocityX": 2.350551237855201, + "velocityY": -0.4868874665140758, + "timestamp": 2.2760716060882515 + }, + { + "x": 7.240076742171475, + "y": 6.120469693395241, + "heading": -3.499075130588546, + "angularVelocity": -0.026960547239283258, + "velocityX": 2.2097680754962985, + "velocityY": -0.48841217731324, + "timestamp": 2.30797964349557 + }, + { + "x": 7.306093765850792, + "y": 6.104910861957847, + "heading": -3.4995131248526885, + "angularVelocity": -0.013726769169506972, + "velocityX": 2.068977882800654, + "velocityY": -0.48761480497154164, + "timestamp": 2.339887680902889 + }, + { + "x": 7.367619305030031, + "y": 6.089444738394438, + "heading": -3.4995333164334084, + "angularVelocity": -0.000632805473504129, + "velocityX": 1.9282144618875081, + "velocityY": -0.48470933407710726, + "timestamp": 2.3717957183102074 + }, + { + "x": 7.424655015480076, + "y": 6.074132622864405, + "heading": -3.4991398465215826, + "angularVelocity": 0.012331373026894028, + "velocityX": 1.787502932943278, + "velocityY": -0.4798827121382698, + "timestamp": 2.403703755717526 + }, + { + "x": 7.477203144806779, + "y": 6.059030580127337, + "heading": -3.498336570568695, + "angularVelocity": 0.025174721423108552, + "velocityX": 1.6468618441148708, + "velocityY": -0.473299016930574, + "timestamp": 2.4356117931248447 + }, + { + "x": 7.525266372907712, + "y": 6.044190059652854, + "heading": -3.4971270881539924, + "angularVelocity": 0.03790525876796509, + "velocityX": 1.5063047434534294, + "velocityY": -0.46510289194657173, + "timestamp": 2.4675198305321633 + }, + { + "x": 7.568847690075842, + "y": 6.029658425029338, + "heading": -3.4955147689946737, + "angularVelocity": 0.05053018895323732, + "velocityX": 1.365841358771074, + "velocityY": -0.4554223889740649, + "timestamp": 2.499427867939482 + }, + { + "x": 7.607950303556081, + "y": 6.015479408015207, + "heading": -3.4935027755974737, + "angularVelocity": 0.0630560059685356, + "velocityX": 1.225478489356149, + "velocityY": -0.44437132980415467, + "timestamp": 2.5313359053468005 + }, + { + "x": 7.642577565744241, + "y": 6.001693499708621, + "heading": -3.4910940829929844, + "angularVelocity": 0.07548858532857207, + "velocityX": 1.0852206842473227, + "velocityY": -0.43205127694331946, + "timestamp": 2.563243942754119 + }, + { + "x": 7.672732918945103, + "y": 5.988338289012847, + "heading": -3.4882914959374425, + "angularVelocity": 0.08783326344287469, + "velocityX": 0.9450707611977816, + "velocityY": -0.41855318537111263, + "timestamp": 2.5951519801614378 + }, + { + "x": 7.698419852868165, + "y": 5.975448756730407, + "heading": -3.4850976639142544, + "angularVelocity": 0.10009490657219974, + "velocityX": 0.8050302058744196, + "velocityY": -0.40395879313730104, + "timestamp": 2.6270600175687564 + }, + { + "x": 7.71964187196861, + "y": 5.963057532136706, + "heading": -3.4815150942206845, + "angularVelocity": 0.11227797083967467, + "velocityX": 0.6650994804079722, + "velocityY": -0.38834179725697887, + "timestamp": 2.658968054976075 + }, + { + "x": 7.736402470430254, + "y": 5.951195117688826, + "heading": -3.477546163384214, + "angularVelocity": 0.12438655457888602, + "velocityX": 0.5252782628930857, + "velocityY": -0.3717688523569133, + "timestamp": 2.6908760923833936 + }, + { + "x": 7.74870511310215, + "y": 5.939890086558444, + "heading": -3.473193127117835, + "angularVelocity": 0.13642444412392482, + "velocityX": 0.38556563397643184, + "velocityY": -0.3543004223691105, + "timestamp": 2.7227841297907123 + }, + { + "x": 7.756553221087926, + "y": 5.929169256892984, + "heading": -3.4684581289933885, + "angularVelocity": 0.14839515398588157, + "velocityX": 0.24596022267342674, + "velocityY": -0.33599150986958115, + "timestamp": 2.754692167198031 + }, + { + "x": 7.759950160980225, + "y": 5.919057846069336, + "heading": -3.4633432079864352, + "angularVelocity": 0.16030196221909535, + "velocityX": 0.10646032060622376, + "velocityY": -0.31689228311261575, + "timestamp": 2.7866002046053495 + }, + { + "x": 7.740992810070082, + "y": 5.897866293760295, + "heading": -3.4483259795142858, + "angularVelocity": 0.18966965117669482, + "velocityX": -0.23943393689649056, + "velocityY": -0.26765220638433596, + "timestamp": 2.865775910254534 + }, + { + "x": 7.6946750115513405, + "y": 5.880751239353706, + "heading": -3.430977462853066, + "angularVelocity": 0.21911414011373867, + "velocityX": -0.5850001353188997, + "velocityY": -0.2161654798812024, + "timestamp": 2.944951615903719 + }, + { + "x": 7.621031262251048, + "y": 5.867937342014943, + "heading": -3.4112891061492308, + "angularVelocity": 0.2486666401316515, + "velocityX": -0.9301306340937104, + "velocityY": -0.16184127736782786, + "timestamp": 3.0241273215529034 + }, + { + "x": 7.520109336525543, + "y": 5.85971721387783, + "heading": -3.389248427591031, + "angularVelocity": 0.2783767871404717, + "velocityX": -1.2746577362085507, + "velocityY": -0.10382134355121005, + "timestamp": 3.103303027202088 + }, + { + "x": 7.39197917102874, + "y": 5.8564875757937624, + "heading": -3.3648366324448036, + "angularVelocity": 0.308324314208095, + "velocityX": -1.6183015288114875, + "velocityY": -0.04079077107790146, + "timestamp": 3.1824787328512727 + }, + { + "x": 7.236751317240548, + "y": 5.858816377779796, + "heading": -3.338024361207094, + "angularVelocity": 0.3386426558231255, + "velocityX": -1.9605490410907365, + "velocityY": 0.029413087852372996, + "timestamp": 3.2616544385004573 + }, + { + "x": 7.054621391051907, + "y": 5.867582559401864, + "heading": -3.3087631347222772, + "angularVelocity": 0.3695732958095547, + "velocityX": -2.3003258978913266, + "velocityY": 0.11071807381053804, + "timestamp": 3.340830144149642 + }, + { + "x": 6.846004001197015, + "y": 5.884320000838926, + "heading": -3.2769649631981674, + "angularVelocity": 0.40161525891544314, + "velocityX": -2.63486618962696, + "velocityY": 0.21139617638803204, + "timestamp": 3.4200058497988266 + }, + { + "x": 6.612122032954981, + "y": 5.912316015175021, + "heading": -3.242440238754101, + "angularVelocity": 0.43605199550781415, + "velocityX": -2.953961272897621, + "velocityY": 0.3535934931876858, + "timestamp": 3.4991815554480112 + }, + { + "x": 6.360418430236016, + "y": 5.961461610916641, + "heading": -3.204725258846095, + "angularVelocity": 0.4763453587027727, + "velocityX": -3.1790509557846622, + "velocityY": 0.6207156013155894, + "timestamp": 3.578357261097196 + }, + { + "x": 6.128768619413155, + "y": 6.029673284245218, + "heading": -3.1672334777487867, + "angularVelocity": 0.4735263271719758, + "velocityX": -2.9257688191534936, + "velocityY": 0.861522771022888, + "timestamp": 3.6575329667463805 + }, + { + "x": 5.923854263987644, + "y": 6.105004809926703, + "heading": -3.1312739726613037, + "angularVelocity": 0.45417347142839426, + "velocityX": -2.5880963579087672, + "velocityY": 0.9514474808126816, + "timestamp": 3.736708672395565 + }, + { + "x": 5.746388049675664, + "y": 6.183799530006089, + "heading": -3.0971258429522432, + "angularVelocity": 0.4312955524560246, + "velocityX": -2.241422578515493, + "velocityY": 0.9951881001037627, + "timestamp": 3.81588437804475 + }, + { + "x": 5.596527243008144, + "y": 6.264409866427464, + "heading": -3.0649099526354786, + "angularVelocity": 0.4068911044444387, + "velocityX": -1.8927625013098104, + "velocityY": 1.0181195830264693, + "timestamp": 3.8950600836939344 + }, + { + "x": 5.47431680968178, + "y": 6.34590840516807, + "heading": -3.034696195300985, + "angularVelocity": 0.38160389082436225, + "velocityX": -1.5435345011998953, + "velocityY": 1.029337699896412, + "timestamp": 3.974235789343119 + }, + { + "x": 5.37976915844747, + "y": 6.427702267182384, + "heading": -3.0065318546626574, + "angularVelocity": 0.35571947742555177, + "velocityX": -1.1941497768676173, + "velocityY": 1.0330676732674617, + "timestamp": 4.053411494992304 + }, + { + "x": 5.312885421758628, + "y": 6.509380270776351, + "heading": -2.9804521557080004, + "angularVelocity": 0.3293901676129292, + "velocityX": -0.8447507494937071, + "velocityY": 1.0316043655596845, + "timestamp": 4.132587200641488 + }, + { + "x": 5.273662535323598, + "y": 6.590640683149381, + "heading": -2.9564850016577298, + "angularVelocity": 0.30270843630324745, + "velocityX": -0.49539042454286053, + "velocityY": 1.026330131278943, + "timestamp": 4.211762906290673 + }, + { + "x": 5.262095928192139, + "y": 6.671252727508545, + "heading": -2.9346533643249577, + "angularVelocity": 0.2757365678495407, + "velocityX": -0.14608783131922698, + "velocityY": 1.0181411545145314, + "timestamp": 4.290938611939858 + }, + { + "x": 5.286450146462021, + "y": 6.764484077147902, + "heading": -2.9120502053248636, + "angularVelocity": 0.24386251496431138, + "velocityX": 0.26275446353576026, + "velocityY": 1.0058607912494006, + "timestamp": 4.383626735833773 + }, + { + "x": 5.34869261123642, + "y": 6.856387459090783, + "heading": -2.8924677533563106, + "angularVelocity": 0.2112725033788139, + "velocityX": 0.671525780860962, + "velocityY": 0.9915335221162508, + "timestamp": 4.4763148597276885 + }, + { + "x": 5.448813085425695, + "y": 6.946710159976592, + "heading": -2.8759905639386316, + "angularVelocity": 0.17777023339621403, + "velocityX": 1.080186651570012, + "velocityY": 0.9744797617134475, + "timestamp": 4.569002983621604 + }, + { + "x": 5.586794422356268, + "y": 7.035098884885215, + "heading": -2.8627324007346533, + "angularVelocity": 0.1430405821910139, + "velocityX": 1.4886625290689568, + "velocityY": 0.9536143488003591, + "timestamp": 4.661691107515519 + }, + { + "x": 5.762604761098077, + "y": 7.121024926575688, + "heading": -2.8528585427257678, + "angularVelocity": 0.10652775775444928, + "velocityX": 1.8967946631763621, + "velocityY": 0.9270447828765889, + "timestamp": 4.754379231409435 + }, + { + "x": 5.976174763029016, + "y": 7.2036110783097, + "heading": -2.8466380444560433, + "angularVelocity": 0.06711213916514423, + "velocityX": 2.3041787119931025, + "velocityY": 0.8910111486185037, + "timestamp": 4.84706735530335 + }, + { + "x": 6.227306274033663, + "y": 7.2811226036526095, + "heading": -2.8445996856753095, + "angularVelocity": 0.021991585276523277, + "velocityX": 2.709424902073476, + "velocityY": 0.8362616707144066, + "timestamp": 4.939755479197266 + }, + { + "x": 6.514972208251902, + "y": 7.348599798549223, + "heading": -2.8482807276605207, + "angularVelocity": -0.039714278707639744, + "velocityX": 3.103589997651488, + "velocityY": 0.7280025969005842, + "timestamp": 5.032443603091181 + }, + { + "x": 6.803286438858483, + "y": 7.378323009778229, + "heading": -2.8654797905272513, + "angularVelocity": -0.1855584312658597, + "velocityX": 3.110584382272814, + "velocityY": 0.3206798236959108, + "timestamp": 5.125131726985097 + }, + { + "x": 7.05515266695702, + "y": 7.39757835383594, + "heading": -2.8830786743462165, + "angularVelocity": -0.18987204702845675, + "velocityX": 2.717351668341086, + "velocityY": 0.2077433790735653, + "timestamp": 5.217819850879012 + }, + { + "x": 7.269495335698046, + "y": 7.411362471913543, + "heading": -2.899617904375188, + "angularVelocity": -0.17843958140636978, + "velocityX": 2.312514912766473, + "velocityY": 0.1487150402718173, + "timestamp": 5.310507974772928 + }, + { + "x": 7.446102948393076, + "y": 7.421417571603764, + "heading": -2.9145959892958864, + "angularVelocity": -0.1615965917903412, + "velocityX": 1.905396347186422, + "velocityY": 0.10848315045980522, + "timestamp": 5.403196098666843 + }, + { + "x": 7.584899849507656, + "y": 7.428623463558208, + "heading": -2.927761388273211, + "angularVelocity": -0.14203976112833264, + "velocityX": 1.4974615439777093, + "velocityY": 0.07774342226077013, + "timestamp": 5.4958842225607585 + }, + { + "x": 7.685850480593157, + "y": 7.433510011721959, + "heading": -2.938963494323689, + "angularVelocity": -0.12085805149426576, + "velocityX": 1.0891431053350733, + "velocityY": 0.05272032660132098, + "timestamp": 5.588572346454674 + }, + { + "x": 7.748935268502705, + "y": 7.436431089750518, + "heading": -2.948102141289854, + "angularVelocity": -0.09859566233776026, + "velocityX": 0.6806134945804951, + "velocityY": 0.03151512735226398, + "timestamp": 5.681260470348589 + }, + { + "x": 7.774142265319824, + "y": 7.4376397132873535, + "heading": -2.9551058096712683, + "angularVelocity": -0.07556165868057244, + "velocityX": 0.27195497932366575, + "velocityY": 0.013039680663066213, + "timestamp": 5.773948594242505 + }, + { + "x": 7.7581383251743805, + "y": 7.437204402709091, + "heading": -2.960077030928164, + "angularVelocity": -0.05046428755227366, + "velocityX": -0.16246056969378117, + "velocityY": -0.004418962074057982, + "timestamp": 5.872458282935643 + }, + { + "x": 7.699340240644664, + "y": 7.4350492376719135, + "heading": -2.9625771572415784, + "angularVelocity": -0.025379496642227335, + "velocityX": -0.5968761581703449, + "velocityY": -0.021877696151197678, + "timestamp": 5.970967971628781 + }, + { + "x": 7.597748009323179, + "y": 7.431174217705324, + "heading": -2.962606855741692, + "angularVelocity": -0.0003014779612800365, + "velocityX": -1.0312917710860698, + "velocityY": -0.03933643500447566, + "timestamp": 6.069477660321919 + }, + { + "x": 7.453361630377404, + "y": 7.4255793508521934, + "heading": -2.9601662281215004, + "angularVelocity": 0.02477550840500792, + "velocityX": -1.4657073924529878, + "velocityY": -0.05679509221229112, + "timestamp": 6.167987349015057 + }, + { + "x": 7.266181104848686, + "y": 7.4182646536312635, + "heading": -2.9552547893870527, + "angularVelocity": 0.04985741808348525, + "velocityX": -1.9001230032488765, + "velocityY": -0.07425358173362791, + "timestamp": 6.266497037708195 + }, + { + "x": 7.036206436366714, + "y": 7.409230150978378, + "heading": -2.947871418759817, + "angularVelocity": 0.07495070510511129, + "velocityX": -2.334538577198753, + "velocityY": -0.09171181812408385, + "timestamp": 6.365006726401333 + }, + { + "x": 6.763437633128491, + "y": 7.398475876181393, + "heading": -2.938014282817138, + "angularVelocity": 0.10006260372402688, + "velocityX": -2.7689540679385254, + "velocityY": -0.10916971660002103, + "timestamp": 6.463516415094471 + }, + { + "x": 6.447874717087643, + "y": 7.386001870945065, + "heading": -2.9256807297504217, + "angularVelocity": 0.12520142160976788, + "velocityX": -3.203369335820747, + "velocityY": -0.12662719171903017, + "timestamp": 6.562026103787609 + }, + { + "x": 6.1055219219448285, + "y": 7.372245899357551, + "heading": -2.905707588465526, + "angularVelocity": 0.20275306469714785, + "velocityX": -3.475321054046335, + "velocityY": -0.13964079848394192, + "timestamp": 6.660535792480747 + }, + { + "x": 5.8059632213622585, + "y": 7.360209533615462, + "heading": -2.88822069052682, + "angularVelocity": 0.17751449802240743, + "velocityX": -3.040905971347722, + "velocityY": -0.12218458815337546, + "timestamp": 6.759045481173885 + }, + { + "x": 5.549198623839576, + "y": 7.349892726247163, + "heading": -2.873224621784891, + "angularVelocity": 0.15222937906790351, + "velocityX": -2.6064908023668067, + "velocityY": -0.10472885972095358, + "timestamp": 6.857555169867023 + }, + { + "x": 5.33522812761357, + "y": 7.341295437788649, + "heading": -2.860723300556775, + "angularVelocity": 0.1269044841574769, + "velocityX": -2.172075651284741, + "velocityY": -0.08727353189893447, + "timestamp": 6.956064858560161 + }, + { + "x": 5.164051729917877, + "y": 7.334417637055141, + "heading": -2.85071984383666, + "angularVelocity": 0.10154794774832755, + "velocityX": -1.7376605282848332, + "velocityY": -0.06981852064248911, + "timestamp": 7.054574547253299 + }, + { + "x": 5.0356694290546695, + "y": 7.3292593010381655, + "heading": -2.8432164608319708, + "angularVelocity": 0.07616898504331324, + "velocityX": -1.303245422520044, + "velocityY": -0.05236374295166474, + "timestamp": 7.153084235946437 + }, + { + "x": 4.950081225204065, + "y": 7.325820414771508, + "heading": -2.8382143743786914, + "angularVelocity": 0.0507776089807905, + "velocityX": -0.8688303149268349, + "velocityY": -0.03490911718713825, + "timestamp": 7.251593924639575 + }, + { + "x": 4.907287120819092, + "y": 7.324100971221924, + "heading": -2.8357137696574513, + "angularVelocity": 0.025384353096777085, + "velocityX": -0.43441518243224797, + "velocityY": -0.01745456281909801, + "timestamp": 7.350103613332713 + }, + { + "x": 4.907287120819092, + "y": 7.324100971221924, + "heading": -2.8357137696574513, + "angularVelocity": 3.103332055729446e-26, + "velocityX": -6.947696608697022e-27, + "velocityY": 2.520053434961675e-27, + "timestamp": 7.448613302025851 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/Robot/src/main/deploy/choreo/New Path (1).traj b/Robot/src/main/deploy/choreo/New Path (1).traj new file mode 100644 index 0000000..3f21b2e --- /dev/null +++ b/Robot/src/main/deploy/choreo/New Path (1).traj @@ -0,0 +1,1220 @@ +{ + "samples": [ + { + "x": 1.373390793800354, + "y": 5.5500569343566895, + "heading": -3.14, + "angularVelocity": -7.792493373112414e-25, + "velocityX": -1.4675606988248466e-25, + "velocityY": 4.296548304526963e-25, + "timestamp": 0 + }, + { + "x": 1.4008162818871832, + "y": 5.548332467221767, + "heading": -3.137847865481668, + "angularVelocity": 0.027272567199688357, + "velocityX": 0.34754494222415555, + "velocityY": -0.021853023321830287, + "timestamp": 0.07891206216760503 + }, + { + "x": 1.4556672566846394, + "y": 5.544883518010237, + "heading": -3.133543459938688, + "angularVelocity": 0.054546864252994244, + "velocityX": 0.6950898670086171, + "velocityY": -0.04370623598969305, + "timestamp": 0.15782412433521006 + }, + { + "x": 1.5379437150886541, + "y": 5.539710064387518, + "heading": -3.127086401796681, + "angularVelocity": 0.08182599674422604, + "velocityX": 1.0426347524572848, + "velocityY": -0.06555973168882138, + "timestamp": 0.23673618650281508 + }, + { + "x": 1.6476456518138687, + "y": 5.532812078143502, + "heading": -3.1184760134609104, + "angularVelocity": 0.10911371594221948, + "velocityX": 1.3901795709281257, + "velocityY": -0.08741358487583052, + "timestamp": 0.31564824867042013 + }, + { + "x": 1.7847730583095482, + "y": 5.524189529642618, + "heading": -3.10771125358726, + "angularVelocity": 0.13641463140053903, + "velocityX": 1.7377242810411937, + "velocityY": -0.10926781361474407, + "timestamp": 0.3945603108380252 + }, + { + "x": 1.949325918220542, + "y": 5.513842409726839, + "heading": -3.0947906414967634, + "angularVelocity": 0.16373431051711962, + "velocityX": 2.0852687838963337, + "velocityY": -0.1311221584071907, + "timestamp": 0.4734723730056302 + }, + { + "x": 2.1090388863997416, + "y": 5.523857134967481, + "heading": -3.1017736522087302, + "angularVelocity": -0.08849104332281946, + "velocityX": 2.0239360598634204, + "velocityY": 0.1269099420994908, + "timestamp": 0.5523844351732352 + }, + { + "x": 2.2413264008507308, + "y": 5.535596537344446, + "heading": -3.110910463319114, + "angularVelocity": -0.11578472111117445, + "velocityX": 1.6763915530431484, + "velocityY": 0.14876562662918416, + "timestamp": 0.6312964973408403 + }, + { + "x": 2.3461884403926034, + "y": 5.54906052840806, + "heading": -3.122199828492689, + "angularVelocity": -0.14306260492340278, + "velocityX": 1.3288467778113737, + "velocityY": 0.17062019029507766, + "timestamp": 0.7102085595084453 + }, + { + "x": 2.4236249923706055, + "y": 5.564249038696289, + "heading": -3.135640447743494, + "angularVelocity": -0.1703240148794674, + "velocityX": 0.9813018422143182, + "velocityY": 0.1924738737148937, + "timestamp": 0.7891206216760503 + }, + { + "x": 2.4236249924844095, + "y": 5.564249038718611, + "heading": -3.135640447763247, + "angularVelocity": -0.17032401487979337, + "velocityX": 0.9813018422127583, + "velocityY": 0.19247387371505806, + "timestamp": 0.7891206217920228 + }, + { + "x": 2.4236249925982136, + "y": 5.5642490387409325, + "heading": -3.135640447783, + "angularVelocity": -0.17032401488011933, + "velocityX": 0.9813018422113264, + "velocityY": 0.19247387371524752, + "timestamp": 0.7891206219079953 + }, + { + "x": 2.4236249927120177, + "y": 5.564249038763254, + "heading": -3.135640447802753, + "angularVelocity": -0.17032401488044527, + "velocityX": 0.9813018422100225, + "velocityY": 0.19247387371546204, + "timestamp": 0.7891206220239677 + }, + { + "x": 2.4236249928258218, + "y": 5.564249038785576, + "heading": -3.135640447822506, + "angularVelocity": -0.17032401488077123, + "velocityX": 0.9813018422088465, + "velocityY": 0.19247387371570168, + "timestamp": 0.7891206221399402 + }, + { + "x": 2.423624992939626, + "y": 5.564249038807898, + "heading": -3.1356404478422593, + "angularVelocity": -0.1703240148810972, + "velocityX": 0.9813018422077986, + "velocityY": 0.19247387371596644, + "timestamp": 0.7891206222559126 + }, + { + "x": 2.42362499305343, + "y": 5.564249038830219, + "heading": -3.1356404478620123, + "angularVelocity": -0.17032401488142315, + "velocityX": 0.9813018422068785, + "velocityY": 0.1924738737162563, + "timestamp": 0.7891206223718851 + }, + { + "x": 2.423624993167234, + "y": 5.564249038852541, + "heading": -3.1356404478817654, + "angularVelocity": -0.17032401488174911, + "velocityX": 0.9813018422060865, + "velocityY": 0.19247387371657124, + "timestamp": 0.7891206224878575 + }, + { + "x": 2.423624993281038, + "y": 5.564249038874863, + "heading": -3.1356404479015185, + "angularVelocity": -0.17032401488207508, + "velocityX": 0.9813018422054224, + "velocityY": 0.19247387371691124, + "timestamp": 0.78912062260383 + }, + { + "x": 2.423624993394842, + "y": 5.564249038897184, + "heading": -3.1356404479212716, + "angularVelocity": -0.170324014882401, + "velocityX": 0.9813018422048863, + "velocityY": 0.1924738737172764, + "timestamp": 0.7891206227198024 + }, + { + "x": 2.4236249935086462, + "y": 5.564249038919506, + "heading": -3.1356404479410247, + "angularVelocity": -0.17032401488272697, + "velocityX": 0.981301842204478, + "velocityY": 0.19247387371766664, + "timestamp": 0.7891206228357749 + }, + { + "x": 2.4236249936224503, + "y": 5.564249038941828, + "heading": -3.135640447960778, + "angularVelocity": -0.17032401488305293, + "velocityX": 0.9813018422041978, + "velocityY": 0.19247387371808197, + "timestamp": 0.7891206229517473 + }, + { + "x": 2.4236249937362544, + "y": 5.5642490389641495, + "heading": -3.135640447980531, + "angularVelocity": -0.1703240148833789, + "velocityX": 0.9813018422040455, + "velocityY": 0.1924738737185224, + "timestamp": 0.7891206230677198 + }, + { + "x": 2.4236249938500585, + "y": 5.564249038986471, + "heading": -3.135640448000284, + "angularVelocity": -0.17032401488370486, + "velocityX": 0.9813018422040212, + "velocityY": 0.19247387371898791, + "timestamp": 0.7891206231836922 + }, + { + "x": 2.4236249939638626, + "y": 5.564249039008793, + "heading": -3.135640448020037, + "angularVelocity": -0.1703240148840308, + "velocityX": 0.9813018422041248, + "velocityY": 0.19247387371947852, + "timestamp": 0.7891206232996647 + }, + { + "x": 2.4236249940776666, + "y": 5.564249039031115, + "heading": -3.13564044803979, + "angularVelocity": -0.17032401488435675, + "velocityX": 0.9813018422043563, + "velocityY": 0.19247387371999425, + "timestamp": 0.7891206234156372 + }, + { + "x": 2.4236249941914707, + "y": 5.564249039053436, + "heading": -3.1356404480595432, + "angularVelocity": -0.1703240148846827, + "velocityX": 0.9813018422047158, + "velocityY": 0.19247387372053507, + "timestamp": 0.7891206235316096 + }, + { + "x": 2.423624994305275, + "y": 5.564249039075758, + "heading": -3.1356404480792963, + "angularVelocity": -0.17032401488500867, + "velocityX": 0.9813018422052033, + "velocityY": 0.19247387372110097, + "timestamp": 0.7891206236475821 + }, + { + "x": 2.423624994419079, + "y": 5.56424903909808, + "heading": -3.1356404480990494, + "angularVelocity": -0.17032401488533463, + "velocityX": 0.9813018422058187, + "velocityY": 0.192473873721692, + "timestamp": 0.7891206237635545 + }, + { + "x": 2.423624994532883, + "y": 5.564249039120401, + "heading": -3.1356404481188025, + "angularVelocity": -0.1703240148856606, + "velocityX": 0.9813018422065621, + "velocityY": 0.19247387372230812, + "timestamp": 0.789120623879527 + }, + { + "x": 2.423624994646687, + "y": 5.564249039142723, + "heading": -3.1356404481385556, + "angularVelocity": -0.17032401488598653, + "velocityX": 0.9813018422074334, + "velocityY": 0.19247387372294933, + "timestamp": 0.7891206239954994 + }, + { + "x": 2.423624994760491, + "y": 5.564249039165045, + "heading": -3.1356404481583087, + "angularVelocity": -0.1703240148863125, + "velocityX": 0.9813018422084328, + "velocityY": 0.19247387372361563, + "timestamp": 0.7891206241114719 + }, + { + "x": 2.423624994874295, + "y": 5.5642490391873665, + "heading": -3.1356404481780618, + "angularVelocity": -0.17032401488663845, + "velocityX": 0.98130184220956, + "velocityY": 0.192473873724307, + "timestamp": 0.7891206242274443 + }, + { + "x": 2.4236249949880992, + "y": 5.564249039209688, + "heading": -3.135640448197815, + "angularVelocity": -0.17032401488696441, + "velocityX": 0.9813018422108152, + "velocityY": 0.1924738737250235, + "timestamp": 0.7891206243434168 + }, + { + "x": 2.4236249951019033, + "y": 5.56424903923201, + "heading": -3.135640448217568, + "angularVelocity": -0.17032401488729038, + "velocityX": 0.9813018422121984, + "velocityY": 0.1924738737257651, + "timestamp": 0.7891206244593892 + }, + { + "x": 2.4236249952157074, + "y": 5.564249039254332, + "heading": -3.135640448237321, + "angularVelocity": -0.1703240148876163, + "velocityX": 0.9813018422137095, + "velocityY": 0.1924738737265318, + "timestamp": 0.7891206245753617 + }, + { + "x": 2.4236249953295115, + "y": 5.564249039276653, + "heading": -3.135640448257074, + "angularVelocity": -0.17032401488794227, + "velocityX": 0.9813018422153487, + "velocityY": 0.19247387372732358, + "timestamp": 0.7891206246913341 + }, + { + "x": 2.4236249954433156, + "y": 5.564249039298975, + "heading": -3.135640448276827, + "angularVelocity": -0.17032401488826823, + "velocityX": 0.9813018422171157, + "velocityY": 0.19247387372814045, + "timestamp": 0.7891206248073066 + }, + { + "x": 2.5202131403633516, + "y": 5.5885147910320585, + "heading": -3.150117025303126, + "angularVelocity": -0.19273984769703859, + "velocityX": 1.2859658956262894, + "velocityY": 0.3230720414634121, + "timestamp": 0.8642300388660642 + }, + { + "x": 2.6397927701444233, + "y": 5.62233325234703, + "heading": -3.166259274426774, + "angularVelocity": -0.21491645655789715, + "velocityX": 1.5920724622818327, + "velocityY": 0.4502559597724382, + "timestamp": 0.9393394529248218 + }, + { + "x": 2.782492954308911, + "y": 5.665387996339145, + "heading": -3.184044486118167, + "angularVelocity": -0.23679071277909178, + "velocityX": 1.8998974489782852, + "velocityY": 0.5732269986613667, + "timestamp": 1.0144488669835794 + }, + { + "x": 2.948469657430915, + "y": 5.717279062041011, + "heading": -3.2034430209640345, + "angularVelocity": -0.25827035251122976, + "velocityX": 2.2097989340212596, + "velocityY": 0.6908729931147001, + "timestamp": 1.089558281042337 + }, + { + "x": 3.13791427133203, + "y": 5.777485440384115, + "heading": -3.2244148248769338, + "angularVelocity": -0.27921671571679574, + "velocityX": 2.5222485926053704, + "velocityY": 0.8015823195745534, + "timestamp": 1.1646676951010946 + }, + { + "x": 3.3510652321882093, + "y": 5.845301698736286, + "heading": -3.2469035661389745, + "angularVelocity": -0.29941308348441903, + "velocityX": 2.837872769043742, + "velocityY": 0.9028995792607077, + "timestamp": 1.2397771091598522 + }, + { + "x": 3.5882224140431322, + "y": 5.919722646911708, + "heading": -3.2708260554869257, + "angularVelocity": -0.3185018768650853, + "velocityX": 3.157489441595122, + "velocityY": 0.9908338269980898, + "timestamp": 1.3148865232186098 + }, + { + "x": 3.849756972147724, + "y": 5.999212365002834, + "heading": -3.296051340937575, + "angularVelocity": -0.33584718729021185, + "velocityX": 3.482047641857447, + "velocityY": 1.0583189748883177, + "timestamp": 1.3899959372773674 + }, + { + "x": 4.136067397800634, + "y": 6.081181645403884, + "heading": -3.322354225031387, + "angularVelocity": -0.3501942389436787, + "velocityX": 3.811911319517566, + "velocityY": 1.091331645017572, + "timestamp": 1.465105351336125 + }, + { + "x": 4.447151871291085, + "y": 6.160640800595462, + "heading": -3.34929544790659, + "angularVelocity": -0.3586930242076843, + "velocityX": 4.141750769711674, + "velocityY": 1.0579120631858112, + "timestamp": 1.5402147653948826 + }, + { + "x": 4.779482954645891, + "y": 6.227113094753417, + "heading": -3.37586605695639, + "angularVelocity": -0.3537587049875555, + "velocityX": 4.424626227210678, + "velocityY": 0.8850061605587058, + "timestamp": 1.6153241794536402 + }, + { + "x": 5.117259626240433, + "y": 6.269293967456475, + "heading": -3.40016260033537, + "angularVelocity": -0.32348199867427174, + "velocityX": 4.497128300459125, + "velocityY": 0.5615923547221425, + "timestamp": 1.6904335935123977 + }, + { + "x": 5.446596145629883, + "y": 6.288059234619141, + "heading": -3.421549469666226, + "angularVelocity": -0.2847428594520122, + "velocityX": 4.384756871246697, + "velocityY": 0.24983908339353045, + "timestamp": 1.7655430075711553 + }, + { + "x": 5.583978781229968, + "y": 6.292316694982243, + "heading": -3.430131520120695, + "angularVelocity": -0.2689620281221212, + "velocityX": 4.305580874384169, + "velocityY": 0.1334290890020681, + "timestamp": 1.7974510449784737 + }, + { + "x": 5.718326508347156, + "y": 6.293262578004624, + "heading": -3.4381858559860787, + "angularVelocity": -0.25242341804249496, + "velocityX": 4.210466642062215, + "velocityY": 0.02964403640080957, + "timestamp": 1.8293590823857921 + }, + { + "x": 5.849241399946558, + "y": 6.291311612489797, + "heading": -3.4457039588897924, + "angularVelocity": -0.23561784160343793, + "velocityX": 4.102881350182191, + "velocityY": -0.061143388103806366, + "timestamp": 1.8612671197931105 + }, + { + "x": 5.976424364300466, + "y": 6.28686095808839, + "heading": -3.452687518488142, + "angularVelocity": -0.21886521910455456, + "velocityX": 3.9859225037995363, + "velocityY": -0.13948380292379126, + "timestamp": 1.893175157200429 + }, + { + "x": 6.0996561002514555, + "y": 6.280274035067278, + "heading": -3.459143915120258, + "angularVelocity": -0.20234389692156543, + "velocityX": 3.8620907446575, + "velocityY": -0.20643460257448376, + "timestamp": 1.9250831946077473 + }, + { + "x": 6.21877796918791, + "y": 6.271874588037581, + "heading": -3.4650831978385432, + "angularVelocity": -0.18613751270466306, + "velocityX": 3.7332872409486546, + "velocityY": -0.26323922472808714, + "timestamp": 1.9569912320150658 + }, + { + "x": 6.33367594677285, + "y": 6.2619468977702875, + "heading": -3.4705163067965152, + "angularVelocity": -0.1702739936215994, + "velocityX": 3.6009102069871273, + "velocityY": -0.3111344687409986, + "timestamp": 1.9888992694223842 + }, + { + "x": 6.444268295780197, + "y": 6.250738999348943, + "heading": -3.475454120537259, + "angularVelocity": -0.15475140879743604, + "velocityX": 3.465971522961229, + "velocityY": -0.3512562768518353, + "timestamp": 2.0208073068297026 + }, + { + "x": 6.5504965143251574, + "y": 6.238466994593223, + "heading": -3.4799069914164837, + "angularVelocity": -0.13955326748499272, + "velocityX": 3.3291993860015867, + "velocityY": -0.3846054396597071, + "timestamp": 2.052715344237021 + }, + { + "x": 6.6523188314701445, + "y": 6.225319459420252, + "heading": -3.483884551234072, + "angularVelocity": -0.1246569874170926, + "velocityX": 3.191118145098868, + "velocityY": -0.4120446207686626, + "timestamp": 2.08462338164434 + }, + { + "x": 6.749705571819693, + "y": 6.211461495939882, + "heading": -3.487395659176523, + "angularVelocity": -0.11003835483926433, + "velocityX": 3.0521068753420764, + "velocityY": -0.4343094908492185, + "timestamp": 2.1165314190516584 + }, + { + "x": 6.842635853833305, + "y": 6.197038271774381, + "heading": -3.4904484207431485, + "angularVelocity": -0.0956737491452672, + "velocityX": 2.912441176726728, + "velocityY": -0.4520247980589766, + "timestamp": 2.148439456458977 + }, + { + "x": 6.931095228941555, + "y": 6.182178030589835, + "heading": -3.4930502391028706, + "angularVelocity": -0.08154115925428285, + "velocityX": 2.7723226589912517, + "velocityY": -0.4657209403025567, + "timestamp": 2.1803474938662957 + }, + { + "x": 7.015073982675889, + "y": 6.1669946184502, + "heading": -3.4952078785818306, + "angularVelocity": -0.06762056379139442, + "velocityX": 2.6318996891696274, + "velocityY": -0.4758491393818042, + "timestamp": 2.2122555312736143 + }, + { + "x": 7.0945659029317385, + "y": 6.151589590913828, + "heading": -3.4969275299383775, + "angularVelocity": -0.05389398710408752, + "velocityX": 2.4912820315804582, + "velocityY": -0.4827945805541645, + "timestamp": 2.244163568680933 + }, + { + "x": 7.169567379757041, + "y": 6.136053967419142, + "heading": -3.498214872438713, + "angularVelocity": -0.040345398994690515, + "velocityX": 2.350551237855201, + "velocityY": -0.4868874665140758, + "timestamp": 2.2760716060882515 + }, + { + "x": 7.240076742171475, + "y": 6.120469693395241, + "heading": -3.499075130588546, + "angularVelocity": -0.026960547239283258, + "velocityX": 2.2097680754962985, + "velocityY": -0.48841217731324, + "timestamp": 2.30797964349557 + }, + { + "x": 7.306093765850792, + "y": 6.104910861957847, + "heading": -3.4995131248526885, + "angularVelocity": -0.013726769169506972, + "velocityX": 2.068977882800654, + "velocityY": -0.48761480497154164, + "timestamp": 2.339887680902889 + }, + { + "x": 7.367619305030031, + "y": 6.089444738394438, + "heading": -3.4995333164334084, + "angularVelocity": -0.000632805473504129, + "velocityX": 1.9282144618875081, + "velocityY": -0.48470933407710726, + "timestamp": 2.3717957183102074 + }, + { + "x": 7.424655015480076, + "y": 6.074132622864405, + "heading": -3.4991398465215826, + "angularVelocity": 0.012331373026894028, + "velocityX": 1.787502932943278, + "velocityY": -0.4798827121382698, + "timestamp": 2.403703755717526 + }, + { + "x": 7.477203144806779, + "y": 6.059030580127337, + "heading": -3.498336570568695, + "angularVelocity": 0.025174721423108552, + "velocityX": 1.6468618441148708, + "velocityY": -0.473299016930574, + "timestamp": 2.4356117931248447 + }, + { + "x": 7.525266372907712, + "y": 6.044190059652854, + "heading": -3.4971270881539924, + "angularVelocity": 0.03790525876796509, + "velocityX": 1.5063047434534294, + "velocityY": -0.46510289194657173, + "timestamp": 2.4675198305321633 + }, + { + "x": 7.568847690075842, + "y": 6.029658425029338, + "heading": -3.4955147689946737, + "angularVelocity": 0.05053018895323732, + "velocityX": 1.365841358771074, + "velocityY": -0.4554223889740649, + "timestamp": 2.499427867939482 + }, + { + "x": 7.607950303556081, + "y": 6.015479408015207, + "heading": -3.4935027755974737, + "angularVelocity": 0.0630560059685356, + "velocityX": 1.225478489356149, + "velocityY": -0.44437132980415467, + "timestamp": 2.5313359053468005 + }, + { + "x": 7.642577565744241, + "y": 6.001693499708621, + "heading": -3.4910940829929844, + "angularVelocity": 0.07548858532857207, + "velocityX": 1.0852206842473227, + "velocityY": -0.43205127694331946, + "timestamp": 2.563243942754119 + }, + { + "x": 7.672732918945103, + "y": 5.988338289012847, + "heading": -3.4882914959374425, + "angularVelocity": 0.08783326344287469, + "velocityX": 0.9450707611977816, + "velocityY": -0.41855318537111263, + "timestamp": 2.5951519801614378 + }, + { + "x": 7.698419852868165, + "y": 5.975448756730407, + "heading": -3.4850976639142544, + "angularVelocity": 0.10009490657219974, + "velocityX": 0.8050302058744196, + "velocityY": -0.40395879313730104, + "timestamp": 2.6270600175687564 + }, + { + "x": 7.71964187196861, + "y": 5.963057532136706, + "heading": -3.4815150942206845, + "angularVelocity": 0.11227797083967467, + "velocityX": 0.6650994804079722, + "velocityY": -0.38834179725697887, + "timestamp": 2.658968054976075 + }, + { + "x": 7.736402470430254, + "y": 5.951195117688826, + "heading": -3.477546163384214, + "angularVelocity": 0.12438655457888602, + "velocityX": 0.5252782628930857, + "velocityY": -0.3717688523569133, + "timestamp": 2.6908760923833936 + }, + { + "x": 7.74870511310215, + "y": 5.939890086558444, + "heading": -3.473193127117835, + "angularVelocity": 0.13642444412392482, + "velocityX": 0.38556563397643184, + "velocityY": -0.3543004223691105, + "timestamp": 2.7227841297907123 + }, + { + "x": 7.756553221087926, + "y": 5.929169256892984, + "heading": -3.4684581289933885, + "angularVelocity": 0.14839515398588157, + "velocityX": 0.24596022267342674, + "velocityY": -0.33599150986958115, + "timestamp": 2.754692167198031 + }, + { + "x": 7.759950160980225, + "y": 5.919057846069336, + "heading": -3.4633432079864352, + "angularVelocity": 0.16030196221909535, + "velocityX": 0.10646032060622376, + "velocityY": -0.31689228311261575, + "timestamp": 2.7866002046053495 + }, + { + "x": 7.740992810070082, + "y": 5.897866293760295, + "heading": -3.4483259795142858, + "angularVelocity": 0.18966965117669482, + "velocityX": -0.23943393689649056, + "velocityY": -0.26765220638433596, + "timestamp": 2.865775910254534 + }, + { + "x": 7.6946750115513405, + "y": 5.880751239353706, + "heading": -3.430977462853066, + "angularVelocity": 0.21911414011373867, + "velocityX": -0.5850001353188997, + "velocityY": -0.2161654798812024, + "timestamp": 2.944951615903719 + }, + { + "x": 7.621031262251048, + "y": 5.867937342014943, + "heading": -3.4112891061492308, + "angularVelocity": 0.2486666401316515, + "velocityX": -0.9301306340937104, + "velocityY": -0.16184127736782786, + "timestamp": 3.0241273215529034 + }, + { + "x": 7.520109336525543, + "y": 5.85971721387783, + "heading": -3.389248427591031, + "angularVelocity": 0.2783767871404717, + "velocityX": -1.2746577362085507, + "velocityY": -0.10382134355121005, + "timestamp": 3.103303027202088 + }, + { + "x": 7.39197917102874, + "y": 5.8564875757937624, + "heading": -3.3648366324448036, + "angularVelocity": 0.308324314208095, + "velocityX": -1.6183015288114875, + "velocityY": -0.04079077107790146, + "timestamp": 3.1824787328512727 + }, + { + "x": 7.236751317240548, + "y": 5.858816377779796, + "heading": -3.338024361207094, + "angularVelocity": 0.3386426558231255, + "velocityX": -1.9605490410907365, + "velocityY": 0.029413087852372996, + "timestamp": 3.2616544385004573 + }, + { + "x": 7.054621391051907, + "y": 5.867582559401864, + "heading": -3.3087631347222772, + "angularVelocity": 0.3695732958095547, + "velocityX": -2.3003258978913266, + "velocityY": 0.11071807381053804, + "timestamp": 3.340830144149642 + }, + { + "x": 6.846004001197015, + "y": 5.884320000838926, + "heading": -3.2769649631981674, + "angularVelocity": 0.40161525891544314, + "velocityX": -2.63486618962696, + "velocityY": 0.21139617638803204, + "timestamp": 3.4200058497988266 + }, + { + "x": 6.612122032954981, + "y": 5.912316015175021, + "heading": -3.242440238754101, + "angularVelocity": 0.43605199550781415, + "velocityX": -2.953961272897621, + "velocityY": 0.3535934931876858, + "timestamp": 3.4991815554480112 + }, + { + "x": 6.360418430236016, + "y": 5.961461610916641, + "heading": -3.204725258846095, + "angularVelocity": 0.4763453587027727, + "velocityX": -3.1790509557846622, + "velocityY": 0.6207156013155894, + "timestamp": 3.578357261097196 + }, + { + "x": 6.128768619413155, + "y": 6.029673284245218, + "heading": -3.1672334777487867, + "angularVelocity": 0.4735263271719758, + "velocityX": -2.9257688191534936, + "velocityY": 0.861522771022888, + "timestamp": 3.6575329667463805 + }, + { + "x": 5.923854263987644, + "y": 6.105004809926703, + "heading": -3.1312739726613037, + "angularVelocity": 0.45417347142839426, + "velocityX": -2.5880963579087672, + "velocityY": 0.9514474808126816, + "timestamp": 3.736708672395565 + }, + { + "x": 5.746388049675664, + "y": 6.183799530006089, + "heading": -3.0971258429522432, + "angularVelocity": 0.4312955524560246, + "velocityX": -2.241422578515493, + "velocityY": 0.9951881001037627, + "timestamp": 3.81588437804475 + }, + { + "x": 5.596527243008144, + "y": 6.264409866427464, + "heading": -3.0649099526354786, + "angularVelocity": 0.4068911044444387, + "velocityX": -1.8927625013098104, + "velocityY": 1.0181195830264693, + "timestamp": 3.8950600836939344 + }, + { + "x": 5.47431680968178, + "y": 6.34590840516807, + "heading": -3.034696195300985, + "angularVelocity": 0.38160389082436225, + "velocityX": -1.5435345011998953, + "velocityY": 1.029337699896412, + "timestamp": 3.974235789343119 + }, + { + "x": 5.37976915844747, + "y": 6.427702267182384, + "heading": -3.0065318546626574, + "angularVelocity": 0.35571947742555177, + "velocityX": -1.1941497768676173, + "velocityY": 1.0330676732674617, + "timestamp": 4.053411494992304 + }, + { + "x": 5.312885421758628, + "y": 6.509380270776351, + "heading": -2.9804521557080004, + "angularVelocity": 0.3293901676129292, + "velocityX": -0.8447507494937071, + "velocityY": 1.0316043655596845, + "timestamp": 4.132587200641488 + }, + { + "x": 5.273662535323598, + "y": 6.590640683149381, + "heading": -2.9564850016577298, + "angularVelocity": 0.30270843630324745, + "velocityX": -0.49539042454286053, + "velocityY": 1.026330131278943, + "timestamp": 4.211762906290673 + }, + { + "x": 5.262095928192139, + "y": 6.671252727508545, + "heading": -2.9346533643249577, + "angularVelocity": 0.2757365678495407, + "velocityX": -0.14608783131922698, + "velocityY": 1.0181411545145314, + "timestamp": 4.290938611939858 + }, + { + "x": 5.286450146462021, + "y": 6.764484077147902, + "heading": -2.9120502053248636, + "angularVelocity": 0.24386251496431138, + "velocityX": 0.26275446353576026, + "velocityY": 1.0058607912494006, + "timestamp": 4.383626735833773 + }, + { + "x": 5.34869261123642, + "y": 6.856387459090783, + "heading": -2.8924677533563106, + "angularVelocity": 0.2112725033788139, + "velocityX": 0.671525780860962, + "velocityY": 0.9915335221162508, + "timestamp": 4.4763148597276885 + }, + { + "x": 5.448813085425695, + "y": 6.946710159976592, + "heading": -2.8759905639386316, + "angularVelocity": 0.17777023339621403, + "velocityX": 1.080186651570012, + "velocityY": 0.9744797617134475, + "timestamp": 4.569002983621604 + }, + { + "x": 5.586794422356268, + "y": 7.035098884885215, + "heading": -2.8627324007346533, + "angularVelocity": 0.1430405821910139, + "velocityX": 1.4886625290689568, + "velocityY": 0.9536143488003591, + "timestamp": 4.661691107515519 + }, + { + "x": 5.762604761098077, + "y": 7.121024926575688, + "heading": -2.8528585427257678, + "angularVelocity": 0.10652775775444928, + "velocityX": 1.8967946631763621, + "velocityY": 0.9270447828765889, + "timestamp": 4.754379231409435 + }, + { + "x": 5.976174763029016, + "y": 7.2036110783097, + "heading": -2.8466380444560433, + "angularVelocity": 0.06711213916514423, + "velocityX": 2.3041787119931025, + "velocityY": 0.8910111486185037, + "timestamp": 4.84706735530335 + }, + { + "x": 6.227306274033663, + "y": 7.2811226036526095, + "heading": -2.8445996856753095, + "angularVelocity": 0.021991585276523277, + "velocityX": 2.709424902073476, + "velocityY": 0.8362616707144066, + "timestamp": 4.939755479197266 + }, + { + "x": 6.514972208251902, + "y": 7.348599798549223, + "heading": -2.8482807276605207, + "angularVelocity": -0.039714278707639744, + "velocityX": 3.103589997651488, + "velocityY": 0.7280025969005842, + "timestamp": 5.032443603091181 + }, + { + "x": 6.803286438858483, + "y": 7.378323009778229, + "heading": -2.8654797905272513, + "angularVelocity": -0.1855584312658597, + "velocityX": 3.110584382272814, + "velocityY": 0.3206798236959108, + "timestamp": 5.125131726985097 + }, + { + "x": 7.05515266695702, + "y": 7.39757835383594, + "heading": -2.8830786743462165, + "angularVelocity": -0.18987204702845675, + "velocityX": 2.717351668341086, + "velocityY": 0.2077433790735653, + "timestamp": 5.217819850879012 + }, + { + "x": 7.269495335698046, + "y": 7.411362471913543, + "heading": -2.899617904375188, + "angularVelocity": -0.17843958140636978, + "velocityX": 2.312514912766473, + "velocityY": 0.1487150402718173, + "timestamp": 5.310507974772928 + }, + { + "x": 7.446102948393076, + "y": 7.421417571603764, + "heading": -2.9145959892958864, + "angularVelocity": -0.1615965917903412, + "velocityX": 1.905396347186422, + "velocityY": 0.10848315045980522, + "timestamp": 5.403196098666843 + }, + { + "x": 7.584899849507656, + "y": 7.428623463558208, + "heading": -2.927761388273211, + "angularVelocity": -0.14203976112833264, + "velocityX": 1.4974615439777093, + "velocityY": 0.07774342226077013, + "timestamp": 5.4958842225607585 + }, + { + "x": 7.685850480593157, + "y": 7.433510011721959, + "heading": -2.938963494323689, + "angularVelocity": -0.12085805149426576, + "velocityX": 1.0891431053350733, + "velocityY": 0.05272032660132098, + "timestamp": 5.588572346454674 + }, + { + "x": 7.748935268502705, + "y": 7.436431089750518, + "heading": -2.948102141289854, + "angularVelocity": -0.09859566233776026, + "velocityX": 0.6806134945804951, + "velocityY": 0.03151512735226398, + "timestamp": 5.681260470348589 + }, + { + "x": 7.774142265319824, + "y": 7.4376397132873535, + "heading": -2.9551058096712683, + "angularVelocity": -0.07556165868057244, + "velocityX": 0.27195497932366575, + "velocityY": 0.013039680663066213, + "timestamp": 5.773948594242505 + }, + { + "x": 7.7581383251743805, + "y": 7.437204402709091, + "heading": -2.960077030928164, + "angularVelocity": -0.05046428755227366, + "velocityX": -0.16246056969378117, + "velocityY": -0.004418962074057982, + "timestamp": 5.872458282935643 + }, + { + "x": 7.699340240644664, + "y": 7.4350492376719135, + "heading": -2.9625771572415784, + "angularVelocity": -0.025379496642227335, + "velocityX": -0.5968761581703449, + "velocityY": -0.021877696151197678, + "timestamp": 5.970967971628781 + }, + { + "x": 7.597748009323179, + "y": 7.431174217705324, + "heading": -2.962606855741692, + "angularVelocity": -0.0003014779612800365, + "velocityX": -1.0312917710860698, + "velocityY": -0.03933643500447566, + "timestamp": 6.069477660321919 + }, + { + "x": 7.453361630377404, + "y": 7.4255793508521934, + "heading": -2.9601662281215004, + "angularVelocity": 0.02477550840500792, + "velocityX": -1.4657073924529878, + "velocityY": -0.05679509221229112, + "timestamp": 6.167987349015057 + }, + { + "x": 7.266181104848686, + "y": 7.4182646536312635, + "heading": -2.9552547893870527, + "angularVelocity": 0.04985741808348525, + "velocityX": -1.9001230032488765, + "velocityY": -0.07425358173362791, + "timestamp": 6.266497037708195 + }, + { + "x": 7.036206436366714, + "y": 7.409230150978378, + "heading": -2.947871418759817, + "angularVelocity": 0.07495070510511129, + "velocityX": -2.334538577198753, + "velocityY": -0.09171181812408385, + "timestamp": 6.365006726401333 + }, + { + "x": 6.763437633128491, + "y": 7.398475876181393, + "heading": -2.938014282817138, + "angularVelocity": 0.10006260372402688, + "velocityX": -2.7689540679385254, + "velocityY": -0.10916971660002103, + "timestamp": 6.463516415094471 + }, + { + "x": 6.447874717087643, + "y": 7.386001870945065, + "heading": -2.9256807297504217, + "angularVelocity": 0.12520142160976788, + "velocityX": -3.203369335820747, + "velocityY": -0.12662719171903017, + "timestamp": 6.562026103787609 + }, + { + "x": 6.1055219219448285, + "y": 7.372245899357551, + "heading": -2.905707588465526, + "angularVelocity": 0.20275306469714785, + "velocityX": -3.475321054046335, + "velocityY": -0.13964079848394192, + "timestamp": 6.660535792480747 + }, + { + "x": 5.8059632213622585, + "y": 7.360209533615462, + "heading": -2.88822069052682, + "angularVelocity": 0.17751449802240743, + "velocityX": -3.040905971347722, + "velocityY": -0.12218458815337546, + "timestamp": 6.759045481173885 + }, + { + "x": 5.549198623839576, + "y": 7.349892726247163, + "heading": -2.873224621784891, + "angularVelocity": 0.15222937906790351, + "velocityX": -2.6064908023668067, + "velocityY": -0.10472885972095358, + "timestamp": 6.857555169867023 + }, + { + "x": 5.33522812761357, + "y": 7.341295437788649, + "heading": -2.860723300556775, + "angularVelocity": 0.1269044841574769, + "velocityX": -2.172075651284741, + "velocityY": -0.08727353189893447, + "timestamp": 6.956064858560161 + }, + { + "x": 5.164051729917877, + "y": 7.334417637055141, + "heading": -2.85071984383666, + "angularVelocity": 0.10154794774832755, + "velocityX": -1.7376605282848332, + "velocityY": -0.06981852064248911, + "timestamp": 7.054574547253299 + }, + { + "x": 5.0356694290546695, + "y": 7.3292593010381655, + "heading": -2.8432164608319708, + "angularVelocity": 0.07616898504331324, + "velocityX": -1.303245422520044, + "velocityY": -0.05236374295166474, + "timestamp": 7.153084235946437 + }, + { + "x": 4.950081225204065, + "y": 7.325820414771508, + "heading": -2.8382143743786914, + "angularVelocity": 0.0507776089807905, + "velocityX": -0.8688303149268349, + "velocityY": -0.03490911718713825, + "timestamp": 7.251593924639575 + }, + { + "x": 4.907287120819092, + "y": 7.324100971221924, + "heading": -2.8357137696574513, + "angularVelocity": 0.025384353096777085, + "velocityX": -0.43441518243224797, + "velocityY": -0.01745456281909801, + "timestamp": 7.350103613332713 + }, + { + "x": 4.907287120819092, + "y": 7.324100971221924, + "heading": -2.8357137696574513, + "angularVelocity": 3.103332055729446e-26, + "velocityX": -6.947696608697022e-27, + "velocityY": 2.520053434961675e-27, + "timestamp": 7.448613302025851 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/Robot/src/main/deploy/choreo/New Path (2).1.traj b/Robot/src/main/deploy/choreo/New Path (2).1.traj new file mode 100644 index 0000000..cfd1669 --- /dev/null +++ b/Robot/src/main/deploy/choreo/New Path (2).1.traj @@ -0,0 +1,1301 @@ +{ + "samples": [ + { + "x": 1.302428960800171, + "y": 5.521671772003174, + "heading": 3.119698237349717, + "angularVelocity": -1.0799521907829878e-24, + "velocityX": -5.729727943881475e-23, + "velocityY": 3.806127884713555e-22, + "timestamp": 0 + }, + { + "x": 1.3217771089315806, + "y": 5.522383365283616, + "heading": 3.1197999674258803, + "angularVelocity": 0.0015360380238209192, + "velocityX": 0.29214065634450126, + "velocityY": 0.010744456088856187, + "timestamp": 0.0662288788336051 + }, + { + "x": 1.3604749484171008, + "y": 5.523763370471075, + "heading": 3.1199951995851856, + "angularVelocity": 0.002947840318953084, + "velocityX": 0.5843046140452638, + "velocityY": 0.020836909997010808, + "timestamp": 0.1324577576672102 + }, + { + "x": 1.4185241489686518, + "y": 5.525761671584798, + "heading": 3.1202743864187497, + "angularVelocity": 0.00421548482295459, + "velocityX": 0.8764937829824225, + "velocityY": 0.030172655024753685, + "timestamp": 0.1986866365008153 + }, + { + "x": 1.4959265053667201, + "y": 5.528319403030693, + "heading": 3.1206263164550307, + "angularVelocity": 0.005313845598465322, + "velocityX": 1.1687100515854367, + "velocityY": 0.03861957941822778, + "timestamp": 0.2649155153344204 + }, + { + "x": 1.5926839210326802, + "y": 5.53136644125991, + "heading": 3.121037637902798, + "angularVelocity": 0.006210605630214725, + "velocityX": 1.4609550602397396, + "velocityY": 0.04600769759175801, + "timestamp": 0.33114439416802555 + }, + { + "x": 1.7087983602256251, + "y": 5.5348178374547645, + "heading": 3.121492181382272, + "angularVelocity": 0.006863221716555551, + "velocityX": 1.7532297275433801, + "velocityY": 0.052113160537196716, + "timestamp": 0.39737327300163067 + }, + { + "x": 1.844271734223262, + "y": 5.538568567278952, + "heading": 3.1219699631830324, + "angularVelocity": 0.007214100694065514, + "velocityX": 2.0455332535222754, + "velocityY": 0.05663284491967778, + "timestamp": 0.4636021518352358 + }, + { + "x": 1.9991056443900284, + "y": 5.542485474471211, + "heading": 3.1224456556980607, + "angularVelocity": 0.007182554248321628, + "velocityX": 2.3378609587484425, + "velocityY": 0.05914198248923271, + "timestamp": 0.5298310306688409 + }, + { + "x": 2.1733007984970207, + "y": 5.546394249267978, + "heading": 3.1228861145568447, + "angularVelocity": 0.006650555868398608, + "velocityX": 2.6301993507189527, + "velocityY": 0.059019190202322734, + "timestamp": 0.5960599095024459 + }, + { + "x": 2.36685562133789, + "y": 5.5500569343566895, + "heading": 3.1232461040248376, + "angularVelocity": 0.005435536194070859, + "velocityX": 2.922513958407195, + "velocityY": 0.055303443954021286, + "timestamp": 0.662288788336051 + }, + { + "x": 2.4263388144591955, + "y": 5.551097384925455, + "heading": 3.123340540157932, + "angularVelocity": 0.004778249196920622, + "velocityX": 3.009711542072027, + "velocityY": 0.0526443844294007, + "timestamp": 0.6820525405394778 + }, + { + "x": 2.4875382986996155, + "y": 5.5519733808753085, + "heading": 3.1234216636032834, + "angularVelocity": 0.00410465808903763, + "velocityX": 3.096551890071197, + "velocityY": 0.044323362327003256, + "timestamp": 0.7018162927429047 + }, + { + "x": 2.550436955892463, + "y": 5.552556955429702, + "heading": 3.123489130605961, + "angularVelocity": 0.003413673779285827, + "velocityX": 3.1825260985584145, + "velocityY": 0.02952751827621247, + "timestamp": 0.7215800449463315 + }, + { + "x": 2.615003335084898, + "y": 5.552703036227377, + "heading": 3.1235425841754783, + "angularVelocity": 0.0027046265793564048, + "velocityX": 3.26690896181343, + "velocityY": 0.007391349384069019, + "timestamp": 0.7413437971497584 + }, + { + "x": 2.6811863409244094, + "y": 5.55224977213001, + "heading": 3.123581673782466, + "angularVelocity": 0.0019778434067488013, + "velocityX": 3.3487065187973872, + "velocityY": -0.022934111534127874, + "timestamp": 0.7611075493531853 + }, + { + "x": 2.7489093580691275, + "y": 5.55102122741091, + "heading": 3.123606094755385, + "angularVelocity": 0.001235644561187388, + "velocityX": 3.426627517267461, + "velocityY": -0.062161511966688114, + "timestamp": 0.7808713015566121 + }, + { + "x": 2.8180650275496353, + "y": 5.54883341134946, + "heading": 3.1236156557026797, + "angularVelocity": 0.00048376174709457927, + "velocityX": 3.4991164010099673, + "velocityY": -0.11069841591471039, + "timestamp": 0.800635053760039 + }, + { + "x": 2.8885126788941395, + "y": 5.54550389601763, + "heading": 3.1236103737324172, + "angularVelocity": -0.0002672554385466297, + "velocityX": 3.564487685303465, + "velocityY": -0.1684657496997351, + "timestamp": 0.8203988059634658 + }, + { + "x": 2.9600805496923224, + "y": 5.540863730529652, + "heading": 3.1235905765119973, + "angularVelocity": -0.0010016934141146043, + "velocityX": 3.6211681902070074, + "velocityY": -0.23478160625658828, + "timestamp": 0.8401625581668927 + }, + { + "x": 3.032573643140278, + "y": 5.534768690288578, + "heading": 3.123556968357328, + "angularVelocity": -0.0017004946390557507, + "velocityX": 3.6679823093201, + "velocityY": -0.3083948927480261, + "timestamp": 0.8599263103703195 + }, + { + "x": 3.105785678320405, + "y": 5.527106543467365, + "heading": 3.1235106204259195, + "angularVelocity": -0.0023450977795856365, + "velocityX": 3.704359092674332, + "velocityY": -0.38768684925548386, + "timestamp": 0.8796900625737464 + }, + { + "x": 3.1795117584951056, + "y": 5.517798656367154, + "heading": 3.1234528819071974, + "angularVelocity": -0.0029214350659676155, + "velocityX": 3.7303685765660113, + "velocityY": -0.4709574884568884, + "timestamp": 0.8994538147771732 + }, + { + "x": 3.2535585488164687, + "y": 5.506796705703182, + "heading": 3.1233852496472454, + "angularVelocity": -0.003422035413907008, + "velocityX": 3.7465957657839812, + "velocityY": -0.556673173733965, + "timestamp": 0.9192175669806001 + }, + { + "x": 3.3277504743592434, + "y": 5.494076719862022, + "heading": 3.123309243507978, + "angularVelocity": -0.003845734275821943, + "velocityX": 3.7539392712032984, + "velocityY": -0.6436017670244403, + "timestamp": 0.9389813191840269 + }, + { + "x": 3.401932191914863, + "y": 5.479632608953573, + "heading": 3.1232263159056575, + "angularVelocity": -0.004195944245161604, + "velocityX": 3.7534227707407335, + "velocityY": -0.7308384946226822, + "timestamp": 0.9587450713874538 + }, + { + "x": 3.4759684441746432, + "y": 5.463470489536251, + "heading": 3.1231378004849635, + "angularVelocity": -0.004478674888388461, + "velocityX": 3.7460625643213086, + "velocityY": -0.8177657385584958, + "timestamp": 0.9785088235908806 + }, + { + "x": 3.5497424561199664, + "y": 5.44560427359136, + "heading": 3.1230448916469586, + "angularVelocity": -0.004700971609475635, + "velocityX": 3.732793812934527, + "velocityY": -0.9039890685228098, + "timestamp": 0.9982725757943075 + }, + { + "x": 3.6231537261374727, + "y": 5.426052491121333, + "heading": 3.1229486436831992, + "angularVelocity": -0.004869923624246792, + "velocityX": 3.7144399131242594, + "velocityY": -0.9892748233625418, + "timestamp": 1.0180363279977345 + }, + { + "x": 3.6961157220105543, + "y": 5.404836115617309, + "heading": 3.1228499803373504, + "angularVelocity": -0.004992136353107221, + "velocityX": 3.6917076839503324, + "velocityY": -1.0734993682194114, + "timestamp": 1.0378000802011613 + }, + { + "x": 3.7685537383740204, + "y": 5.381977131361945, + "heading": 3.122749708770004, + "angularVelocity": -0.0050735086290428135, + "velocityX": 3.6651955366504962, + "velocityY": -1.1566115593880046, + "timestamp": 1.0575638324045882 + }, + { + "x": 3.8404030175360155, + "y": 5.357497620278821, + "heading": 3.1226485344894, + "angularVelocity": -0.005119183825147258, + "velocityX": 3.6354068004119586, + "velocityY": -1.2386064564642996, + "timestamp": 1.077327584608015 + }, + { + "x": 3.9116071530358583, + "y": 5.331419200542643, + "heading": 3.12254707551822, + "angularVelocity": -0.005133588507666248, + "velocityX": 3.6027640281533504, + "velocityY": -1.319507524065004, + "timestamp": 1.0970913368114419 + }, + { + "x": 3.982116756748269, + "y": 5.303762698026613, + "heading": 3.1224458750644937, + "angularVelocity": -0.005120508124394676, + "velocityX": 3.567622331360009, + "velocityY": -1.399354850807865, + "timestamp": 1.1168550890148687 + }, + { + "x": 4.051888355776174, + "y": 5.2745479692393795, + "heading": 3.122345412497445, + "angularVelocity": -0.005083172770798156, + "velocityX": 3.5302810068528747, + "velocityY": -1.478197484289892, + "timestamp": 1.1366188412182956 + }, + { + "x": 4.120883482732203, + "y": 5.24379382125935, + "heading": 3.122246112688224, + "angularVelocity": -0.005024339922835519, + "velocityX": 3.4909933218078715, + "velocityY": -1.5560885232459687, + "timestamp": 1.1563825934217224 + }, + { + "x": 4.1890679256640215, + "y": 5.211517992630351, + "heading": 3.122148353884756, + "angularVelocity": -0.0049463686076292, + "velocityX": 3.44997459136313, + "velocityY": -1.6330820330464426, + "timestamp": 1.1761463456251493 + }, + { + "x": 4.256411108437102, + "y": 5.177737171593987, + "heading": 3.1220524743221056, + "angularVelocity": -0.004851283382997846, + "velocityX": 3.407408779462607, + "velocityY": -1.7092311565466005, + "timestamp": 1.1959100978285762 + }, + { + "x": 4.3228855772355175, + "y": 5.14246703628049, + "heading": 3.1219587777652085, + "angularVelocity": -0.004740828357538679, + "velocityX": 3.363453868182489, + "velocityY": -1.7845870030379125, + "timestamp": 1.215673850032003 + }, + { + "x": 4.388466573286225, + "y": 5.105722306923142, + "heading": 3.1218675381607466, + "angularVelocity": -0.0046165122656358646, + "velocityX": 3.3182462204386316, + "velocityY": -1.8591980398831336, + "timestamp": 1.2354376022354299 + }, + { + "x": 4.453131675720215, + "y": 5.067516803741455, + "heading": 3.1217790035501856, + "angularVelocity": -0.004479645851134587, + "velocityX": 3.2719041287503026, + "velocityY": -1.9331098056907883, + "timestamp": 1.2552013544388567 + }, + { + "x": 4.5756878518534965, + "y": 4.98749687604741, + "heading": 3.1216174796705585, + "angularVelocity": -0.004190461413754277, + "velocityX": 3.179510845639871, + "velocityY": -2.0759804686941523, + "timestamp": 1.293746961635431 + }, + { + "x": 4.6944682593112, + "y": 4.902114649138773, + "heading": 3.12147264475533, + "angularVelocity": -0.003757494712439645, + "velocityX": 3.081554970763572, + "velocityY": -2.2150961709646055, + "timestamp": 1.332292568832005 + }, + { + "x": 4.8091447997962735, + "y": 4.811616865702052, + "heading": 3.121353020228485, + "angularVelocity": -0.0031034542077534163, + "velocityX": 2.975087145475454, + "velocityY": -2.347810555304065, + "timestamp": 1.3708381760285793 + }, + { + "x": 4.919165948005501, + "y": 4.716499788885737, + "heading": 3.1212730435084532, + "angularVelocity": -0.0020748595196256386, + "velocityX": 2.8543109373823983, + "velocityY": -2.4676502391371975, + "timestamp": 1.4093837832251535 + }, + { + "x": 5.023524570482215, + "y": 4.6180742282694816, + "heading": 3.121259950874307, + "angularVelocity": -0.00033966605012470416, + "velocityX": 2.7074063704460154, + "velocityY": -2.5534832053444214, + "timestamp": 1.4479293904217276 + }, + { + "x": 5.1214255127288455, + "y": 4.520792255143304, + "heading": 3.1213609572395313, + "angularVelocity": 0.0026204377767055976, + "velocityX": 2.539872877014423, + "velocityY": -2.5238147794653165, + "timestamp": 1.4864749976183018 + }, + { + "x": 5.216508916211957, + "y": 4.429432365379414, + "heading": 3.1215812446063884, + "angularVelocity": 0.005714979809078672, + "velocityX": 2.466776641971333, + "velocityY": -2.3701764327637718, + "timestamp": 1.525020604814876 + }, + { + "x": 5.311195347524098, + "y": 4.3446186668561, + "heading": 3.1218832424332517, + "angularVelocity": 0.00783481825368405, + "velocityX": 2.456477876435081, + "velocityY": -2.200346672210494, + "timestamp": 1.5635662120114502 + }, + { + "x": 5.406488770050426, + "y": 4.2663350523445525, + "heading": 3.1222460060108177, + "angularVelocity": 0.009411281957914153, + "velocityX": 2.4722252276466263, + "velocityY": -2.0309347862213687, + "timestamp": 1.6021118192080244 + }, + { + "x": 5.502899923549765, + "y": 4.194513729448243, + "heading": 3.122658002124391, + "angularVelocity": 0.010688536088486314, + "velocityX": 2.501222850315031, + "velocityY": -1.863281658271366, + "timestamp": 1.6406574264045986 + }, + { + "x": 5.600732691860309, + "y": 4.12909471859619, + "heading": 3.1231121352270255, + "angularVelocity": 0.011781708362225283, + "velocityX": 2.5381042205826043, + "velocityY": -1.6971845979343585, + "timestamp": 1.6792030336011727 + }, + { + "x": 5.700187347441565, + "y": 4.070030241682816, + "heading": 3.1236036443195165, + "angularVelocity": 0.012751364636292445, + "velocityX": 2.580181318043823, + "velocityY": -1.532327059012421, + "timestamp": 1.717748640797747 + }, + { + "x": 5.801405429840088, + "y": 4.017282485961914, + "heading": 3.124129127034147, + "angularVelocity": 0.013632752286159615, + "velocityX": 2.6259304175008134, + "velocityY": -1.3684505072627247, + "timestamp": 1.756294247994321 + }, + { + "x": 5.881714978660041, + "y": 3.979872135421584, + "heading": 3.124559351404669, + "angularVelocity": 0.014270430872906349, + "velocityX": 2.663846921269128, + "velocityY": -1.240891632125537, + "timestamp": 1.7864422073904604 + }, + { + "x": 5.963108059198022, + "y": 3.9463246319250165, + "heading": 3.1250109743561647, + "angularVelocity": 0.014980216258125125, + "velocityX": 2.699787387547169, + "velocityY": -1.11276199678257, + "timestamp": 1.8165901667865998 + }, + { + "x": 6.045516764082525, + "y": 3.91665838172662, + "heading": 3.1254864627529404, + "angularVelocity": 0.015771826893091, + "velocityX": 2.7334753839112973, + "velocityY": -0.9840218307510211, + "timestamp": 1.8467381261827391 + }, + { + "x": 6.128863052724697, + "y": 3.8908929706985393, + "heading": 3.12598863599481, + "angularVelocity": 0.016656956289182748, + "velocityX": 2.7645747941681287, + "velocityY": -0.8546320064163314, + "timestamp": 1.8768860855788785 + }, + { + "x": 6.213056423203702, + "y": 3.8690490451787327, + "heading": 3.1265207413378744, + "angularVelocity": 0.017649796328583447, + "velocityX": 2.792672279165528, + "velocityY": -0.7245573484022891, + "timestamp": 1.9070340449750178 + }, + { + "x": 6.297990860537032, + "y": 3.851148011868822, + "heading": 3.1270865497545324, + "angularVelocity": 0.018767718545174807, + "velocityX": 2.8172532746679417, + "velocityY": -0.5937726356432314, + "timestamp": 1.9371820043711572 + }, + { + "x": 6.383540783527209, + "y": 3.837211412188912, + "heading": 3.127690478291489, + "angularVelocity": 0.020032153056224314, + "velocityX": 2.83766877439583, + "velocityY": -0.46227339956199653, + "timestamp": 1.9673299637672965 + }, + { + "x": 6.469555594214739, + "y": 3.8272597097732, + "heading": 3.1283377457202124, + "angularVelocity": 0.021469692864407197, + "velocityX": 2.8530889788362965, + "velocityY": -0.3300953900377929, + "timestamp": 1.9974779231634359 + }, + { + "x": 6.555852273967605, + "y": 3.8213100114054206, + "heading": 3.1290345673449957, + "angularVelocity": 0.023113392705193338, + "velocityX": 2.8624385026840957, + "velocityY": -0.1973499529305097, + "timestamp": 2.027625882559575 + }, + { + "x": 6.642205282475191, + "y": 3.8193718287910654, + "heading": 3.1297883882708835, + "angularVelocity": 0.02500404475084348, + "velocityX": 2.864306912879999, + "velocityY": -0.06428901501716788, + "timestamp": 2.0577738419557146 + }, + { + "x": 6.7283328946409515, + "y": 3.821439211302739, + "heading": 3.13060813228516, + "angularVelocity": 0.02719069650803073, + "velocityX": 2.856830574635499, + "velocityY": 0.06857454212766521, + "timestamp": 2.087921801351854 + }, + { + "x": 6.813879408595297, + "y": 3.827476175013503, + "heading": 3.131504382371298, + "angularVelocity": 0.029728383084265037, + "velocityX": 2.837555697560754, + "velocityY": 0.20024452174156598, + "timestamp": 2.1180697607479932 + }, + { + "x": 6.898394452961866, + "y": 3.837390144910791, + "heading": 3.1324892640321416, + "angularVelocity": 0.032668269447444956, + "velocityX": 2.8033421186507304, + "velocityY": 0.3288438121804522, + "timestamp": 2.1482177201441326 + }, + { + "x": 6.981316545653192, + "y": 3.850986156064535, + "heading": 3.1335755392972953, + "angularVelocity": 0.03603146902516665, + "velocityX": 2.7505043244135474, + "velocityY": 0.45097616641626476, + "timestamp": 2.178365679540272 + }, + { + "x": 7.061981725860651, + "y": 3.867898952893054, + "heading": 3.1347742460736483, + "angularVelocity": 0.03976079311379567, + "velocityX": 2.675643122226997, + "velocityY": 0.5609930876676483, + "timestamp": 2.2085136389364113 + }, + { + "x": 7.139693035364971, + "y": 3.8875263790502017, + "heading": 3.1360911020047273, + "angularVelocity": 0.0436797699564237, + "velocityX": 2.577663996531448, + "velocityY": 0.651036638972683, + "timestamp": 2.2386615983325506 + }, + { + "x": 7.213860796163365, + "y": 3.9090347061974744, + "heading": 3.1375244322919986, + "angularVelocity": 0.04754319416573168, + "velocityX": 2.4601254043049017, + "velocityY": 0.7134256373592958, + "timestamp": 2.26880955772869 + }, + { + "x": 7.2841296289302875, + "y": 3.9314880045121248, + "heading": 3.1390672357385005, + "angularVelocity": 0.05117439048625725, + "velocityX": 2.330798971950365, + "velocityY": 0.7447700860817068, + "timestamp": 2.2989575171248293 + }, + { + "x": 7.350387407544562, + "y": 3.954020046909829, + "heading": 3.1407107850332614, + "angularVelocity": 0.05451610416363438, + "velocityX": 2.1977533452150904, + "velocityY": 0.7473820069092071, + "timestamp": 2.3291054765209687 + }, + { + "x": 7.412682228157059, + "y": 3.9759276926483125, + "heading": 3.142446557806337, + "angularVelocity": 0.05757513303862974, + "velocityX": 2.0663030553396227, + "velocityY": 0.7266709315420209, + "timestamp": 2.359253435917108 + }, + { + "x": 7.4711363711768914, + "y": 3.9966759270340453, + "heading": 3.1442667998509624, + "angularVelocity": 0.0603769568847877, + "velocityX": 1.9389087749440745, + "velocityY": 0.6882135574452845, + "timestamp": 2.3894013953132474 + }, + { + "x": 7.525893877095674, + "y": 4.0158653092903895, + "heading": 3.1461646469014637, + "angularVelocity": 0.06295109481751612, + "velocityX": 1.8162922803257737, + "velocityY": 0.6365068363068591, + "timestamp": 2.4195493547093867 + }, + { + "x": 7.5770959995446026, + "y": 4.033196068837796, + "heading": 3.1481341124991036, + "angularVelocity": 0.06532666346539286, + "velocityX": 1.6983611320468093, + "velocityY": 0.5748568027335785, + "timestamp": 2.449697314105526 + }, + { + "x": 7.624871879535836, + "y": 4.04843968713685, + "heading": 3.1501700145363154, + "angularVelocity": 0.06753034294826869, + "velocityX": 1.5847135576728835, + "velocityY": 0.5056268684309618, + "timestamp": 2.4798452735016654 + }, + { + "x": 7.669336187729304, + "y": 4.061418611160472, + "heading": 3.152267875920012, + "angularVelocity": 0.06958551841372541, + "velocityX": 1.4748695793706585, + "velocityY": 0.43050754623493903, + "timestamp": 2.5099932328978047 + }, + { + "x": 7.710589606461839, + "y": 4.071992212309712, + "heading": 3.1544238219627907, + "angularVelocity": 0.07151217150223022, + "velocityX": 1.3683652080882895, + "velocityY": 0.35072360985710144, + "timestamp": 2.540141192293944 + }, + { + "x": 7.748720297763379, + "y": 4.080047084432328, + "heading": 3.156634486140592, + "angularVelocity": 0.07332715786014068, + "velocityX": 1.2647851484907588, + "velocityY": 0.2671780207998906, + "timestamp": 2.5702891516900834 + }, + { + "x": 7.783805577887537, + "y": 4.085490267375178, + "heading": 3.158896928505875, + "angularVelocity": 0.07504462691998749, + "velocityX": 1.1637696489882914, + "velocityY": 0.1805489675545591, + "timestamp": 2.6004371110862228 + }, + { + "x": 7.815913495819532, + "y": 4.088244438171387, + "heading": 3.161208567177335, + "angularVelocity": 0.07667645564617116, + "velocityX": 1.0650113166898847, + "velocityY": 0.09135513153707456, + "timestamp": 2.630585070482362 + }, + { + "x": 7.845104217529297, + "y": 4.088244438171387, + "heading": 3.163567121665249, + "angularVelocity": 0.07823264111918607, + "velocityX": 0.968248674021464, + "velocityY": 9.949308093023297e-24, + "timestamp": 2.6607330298785015 + }, + { + "x": 7.878491824221837, + "y": 4.093585686533033, + "heading": 3.1666157231902687, + "angularVelocity": 0.07980374508976079, + "velocityX": 0.8739928887988145, + "velocityY": 0.1398187395813148, + "timestamp": 2.6989342636876534 + }, + { + "x": 7.908226247157587, + "y": 4.10423241668243, + "heading": 3.1697235484523434, + "angularVelocity": 0.08135405462559406, + "velocityX": 0.7783628948818408, + "velocityY": 0.278701211656863, + "timestamp": 2.7371354974968054 + }, + { + "x": 7.934251131583176, + "y": 4.120145380644015, + "heading": 3.1728897318487155, + "angularVelocity": 0.08288170513523152, + "velocityX": 0.6812576932882871, + "velocityY": 0.4165562830007863, + "timestamp": 2.7753367313059574 + }, + { + "x": 7.956505849641918, + "y": 4.141281346281812, + "heading": 3.176113327242148, + "angularVelocity": 0.0843845884543132, + "velocityX": 0.582565426287611, + "velocityY": 0.5532796595887517, + "timestamp": 2.8135379651151093 + }, + { + "x": 7.974925029508088, + "y": 4.167592498355179, + "heading": 3.179393297087888, + "angularVelocity": 0.08586031179323926, + "velocityX": 0.4821619102197233, + "velocityY": 0.6887513687336226, + "timestamp": 2.8517391989242613 + }, + { + "x": 7.989438020301411, + "y": 4.199025720545251, + "heading": 3.1827284996727405, + "angularVelocity": 0.08730614831746564, + "velocityX": 0.37990895440256806, + "velocityY": 0.8228326432362902, + "timestamp": 2.8899404327334133 + }, + { + "x": 7.999968283780463, + "y": 4.235521729180706, + "heading": 3.1861176740734445, + "angularVelocity": 0.0887189774454874, + "velocityX": 0.27565244441213116, + "velocityY": 0.9553620392939309, + "timestamp": 2.9281416665425652 + }, + { + "x": 8.00643270325469, + "y": 4.27701402090998, + "heading": 3.1895594223515835, + "angularVelocity": 0.09009521250892577, + "velocityX": 0.16922017510021273, + "velocityY": 1.0861505661456612, + "timestamp": 2.966342900351717 + }, + { + "x": 8.008740800208267, + "y": 4.323427585324584, + "heading": 3.1930521883927083, + "angularVelocity": 0.09143071290771301, + "velocityX": 0.060419434751950635, + "velocityY": 1.2149755331589407, + "timestamp": 3.004544134160869 + }, + { + "x": 8.006793850403897, + "y": 4.3746773185892955, + "heading": 3.1965942326665227, + "angularVelocity": 0.09272067733493149, + "velocityX": -0.05096562624382283, + "velocityY": 1.341572723036849, + "timestamp": 3.042745367970021 + }, + { + "x": 8.000483894798665, + "y": 4.430666054241619, + "heading": 3.200183602033556, + "angularVelocity": 0.09395951410798249, + "velocityX": -0.16517674891749978, + "velocityY": 1.4656263703951196, + "timestamp": 3.080946601779173 + }, + { + "x": 7.989692648311953, + "y": 4.49128210092684, + "heading": 3.203818093558477, + "angularVelocity": 0.09514068427942153, + "velocityX": -0.2824842396615403, + "velocityY": 1.5867562547338276, + "timestamp": 3.119147835588325 + }, + { + "x": 7.974290324535485, + "y": 4.556396142126618, + "heading": 3.207495211129814, + "angularVelocity": 0.09625651332906135, + "velocityX": -0.4031891706277398, + "velocityY": 1.7045009992367957, + "timestamp": 3.157349069397477 + }, + { + "x": 7.954134422332773, + "y": 4.625857308190122, + "heading": 3.2112121135725333, + "angularVelocity": 0.09729796847109581, + "velocityX": -0.5276243773541068, + "velocityY": 1.8182964039989622, + "timestamp": 3.195550303206629 + }, + { + "x": 7.929068570159579, + "y": 4.699488175513398, + "heading": 3.2149655529585908, + "angularVelocity": 0.09825440206483436, + "velocityX": -0.6561529477927303, + "velocityY": 1.9274473618083952, + "timestamp": 3.233751537015781 + }, + { + "x": 7.898921611054975, + "y": 4.7770783843465825, + "heading": 3.2187518021293573, + "angularVelocity": 0.09911326921224117, + "velocityX": -0.7891619222356322, + "velocityY": 2.031091697739763, + "timestamp": 3.271952770824933 + }, + { + "x": 7.863507259665322, + "y": 4.858376507302923, + "heading": 3.2225665713147067, + "angularVelocity": 0.09985984233932846, + "velocityX": -0.9270473191148296, + "velocityY": 2.1281543774867226, + "timestamp": 3.310154004634085 + }, + { + "x": 7.82262490744023, + "y": 4.943079777954261, + "heading": 3.226404915584542, + "angularVelocity": 0.10047697121541134, + "velocityX": -1.0701840791146968, + "velocityY": 2.217291490492266, + "timestamp": 3.348355238443237 + }, + { + "x": 7.776062535619055, + "y": 5.030821379568998, + "heading": 3.2302611382194035, + "angularVelocity": 0.1009449761263412, + "velocityX": -1.2188708891915252, + "velocityY": 2.2968263813960155, + "timestamp": 3.386556472252389 + }, + { + "x": 7.723603242812097, + "y": 5.121155356689003, + "heading": 3.23412870026526, + "angularVelocity": 0.10124180975877002, + "velocityX": -1.373235562731729, + "velocityY": 2.364687422696869, + "timestamp": 3.424757706061541 + }, + { + "x": 7.665037532882504, + "y": 5.213540129232238, + "heading": 3.2380001528589366, + "angularVelocity": 0.10134365327094359, + "velocityX": -1.5330842512097755, + "velocityY": 2.4183714328384474, + "timestamp": 3.462958939870693 + }, + { + "x": 7.600183883971553, + "y": 5.307323440188604, + "heading": 3.2418671133682277, + "angularVelocity": 0.10122606323685251, + "velocityX": -1.6976846673317019, + "velocityY": 2.4549812036148855, + "timestamp": 3.501160173679845 + }, + { + "x": 7.528919309283333, + "y": 5.401734523083348, + "heading": 3.2457203037789037, + "angularVelocity": 0.10086560109356, + "velocityX": -1.8655045291010028, + "velocityY": 2.4714144932178943, + "timestamp": 3.5393614074889967 + }, + { + "x": 7.451218127874509, + "y": 5.495892378933352, + "heading": 3.249549659566478, + "angularVelocity": 0.10024167823230529, + "velocityX": -2.0339966451609546, + "velocityY": 2.4647857270894256, + "timestamp": 3.5775626412981487 + }, + { + "x": 7.36719026389099, + "y": 5.588839011710391, + "heading": 3.253344517033594, + "angularVelocity": 0.09933861000601822, + "velocityX": -2.1996112587177326, + "velocityY": 2.433079340876453, + "timestamp": 3.6157638751073007 + }, + { + "x": 7.277103414955576, + "y": 5.6795987285633585, + "heading": 3.257093918809728, + "angularVelocity": 0.0981487088837405, + "velocityX": -2.3582183074367444, + "velocityY": 2.375832081926742, + "timestamp": 3.6539651089164527 + }, + { + "x": 7.181374686983581, + "y": 5.767249906226262, + "heading": 3.260787096257733, + "angularVelocity": 0.09667691537021512, + "velocityX": -2.5059067057949522, + "velocityY": 2.2944593386903587, + "timestamp": 3.6921663427256046 + }, + { + "x": 7.080531109805766, + "y": 5.8509854681858595, + "heading": 3.2644140920664264, + "angularVelocity": 0.09494446767906256, + "velocityX": -2.6397989573220397, + "velocityY": 2.1919596204125034, + "timestamp": 3.7303675765347566 + }, + { + "x": 6.97515470473837, + "y": 5.930144227513004, + "heading": 3.267966327996027, + "angularVelocity": 0.09298746598989815, + "velocityX": -2.758455540829955, + "velocityY": 2.0721519028053215, + "timestamp": 3.7685688103439086 + }, + { + "x": 6.8658318519592285, + "y": 6.004211902618408, + "heading": 3.271436911327556, + "angularVelocity": 0.09085003245884067, + "velocityX": -2.8617623536795467, + "velocityY": 1.9388817511872807, + "timestamp": 3.8067700441530605 + }, + { + "x": 6.76937026572913, + "y": 6.063823537641736, + "heading": 3.274355520630392, + "angularVelocity": 0.08889560431653577, + "velocityX": -2.9380468954592764, + "velocityY": 1.8156634786800647, + "timestamp": 3.839601918409111 + }, + { + "x": 6.670811763453878, + "y": 6.119164152757477, + "heading": 3.2771960124627224, + "angularVelocity": 0.08651628628259647, + "velocityX": -3.00191519700062, + "velocityY": 1.6855758731331847, + "timestamp": 3.8724337926651615 + }, + { + "x": 6.570666307865554, + "y": 6.170019228878687, + "heading": 3.2799422951123653, + "angularVelocity": 0.08364684355894975, + "velocityX": -3.0502509484322142, + "velocityY": 1.5489544009762712, + "timestamp": 3.905265666921212 + }, + { + "x": 6.46955818993848, + "y": 6.216214742131844, + "heading": 3.2825768229193018, + "angularVelocity": 0.08024299150240671, + "velocityX": -3.079571916563354, + "velocityY": 1.4070324737748963, + "timestamp": 3.9380975411772625 + }, + { + "x": 6.368220806913084, + "y": 6.257657843766639, + "heading": 3.285082327525109, + "angularVelocity": 0.07631317622219781, + "velocityX": -3.086554920230331, + "velocityY": 1.2622825401799327, + "timestamp": 3.970929415433313 + }, + { + "x": 6.267462404278832, + "y": 6.2943783920802225, + "heading": 3.2874445528955567, + "angularVelocity": 0.07194914771010016, + "velocityX": -3.068920216020964, + "velocityY": 1.118442036759947, + "timestamp": 4.003761289689364 + }, + { + "x": 6.1681022663365965, + "y": 6.326551148740925, + "heading": 3.289654959048111, + "angularVelocity": 0.06732500664797306, + "velocityX": -3.026331581539983, + "velocityY": 0.9799244602909629, + "timestamp": 4.0365931639454145 + }, + { + "x": 6.070897503286478, + "y": 6.354482225299535, + "heading": 3.2917117995402965, + "angularVelocity": 0.06264767208064463, + "velocityX": -2.960682728376521, + "velocityY": 0.8507304925932898, + "timestamp": 4.069425038201465 + }, + { + "x": 5.976489922312775, + "y": 6.378564495957317, + "heading": 3.2936190060486643, + "angularVelocity": 0.058090089328853824, + "velocityX": -2.875485579575279, + "velocityY": 0.7335027683758681, + "timestamp": 4.1022569124575154 + }, + { + "x": 5.885387749283588, + "y": 6.399224497871891, + "heading": 3.295383909871142, + "angularVelocity": 0.05375580476197676, + "velocityX": -2.774808782425772, + "velocityY": 0.6292666009089228, + "timestamp": 4.135088786713566 + }, + { + "x": 5.79797544997618, + "y": 6.4168807330266695, + "heading": 3.2970151433758272, + "angularVelocity": 0.049684446643635256, + "velocityX": -2.6624218473088823, + "velocityY": 0.5377772531985251, + "timestamp": 4.167920660969616 + }, + { + "x": 5.7145364982554065, + "y": 6.431919635509067, + "heading": 3.298521304612775, + "angularVelocity": 0.04587496970782373, + "velocityX": -2.541400806729675, + "velocityY": 0.4580579946521674, + "timestamp": 4.200752535225667 + }, + { + "x": 5.635277634199153, + "y": 6.444685893070414, + "heading": 3.2999103137042596, + "angularVelocity": 0.04230672549035185, + "velocityX": -2.4140828342033305, + "velocityY": 0.3888373067520809, + "timestamp": 4.233584409481717 + }, + { + "x": 5.5603494135057785, + "y": 6.455481443626585, + "heading": 3.301189197207777, + "angularVelocity": 0.038952497610812056, + "velocityX": -2.2821792051535352, + "velocityY": 0.3288131061900009, + "timestamp": 4.266416283737768 + }, + { + "x": 5.489861905204234, + "y": 6.4645686834525575, + "heading": 3.302364085244027, + "angularVelocity": 0.03578498221232499, + "velocityX": -2.146923070910393, + "velocityY": 0.27678102550899025, + "timestamp": 4.299248157993818 + }, + { + "x": 5.423896104976939, + "y": 6.472175218090503, + "heading": 3.3034402954817073, + "angularVelocity": 0.03277943346417197, + "velocityX": -2.009199953461021, + "velocityY": 0.23168140139134377, + "timestamp": 4.332080032249869 + }, + { + "x": 5.3625120552036805, + "y": 6.478498802228473, + "heading": 3.304422441816652, + "angularVelocity": 0.029914415707270804, + "velocityX": -1.8696480528201007, + "velocityY": 0.19260503036328994, + "timestamp": 4.364911906505919 + }, + { + "x": 5.305754584196663, + "y": 6.483711879532544, + "heading": 3.305314540782059, + "angularVelocity": 0.027171734347235434, + "velocityX": -1.728730762196986, + "velocityY": 0.15878098409538036, + "timestamp": 4.39774378076197 + }, + { + "x": 5.253657370247068, + "y": 6.487965524914462, + "heading": 3.3061201058429943, + "angularVelocity": 0.024536066831048555, + "velocityX": -1.5867876912325205, + "velocityY": 0.12955840865933993, + "timestamp": 4.43057565501802 + }, + { + "x": 5.206245838192873, + "y": 6.491392772869967, + "heading": 3.3068422273399647, + "angularVelocity": 0.021994525543636716, + "velocityX": -1.444070225307262, + "velocityY": 0.1043878253424375, + "timestamp": 4.463407529274071 + }, + { + "x": 5.163539242392539, + "y": 6.494111389192325, + "heading": 3.3074836389039146, + "angularVelocity": 0.019536245751546197, + "velocityX": -1.3007663061594368, + "velocityY": 0.08280417685432861, + "timestamp": 4.496239403530121 + }, + { + "x": 5.125552179765353, + "y": 6.496226165889178, + "heading": 3.308046772172593, + "angularVelocity": 0.01715202928369231, + "velocityX": -1.1570177910323254, + "velocityY": 0.06441230495586951, + "timestamp": 4.529071277786172 + }, + { + "x": 5.092295700352608, + "y": 6.497830819141238, + "heading": 3.308533801781415, + "angularVelocity": 0.01483404831001049, + "velocityX": -1.0129327114676103, + "velocityY": 0.04887485982510178, + "timestamp": 4.561903152042222 + }, + { + "x": 5.063778130929228, + "y": 6.499009561458986, + "heading": 3.308946682418035, + "angularVelocity": 0.012575603616169764, + "velocityX": -0.8685940132743156, + "velocityY": 0.035902376713398354, + "timestamp": 4.594735026298273 + }, + { + "x": 5.040005691939611, + "y": 6.499838408157472, + "heading": 3.3092871794520544, + "angularVelocity": 0.010370928913894002, + "velocityX": -0.7240658515020671, + "velocityY": 0.025245183751066175, + "timestamp": 4.627566900554323 + }, + { + "x": 5.020982964030012, + "y": 6.50038626757828, + "heading": 3.3095568943744427, + "angularVelocity": 0.008215032753991694, + "velocityX": -0.5793981714611864, + "velocityY": 0.01668681527396481, + "timestamp": 4.660398774810374 + }, + { + "x": 5.006713244009331, + "y": 6.500715855130694, + "heading": 3.3097572860387627, + "angularVelocity": 0.00610357065688574, + "velocityX": -0.4346300765345807, + "velocityY": 0.010038645672288071, + "timestamp": 4.693230649066424 + }, + { + "x": 4.997198818717405, + "y": 6.500884463422624, + "heading": 3.30988968849574, + "angularVelocity": 0.004032741352049697, + "velocityX": -0.2897923285684355, + "velocityY": 0.005135506143067766, + "timestamp": 4.726062523322475 + }, + { + "x": 4.992441177368164, + "y": 6.5009446144104, + "heading": 3.309955326051445, + "angularVelocity": 0.001999202214064957, + "velocityX": -0.14490922181708896, + "velocityY": 0.0018320911961180106, + "timestamp": 4.758894397578525 + }, + { + "x": 4.992441177368164, + "y": 6.5009446144104, + "heading": 3.309955326051445, + "angularVelocity": 5.207192621874317e-25, + "velocityX": 4.1285632704076663e-23, + "velocityY": -5.50747176857669e-23, + "timestamp": 4.791726271834576 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/Robot/src/main/deploy/choreo/New Path (2).traj b/Robot/src/main/deploy/choreo/New Path (2).traj new file mode 100644 index 0000000..cfd1669 --- /dev/null +++ b/Robot/src/main/deploy/choreo/New Path (2).traj @@ -0,0 +1,1301 @@ +{ + "samples": [ + { + "x": 1.302428960800171, + "y": 5.521671772003174, + "heading": 3.119698237349717, + "angularVelocity": -1.0799521907829878e-24, + "velocityX": -5.729727943881475e-23, + "velocityY": 3.806127884713555e-22, + "timestamp": 0 + }, + { + "x": 1.3217771089315806, + "y": 5.522383365283616, + "heading": 3.1197999674258803, + "angularVelocity": 0.0015360380238209192, + "velocityX": 0.29214065634450126, + "velocityY": 0.010744456088856187, + "timestamp": 0.0662288788336051 + }, + { + "x": 1.3604749484171008, + "y": 5.523763370471075, + "heading": 3.1199951995851856, + "angularVelocity": 0.002947840318953084, + "velocityX": 0.5843046140452638, + "velocityY": 0.020836909997010808, + "timestamp": 0.1324577576672102 + }, + { + "x": 1.4185241489686518, + "y": 5.525761671584798, + "heading": 3.1202743864187497, + "angularVelocity": 0.00421548482295459, + "velocityX": 0.8764937829824225, + "velocityY": 0.030172655024753685, + "timestamp": 0.1986866365008153 + }, + { + "x": 1.4959265053667201, + "y": 5.528319403030693, + "heading": 3.1206263164550307, + "angularVelocity": 0.005313845598465322, + "velocityX": 1.1687100515854367, + "velocityY": 0.03861957941822778, + "timestamp": 0.2649155153344204 + }, + { + "x": 1.5926839210326802, + "y": 5.53136644125991, + "heading": 3.121037637902798, + "angularVelocity": 0.006210605630214725, + "velocityX": 1.4609550602397396, + "velocityY": 0.04600769759175801, + "timestamp": 0.33114439416802555 + }, + { + "x": 1.7087983602256251, + "y": 5.5348178374547645, + "heading": 3.121492181382272, + "angularVelocity": 0.006863221716555551, + "velocityX": 1.7532297275433801, + "velocityY": 0.052113160537196716, + "timestamp": 0.39737327300163067 + }, + { + "x": 1.844271734223262, + "y": 5.538568567278952, + "heading": 3.1219699631830324, + "angularVelocity": 0.007214100694065514, + "velocityX": 2.0455332535222754, + "velocityY": 0.05663284491967778, + "timestamp": 0.4636021518352358 + }, + { + "x": 1.9991056443900284, + "y": 5.542485474471211, + "heading": 3.1224456556980607, + "angularVelocity": 0.007182554248321628, + "velocityX": 2.3378609587484425, + "velocityY": 0.05914198248923271, + "timestamp": 0.5298310306688409 + }, + { + "x": 2.1733007984970207, + "y": 5.546394249267978, + "heading": 3.1228861145568447, + "angularVelocity": 0.006650555868398608, + "velocityX": 2.6301993507189527, + "velocityY": 0.059019190202322734, + "timestamp": 0.5960599095024459 + }, + { + "x": 2.36685562133789, + "y": 5.5500569343566895, + "heading": 3.1232461040248376, + "angularVelocity": 0.005435536194070859, + "velocityX": 2.922513958407195, + "velocityY": 0.055303443954021286, + "timestamp": 0.662288788336051 + }, + { + "x": 2.4263388144591955, + "y": 5.551097384925455, + "heading": 3.123340540157932, + "angularVelocity": 0.004778249196920622, + "velocityX": 3.009711542072027, + "velocityY": 0.0526443844294007, + "timestamp": 0.6820525405394778 + }, + { + "x": 2.4875382986996155, + "y": 5.5519733808753085, + "heading": 3.1234216636032834, + "angularVelocity": 0.00410465808903763, + "velocityX": 3.096551890071197, + "velocityY": 0.044323362327003256, + "timestamp": 0.7018162927429047 + }, + { + "x": 2.550436955892463, + "y": 5.552556955429702, + "heading": 3.123489130605961, + "angularVelocity": 0.003413673779285827, + "velocityX": 3.1825260985584145, + "velocityY": 0.02952751827621247, + "timestamp": 0.7215800449463315 + }, + { + "x": 2.615003335084898, + "y": 5.552703036227377, + "heading": 3.1235425841754783, + "angularVelocity": 0.0027046265793564048, + "velocityX": 3.26690896181343, + "velocityY": 0.007391349384069019, + "timestamp": 0.7413437971497584 + }, + { + "x": 2.6811863409244094, + "y": 5.55224977213001, + "heading": 3.123581673782466, + "angularVelocity": 0.0019778434067488013, + "velocityX": 3.3487065187973872, + "velocityY": -0.022934111534127874, + "timestamp": 0.7611075493531853 + }, + { + "x": 2.7489093580691275, + "y": 5.55102122741091, + "heading": 3.123606094755385, + "angularVelocity": 0.001235644561187388, + "velocityX": 3.426627517267461, + "velocityY": -0.062161511966688114, + "timestamp": 0.7808713015566121 + }, + { + "x": 2.8180650275496353, + "y": 5.54883341134946, + "heading": 3.1236156557026797, + "angularVelocity": 0.00048376174709457927, + "velocityX": 3.4991164010099673, + "velocityY": -0.11069841591471039, + "timestamp": 0.800635053760039 + }, + { + "x": 2.8885126788941395, + "y": 5.54550389601763, + "heading": 3.1236103737324172, + "angularVelocity": -0.0002672554385466297, + "velocityX": 3.564487685303465, + "velocityY": -0.1684657496997351, + "timestamp": 0.8203988059634658 + }, + { + "x": 2.9600805496923224, + "y": 5.540863730529652, + "heading": 3.1235905765119973, + "angularVelocity": -0.0010016934141146043, + "velocityX": 3.6211681902070074, + "velocityY": -0.23478160625658828, + "timestamp": 0.8401625581668927 + }, + { + "x": 3.032573643140278, + "y": 5.534768690288578, + "heading": 3.123556968357328, + "angularVelocity": -0.0017004946390557507, + "velocityX": 3.6679823093201, + "velocityY": -0.3083948927480261, + "timestamp": 0.8599263103703195 + }, + { + "x": 3.105785678320405, + "y": 5.527106543467365, + "heading": 3.1235106204259195, + "angularVelocity": -0.0023450977795856365, + "velocityX": 3.704359092674332, + "velocityY": -0.38768684925548386, + "timestamp": 0.8796900625737464 + }, + { + "x": 3.1795117584951056, + "y": 5.517798656367154, + "heading": 3.1234528819071974, + "angularVelocity": -0.0029214350659676155, + "velocityX": 3.7303685765660113, + "velocityY": -0.4709574884568884, + "timestamp": 0.8994538147771732 + }, + { + "x": 3.2535585488164687, + "y": 5.506796705703182, + "heading": 3.1233852496472454, + "angularVelocity": -0.003422035413907008, + "velocityX": 3.7465957657839812, + "velocityY": -0.556673173733965, + "timestamp": 0.9192175669806001 + }, + { + "x": 3.3277504743592434, + "y": 5.494076719862022, + "heading": 3.123309243507978, + "angularVelocity": -0.003845734275821943, + "velocityX": 3.7539392712032984, + "velocityY": -0.6436017670244403, + "timestamp": 0.9389813191840269 + }, + { + "x": 3.401932191914863, + "y": 5.479632608953573, + "heading": 3.1232263159056575, + "angularVelocity": -0.004195944245161604, + "velocityX": 3.7534227707407335, + "velocityY": -0.7308384946226822, + "timestamp": 0.9587450713874538 + }, + { + "x": 3.4759684441746432, + "y": 5.463470489536251, + "heading": 3.1231378004849635, + "angularVelocity": -0.004478674888388461, + "velocityX": 3.7460625643213086, + "velocityY": -0.8177657385584958, + "timestamp": 0.9785088235908806 + }, + { + "x": 3.5497424561199664, + "y": 5.44560427359136, + "heading": 3.1230448916469586, + "angularVelocity": -0.004700971609475635, + "velocityX": 3.732793812934527, + "velocityY": -0.9039890685228098, + "timestamp": 0.9982725757943075 + }, + { + "x": 3.6231537261374727, + "y": 5.426052491121333, + "heading": 3.1229486436831992, + "angularVelocity": -0.004869923624246792, + "velocityX": 3.7144399131242594, + "velocityY": -0.9892748233625418, + "timestamp": 1.0180363279977345 + }, + { + "x": 3.6961157220105543, + "y": 5.404836115617309, + "heading": 3.1228499803373504, + "angularVelocity": -0.004992136353107221, + "velocityX": 3.6917076839503324, + "velocityY": -1.0734993682194114, + "timestamp": 1.0378000802011613 + }, + { + "x": 3.7685537383740204, + "y": 5.381977131361945, + "heading": 3.122749708770004, + "angularVelocity": -0.0050735086290428135, + "velocityX": 3.6651955366504962, + "velocityY": -1.1566115593880046, + "timestamp": 1.0575638324045882 + }, + { + "x": 3.8404030175360155, + "y": 5.357497620278821, + "heading": 3.1226485344894, + "angularVelocity": -0.005119183825147258, + "velocityX": 3.6354068004119586, + "velocityY": -1.2386064564642996, + "timestamp": 1.077327584608015 + }, + { + "x": 3.9116071530358583, + "y": 5.331419200542643, + "heading": 3.12254707551822, + "angularVelocity": -0.005133588507666248, + "velocityX": 3.6027640281533504, + "velocityY": -1.319507524065004, + "timestamp": 1.0970913368114419 + }, + { + "x": 3.982116756748269, + "y": 5.303762698026613, + "heading": 3.1224458750644937, + "angularVelocity": -0.005120508124394676, + "velocityX": 3.567622331360009, + "velocityY": -1.399354850807865, + "timestamp": 1.1168550890148687 + }, + { + "x": 4.051888355776174, + "y": 5.2745479692393795, + "heading": 3.122345412497445, + "angularVelocity": -0.005083172770798156, + "velocityX": 3.5302810068528747, + "velocityY": -1.478197484289892, + "timestamp": 1.1366188412182956 + }, + { + "x": 4.120883482732203, + "y": 5.24379382125935, + "heading": 3.122246112688224, + "angularVelocity": -0.005024339922835519, + "velocityX": 3.4909933218078715, + "velocityY": -1.5560885232459687, + "timestamp": 1.1563825934217224 + }, + { + "x": 4.1890679256640215, + "y": 5.211517992630351, + "heading": 3.122148353884756, + "angularVelocity": -0.0049463686076292, + "velocityX": 3.44997459136313, + "velocityY": -1.6330820330464426, + "timestamp": 1.1761463456251493 + }, + { + "x": 4.256411108437102, + "y": 5.177737171593987, + "heading": 3.1220524743221056, + "angularVelocity": -0.004851283382997846, + "velocityX": 3.407408779462607, + "velocityY": -1.7092311565466005, + "timestamp": 1.1959100978285762 + }, + { + "x": 4.3228855772355175, + "y": 5.14246703628049, + "heading": 3.1219587777652085, + "angularVelocity": -0.004740828357538679, + "velocityX": 3.363453868182489, + "velocityY": -1.7845870030379125, + "timestamp": 1.215673850032003 + }, + { + "x": 4.388466573286225, + "y": 5.105722306923142, + "heading": 3.1218675381607466, + "angularVelocity": -0.0046165122656358646, + "velocityX": 3.3182462204386316, + "velocityY": -1.8591980398831336, + "timestamp": 1.2354376022354299 + }, + { + "x": 4.453131675720215, + "y": 5.067516803741455, + "heading": 3.1217790035501856, + "angularVelocity": -0.004479645851134587, + "velocityX": 3.2719041287503026, + "velocityY": -1.9331098056907883, + "timestamp": 1.2552013544388567 + }, + { + "x": 4.5756878518534965, + "y": 4.98749687604741, + "heading": 3.1216174796705585, + "angularVelocity": -0.004190461413754277, + "velocityX": 3.179510845639871, + "velocityY": -2.0759804686941523, + "timestamp": 1.293746961635431 + }, + { + "x": 4.6944682593112, + "y": 4.902114649138773, + "heading": 3.12147264475533, + "angularVelocity": -0.003757494712439645, + "velocityX": 3.081554970763572, + "velocityY": -2.2150961709646055, + "timestamp": 1.332292568832005 + }, + { + "x": 4.8091447997962735, + "y": 4.811616865702052, + "heading": 3.121353020228485, + "angularVelocity": -0.0031034542077534163, + "velocityX": 2.975087145475454, + "velocityY": -2.347810555304065, + "timestamp": 1.3708381760285793 + }, + { + "x": 4.919165948005501, + "y": 4.716499788885737, + "heading": 3.1212730435084532, + "angularVelocity": -0.0020748595196256386, + "velocityX": 2.8543109373823983, + "velocityY": -2.4676502391371975, + "timestamp": 1.4093837832251535 + }, + { + "x": 5.023524570482215, + "y": 4.6180742282694816, + "heading": 3.121259950874307, + "angularVelocity": -0.00033966605012470416, + "velocityX": 2.7074063704460154, + "velocityY": -2.5534832053444214, + "timestamp": 1.4479293904217276 + }, + { + "x": 5.1214255127288455, + "y": 4.520792255143304, + "heading": 3.1213609572395313, + "angularVelocity": 0.0026204377767055976, + "velocityX": 2.539872877014423, + "velocityY": -2.5238147794653165, + "timestamp": 1.4864749976183018 + }, + { + "x": 5.216508916211957, + "y": 4.429432365379414, + "heading": 3.1215812446063884, + "angularVelocity": 0.005714979809078672, + "velocityX": 2.466776641971333, + "velocityY": -2.3701764327637718, + "timestamp": 1.525020604814876 + }, + { + "x": 5.311195347524098, + "y": 4.3446186668561, + "heading": 3.1218832424332517, + "angularVelocity": 0.00783481825368405, + "velocityX": 2.456477876435081, + "velocityY": -2.200346672210494, + "timestamp": 1.5635662120114502 + }, + { + "x": 5.406488770050426, + "y": 4.2663350523445525, + "heading": 3.1222460060108177, + "angularVelocity": 0.009411281957914153, + "velocityX": 2.4722252276466263, + "velocityY": -2.0309347862213687, + "timestamp": 1.6021118192080244 + }, + { + "x": 5.502899923549765, + "y": 4.194513729448243, + "heading": 3.122658002124391, + "angularVelocity": 0.010688536088486314, + "velocityX": 2.501222850315031, + "velocityY": -1.863281658271366, + "timestamp": 1.6406574264045986 + }, + { + "x": 5.600732691860309, + "y": 4.12909471859619, + "heading": 3.1231121352270255, + "angularVelocity": 0.011781708362225283, + "velocityX": 2.5381042205826043, + "velocityY": -1.6971845979343585, + "timestamp": 1.6792030336011727 + }, + { + "x": 5.700187347441565, + "y": 4.070030241682816, + "heading": 3.1236036443195165, + "angularVelocity": 0.012751364636292445, + "velocityX": 2.580181318043823, + "velocityY": -1.532327059012421, + "timestamp": 1.717748640797747 + }, + { + "x": 5.801405429840088, + "y": 4.017282485961914, + "heading": 3.124129127034147, + "angularVelocity": 0.013632752286159615, + "velocityX": 2.6259304175008134, + "velocityY": -1.3684505072627247, + "timestamp": 1.756294247994321 + }, + { + "x": 5.881714978660041, + "y": 3.979872135421584, + "heading": 3.124559351404669, + "angularVelocity": 0.014270430872906349, + "velocityX": 2.663846921269128, + "velocityY": -1.240891632125537, + "timestamp": 1.7864422073904604 + }, + { + "x": 5.963108059198022, + "y": 3.9463246319250165, + "heading": 3.1250109743561647, + "angularVelocity": 0.014980216258125125, + "velocityX": 2.699787387547169, + "velocityY": -1.11276199678257, + "timestamp": 1.8165901667865998 + }, + { + "x": 6.045516764082525, + "y": 3.91665838172662, + "heading": 3.1254864627529404, + "angularVelocity": 0.015771826893091, + "velocityX": 2.7334753839112973, + "velocityY": -0.9840218307510211, + "timestamp": 1.8467381261827391 + }, + { + "x": 6.128863052724697, + "y": 3.8908929706985393, + "heading": 3.12598863599481, + "angularVelocity": 0.016656956289182748, + "velocityX": 2.7645747941681287, + "velocityY": -0.8546320064163314, + "timestamp": 1.8768860855788785 + }, + { + "x": 6.213056423203702, + "y": 3.8690490451787327, + "heading": 3.1265207413378744, + "angularVelocity": 0.017649796328583447, + "velocityX": 2.792672279165528, + "velocityY": -0.7245573484022891, + "timestamp": 1.9070340449750178 + }, + { + "x": 6.297990860537032, + "y": 3.851148011868822, + "heading": 3.1270865497545324, + "angularVelocity": 0.018767718545174807, + "velocityX": 2.8172532746679417, + "velocityY": -0.5937726356432314, + "timestamp": 1.9371820043711572 + }, + { + "x": 6.383540783527209, + "y": 3.837211412188912, + "heading": 3.127690478291489, + "angularVelocity": 0.020032153056224314, + "velocityX": 2.83766877439583, + "velocityY": -0.46227339956199653, + "timestamp": 1.9673299637672965 + }, + { + "x": 6.469555594214739, + "y": 3.8272597097732, + "heading": 3.1283377457202124, + "angularVelocity": 0.021469692864407197, + "velocityX": 2.8530889788362965, + "velocityY": -0.3300953900377929, + "timestamp": 1.9974779231634359 + }, + { + "x": 6.555852273967605, + "y": 3.8213100114054206, + "heading": 3.1290345673449957, + "angularVelocity": 0.023113392705193338, + "velocityX": 2.8624385026840957, + "velocityY": -0.1973499529305097, + "timestamp": 2.027625882559575 + }, + { + "x": 6.642205282475191, + "y": 3.8193718287910654, + "heading": 3.1297883882708835, + "angularVelocity": 0.02500404475084348, + "velocityX": 2.864306912879999, + "velocityY": -0.06428901501716788, + "timestamp": 2.0577738419557146 + }, + { + "x": 6.7283328946409515, + "y": 3.821439211302739, + "heading": 3.13060813228516, + "angularVelocity": 0.02719069650803073, + "velocityX": 2.856830574635499, + "velocityY": 0.06857454212766521, + "timestamp": 2.087921801351854 + }, + { + "x": 6.813879408595297, + "y": 3.827476175013503, + "heading": 3.131504382371298, + "angularVelocity": 0.029728383084265037, + "velocityX": 2.837555697560754, + "velocityY": 0.20024452174156598, + "timestamp": 2.1180697607479932 + }, + { + "x": 6.898394452961866, + "y": 3.837390144910791, + "heading": 3.1324892640321416, + "angularVelocity": 0.032668269447444956, + "velocityX": 2.8033421186507304, + "velocityY": 0.3288438121804522, + "timestamp": 2.1482177201441326 + }, + { + "x": 6.981316545653192, + "y": 3.850986156064535, + "heading": 3.1335755392972953, + "angularVelocity": 0.03603146902516665, + "velocityX": 2.7505043244135474, + "velocityY": 0.45097616641626476, + "timestamp": 2.178365679540272 + }, + { + "x": 7.061981725860651, + "y": 3.867898952893054, + "heading": 3.1347742460736483, + "angularVelocity": 0.03976079311379567, + "velocityX": 2.675643122226997, + "velocityY": 0.5609930876676483, + "timestamp": 2.2085136389364113 + }, + { + "x": 7.139693035364971, + "y": 3.8875263790502017, + "heading": 3.1360911020047273, + "angularVelocity": 0.0436797699564237, + "velocityX": 2.577663996531448, + "velocityY": 0.651036638972683, + "timestamp": 2.2386615983325506 + }, + { + "x": 7.213860796163365, + "y": 3.9090347061974744, + "heading": 3.1375244322919986, + "angularVelocity": 0.04754319416573168, + "velocityX": 2.4601254043049017, + "velocityY": 0.7134256373592958, + "timestamp": 2.26880955772869 + }, + { + "x": 7.2841296289302875, + "y": 3.9314880045121248, + "heading": 3.1390672357385005, + "angularVelocity": 0.05117439048625725, + "velocityX": 2.330798971950365, + "velocityY": 0.7447700860817068, + "timestamp": 2.2989575171248293 + }, + { + "x": 7.350387407544562, + "y": 3.954020046909829, + "heading": 3.1407107850332614, + "angularVelocity": 0.05451610416363438, + "velocityX": 2.1977533452150904, + "velocityY": 0.7473820069092071, + "timestamp": 2.3291054765209687 + }, + { + "x": 7.412682228157059, + "y": 3.9759276926483125, + "heading": 3.142446557806337, + "angularVelocity": 0.05757513303862974, + "velocityX": 2.0663030553396227, + "velocityY": 0.7266709315420209, + "timestamp": 2.359253435917108 + }, + { + "x": 7.4711363711768914, + "y": 3.9966759270340453, + "heading": 3.1442667998509624, + "angularVelocity": 0.0603769568847877, + "velocityX": 1.9389087749440745, + "velocityY": 0.6882135574452845, + "timestamp": 2.3894013953132474 + }, + { + "x": 7.525893877095674, + "y": 4.0158653092903895, + "heading": 3.1461646469014637, + "angularVelocity": 0.06295109481751612, + "velocityX": 1.8162922803257737, + "velocityY": 0.6365068363068591, + "timestamp": 2.4195493547093867 + }, + { + "x": 7.5770959995446026, + "y": 4.033196068837796, + "heading": 3.1481341124991036, + "angularVelocity": 0.06532666346539286, + "velocityX": 1.6983611320468093, + "velocityY": 0.5748568027335785, + "timestamp": 2.449697314105526 + }, + { + "x": 7.624871879535836, + "y": 4.04843968713685, + "heading": 3.1501700145363154, + "angularVelocity": 0.06753034294826869, + "velocityX": 1.5847135576728835, + "velocityY": 0.5056268684309618, + "timestamp": 2.4798452735016654 + }, + { + "x": 7.669336187729304, + "y": 4.061418611160472, + "heading": 3.152267875920012, + "angularVelocity": 0.06958551841372541, + "velocityX": 1.4748695793706585, + "velocityY": 0.43050754623493903, + "timestamp": 2.5099932328978047 + }, + { + "x": 7.710589606461839, + "y": 4.071992212309712, + "heading": 3.1544238219627907, + "angularVelocity": 0.07151217150223022, + "velocityX": 1.3683652080882895, + "velocityY": 0.35072360985710144, + "timestamp": 2.540141192293944 + }, + { + "x": 7.748720297763379, + "y": 4.080047084432328, + "heading": 3.156634486140592, + "angularVelocity": 0.07332715786014068, + "velocityX": 1.2647851484907588, + "velocityY": 0.2671780207998906, + "timestamp": 2.5702891516900834 + }, + { + "x": 7.783805577887537, + "y": 4.085490267375178, + "heading": 3.158896928505875, + "angularVelocity": 0.07504462691998749, + "velocityX": 1.1637696489882914, + "velocityY": 0.1805489675545591, + "timestamp": 2.6004371110862228 + }, + { + "x": 7.815913495819532, + "y": 4.088244438171387, + "heading": 3.161208567177335, + "angularVelocity": 0.07667645564617116, + "velocityX": 1.0650113166898847, + "velocityY": 0.09135513153707456, + "timestamp": 2.630585070482362 + }, + { + "x": 7.845104217529297, + "y": 4.088244438171387, + "heading": 3.163567121665249, + "angularVelocity": 0.07823264111918607, + "velocityX": 0.968248674021464, + "velocityY": 9.949308093023297e-24, + "timestamp": 2.6607330298785015 + }, + { + "x": 7.878491824221837, + "y": 4.093585686533033, + "heading": 3.1666157231902687, + "angularVelocity": 0.07980374508976079, + "velocityX": 0.8739928887988145, + "velocityY": 0.1398187395813148, + "timestamp": 2.6989342636876534 + }, + { + "x": 7.908226247157587, + "y": 4.10423241668243, + "heading": 3.1697235484523434, + "angularVelocity": 0.08135405462559406, + "velocityX": 0.7783628948818408, + "velocityY": 0.278701211656863, + "timestamp": 2.7371354974968054 + }, + { + "x": 7.934251131583176, + "y": 4.120145380644015, + "heading": 3.1728897318487155, + "angularVelocity": 0.08288170513523152, + "velocityX": 0.6812576932882871, + "velocityY": 0.4165562830007863, + "timestamp": 2.7753367313059574 + }, + { + "x": 7.956505849641918, + "y": 4.141281346281812, + "heading": 3.176113327242148, + "angularVelocity": 0.0843845884543132, + "velocityX": 0.582565426287611, + "velocityY": 0.5532796595887517, + "timestamp": 2.8135379651151093 + }, + { + "x": 7.974925029508088, + "y": 4.167592498355179, + "heading": 3.179393297087888, + "angularVelocity": 0.08586031179323926, + "velocityX": 0.4821619102197233, + "velocityY": 0.6887513687336226, + "timestamp": 2.8517391989242613 + }, + { + "x": 7.989438020301411, + "y": 4.199025720545251, + "heading": 3.1827284996727405, + "angularVelocity": 0.08730614831746564, + "velocityX": 0.37990895440256806, + "velocityY": 0.8228326432362902, + "timestamp": 2.8899404327334133 + }, + { + "x": 7.999968283780463, + "y": 4.235521729180706, + "heading": 3.1861176740734445, + "angularVelocity": 0.0887189774454874, + "velocityX": 0.27565244441213116, + "velocityY": 0.9553620392939309, + "timestamp": 2.9281416665425652 + }, + { + "x": 8.00643270325469, + "y": 4.27701402090998, + "heading": 3.1895594223515835, + "angularVelocity": 0.09009521250892577, + "velocityX": 0.16922017510021273, + "velocityY": 1.0861505661456612, + "timestamp": 2.966342900351717 + }, + { + "x": 8.008740800208267, + "y": 4.323427585324584, + "heading": 3.1930521883927083, + "angularVelocity": 0.09143071290771301, + "velocityX": 0.060419434751950635, + "velocityY": 1.2149755331589407, + "timestamp": 3.004544134160869 + }, + { + "x": 8.006793850403897, + "y": 4.3746773185892955, + "heading": 3.1965942326665227, + "angularVelocity": 0.09272067733493149, + "velocityX": -0.05096562624382283, + "velocityY": 1.341572723036849, + "timestamp": 3.042745367970021 + }, + { + "x": 8.000483894798665, + "y": 4.430666054241619, + "heading": 3.200183602033556, + "angularVelocity": 0.09395951410798249, + "velocityX": -0.16517674891749978, + "velocityY": 1.4656263703951196, + "timestamp": 3.080946601779173 + }, + { + "x": 7.989692648311953, + "y": 4.49128210092684, + "heading": 3.203818093558477, + "angularVelocity": 0.09514068427942153, + "velocityX": -0.2824842396615403, + "velocityY": 1.5867562547338276, + "timestamp": 3.119147835588325 + }, + { + "x": 7.974290324535485, + "y": 4.556396142126618, + "heading": 3.207495211129814, + "angularVelocity": 0.09625651332906135, + "velocityX": -0.4031891706277398, + "velocityY": 1.7045009992367957, + "timestamp": 3.157349069397477 + }, + { + "x": 7.954134422332773, + "y": 4.625857308190122, + "heading": 3.2112121135725333, + "angularVelocity": 0.09729796847109581, + "velocityX": -0.5276243773541068, + "velocityY": 1.8182964039989622, + "timestamp": 3.195550303206629 + }, + { + "x": 7.929068570159579, + "y": 4.699488175513398, + "heading": 3.2149655529585908, + "angularVelocity": 0.09825440206483436, + "velocityX": -0.6561529477927303, + "velocityY": 1.9274473618083952, + "timestamp": 3.233751537015781 + }, + { + "x": 7.898921611054975, + "y": 4.7770783843465825, + "heading": 3.2187518021293573, + "angularVelocity": 0.09911326921224117, + "velocityX": -0.7891619222356322, + "velocityY": 2.031091697739763, + "timestamp": 3.271952770824933 + }, + { + "x": 7.863507259665322, + "y": 4.858376507302923, + "heading": 3.2225665713147067, + "angularVelocity": 0.09985984233932846, + "velocityX": -0.9270473191148296, + "velocityY": 2.1281543774867226, + "timestamp": 3.310154004634085 + }, + { + "x": 7.82262490744023, + "y": 4.943079777954261, + "heading": 3.226404915584542, + "angularVelocity": 0.10047697121541134, + "velocityX": -1.0701840791146968, + "velocityY": 2.217291490492266, + "timestamp": 3.348355238443237 + }, + { + "x": 7.776062535619055, + "y": 5.030821379568998, + "heading": 3.2302611382194035, + "angularVelocity": 0.1009449761263412, + "velocityX": -1.2188708891915252, + "velocityY": 2.2968263813960155, + "timestamp": 3.386556472252389 + }, + { + "x": 7.723603242812097, + "y": 5.121155356689003, + "heading": 3.23412870026526, + "angularVelocity": 0.10124180975877002, + "velocityX": -1.373235562731729, + "velocityY": 2.364687422696869, + "timestamp": 3.424757706061541 + }, + { + "x": 7.665037532882504, + "y": 5.213540129232238, + "heading": 3.2380001528589366, + "angularVelocity": 0.10134365327094359, + "velocityX": -1.5330842512097755, + "velocityY": 2.4183714328384474, + "timestamp": 3.462958939870693 + }, + { + "x": 7.600183883971553, + "y": 5.307323440188604, + "heading": 3.2418671133682277, + "angularVelocity": 0.10122606323685251, + "velocityX": -1.6976846673317019, + "velocityY": 2.4549812036148855, + "timestamp": 3.501160173679845 + }, + { + "x": 7.528919309283333, + "y": 5.401734523083348, + "heading": 3.2457203037789037, + "angularVelocity": 0.10086560109356, + "velocityX": -1.8655045291010028, + "velocityY": 2.4714144932178943, + "timestamp": 3.5393614074889967 + }, + { + "x": 7.451218127874509, + "y": 5.495892378933352, + "heading": 3.249549659566478, + "angularVelocity": 0.10024167823230529, + "velocityX": -2.0339966451609546, + "velocityY": 2.4647857270894256, + "timestamp": 3.5775626412981487 + }, + { + "x": 7.36719026389099, + "y": 5.588839011710391, + "heading": 3.253344517033594, + "angularVelocity": 0.09933861000601822, + "velocityX": -2.1996112587177326, + "velocityY": 2.433079340876453, + "timestamp": 3.6157638751073007 + }, + { + "x": 7.277103414955576, + "y": 5.6795987285633585, + "heading": 3.257093918809728, + "angularVelocity": 0.0981487088837405, + "velocityX": -2.3582183074367444, + "velocityY": 2.375832081926742, + "timestamp": 3.6539651089164527 + }, + { + "x": 7.181374686983581, + "y": 5.767249906226262, + "heading": 3.260787096257733, + "angularVelocity": 0.09667691537021512, + "velocityX": -2.5059067057949522, + "velocityY": 2.2944593386903587, + "timestamp": 3.6921663427256046 + }, + { + "x": 7.080531109805766, + "y": 5.8509854681858595, + "heading": 3.2644140920664264, + "angularVelocity": 0.09494446767906256, + "velocityX": -2.6397989573220397, + "velocityY": 2.1919596204125034, + "timestamp": 3.7303675765347566 + }, + { + "x": 6.97515470473837, + "y": 5.930144227513004, + "heading": 3.267966327996027, + "angularVelocity": 0.09298746598989815, + "velocityX": -2.758455540829955, + "velocityY": 2.0721519028053215, + "timestamp": 3.7685688103439086 + }, + { + "x": 6.8658318519592285, + "y": 6.004211902618408, + "heading": 3.271436911327556, + "angularVelocity": 0.09085003245884067, + "velocityX": -2.8617623536795467, + "velocityY": 1.9388817511872807, + "timestamp": 3.8067700441530605 + }, + { + "x": 6.76937026572913, + "y": 6.063823537641736, + "heading": 3.274355520630392, + "angularVelocity": 0.08889560431653577, + "velocityX": -2.9380468954592764, + "velocityY": 1.8156634786800647, + "timestamp": 3.839601918409111 + }, + { + "x": 6.670811763453878, + "y": 6.119164152757477, + "heading": 3.2771960124627224, + "angularVelocity": 0.08651628628259647, + "velocityX": -3.00191519700062, + "velocityY": 1.6855758731331847, + "timestamp": 3.8724337926651615 + }, + { + "x": 6.570666307865554, + "y": 6.170019228878687, + "heading": 3.2799422951123653, + "angularVelocity": 0.08364684355894975, + "velocityX": -3.0502509484322142, + "velocityY": 1.5489544009762712, + "timestamp": 3.905265666921212 + }, + { + "x": 6.46955818993848, + "y": 6.216214742131844, + "heading": 3.2825768229193018, + "angularVelocity": 0.08024299150240671, + "velocityX": -3.079571916563354, + "velocityY": 1.4070324737748963, + "timestamp": 3.9380975411772625 + }, + { + "x": 6.368220806913084, + "y": 6.257657843766639, + "heading": 3.285082327525109, + "angularVelocity": 0.07631317622219781, + "velocityX": -3.086554920230331, + "velocityY": 1.2622825401799327, + "timestamp": 3.970929415433313 + }, + { + "x": 6.267462404278832, + "y": 6.2943783920802225, + "heading": 3.2874445528955567, + "angularVelocity": 0.07194914771010016, + "velocityX": -3.068920216020964, + "velocityY": 1.118442036759947, + "timestamp": 4.003761289689364 + }, + { + "x": 6.1681022663365965, + "y": 6.326551148740925, + "heading": 3.289654959048111, + "angularVelocity": 0.06732500664797306, + "velocityX": -3.026331581539983, + "velocityY": 0.9799244602909629, + "timestamp": 4.0365931639454145 + }, + { + "x": 6.070897503286478, + "y": 6.354482225299535, + "heading": 3.2917117995402965, + "angularVelocity": 0.06264767208064463, + "velocityX": -2.960682728376521, + "velocityY": 0.8507304925932898, + "timestamp": 4.069425038201465 + }, + { + "x": 5.976489922312775, + "y": 6.378564495957317, + "heading": 3.2936190060486643, + "angularVelocity": 0.058090089328853824, + "velocityX": -2.875485579575279, + "velocityY": 0.7335027683758681, + "timestamp": 4.1022569124575154 + }, + { + "x": 5.885387749283588, + "y": 6.399224497871891, + "heading": 3.295383909871142, + "angularVelocity": 0.05375580476197676, + "velocityX": -2.774808782425772, + "velocityY": 0.6292666009089228, + "timestamp": 4.135088786713566 + }, + { + "x": 5.79797544997618, + "y": 6.4168807330266695, + "heading": 3.2970151433758272, + "angularVelocity": 0.049684446643635256, + "velocityX": -2.6624218473088823, + "velocityY": 0.5377772531985251, + "timestamp": 4.167920660969616 + }, + { + "x": 5.7145364982554065, + "y": 6.431919635509067, + "heading": 3.298521304612775, + "angularVelocity": 0.04587496970782373, + "velocityX": -2.541400806729675, + "velocityY": 0.4580579946521674, + "timestamp": 4.200752535225667 + }, + { + "x": 5.635277634199153, + "y": 6.444685893070414, + "heading": 3.2999103137042596, + "angularVelocity": 0.04230672549035185, + "velocityX": -2.4140828342033305, + "velocityY": 0.3888373067520809, + "timestamp": 4.233584409481717 + }, + { + "x": 5.5603494135057785, + "y": 6.455481443626585, + "heading": 3.301189197207777, + "angularVelocity": 0.038952497610812056, + "velocityX": -2.2821792051535352, + "velocityY": 0.3288131061900009, + "timestamp": 4.266416283737768 + }, + { + "x": 5.489861905204234, + "y": 6.4645686834525575, + "heading": 3.302364085244027, + "angularVelocity": 0.03578498221232499, + "velocityX": -2.146923070910393, + "velocityY": 0.27678102550899025, + "timestamp": 4.299248157993818 + }, + { + "x": 5.423896104976939, + "y": 6.472175218090503, + "heading": 3.3034402954817073, + "angularVelocity": 0.03277943346417197, + "velocityX": -2.009199953461021, + "velocityY": 0.23168140139134377, + "timestamp": 4.332080032249869 + }, + { + "x": 5.3625120552036805, + "y": 6.478498802228473, + "heading": 3.304422441816652, + "angularVelocity": 0.029914415707270804, + "velocityX": -1.8696480528201007, + "velocityY": 0.19260503036328994, + "timestamp": 4.364911906505919 + }, + { + "x": 5.305754584196663, + "y": 6.483711879532544, + "heading": 3.305314540782059, + "angularVelocity": 0.027171734347235434, + "velocityX": -1.728730762196986, + "velocityY": 0.15878098409538036, + "timestamp": 4.39774378076197 + }, + { + "x": 5.253657370247068, + "y": 6.487965524914462, + "heading": 3.3061201058429943, + "angularVelocity": 0.024536066831048555, + "velocityX": -1.5867876912325205, + "velocityY": 0.12955840865933993, + "timestamp": 4.43057565501802 + }, + { + "x": 5.206245838192873, + "y": 6.491392772869967, + "heading": 3.3068422273399647, + "angularVelocity": 0.021994525543636716, + "velocityX": -1.444070225307262, + "velocityY": 0.1043878253424375, + "timestamp": 4.463407529274071 + }, + { + "x": 5.163539242392539, + "y": 6.494111389192325, + "heading": 3.3074836389039146, + "angularVelocity": 0.019536245751546197, + "velocityX": -1.3007663061594368, + "velocityY": 0.08280417685432861, + "timestamp": 4.496239403530121 + }, + { + "x": 5.125552179765353, + "y": 6.496226165889178, + "heading": 3.308046772172593, + "angularVelocity": 0.01715202928369231, + "velocityX": -1.1570177910323254, + "velocityY": 0.06441230495586951, + "timestamp": 4.529071277786172 + }, + { + "x": 5.092295700352608, + "y": 6.497830819141238, + "heading": 3.308533801781415, + "angularVelocity": 0.01483404831001049, + "velocityX": -1.0129327114676103, + "velocityY": 0.04887485982510178, + "timestamp": 4.561903152042222 + }, + { + "x": 5.063778130929228, + "y": 6.499009561458986, + "heading": 3.308946682418035, + "angularVelocity": 0.012575603616169764, + "velocityX": -0.8685940132743156, + "velocityY": 0.035902376713398354, + "timestamp": 4.594735026298273 + }, + { + "x": 5.040005691939611, + "y": 6.499838408157472, + "heading": 3.3092871794520544, + "angularVelocity": 0.010370928913894002, + "velocityX": -0.7240658515020671, + "velocityY": 0.025245183751066175, + "timestamp": 4.627566900554323 + }, + { + "x": 5.020982964030012, + "y": 6.50038626757828, + "heading": 3.3095568943744427, + "angularVelocity": 0.008215032753991694, + "velocityX": -0.5793981714611864, + "velocityY": 0.01668681527396481, + "timestamp": 4.660398774810374 + }, + { + "x": 5.006713244009331, + "y": 6.500715855130694, + "heading": 3.3097572860387627, + "angularVelocity": 0.00610357065688574, + "velocityX": -0.4346300765345807, + "velocityY": 0.010038645672288071, + "timestamp": 4.693230649066424 + }, + { + "x": 4.997198818717405, + "y": 6.500884463422624, + "heading": 3.30988968849574, + "angularVelocity": 0.004032741352049697, + "velocityX": -0.2897923285684355, + "velocityY": 0.005135506143067766, + "timestamp": 4.726062523322475 + }, + { + "x": 4.992441177368164, + "y": 6.5009446144104, + "heading": 3.309955326051445, + "angularVelocity": 0.001999202214064957, + "velocityX": -0.14490922181708896, + "velocityY": 0.0018320911961180106, + "timestamp": 4.758894397578525 + }, + { + "x": 4.992441177368164, + "y": 6.5009446144104, + "heading": 3.309955326051445, + "angularVelocity": 5.207192621874317e-25, + "velocityX": 4.1285632704076663e-23, + "velocityY": -5.50747176857669e-23, + "timestamp": 4.791726271834576 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/Robot/src/main/deploy/pathplanner/autos/Big.auto b/Robot/src/main/deploy/pathplanner/autos/Big.auto deleted file mode 100644 index a40adca..0000000 --- a/Robot/src/main/deploy/pathplanner/autos/Big.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.3224515231846692, - "y": 5.535659185876845 - }, - "rotation": 179.92075999093154 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Big A" - } - }, - { - "type": "path", - "data": { - "pathName": "Big B" - } - }, - { - "type": "path", - "data": { - "pathName": "Big C" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/Robot/src/main/deploy/pathplanner/paths/Amp Center A Grab.path b/Robot/src/main/deploy/pathplanner/paths/Amp Center A Grab.path index 4943d21..d5432b7 100644 --- a/Robot/src/main/deploy/pathplanner/paths/Amp Center A Grab.path +++ b/Robot/src/main/deploy/pathplanner/paths/Amp Center A Grab.path @@ -53,6 +53,23 @@ ] } } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 0.75, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim at Note" + } + } + ] + } + } } ], "globalConstraints": { diff --git a/Robot/src/main/deploy/pathplanner/paths/Amp Center Start.path b/Robot/src/main/deploy/pathplanner/paths/Amp Center Start.path deleted file mode 100644 index ff1c2b9..0000000 --- a/Robot/src/main/deploy/pathplanner/paths/Amp Center Start.path +++ /dev/null @@ -1,58 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.7859976973515199, - "y": 6.677362186633205 - }, - "prevControl": null, - "nextControl": { - "x": 2.341320238196032, - "y": 6.805997885500045 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.5752033270448305, - "y": 5.578111669043852 - }, - "prevControl": { - "x": 1.2408753057823887, - "y": 5.5715840531436776 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": 180.0, - "rotateFast": false - } - ], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.7, - "maxAcceleration": 3.8, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 640.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 180.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -120.2564371635293, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/Robot/src/main/deploy/pathplanner/paths/Big A.path b/Robot/src/main/deploy/pathplanner/paths/Big A.path deleted file mode 100644 index fae2c38..0000000 --- a/Robot/src/main/deploy/pathplanner/paths/Big A.path +++ /dev/null @@ -1,128 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 1.3224515231846692, - "y": 5.535659185876845 - }, - "prevControl": null, - "nextControl": { - "x": 2.322451523184671, - "y": 5.535659185876845 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.859140730157946, - "y": 5.557362183607978 - }, - "prevControl": { - "x": 2.4429454014041876, - "y": 5.545121144526985 - }, - "nextControl": { - "x": 4.516599627023483, - "y": 5.606110974692258 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.720672756460799, - "y": 6.343479399170583 - }, - "prevControl": { - "x": 4.238795494546496, - "y": 6.011095714255226 - }, - "nextControl": { - "x": 7.2320603735998255, - "y": 6.6824822291830746 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.853565561956061, - "y": 7.219236710036188 - }, - "prevControl": { - "x": 8.136067920299805, - "y": 6.7389827008518255 - }, - "nextControl": { - "x": 7.521335891927708, - "y": 7.7840271490843875 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.113292686021752, - "y": 6.442355224590893 - }, - "prevControl": { - "x": 6.122646951398346, - "y": 6.518086913307883 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "End of Big A" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.65, - "rotationDegrees": 180.0, - "rotateFast": false - }, - { - "waypointRelativePos": 1.2000000000000002, - "rotationDegrees": -177.3131039456892, - "rotateFast": false - }, - { - "waypointRelativePos": 3.0, - "rotationDegrees": -155.24203483014404, - "rotateFast": false - } - ], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 0.1, - "maxWaypointRelativePos": 1.15, - "constraints": { - "maxVelocity": 2.0, - "maxAcceleration": 2.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 640.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.7, - "maxAcceleration": 3.8, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 640.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -172.79516603914237, - "rotateFast": true - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 180.0, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/Robot/src/main/deploy/pathplanner/paths/Big B.path b/Robot/src/main/deploy/pathplanner/paths/Big B.path deleted file mode 100644 index 9419578..0000000 --- a/Robot/src/main/deploy/pathplanner/paths/Big B.path +++ /dev/null @@ -1,123 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 5.113292686021752, - "y": 6.442355224590893 - }, - "prevControl": null, - "nextControl": { - "x": 6.113292686021752, - "y": 6.442355224590893 - }, - "isLocked": false, - "linkedName": "End of Big A" - }, - { - "anchor": { - "x": 7.895940915707621, - "y": 5.933850979572156 - }, - "prevControl": { - "x": 7.754689736535749, - "y": 5.707849092897162 - }, - "nextControl": { - "x": 8.076855470005238, - "y": 6.223314266448346 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.424045280199869, - "y": 6.442355224590893 - }, - "prevControl": { - "x": 6.356303062734221, - "y": 6.738982700851825 - }, - "nextControl": { - "x": 4.491787497665517, - "y": 6.145727748329961 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.8645155650687049, - "y": 5.651348621228412 - }, - "prevControl": { - "x": 1.8645155650687049, - "y": 5.086343904540926 - }, - "nextControl": { - "x": 1.8645155650687049, - "y": 6.230650687757968 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.599021696762437, - "y": 6.767232936686198 - }, - "prevControl": { - "x": 2.2176435129983836, - "y": 6.399979870839332 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.0, - "rotationDegrees": 158.74949449286683, - "rotateFast": false - }, - { - "waypointRelativePos": 3.0, - "rotationDegrees": 180.0, - "rotateFast": false - } - ], - "constraintZones": [ - { - "name": "New Constraints Zone", - "minWaypointRelativePos": 2.7, - "maxWaypointRelativePos": 3.6, - "constraints": { - "maxVelocity": 2.0, - "maxAcceleration": 3.8, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 640.0 - } - } - ], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.7, - "maxAcceleration": 3.8, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 640.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": -135.9093804491991, - "rotateFast": true - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": -172.4540799967114, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/Robot/src/main/deploy/pathplanner/paths/Big C.path b/Robot/src/main/deploy/pathplanner/paths/Center Start.path similarity index 55% rename from Robot/src/main/deploy/pathplanner/paths/Big C.path rename to Robot/src/main/deploy/pathplanner/paths/Center Start.path index b5ffba6..8616f5b 100644 --- a/Robot/src/main/deploy/pathplanner/paths/Big C.path +++ b/Robot/src/main/deploy/pathplanner/paths/Center Start.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.305277592621795, - "y": 7.035610177112754 + "x": 1.359011265031077, + "y": 5.5430292057165325 }, "prevControl": null, "nextControl": { - "x": 4.305277592621794, - "y": 7.035610177112754 + "x": 1.6981417438618351, + "y": 5.5430292057165325 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 8.291444217388863, - "y": 7.162736238367439 + "x": 2.481650091505311, + "y": 5.5430292057165325 }, "prevControl": { - "x": 7.291444217388863, - "y": 7.162736238367439 + "x": 2.142519612674553, + "y": 5.531335051274091 }, "nextControl": null, "isLocked": false, @@ -29,7 +29,19 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "New Constraints Zone", + "minWaypointRelativePos": 0.5, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 5.7, + "maxAcceleration": 3.8, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 640.0 + } + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.7, @@ -45,7 +57,7 @@ "reversed": false, "folder": null, "previewStartingState": { - "rotation": -153.53092156316825, + "rotation": 179.30130561701654, "velocity": 0 }, "useDefaultConstraints": true diff --git a/Robot/src/main/deploy/pathplanner/paths/Source Center A Fast From Subwoofer.path b/Robot/src/main/deploy/pathplanner/paths/Source Center A Fast From Subwoofer.path index 68e6d03..51be526 100644 --- a/Robot/src/main/deploy/pathplanner/paths/Source Center A Fast From Subwoofer.path +++ b/Robot/src/main/deploy/pathplanner/paths/Source Center A Fast From Subwoofer.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.825878671700363, - "y": 2.2101951896211496 + "x": 7.779102053930604, + "y": 0.7367317298737166 }, "prevControl": { - "x": 6.2939444397407325, - "y": 0.19880062552148048 + "x": 4.715233590011339, + "y": 0.7133434209888363 }, "nextControl": null, "isLocked": false, @@ -58,6 +58,23 @@ ] } } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 0.85, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim at Note" + } + } + ] + } + } } ], "globalConstraints": { @@ -68,7 +85,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -153.43494882292197, + "rotation": -177.6140559696112, "rotateFast": false }, "reversed": false, diff --git a/Robot/src/main/deploy/pathplanner/paths/Source Center A Grab.path b/Robot/src/main/deploy/pathplanner/paths/Source Center A Grab.path index 4560cae..d3a2c5a 100644 --- a/Robot/src/main/deploy/pathplanner/paths/Source Center A Grab.path +++ b/Robot/src/main/deploy/pathplanner/paths/Source Center A Grab.path @@ -47,6 +47,23 @@ ] } } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 0.9, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim at Note" + } + } + ] + } + } } ], "globalConstraints": { diff --git a/Robot/src/main/deploy/pathplanner/paths/Source Center B Grab.path b/Robot/src/main/deploy/pathplanner/paths/Source Center B Grab.path index b384859..14fea01 100644 --- a/Robot/src/main/deploy/pathplanner/paths/Source Center B Grab.path +++ b/Robot/src/main/deploy/pathplanner/paths/Source Center B Grab.path @@ -16,28 +16,28 @@ }, { "anchor": { - "x": 5.685848408733854, - "y": 1.777511475250871 + "x": 5.498741937654815, + "y": 1.7892056296933105 }, "prevControl": { - "x": 4.973000273866197, - "y": 1.7237116160155765 + "x": 4.784166617830332, + "y": 1.8099179578041633 }, "nextControl": { - "x": 6.925428779632489, - "y": 1.8710647107903897 + "x": 6.30563859418317, + "y": 1.7658173208084327 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 7.767407899488164, - "y": 2.408995815142628 + "x": 7.896043598355003, + "y": 2.2569718073909093 }, "prevControl": { - "x": 6.399191829722692, - "y": 2.3973016607001885 + "x": 6.983899551844686, + "y": 1.9178413285601503 }, "nextControl": null, "isLocked": false, @@ -63,6 +63,23 @@ ] } } + }, + { + "name": "New Event Marker", + "waypointRelativePos": 1.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim at Note" + } + } + ] + } + } } ], "globalConstraints": { @@ -73,7 +90,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -176.9335144988741, + "rotation": -156.61477894278636, "rotateFast": false }, "reversed": false, diff --git a/Robot/src/main/java/com/swrobotics/robot/AutoFactory.java b/Robot/src/main/java/com/swrobotics/robot/AutoFactory.java new file mode 100644 index 0000000..e96169c --- /dev/null +++ b/Robot/src/main/java/com/swrobotics/robot/AutoFactory.java @@ -0,0 +1,72 @@ +package com.swrobotics.robot; + +import java.util.ArrayList; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; +import com.swrobotics.robot.commands.RobotCommands; +import com.swrobotics.robot.utils.DynamicAutoCommand; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +public class AutoFactory { + private final RobotContainer robot; + private final ArrayList sequence; + + public AutoFactory(RobotContainer robot) { + this.robot = robot; + sequence = new ArrayList(); + } + + public AutoFactory goToNote(Notes note) { + sequence.add(note.getPath().grabNote); + sequence.add(note.getPath().scoreNote); + return this; + } + + public AutoFactory shoot(boolean waitForNote) { + sequence.add(RobotCommands.aimAndShoot(robot, waitForNote)); + return this; + } + + public AutoFactory fling() { + sequence.add(RobotCommands.flingNote(robot)); + return this; + } + + public Command build() { + // return new DynamicAutoCommand(sequence); + Command[] commands = new Command[sequence.size()]; + return new SequentialCommandGroup(sequence.toArray(commands)); + } + + + protected record NotePath(Command grabNote, Command scoreNote) {} + + public enum Notes { + SOURCE_WALL(new NotePath(ppPath("Source Center A Grab"), ppPath("Source Center A Score")));//, + // SOURCE_2, + // CENTER, + // AMP_2, + // AMP_WALL; + + private final NotePath path; + private Notes(NotePath path) { + this.path = path; + } + + public NotePath getPath() { + return path; + } + } + + protected static Command ppPath(String name) { + return AutoBuilder.followPath(PathPlannerPath.fromPathFile(name)); + } + + protected static Command choreoPath(String name) { + return AutoBuilder.followPath(PathPlannerPath.fromChoreoTrajectory(name)); + } +} diff --git a/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java b/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java index 4d75aaf..9bebc72 100644 --- a/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java +++ b/Robot/src/main/java/com/swrobotics/robot/RobotContainer.java @@ -7,6 +7,7 @@ import com.swrobotics.lib.field.FieldInfo; import com.swrobotics.messenger.client.MessengerClient; import com.swrobotics.robot.commands.RobotCommands; +import com.swrobotics.robot.AutoFactory.Notes; import com.swrobotics.robot.commands.IntakeSetCommand; import com.swrobotics.robot.commands.PlaySongCommand; import com.swrobotics.robot.config.IOAllocation; @@ -116,6 +117,7 @@ public RobotContainer() { NamedCommands.registerCommand("Shoot Quick", RobotCommands.shootQuick(this)); NamedCommands.registerCommand("Fling", RobotCommands.flingNote(this)); NamedCommands.registerCommand("Wait and Shoot", RobotCommands.aimAndShoot(this, true)); // Waits for indexer to have note + NamedCommands.registerCommand("Aim at Note", RobotCommands.aimAtNote(this)); NamedCommands.registerCommand("Eject Hard", RobotCommands.ejectHard(this)); // Create a chooser to select the autonomous @@ -127,6 +129,13 @@ public RobotContainer() { DriveRequestType.OpenLoopVoltage)), drive ).withTimeout(5)))); + + autos.add(new AutoEntry("Test", + new AutoFactory(this) + .goToNote(Notes.SOURCE_WALL) + .shoot(false) + .build())); + autos.sort(Comparator.comparing(AutoEntry::name, String.CASE_INSENSITIVE_ORDER)); SendableChooser autoChooser = new SendableChooser<>(); diff --git a/Robot/src/main/java/com/swrobotics/robot/commands/RobotCommands.java b/Robot/src/main/java/com/swrobotics/robot/commands/RobotCommands.java index 49cecfa..ba5a68d 100644 --- a/Robot/src/main/java/com/swrobotics/robot/commands/RobotCommands.java +++ b/Robot/src/main/java/com/swrobotics/robot/commands/RobotCommands.java @@ -1,13 +1,17 @@ package com.swrobotics.robot.commands; +import com.pathplanner.lib.auto.AutoBuilder; import com.swrobotics.robot.RobotContainer; import com.swrobotics.robot.config.NTData; +import com.swrobotics.robot.subsystems.NoteTracker; import com.swrobotics.robot.subsystems.amp.AmpArm2Subsystem; import com.swrobotics.robot.subsystems.speaker.IntakeSubsystem.State; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.PrintCommand; public final class RobotCommands { public static Command justShoot(RobotContainer robot) { @@ -70,4 +74,11 @@ public static Command flingNote(RobotContainer robot) { Commands.runOnce(() -> robot.ampArm2.setPosition(AmpArm2Subsystem.Position.RETRACT)) ); } + + public static Command aimAtNote(RobotContainer robot) { + return Commands.run( + () -> robot.drive.setAutoOverride( + () -> NoteTracker.getAngleToNote())) + .until(() -> robot.indexer.hasPiece()); + } } diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/NoteTracker.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/NoteTracker.java new file mode 100644 index 0000000..2bdbf0f --- /dev/null +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/NoteTracker.java @@ -0,0 +1,36 @@ +package com.swrobotics.robot.subsystems; + +import java.util.Optional; + +import com.swrobotics.robot.utils.LimelightHelpers; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotBase; + +public class NoteTracker { + + private static final double MOUNTING_HEIGHT_METERS = 1; // FIXME: Measure + private static final double PITCH = -10; + private static final double YAW = -20; + + public static Optional getDistanceToNote() { + if (LimelightHelpers.getLatestResults("").targetingResults.valid) { + return Optional.of( + (Math.tan(Math.toRadians(LimelightHelpers.getTY("")) + PITCH)) * MOUNTING_HEIGHT_METERS); + } + return Optional.empty(); + } + + public static Optional getAngleToNote() { + // if (RobotBase.isSimulation()) { + // return Optional.of(Rotation2d.fromDegrees(90)); + // } + + if (LimelightHelpers.getLatestResults("").targetingResults.valid) { + return Optional.ofNullable(Rotation2d.fromDegrees(LimelightHelpers.getTX("") + YAW)); + } + + return Optional.empty(); + } +} diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java index 39bb17e..c06d156 100644 --- a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java +++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveDrive.java @@ -18,6 +18,7 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.SteerRequestType; import com.kauailabs.navx.frc.AHRS; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; @@ -43,6 +44,10 @@ import static com.swrobotics.robot.subsystems.swerve.SwerveConstants.SWERVE_MODULE_BUILDER; +import java.util.Optional; +import java.util.function.Consumer; +import java.util.function.Supplier; + @SuppressWarnings("unused") public final class SwerveDrive extends SubsystemBase { public static final int DRIVER_PRIORITY = 0; @@ -89,6 +94,7 @@ public static record TurnRequest(int priority, Rotation2d turn) { private DriveRequest currentDriveRequest; private TurnRequest currentTurnRequest; private int lastSelectedPriority; + private Optional autoOverride = Optional.empty(); public SwerveDrive(FieldInfo fieldInfo, MessengerClient msg) { this.fieldInfo = fieldInfo; @@ -135,6 +141,8 @@ public SwerveDrive(FieldInfo fieldInfo, MessengerClient msg) { () -> DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Red, this); + PPHolonomicDriveController.setRotationTargetOverride(this::getAutoOverride); + // Pathfinding.setPathfinder(new LocalADStar()); // Pathfinding.setPathfinder(new ThetaStarPathfinder(msg)); @@ -336,4 +344,17 @@ public void setEstimatorIgnoreVision(boolean ignoreVision) { public boolean hasSeenWhereWeAre() { return estimator.hasSeenWhereWeAre(); } + + public Optional getAutoOverride() { + return autoOverride; + } + + /** Sets the angle the drivetrain should go to */ + public void setAutoOverride(Supplier> angle) { + if (angle.get().isPresent()) { + autoOverride = Optional.of(getEstimatedPose().getRotation().minus(angle.get().get())); + } else { + autoOverride = Optional.empty(); + } + } } diff --git a/Robot/src/main/java/com/swrobotics/robot/utils/DynamicAutoCommand.java b/Robot/src/main/java/com/swrobotics/robot/utils/DynamicAutoCommand.java new file mode 100644 index 0000000..8db4fd9 --- /dev/null +++ b/Robot/src/main/java/com/swrobotics/robot/utils/DynamicAutoCommand.java @@ -0,0 +1,49 @@ +package com.swrobotics.robot.utils; + +import java.util.ArrayList; +import java.util.List; + +import edu.wpi.first.wpilibj2.command.Command; + +public class DynamicAutoCommand extends Command { + private final List commands; + private int i = 0; + private Command currentCommand; + + public DynamicAutoCommand(List commands) { + this.commands = commands; + currentCommand = commands.get(0); + } + + @Override + public void initialize() { + if (currentCommand != null) { + currentCommand.cancel(); + } + i = 0; + currentCommand = commands.get(0); + currentCommand.schedule(); + System.out.println(commands); + } + + @Override + public void execute() { + if (!currentCommand.isFinished() && currentCommand.isScheduled()) { + return; + } else { + i++; + try { + currentCommand = commands.get(i); + currentCommand.schedule(); + } catch (IndexOutOfBoundsException e) { + end(false); + } + + } + } + + @Override + public boolean isFinished() { + return i == commands.size() && currentCommand.isFinished(); + } +} diff --git a/Robot/src/main/java/com/swrobotics/robot/utils/LimelightHelpers.java b/Robot/src/main/java/com/swrobotics/robot/utils/LimelightHelpers.java new file mode 100644 index 0000000..a846ecb --- /dev/null +++ b/Robot/src/main/java/com/swrobotics/robot/utils/LimelightHelpers.java @@ -0,0 +1,992 @@ +package com.swrobotics.robot.utils; + +//LimelightHelpers v1.5.0 (March 27, 2024) + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; + +public class LimelightHelpers { + + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Barcode { + + } + + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Detector() { + } + } + + public static class Results { + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public Results() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + } + + public static class LimelightResults { + @JsonProperty("Results") + public Results targetingResults; + + public String error; + + public LimelightResults() { + targetingResults = new Results(); + error = ""; + } + + + } + + public static class RawFiducial { + public int id; + public double txnc; + public double tync; + public double ta; + public double distToCamera; + public double distToRobot; + public double ambiguity; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + } + } + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + private static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + private static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + private static double extractBotPoseEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); + var poseArray = poseEntry.getDoubleArray(new double[0]); + var pose = toPose2D(poseArray); + double latency = extractBotPoseEntry(poseArray,6); + int tagCount = (int)extractBotPoseEntry(poseArray,7); + double tagSpan = extractBotPoseEntry(poseArray,8); + double tagDist = extractBotPoseEntry(poseArray,9); + double tagArea = extractBotPoseEntry(poseArray,10); + //getlastchange() in microseconds, ll latency in milliseconds + var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0); + + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial*tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } + else{ + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials); + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * The LEDs will be controlled by Limelight pipeline settings, and not by robot + * code. + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + public static void setCameraMode_Processor(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 0); + } + public static void setCameraMode_Driver(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 1); + } + + + /** + * Sets the crop window. The crop window in the UI must be completely open for + * dynamic cropping to work. + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Parses Limelight's JSON results dump into a LimelightResults Object + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.targetingResults.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} \ No newline at end of file