diff --git a/choreo.chor b/choreo.chor index 6ad70c0..c4e8fdc 100644 --- a/choreo.chor +++ b/choreo.chor @@ -1,5 +1,5 @@ { - "version": "v0.3", + "version": "v0.3.1", "robotConfiguration": { "mass": 50, "rotationalInertia": 6, @@ -216,41 +216,7 @@ "timestamp": 1.808946786161252 } ], - "trajectoryWaypoints": [ - { - "timestamp": 0, - "isStopPoint": true, - "x": 1.3126474618911743, - "y": 5.567087650299072, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 - }, - { - "timestamp": 0.9044733930805594, - "isStopPoint": true, - "x": 2.3261709213256836, - "y": 5.586577892303467, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 - }, - { - "timestamp": 1.808946786161252, - "isStopPoint": true, - "x": 1.3126474618911743, - "y": 5.567087650299072, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], + "trajectoryWaypoints": [], "constraints": [ { "scope": [ @@ -328,17 +294,17 @@ "controlIntervalCount": 18 }, { - "x": 0.6235248446464539, - "y": 6.866034507751465, - "heading": 1.3037, + "x": 0.6103193163871765, + "y": 6.79156494140625, + "heading": 1.1689257238838953, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 21 + "controlIntervalCount": 30 }, { - "x": 3.296103000640869, - "y": 7.606926441192627, + "x": 6.834057807922363, + "y": 7.271172523498535, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -351,568 +317,649 @@ "x": 0.6235248446464539, "y": 6.866034507751465, "heading": 1.3037, - "angularVelocity": 2.3321158035498043e-24, - "velocityX": -1.6085262930190747e-26, - "velocityY": -4.0384459981982936e-25, + "angularVelocity": 2.7068477576187765e-23, + "velocityX": -4.853948155553872e-23, + "velocityY": -6.243185313363585e-23, "timestamp": 0 }, { - "x": 0.6371648015347651, - "y": 6.867786445253041, - "heading": 1.3036999632733615, - "angularVelocity": -7.263599506268419e-7, - "velocityX": 0.26976382275301153, - "velocityY": 0.03464888940043332, - "timestamp": 0.050562587485274306 - }, - { - "x": 0.6644447150656874, - "y": 6.871290320309862, - "heading": 1.303699886456694, - "angularVelocity": -0.0000015192392509530055, - "velocityX": 0.5395276406466987, - "velocityY": 0.0692977798622562, - "timestamp": 0.10112517497054861 - }, - { - "x": 0.7053645848706707, - "y": 6.876546133002426, - "heading": 1.3036997645049127, - "angularVelocity": -0.0000024118975620600877, - "velocityX": 0.809291451251395, - "velocityY": 0.10394667191616248, - "timestamp": 0.1516877624558229 - }, - { - "x": 0.759924410335464, - "y": 6.8835538834649, - "heading": 1.303699589009544, - "angularVelocity": -0.0000034708541873039917, - "velocityX": 1.0790552497077626, - "velocityY": 0.13859556662353859, - "timestamp": 0.20225034994109722 - }, - { - "x": 0.8281241902315634, - "y": 6.892313571965616, - "heading": 1.3036993431536497, - "angularVelocity": -0.000004862407296943842, - "velocityX": 1.3488190238674322, - "velocityY": 0.17324446663784457, - "timestamp": 0.2528129374263715 - }, - { - "x": 0.909963920873441, - "y": 6.902825199309566, - "heading": 1.3036989764864741, - "angularVelocity": -0.000007251748650640463, - "velocityX": 1.61858272513669, - "velocityY": 0.20789338257286122, - "timestamp": 0.3033755249116458 - }, - { - "x": 0.9952199524201776, - "y": 6.913775599360361, - "heading": 1.2650877104567244, - "angularVelocity": -0.763633111952482, - "velocityX": 1.6861485099346676, - "velocityY": 0.21657119612368925, - "timestamp": 0.35393811239692013 - }, - { - "x": 1.08047598194801, - "y": 6.924726015129663, - "heading": 1.1860996824058685, - "angularVelocity": -1.5621832659149426, - "velocityX": 1.6861484700058589, - "velocityY": 0.21657150699596817, - "timestamp": 0.40450069988219445 - }, - { - "x": 1.165732011476475, - "y": 6.93567643089404, - "heading": 1.068522180672941, - "angularVelocity": -2.325385380389609, - "velocityX": 1.6861484700183644, - "velocityY": 0.21657150689860336, - "timestamp": 0.45506328736746876 - }, - { - "x": 1.25098804100406, - "y": 6.94662684666527, - "heading": 0.9153751201313117, - "angularVelocity": -3.0288612224647617, - "velocityX": 1.6861484700009595, - "velocityY": 0.2165715070341085, - "timestamp": 0.505625874852743 - }, - { - "x": 1.3362440705163487, - "y": 6.957577262555589, - "heading": 0.7326430566977549, - "angularVelocity": -3.6139776961923715, - "velocityX": 1.6861484696984423, - "velocityY": 0.21657150938939215, - "timestamp": 0.5561884623380173 - }, - { - "x": 1.4215001000011822, - "y": 6.968527678659667, - "heading": 0.5352466423996767, - "angularVelocity": -3.904001438920968, - "velocityX": 1.6861484691554405, - "velocityY": 0.21657151361700638, - "timestamp": 0.6067510498232916 - }, - { - "x": 1.5067561295108884, - "y": 6.9794780945700925, - "heading": 0.35194096302396116, - "angularVelocity": -3.6253223676320148, - "velocityX": 1.6861484696473656, - "velocityY": 0.2165715097870587, - "timestamp": 0.657313637308566 - }, - { - "x": 1.5920121590364624, - "y": 6.99042851035698, - "heading": 0.19809518233895146, - "angularVelocity": -3.0426801383496302, - "velocityX": 1.6861484699611848, - "velocityY": 0.21657150734378086, - "timestamp": 0.7078762247938403 - }, - { - "x": 1.6772681885652982, - "y": 7.001378926118472, - "heading": 0.07976876190025861, - "angularVelocity": -2.3401970967793897, - "velocityX": 1.6861484700257006, - "velocityY": 0.21657150684148851, - "timestamp": 0.7584388122791146 + "x": 0.6479605553240734, + "y": 6.869173065177977, + "heading": 1.3036998817532777, + "angularVelocity": -0.0000017472450406683682, + "velocityX": 0.3610685643473286, + "velocityY": 0.04637615983684271, + "timestamp": 0.06767609559638565 + }, + { + "x": 0.6968319743321776, + "y": 6.875450180817172, + "heading": 1.3036996103660266, + "angularVelocity": -0.000004010090249051131, + "velocityX": 0.722137094012766, + "velocityY": 0.09275233129037688, + "timestamp": 0.1353521911927713 + }, + { + "x": 0.7701390946293506, + "y": 6.884865857027548, + "heading": 1.303699081157409, + "angularVelocity": -0.00000781972738083724, + "velocityX": 1.083205519632373, + "velocityY": 0.13912853759369243, + "timestamp": 0.20302828678915696 + }, + { + "x": 0.8641136418785188, + "y": 6.89693604798726, + "heading": 1.2781268289721, + "angularVelocity": -0.3778624041466495, + "velocityX": 1.388592920750396, + "velocityY": 0.17835235400838984, + "timestamp": 0.2707043823855426 + }, + { + "x": 0.958088185941953, + "y": 6.909006263750245, + "heading": 1.1796751937375807, + "angularVelocity": -1.4547475643641785, + "velocityX": 1.3885928736771431, + "velocityY": 0.17835272050816342, + "timestamp": 0.3383804779819283 + }, + { + "x": 1.0520627300139076, + "y": 6.921076479446895, + "heading": 1.0124911373568812, + "angularVelocity": -2.4703561118207933, + "velocityX": 1.3885928738030422, + "velocityY": 0.178352719527955, + "timestamp": 0.40605657357831393 + }, + { + "x": 1.1460372740816214, + "y": 6.933146695176562, + "heading": 0.785208686841268, + "angularVelocity": -3.358385978279628, + "velocityX": 1.3885928737403777, + "velocityY": 0.17835272001583247, + "timestamp": 0.4737326691746996 + }, + { + "x": 1.2400118181361255, + "y": 6.945216911009073, + "heading": 0.5237947450805, + "angularVelocity": -3.862721976749634, + "velocityX": 1.3885928735451927, + "velocityY": 0.1783527215354694, + "timestamp": 0.5414087647710852 + }, + { + "x": 1.3339863622028791, + "y": 6.9572871267462135, + "heading": 0.2954599282674446, + "angularVelocity": -3.3739360227697586, + "velocityX": 1.388592873726193, + "velocityY": 0.17835272012626976, + "timestamp": 0.6090848603674709 + }, + { + "x": 1.4279609062740386, + "y": 6.969357342449054, + "heading": 0.126956229009992, + "angularVelocity": -2.489855506180409, + "velocityX": 1.3885928737912927, + "velocityY": 0.17835271961943208, + "timestamp": 0.6767609559638565 + }, + { + "x": 1.521935450341706, + "y": 6.981427558179083, + "heading": 0.027070659114329158, + "angularVelocity": -1.4759357645478204, + "velocityX": 1.3885928737396922, + "velocityY": 0.1783527200211782, + "timestamp": 0.7444370515602422 + }, + { + "x": 1.6159099957667231, + "y": 6.993497763341015, + "heading": 9.235413386651335e-7, + "angularVelocity": -0.3999896172266207, + "velocityX": 1.388592893796256, + "velocityY": 0.1783525638641785, + "timestamp": 0.8121131471566279 + }, + { + "x": 1.6892171169315986, + "y": 7.002913432795811, + "heading": 3.921850758219532e-7, + "angularVelocity": -0.000007851461556117877, + "velocityX": 1.0832055324537762, + "velocityY": 0.13912843777145287, + "timestamp": 0.8797892427530135 + }, + { + "x": 1.7380885363341119, + "y": 7.00919054536428, + "heading": 1.1913070878580363e-7, + "angularVelocity": -0.000004034723998627552, + "velocityX": 0.7221370998406585, + "velocityY": 0.09275228591651416, + "timestamp": 0.9474653383493992 }, { "x": 1.7625242471694946, "y": 7.0123291015625, - "heading": 8.02748616433338e-24, - "angularVelocity": -1.577624205317622, - "velocityX": 1.686149045062736, - "velocityY": 0.21656675397035893, - "timestamp": 0.8090013997643889 - }, - { - "x": 1.894717013309512, - "y": 7.013383749891871, - "heading": -0.04465409215915743, - "angularVelocity": -0.574233690167933, - "velocityX": 1.6999458782753816, - "velocityY": 0.013562353923706081, - "timestamp": 0.8867643261195083 - }, - { - "x": 2.026909781256637, - "y": 7.014438171688664, - "heading": -6.873205041659995e-7, - "angularVelocity": 0.5742248515023068, - "velocityX": 1.6999459015140637, - "velocityY": 0.013559440805737809, - "timestamp": 0.9645272524746277 - }, - { - "x": 2.1570162459060764, - "y": 7.015475952303694, - "heading": -4.0701288137533413e-7, - "angularVelocity": 0.0000036046434455229545, - "velocityX": 1.6731168790547177, - "velocityY": 0.0133454418920676, - "timestamp": 1.0422901788297472 - }, - { - "x": 2.25459609761466, - "y": 7.016254287790654, - "heading": -2.021152112745776e-7, - "angularVelocity": 0.000002634901741802409, - "velocityX": 1.2548377007182918, - "velocityY": 0.010009081749382266, - "timestamp": 1.1200531051848666 - }, - { - "x": 2.3196493330208185, - "y": 7.016773178122739, - "heading": -6.706257500555856e-8, - "angularVelocity": 0.0000017367226594878274, - "velocityX": 0.8365584791534231, - "velocityY": 0.006672721262003089, - "timestamp": 1.197816031539986 + "heading": -6.978679086372593e-23, + "angularVelocity": -0.000001760307058732952, + "velocityX": 0.3610685666784826, + "velocityY": 0.04637614168728246, + "timestamp": 1.015141433945785 }, { - "x": 2.3521759510040283, - "y": 7.017032623291016, - "heading": -5.459567245742045e-24, - "angularVelocity": 8.62397779364737e-7, - "velocityX": 0.4182792431790796, - "velocityY": 0.0033363606597260143, - "timestamp": 1.2755789578951053 + "x": 1.7625242471694946, + "y": 7.0123291015625, + "heading": -3.402897415947517e-23, + "angularVelocity": 2.5545845274476152e-23, + "velocityX": -1.6289861869168524e-23, + "velocityY": -2.7174652182389944e-23, + "timestamp": 1.0828175295421705 + }, + { + "x": 1.81479684926806, + "y": 7.0127460685926595, + "heading": -4.386560783337548e-21, + "angularVelocity": -4.415213777084415e-20, + "velocityX": 0.5302539374684592, + "velocityY": 0.0042297188328221345, + "timestamp": 1.181397848149752 + }, + { + "x": 1.9193420469047702, + "y": 7.013580002600649, + "heading": -1.4475848885926105e-20, + "angularVelocity": -1.0234586623955383e-19, + "velocityX": 1.06050780838793, + "velocityY": 0.00845943713479765, + "timestamp": 1.2799781667573336 + }, + { + "x": 2.0573500990867615, + "y": 7.014680862426758, + "heading": 7.919072179411037e-17, + "angularVelocity": 8.034585276427108e-16, + "velocityX": 1.3999554285410607, + "velocityY": 0.011167136013136953, + "timestamp": 1.378558485364915 + }, + { + "x": 2.1953581512687528, + "y": 7.015781722252867, + "heading": 1.482512560830555e-20, + "angularVelocity": -8.03161298186475e-16, + "velocityX": 1.3999554285410607, + "velocityY": 0.011167136013136954, + "timestamp": 1.4771388039724966 + }, + { + "x": 2.2999033489054628, + "y": 7.016615656260856, + "heading": 4.4513458995432525e-21, + "angularVelocity": -1.0523175269429369e-19, + "velocityX": 1.06050780838793, + "velocityY": 0.00845943713479765, + "timestamp": 1.5757191225800782 }, { "x": 2.3521759510040283, "y": 7.017032623291016, - "heading": -3.0375060318554146e-24, - "angularVelocity": 7.015482005050734e-25, - "velocityX": -5.736202459306136e-26, - "velocityY": 3.1210527485389075e-25, - "timestamp": 1.3533418842502247 - }, - { - "x": 2.32247783894149, - "y": 7.014438485612277, - "heading": 1.4569198322416039e-8, - "angularVelocity": 1.9570400792959067e-7, - "velocityX": -0.3989265181213512, - "velocityY": -0.03484633331329086, - "timestamp": 1.42778695298718 - }, - { - "x": 2.2630816153943543, - "y": 7.009250210303195, - "heading": 4.6009116708770193e-8, - "angularVelocity": 4.2232372029159017e-7, - "velocityX": -0.7978530284793834, - "velocityY": -0.06969266597649279, - "timestamp": 1.5022320217241354 - }, - { - "x": 2.1739872815185026, - "y": 7.001467797460562, - "heading": 9.892279855196894e-8, - "angularVelocity": 7.107748403076132e-7, - "velocityX": -1.1967795233107767, - "velocityY": -0.10453899733951685, - "timestamp": 1.5766770904610907 - }, - { - "x": 2.0551948407815805, - "y": 6.991091247374754, - "heading": 1.87119372985259e-7, - "angularVelocity": 0.0000011847201692421604, - "velocityX": -1.5957059715622512, - "velocityY": -0.13938532480200694, - "timestamp": 1.651122159198046 - }, - { - "x": 1.929118297895444, - "y": 6.980078429264653, - "heading": 0.06161507996236991, - "angularVelocity": 0.8276557989449552, - "velocityX": -1.693551299302524, - "velocityY": -0.14793213703669883, - "timestamp": 1.7255672279350014 - }, - { - "x": 1.8030417550185982, - "y": 6.969065611048185, - "heading": 0.1818100793075498, - "angularVelocity": 1.614546153082047, - "velocityX": -1.6935512991777255, - "velocityY": -0.1479321384654859, - "timestamp": 1.8000122966719567 - }, - { - "x": 1.6769652121424672, - "y": 6.958052792823533, - "heading": 0.34903336005588126, - "angularVelocity": 2.2462640385113897, - "velocityX": -1.6935512991681227, - "velocityY": -0.14793213857541956, - "timestamp": 1.874457365408912 - }, - { - "x": 1.5508886692676784, - "y": 6.947039974583516, - "heading": 0.5474314025531648, - "angularVelocity": 2.6650259830950556, - "velocityX": -1.6935512991500936, - "velocityY": -0.14793213878181766, - "timestamp": 1.9489024341458674 - }, - { - "x": 1.4248121263920972, - "y": 6.936027156352571, - "heading": 0.7568159477198164, - "angularVelocity": 2.8126046321011815, - "velocityX": -1.6935512991607387, - "velocityY": -0.14793213865995034, - "timestamp": 2.0233475028828227 - }, - { - "x": 1.2987355835144632, - "y": 6.925014338145126, - "heading": 0.9551163962131972, - "angularVelocity": 2.6637150298572045, - "velocityX": -1.6935512991883124, - "velocityY": -0.14793213834428362, - "timestamp": 2.0977925716197783 - }, - { - "x": 1.172659040636784, - "y": 6.914001519938199, - "heading": 1.1222044891261886, - "angularVelocity": 2.2444480977428, - "velocityX": -1.6935512991889197, - "velocityY": -0.14793213833733215, - "timestamp": 2.172237640356734 - }, - { - "x": 1.0465824977596736, - "y": 6.902988701724757, - "heading": 1.2422496454510472, - "angularVelocity": 1.61253335327054, - "velocityX": -1.6935512991812764, - "velocityY": -0.14793213842483294, - "timestamp": 2.2466827090936894 - }, - { - "x": 0.9205059547934591, - "y": 6.891975884531403, - "heading": 1.3036998128368849, - "angularVelocity": 0.8254430874792525, - "velocityX": -1.6935513003781917, - "velocityY": -0.1479321247222771, - "timestamp": 2.321127777830645 - }, - { - "x": 0.80171351409583, - "y": 6.881599333995763, - "heading": 1.303699901063561, - "angularVelocity": 0.0000011851245168080392, - "velocityX": -1.5957059710344408, - "velocityY": -0.13938533084447882, - "timestamp": 2.3955728465676005 - }, - { - "x": 0.7126191802404107, - "y": 6.873816920919218, - "heading": 1.3036999539873229, - "angularVelocity": 7.109102435762517e-7, - "velocityX": -1.1967795230363152, - "velocityY": -0.10453900048160246, - "timestamp": 2.470017915304556 - }, - { - "x": 0.653222956704277, - "y": 6.868628645484184, - "heading": 1.3036999854303395, - "angularVelocity": 4.223653346202014e-7, - "velocityX": -0.7978530283315964, - "velocityY": -0.06969266766838508, - "timestamp": 2.5444629840415116 + "heading": 3.1297526369907225e-26, + "angularVelocity": -4.515419167199066e-20, + "velocityX": 0.5302539374684592, + "velocityY": 0.004229718832822134, + "timestamp": 1.6742994411876597 }, { - "x": 0.6235248446464539, - "y": 6.866034507751465, - "heading": 1.3037, - "angularVelocity": 1.9571021880679713e-7, - "velocityX": -0.39892651805801393, - "velocityY": -0.034846334038387564, - "timestamp": 2.618908052778467 - }, - { - "x": 0.6235248446464539, - "y": 6.866034507751465, - "heading": 1.3037, - "angularVelocity": 2.310134713088712e-26, - "velocityX": 9.797912030064837e-26, - "velocityY": 2.886038048031371e-26, - "timestamp": 2.6933531215154227 - }, - { - "x": 0.6684031135092222, - "y": 6.878475658375004, - "heading": 1.303699996148483, - "angularVelocity": -4.139316696396632e-8, - "velocityX": 0.48231739179768857, - "velocityY": 0.13370799435372274, - "timestamp": 2.786400292581551 - }, - { - "x": 0.7581596501389595, - "y": 6.903357959319175, - "heading": 1.3036999873891952, - "angularVelocity": -9.413814140388183e-8, - "velocityX": 0.9646347718185619, - "velocityY": 0.26741598545201006, - "timestamp": 2.8794474636476792 - }, - { - "x": 0.8927944512482682, - "y": 6.940681409675249, - "heading": 1.3036999705533763, - "angularVelocity": -1.8093853561830772e-7, - "velocityX": 1.4469521165089936, - "velocityY": 0.401123966783992, - "timestamp": 2.9724946347138075 - }, - { - "x": 1.0452258043087184, - "y": 6.982938418917629, - "heading": 1.2738420182759358, - "angularVelocity": -0.320890489579964, - "velocityX": 1.6382158781820235, - "velocityY": 0.4541460934083428, - "timestamp": 3.0655418057799357 - }, - { - "x": 1.1976571573646206, - "y": 7.025195428176415, - "heading": 1.2121674042798947, - "angularVelocity": -0.6628316937460592, - "velocityX": 1.638215878133146, - "velocityY": 0.454146093584667, - "timestamp": 3.158588976846064 - }, - { - "x": 1.3500885104205298, - "y": 7.067452437435176, - "heading": 1.1261361159969507, - "angularVelocity": -0.9245986449367907, - "velocityX": 1.6382158781332203, - "velocityY": 0.4541460935843984, - "timestamp": 3.2516361479121922 - }, - { - "x": 1.5025198634764079, - "y": 7.10970944669405, - "heading": 1.0220884699278219, - "angularVelocity": -1.118224712013896, - "velocityX": 1.638215878132886, - "velocityY": 0.4541460935856051, - "timestamp": 3.3446833189783205 - }, - { - "x": 1.6549512165321918, - "y": 7.151966455953263, - "heading": 0.9053979173511129, - "angularVelocity": -1.2541010246703523, - "velocityX": 1.6382158781318752, - "velocityY": 0.45414609358925084, - "timestamp": 3.4377304900444488 - }, - { - "x": 1.8073825695878554, - "y": 7.194223465212911, - "heading": 0.7806890917766147, - "angularVelocity": -1.3402753049404161, - "velocityX": 1.6382158781305804, - "velocityY": 0.4541460935939214, - "timestamp": 3.530777661110577 - }, - { - "x": 1.9598139226434546, - "y": 7.236480474472791, - "heading": 0.6520865211987327, - "angularVelocity": -1.3821223053249492, - "velocityX": 1.6382158781298872, - "velocityY": 0.4541460935964215, - "timestamp": 3.6238248321767053 - }, - { - "x": 2.1122452756990984, - "y": 7.2787374837325105, - "heading": 0.523466246349196, - "angularVelocity": -1.3823125773284042, - "velocityX": 1.6382158781303655, - "velocityY": 0.45414609359469665, - "timestamp": 3.7168720032428335 - }, - { - "x": 2.264676628754865, - "y": 7.320994492991785, - "heading": 0.39870934727668983, - "angularVelocity": -1.3407919622171214, - "velocityX": 1.638215878131692, - "velocityY": 0.45414609358991137, - "timestamp": 3.8099191743089618 - }, - { - "x": 2.4171079818107506, - "y": 7.363251502250633, - "heading": 0.2819512067590763, - "angularVelocity": -1.2548274082898667, - "velocityX": 1.6382158781329623, - "velocityY": 0.4541460935853291, - "timestamp": 3.90296634537509 - }, - { - "x": 2.5695393348666884, - "y": 7.405508511509292, - "heading": 0.17782836416626208, - "angularVelocity": -1.1190328668758192, - "velocityX": 1.6382158781335259, - "velocityY": 0.4541460935832961, - "timestamp": 3.9960135164412183 - }, - { - "x": 2.721970687922618, - "y": 7.447765520767979, - "heading": 0.09171880670511134, - "angularVelocity": -0.925439822345088, - "velocityX": 1.6382158781334417, - "velocityY": 0.4541460935836001, - "timestamp": 4.089060687507346 - }, - { - "x": 2.874402040978524, - "y": 7.490022530026754, - "heading": 0.029958419943994605, - "angularVelocity": -0.6637535139808163, - "velocityX": 1.6382158781331815, - "velocityY": 0.45414609358453867, - "timestamp": 4.182107858573474 - }, - { - "x": 3.0268333940396634, - "y": 7.532279539266648, - "heading": 2.9431902776520237e-8, - "angularVelocity": -0.3219699230920253, - "velocityX": 1.6382158781894283, - "velocityY": 0.45414609338163237, - "timestamp": 4.275155029639602 - }, - { - "x": 3.1614681951486, - "y": 7.569602989624063, - "heading": 1.2605096609367751e-8, - "angularVelocity": -1.8084167389886298e-7, - "velocityX": 1.4469521165049974, - "velocityY": 0.4011239667984073, - "timestamp": 4.3682022007057295 - }, - { - "x": 3.2512247317781684, - "y": 7.594485290568843, - "heading": 3.849889723802212e-9, - "angularVelocity": -9.409428341828091e-8, - "velocityX": 0.9646347718167455, - "velocityY": 0.26741598545856243, - "timestamp": 4.461249371771857 - }, - { - "x": 3.296103000640869, - "y": 7.606926441192627, - "heading": 8.360584447548999e-24, - "angularVelocity": -4.137567730099069e-8, - "velocityX": 0.482317391796962, - "velocityY": 0.1337079943563437, - "timestamp": 4.554296542837985 - }, - { - "x": 3.296103000640869, - "y": 7.606926441192627, - "heading": 4.186402997192896e-24, - "angularVelocity": 1.3134786042557728e-25, - "velocityX": -6.505907384915032e-26, - "velocityY": -1.7970275247888583e-26, - "timestamp": 4.647343713904113 + "x": 2.3521759510040283, + "y": 7.017032623291016, + "heading": 3.2629175808510626e-26, + "angularVelocity": 1.351359118023865e-26, + "velocityX": 8.97547505231035e-28, + "velocityY": 4.8236792223928767e-26, + "timestamp": 1.7728797597952413 + }, + { + "x": 2.3142846243767354, + "y": 7.012127931705875, + "heading": 1.2115480408206838e-8, + "angularVelocity": 1.4375415553606747e-7, + "velocityX": -0.4495930394768391, + "velocityY": -0.05819577707452758, + "timestamp": 1.8571589262863073 + }, + { + "x": 2.2385019735410854, + "y": 7.00231854884861, + "heading": 3.970446596226117e-8, + "angularVelocity": 3.2735237784985534e-7, + "velocityX": -0.899186050252208, + "velocityY": -0.11639155043500052, + "timestamp": 1.9414380927773733 + }, + { + "x": 2.12482800575389, + "y": 6.987604475658275, + "heading": 9.284103659267056e-8, + "angularVelocity": 6.304828683378364e-7, + "velocityX": -1.348778974923124, + "velocityY": -0.1745873126533039, + "timestamp": 2.0257172592684394 + }, + { + "x": 2.007813392652147, + "y": 6.972457985967442, + "heading": 0.045388788071344756, + "angularVelocity": 0.538551781182121, + "velocityX": -1.3884168291359105, + "velocityY": -0.1797180765004146, + "timestamp": 2.1099964257595056 + }, + { + "x": 1.890798779550621, + "y": 6.957311496274927, + "heading": 0.1278606190497445, + "angularVelocity": 0.9785553703493507, + "velocityX": -1.3884168291333299, + "velocityY": -0.17971807652037916, + "timestamp": 2.194275592250572 + }, + { + "x": 1.7737841664491285, + "y": 6.942165006582153, + "heading": 0.2386191497967303, + "angularVelocity": 1.3141863565858434, + "velocityX": -1.3884168291329324, + "velocityY": -0.179718076523449, + "timestamp": 2.278554758741638 + }, + { + "x": 1.6567695533477331, + "y": 6.927018516888629, + "heading": 0.3691813127912384, + "angularVelocity": 1.5491629596069652, + "velocityX": -1.3884168291317809, + "velocityY": -0.17971807653234628, + "timestamp": 2.3628339252327044 + }, + { + "x": 1.5397549402464386, + "y": 6.911872027194326, + "heading": 0.5114296088502888, + "angularVelocity": 1.6878227678500983, + "velocityX": -1.3884168291305836, + "velocityY": -0.17971807654159477, + "timestamp": 2.4471130917237707 + }, + { + "x": 1.4227403271451358, + "y": 6.896725537500087, + "heading": 0.6575337184093297, + "angularVelocity": 1.7335732618396096, + "velocityX": -1.3884168291306829, + "velocityY": -0.17971807654082692, + "timestamp": 2.531392258214837 + }, + { + "x": 1.3057257140437122, + "y": 6.881579047806781, + "heading": 0.799773650598925, + "angularVelocity": 1.687723527791098, + "velocityX": -1.3884168291321157, + "velocityY": -0.17971807652975855, + "timestamp": 2.615671424705903 + }, + { + "x": 1.188711100942176, + "y": 6.866432558114345, + "heading": 0.930324246141598, + "angularVelocity": 1.5490257079905023, + "velocityX": -1.388416829133451, + "velocityY": -0.17971807651944244, + "timestamp": 2.6999505911969695 + }, + { + "x": 1.071696487840612, + "y": 6.851286068422124, + "heading": 1.0410732405530951, + "angularVelocity": 1.3140732048321446, + "velocityX": -1.3884168291337817, + "velocityY": -0.17971807651688804, + "timestamp": 2.7842297576880357 + }, + { + "x": 0.9546818747390692, + "y": 6.836139578729739, + "heading": 1.1235395305504658, + "angularVelocity": 0.9784896247889747, + "velocityX": -1.3884168291335306, + "velocityY": -0.1797180765188294, + "timestamp": 2.868508924179102 + }, + { + "x": 0.8376672616369766, + "y": 6.820993089041604, + "heading": 1.1689256310372955, + "angularVelocity": 0.5385209936982573, + "velocityX": -1.3884168291400538, + "velocityY": -0.17971807646840415, + "timestamp": 2.9527880906701682 + }, + { + "x": 0.7239932938499879, + "y": 6.806279015849672, + "heading": 1.1689256841765412, + "angularVelocity": 6.305146069742176e-7, + "velocityX": -1.3487789749206713, + "velocityY": -0.1745873126722503, + "timestamp": 3.0370672571612345 + }, + { + "x": 0.6482106430144319, + "y": 6.796469632991681, + "heading": 1.1689257117674343, + "angularVelocity": 3.27375012689987e-7, + "velocityX": -0.8991860502510931, + "velocityY": -0.1163915504436125, + "timestamp": 3.1213464236523008 + }, + { + "x": 0.6103193163871765, + "y": 6.79156494140625, + "heading": 1.1689257238838953, + "angularVelocity": 1.4376578928572422e-7, + "velocityX": -0.44959303947639306, + "velocityY": -0.05819577707797239, + "timestamp": 3.205625590143367 + }, + { + "x": 0.6103193163871765, + "y": 6.79156494140625, + "heading": 1.1689257238838953, + "angularVelocity": 3.0587665357762507e-24, + "velocityX": 4.4273867233926817e-26, + "velocityY": 3.2272073611359003e-27, + "timestamp": 3.2899047566344333 + }, + { + "x": 0.6733155792661583, + "y": 6.796419497130506, + "heading": 1.1689257179975108, + "angularVelocity": -5.4312792519524506e-8, + "velocityX": 0.5812571386949579, + "velocityY": 0.044792262921005174, + "timestamp": 3.3982840928786664 + }, + { + "x": 0.7993081014378632, + "y": 6.806128608302689, + "heading": 1.1689257047296802, + "angularVelocity": -1.2242029760434803e-7, + "velocityX": 1.1625142443000427, + "velocityY": 0.08958452329237418, + "timestamp": 3.5066634291228995 + }, + { + "x": 0.9882968721435262, + "y": 6.820692274093818, + "heading": 1.1689256795952214, + "angularVelocity": -2.3191190757497266e-7, + "velocityX": 1.743771250635604, + "velocityY": 0.13437677601484202, + "timestamp": 3.6150427653671326 + }, + { + "x": 1.226026584318479, + "y": 6.839011966124108, + "heading": 1.1623600098622169, + "angularVelocity": -0.06058045712892038, + "velocityX": 2.193496661016895, + "velocityY": 0.16903307092605394, + "timestamp": 3.7234221016113658 + }, + { + "x": 1.463756296493383, + "y": 6.8573316581551245, + "heading": 1.1373583072341475, + "angularVelocity": -0.23068698789340977, + "velocityX": 2.193496661016445, + "velocityY": 0.16903307093275596, + "timestamp": 3.831801437855599 + }, + { + "x": 1.701486008668291, + "y": 6.875651350186089, + "heading": 1.0993044418538873, + "angularVelocity": -0.3511173504006862, + "velocityX": 2.1934966610164817, + "velocityY": 0.16903307093227754, + "timestamp": 3.940180774099832 + }, + { + "x": 1.9392157208432044, + "y": 6.893971042216987, + "heading": 1.052051199011011, + "angularVelocity": -0.43599863664408267, + "velocityX": 2.193496661016529, + "velocityY": 0.16903307093165879, + "timestamp": 4.048560110344066 + }, + { + "x": 2.1769454330181204, + "y": 6.91229073424785, + "heading": 0.9983385147189527, + "angularVelocity": -0.49559894121344894, + "velocityX": 2.193496661016554, + "velocityY": 0.16903307093133652, + "timestamp": 4.156939446588299 + }, + { + "x": 2.414675145193034, + "y": 6.930610426278742, + "heading": 0.94010924392218, + "angularVelocity": -0.5372728124626364, + "velocityX": 2.1934966610165327, + "velocityY": 0.16903307093161182, + "timestamp": 4.265318782832533 + }, + { + "x": 2.6524048573679395, + "y": 6.948930118309735, + "heading": 0.8787410045179366, + "angularVelocity": -0.5662356084738315, + "velocityX": 2.1934966610164612, + "velocityY": 0.16903307093254125, + "timestamp": 4.373698119076766 + }, + { + "x": 2.890134569542833, + "y": 6.967249810340885, + "heading": 0.8152156203040447, + "angularVelocity": -0.5861392624765411, + "velocityX": 2.19349666101635, + "velocityY": 0.16903307093398154, + "timestamp": 4.482077455321 + }, + { + "x": 3.1278642817177125, + "y": 6.985569502372218, + "heading": 0.7502413213076805, + "angularVelocity": -0.5995081834598467, + "velocityX": 2.1934966610162197, + "velocityY": 0.16903307093567946, + "timestamp": 4.5904567915652335 + }, + { + "x": 3.365593993892578, + "y": 7.003889194403733, + "heading": 0.6843427304656562, + "angularVelocity": -0.6080364867110983, + "velocityX": 2.1934966610160904, + "velocityY": 0.1690330709373518, + "timestamp": 4.698836127809467 + }, + { + "x": 3.603323706067432, + "y": 7.022208886435398, + "heading": 0.6179279596644911, + "angularVelocity": -0.6127992023451708, + "velocityX": 2.1934966610159834, + "velocityY": 0.1690330709387406, + "timestamp": 4.807215464053701 + }, + { + "x": 3.8410534182422786, + "y": 7.040528578467161, + "heading": 0.5513412518560862, + "angularVelocity": -0.6143856395129749, + "velocityX": 2.1934966610159137, + "velocityY": 0.1690330709396457, + "timestamp": 4.915594800297934 + }, + { + "x": 4.078783130417123, + "y": 7.058848270498957, + "heading": 0.48490739393082904, + "angularVelocity": -0.6129753163974749, + "velocityX": 2.1934966610158906, + "velocityY": 0.16903307093994402, + "timestamp": 5.023974136542168 + }, + { + "x": 4.31651284259197, + "y": 7.077167962530715, + "heading": 0.4189731502579807, + "angularVelocity": -0.6083654500731145, + "velocityX": 2.1934966610159172, + "velocityY": 0.1690330709396015, + "timestamp": 5.132353472786401 + }, + { + "x": 4.554242554766825, + "y": 7.0954876545623735, + "heading": 0.353951234014042, + "angularVelocity": -0.5999475407139573, + "velocityX": 2.1934966610159883, + "velocityY": 0.1690330709386799, + "timestamp": 5.240732809030635 + }, + { + "x": 4.79197226694169, + "y": 7.1138073465938865, + "heading": 0.2903723240116442, + "angularVelocity": -0.5866331369581602, + "velocityX": 2.1934966610160918, + "velocityY": 0.1690330709373368, + "timestamp": 5.3491121452748684 + }, + { + "x": 5.0297019791165685, + "y": 7.132127038625234, + "heading": 0.22895107669335069, + "angularVelocity": -0.5667247046049495, + "velocityX": 2.193496661016209, + "velocityY": 0.16903307093581316, + "timestamp": 5.457491481519102 + }, + { + "x": 5.267431691291459, + "y": 7.150446730656428, + "heading": 0.17067503234950135, + "angularVelocity": -0.5377043850178608, + "velocityX": 2.1934966610163182, + "velocityY": 0.16903307093439685, + "timestamp": 5.565870817763336 + }, + { + "x": 5.505161403466358, + "y": 7.16876642268751, + "heading": 0.11692607465311596, + "angularVelocity": -0.49593363051478706, + "velocityX": 2.1934966610163986, + "velocityY": 0.16903307093335337, + "timestamp": 5.674250154007569 + }, + { + "x": 5.742891115641262, + "y": 7.187086114718534, + "heading": 0.06964863269353613, + "angularVelocity": -0.43622191829113954, + "velocityX": 2.1934966610164395, + "velocityY": 0.16903307093282607, + "timestamp": 5.782629490251803 + }, + { + "x": 5.980620827816166, + "y": 7.20540580674955, + "heading": 0.031581441152300896, + "angularVelocity": -0.35124030890400276, + "velocityX": 2.1934966610164457, + "velocityY": 0.1690330709327442, + "timestamp": 5.891008826496036 + }, + { + "x": 6.218350539991069, + "y": 7.223725498780575, + "heading": 0.006572709542556832, + "angularVelocity": -0.23075184326084827, + "velocityX": 2.193496661016438, + "velocityY": 0.16903307093283967, + "timestamp": 5.99938816274027 + }, + { + "x": 6.456080252165983, + "y": 7.242045190811364, + "heading": 4.42760723315573e-8, + "angularVelocity": -0.06064500387484363, + "velocityX": 2.1934966610165403, + "velocityY": 0.16903307093065992, + "timestamp": 6.107767498984503 + }, + { + "x": 6.645069022871665, + "y": 7.25660885660225, + "heading": 1.914820045081504e-8, + "angularVelocity": -2.318511328037341e-7, + "velocityX": 1.7437712506357763, + "velocityY": 0.1343767760126046, + "timestamp": 6.216146835228737 + }, + { + "x": 6.771061545043378, + "y": 7.266317967774324, + "heading": 5.884427199036025e-9, + "angularVelocity": -1.2238286108238435e-7, + "velocityX": 1.1625142443001213, + "velocityY": 0.08958452329135717, + "timestamp": 6.324526171472971 + }, + { + "x": 6.834057807922363, + "y": 7.271172523498535, + "heading": 6.250436275292675e-24, + "angularVelocity": -5.429473369144319e-8, + "velocityX": 0.5812571386949893, + "velocityY": 0.04479226292059838, + "timestamp": 6.432905507717204 + }, + { + "x": 6.834057807922363, + "y": 7.271172523498535, + "heading": 2.9708189294917462e-24, + "angularVelocity": -2.8492370133741702e-24, + "velocityX": 1.1892634808647167e-25, + "velocityY": 9.309147669588167e-27, + "timestamp": 6.541284843961438 } ], "trajectoryWaypoints": [ @@ -928,8 +975,8 @@ "controlIntervalCount": 16 }, { - "timestamp": 0.8090013997643889, - "isStopPoint": false, + "timestamp": 1.0828175295421705, + "isStopPoint": true, "x": 1.7625242471694946, "y": 7.0123291015625, "heading": 0, @@ -939,7 +986,7 @@ "controlIntervalCount": 7 }, { - "timestamp": 1.3533418842502247, + "timestamp": 1.7728797597952413, "isStopPoint": true, "x": 2.3521759510040283, "y": 7.017032623291016, @@ -950,21 +997,21 @@ "controlIntervalCount": 18 }, { - "timestamp": 2.6933531215154227, + "timestamp": 3.2899047566344333, "isStopPoint": true, - "x": 0.6235248446464539, - "y": 6.866034507751465, - "heading": 1.3037, + "x": 0.6103193163871765, + "y": 6.79156494140625, + "heading": 1.1689257238838953, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 21 + "controlIntervalCount": 30 }, { - "timestamp": 4.647343713904113, + "timestamp": 6.541284843961438, "isStopPoint": true, - "x": 3.296103000640869, - "y": 7.606926441192627, + "x": 6.834057807922363, + "y": 7.271172523498535, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -985,12 +1032,6 @@ ], "type": "StopPoint" }, - { - "scope": [ - 4 - ], - "type": "StopPoint" - }, { "scope": [ "last" @@ -1012,16 +1053,38 @@ { "scope": [ "first", - "last" + 2 ], "type": "MaxVelocity", - "velocity": 1.7 + "velocity": 1.4 }, { "scope": [ 2 ], "type": "WptZeroVelocity" + }, + { + "scope": [ + 2, + 3 + ], + "type": "MaxVelocity", + "velocity": 1.4 + }, + { + "scope": [ + 3, + "last" + ], + "type": "MaxVelocity", + "velocity": 2.2 + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, @@ -1061,17 +1124,26 @@ "controlIntervalCount": 18 }, { - "x": 0.6313003897666931, - "y": 4.255156517028809, - "heading": -1.2897, + "x": 0.6103193163871765, + "y": 4.308632850646973, + "heading": -1.1752681674346586, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 22 + "controlIntervalCount": 21 }, { - "x": 2.8538730144500732, - "y": 2.2070720195770264, + "x": 2.707773208618164, + "y": 2.199841260910034, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "x": 7.06566047668457, + "y": 0.7616762518882751, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -1084,586 +1156,757 @@ "x": 0.6313003897666931, "y": 4.255156517028809, "heading": -1.2897, - "angularVelocity": 2.5588458209070936e-23, - "velocityX": 3.6747187166411656e-23, - "velocityY": 2.9631382958424116e-23, + "angularVelocity": -1.3029252310464996e-24, + "velocityX": 9.818866312976863e-25, + "velocityY": -2.13204077766819e-25, "timestamp": 0 }, { - "x": 0.6442648165759174, - "y": 4.253481867543582, - "heading": -1.2896998999232763, - "angularVelocity": 0.000002030081537303308, - "velocityX": 0.26298666147739386, - "velocityY": -0.03397068638245913, - "timestamp": 0.0492968986959011 - }, - { - "x": 0.6701936695279935, - "y": 4.25013256816077, - "heading": -1.289699690051548, - "angularVelocity": 0.000004257300839893244, - "velocityX": 0.5259733094372531, - "velocityY": -0.06794138112974009, - "timestamp": 0.0985937973918022 - }, - { - "x": 0.7090869476233618, - "y": 4.245108618261832, - "heading": -1.2896993558074163, - "angularVelocity": 0.000006780226354565095, - "velocityX": 0.7889599371207936, - "velocityY": -0.10191208842422805, - "timestamp": 0.1478906960877033 - }, - { - "x": 0.7609446491960873, - "y": 4.238410016815877, - "heading": -1.2896988728952734, - "angularVelocity": 0.00000979599439019053, - "velocityX": 1.051946531010426, - "velocityY": -0.13588281663065846, - "timestamp": 0.1971875947836044 - }, - { - "x": 0.8257667709142923, - "y": 4.230036761761128, - "heading": -1.2896981927241198, - "angularVelocity": 0.000013797443073734933, - "velocityX": 1.3149330573120783, - "velocityY": -0.1698535866607121, - "timestamp": 0.2464844934795055 - }, - { - "x": 0.90355330278228, - "y": 4.219988846912366, - "heading": -1.2896971695227037, - "angularVelocity": 0.00002075589830370764, - "velocityX": 1.5779193808485028, - "velocityY": -0.20382448215952323, - "timestamp": 0.2957813921754066 - }, - { - "x": 0.9915565757190965, - "y": 4.208621348540917, - "heading": -1.2730220238110053, - "angularVelocity": 0.3382595285468722, - "velocityX": 1.7851685453822215, - "velocityY": -0.23059256610788906, - "timestamp": 0.3450782908713077 - }, - { - "x": 1.0795598357636118, - "y": 4.1972537503614955, - "heading": -1.2172146776466959, - "angularVelocity": 1.1320660658304207, - "velocityX": 1.7851682838586471, - "velocityY": -0.23059459073774355, - "timestamp": 0.3943751895672088 - }, - { - "x": 1.167563095807722, - "y": 4.1858861521789406, - "heading": -1.1235960458886458, - "angularVelocity": 1.8990775126759454, - "velocityX": 1.7851682838504348, - "velocityY": -0.23059459080130768, - "timestamp": 0.4436720882631099 - }, - { - "x": 1.2555663558538792, - "y": 4.17451855401223, - "heading": -0.9942697695783443, - "angularVelocity": 2.6234160714263077, - "velocityX": 1.7851682838919467, - "velocityY": -0.2305945904799162, - "timestamp": 0.492968986959011 - }, - { - "x": 1.343569615881345, - "y": 4.163150955700822, - "heading": -0.8330305095755579, - "angularVelocity": 3.270778979372043, - "velocityX": 1.7851682835127938, - "velocityY": -0.23059459341511745, - "timestamp": 0.5422658856549121 - }, - { - "x": 1.4315728758027453, - "y": 4.151783356568302, - "heading": -0.6481619169999266, - "angularVelocity": 3.750105939037553, - "velocityX": 1.7851682813612264, - "velocityY": -0.23059461007159965, - "timestamp": 0.5915627843508132 - }, - { - "x": 1.5195761357005462, - "y": 4.140415757253084, - "heading": -0.46092963892673033, - "angularVelocity": 3.798053894387562, - "velocityX": 1.785168280882508, - "velocityY": -0.2305946137776485, - "timestamp": 0.6408596830467144 - }, - { - "x": 1.607579395709073, - "y": 4.129048158795058, - "heading": -0.29529130470998255, - "angularVelocity": 3.3600153072209418, - "velocityX": 1.7851682831286084, - "velocityY": -0.23059459638932683, - "timestamp": 0.6901565817426155 - }, - { - "x": 1.6955826557496305, - "y": 4.117680560584999, - "heading": -0.16079446814860227, - "angularVelocity": 2.7283021877512823, - "velocityX": 1.785168283778364, - "velocityY": -0.23059459135922616, - "timestamp": 0.7394534804385167 - }, - { - "x": 1.7835859158077405, - "y": 4.106312962510822, - "heading": -0.06161125041031065, - "angularVelocity": 2.011956540108646, - "velocityX": 1.7851682841344167, - "velocityY": -0.23059458860283424, - "timestamp": 0.7887503791344178 + "x": 0.6559720855575579, + "y": 4.251969633511474, + "heading": -1.2896999477311053, + "angularVelocity": 7.68600953739464e-7, + "velocityX": 0.3627910812108772, + "velocityY": -0.04686232056148057, + "timestamp": 0.06800524342692997 + }, + { + "x": 0.7053154748885234, + "y": 4.245595866526154, + "heading": -1.2896998274887912, + "angularVelocity": 0.0000017681329843892469, + "velocityX": 0.7255821293248338, + "velocityY": -0.09372464039730581, + "timestamp": 0.13601048685385994 + }, + { + "x": 0.7793305510073084, + "y": 4.236035216220894, + "heading": -1.289699592159626, + "angularVelocity": 0.0000034604561825362674, + "velocityX": 1.0883730781481924, + "velocityY": -0.14058695805615118, + "timestamp": 0.2040157302807899 + }, + { + "x": 0.8737534106566409, + "y": 4.2238384675544545, + "heading": -1.2686933528400068, + "angularVelocity": 0.3088914657321989, + "velocityX": 1.3884644020249364, + "velocityY": -0.17935012142916007, + "timestamp": 0.2720209737077199 + }, + { + "x": 0.9681762696017908, + "y": 4.211641713436369, + "heading": -1.1852876077469632, + "angularVelocity": 1.2264605034854565, + "velocityX": 1.388464391670108, + "velocityY": -0.17935020159425882, + "timestamp": 0.34002621713464987 + }, + { + "x": 1.06259912854715, + "y": 4.199444959319902, + "heading": -1.0458164737621631, + "angularVelocity": 2.0508879456428724, + "velocityX": 1.3884643916731867, + "velocityY": -0.17935020157042256, + "timestamp": 0.40803146056157985 + }, + { + "x": 1.1570219874918148, + "y": 4.1872482051980615, + "heading": -0.8604729695452614, + "angularVelocity": 2.7254296121452617, + "velocityX": 1.388464391662975, + "velocityY": -0.17935020164947402, + "timestamp": 0.47603670398850984 + }, + { + "x": 1.2514448464329961, + "y": 4.175051451049253, + "heading": -0.6468064986930537, + "angularVelocity": 3.1419117127606087, + "velocityX": 1.3884643916117518, + "velocityY": -0.17935020204602378, + "timestamp": 0.5440419474154398 + }, + { + "x": 1.3458677053738775, + "y": 4.162854696898122, + "heading": -0.43285566722442825, + "angularVelocity": 3.1460931641030103, + "velocityX": 1.3884643916073398, + "velocityY": -0.17935020208018032, + "timestamp": 0.6120471908423697 + }, + { + "x": 1.4402905643180246, + "y": 4.1506579427722725, + "heading": -0.2468424865709028, + "angularVelocity": 2.735277035709639, + "velocityX": 1.3884643916553625, + "velocityY": -0.17935020170840796, + "timestamp": 0.6800524342692996 + }, + { + "x": 1.5347134232631718, + "y": 4.138461188654166, + "heading": -0.1064956083992093, + "angularVelocity": 2.0637655436450704, + "velocityX": 1.3884643916700699, + "velocityY": -0.17935020159455195, + "timestamp": 0.7480576776962296 + }, + { + "x": 1.6291362822082969, + "y": 4.126264434535885, + "heading": -0.022089749460474963, + "angularVelocity": 1.241166926038967, + "velocityX": 1.3884643916697392, + "velocityY": -0.17935020159711362, + "timestamp": 0.8160629211231595 + }, + { + "x": 1.7235591414801577, + "y": 4.114067682947199, + "heading": -4.084921018506283e-7, + "angularVelocity": 0.3248182030567626, + "velocityX": 1.3884643964743102, + "velocityY": -0.17935016440007143, + "timestamp": 0.8840681645500894 + }, + { + "x": 1.7975742177797642, + "y": 4.104507034041778, + "heading": -1.729250271854775e-7, + "angularVelocity": 0.000003463954583417705, + "velocityX": 1.0883730808071284, + "velocityY": -0.14058693747185988, + "timestamp": 0.9520734079770193 + }, + { + "x": 1.8469176071929212, + "y": 4.0981332676927495, + "heading": -5.242149315395944e-8, + "angularVelocity": 0.0000017719741590366667, + "velocityX": 0.7255821305334361, + "velocityY": -0.09372463104079504, + "timestamp": 1.0200786514039493 }, { "x": 1.8715893030166624, "y": 4.094946384429932, - "heading": 3.044635127032748e-22, - "angularVelocity": 1.2497997245298, - "velocityX": 1.7851708634206476, - "velocityY": -0.23057389778225362, - "timestamp": 0.838047277830319 - }, - { - "x": 2.001501484117773, - "y": 4.096925277087298, - "heading": 0.03742913022292678, - "angularVelocity": 0.5185396539200237, - "velocityX": 1.7997911526916002, - "velocityY": 0.027415392972907092, - "timestamp": 0.9102290867623309 - }, - { - "x": 2.1314136513515103, - "y": 4.098905079913163, - "heading": 0.0000021975515072904746, - "angularVelocity": -0.5185092092478852, - "velocityX": 1.7997909605743085, - "velocityY": 0.02742800236179506, - "timestamp": 0.9824108956943429 - }, - { - "x": 2.243505108146192, - "y": 4.10061330393091, - "heading": 0.0000012819688946605248, - "angularVelocity": -0.000012684395503198552, - "velocityX": 1.5529045122748328, - "velocityY": 0.023665575066908254, - "timestamp": 1.054592704626355 - }, - { - "x": 2.3275737085405206, - "y": 4.101894472003175, - "heading": 6.315287448797483e-7, - "angularVelocity": -0.00000901113673104848, - "velocityX": 1.1646784922432887, - "velocityY": 0.017749182116954924, - "timestamp": 1.126774513558367 - }, - { - "x": 2.3836194443971235, - "y": 4.102748584068442, - "heading": 2.0840831354227822e-7, - "angularVelocity": -0.000005861870706731764, - "velocityX": 0.7764523594773347, - "velocityY": 0.011832788314715953, - "timestamp": 1.1989563224903792 + "heading": -9.905348338146396e-24, + "angularVelocity": 7.708448718411703e-7, + "velocityX": 0.36279108169431745, + "velocityY": -0.046862316818874365, + "timestamp": 1.0880838948308793 }, { - "x": 2.41164231300354, - "y": 4.103175640106201, - "heading": -4.343161804381014e-22, - "angularVelocity": -0.000002887269197403718, - "velocityX": 0.3882261891332122, - "velocityY": 0.0059163942283817755, - "timestamp": 1.2711381314223913 + "x": 1.8715893030166624, + "y": 4.094946384429932, + "heading": -4.879834777552459e-24, + "angularVelocity": 2.1422164275294007e-24, + "velocityX": -3.536647328700017e-24, + "velocityY": 5.299012492424587e-25, + "timestamp": 1.1560891382578093 + }, + { + "x": 1.9311588685312648, + "y": 4.095854097517496, + "heading": 5.98134982599928e-18, + "angularVelocity": 5.683500198215718e-17, + "velocityX": 0.5660317002762608, + "velocityY": 0.008625115491089496, + "timestamp": 1.261329826165445 + }, + { + "x": 2.0363873382706825, + "y": 4.097457554892782, + "heading": 2.3583226342325868e-17, + "angularVelocity": 1.6725352965931608e-16, + "velocityX": 0.9998839026192134, + "velocityY": 0.015236097436880057, + "timestamp": 1.3665705140730808 + }, + { + "x": 2.1416158080101013, + "y": 4.099061012268066, + "heading": 3.485956605861461e-16, + "angularVelocity": 3.0882773640653748e-15, + "velocityX": 0.9998839026192254, + "velocityY": 0.01523609743688024, + "timestamp": 1.4718112019807166 + }, + { + "x": 2.24684427774952, + "y": 4.100664469643351, + "heading": 2.1220134489662188e-16, + "angularVelocity": -1.29602265436758e-15, + "velocityX": 0.9998839026192254, + "velocityY": 0.01523609743688024, + "timestamp": 1.5770518898883523 + }, + { + "x": 2.3520727474889376, + "y": 4.102267927018636, + "heading": 1.5391468007019663e-22, + "angularVelocity": -2.016341732469286e-15, + "velocityX": 0.9998839026192134, + "velocityY": 0.015236097436880057, + "timestamp": 1.682292577795988 }, { "x": 2.41164231300354, "y": 4.103175640106201, - "heading": -2.1484022653531463e-22, - "angularVelocity": 8.099300815455187e-23, - "velocityX": 1.2310180125110273e-23, - "velocityY": -5.1753556121623715e-23, - "timestamp": 1.3433199403544034 - }, - { - "x": 2.3823203832476643, - "y": 4.105678739540131, - "heading": -4.713122979131873e-8, - "angularVelocity": -6.372031214090865e-7, - "velocityX": -0.39642558127400507, - "velocityY": 0.03384131455001896, - "timestamp": 1.4172857264715435 - }, - { - "x": 2.3236765252241245, - "y": 4.110684938319997, - "heading": -1.489263961217733e-7, - "angularVelocity": -0.0000013762466631428888, - "velocityX": -0.7928511424277298, - "velocityY": 0.06768262791039739, - "timestamp": 1.4912515125886836 - }, - { - "x": 2.2357107419093456, - "y": 4.118194236269814, - "heading": -3.204509063199309e-7, - "angularVelocity": -0.000002318970962148814, - "velocityX": -1.1892766633409002, - "velocityY": 0.10152393889149473, - "timestamp": 1.5652172987058237 - }, - { - "x": 2.118423042232596, - "y": 4.128206632861628, - "heading": -6.069009309173134e-7, - "angularVelocity": -0.000003872736837322732, - "velocityX": -1.585702063532462, - "velocityY": 0.1353652427347487, - "timestamp": 1.6391830848229638 - }, - { - "x": 1.9857671108062405, - "y": 4.1395309526428035, - "heading": -0.05006393031110381, - "angularVelocity": -0.676844336256865, - "velocityX": -1.7934769356235545, - "velocityY": 0.15310213513098953, - "timestamp": 1.713148870940104 - }, - { - "x": 1.8531111795597293, - "y": 4.150855274530819, - "heading": -0.16469064728369365, - "angularVelocity": -1.5497262043704214, - "velocityX": -1.7934769331921003, - "velocityY": 0.1531021636149704, - "timestamp": 1.787114657057244 - }, - { - "x": 1.7204552483160596, - "y": 4.162179596452121, - "heading": -0.3328870431123415, - "angularVelocity": -2.2739756400651823, - "velocityX": -1.7934769331536837, - "velocityY": 0.15310216406498003, - "timestamp": 1.8610804431743841 - }, - { - "x": 1.5877993170796554, - "y": 4.173503918458529, - "heading": -0.5380010346158053, - "angularVelocity": -2.7730928348226245, - "velocityX": -1.793476933055458, - "velocityY": 0.15310216521560938, - "timestamp": 1.9350462292915243 - }, - { - "x": 1.455143385839801, - "y": 4.1848282404245225, - "heading": -0.7563600030365335, - "angularVelocity": -2.952161801875693, - "velocityX": -1.793476933102103, - "velocityY": 0.15310216466919027, - "timestamp": 2.0090120154086644 - }, - { - "x": 1.322487454589416, - "y": 4.196152562267158, - "heading": -0.9608101638901362, - "angularVelocity": -2.7641180008526347, - "velocityX": -1.7934769332444735, - "velocityY": 0.15310216300143226, - "timestamp": 2.0829778015258045 - }, - { - "x": 1.189831523339715, - "y": 4.207476884117805, - "heading": -1.1279167268349728, - "angularVelocity": -2.2592413562696274, - "velocityX": -1.7934769332352274, - "velocityY": 0.15310216310975738, - "timestamp": 2.1569435876429446 - }, - { - "x": 1.0571755920924437, - "y": 4.218801205996916, - "heading": -1.241183305817285, - "angularVelocity": -1.5313374592265028, - "velocityX": -1.7934769332023779, - "velocityY": 0.15310216349457723, - "timestamp": 2.2309093737600847 - }, - { - "x": 0.9245196595573416, - "y": 4.230125512789948, - "heading": -1.2896993930671263, - "angularVelocity": -0.6559260679391166, - "velocityX": -1.793476950613546, - "velocityY": 0.15310195953433603, - "timestamp": 2.304875159877225 - }, - { - "x": 0.8072319603911625, - "y": 4.240137915362714, - "heading": -1.289699679914329, - "angularVelocity": -0.000003878106589948581, - "velocityX": -1.5857020566296687, - "velocityY": 0.13536532359582226, - "timestamp": 2.378840945994365 - }, - { - "x": 0.7192661773418804, - "y": 4.24764721642263, - "heading": -1.289699851355636, - "angularVelocity": -0.000002317846073346953, - "velocityX": -1.1892766597514466, - "velocityY": 0.10152398093927968, - "timestamp": 2.452806732111505 - }, - { - "x": 0.6606223194613006, - "y": 4.252653416877164, - "heading": -1.289699952984873, - "angularVelocity": -0.000001374003340703804, - "velocityX": -0.7928511404949471, - "velocityY": 0.06768265055151669, - "timestamp": 2.526772518228645 - }, - { - "x": 0.6313003897666931, - "y": 4.255156517028809, - "heading": -1.2897, - "angularVelocity": -6.356334408172667e-7, - "velocityX": -0.3964255804456696, - "velocityY": 0.033841324253356724, - "timestamp": 2.6007383043457852 + "heading": 2.0916641305797265e-25, + "angularVelocity": -1.4605141481442386e-21, + "velocityX": 0.5660317002762608, + "velocityY": 0.008625115491089496, + "timestamp": 1.7875332657036238 }, { - "x": 0.6313003897666931, - "y": 4.255156517028809, - "heading": -1.2897, - "angularVelocity": 4.084009254517039e-24, - "velocityX": 9.66972044230157e-25, - "velocityY": -2.029977656808017e-25, - "timestamp": 2.6747040904629253 - }, - { - "x": 0.6646314231944946, - "y": 4.224442212318432, - "heading": -1.2896999897619994, - "angularVelocity": 1.1153252970301958e-7, - "velocityX": 0.3631074603784824, - "velocityY": -0.3346008819328531, - "timestamp": 2.7664979450784686 - }, - { - "x": 0.7312934878819572, - "y": 4.163013604898331, - "heading": -1.2896999663947442, - "angularVelocity": 2.545623060054011e-7, - "velocityX": 0.7262148971372956, - "velocityY": -0.6692017420706505, - "timestamp": 2.858291799694012 - }, - { - "x": 0.8312865773246638, - "y": 4.070870700770457, - "heading": -1.289699921224478, - "angularVelocity": 4.920837706738393e-7, - "velocityX": 1.0893222630371484, - "velocityY": -1.0038025368233243, - "timestamp": 2.950085654309555 - }, - { - "x": 0.9527932606026395, - "y": 3.958903176412677, - "heading": -1.2720972534731236, - "angularVelocity": 0.19176303059805994, - "velocityX": 1.3236908264379732, - "velocityY": -1.2197714632066494, - "timestamp": 3.0418795089250983 - }, - { - "x": 1.0742999439102807, - "y": 3.846935652087084, - "heading": -1.2219246048700179, - "angularVelocity": 0.5465796028856406, - "velocityX": 1.323690826761149, - "velocityY": -1.2197714628560048, - "timestamp": 3.1336733635406415 - }, - { - "x": 1.195806627217665, - "y": 3.7349681277612126, - "heading": -1.1466520807441116, - "angularVelocity": 0.8200170310002478, - "velocityX": 1.323690826758351, - "velocityY": -1.2197714628590413, - "timestamp": 3.2254672181561848 - }, - { - "x": 1.3173133105246166, - "y": 3.6230006034348716, - "heading": -1.0525810772100828, - "angularVelocity": 1.024807204447648, - "velocityX": 1.3236908267536351, - "velocityY": -1.2197714628641585, - "timestamp": 3.317261072771728 - }, - { - "x": 1.4388199938313129, - "y": 3.5110330791082536, - "heading": -0.944987004768565, - "angularVelocity": 1.1721271853344641, - "velocityX": 1.3236908267508531, - "velocityY": -1.2197714628671776, - "timestamp": 3.4090549273872712 - }, - { - "x": 1.560326677138363, - "y": 3.3990655547820197, - "heading": -0.8283257581326825, - "angularVelocity": 1.2709047585428277, - "velocityX": 1.3236908267547107, - "velocityY": -1.2197714628629912, - "timestamp": 3.5008487820028145 - }, - { - "x": 1.6818333604464215, - "y": 3.28709803045688, - "heading": -0.706465158639104, - "angularVelocity": 1.3275463810073416, - "velocityX": 1.3236908267656953, - "velocityY": -1.219771462851071, - "timestamp": 3.5926426366183577 - }, - { - "x": 1.8033400437556388, - "y": 3.1751305061329975, - "heading": -0.5829200485324901, - "angularVelocity": 1.3458973982959206, - "velocityX": 1.3236908267783167, - "velocityY": -1.219771462837374, - "timestamp": 3.684436491233901 - }, - { - "x": 1.924846727065488, - "y": 3.063162981809801, - "heading": -0.4610849728764882, - "angularVelocity": 1.3272683249470145, - "velocityX": 1.3236908267852028, - "velocityY": -1.2197714628299015, - "timestamp": 3.776230345849444 - }, - { - "x": 2.0463534103751515, - "y": 2.951195457486403, - "heading": -0.3444658570370702, - "angularVelocity": 1.2704457866798307, - "velocityX": 1.3236908267831804, - "velocityY": -1.2197714628320961, - "timestamp": 3.8680242004649874 - }, - { - "x": 2.1678600936840966, - "y": 2.8392279331622254, - "heading": -0.23692016972533708, - "angularVelocity": 1.1716000789177297, - "velocityX": 1.3236908267753509, - "velocityY": -1.2197714628405927, - "timestamp": 3.9598180550805306 - }, - { - "x": 2.289366776992324, - "y": 2.7272604088372687, - "heading": -0.14289670217852118, - "angularVelocity": 1.0242893485692561, - "velocityX": 1.3236908267675338, - "velocityY": -1.219771462849076, - "timestamp": 4.051611909696074 - }, - { - "x": 2.4108734603001536, - "y": 2.6152928845118804, - "heading": -0.06767016726185003, - "angularVelocity": 0.819516025683196, - "velocityX": 1.3236908267631986, - "velocityY": -1.2197714628537804, - "timestamp": 4.143405764311618 - }, - { - "x": 2.5323801436078655, - "y": 2.5033253601863636, - "heading": -0.017545761093029383, - "angularVelocity": 0.5460540509901755, - "velocityX": 1.323690826761916, - "velocityY": -1.2197714628551724, - "timestamp": 4.235199618927162 - }, - { - "x": 2.6538868269212, - "y": 2.391357835866955, - "heading": -7.879123548258675e-8, - "angularVelocity": 0.19114223250869905, - "velocityX": 1.3236908268231702, - "velocityY": -1.2197714627886354, - "timestamp": 4.326993473542705 - }, - { - "x": 2.7538799163461247, - "y": 2.2992149317197845, - "heading": -3.3611593837330055e-8, - "angularVelocity": 4.921859075913182e-7, - "velocityX": 1.0893222628434338, - "velocityY": -1.0038025370335428, - "timestamp": 4.418787328158249 - }, - { - "x": 2.820541981025505, - "y": 2.2377863242909117, - "heading": -1.0239860234289194e-8, - "angularVelocity": 2.546110924410768e-7, - "velocityX": 0.7262148970492437, - "velocityY": -0.6692017421662043, - "timestamp": 4.510581182773793 - }, - { - "x": 2.8538730144500732, - "y": 2.2070720195770264, - "heading": 1.5542259532589824e-23, - "angularVelocity": 1.1155278615519993e-7, - "velocityX": 0.36310746034326163, - "velocityY": -0.33460088197107457, - "timestamp": 4.602375037389336 - }, - { - "x": 2.8538730144500732, - "y": 2.2070720195770264, - "heading": 7.662197382375648e-24, - "angularVelocity": -2.3734134355543795e-24, - "velocityX": -9.29253035229337e-26, - "velocityY": 8.677134783852105e-26, - "timestamp": 4.69416889200488 + "x": 2.41164231300354, + "y": 4.103175640106201, + "heading": 2.1285305918257914e-25, + "angularVelocity": 3.5030688234893543e-26, + "velocityX": 9.550053625303018e-28, + "velocityY": 2.3969568427667223e-26, + "timestamp": 1.8927739536112596 + }, + { + "x": 2.3717259824669474, + "y": 4.107728459307588, + "heading": -1.1672418235553326e-8, + "angularVelocity": -1.35062850158138e-7, + "velocityX": -0.46187630200786284, + "velocityY": 0.052681177557626406, + "timestamp": 1.9791960765109815 + }, + { + "x": 2.2918933238426775, + "y": 4.116834097431042, + "heading": -3.822420718940779e-8, + "angularVelocity": -3.0723370432202035e-7, + "velocityX": -0.9237525756790543, + "velocityY": 0.10536235188320037, + "timestamp": 2.0656181994107032 + }, + { + "x": 2.172144344477514, + "y": 4.130492553638595, + "heading": -8.92762742154659e-8, + "angularVelocity": -5.907291479670698e-7, + "velocityX": -1.3856287643397973, + "velocityY": 0.1580435165125675, + "timestamp": 2.152040322310425 + }, + { + "x": 2.051932793607985, + "y": 4.144203770317826, + "heading": -0.046276400034424556, + "angularVelocity": -0.5354683408072037, + "velocityX": -1.3909812306857334, + "velocityY": 0.15865401380084698, + "timestamp": 2.2384624452101467 + }, + { + "x": 1.9317212427384578, + "y": 4.157914986997074, + "heading": -0.1295981296882628, + "angularVelocity": -0.9641250047805334, + "velocityX": -1.3909812306857108, + "velocityY": 0.1586540138010495, + "timestamp": 2.3248845681098684 + }, + { + "x": 1.8115096918689613, + "y": 4.171626203676588, + "heading": -0.24101062814613916, + "angularVelocity": -1.289166416186646, + "velocityX": -1.3909812306853604, + "velocityY": 0.15865401380412056, + "timestamp": 2.41130669100959 + }, + { + "x": 1.691298140999544, + "y": 4.185337420356798, + "heading": -0.371998449853314, + "angularVelocity": -1.5156746595910844, + "velocityX": -1.3909812306844416, + "velocityY": 0.15865401381217495, + "timestamp": 2.497728813909312 + }, + { + "x": 1.5710865901302022, + "y": 4.199048637037669, + "heading": -0.5145015619975004, + "angularVelocity": -1.6489193665091622, + "velocityX": -1.3909812306835692, + "velocityY": 0.15865401381982447, + "timestamp": 2.5841509368090336 + }, + { + "x": 1.4508750392608447, + "y": 4.212759853718402, + "heading": -0.6607983544001016, + "angularVelocity": -1.6928164628905682, + "velocityX": -1.3909812306837512, + "velocityY": 0.15865401381822844, + "timestamp": 2.6705730597087554 + }, + { + "x": 1.3306634883913862, + "y": 4.22647107039825, + "heading": -0.8032937885994436, + "angularVelocity": -1.648830524155066, + "velocityX": -1.3909812306849194, + "velocityY": 0.15865401380798513, + "timestamp": 2.756995182608477 + }, + { + "x": 1.2104519375218399, + "y": 4.240182287077329, + "heading": -0.9342710439721007, + "angularVelocity": -1.5155523953586953, + "velocityX": -1.3909812306859348, + "velocityY": 0.15865401379908228, + "timestamp": 2.843417305508199 + }, + { + "x": 1.090240386652276, + "y": 4.253893503756253, + "heading": -1.0456752666575715, + "angularVelocity": -1.2890706563033194, + "velocityX": -1.3909812306861384, + "velocityY": 0.15865401379729932, + "timestamp": 2.9298394284079206 + }, + { + "x": 0.9700288357827326, + "y": 4.267604720435357, + "heading": -1.1289928747720797, + "angularVelocity": -0.964077313990357, + "velocityX": -1.3909812306859015, + "velocityY": 0.15865401379937677, + "timestamp": 3.0162615513076423 + }, + { + "x": 0.849817284913202, + "y": 4.281315937114572, + "heading": -1.1752680781537357, + "angularVelocity": -0.5354555272305733, + "velocityX": -1.3909812306857543, + "velocityY": 0.15865401380066316, + "timestamp": 3.102683674207364 + }, + { + "x": 0.7300683055480387, + "y": 4.294974393322129, + "heading": -1.1752681292079605, + "angularVelocity": -5.907541149820297e-7, + "velocityX": -1.385628764339792, + "velocityY": 0.15804351651261175, + "timestamp": 3.1891057971070857 + }, + { + "x": 0.650235646923769, + "y": 4.304080031445585, + "heading": -1.1752681557613822, + "angularVelocity": -3.072525969073027e-7, + "velocityX": -0.9237525756790519, + "velocityY": 0.10536235188322049, + "timestamp": 3.2755279200068075 + }, + { + "x": 0.6103193163871765, + "y": 4.308632850646973, + "heading": -1.1752681674346586, + "angularVelocity": -1.350727780899982e-7, + "velocityX": -0.46187630200786184, + "velocityY": 0.05268117755763445, + "timestamp": 3.361950042906529 + }, + { + "x": 0.6103193163871765, + "y": 4.308632850646973, + "heading": -1.1752681674346586, + "angularVelocity": -1.0539576719763061e-24, + "velocityX": -1.6445678469636073e-24, + "velocityY": 1.6880484103936767e-24, + "timestamp": 3.448372165806251 + }, + { + "x": 0.6301200506356015, + "y": 4.288467589301025, + "heading": -1.1752681571276171, + "angularVelocity": 1.421971563292891e-7, + "velocityX": 0.27317326110924534, + "velocityY": -0.2782023198675527, + "timestamp": 3.520856325375977 + }, + { + "x": 0.6697215183493939, + "y": 4.248137067408285, + "heading": -1.1752681353347625, + "angularVelocity": 3.006567885879182e-7, + "velocityX": 0.546346511415333, + "velocityY": -0.5564046287098577, + "timestamp": 3.5933404849457036 + }, + { + "x": 0.7291237182234573, + "y": 4.187641286300679, + "heading": -1.175268100091475, + "angularVelocity": 4.862205422910537e-7, + "velocityX": 0.8195197437161622, + "velocityY": -0.8346069191767541, + "timestamp": 3.66582464451543 + }, + { + "x": 0.8083266476476012, + "y": 4.106980248642059, + "heading": -1.1752680474685175, + "angularVelocity": 7.259925178088886e-7, + "velocityX": 1.0926929400064915, + "velocityY": -1.1128091728928513, + "timestamp": 3.7383088040851566 + }, + { + "x": 0.9073302987912635, + "y": 4.006153962423965, + "heading": -1.1752679656781906, + "angularVelocity": 0.000001128388979975281, + "velocityX": 1.3658660282654653, + "velocityY": -1.3910113163566968, + "timestamp": 3.810792963654883 + }, + { + "x": 1.0190560711374494, + "y": 3.8923713433209195, + "heading": -1.1633695164980866, + "angularVelocity": 0.1641524058599062, + "velocityX": 1.5413819103291277, + "velocityY": -1.5697584103681614, + "timestamp": 3.8832771232246097 + }, + { + "x": 1.1307818435783537, + "y": 3.7785887243108656, + "heading": -1.1293524727567825, + "angularVelocity": 0.46930314075837803, + "velocityX": 1.5413819116358685, + "velocityY": -1.5697584090852401, + "timestamp": 3.9557612827943363 + }, + { + "x": 1.2425076160184954, + "y": 3.6648061053000625, + "heading": -1.0777990352762379, + "angularVelocity": 0.7112372935903718, + "velocityX": 1.5413819116253513, + "velocityY": -1.5697584090955665, + "timestamp": 4.028245442364063 + }, + { + "x": 1.3542333884574531, + "y": 3.5510234862880976, + "heading": -1.0126066866684242, + "angularVelocity": 0.8994013174023452, + "velocityX": 1.5413819116090188, + "velocityY": -1.5697584091116035, + "timestamp": 4.100729601933789 + }, + { + "x": 1.4659591608957312, + "y": 3.437240867275465, + "heading": -0.9370316872968693, + "angularVelocity": 1.0426415898339216, + "velocityX": 1.5413819115996374, + "velocityY": -1.5697584091208152, + "timestamp": 4.173213761503516 + }, + { + "x": 1.5776849333349647, + "y": 3.32345824826377, + "heading": -0.8537746458661194, + "angularVelocity": 1.1486239465970567, + "velocityX": 1.5413819116128218, + "velocityY": -1.569758409107869, + "timestamp": 4.245697921073242 + }, + { + "x": 1.6894107057772814, + "y": 3.209675629255103, + "heading": -0.7650856064117465, + "angularVelocity": 1.2235644309162248, + "velocityX": 1.541381911655357, + "velocityY": -1.569758409066102, + "timestamp": 4.318182080642969 + }, + { + "x": 1.8011364782241641, + "y": 3.095893010250919, + "heading": -0.6728754035736392, + "angularVelocity": 1.2721428155541408, + "velocityX": 1.5413819117183474, + "velocityY": -1.5697584090042507, + "timestamp": 4.3906662402126955 + }, + { + "x": 1.912862250675531, + "y": 2.9821103912511386, + "heading": -0.5788183350553536, + "angularVelocity": 1.2976223919352636, + "velocityX": 1.5413819117802157, + "velocityY": -1.5697584089435004, + "timestamp": 4.463150399782422 + }, + { + "x": 2.0245880231296054, + "y": 2.8683277722540166, + "heading": -0.4844571570147463, + "angularVelocity": 1.3018179227122904, + "velocityX": 1.541381911817565, + "velocityY": -1.5697584089068264, + "timestamp": 4.5356345593521485 + }, + { + "x": 2.136313795583653, + "y": 2.7545451532568683, + "heading": -0.39129970033528816, + "angularVelocity": 1.2852112410828893, + "velocityX": 1.5413819118172043, + "velocityY": -1.5697584089071805, + "timestamp": 4.608118718921875 + }, + { + "x": 2.2480395680351752, + "y": 2.6407625342572403, + "heading": -0.3009250355302803, + "angularVelocity": 1.2468195167258806, + "velocityX": 1.5413819117823546, + "velocityY": -1.5697584089414005, + "timestamp": 4.680602878491602 + }, + { + "x": 2.3597653404829035, + "y": 2.5269799152538868, + "heading": -0.21509325739484195, + "angularVelocity": 1.1841453173347793, + "velocityX": 1.5413819117300178, + "velocityY": -1.5697584089927912, + "timestamp": 4.753087038061328 + }, + { + "x": 2.471491112929133, + "y": 2.413197296249062, + "heading": -0.13585736651971517, + "angularVelocity": 1.0931476800652649, + "velocityX": 1.5413819117093417, + "velocityY": -1.5697584090130938, + "timestamp": 4.825571197631055 + }, + { + "x": 2.5832171123561207, + "y": 2.299414900122313, + "heading": -0.06567656811151243, + "angularVelocity": 0.9682225582086267, + "velocityX": 1.541385043162603, + "velocityY": -1.5697553341609145, + "timestamp": 4.898055357200781 + }, + { + "x": 2.707773208618164, + "y": 2.199841260910034, + "heading": 8.190882767180045e-23, + "angularVelocity": 0.9060816666893224, + "velocityX": 1.7183905697661621, + "velocityY": -1.3737296507727865, + "timestamp": 4.970539516770508 + }, + { + "x": 2.9476739245436137, + "y": 2.102450103083525, + "heading": 0.10663557803360654, + "angularVelocity": 0.9060793794235005, + "velocityX": 2.03842934804067, + "velocityY": -0.8275298120198638, + "timestamp": 5.08822851892113 + }, + { + "x": 3.19386895017516, + "y": 2.0222915070712038, + "heading": 0.17351190219088491, + "angularVelocity": 0.5682461651912727, + "velocityX": 2.091911913030402, + "velocityY": -0.6811052396359981, + "timestamp": 5.205917521071752 + }, + { + "x": 3.4400639885616346, + "y": 1.9421329502337281, + "heading": 0.209115544808741, + "angularVelocity": 0.30252310723383896, + "velocityX": 2.0919120214086484, + "velocityY": -0.6811049067684878, + "timestamp": 5.323606523222375 + }, + { + "x": 3.6862590269481257, + "y": 1.861974393396304, + "heading": 0.2231570731239334, + "angularVelocity": 0.11931045432114051, + "velocityX": 2.091912021408791, + "velocityY": -0.6811049067680496, + "timestamp": 5.441295525372997 + }, + { + "x": 3.9324540653346163, + "y": 1.781815836558879, + "heading": 0.2223654503026519, + "angularVelocity": -0.006726395897794665, + "velocityX": 2.0919120214087887, + "velocityY": -0.6811049067680567, + "timestamp": 5.558984527523619 + }, + { + "x": 4.178649103721107, + "y": 1.7016572797214544, + "heading": 0.21145396988639198, + "angularVelocity": -0.09271452911373225, + "velocityX": 2.0919120214087896, + "velocityY": -0.6811049067680542, + "timestamp": 5.676673529674241 + }, + { + "x": 4.424844142107599, + "y": 1.6214987228840316, + "heading": 0.1937295052437245, + "angularVelocity": -0.15060425629221624, + "velocityX": 2.091912021408795, + "velocityY": -0.6811049067680375, + "timestamp": 5.794362531824864 + }, + { + "x": 4.671039180494092, + "y": 1.5413401660466126, + "heading": 0.1715372470156766, + "angularVelocity": -0.18856696736748246, + "velocityX": 2.0919120214088047, + "velocityY": -0.6811049067680072, + "timestamp": 5.912051533975486 + }, + { + "x": 4.917234218880585, + "y": 1.4611816092091974, + "heading": 0.14658987046095176, + "angularVelocity": -0.21197712699438517, + "velocityX": 2.0919120214088154, + "velocityY": -0.681104906767975, + "timestamp": 6.029740536126108 + }, + { + "x": 5.16342925726708, + "y": 1.3810230523717848, + "heading": 0.12020702939394358, + "angularVelocity": -0.22417422685972488, + "velocityX": 2.091912021408823, + "velocityY": -0.6811049067679519, + "timestamp": 6.1474295382767306 + }, + { + "x": 5.409624295653576, + "y": 1.3008644955343727, + "heading": 0.0935025745426398, + "angularVelocity": -0.22690696975344068, + "velocityX": 2.091912021408825, + "velocityY": -0.6811049067679456, + "timestamp": 6.265118540427353 + }, + { + "x": 5.65581933404007, + "y": 1.2207059386969594, + "heading": 0.06754361372222109, + "angularVelocity": -0.2205725288349003, + "velocityX": 2.091912021408821, + "velocityY": -0.6811049067679572, + "timestamp": 6.382807542577975 + }, + { + "x": 5.902014372426565, + "y": 1.140547381859543, + "heading": 0.04350373821186817, + "angularVelocity": -0.20426611723316299, + "velocityX": 2.0919120214088127, + "velocityY": -0.6811049067679843, + "timestamp": 6.500496544728597 + }, + { + "x": 6.148209410813057, + "y": 1.0603888250221227, + "heading": 0.022824509740632836, + "angularVelocity": -0.1757107978940074, + "velocityX": 2.0919120214088016, + "velocityY": -0.6811049067680177, + "timestamp": 6.61818554687922 + }, + { + "x": 6.3944044491995475, + "y": 0.9802302681846989, + "heading": 0.0074310588512180266, + "angularVelocity": -0.13079770078867528, + "velocityX": 2.0919120214087927, + "velocityY": -0.681104906768045, + "timestamp": 6.735874549029842 + }, + { + "x": 6.640599487586059, + "y": 0.9000717113473415, + "heading": 1.8581771247773188e-8, + "angularVelocity": -0.06314133125146484, + "velocityX": 2.091912021408959, + "velocityY": -0.681104906767483, + "timestamp": 6.853563551180464 + }, + { + "x": 6.853129975330638, + "y": 0.8308739838333334, + "heading": 8.159371436393743e-9, + "angularVelocity": -8.855882555653375e-8, + "velocityX": 1.8058653218299596, + "velocityY": -0.5879710614373866, + "timestamp": 6.9712525533310865 + }, + { + "x": 6.9948169750991465, + "y": 0.7847421629058822, + "heading": 2.531204760537721e-9, + "angularVelocity": -4.7822367196664074e-8, + "velocityX": 1.2039102820089622, + "velocityY": -0.3919807295877177, + "timestamp": 7.088941555481709 + }, + { + "x": 7.06566047668457, + "y": 0.7616762518882751, + "heading": -1.9444191856833368e-23, + "angularVelocity": -2.1507572621766622e-8, + "velocityX": 0.6019551554592679, + "velocityY": -0.19599036950017273, + "timestamp": 7.206630557632331 + }, + { + "x": 7.06566047668457, + "y": 0.7616762518882751, + "heading": -9.311643766094953e-24, + "angularVelocity": 6.975199973230743e-24, + "velocityX": 3.42127778712228e-24, + "velocityY": -1.1784054582051222e-24, + "timestamp": 7.324319559782953 } ], "trajectoryWaypoints": [ @@ -1679,8 +1922,8 @@ "controlIntervalCount": 17 }, { - "timestamp": 0.838047277830319, - "isStopPoint": false, + "timestamp": 1.1560891382578093, + "isStopPoint": true, "x": 1.8715893030166626, "y": 4.094946384429932, "heading": 0, @@ -1690,7 +1933,7 @@ "controlIntervalCount": 7 }, { - "timestamp": 1.3433199403544034, + "timestamp": 1.8927739536112596, "isStopPoint": true, "x": 2.41164231300354, "y": 4.103175640106201, @@ -1701,21 +1944,32 @@ "controlIntervalCount": 18 }, { - "timestamp": 2.6747040904629253, + "timestamp": 3.448372165806251, "isStopPoint": true, - "x": 0.6313003897666931, - "y": 4.255156517028809, - "heading": -1.2897, + "x": 0.6103193163871765, + "y": 4.308632850646973, + "heading": -1.1752681674346586, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 22 + "controlIntervalCount": 21 }, { - "timestamp": 4.69416889200488, + "timestamp": 4.970539516770508, + "isStopPoint": false, + "x": 2.707773208618164, + "y": 2.199841260910034, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "timestamp": 7.324319559782953, "isStopPoint": true, - "x": 2.8538730144500732, - "y": 2.2070720195770264, + "x": 7.06566047668457, + "y": 0.7616762518882751, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -1756,23 +2010,47 @@ }, { "scope": [ - 4 + "first", + 1 ], - "type": "StopPoint" + "type": "MaxVelocity", + "velocity": 1.4 }, { "scope": [ - "first", + 2 + ], + "type": "WptZeroVelocity" + }, + { + "scope": [ + 1, + 2 + ], + "type": "MaxVelocity", + "velocity": 1 + }, + { + "scope": [ + 3, "last" ], "type": "MaxVelocity", - "velocity": 1.8 + "velocity": 2.2 }, { "scope": [ - 2 + 1 ], - "type": "WptZeroVelocity" + "type": "StopPoint" + }, + { + "scope": [ + 2, + 3 + ], + "type": "MaxVelocity", + "velocity": 1.4 } ], "usesControlIntervalGuessing": true, @@ -1791,11 +2069,20 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 13 + }, + { + "x": 3.1499392986297607, + "y": 6.247360706329346, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 }, { - "x": 4.08034610748291, - "y": 5.723013877868652, + "x": 6.993381977081299, + "y": 6.5308003425598145, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -1807,146 +2094,290 @@ { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": -8.351489873798432e-33, - "angularVelocity": -9.52467798814217e-33, - "velocityX": 5.856461918837007e-19, - "velocityY": 3.2988456083555626e-20, + "heading": 2.4064931720572804e-32, + "angularVelocity": 5.341037702942834e-32, + "velocityX": -1.8920825597869374e-21, + "velocityY": 5.007478148860853e-21, "timestamp": 0 }, { - "x": 1.3974808893772463, - "y": 5.571866984328364, - "heading": -4.619528479929238e-22, - "angularVelocity": -3.675562971006093e-21, - "velocityX": 0.6749835681204585, - "velocityY": 0.03802713189750964, - "timestamp": 0.12568221137930363 - }, - { - "x": 1.5671477356384225, - "y": 5.581425651896191, - "heading": -1.730475779700296e-21, - "angularVelocity": -1.009309860545018e-20, - "velocityX": 1.3499670669315913, - "velocityY": 0.07605425989027997, - "timestamp": 0.25136442275860726 - }, - { - "x": 1.7930175454481574, - "y": 5.594150674333725, - "heading": -1.411814827850765e-19, - "angularVelocity": -1.109552445453708e-18, - "velocityX": 1.797150188009124, - "velocityY": 0.10124760137399545, - "timestamp": 0.3770466341379109 - }, - { - "x": 2.018887355257855, - "y": 5.606875696771257, - "heading": 6.884754478081912e-20, - "angularVelocity": 1.671111824496415e-18, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399623, - "timestamp": 0.5027288455172145 - }, - { - "x": 2.2447571650676026, - "y": 5.619600719208795, - "heading": 2.509060822170839e-19, - "angularVelocity": 1.4485625091677524e-18, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399623, - "timestamp": 0.6284110568965182 - }, - { - "x": 2.470626974877369, - "y": 5.632325741646326, - "heading": 2.991709519339792e-19, - "angularVelocity": 3.84023092969776e-19, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399623, - "timestamp": 0.7540932682758219 - }, - { - "x": 2.6964967846870462, - "y": 5.645050764083861, - "heading": 3.625534697671079e-19, - "angularVelocity": 5.043077983998515e-19, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399622, - "timestamp": 0.8797754796551256 - }, - { - "x": 2.9223665944967436, - "y": 5.657775786521395, - "heading": 7.427083243993144e-19, - "angularVelocity": 3.0247307984331e-18, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399622, - "timestamp": 1.0054576910344293 - }, - { - "x": 3.1482364043064752, - "y": 5.6705008089589315, - "heading": 7.718223822590269e-19, - "angularVelocity": 2.3164819703884333e-19, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399624, - "timestamp": 1.131139902413733 - }, - { - "x": 3.3741062141162126, - "y": 5.683225831396466, - "heading": 6.75954597848925e-19, - "angularVelocity": -7.627792740852862e-19, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399623, - "timestamp": 1.2568221137930367 - }, - { - "x": 3.5999760239259384, - "y": 5.695950853833998, - "heading": 5.558602175529875e-19, - "angularVelocity": -9.555400262857742e-19, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399623, - "timestamp": 1.3825043251723403 - }, - { - "x": 3.8258458337356256, - "y": 5.708675876271534, - "heading": 1.467111215234519e-21, - "angularVelocity": -4.4110706322056545e-18, - "velocityX": 1.797150188009124, - "velocityY": 0.10124760137399544, - "timestamp": 1.508186536551644 - }, - { - "x": 3.99551267999683, - "y": 5.718234543839361, - "heading": 3.9980033222945746e-22, - "angularVelocity": -8.492139436872755e-21, - "velocityX": 1.3499670669315909, - "velocityY": 0.07605425989027999, - "timestamp": 1.6338687479309477 - }, - { - "x": 4.08034610748291, - "y": 5.723013877868652, - "heading": 2.0668023676312887e-32, - "angularVelocity": -3.1810414607342227e-21, - "velocityX": 0.6749835681204581, - "velocityY": 0.03802713189750965, - "timestamp": 1.7595509593102514 - }, - { - "x": 4.08034610748291, - "y": 5.723013877868652, - "heading": 9.734730637251588e-33, - "angularVelocity": -9.536452132221093e-33, - "velocityX": 4.231540995901582e-19, - "velocityY": 2.3828735253686786e-20, - "timestamp": 1.885233170689555 + "x": 1.3458512299446481, + "y": 5.579396056710049, + "heading": 3.413187474556536e-18, + "angularVelocity": 4.206691136177212e-17, + "velocityX": 0.409230368386038, + "velocityY": 0.15169885784339893, + "timestamp": 0.08113710667276726 + }, + { + "x": 1.412258764942075, + "y": 5.6040128691207105, + "heading": 6.827483140754657e-18, + "angularVelocity": 4.208056962106495e-17, + "velocityX": 0.8184607230974358, + "velocityY": 0.3033977106177016, + "timestamp": 0.16227421334553452 + }, + { + "x": 1.511870065034253, + "y": 5.64093808684557, + "heading": 7.957629289102601e-18, + "angularVelocity": 1.3928844577901407e-17, + "velocityX": 1.2276910550177553, + "velocityY": 0.45509655494350654, + "timestamp": 0.2434113200183018 + }, + { + "x": 1.644685126522774, + "y": 5.690171708513654, + "heading": 1.0892134038406229e-17, + "angularVelocity": 3.6167234322702574e-17, + "velocityX": 1.6369213413558763, + "velocityY": 0.6067953823723014, + "timestamp": 0.32454842669106904 + }, + { + "x": 1.810703938312389, + "y": 5.75171373001203, + "heading": 1.3242101695542206e-17, + "angularVelocity": 2.896292157187237e-17, + "velocityX": 2.0461514909470804, + "velocityY": 0.7584941591099562, + "timestamp": 0.4056855333638363 + }, + { + "x": 1.9780760222264302, + "y": 5.813757400096333, + "heading": 1.125270337500381e-16, + "angularVelocity": 1.22366862864508e-15, + "velocityX": 2.062830322371079, + "velocityY": 0.7646768861814419, + "timestamp": 0.4868226400366035 + }, + { + "x": 2.145448106140464, + "y": 5.875801070180656, + "heading": 2.6117964956039346e-16, + "angularVelocity": 1.832116301729673e-15, + "velocityX": 2.0628303223709894, + "velocityY": 0.7646768861816947, + "timestamp": 0.5679597467093708 + }, + { + "x": 2.312820190054498, + "y": 5.937844740264981, + "heading": 3.764540638551346e-16, + "angularVelocity": 1.4207360728261472e-15, + "velocityX": 2.0628303223709845, + "velocityY": 0.7646768861817085, + "timestamp": 0.6490968533821381 + }, + { + "x": 2.4801922739685316, + "y": 5.999888410349303, + "heading": 4.151881500968984e-16, + "angularVelocity": 4.773905285775699e-16, + "velocityX": 2.0628303223709925, + "velocityY": 0.7646768861816863, + "timestamp": 0.7302339600549054 + }, + { + "x": 2.6475643578824943, + "y": 6.061932080433819, + "heading": 3.881070204693485e-16, + "angularVelocity": -3.337699695993216e-16, + "velocityX": 2.06283032237011, + "velocityY": 0.7646768861840669, + "timestamp": 0.8113710667276727 + }, + { + "x": 2.8149364417978595, + "y": 6.123975750514552, + "heading": 2.850633506870914e-16, + "angularVelocity": -1.2699943836774666e-15, + "velocityX": 2.0628303223873994, + "velocityY": 0.7646768861374271, + "timestamp": 0.89250817340044 + }, + { + "x": 2.982308527462817, + "y": 6.186019415875497, + "heading": 1.5426652083783285e-16, + "angularVelocity": -1.6120470055306626e-15, + "velocityX": 2.062830343950803, + "velocityY": 0.7646768279669163, + "timestamp": 0.9736452800732073 + }, + { + "x": 3.1499392986297607, + "y": 6.247360706329346, + "heading": -2.6221041253671193e-33, + "angularVelocity": -1.901306654426338e-15, + "velocityX": 2.0660185954499513, + "velocityY": 0.7560201856992955, + "timestamp": 1.0547823867459745 + }, + { + "x": 3.395592203319553, + "y": 6.266105388578719, + "heading": -2.25016120284868e-16, + "angularVelocity": -2.0093413357633876e-15, + "velocityX": 2.193622994737605, + "velocityY": 0.16738562917950292, + "timestamp": 1.1667674020740877 + }, + { + "x": 3.6412951619585874, + "y": 6.284182136410596, + "heading": -5.420070367092222e-16, + "angularVelocity": -2.830654757652916e-15, + "velocityX": 2.1940699647995836, + "velocityY": 0.16142113102287564, + "timestamp": 1.278752417402201 + }, + { + "x": 3.8869981207102944, + "y": 6.302258882711007, + "heading": -1.2457261466691004e-15, + "angularVelocity": -6.28404709235429e-15, + "velocityX": 2.1940699658057228, + "velocityY": 0.1614211173472237, + "timestamp": 1.3907374327303141 + }, + { + "x": 4.132701079462025, + "y": 6.320335629011082, + "heading": -2.1563980313879527e-15, + "angularVelocity": -8.132086976553131e-15, + "velocityX": 2.194069965805942, + "velocityY": 0.16142111734423847, + "timestamp": 1.5027224480584274 + }, + { + "x": 4.378404038213756, + "y": 6.338412375311165, + "heading": -3.277495117328277e-15, + "angularVelocity": -1.0011134817060459e-14, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734430015, + "timestamp": 1.6147074633865406 + }, + { + "x": 4.624106996965487, + "y": 6.356489121611248, + "heading": -4.357861685209099e-15, + "angularVelocity": -9.647420815324042e-15, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429992, + "timestamp": 1.7266924787146538 + }, + { + "x": 4.869809955717218, + "y": 6.374565867911331, + "heading": -4.984271791839074e-15, + "angularVelocity": -5.593695770765471e-15, + "velocityX": 2.1940699658059377, + "velocityY": 0.1614211173442999, + "timestamp": 1.838677494042767 + }, + { + "x": 5.115512914468949, + "y": 6.392642614211413, + "heading": -5.0340664077739056e-15, + "angularVelocity": -4.446542761898409e-16, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429992, + "timestamp": 1.9506625093708803 + }, + { + "x": 5.36121587322068, + "y": 6.410719360511496, + "heading": -4.508040648188578e-15, + "angularVelocity": 4.6972870258050625e-15, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429992, + "timestamp": 2.0626475246989937 + }, + { + "x": 5.606918831972411, + "y": 6.428796106811578, + "heading": -3.510023344735256e-15, + "angularVelocity": 8.91206114076206e-15, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429992, + "timestamp": 2.174632540027107 + }, + { + "x": 5.852621790724142, + "y": 6.446872853111661, + "heading": -2.3215831189941424e-15, + "angularVelocity": 1.0612493307778848e-14, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429995, + "timestamp": 2.28661755535522 + }, + { + "x": 6.098324749475873, + "y": 6.464949599411743, + "heading": -1.250017912777245e-15, + "angularVelocity": 9.568826713799511e-15, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429923, + "timestamp": 2.3986025706833334 + }, + { + "x": 6.344027708227604, + "y": 6.48302634571183, + "heading": -4.511696357394538e-16, + "angularVelocity": 7.133528309097315e-15, + "velocityX": 2.194069965805935, + "velocityY": 0.16142111734433204, + "timestamp": 2.5105875860114466 + }, + { + "x": 6.589730666979331, + "y": 6.501103092011914, + "heading": -4.711917488169644e-19, + "angularVelocity": 4.024631712288509e-15, + "velocityX": 2.1940699658059026, + "velocityY": 0.16142111734431822, + "timestamp": 2.62257260133956 + }, + { + "x": 6.791556314990098, + "y": 6.515951716767905, + "heading": 1.0824486990110142e-21, + "angularVelocity": 4.2172981459668045e-18, + "velocityX": 1.8022558412786114, + "velocityY": 0.13259474682827085, + "timestamp": 2.734557616667673 + }, + { + "x": 6.926106755210862, + "y": 6.525850800542852, + "heading": 2.72309248565345e-22, + "angularVelocity": -7.234355816903654e-21, + "velocityX": 1.2015039675311439, + "velocityY": 0.08839650328164239, + "timestamp": 2.8465426319957863 + }, + { + "x": 6.993381977081299, + "y": 6.5308003425598145, + "heading": -3.4836420737679767e-34, + "angularVelocity": -2.4316579121587528e-21, + "velocityX": 0.6007519994824513, + "velocityY": 0.04419825279713492, + "timestamp": 2.9585276473238995 + }, + { + "x": 6.993381977081299, + "y": 6.5308003425598145, + "heading": -6.944682501042333e-34, + "angularVelocity": -9.29206724482002e-33, + "velocityX": -5.0941360422492845e-22, + "velocityY": -3.0837835472219657e-21, + "timestamp": 3.0705126626520127 } ], "trajectoryWaypoints": [ @@ -1959,13 +2390,24 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 13 }, { - "timestamp": 1.885233170689555, + "timestamp": 1.0547823867459745, + "isStopPoint": false, + "x": 3.1499392986297607, + "y": 6.247360706329346, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "timestamp": 3.0705126626520127, "isStopPoint": true, - "x": 4.08034610748291, - "y": 5.723013877868652, + "x": 6.993381977081299, + "y": 6.5308003425598145, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -1992,7 +2434,7 @@ "last" ], "type": "MaxVelocity", - "velocity": 1.8 + "velocity": 2.2 } ], "usesControlIntervalGuessing": true, @@ -2277,30 +2719,7 @@ "timestamp": 1.6910691790334251 } ], - "trajectoryWaypoints": [ - { - "timestamp": 0, - "isStopPoint": true, - "x": 0.7669039964675903, - "y": 6.639083385467529, - "heading": 1.0074798594118668, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 27 - }, - { - "timestamp": 1.6910691790334251, - "isStopPoint": true, - "x": 2.832932472229004, - "y": 7.730569839477539, - "heading": -1.57, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], + "trajectoryWaypoints": [], "constraints": [ { "scope": [ @@ -3247,107 +3666,7 @@ "timestamp": 8.407522891124284 } ], - "trajectoryWaypoints": [ - { - "timestamp": 0, - "isStopPoint": true, - "x": 1.3126474618911743, - "y": 5.567087650299072, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 - }, - { - "timestamp": 1.0961836295771141, - "isStopPoint": true, - "x": 2.349189281463623, - "y": 5.567087650299072, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 - }, - { - "timestamp": 2.1923672591542283, - "isStopPoint": true, - "x": 1.3126474618911743, - "y": 5.567087650299072, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 - }, - { - "timestamp": 3.0618066834125037, - "isStopPoint": false, - "x": 2.1126484870910645, - "y": 6.068657398223877, - "heading": 0.9505466984245466, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 8 - }, - { - "timestamp": 3.7729373982230903, - "isStopPoint": true, - "x": 2.583202600479126, - "y": 6.5326762199401855, - "heading": 0.9505466984245466, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 - }, - { - "timestamp": 5.334595574565935, - "isStopPoint": true, - "x": 1.3126474618911743, - "y": 5.567087650299072, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 - }, - { - "timestamp": 6.436871792912134, - "isStopPoint": false, - "x": 2.2034590244293213, - "y": 4.713430881500244, - "heading": -0.7298997067825479, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 5 - }, - { - "timestamp": 6.8730900190962805, - "isStopPoint": true, - "x": 2.4599039554595947, - "y": 4.502241134643555, - "heading": -0.7298997067825479, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 15 - }, - { - "timestamp": 8.407522891124284, - "isStopPoint": true, - "x": 1.3126474618911743, - "y": 5.567087650299072, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], + "trajectoryWaypoints": [], "constraints": [ { "scope": [ @@ -3850,41 +4169,7 @@ "timestamp": 2.5804057736327106 } ], - "trajectoryWaypoints": [ - { - "timestamp": 0, - "isStopPoint": true, - "x": 0.6235248446464539, - "y": 6.866034507751465, - "heading": 1.3037, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 20 - }, - { - "timestamp": 0.9753230879456769, - "isStopPoint": false, - "x": 2.791773796081543, - "y": 6.312439918518066, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 20 - }, - { - "timestamp": 2.5804057736327106, - "isStopPoint": true, - "x": 7.272015571594238, - "y": 6.342609882354736, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], + "trajectoryWaypoints": [], "constraints": [ { "scope": [ @@ -3897,6 +4182,14 @@ "last" ], "type": "StopPoint" + }, + { + "scope": [ + "first", + "last" + ], + "type": "MaxVelocity", + "velocity": 2.3 } ], "usesControlIntervalGuessing": true, diff --git a/src/main/deploy/choreo/2 piece left.1.traj b/src/main/deploy/choreo/2 piece left.1.traj index 96a144b..cfb83b0 100644 --- a/src/main/deploy/choreo/2 piece left.1.traj +++ b/src/main/deploy/choreo/2 piece left.1.traj @@ -4,217 +4,154 @@ "x": 0.6235248446464539, "y": 6.866034507751465, "heading": 1.3037, - "angularVelocity": 2.3321158035498043e-24, - "velocityX": -1.6085262930190747e-26, - "velocityY": -4.0384459981982936e-25, + "angularVelocity": 2.7068477576187765e-23, + "velocityX": -4.853948155553872e-23, + "velocityY": -6.243185313363585e-23, "timestamp": 0 }, { - "x": 0.6371648015347651, - "y": 6.867786445253041, - "heading": 1.3036999632733615, - "angularVelocity": -7.263599506268419e-7, - "velocityX": 0.26976382275301153, - "velocityY": 0.03464888940043332, - "timestamp": 0.050562587485274306 - }, - { - "x": 0.6644447150656874, - "y": 6.871290320309862, - "heading": 1.303699886456694, - "angularVelocity": -0.0000015192392509530055, - "velocityX": 0.5395276406466987, - "velocityY": 0.0692977798622562, - "timestamp": 0.10112517497054861 - }, - { - "x": 0.7053645848706707, - "y": 6.876546133002426, - "heading": 1.3036997645049127, - "angularVelocity": -0.0000024118975620600877, - "velocityX": 0.809291451251395, - "velocityY": 0.10394667191616248, - "timestamp": 0.1516877624558229 - }, - { - "x": 0.759924410335464, - "y": 6.8835538834649, - "heading": 1.303699589009544, - "angularVelocity": -0.0000034708541873039917, - "velocityX": 1.0790552497077626, - "velocityY": 0.13859556662353859, - "timestamp": 0.20225034994109722 - }, - { - "x": 0.8281241902315634, - "y": 6.892313571965616, - "heading": 1.3036993431536497, - "angularVelocity": -0.000004862407296943842, - "velocityX": 1.3488190238674322, - "velocityY": 0.17324446663784457, - "timestamp": 0.2528129374263715 - }, - { - "x": 0.909963920873441, - "y": 6.902825199309566, - "heading": 1.3036989764864741, - "angularVelocity": -0.000007251748650640463, - "velocityX": 1.61858272513669, - "velocityY": 0.20789338257286122, - "timestamp": 0.3033755249116458 - }, - { - "x": 0.9952199524201776, - "y": 6.913775599360361, - "heading": 1.2650877104567244, - "angularVelocity": -0.763633111952482, - "velocityX": 1.6861485099346676, - "velocityY": 0.21657119612368925, - "timestamp": 0.35393811239692013 - }, - { - "x": 1.08047598194801, - "y": 6.924726015129663, - "heading": 1.1860996824058685, - "angularVelocity": -1.5621832659149426, - "velocityX": 1.6861484700058589, - "velocityY": 0.21657150699596817, - "timestamp": 0.40450069988219445 - }, - { - "x": 1.165732011476475, - "y": 6.93567643089404, - "heading": 1.068522180672941, - "angularVelocity": -2.325385380389609, - "velocityX": 1.6861484700183644, - "velocityY": 0.21657150689860336, - "timestamp": 0.45506328736746876 - }, - { - "x": 1.25098804100406, - "y": 6.94662684666527, - "heading": 0.9153751201313117, - "angularVelocity": -3.0288612224647617, - "velocityX": 1.6861484700009595, - "velocityY": 0.2165715070341085, - "timestamp": 0.505625874852743 - }, - { - "x": 1.3362440705163487, - "y": 6.957577262555589, - "heading": 0.7326430566977549, - "angularVelocity": -3.6139776961923715, - "velocityX": 1.6861484696984423, - "velocityY": 0.21657150938939215, - "timestamp": 0.5561884623380173 - }, - { - "x": 1.4215001000011822, - "y": 6.968527678659667, - "heading": 0.5352466423996767, - "angularVelocity": -3.904001438920968, - "velocityX": 1.6861484691554405, - "velocityY": 0.21657151361700638, - "timestamp": 0.6067510498232916 - }, - { - "x": 1.5067561295108884, - "y": 6.9794780945700925, - "heading": 0.35194096302396116, - "angularVelocity": -3.6253223676320148, - "velocityX": 1.6861484696473656, - "velocityY": 0.2165715097870587, - "timestamp": 0.657313637308566 - }, - { - "x": 1.5920121590364624, - "y": 6.99042851035698, - "heading": 0.19809518233895146, - "angularVelocity": -3.0426801383496302, - "velocityX": 1.6861484699611848, - "velocityY": 0.21657150734378086, - "timestamp": 0.7078762247938403 - }, - { - "x": 1.6772681885652982, - "y": 7.001378926118472, - "heading": 0.07976876190025861, - "angularVelocity": -2.3401970967793897, - "velocityX": 1.6861484700257006, - "velocityY": 0.21657150684148851, - "timestamp": 0.7584388122791146 + "x": 0.6479605553240734, + "y": 6.869173065177977, + "heading": 1.3036998817532777, + "angularVelocity": -0.0000017472450406683682, + "velocityX": 0.3610685643473286, + "velocityY": 0.04637615983684271, + "timestamp": 0.06767609559638565 + }, + { + "x": 0.6968319743321776, + "y": 6.875450180817172, + "heading": 1.3036996103660266, + "angularVelocity": -0.000004010090249051131, + "velocityX": 0.722137094012766, + "velocityY": 0.09275233129037688, + "timestamp": 0.1353521911927713 + }, + { + "x": 0.7701390946293506, + "y": 6.884865857027548, + "heading": 1.303699081157409, + "angularVelocity": -0.00000781972738083724, + "velocityX": 1.083205519632373, + "velocityY": 0.13912853759369243, + "timestamp": 0.20302828678915696 + }, + { + "x": 0.8641136418785188, + "y": 6.89693604798726, + "heading": 1.2781268289721, + "angularVelocity": -0.3778624041466495, + "velocityX": 1.388592920750396, + "velocityY": 0.17835235400838984, + "timestamp": 0.2707043823855426 + }, + { + "x": 0.958088185941953, + "y": 6.909006263750245, + "heading": 1.1796751937375807, + "angularVelocity": -1.4547475643641785, + "velocityX": 1.3885928736771431, + "velocityY": 0.17835272050816342, + "timestamp": 0.3383804779819283 + }, + { + "x": 1.0520627300139076, + "y": 6.921076479446895, + "heading": 1.0124911373568812, + "angularVelocity": -2.4703561118207933, + "velocityX": 1.3885928738030422, + "velocityY": 0.178352719527955, + "timestamp": 0.40605657357831393 + }, + { + "x": 1.1460372740816214, + "y": 6.933146695176562, + "heading": 0.785208686841268, + "angularVelocity": -3.358385978279628, + "velocityX": 1.3885928737403777, + "velocityY": 0.17835272001583247, + "timestamp": 0.4737326691746996 + }, + { + "x": 1.2400118181361255, + "y": 6.945216911009073, + "heading": 0.5237947450805, + "angularVelocity": -3.862721976749634, + "velocityX": 1.3885928735451927, + "velocityY": 0.1783527215354694, + "timestamp": 0.5414087647710852 + }, + { + "x": 1.3339863622028791, + "y": 6.9572871267462135, + "heading": 0.2954599282674446, + "angularVelocity": -3.3739360227697586, + "velocityX": 1.388592873726193, + "velocityY": 0.17835272012626976, + "timestamp": 0.6090848603674709 + }, + { + "x": 1.4279609062740386, + "y": 6.969357342449054, + "heading": 0.126956229009992, + "angularVelocity": -2.489855506180409, + "velocityX": 1.3885928737912927, + "velocityY": 0.17835271961943208, + "timestamp": 0.6767609559638565 + }, + { + "x": 1.521935450341706, + "y": 6.981427558179083, + "heading": 0.027070659114329158, + "angularVelocity": -1.4759357645478204, + "velocityX": 1.3885928737396922, + "velocityY": 0.1783527200211782, + "timestamp": 0.7444370515602422 + }, + { + "x": 1.6159099957667231, + "y": 6.993497763341015, + "heading": 9.235413386651335e-7, + "angularVelocity": -0.3999896172266207, + "velocityX": 1.388592893796256, + "velocityY": 0.1783525638641785, + "timestamp": 0.8121131471566279 + }, + { + "x": 1.6892171169315986, + "y": 7.002913432795811, + "heading": 3.921850758219532e-7, + "angularVelocity": -0.000007851461556117877, + "velocityX": 1.0832055324537762, + "velocityY": 0.13912843777145287, + "timestamp": 0.8797892427530135 + }, + { + "x": 1.7380885363341119, + "y": 7.00919054536428, + "heading": 1.1913070878580363e-7, + "angularVelocity": -0.000004034723998627552, + "velocityX": 0.7221370998406585, + "velocityY": 0.09275228591651416, + "timestamp": 0.9474653383493992 }, { "x": 1.7625242471694946, "y": 7.0123291015625, - "heading": 8.02748616433338e-24, - "angularVelocity": -1.577624205317622, - "velocityX": 1.686149045062736, - "velocityY": 0.21656675397035893, - "timestamp": 0.8090013997643889 - }, - { - "x": 1.894717013309512, - "y": 7.013383749891871, - "heading": -0.04465409215915743, - "angularVelocity": -0.574233690167933, - "velocityX": 1.6999458782753816, - "velocityY": 0.013562353923706081, - "timestamp": 0.8867643261195083 - }, - { - "x": 2.026909781256637, - "y": 7.014438171688664, - "heading": -6.873205041659995e-7, - "angularVelocity": 0.5742248515023068, - "velocityX": 1.6999459015140637, - "velocityY": 0.013559440805737809, - "timestamp": 0.9645272524746277 - }, - { - "x": 2.1570162459060764, - "y": 7.015475952303694, - "heading": -4.0701288137533413e-7, - "angularVelocity": 0.0000036046434455229545, - "velocityX": 1.6731168790547177, - "velocityY": 0.0133454418920676, - "timestamp": 1.0422901788297472 - }, - { - "x": 2.25459609761466, - "y": 7.016254287790654, - "heading": -2.021152112745776e-7, - "angularVelocity": 0.000002634901741802409, - "velocityX": 1.2548377007182918, - "velocityY": 0.010009081749382266, - "timestamp": 1.1200531051848666 - }, - { - "x": 2.3196493330208185, - "y": 7.016773178122739, - "heading": -6.706257500555856e-8, - "angularVelocity": 0.0000017367226594878274, - "velocityX": 0.8365584791534231, - "velocityY": 0.006672721262003089, - "timestamp": 1.197816031539986 - }, - { - "x": 2.3521759510040283, - "y": 7.017032623291016, - "heading": -5.459567245742045e-24, - "angularVelocity": 8.62397779364737e-7, - "velocityX": 0.4182792431790796, - "velocityY": 0.0033363606597260143, - "timestamp": 1.2755789578951053 - }, - { - "x": 2.3521759510040283, - "y": 7.017032623291016, - "heading": -3.0375060318554146e-24, - "angularVelocity": 7.015482005050734e-25, - "velocityX": -5.736202459306136e-26, - "velocityY": 3.1210527485389075e-25, - "timestamp": 1.3533418842502247 + "heading": -6.978679086372593e-23, + "angularVelocity": -0.000001760307058732952, + "velocityX": 0.3610685666784826, + "velocityY": 0.04637614168728246, + "timestamp": 1.015141433945785 + }, + { + "x": 1.7625242471694946, + "y": 7.0123291015625, + "heading": -3.402897415947517e-23, + "angularVelocity": 2.5545845274476152e-23, + "velocityX": -1.6289861869168524e-23, + "velocityY": -2.7174652182389944e-23, + "timestamp": 1.0828175295421705 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/2 piece left.2.traj b/src/main/deploy/choreo/2 piece left.2.traj index 9de977f..5ea7400 100644 --- a/src/main/deploy/choreo/2 piece left.2.traj +++ b/src/main/deploy/choreo/2 piece left.2.traj @@ -1,175 +1,76 @@ { "samples": [ { - "x": 2.3521759510040283, - "y": 7.017032623291016, - "heading": -3.0375060318554146e-24, - "angularVelocity": 7.015482005050734e-25, - "velocityX": -5.736202459306136e-26, - "velocityY": 3.1210527485389075e-25, + "x": 1.7625242471694946, + "y": 7.0123291015625, + "heading": -3.402897415947517e-23, + "angularVelocity": 2.5545845274476152e-23, + "velocityX": -1.6289861869168524e-23, + "velocityY": -2.7174652182389944e-23, "timestamp": 0 }, { - "x": 2.32247783894149, - "y": 7.014438485612277, - "heading": 1.4569198322416039e-8, - "angularVelocity": 1.9570400792959067e-7, - "velocityX": -0.3989265181213512, - "velocityY": -0.03484633331329086, - "timestamp": 0.07444506873695533 - }, - { - "x": 2.2630816153943543, - "y": 7.009250210303195, - "heading": 4.6009116708770193e-8, - "angularVelocity": 4.2232372029159017e-7, - "velocityX": -0.7978530284793834, - "velocityY": -0.06969266597649279, - "timestamp": 0.14889013747391067 - }, - { - "x": 2.1739872815185026, - "y": 7.001467797460562, - "heading": 9.892279855196894e-8, - "angularVelocity": 7.107748403076132e-7, - "velocityX": -1.1967795233107767, - "velocityY": -0.10453899733951685, - "timestamp": 0.223335206210866 - }, - { - "x": 2.0551948407815805, - "y": 6.991091247374754, - "heading": 1.87119372985259e-7, - "angularVelocity": 0.0000011847201692421604, - "velocityX": -1.5957059715622512, - "velocityY": -0.13938532480200694, - "timestamp": 0.29778027494782133 - }, - { - "x": 1.929118297895444, - "y": 6.980078429264653, - "heading": 0.06161507996236991, - "angularVelocity": 0.8276557989449552, - "velocityX": -1.693551299302524, - "velocityY": -0.14793213703669883, - "timestamp": 0.37222534368477667 - }, - { - "x": 1.8030417550185982, - "y": 6.969065611048185, - "heading": 0.1818100793075498, - "angularVelocity": 1.614546153082047, - "velocityX": -1.6935512991777255, - "velocityY": -0.1479321384654859, - "timestamp": 0.446670412421732 - }, - { - "x": 1.6769652121424672, - "y": 6.958052792823533, - "heading": 0.34903336005588126, - "angularVelocity": 2.2462640385113897, - "velocityX": -1.6935512991681227, - "velocityY": -0.14793213857541956, - "timestamp": 0.5211154811586873 + "x": 1.81479684926806, + "y": 7.0127460685926595, + "heading": -4.386560783337548e-21, + "angularVelocity": -4.415213777084415e-20, + "velocityX": 0.5302539374684592, + "velocityY": 0.0042297188328221345, + "timestamp": 0.09858031860758154 }, { - "x": 1.5508886692676784, - "y": 6.947039974583516, - "heading": 0.5474314025531648, - "angularVelocity": 2.6650259830950556, - "velocityX": -1.6935512991500936, - "velocityY": -0.14793213878181766, - "timestamp": 0.5955605498956427 + "x": 1.9193420469047702, + "y": 7.013580002600649, + "heading": -1.4475848885926105e-20, + "angularVelocity": -1.0234586623955383e-19, + "velocityX": 1.06050780838793, + "velocityY": 0.00845943713479765, + "timestamp": 0.19716063721516308 }, { - "x": 1.4248121263920972, - "y": 6.936027156352571, - "heading": 0.7568159477198164, - "angularVelocity": 2.8126046321011815, - "velocityX": -1.6935512991607387, - "velocityY": -0.14793213865995034, - "timestamp": 0.670005618632598 + "x": 2.0573500990867615, + "y": 7.014680862426758, + "heading": 7.919072179411037e-17, + "angularVelocity": 8.034585276427108e-16, + "velocityX": 1.3999554285410607, + "velocityY": 0.011167136013136953, + "timestamp": 0.2957409558227446 }, { - "x": 1.2987355835144632, - "y": 6.925014338145126, - "heading": 0.9551163962131972, - "angularVelocity": 2.6637150298572045, - "velocityX": -1.6935512991883124, - "velocityY": -0.14793213834428362, - "timestamp": 0.7444506873695536 + "x": 2.1953581512687528, + "y": 7.015781722252867, + "heading": 1.482512560830555e-20, + "angularVelocity": -8.03161298186475e-16, + "velocityX": 1.3999554285410607, + "velocityY": 0.011167136013136954, + "timestamp": 0.39432127443032616 }, { - "x": 1.172659040636784, - "y": 6.914001519938199, - "heading": 1.1222044891261886, - "angularVelocity": 2.2444480977428, - "velocityX": -1.6935512991889197, - "velocityY": -0.14793213833733215, - "timestamp": 0.8188957561065091 + "x": 2.2999033489054628, + "y": 7.016615656260856, + "heading": 4.4513458995432525e-21, + "angularVelocity": -1.0523175269429369e-19, + "velocityX": 1.06050780838793, + "velocityY": 0.00845943713479765, + "timestamp": 0.4929015930379077 }, { - "x": 1.0465824977596736, - "y": 6.902988701724757, - "heading": 1.2422496454510472, - "angularVelocity": 1.61253335327054, - "velocityX": -1.6935512991812764, - "velocityY": -0.14793213842483294, - "timestamp": 0.8933408248434647 - }, - { - "x": 0.9205059547934591, - "y": 6.891975884531403, - "heading": 1.3036998128368849, - "angularVelocity": 0.8254430874792525, - "velocityX": -1.6935513003781917, - "velocityY": -0.1479321247222771, - "timestamp": 0.9677858935804202 - }, - { - "x": 0.80171351409583, - "y": 6.881599333995763, - "heading": 1.303699901063561, - "angularVelocity": 0.0000011851245168080392, - "velocityX": -1.5957059710344408, - "velocityY": -0.13938533084447882, - "timestamp": 1.0422309623173758 - }, - { - "x": 0.7126191802404107, - "y": 6.873816920919218, - "heading": 1.3036999539873229, - "angularVelocity": 7.109102435762517e-7, - "velocityX": -1.1967795230363152, - "velocityY": -0.10453900048160246, - "timestamp": 1.1166760310543313 - }, - { - "x": 0.653222956704277, - "y": 6.868628645484184, - "heading": 1.3036999854303395, - "angularVelocity": 4.223653346202014e-7, - "velocityX": -0.7978530283315964, - "velocityY": -0.06969266766838508, - "timestamp": 1.1911210997912869 - }, - { - "x": 0.6235248446464539, - "y": 6.866034507751465, - "heading": 1.3037, - "angularVelocity": 1.9571021880679713e-7, - "velocityX": -0.39892651805801393, - "velocityY": -0.034846334038387564, - "timestamp": 1.2655661685282424 + "x": 2.3521759510040283, + "y": 7.017032623291016, + "heading": 3.1297526369907225e-26, + "angularVelocity": -4.515419167199066e-20, + "velocityX": 0.5302539374684592, + "velocityY": 0.004229718832822134, + "timestamp": 0.5914819116454892 }, { - "x": 0.6235248446464539, - "y": 6.866034507751465, - "heading": 1.3037, - "angularVelocity": 2.310134713088712e-26, - "velocityX": 9.797912030064837e-26, - "velocityY": 2.886038048031371e-26, - "timestamp": 1.340011237265198 + "x": 2.3521759510040283, + "y": 7.017032623291016, + "heading": 3.2629175808510626e-26, + "angularVelocity": 1.351359118023865e-26, + "velocityX": 8.97547505231035e-28, + "velocityY": 4.8236792223928767e-26, + "timestamp": 0.6900622302530708 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/2 piece left.3.traj b/src/main/deploy/choreo/2 piece left.3.traj index c903530..978c900 100644 --- a/src/main/deploy/choreo/2 piece left.3.traj +++ b/src/main/deploy/choreo/2 piece left.3.traj @@ -1,202 +1,175 @@ { "samples": [ { - "x": 0.6235248446464539, - "y": 6.866034507751465, - "heading": 1.3037, - "angularVelocity": 2.310134713088712e-26, - "velocityX": 9.797912030064837e-26, - "velocityY": 2.886038048031371e-26, + "x": 2.3521759510040283, + "y": 7.017032623291016, + "heading": 3.2629175808510626e-26, + "angularVelocity": 1.351359118023865e-26, + "velocityX": 8.97547505231035e-28, + "velocityY": 4.8236792223928767e-26, "timestamp": 0 }, { - "x": 0.6684031135092222, - "y": 6.878475658375004, - "heading": 1.303699996148483, - "angularVelocity": -4.139316696396632e-8, - "velocityX": 0.48231739179768857, - "velocityY": 0.13370799435372274, - "timestamp": 0.09304717106612825 - }, - { - "x": 0.7581596501389595, - "y": 6.903357959319175, - "heading": 1.3036999873891952, - "angularVelocity": -9.413814140388183e-8, - "velocityX": 0.9646347718185619, - "velocityY": 0.26741598545201006, - "timestamp": 0.1860943421322565 - }, - { - "x": 0.8927944512482682, - "y": 6.940681409675249, - "heading": 1.3036999705533763, - "angularVelocity": -1.8093853561830772e-7, - "velocityX": 1.4469521165089936, - "velocityY": 0.401123966783992, - "timestamp": 0.27914151319838476 - }, - { - "x": 1.0452258043087184, - "y": 6.982938418917629, - "heading": 1.2738420182759358, - "angularVelocity": -0.320890489579964, - "velocityX": 1.6382158781820235, - "velocityY": 0.4541460934083428, - "timestamp": 0.372188684264513 - }, - { - "x": 1.1976571573646206, - "y": 7.025195428176415, - "heading": 1.2121674042798947, - "angularVelocity": -0.6628316937460592, - "velocityX": 1.638215878133146, - "velocityY": 0.454146093584667, - "timestamp": 0.46523585533064127 - }, - { - "x": 1.3500885104205298, - "y": 7.067452437435176, - "heading": 1.1261361159969507, - "angularVelocity": -0.9245986449367907, - "velocityX": 1.6382158781332203, - "velocityY": 0.4541460935843984, - "timestamp": 0.5582830263967695 - }, - { - "x": 1.5025198634764079, - "y": 7.10970944669405, - "heading": 1.0220884699278219, - "angularVelocity": -1.118224712013896, - "velocityX": 1.638215878132886, - "velocityY": 0.4541460935856051, - "timestamp": 0.6513301974628978 - }, - { - "x": 1.6549512165321918, - "y": 7.151966455953263, - "heading": 0.9053979173511129, - "angularVelocity": -1.2541010246703523, - "velocityX": 1.6382158781318752, - "velocityY": 0.45414609358925084, - "timestamp": 0.744377368529026 - }, - { - "x": 1.8073825695878554, - "y": 7.194223465212911, - "heading": 0.7806890917766147, - "angularVelocity": -1.3402753049404161, - "velocityX": 1.6382158781305804, - "velocityY": 0.4541460935939214, - "timestamp": 0.8374245395951543 - }, - { - "x": 1.9598139226434546, - "y": 7.236480474472791, - "heading": 0.6520865211987327, - "angularVelocity": -1.3821223053249492, - "velocityX": 1.6382158781298872, - "velocityY": 0.4541460935964215, - "timestamp": 0.9304717106612825 - }, - { - "x": 2.1122452756990984, - "y": 7.2787374837325105, - "heading": 0.523466246349196, - "angularVelocity": -1.3823125773284042, - "velocityX": 1.6382158781303655, - "velocityY": 0.45414609359469665, - "timestamp": 1.0235188817274108 - }, - { - "x": 2.264676628754865, - "y": 7.320994492991785, - "heading": 0.39870934727668983, - "angularVelocity": -1.3407919622171214, - "velocityX": 1.638215878131692, - "velocityY": 0.45414609358991137, - "timestamp": 1.116566052793539 - }, - { - "x": 2.4171079818107506, - "y": 7.363251502250633, - "heading": 0.2819512067590763, - "angularVelocity": -1.2548274082898667, - "velocityX": 1.6382158781329623, - "velocityY": 0.4541460935853291, - "timestamp": 1.2096132238596673 - }, - { - "x": 2.5695393348666884, - "y": 7.405508511509292, - "heading": 0.17782836416626208, - "angularVelocity": -1.1190328668758192, - "velocityX": 1.6382158781335259, - "velocityY": 0.4541460935832961, - "timestamp": 1.3026603949257956 - }, - { - "x": 2.721970687922618, - "y": 7.447765520767979, - "heading": 0.09171880670511134, - "angularVelocity": -0.925439822345088, - "velocityX": 1.6382158781334417, - "velocityY": 0.4541460935836001, - "timestamp": 1.3957075659919234 - }, - { - "x": 2.874402040978524, - "y": 7.490022530026754, - "heading": 0.029958419943994605, - "angularVelocity": -0.6637535139808163, - "velocityX": 1.6382158781331815, - "velocityY": 0.45414609358453867, - "timestamp": 1.4887547370580512 - }, - { - "x": 3.0268333940396634, - "y": 7.532279539266648, - "heading": 2.9431902776520237e-8, - "angularVelocity": -0.3219699230920253, - "velocityX": 1.6382158781894283, - "velocityY": 0.45414609338163237, - "timestamp": 1.581801908124179 - }, - { - "x": 3.1614681951486, - "y": 7.569602989624063, - "heading": 1.2605096609367751e-8, - "angularVelocity": -1.8084167389886298e-7, - "velocityX": 1.4469521165049974, - "velocityY": 0.4011239667984073, - "timestamp": 1.6748490791903068 - }, - { - "x": 3.2512247317781684, - "y": 7.594485290568843, - "heading": 3.849889723802212e-9, - "angularVelocity": -9.409428341828091e-8, - "velocityX": 0.9646347718167455, - "velocityY": 0.26741598545856243, - "timestamp": 1.7678962502564346 - }, - { - "x": 3.296103000640869, - "y": 7.606926441192627, - "heading": 8.360584447548999e-24, - "angularVelocity": -4.137567730099069e-8, - "velocityX": 0.482317391796962, - "velocityY": 0.1337079943563437, - "timestamp": 1.8609434213225624 - }, - { - "x": 3.296103000640869, - "y": 7.606926441192627, - "heading": 4.186402997192896e-24, - "angularVelocity": 1.3134786042557728e-25, - "velocityX": -6.505907384915032e-26, - "velocityY": -1.7970275247888583e-26, - "timestamp": 1.9539905923886902 + "x": 2.3142846243767354, + "y": 7.012127931705875, + "heading": 1.2115480408206838e-8, + "angularVelocity": 1.4375415553606747e-7, + "velocityX": -0.4495930394768391, + "velocityY": -0.05819577707452758, + "timestamp": 0.08427916649106604 + }, + { + "x": 2.2385019735410854, + "y": 7.00231854884861, + "heading": 3.970446596226117e-8, + "angularVelocity": 3.2735237784985534e-7, + "velocityX": -0.899186050252208, + "velocityY": -0.11639155043500052, + "timestamp": 0.16855833298213208 + }, + { + "x": 2.12482800575389, + "y": 6.987604475658275, + "heading": 9.284103659267056e-8, + "angularVelocity": 6.304828683378364e-7, + "velocityX": -1.348778974923124, + "velocityY": -0.1745873126533039, + "timestamp": 0.2528374994731981 + }, + { + "x": 2.007813392652147, + "y": 6.972457985967442, + "heading": 0.045388788071344756, + "angularVelocity": 0.538551781182121, + "velocityX": -1.3884168291359105, + "velocityY": -0.1797180765004146, + "timestamp": 0.3371166659642644 + }, + { + "x": 1.890798779550621, + "y": 6.957311496274927, + "heading": 0.1278606190497445, + "angularVelocity": 0.9785553703493507, + "velocityX": -1.3884168291333299, + "velocityY": -0.17971807652037916, + "timestamp": 0.42139583245533063 + }, + { + "x": 1.7737841664491285, + "y": 6.942165006582153, + "heading": 0.2386191497967303, + "angularVelocity": 1.3141863565858434, + "velocityX": -1.3884168291329324, + "velocityY": -0.179718076523449, + "timestamp": 0.5056749989463969 + }, + { + "x": 1.6567695533477331, + "y": 6.927018516888629, + "heading": 0.3691813127912384, + "angularVelocity": 1.5491629596069652, + "velocityX": -1.3884168291317809, + "velocityY": -0.17971807653234628, + "timestamp": 0.5899541654374632 + }, + { + "x": 1.5397549402464386, + "y": 6.911872027194326, + "heading": 0.5114296088502888, + "angularVelocity": 1.6878227678500983, + "velocityX": -1.3884168291305836, + "velocityY": -0.17971807654159477, + "timestamp": 0.6742333319285294 + }, + { + "x": 1.4227403271451358, + "y": 6.896725537500087, + "heading": 0.6575337184093297, + "angularVelocity": 1.7335732618396096, + "velocityX": -1.3884168291306829, + "velocityY": -0.17971807654082692, + "timestamp": 0.7585124984195957 + }, + { + "x": 1.3057257140437122, + "y": 6.881579047806781, + "heading": 0.799773650598925, + "angularVelocity": 1.687723527791098, + "velocityX": -1.3884168291321157, + "velocityY": -0.17971807652975855, + "timestamp": 0.8427916649106619 + }, + { + "x": 1.188711100942176, + "y": 6.866432558114345, + "heading": 0.930324246141598, + "angularVelocity": 1.5490257079905023, + "velocityX": -1.388416829133451, + "velocityY": -0.17971807651944244, + "timestamp": 0.9270708314017282 + }, + { + "x": 1.071696487840612, + "y": 6.851286068422124, + "heading": 1.0410732405530951, + "angularVelocity": 1.3140732048321446, + "velocityX": -1.3884168291337817, + "velocityY": -0.17971807651688804, + "timestamp": 1.0113499978927945 + }, + { + "x": 0.9546818747390692, + "y": 6.836139578729739, + "heading": 1.1235395305504658, + "angularVelocity": 0.9784896247889747, + "velocityX": -1.3884168291335306, + "velocityY": -0.1797180765188294, + "timestamp": 1.0956291643838607 + }, + { + "x": 0.8376672616369766, + "y": 6.820993089041604, + "heading": 1.1689256310372955, + "angularVelocity": 0.5385209936982573, + "velocityX": -1.3884168291400538, + "velocityY": -0.17971807646840415, + "timestamp": 1.179908330874927 + }, + { + "x": 0.7239932938499879, + "y": 6.806279015849672, + "heading": 1.1689256841765412, + "angularVelocity": 6.305146069742176e-7, + "velocityX": -1.3487789749206713, + "velocityY": -0.1745873126722503, + "timestamp": 1.2641874973659932 + }, + { + "x": 0.6482106430144319, + "y": 6.796469632991681, + "heading": 1.1689257117674343, + "angularVelocity": 3.27375012689987e-7, + "velocityX": -0.8991860502510931, + "velocityY": -0.1163915504436125, + "timestamp": 1.3484666638570595 + }, + { + "x": 0.6103193163871765, + "y": 6.79156494140625, + "heading": 1.1689257238838953, + "angularVelocity": 1.4376578928572422e-7, + "velocityX": -0.44959303947639306, + "velocityY": -0.05819577707797239, + "timestamp": 1.4327458303481257 + }, + { + "x": 0.6103193163871765, + "y": 6.79156494140625, + "heading": 1.1689257238838953, + "angularVelocity": 3.0587665357762507e-24, + "velocityX": 4.4273867233926817e-26, + "velocityY": 3.2272073611359003e-27, + "timestamp": 1.517024996839192 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/2 piece left.4.traj b/src/main/deploy/choreo/2 piece left.4.traj new file mode 100644 index 0000000..f8900f4 --- /dev/null +++ b/src/main/deploy/choreo/2 piece left.4.traj @@ -0,0 +1,284 @@ +{ + "samples": [ + { + "x": 0.6103193163871765, + "y": 6.79156494140625, + "heading": 1.1689257238838953, + "angularVelocity": 3.0587665357762507e-24, + "velocityX": 4.4273867233926817e-26, + "velocityY": 3.2272073611359003e-27, + "timestamp": 0 + }, + { + "x": 0.6733155792661583, + "y": 6.796419497130506, + "heading": 1.1689257179975108, + "angularVelocity": -5.4312792519524506e-8, + "velocityX": 0.5812571386949579, + "velocityY": 0.044792262921005174, + "timestamp": 0.10837933624423313 + }, + { + "x": 0.7993081014378632, + "y": 6.806128608302689, + "heading": 1.1689257047296802, + "angularVelocity": -1.2242029760434803e-7, + "velocityX": 1.1625142443000427, + "velocityY": 0.08958452329237418, + "timestamp": 0.21675867248846625 + }, + { + "x": 0.9882968721435262, + "y": 6.820692274093818, + "heading": 1.1689256795952214, + "angularVelocity": -2.3191190757497266e-7, + "velocityX": 1.743771250635604, + "velocityY": 0.13437677601484202, + "timestamp": 0.3251380087326994 + }, + { + "x": 1.226026584318479, + "y": 6.839011966124108, + "heading": 1.1623600098622169, + "angularVelocity": -0.06058045712892038, + "velocityX": 2.193496661016895, + "velocityY": 0.16903307092605394, + "timestamp": 0.4335173449769325 + }, + { + "x": 1.463756296493383, + "y": 6.8573316581551245, + "heading": 1.1373583072341475, + "angularVelocity": -0.23068698789340977, + "velocityX": 2.193496661016445, + "velocityY": 0.16903307093275596, + "timestamp": 0.5418966812211656 + }, + { + "x": 1.701486008668291, + "y": 6.875651350186089, + "heading": 1.0993044418538873, + "angularVelocity": -0.3511173504006862, + "velocityX": 2.1934966610164817, + "velocityY": 0.16903307093227754, + "timestamp": 0.6502760174653988 + }, + { + "x": 1.9392157208432044, + "y": 6.893971042216987, + "heading": 1.052051199011011, + "angularVelocity": -0.43599863664408267, + "velocityX": 2.193496661016529, + "velocityY": 0.16903307093165879, + "timestamp": 0.7586553537096323 + }, + { + "x": 2.1769454330181204, + "y": 6.91229073424785, + "heading": 0.9983385147189527, + "angularVelocity": -0.49559894121344894, + "velocityX": 2.193496661016554, + "velocityY": 0.16903307093133652, + "timestamp": 0.8670346899538659 + }, + { + "x": 2.414675145193034, + "y": 6.930610426278742, + "heading": 0.94010924392218, + "angularVelocity": -0.5372728124626364, + "velocityX": 2.1934966610165327, + "velocityY": 0.16903307093161182, + "timestamp": 0.9754140261980995 + }, + { + "x": 2.6524048573679395, + "y": 6.948930118309735, + "heading": 0.8787410045179366, + "angularVelocity": -0.5662356084738315, + "velocityX": 2.1934966610164612, + "velocityY": 0.16903307093254125, + "timestamp": 1.083793362442333 + }, + { + "x": 2.890134569542833, + "y": 6.967249810340885, + "heading": 0.8152156203040447, + "angularVelocity": -0.5861392624765411, + "velocityX": 2.19349666101635, + "velocityY": 0.16903307093398154, + "timestamp": 1.1921726986865666 + }, + { + "x": 3.1278642817177125, + "y": 6.985569502372218, + "heading": 0.7502413213076805, + "angularVelocity": -0.5995081834598467, + "velocityX": 2.1934966610162197, + "velocityY": 0.16903307093567946, + "timestamp": 1.3005520349308002 + }, + { + "x": 3.365593993892578, + "y": 7.003889194403733, + "heading": 0.6843427304656562, + "angularVelocity": -0.6080364867110983, + "velocityX": 2.1934966610160904, + "velocityY": 0.1690330709373518, + "timestamp": 1.4089313711750338 + }, + { + "x": 3.603323706067432, + "y": 7.022208886435398, + "heading": 0.6179279596644911, + "angularVelocity": -0.6127992023451708, + "velocityX": 2.1934966610159834, + "velocityY": 0.1690330709387406, + "timestamp": 1.5173107074192673 + }, + { + "x": 3.8410534182422786, + "y": 7.040528578467161, + "heading": 0.5513412518560862, + "angularVelocity": -0.6143856395129749, + "velocityX": 2.1934966610159137, + "velocityY": 0.1690330709396457, + "timestamp": 1.625690043663501 + }, + { + "x": 4.078783130417123, + "y": 7.058848270498957, + "heading": 0.48490739393082904, + "angularVelocity": -0.6129753163974749, + "velocityX": 2.1934966610158906, + "velocityY": 0.16903307093994402, + "timestamp": 1.7340693799077345 + }, + { + "x": 4.31651284259197, + "y": 7.077167962530715, + "heading": 0.4189731502579807, + "angularVelocity": -0.6083654500731145, + "velocityX": 2.1934966610159172, + "velocityY": 0.1690330709396015, + "timestamp": 1.842448716151968 + }, + { + "x": 4.554242554766825, + "y": 7.0954876545623735, + "heading": 0.353951234014042, + "angularVelocity": -0.5999475407139573, + "velocityX": 2.1934966610159883, + "velocityY": 0.1690330709386799, + "timestamp": 1.9508280523962016 + }, + { + "x": 4.79197226694169, + "y": 7.1138073465938865, + "heading": 0.2903723240116442, + "angularVelocity": -0.5866331369581602, + "velocityX": 2.1934966610160918, + "velocityY": 0.1690330709373368, + "timestamp": 2.059207388640435 + }, + { + "x": 5.0297019791165685, + "y": 7.132127038625234, + "heading": 0.22895107669335069, + "angularVelocity": -0.5667247046049495, + "velocityX": 2.193496661016209, + "velocityY": 0.16903307093581316, + "timestamp": 2.1675867248846687 + }, + { + "x": 5.267431691291459, + "y": 7.150446730656428, + "heading": 0.17067503234950135, + "angularVelocity": -0.5377043850178608, + "velocityX": 2.1934966610163182, + "velocityY": 0.16903307093439685, + "timestamp": 2.2759660611289023 + }, + { + "x": 5.505161403466358, + "y": 7.16876642268751, + "heading": 0.11692607465311596, + "angularVelocity": -0.49593363051478706, + "velocityX": 2.1934966610163986, + "velocityY": 0.16903307093335337, + "timestamp": 2.384345397373136 + }, + { + "x": 5.742891115641262, + "y": 7.187086114718534, + "heading": 0.06964863269353613, + "angularVelocity": -0.43622191829113954, + "velocityX": 2.1934966610164395, + "velocityY": 0.16903307093282607, + "timestamp": 2.4927247336173695 + }, + { + "x": 5.980620827816166, + "y": 7.20540580674955, + "heading": 0.031581441152300896, + "angularVelocity": -0.35124030890400276, + "velocityX": 2.1934966610164457, + "velocityY": 0.1690330709327442, + "timestamp": 2.601104069861603 + }, + { + "x": 6.218350539991069, + "y": 7.223725498780575, + "heading": 0.006572709542556832, + "angularVelocity": -0.23075184326084827, + "velocityX": 2.193496661016438, + "velocityY": 0.16903307093283967, + "timestamp": 2.7094834061058366 + }, + { + "x": 6.456080252165983, + "y": 7.242045190811364, + "heading": 4.42760723315573e-8, + "angularVelocity": -0.06064500387484363, + "velocityX": 2.1934966610165403, + "velocityY": 0.16903307093065992, + "timestamp": 2.81786274235007 + }, + { + "x": 6.645069022871665, + "y": 7.25660885660225, + "heading": 1.914820045081504e-8, + "angularVelocity": -2.318511328037341e-7, + "velocityX": 1.7437712506357763, + "velocityY": 0.1343767760126046, + "timestamp": 2.9262420785943037 + }, + { + "x": 6.771061545043378, + "y": 7.266317967774324, + "heading": 5.884427199036025e-9, + "angularVelocity": -1.2238286108238435e-7, + "velocityX": 1.1625142443001213, + "velocityY": 0.08958452329135717, + "timestamp": 3.0346214148385373 + }, + { + "x": 6.834057807922363, + "y": 7.271172523498535, + "heading": 6.250436275292675e-24, + "angularVelocity": -5.429473369144319e-8, + "velocityX": 0.5812571386949893, + "velocityY": 0.04479226292059838, + "timestamp": 3.143000751082771 + }, + { + "x": 6.834057807922363, + "y": 7.271172523498535, + "heading": 2.9708189294917462e-24, + "angularVelocity": -2.8492370133741702e-24, + "velocityX": 1.1892634808647167e-25, + "velocityY": 9.309147669588167e-27, + "timestamp": 3.2513800873270045 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece left.traj b/src/main/deploy/choreo/2 piece left.traj index 160ac83..1c05769 100644 --- a/src/main/deploy/choreo/2 piece left.traj +++ b/src/main/deploy/choreo/2 piece left.traj @@ -4,568 +4,649 @@ "x": 0.6235248446464539, "y": 6.866034507751465, "heading": 1.3037, - "angularVelocity": 2.3321158035498043e-24, - "velocityX": -1.6085262930190747e-26, - "velocityY": -4.0384459981982936e-25, + "angularVelocity": 2.7068477576187765e-23, + "velocityX": -4.853948155553872e-23, + "velocityY": -6.243185313363585e-23, "timestamp": 0 }, { - "x": 0.6371648015347651, - "y": 6.867786445253041, - "heading": 1.3036999632733615, - "angularVelocity": -7.263599506268419e-7, - "velocityX": 0.26976382275301153, - "velocityY": 0.03464888940043332, - "timestamp": 0.050562587485274306 - }, - { - "x": 0.6644447150656874, - "y": 6.871290320309862, - "heading": 1.303699886456694, - "angularVelocity": -0.0000015192392509530055, - "velocityX": 0.5395276406466987, - "velocityY": 0.0692977798622562, - "timestamp": 0.10112517497054861 - }, - { - "x": 0.7053645848706707, - "y": 6.876546133002426, - "heading": 1.3036997645049127, - "angularVelocity": -0.0000024118975620600877, - "velocityX": 0.809291451251395, - "velocityY": 0.10394667191616248, - "timestamp": 0.1516877624558229 - }, - { - "x": 0.759924410335464, - "y": 6.8835538834649, - "heading": 1.303699589009544, - "angularVelocity": -0.0000034708541873039917, - "velocityX": 1.0790552497077626, - "velocityY": 0.13859556662353859, - "timestamp": 0.20225034994109722 - }, - { - "x": 0.8281241902315634, - "y": 6.892313571965616, - "heading": 1.3036993431536497, - "angularVelocity": -0.000004862407296943842, - "velocityX": 1.3488190238674322, - "velocityY": 0.17324446663784457, - "timestamp": 0.2528129374263715 - }, - { - "x": 0.909963920873441, - "y": 6.902825199309566, - "heading": 1.3036989764864741, - "angularVelocity": -0.000007251748650640463, - "velocityX": 1.61858272513669, - "velocityY": 0.20789338257286122, - "timestamp": 0.3033755249116458 - }, - { - "x": 0.9952199524201776, - "y": 6.913775599360361, - "heading": 1.2650877104567244, - "angularVelocity": -0.763633111952482, - "velocityX": 1.6861485099346676, - "velocityY": 0.21657119612368925, - "timestamp": 0.35393811239692013 - }, - { - "x": 1.08047598194801, - "y": 6.924726015129663, - "heading": 1.1860996824058685, - "angularVelocity": -1.5621832659149426, - "velocityX": 1.6861484700058589, - "velocityY": 0.21657150699596817, - "timestamp": 0.40450069988219445 - }, - { - "x": 1.165732011476475, - "y": 6.93567643089404, - "heading": 1.068522180672941, - "angularVelocity": -2.325385380389609, - "velocityX": 1.6861484700183644, - "velocityY": 0.21657150689860336, - "timestamp": 0.45506328736746876 - }, - { - "x": 1.25098804100406, - "y": 6.94662684666527, - "heading": 0.9153751201313117, - "angularVelocity": -3.0288612224647617, - "velocityX": 1.6861484700009595, - "velocityY": 0.2165715070341085, - "timestamp": 0.505625874852743 - }, - { - "x": 1.3362440705163487, - "y": 6.957577262555589, - "heading": 0.7326430566977549, - "angularVelocity": -3.6139776961923715, - "velocityX": 1.6861484696984423, - "velocityY": 0.21657150938939215, - "timestamp": 0.5561884623380173 - }, - { - "x": 1.4215001000011822, - "y": 6.968527678659667, - "heading": 0.5352466423996767, - "angularVelocity": -3.904001438920968, - "velocityX": 1.6861484691554405, - "velocityY": 0.21657151361700638, - "timestamp": 0.6067510498232916 - }, - { - "x": 1.5067561295108884, - "y": 6.9794780945700925, - "heading": 0.35194096302396116, - "angularVelocity": -3.6253223676320148, - "velocityX": 1.6861484696473656, - "velocityY": 0.2165715097870587, - "timestamp": 0.657313637308566 - }, - { - "x": 1.5920121590364624, - "y": 6.99042851035698, - "heading": 0.19809518233895146, - "angularVelocity": -3.0426801383496302, - "velocityX": 1.6861484699611848, - "velocityY": 0.21657150734378086, - "timestamp": 0.7078762247938403 - }, - { - "x": 1.6772681885652982, - "y": 7.001378926118472, - "heading": 0.07976876190025861, - "angularVelocity": -2.3401970967793897, - "velocityX": 1.6861484700257006, - "velocityY": 0.21657150684148851, - "timestamp": 0.7584388122791146 + "x": 0.6479605553240734, + "y": 6.869173065177977, + "heading": 1.3036998817532777, + "angularVelocity": -0.0000017472450406683682, + "velocityX": 0.3610685643473286, + "velocityY": 0.04637615983684271, + "timestamp": 0.06767609559638565 + }, + { + "x": 0.6968319743321776, + "y": 6.875450180817172, + "heading": 1.3036996103660266, + "angularVelocity": -0.000004010090249051131, + "velocityX": 0.722137094012766, + "velocityY": 0.09275233129037688, + "timestamp": 0.1353521911927713 + }, + { + "x": 0.7701390946293506, + "y": 6.884865857027548, + "heading": 1.303699081157409, + "angularVelocity": -0.00000781972738083724, + "velocityX": 1.083205519632373, + "velocityY": 0.13912853759369243, + "timestamp": 0.20302828678915696 + }, + { + "x": 0.8641136418785188, + "y": 6.89693604798726, + "heading": 1.2781268289721, + "angularVelocity": -0.3778624041466495, + "velocityX": 1.388592920750396, + "velocityY": 0.17835235400838984, + "timestamp": 0.2707043823855426 + }, + { + "x": 0.958088185941953, + "y": 6.909006263750245, + "heading": 1.1796751937375807, + "angularVelocity": -1.4547475643641785, + "velocityX": 1.3885928736771431, + "velocityY": 0.17835272050816342, + "timestamp": 0.3383804779819283 + }, + { + "x": 1.0520627300139076, + "y": 6.921076479446895, + "heading": 1.0124911373568812, + "angularVelocity": -2.4703561118207933, + "velocityX": 1.3885928738030422, + "velocityY": 0.178352719527955, + "timestamp": 0.40605657357831393 + }, + { + "x": 1.1460372740816214, + "y": 6.933146695176562, + "heading": 0.785208686841268, + "angularVelocity": -3.358385978279628, + "velocityX": 1.3885928737403777, + "velocityY": 0.17835272001583247, + "timestamp": 0.4737326691746996 + }, + { + "x": 1.2400118181361255, + "y": 6.945216911009073, + "heading": 0.5237947450805, + "angularVelocity": -3.862721976749634, + "velocityX": 1.3885928735451927, + "velocityY": 0.1783527215354694, + "timestamp": 0.5414087647710852 + }, + { + "x": 1.3339863622028791, + "y": 6.9572871267462135, + "heading": 0.2954599282674446, + "angularVelocity": -3.3739360227697586, + "velocityX": 1.388592873726193, + "velocityY": 0.17835272012626976, + "timestamp": 0.6090848603674709 + }, + { + "x": 1.4279609062740386, + "y": 6.969357342449054, + "heading": 0.126956229009992, + "angularVelocity": -2.489855506180409, + "velocityX": 1.3885928737912927, + "velocityY": 0.17835271961943208, + "timestamp": 0.6767609559638565 + }, + { + "x": 1.521935450341706, + "y": 6.981427558179083, + "heading": 0.027070659114329158, + "angularVelocity": -1.4759357645478204, + "velocityX": 1.3885928737396922, + "velocityY": 0.1783527200211782, + "timestamp": 0.7444370515602422 + }, + { + "x": 1.6159099957667231, + "y": 6.993497763341015, + "heading": 9.235413386651335e-7, + "angularVelocity": -0.3999896172266207, + "velocityX": 1.388592893796256, + "velocityY": 0.1783525638641785, + "timestamp": 0.8121131471566279 + }, + { + "x": 1.6892171169315986, + "y": 7.002913432795811, + "heading": 3.921850758219532e-7, + "angularVelocity": -0.000007851461556117877, + "velocityX": 1.0832055324537762, + "velocityY": 0.13912843777145287, + "timestamp": 0.8797892427530135 + }, + { + "x": 1.7380885363341119, + "y": 7.00919054536428, + "heading": 1.1913070878580363e-7, + "angularVelocity": -0.000004034723998627552, + "velocityX": 0.7221370998406585, + "velocityY": 0.09275228591651416, + "timestamp": 0.9474653383493992 }, { "x": 1.7625242471694946, "y": 7.0123291015625, - "heading": 8.02748616433338e-24, - "angularVelocity": -1.577624205317622, - "velocityX": 1.686149045062736, - "velocityY": 0.21656675397035893, - "timestamp": 0.8090013997643889 - }, - { - "x": 1.894717013309512, - "y": 7.013383749891871, - "heading": -0.04465409215915743, - "angularVelocity": -0.574233690167933, - "velocityX": 1.6999458782753816, - "velocityY": 0.013562353923706081, - "timestamp": 0.8867643261195083 - }, - { - "x": 2.026909781256637, - "y": 7.014438171688664, - "heading": -6.873205041659995e-7, - "angularVelocity": 0.5742248515023068, - "velocityX": 1.6999459015140637, - "velocityY": 0.013559440805737809, - "timestamp": 0.9645272524746277 - }, - { - "x": 2.1570162459060764, - "y": 7.015475952303694, - "heading": -4.0701288137533413e-7, - "angularVelocity": 0.0000036046434455229545, - "velocityX": 1.6731168790547177, - "velocityY": 0.0133454418920676, - "timestamp": 1.0422901788297472 - }, - { - "x": 2.25459609761466, - "y": 7.016254287790654, - "heading": -2.021152112745776e-7, - "angularVelocity": 0.000002634901741802409, - "velocityX": 1.2548377007182918, - "velocityY": 0.010009081749382266, - "timestamp": 1.1200531051848666 - }, - { - "x": 2.3196493330208185, - "y": 7.016773178122739, - "heading": -6.706257500555856e-8, - "angularVelocity": 0.0000017367226594878274, - "velocityX": 0.8365584791534231, - "velocityY": 0.006672721262003089, - "timestamp": 1.197816031539986 + "heading": -6.978679086372593e-23, + "angularVelocity": -0.000001760307058732952, + "velocityX": 0.3610685666784826, + "velocityY": 0.04637614168728246, + "timestamp": 1.015141433945785 }, { - "x": 2.3521759510040283, - "y": 7.017032623291016, - "heading": -5.459567245742045e-24, - "angularVelocity": 8.62397779364737e-7, - "velocityX": 0.4182792431790796, - "velocityY": 0.0033363606597260143, - "timestamp": 1.2755789578951053 + "x": 1.7625242471694946, + "y": 7.0123291015625, + "heading": -3.402897415947517e-23, + "angularVelocity": 2.5545845274476152e-23, + "velocityX": -1.6289861869168524e-23, + "velocityY": -2.7174652182389944e-23, + "timestamp": 1.0828175295421705 + }, + { + "x": 1.81479684926806, + "y": 7.0127460685926595, + "heading": -4.386560783337548e-21, + "angularVelocity": -4.415213777084415e-20, + "velocityX": 0.5302539374684592, + "velocityY": 0.0042297188328221345, + "timestamp": 1.181397848149752 + }, + { + "x": 1.9193420469047702, + "y": 7.013580002600649, + "heading": -1.4475848885926105e-20, + "angularVelocity": -1.0234586623955383e-19, + "velocityX": 1.06050780838793, + "velocityY": 0.00845943713479765, + "timestamp": 1.2799781667573336 + }, + { + "x": 2.0573500990867615, + "y": 7.014680862426758, + "heading": 7.919072179411037e-17, + "angularVelocity": 8.034585276427108e-16, + "velocityX": 1.3999554285410607, + "velocityY": 0.011167136013136953, + "timestamp": 1.378558485364915 + }, + { + "x": 2.1953581512687528, + "y": 7.015781722252867, + "heading": 1.482512560830555e-20, + "angularVelocity": -8.03161298186475e-16, + "velocityX": 1.3999554285410607, + "velocityY": 0.011167136013136954, + "timestamp": 1.4771388039724966 + }, + { + "x": 2.2999033489054628, + "y": 7.016615656260856, + "heading": 4.4513458995432525e-21, + "angularVelocity": -1.0523175269429369e-19, + "velocityX": 1.06050780838793, + "velocityY": 0.00845943713479765, + "timestamp": 1.5757191225800782 }, { "x": 2.3521759510040283, "y": 7.017032623291016, - "heading": -3.0375060318554146e-24, - "angularVelocity": 7.015482005050734e-25, - "velocityX": -5.736202459306136e-26, - "velocityY": 3.1210527485389075e-25, - "timestamp": 1.3533418842502247 - }, - { - "x": 2.32247783894149, - "y": 7.014438485612277, - "heading": 1.4569198322416039e-8, - "angularVelocity": 1.9570400792959067e-7, - "velocityX": -0.3989265181213512, - "velocityY": -0.03484633331329086, - "timestamp": 1.42778695298718 - }, - { - "x": 2.2630816153943543, - "y": 7.009250210303195, - "heading": 4.6009116708770193e-8, - "angularVelocity": 4.2232372029159017e-7, - "velocityX": -0.7978530284793834, - "velocityY": -0.06969266597649279, - "timestamp": 1.5022320217241354 - }, - { - "x": 2.1739872815185026, - "y": 7.001467797460562, - "heading": 9.892279855196894e-8, - "angularVelocity": 7.107748403076132e-7, - "velocityX": -1.1967795233107767, - "velocityY": -0.10453899733951685, - "timestamp": 1.5766770904610907 - }, - { - "x": 2.0551948407815805, - "y": 6.991091247374754, - "heading": 1.87119372985259e-7, - "angularVelocity": 0.0000011847201692421604, - "velocityX": -1.5957059715622512, - "velocityY": -0.13938532480200694, - "timestamp": 1.651122159198046 - }, - { - "x": 1.929118297895444, - "y": 6.980078429264653, - "heading": 0.06161507996236991, - "angularVelocity": 0.8276557989449552, - "velocityX": -1.693551299302524, - "velocityY": -0.14793213703669883, - "timestamp": 1.7255672279350014 - }, - { - "x": 1.8030417550185982, - "y": 6.969065611048185, - "heading": 0.1818100793075498, - "angularVelocity": 1.614546153082047, - "velocityX": -1.6935512991777255, - "velocityY": -0.1479321384654859, - "timestamp": 1.8000122966719567 - }, - { - "x": 1.6769652121424672, - "y": 6.958052792823533, - "heading": 0.34903336005588126, - "angularVelocity": 2.2462640385113897, - "velocityX": -1.6935512991681227, - "velocityY": -0.14793213857541956, - "timestamp": 1.874457365408912 - }, - { - "x": 1.5508886692676784, - "y": 6.947039974583516, - "heading": 0.5474314025531648, - "angularVelocity": 2.6650259830950556, - "velocityX": -1.6935512991500936, - "velocityY": -0.14793213878181766, - "timestamp": 1.9489024341458674 - }, - { - "x": 1.4248121263920972, - "y": 6.936027156352571, - "heading": 0.7568159477198164, - "angularVelocity": 2.8126046321011815, - "velocityX": -1.6935512991607387, - "velocityY": -0.14793213865995034, - "timestamp": 2.0233475028828227 - }, - { - "x": 1.2987355835144632, - "y": 6.925014338145126, - "heading": 0.9551163962131972, - "angularVelocity": 2.6637150298572045, - "velocityX": -1.6935512991883124, - "velocityY": -0.14793213834428362, - "timestamp": 2.0977925716197783 - }, - { - "x": 1.172659040636784, - "y": 6.914001519938199, - "heading": 1.1222044891261886, - "angularVelocity": 2.2444480977428, - "velocityX": -1.6935512991889197, - "velocityY": -0.14793213833733215, - "timestamp": 2.172237640356734 - }, - { - "x": 1.0465824977596736, - "y": 6.902988701724757, - "heading": 1.2422496454510472, - "angularVelocity": 1.61253335327054, - "velocityX": -1.6935512991812764, - "velocityY": -0.14793213842483294, - "timestamp": 2.2466827090936894 - }, - { - "x": 0.9205059547934591, - "y": 6.891975884531403, - "heading": 1.3036998128368849, - "angularVelocity": 0.8254430874792525, - "velocityX": -1.6935513003781917, - "velocityY": -0.1479321247222771, - "timestamp": 2.321127777830645 - }, - { - "x": 0.80171351409583, - "y": 6.881599333995763, - "heading": 1.303699901063561, - "angularVelocity": 0.0000011851245168080392, - "velocityX": -1.5957059710344408, - "velocityY": -0.13938533084447882, - "timestamp": 2.3955728465676005 - }, - { - "x": 0.7126191802404107, - "y": 6.873816920919218, - "heading": 1.3036999539873229, - "angularVelocity": 7.109102435762517e-7, - "velocityX": -1.1967795230363152, - "velocityY": -0.10453900048160246, - "timestamp": 2.470017915304556 - }, - { - "x": 0.653222956704277, - "y": 6.868628645484184, - "heading": 1.3036999854303395, - "angularVelocity": 4.223653346202014e-7, - "velocityX": -0.7978530283315964, - "velocityY": -0.06969266766838508, - "timestamp": 2.5444629840415116 - }, - { - "x": 0.6235248446464539, - "y": 6.866034507751465, - "heading": 1.3037, - "angularVelocity": 1.9571021880679713e-7, - "velocityX": -0.39892651805801393, - "velocityY": -0.034846334038387564, - "timestamp": 2.618908052778467 + "heading": 3.1297526369907225e-26, + "angularVelocity": -4.515419167199066e-20, + "velocityX": 0.5302539374684592, + "velocityY": 0.004229718832822134, + "timestamp": 1.6742994411876597 }, { - "x": 0.6235248446464539, - "y": 6.866034507751465, - "heading": 1.3037, - "angularVelocity": 2.310134713088712e-26, - "velocityX": 9.797912030064837e-26, - "velocityY": 2.886038048031371e-26, - "timestamp": 2.6933531215154227 - }, - { - "x": 0.6684031135092222, - "y": 6.878475658375004, - "heading": 1.303699996148483, - "angularVelocity": -4.139316696396632e-8, - "velocityX": 0.48231739179768857, - "velocityY": 0.13370799435372274, - "timestamp": 2.786400292581551 - }, - { - "x": 0.7581596501389595, - "y": 6.903357959319175, - "heading": 1.3036999873891952, - "angularVelocity": -9.413814140388183e-8, - "velocityX": 0.9646347718185619, - "velocityY": 0.26741598545201006, - "timestamp": 2.8794474636476792 - }, - { - "x": 0.8927944512482682, - "y": 6.940681409675249, - "heading": 1.3036999705533763, - "angularVelocity": -1.8093853561830772e-7, - "velocityX": 1.4469521165089936, - "velocityY": 0.401123966783992, - "timestamp": 2.9724946347138075 - }, - { - "x": 1.0452258043087184, - "y": 6.982938418917629, - "heading": 1.2738420182759358, - "angularVelocity": -0.320890489579964, - "velocityX": 1.6382158781820235, - "velocityY": 0.4541460934083428, - "timestamp": 3.0655418057799357 - }, - { - "x": 1.1976571573646206, - "y": 7.025195428176415, - "heading": 1.2121674042798947, - "angularVelocity": -0.6628316937460592, - "velocityX": 1.638215878133146, - "velocityY": 0.454146093584667, - "timestamp": 3.158588976846064 - }, - { - "x": 1.3500885104205298, - "y": 7.067452437435176, - "heading": 1.1261361159969507, - "angularVelocity": -0.9245986449367907, - "velocityX": 1.6382158781332203, - "velocityY": 0.4541460935843984, - "timestamp": 3.2516361479121922 - }, - { - "x": 1.5025198634764079, - "y": 7.10970944669405, - "heading": 1.0220884699278219, - "angularVelocity": -1.118224712013896, - "velocityX": 1.638215878132886, - "velocityY": 0.4541460935856051, - "timestamp": 3.3446833189783205 - }, - { - "x": 1.6549512165321918, - "y": 7.151966455953263, - "heading": 0.9053979173511129, - "angularVelocity": -1.2541010246703523, - "velocityX": 1.6382158781318752, - "velocityY": 0.45414609358925084, - "timestamp": 3.4377304900444488 - }, - { - "x": 1.8073825695878554, - "y": 7.194223465212911, - "heading": 0.7806890917766147, - "angularVelocity": -1.3402753049404161, - "velocityX": 1.6382158781305804, - "velocityY": 0.4541460935939214, - "timestamp": 3.530777661110577 - }, - { - "x": 1.9598139226434546, - "y": 7.236480474472791, - "heading": 0.6520865211987327, - "angularVelocity": -1.3821223053249492, - "velocityX": 1.6382158781298872, - "velocityY": 0.4541460935964215, - "timestamp": 3.6238248321767053 - }, - { - "x": 2.1122452756990984, - "y": 7.2787374837325105, - "heading": 0.523466246349196, - "angularVelocity": -1.3823125773284042, - "velocityX": 1.6382158781303655, - "velocityY": 0.45414609359469665, - "timestamp": 3.7168720032428335 - }, - { - "x": 2.264676628754865, - "y": 7.320994492991785, - "heading": 0.39870934727668983, - "angularVelocity": -1.3407919622171214, - "velocityX": 1.638215878131692, - "velocityY": 0.45414609358991137, - "timestamp": 3.8099191743089618 - }, - { - "x": 2.4171079818107506, - "y": 7.363251502250633, - "heading": 0.2819512067590763, - "angularVelocity": -1.2548274082898667, - "velocityX": 1.6382158781329623, - "velocityY": 0.4541460935853291, - "timestamp": 3.90296634537509 - }, - { - "x": 2.5695393348666884, - "y": 7.405508511509292, - "heading": 0.17782836416626208, - "angularVelocity": -1.1190328668758192, - "velocityX": 1.6382158781335259, - "velocityY": 0.4541460935832961, - "timestamp": 3.9960135164412183 - }, - { - "x": 2.721970687922618, - "y": 7.447765520767979, - "heading": 0.09171880670511134, - "angularVelocity": -0.925439822345088, - "velocityX": 1.6382158781334417, - "velocityY": 0.4541460935836001, - "timestamp": 4.089060687507346 - }, - { - "x": 2.874402040978524, - "y": 7.490022530026754, - "heading": 0.029958419943994605, - "angularVelocity": -0.6637535139808163, - "velocityX": 1.6382158781331815, - "velocityY": 0.45414609358453867, - "timestamp": 4.182107858573474 - }, - { - "x": 3.0268333940396634, - "y": 7.532279539266648, - "heading": 2.9431902776520237e-8, - "angularVelocity": -0.3219699230920253, - "velocityX": 1.6382158781894283, - "velocityY": 0.45414609338163237, - "timestamp": 4.275155029639602 - }, - { - "x": 3.1614681951486, - "y": 7.569602989624063, - "heading": 1.2605096609367751e-8, - "angularVelocity": -1.8084167389886298e-7, - "velocityX": 1.4469521165049974, - "velocityY": 0.4011239667984073, - "timestamp": 4.3682022007057295 - }, - { - "x": 3.2512247317781684, - "y": 7.594485290568843, - "heading": 3.849889723802212e-9, - "angularVelocity": -9.409428341828091e-8, - "velocityX": 0.9646347718167455, - "velocityY": 0.26741598545856243, - "timestamp": 4.461249371771857 - }, - { - "x": 3.296103000640869, - "y": 7.606926441192627, - "heading": 8.360584447548999e-24, - "angularVelocity": -4.137567730099069e-8, - "velocityX": 0.482317391796962, - "velocityY": 0.1337079943563437, - "timestamp": 4.554296542837985 - }, - { - "x": 3.296103000640869, - "y": 7.606926441192627, - "heading": 4.186402997192896e-24, - "angularVelocity": 1.3134786042557728e-25, - "velocityX": -6.505907384915032e-26, - "velocityY": -1.7970275247888583e-26, - "timestamp": 4.647343713904113 + "x": 2.3521759510040283, + "y": 7.017032623291016, + "heading": 3.2629175808510626e-26, + "angularVelocity": 1.351359118023865e-26, + "velocityX": 8.97547505231035e-28, + "velocityY": 4.8236792223928767e-26, + "timestamp": 1.7728797597952413 + }, + { + "x": 2.3142846243767354, + "y": 7.012127931705875, + "heading": 1.2115480408206838e-8, + "angularVelocity": 1.4375415553606747e-7, + "velocityX": -0.4495930394768391, + "velocityY": -0.05819577707452758, + "timestamp": 1.8571589262863073 + }, + { + "x": 2.2385019735410854, + "y": 7.00231854884861, + "heading": 3.970446596226117e-8, + "angularVelocity": 3.2735237784985534e-7, + "velocityX": -0.899186050252208, + "velocityY": -0.11639155043500052, + "timestamp": 1.9414380927773733 + }, + { + "x": 2.12482800575389, + "y": 6.987604475658275, + "heading": 9.284103659267056e-8, + "angularVelocity": 6.304828683378364e-7, + "velocityX": -1.348778974923124, + "velocityY": -0.1745873126533039, + "timestamp": 2.0257172592684394 + }, + { + "x": 2.007813392652147, + "y": 6.972457985967442, + "heading": 0.045388788071344756, + "angularVelocity": 0.538551781182121, + "velocityX": -1.3884168291359105, + "velocityY": -0.1797180765004146, + "timestamp": 2.1099964257595056 + }, + { + "x": 1.890798779550621, + "y": 6.957311496274927, + "heading": 0.1278606190497445, + "angularVelocity": 0.9785553703493507, + "velocityX": -1.3884168291333299, + "velocityY": -0.17971807652037916, + "timestamp": 2.194275592250572 + }, + { + "x": 1.7737841664491285, + "y": 6.942165006582153, + "heading": 0.2386191497967303, + "angularVelocity": 1.3141863565858434, + "velocityX": -1.3884168291329324, + "velocityY": -0.179718076523449, + "timestamp": 2.278554758741638 + }, + { + "x": 1.6567695533477331, + "y": 6.927018516888629, + "heading": 0.3691813127912384, + "angularVelocity": 1.5491629596069652, + "velocityX": -1.3884168291317809, + "velocityY": -0.17971807653234628, + "timestamp": 2.3628339252327044 + }, + { + "x": 1.5397549402464386, + "y": 6.911872027194326, + "heading": 0.5114296088502888, + "angularVelocity": 1.6878227678500983, + "velocityX": -1.3884168291305836, + "velocityY": -0.17971807654159477, + "timestamp": 2.4471130917237707 + }, + { + "x": 1.4227403271451358, + "y": 6.896725537500087, + "heading": 0.6575337184093297, + "angularVelocity": 1.7335732618396096, + "velocityX": -1.3884168291306829, + "velocityY": -0.17971807654082692, + "timestamp": 2.531392258214837 + }, + { + "x": 1.3057257140437122, + "y": 6.881579047806781, + "heading": 0.799773650598925, + "angularVelocity": 1.687723527791098, + "velocityX": -1.3884168291321157, + "velocityY": -0.17971807652975855, + "timestamp": 2.615671424705903 + }, + { + "x": 1.188711100942176, + "y": 6.866432558114345, + "heading": 0.930324246141598, + "angularVelocity": 1.5490257079905023, + "velocityX": -1.388416829133451, + "velocityY": -0.17971807651944244, + "timestamp": 2.6999505911969695 + }, + { + "x": 1.071696487840612, + "y": 6.851286068422124, + "heading": 1.0410732405530951, + "angularVelocity": 1.3140732048321446, + "velocityX": -1.3884168291337817, + "velocityY": -0.17971807651688804, + "timestamp": 2.7842297576880357 + }, + { + "x": 0.9546818747390692, + "y": 6.836139578729739, + "heading": 1.1235395305504658, + "angularVelocity": 0.9784896247889747, + "velocityX": -1.3884168291335306, + "velocityY": -0.1797180765188294, + "timestamp": 2.868508924179102 + }, + { + "x": 0.8376672616369766, + "y": 6.820993089041604, + "heading": 1.1689256310372955, + "angularVelocity": 0.5385209936982573, + "velocityX": -1.3884168291400538, + "velocityY": -0.17971807646840415, + "timestamp": 2.9527880906701682 + }, + { + "x": 0.7239932938499879, + "y": 6.806279015849672, + "heading": 1.1689256841765412, + "angularVelocity": 6.305146069742176e-7, + "velocityX": -1.3487789749206713, + "velocityY": -0.1745873126722503, + "timestamp": 3.0370672571612345 + }, + { + "x": 0.6482106430144319, + "y": 6.796469632991681, + "heading": 1.1689257117674343, + "angularVelocity": 3.27375012689987e-7, + "velocityX": -0.8991860502510931, + "velocityY": -0.1163915504436125, + "timestamp": 3.1213464236523008 + }, + { + "x": 0.6103193163871765, + "y": 6.79156494140625, + "heading": 1.1689257238838953, + "angularVelocity": 1.4376578928572422e-7, + "velocityX": -0.44959303947639306, + "velocityY": -0.05819577707797239, + "timestamp": 3.205625590143367 + }, + { + "x": 0.6103193163871765, + "y": 6.79156494140625, + "heading": 1.1689257238838953, + "angularVelocity": 3.0587665357762507e-24, + "velocityX": 4.4273867233926817e-26, + "velocityY": 3.2272073611359003e-27, + "timestamp": 3.2899047566344333 + }, + { + "x": 0.6733155792661583, + "y": 6.796419497130506, + "heading": 1.1689257179975108, + "angularVelocity": -5.4312792519524506e-8, + "velocityX": 0.5812571386949579, + "velocityY": 0.044792262921005174, + "timestamp": 3.3982840928786664 + }, + { + "x": 0.7993081014378632, + "y": 6.806128608302689, + "heading": 1.1689257047296802, + "angularVelocity": -1.2242029760434803e-7, + "velocityX": 1.1625142443000427, + "velocityY": 0.08958452329237418, + "timestamp": 3.5066634291228995 + }, + { + "x": 0.9882968721435262, + "y": 6.820692274093818, + "heading": 1.1689256795952214, + "angularVelocity": -2.3191190757497266e-7, + "velocityX": 1.743771250635604, + "velocityY": 0.13437677601484202, + "timestamp": 3.6150427653671326 + }, + { + "x": 1.226026584318479, + "y": 6.839011966124108, + "heading": 1.1623600098622169, + "angularVelocity": -0.06058045712892038, + "velocityX": 2.193496661016895, + "velocityY": 0.16903307092605394, + "timestamp": 3.7234221016113658 + }, + { + "x": 1.463756296493383, + "y": 6.8573316581551245, + "heading": 1.1373583072341475, + "angularVelocity": -0.23068698789340977, + "velocityX": 2.193496661016445, + "velocityY": 0.16903307093275596, + "timestamp": 3.831801437855599 + }, + { + "x": 1.701486008668291, + "y": 6.875651350186089, + "heading": 1.0993044418538873, + "angularVelocity": -0.3511173504006862, + "velocityX": 2.1934966610164817, + "velocityY": 0.16903307093227754, + "timestamp": 3.940180774099832 + }, + { + "x": 1.9392157208432044, + "y": 6.893971042216987, + "heading": 1.052051199011011, + "angularVelocity": -0.43599863664408267, + "velocityX": 2.193496661016529, + "velocityY": 0.16903307093165879, + "timestamp": 4.048560110344066 + }, + { + "x": 2.1769454330181204, + "y": 6.91229073424785, + "heading": 0.9983385147189527, + "angularVelocity": -0.49559894121344894, + "velocityX": 2.193496661016554, + "velocityY": 0.16903307093133652, + "timestamp": 4.156939446588299 + }, + { + "x": 2.414675145193034, + "y": 6.930610426278742, + "heading": 0.94010924392218, + "angularVelocity": -0.5372728124626364, + "velocityX": 2.1934966610165327, + "velocityY": 0.16903307093161182, + "timestamp": 4.265318782832533 + }, + { + "x": 2.6524048573679395, + "y": 6.948930118309735, + "heading": 0.8787410045179366, + "angularVelocity": -0.5662356084738315, + "velocityX": 2.1934966610164612, + "velocityY": 0.16903307093254125, + "timestamp": 4.373698119076766 + }, + { + "x": 2.890134569542833, + "y": 6.967249810340885, + "heading": 0.8152156203040447, + "angularVelocity": -0.5861392624765411, + "velocityX": 2.19349666101635, + "velocityY": 0.16903307093398154, + "timestamp": 4.482077455321 + }, + { + "x": 3.1278642817177125, + "y": 6.985569502372218, + "heading": 0.7502413213076805, + "angularVelocity": -0.5995081834598467, + "velocityX": 2.1934966610162197, + "velocityY": 0.16903307093567946, + "timestamp": 4.5904567915652335 + }, + { + "x": 3.365593993892578, + "y": 7.003889194403733, + "heading": 0.6843427304656562, + "angularVelocity": -0.6080364867110983, + "velocityX": 2.1934966610160904, + "velocityY": 0.1690330709373518, + "timestamp": 4.698836127809467 + }, + { + "x": 3.603323706067432, + "y": 7.022208886435398, + "heading": 0.6179279596644911, + "angularVelocity": -0.6127992023451708, + "velocityX": 2.1934966610159834, + "velocityY": 0.1690330709387406, + "timestamp": 4.807215464053701 + }, + { + "x": 3.8410534182422786, + "y": 7.040528578467161, + "heading": 0.5513412518560862, + "angularVelocity": -0.6143856395129749, + "velocityX": 2.1934966610159137, + "velocityY": 0.1690330709396457, + "timestamp": 4.915594800297934 + }, + { + "x": 4.078783130417123, + "y": 7.058848270498957, + "heading": 0.48490739393082904, + "angularVelocity": -0.6129753163974749, + "velocityX": 2.1934966610158906, + "velocityY": 0.16903307093994402, + "timestamp": 5.023974136542168 + }, + { + "x": 4.31651284259197, + "y": 7.077167962530715, + "heading": 0.4189731502579807, + "angularVelocity": -0.6083654500731145, + "velocityX": 2.1934966610159172, + "velocityY": 0.1690330709396015, + "timestamp": 5.132353472786401 + }, + { + "x": 4.554242554766825, + "y": 7.0954876545623735, + "heading": 0.353951234014042, + "angularVelocity": -0.5999475407139573, + "velocityX": 2.1934966610159883, + "velocityY": 0.1690330709386799, + "timestamp": 5.240732809030635 + }, + { + "x": 4.79197226694169, + "y": 7.1138073465938865, + "heading": 0.2903723240116442, + "angularVelocity": -0.5866331369581602, + "velocityX": 2.1934966610160918, + "velocityY": 0.1690330709373368, + "timestamp": 5.3491121452748684 + }, + { + "x": 5.0297019791165685, + "y": 7.132127038625234, + "heading": 0.22895107669335069, + "angularVelocity": -0.5667247046049495, + "velocityX": 2.193496661016209, + "velocityY": 0.16903307093581316, + "timestamp": 5.457491481519102 + }, + { + "x": 5.267431691291459, + "y": 7.150446730656428, + "heading": 0.17067503234950135, + "angularVelocity": -0.5377043850178608, + "velocityX": 2.1934966610163182, + "velocityY": 0.16903307093439685, + "timestamp": 5.565870817763336 + }, + { + "x": 5.505161403466358, + "y": 7.16876642268751, + "heading": 0.11692607465311596, + "angularVelocity": -0.49593363051478706, + "velocityX": 2.1934966610163986, + "velocityY": 0.16903307093335337, + "timestamp": 5.674250154007569 + }, + { + "x": 5.742891115641262, + "y": 7.187086114718534, + "heading": 0.06964863269353613, + "angularVelocity": -0.43622191829113954, + "velocityX": 2.1934966610164395, + "velocityY": 0.16903307093282607, + "timestamp": 5.782629490251803 + }, + { + "x": 5.980620827816166, + "y": 7.20540580674955, + "heading": 0.031581441152300896, + "angularVelocity": -0.35124030890400276, + "velocityX": 2.1934966610164457, + "velocityY": 0.1690330709327442, + "timestamp": 5.891008826496036 + }, + { + "x": 6.218350539991069, + "y": 7.223725498780575, + "heading": 0.006572709542556832, + "angularVelocity": -0.23075184326084827, + "velocityX": 2.193496661016438, + "velocityY": 0.16903307093283967, + "timestamp": 5.99938816274027 + }, + { + "x": 6.456080252165983, + "y": 7.242045190811364, + "heading": 4.42760723315573e-8, + "angularVelocity": -0.06064500387484363, + "velocityX": 2.1934966610165403, + "velocityY": 0.16903307093065992, + "timestamp": 6.107767498984503 + }, + { + "x": 6.645069022871665, + "y": 7.25660885660225, + "heading": 1.914820045081504e-8, + "angularVelocity": -2.318511328037341e-7, + "velocityX": 1.7437712506357763, + "velocityY": 0.1343767760126046, + "timestamp": 6.216146835228737 + }, + { + "x": 6.771061545043378, + "y": 7.266317967774324, + "heading": 5.884427199036025e-9, + "angularVelocity": -1.2238286108238435e-7, + "velocityX": 1.1625142443001213, + "velocityY": 0.08958452329135717, + "timestamp": 6.324526171472971 + }, + { + "x": 6.834057807922363, + "y": 7.271172523498535, + "heading": 6.250436275292675e-24, + "angularVelocity": -5.429473369144319e-8, + "velocityX": 0.5812571386949893, + "velocityY": 0.04479226292059838, + "timestamp": 6.432905507717204 + }, + { + "x": 6.834057807922363, + "y": 7.271172523498535, + "heading": 2.9708189294917462e-24, + "angularVelocity": -2.8492370133741702e-24, + "velocityX": 1.1892634808647167e-25, + "velocityY": 9.309147669588167e-27, + "timestamp": 6.541284843961438 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/2 piece right.1.traj b/src/main/deploy/choreo/2 piece right.1.traj index 4f7ec6f..82d03ff 100644 --- a/src/main/deploy/choreo/2 piece right.1.traj +++ b/src/main/deploy/choreo/2 piece right.1.traj @@ -4,226 +4,163 @@ "x": 0.6313003897666931, "y": 4.255156517028809, "heading": -1.2897, - "angularVelocity": 2.5588458209070936e-23, - "velocityX": 3.6747187166411656e-23, - "velocityY": 2.9631382958424116e-23, + "angularVelocity": -1.3029252310464996e-24, + "velocityX": 9.818866312976863e-25, + "velocityY": -2.13204077766819e-25, "timestamp": 0 }, { - "x": 0.6442648165759174, - "y": 4.253481867543582, - "heading": -1.2896998999232763, - "angularVelocity": 0.000002030081537303308, - "velocityX": 0.26298666147739386, - "velocityY": -0.03397068638245913, - "timestamp": 0.0492968986959011 - }, - { - "x": 0.6701936695279935, - "y": 4.25013256816077, - "heading": -1.289699690051548, - "angularVelocity": 0.000004257300839893244, - "velocityX": 0.5259733094372531, - "velocityY": -0.06794138112974009, - "timestamp": 0.0985937973918022 - }, - { - "x": 0.7090869476233618, - "y": 4.245108618261832, - "heading": -1.2896993558074163, - "angularVelocity": 0.000006780226354565095, - "velocityX": 0.7889599371207936, - "velocityY": -0.10191208842422805, - "timestamp": 0.1478906960877033 - }, - { - "x": 0.7609446491960873, - "y": 4.238410016815877, - "heading": -1.2896988728952734, - "angularVelocity": 0.00000979599439019053, - "velocityX": 1.051946531010426, - "velocityY": -0.13588281663065846, - "timestamp": 0.1971875947836044 - }, - { - "x": 0.8257667709142923, - "y": 4.230036761761128, - "heading": -1.2896981927241198, - "angularVelocity": 0.000013797443073734933, - "velocityX": 1.3149330573120783, - "velocityY": -0.1698535866607121, - "timestamp": 0.2464844934795055 - }, - { - "x": 0.90355330278228, - "y": 4.219988846912366, - "heading": -1.2896971695227037, - "angularVelocity": 0.00002075589830370764, - "velocityX": 1.5779193808485028, - "velocityY": -0.20382448215952323, - "timestamp": 0.2957813921754066 - }, - { - "x": 0.9915565757190965, - "y": 4.208621348540917, - "heading": -1.2730220238110053, - "angularVelocity": 0.3382595285468722, - "velocityX": 1.7851685453822215, - "velocityY": -0.23059256610788906, - "timestamp": 0.3450782908713077 - }, - { - "x": 1.0795598357636118, - "y": 4.1972537503614955, - "heading": -1.2172146776466959, - "angularVelocity": 1.1320660658304207, - "velocityX": 1.7851682838586471, - "velocityY": -0.23059459073774355, - "timestamp": 0.3943751895672088 - }, - { - "x": 1.167563095807722, - "y": 4.1858861521789406, - "heading": -1.1235960458886458, - "angularVelocity": 1.8990775126759454, - "velocityX": 1.7851682838504348, - "velocityY": -0.23059459080130768, - "timestamp": 0.4436720882631099 - }, - { - "x": 1.2555663558538792, - "y": 4.17451855401223, - "heading": -0.9942697695783443, - "angularVelocity": 2.6234160714263077, - "velocityX": 1.7851682838919467, - "velocityY": -0.2305945904799162, - "timestamp": 0.492968986959011 - }, - { - "x": 1.343569615881345, - "y": 4.163150955700822, - "heading": -0.8330305095755579, - "angularVelocity": 3.270778979372043, - "velocityX": 1.7851682835127938, - "velocityY": -0.23059459341511745, - "timestamp": 0.5422658856549121 - }, - { - "x": 1.4315728758027453, - "y": 4.151783356568302, - "heading": -0.6481619169999266, - "angularVelocity": 3.750105939037553, - "velocityX": 1.7851682813612264, - "velocityY": -0.23059461007159965, - "timestamp": 0.5915627843508132 - }, - { - "x": 1.5195761357005462, - "y": 4.140415757253084, - "heading": -0.46092963892673033, - "angularVelocity": 3.798053894387562, - "velocityX": 1.785168280882508, - "velocityY": -0.2305946137776485, - "timestamp": 0.6408596830467144 - }, - { - "x": 1.607579395709073, - "y": 4.129048158795058, - "heading": -0.29529130470998255, - "angularVelocity": 3.3600153072209418, - "velocityX": 1.7851682831286084, - "velocityY": -0.23059459638932683, - "timestamp": 0.6901565817426155 - }, - { - "x": 1.6955826557496305, - "y": 4.117680560584999, - "heading": -0.16079446814860227, - "angularVelocity": 2.7283021877512823, - "velocityX": 1.785168283778364, - "velocityY": -0.23059459135922616, - "timestamp": 0.7394534804385167 - }, - { - "x": 1.7835859158077405, - "y": 4.106312962510822, - "heading": -0.06161125041031065, - "angularVelocity": 2.011956540108646, - "velocityX": 1.7851682841344167, - "velocityY": -0.23059458860283424, - "timestamp": 0.7887503791344178 + "x": 0.6559720855575579, + "y": 4.251969633511474, + "heading": -1.2896999477311053, + "angularVelocity": 7.68600953739464e-7, + "velocityX": 0.3627910812108772, + "velocityY": -0.04686232056148057, + "timestamp": 0.06800524342692997 + }, + { + "x": 0.7053154748885234, + "y": 4.245595866526154, + "heading": -1.2896998274887912, + "angularVelocity": 0.0000017681329843892469, + "velocityX": 0.7255821293248338, + "velocityY": -0.09372464039730581, + "timestamp": 0.13601048685385994 + }, + { + "x": 0.7793305510073084, + "y": 4.236035216220894, + "heading": -1.289699592159626, + "angularVelocity": 0.0000034604561825362674, + "velocityX": 1.0883730781481924, + "velocityY": -0.14058695805615118, + "timestamp": 0.2040157302807899 + }, + { + "x": 0.8737534106566409, + "y": 4.2238384675544545, + "heading": -1.2686933528400068, + "angularVelocity": 0.3088914657321989, + "velocityX": 1.3884644020249364, + "velocityY": -0.17935012142916007, + "timestamp": 0.2720209737077199 + }, + { + "x": 0.9681762696017908, + "y": 4.211641713436369, + "heading": -1.1852876077469632, + "angularVelocity": 1.2264605034854565, + "velocityX": 1.388464391670108, + "velocityY": -0.17935020159425882, + "timestamp": 0.34002621713464987 + }, + { + "x": 1.06259912854715, + "y": 4.199444959319902, + "heading": -1.0458164737621631, + "angularVelocity": 2.0508879456428724, + "velocityX": 1.3884643916731867, + "velocityY": -0.17935020157042256, + "timestamp": 0.40803146056157985 + }, + { + "x": 1.1570219874918148, + "y": 4.1872482051980615, + "heading": -0.8604729695452614, + "angularVelocity": 2.7254296121452617, + "velocityX": 1.388464391662975, + "velocityY": -0.17935020164947402, + "timestamp": 0.47603670398850984 + }, + { + "x": 1.2514448464329961, + "y": 4.175051451049253, + "heading": -0.6468064986930537, + "angularVelocity": 3.1419117127606087, + "velocityX": 1.3884643916117518, + "velocityY": -0.17935020204602378, + "timestamp": 0.5440419474154398 + }, + { + "x": 1.3458677053738775, + "y": 4.162854696898122, + "heading": -0.43285566722442825, + "angularVelocity": 3.1460931641030103, + "velocityX": 1.3884643916073398, + "velocityY": -0.17935020208018032, + "timestamp": 0.6120471908423697 + }, + { + "x": 1.4402905643180246, + "y": 4.1506579427722725, + "heading": -0.2468424865709028, + "angularVelocity": 2.735277035709639, + "velocityX": 1.3884643916553625, + "velocityY": -0.17935020170840796, + "timestamp": 0.6800524342692996 + }, + { + "x": 1.5347134232631718, + "y": 4.138461188654166, + "heading": -0.1064956083992093, + "angularVelocity": 2.0637655436450704, + "velocityX": 1.3884643916700699, + "velocityY": -0.17935020159455195, + "timestamp": 0.7480576776962296 + }, + { + "x": 1.6291362822082969, + "y": 4.126264434535885, + "heading": -0.022089749460474963, + "angularVelocity": 1.241166926038967, + "velocityX": 1.3884643916697392, + "velocityY": -0.17935020159711362, + "timestamp": 0.8160629211231595 + }, + { + "x": 1.7235591414801577, + "y": 4.114067682947199, + "heading": -4.084921018506283e-7, + "angularVelocity": 0.3248182030567626, + "velocityX": 1.3884643964743102, + "velocityY": -0.17935016440007143, + "timestamp": 0.8840681645500894 + }, + { + "x": 1.7975742177797642, + "y": 4.104507034041778, + "heading": -1.729250271854775e-7, + "angularVelocity": 0.000003463954583417705, + "velocityX": 1.0883730808071284, + "velocityY": -0.14058693747185988, + "timestamp": 0.9520734079770193 + }, + { + "x": 1.8469176071929212, + "y": 4.0981332676927495, + "heading": -5.242149315395944e-8, + "angularVelocity": 0.0000017719741590366667, + "velocityX": 0.7255821305334361, + "velocityY": -0.09372463104079504, + "timestamp": 1.0200786514039493 }, { "x": 1.8715893030166624, "y": 4.094946384429932, - "heading": 3.044635127032748e-22, - "angularVelocity": 1.2497997245298, - "velocityX": 1.7851708634206476, - "velocityY": -0.23057389778225362, - "timestamp": 0.838047277830319 - }, - { - "x": 2.001501484117773, - "y": 4.096925277087298, - "heading": 0.03742913022292678, - "angularVelocity": 0.5185396539200237, - "velocityX": 1.7997911526916002, - "velocityY": 0.027415392972907092, - "timestamp": 0.9102290867623309 - }, - { - "x": 2.1314136513515103, - "y": 4.098905079913163, - "heading": 0.0000021975515072904746, - "angularVelocity": -0.5185092092478852, - "velocityX": 1.7997909605743085, - "velocityY": 0.02742800236179506, - "timestamp": 0.9824108956943429 - }, - { - "x": 2.243505108146192, - "y": 4.10061330393091, - "heading": 0.0000012819688946605248, - "angularVelocity": -0.000012684395503198552, - "velocityX": 1.5529045122748328, - "velocityY": 0.023665575066908254, - "timestamp": 1.054592704626355 - }, - { - "x": 2.3275737085405206, - "y": 4.101894472003175, - "heading": 6.315287448797483e-7, - "angularVelocity": -0.00000901113673104848, - "velocityX": 1.1646784922432887, - "velocityY": 0.017749182116954924, - "timestamp": 1.126774513558367 - }, - { - "x": 2.3836194443971235, - "y": 4.102748584068442, - "heading": 2.0840831354227822e-7, - "angularVelocity": -0.000005861870706731764, - "velocityX": 0.7764523594773347, - "velocityY": 0.011832788314715953, - "timestamp": 1.1989563224903792 - }, - { - "x": 2.41164231300354, - "y": 4.103175640106201, - "heading": -4.343161804381014e-22, - "angularVelocity": -0.000002887269197403718, - "velocityX": 0.3882261891332122, - "velocityY": 0.0059163942283817755, - "timestamp": 1.2711381314223913 - }, - { - "x": 2.41164231300354, - "y": 4.103175640106201, - "heading": -2.1484022653531463e-22, - "angularVelocity": 8.099300815455187e-23, - "velocityX": 1.2310180125110273e-23, - "velocityY": -5.1753556121623715e-23, - "timestamp": 1.3433199403544034 + "heading": -9.905348338146396e-24, + "angularVelocity": 7.708448718411703e-7, + "velocityX": 0.36279108169431745, + "velocityY": -0.046862316818874365, + "timestamp": 1.0880838948308793 + }, + { + "x": 1.8715893030166624, + "y": 4.094946384429932, + "heading": -4.879834777552459e-24, + "angularVelocity": 2.1422164275294007e-24, + "velocityX": -3.536647328700017e-24, + "velocityY": 5.299012492424587e-25, + "timestamp": 1.1560891382578093 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/2 piece right.2.traj b/src/main/deploy/choreo/2 piece right.2.traj index 98d02a7..52c62f0 100644 --- a/src/main/deploy/choreo/2 piece right.2.traj +++ b/src/main/deploy/choreo/2 piece right.2.traj @@ -1,175 +1,76 @@ { "samples": [ { - "x": 2.41164231300354, - "y": 4.103175640106201, - "heading": -2.1484022653531463e-22, - "angularVelocity": 8.099300815455187e-23, - "velocityX": 1.2310180125110273e-23, - "velocityY": -5.1753556121623715e-23, + "x": 1.8715893030166624, + "y": 4.094946384429932, + "heading": -4.879834777552459e-24, + "angularVelocity": 2.1422164275294007e-24, + "velocityX": -3.536647328700017e-24, + "velocityY": 5.299012492424587e-25, "timestamp": 0 }, { - "x": 2.3823203832476643, - "y": 4.105678739540131, - "heading": -4.713122979131873e-8, - "angularVelocity": -6.372031214090865e-7, - "velocityX": -0.39642558127400507, - "velocityY": 0.03384131455001896, - "timestamp": 0.07396578611714011 - }, - { - "x": 2.3236765252241245, - "y": 4.110684938319997, - "heading": -1.489263961217733e-7, - "angularVelocity": -0.0000013762466631428888, - "velocityX": -0.7928511424277298, - "velocityY": 0.06768262791039739, - "timestamp": 0.14793157223428022 - }, - { - "x": 2.2357107419093456, - "y": 4.118194236269814, - "heading": -3.204509063199309e-7, - "angularVelocity": -0.000002318970962148814, - "velocityX": -1.1892766633409002, - "velocityY": 0.10152393889149473, - "timestamp": 0.22189735835142033 - }, - { - "x": 2.118423042232596, - "y": 4.128206632861628, - "heading": -6.069009309173134e-7, - "angularVelocity": -0.000003872736837322732, - "velocityX": -1.585702063532462, - "velocityY": 0.1353652427347487, - "timestamp": 0.29586314446856044 - }, - { - "x": 1.9857671108062405, - "y": 4.1395309526428035, - "heading": -0.05006393031110381, - "angularVelocity": -0.676844336256865, - "velocityX": -1.7934769356235545, - "velocityY": 0.15310213513098953, - "timestamp": 0.36982893058570054 - }, - { - "x": 1.8531111795597293, - "y": 4.150855274530819, - "heading": -0.16469064728369365, - "angularVelocity": -1.5497262043704214, - "velocityX": -1.7934769331921003, - "velocityY": 0.1531021636149704, - "timestamp": 0.44379471670284065 - }, - { - "x": 1.7204552483160596, - "y": 4.162179596452121, - "heading": -0.3328870431123415, - "angularVelocity": -2.2739756400651823, - "velocityX": -1.7934769331536837, - "velocityY": 0.15310216406498003, - "timestamp": 0.5177605028199808 + "x": 1.9311588685312648, + "y": 4.095854097517496, + "heading": 5.98134982599928e-18, + "angularVelocity": 5.683500198215718e-17, + "velocityX": 0.5660317002762608, + "velocityY": 0.008625115491089496, + "timestamp": 0.10524068790763574 }, { - "x": 1.5877993170796554, - "y": 4.173503918458529, - "heading": -0.5380010346158053, - "angularVelocity": -2.7730928348226245, - "velocityX": -1.793476933055458, - "velocityY": 0.15310216521560938, - "timestamp": 0.5917262889371209 + "x": 2.0363873382706825, + "y": 4.097457554892782, + "heading": 2.3583226342325868e-17, + "angularVelocity": 1.6725352965931608e-16, + "velocityX": 0.9998839026192134, + "velocityY": 0.015236097436880057, + "timestamp": 0.2104813758152715 }, { - "x": 1.455143385839801, - "y": 4.1848282404245225, - "heading": -0.7563600030365335, - "angularVelocity": -2.952161801875693, - "velocityX": -1.793476933102103, - "velocityY": 0.15310216466919027, - "timestamp": 0.665692075054261 + "x": 2.1416158080101013, + "y": 4.099061012268066, + "heading": 3.485956605861461e-16, + "angularVelocity": 3.0882773640653748e-15, + "velocityX": 0.9998839026192254, + "velocityY": 0.01523609743688024, + "timestamp": 0.31572206372290723 }, { - "x": 1.322487454589416, - "y": 4.196152562267158, - "heading": -0.9608101638901362, - "angularVelocity": -2.7641180008526347, - "velocityX": -1.7934769332444735, - "velocityY": 0.15310216300143226, - "timestamp": 0.7396578611714011 + "x": 2.24684427774952, + "y": 4.100664469643351, + "heading": 2.1220134489662188e-16, + "angularVelocity": -1.29602265436758e-15, + "velocityX": 0.9998839026192254, + "velocityY": 0.01523609743688024, + "timestamp": 0.420962751630543 }, { - "x": 1.189831523339715, - "y": 4.207476884117805, - "heading": -1.1279167268349728, - "angularVelocity": -2.2592413562696274, - "velocityX": -1.7934769332352274, - "velocityY": 0.15310216310975738, - "timestamp": 0.8136236472885412 + "x": 2.3520727474889376, + "y": 4.102267927018636, + "heading": 1.5391468007019663e-22, + "angularVelocity": -2.016341732469286e-15, + "velocityX": 0.9998839026192134, + "velocityY": 0.015236097436880057, + "timestamp": 0.5262034395381787 }, { - "x": 1.0571755920924437, - "y": 4.218801205996916, - "heading": -1.241183305817285, - "angularVelocity": -1.5313374592265028, - "velocityX": -1.7934769332023779, - "velocityY": 0.15310216349457723, - "timestamp": 0.8875894334056813 - }, - { - "x": 0.9245196595573416, - "y": 4.230125512789948, - "heading": -1.2896993930671263, - "angularVelocity": -0.6559260679391166, - "velocityX": -1.793476950613546, - "velocityY": 0.15310195953433603, - "timestamp": 0.9615552195228214 - }, - { - "x": 0.8072319603911625, - "y": 4.240137915362714, - "heading": -1.289699679914329, - "angularVelocity": -0.000003878106589948581, - "velocityX": -1.5857020566296687, - "velocityY": 0.13536532359582226, - "timestamp": 1.0355210056399615 - }, - { - "x": 0.7192661773418804, - "y": 4.24764721642263, - "heading": -1.289699851355636, - "angularVelocity": -0.000002317846073346953, - "velocityX": -1.1892766597514466, - "velocityY": 0.10152398093927968, - "timestamp": 1.1094867917571016 - }, - { - "x": 0.6606223194613006, - "y": 4.252653416877164, - "heading": -1.289699952984873, - "angularVelocity": -0.000001374003340703804, - "velocityX": -0.7928511404949471, - "velocityY": 0.06768265055151669, - "timestamp": 1.1834525778742417 - }, - { - "x": 0.6313003897666931, - "y": 4.255156517028809, - "heading": -1.2897, - "angularVelocity": -6.356334408172667e-7, - "velocityX": -0.3964255804456696, - "velocityY": 0.033841324253356724, - "timestamp": 1.2574183639913818 + "x": 2.41164231300354, + "y": 4.103175640106201, + "heading": 2.0916641305797265e-25, + "angularVelocity": -1.4605141481442386e-21, + "velocityX": 0.5660317002762608, + "velocityY": 0.008625115491089496, + "timestamp": 0.6314441274458145 }, { - "x": 0.6313003897666931, - "y": 4.255156517028809, - "heading": -1.2897, - "angularVelocity": 4.084009254517039e-24, - "velocityX": 9.66972044230157e-25, - "velocityY": -2.029977656808017e-25, - "timestamp": 1.331384150108522 + "x": 2.41164231300354, + "y": 4.103175640106201, + "heading": 2.1285305918257914e-25, + "angularVelocity": 3.5030688234893543e-26, + "velocityX": 9.550053625303018e-28, + "velocityY": 2.3969568427667223e-26, + "timestamp": 0.7366848153534502 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/2 piece right.3.traj b/src/main/deploy/choreo/2 piece right.3.traj index a3233a3..53cf2ea 100644 --- a/src/main/deploy/choreo/2 piece right.3.traj +++ b/src/main/deploy/choreo/2 piece right.3.traj @@ -1,211 +1,175 @@ { "samples": [ { - "x": 0.6313003897666931, - "y": 4.255156517028809, - "heading": -1.2897, - "angularVelocity": 4.084009254517039e-24, - "velocityX": 9.66972044230157e-25, - "velocityY": -2.029977656808017e-25, + "x": 2.41164231300354, + "y": 4.103175640106201, + "heading": 2.1285305918257914e-25, + "angularVelocity": 3.5030688234893543e-26, + "velocityX": 9.550053625303018e-28, + "velocityY": 2.3969568427667223e-26, "timestamp": 0 }, { - "x": 0.6646314231944946, - "y": 4.224442212318432, - "heading": -1.2896999897619994, - "angularVelocity": 1.1153252970301958e-7, - "velocityX": 0.3631074603784824, - "velocityY": -0.3346008819328531, - "timestamp": 0.09179385461554324 - }, - { - "x": 0.7312934878819572, - "y": 4.163013604898331, - "heading": -1.2896999663947442, - "angularVelocity": 2.545623060054011e-7, - "velocityX": 0.7262148971372956, - "velocityY": -0.6692017420706505, - "timestamp": 0.18358770923108647 - }, - { - "x": 0.8312865773246638, - "y": 4.070870700770457, - "heading": -1.289699921224478, - "angularVelocity": 4.920837706738393e-7, - "velocityX": 1.0893222630371484, - "velocityY": -1.0038025368233243, - "timestamp": 0.2753815638466297 - }, - { - "x": 0.9527932606026395, - "y": 3.958903176412677, - "heading": -1.2720972534731236, - "angularVelocity": 0.19176303059805994, - "velocityX": 1.3236908264379732, - "velocityY": -1.2197714632066494, - "timestamp": 0.36717541846217294 - }, - { - "x": 1.0742999439102807, - "y": 3.846935652087084, - "heading": -1.2219246048700179, - "angularVelocity": 0.5465796028856406, - "velocityX": 1.323690826761149, - "velocityY": -1.2197714628560048, - "timestamp": 0.4589692730777162 - }, - { - "x": 1.195806627217665, - "y": 3.7349681277612126, - "heading": -1.1466520807441116, - "angularVelocity": 0.8200170310002478, - "velocityX": 1.323690826758351, - "velocityY": -1.2197714628590413, - "timestamp": 0.5507631276932594 - }, - { - "x": 1.3173133105246166, - "y": 3.6230006034348716, - "heading": -1.0525810772100828, - "angularVelocity": 1.024807204447648, - "velocityX": 1.3236908267536351, - "velocityY": -1.2197714628641585, - "timestamp": 0.6425569823088026 - }, - { - "x": 1.4388199938313129, - "y": 3.5110330791082536, - "heading": -0.944987004768565, - "angularVelocity": 1.1721271853344641, - "velocityX": 1.3236908267508531, - "velocityY": -1.2197714628671776, - "timestamp": 0.7343508369243459 - }, - { - "x": 1.560326677138363, - "y": 3.3990655547820197, - "heading": -0.8283257581326825, - "angularVelocity": 1.2709047585428277, - "velocityX": 1.3236908267547107, - "velocityY": -1.2197714628629912, - "timestamp": 0.8261446915398891 - }, - { - "x": 1.6818333604464215, - "y": 3.28709803045688, - "heading": -0.706465158639104, - "angularVelocity": 1.3275463810073416, - "velocityX": 1.3236908267656953, - "velocityY": -1.219771462851071, - "timestamp": 0.9179385461554324 - }, - { - "x": 1.8033400437556388, - "y": 3.1751305061329975, - "heading": -0.5829200485324901, - "angularVelocity": 1.3458973982959206, - "velocityX": 1.3236908267783167, - "velocityY": -1.219771462837374, - "timestamp": 1.0097324007709756 - }, - { - "x": 1.924846727065488, - "y": 3.063162981809801, - "heading": -0.4610849728764882, - "angularVelocity": 1.3272683249470145, - "velocityX": 1.3236908267852028, - "velocityY": -1.2197714628299015, - "timestamp": 1.1015262553865188 - }, - { - "x": 2.0463534103751515, - "y": 2.951195457486403, - "heading": -0.3444658570370702, - "angularVelocity": 1.2704457866798307, - "velocityX": 1.3236908267831804, - "velocityY": -1.2197714628320961, - "timestamp": 1.193320110002062 - }, - { - "x": 2.1678600936840966, - "y": 2.8392279331622254, - "heading": -0.23692016972533708, - "angularVelocity": 1.1716000789177297, - "velocityX": 1.3236908267753509, - "velocityY": -1.2197714628405927, - "timestamp": 1.2851139646176053 - }, - { - "x": 2.289366776992324, - "y": 2.7272604088372687, - "heading": -0.14289670217852118, - "angularVelocity": 1.0242893485692561, - "velocityX": 1.3236908267675338, - "velocityY": -1.219771462849076, - "timestamp": 1.376907819233149 - }, - { - "x": 2.4108734603001536, - "y": 2.6152928845118804, - "heading": -0.06767016726185003, - "angularVelocity": 0.819516025683196, - "velocityX": 1.3236908267631986, - "velocityY": -1.2197714628537804, - "timestamp": 1.4687016738486927 - }, - { - "x": 2.5323801436078655, - "y": 2.5033253601863636, - "heading": -0.017545761093029383, - "angularVelocity": 0.5460540509901755, - "velocityX": 1.323690826761916, - "velocityY": -1.2197714628551724, - "timestamp": 1.5604955284642363 - }, - { - "x": 2.6538868269212, - "y": 2.391357835866955, - "heading": -7.879123548258675e-8, - "angularVelocity": 0.19114223250869905, - "velocityX": 1.3236908268231702, - "velocityY": -1.2197714627886354, - "timestamp": 1.65228938307978 - }, - { - "x": 2.7538799163461247, - "y": 2.2992149317197845, - "heading": -3.3611593837330055e-8, - "angularVelocity": 4.921859075913182e-7, - "velocityX": 1.0893222628434338, - "velocityY": -1.0038025370335428, - "timestamp": 1.7440832376953237 - }, - { - "x": 2.820541981025505, - "y": 2.2377863242909117, - "heading": -1.0239860234289194e-8, - "angularVelocity": 2.546110924410768e-7, - "velocityX": 0.7262148970492437, - "velocityY": -0.6692017421662043, - "timestamp": 1.8358770923108674 - }, - { - "x": 2.8538730144500732, - "y": 2.2070720195770264, - "heading": 1.5542259532589824e-23, - "angularVelocity": 1.1155278615519993e-7, - "velocityX": 0.36310746034326163, - "velocityY": -0.33460088197107457, - "timestamp": 1.927670946926411 - }, - { - "x": 2.8538730144500732, - "y": 2.2070720195770264, - "heading": 7.662197382375648e-24, - "angularVelocity": -2.3734134355543795e-24, - "velocityX": -9.29253035229337e-26, - "velocityY": 8.677134783852105e-26, - "timestamp": 2.0194648015419547 + "x": 2.3717259824669474, + "y": 4.107728459307588, + "heading": -1.1672418235553326e-8, + "angularVelocity": -1.35062850158138e-7, + "velocityX": -0.46187630200786284, + "velocityY": 0.052681177557626406, + "timestamp": 0.08642212289972195 + }, + { + "x": 2.2918933238426775, + "y": 4.116834097431042, + "heading": -3.822420718940779e-8, + "angularVelocity": -3.0723370432202035e-7, + "velocityX": -0.9237525756790543, + "velocityY": 0.10536235188320037, + "timestamp": 0.17284424579944369 + }, + { + "x": 2.172144344477514, + "y": 4.130492553638595, + "heading": -8.92762742154659e-8, + "angularVelocity": -5.907291479670698e-7, + "velocityX": -1.3856287643397973, + "velocityY": 0.1580435165125675, + "timestamp": 0.2592663686991654 + }, + { + "x": 2.051932793607985, + "y": 4.144203770317826, + "heading": -0.046276400034424556, + "angularVelocity": -0.5354683408072037, + "velocityX": -1.3909812306857334, + "velocityY": 0.15865401380084698, + "timestamp": 0.34568849159888715 + }, + { + "x": 1.9317212427384578, + "y": 4.157914986997074, + "heading": -0.1295981296882628, + "angularVelocity": -0.9641250047805334, + "velocityX": -1.3909812306857108, + "velocityY": 0.1586540138010495, + "timestamp": 0.4321106144986089 + }, + { + "x": 1.8115096918689613, + "y": 4.171626203676588, + "heading": -0.24101062814613916, + "angularVelocity": -1.289166416186646, + "velocityX": -1.3909812306853604, + "velocityY": 0.15865401380412056, + "timestamp": 0.5185327373983306 + }, + { + "x": 1.691298140999544, + "y": 4.185337420356798, + "heading": -0.371998449853314, + "angularVelocity": -1.5156746595910844, + "velocityX": -1.3909812306844416, + "velocityY": 0.15865401381217495, + "timestamp": 0.6049548602980523 + }, + { + "x": 1.5710865901302022, + "y": 4.199048637037669, + "heading": -0.5145015619975004, + "angularVelocity": -1.6489193665091622, + "velocityX": -1.3909812306835692, + "velocityY": 0.15865401381982447, + "timestamp": 0.6913769831977741 + }, + { + "x": 1.4508750392608447, + "y": 4.212759853718402, + "heading": -0.6607983544001016, + "angularVelocity": -1.6928164628905682, + "velocityX": -1.3909812306837512, + "velocityY": 0.15865401381822844, + "timestamp": 0.7777991060974958 + }, + { + "x": 1.3306634883913862, + "y": 4.22647107039825, + "heading": -0.8032937885994436, + "angularVelocity": -1.648830524155066, + "velocityX": -1.3909812306849194, + "velocityY": 0.15865401380798513, + "timestamp": 0.8642212289972175 + }, + { + "x": 1.2104519375218399, + "y": 4.240182287077329, + "heading": -0.9342710439721007, + "angularVelocity": -1.5155523953586953, + "velocityX": -1.3909812306859348, + "velocityY": 0.15865401379908228, + "timestamp": 0.9506433518969393 + }, + { + "x": 1.090240386652276, + "y": 4.253893503756253, + "heading": -1.0456752666575715, + "angularVelocity": -1.2890706563033194, + "velocityX": -1.3909812306861384, + "velocityY": 0.15865401379729932, + "timestamp": 1.037065474796661 + }, + { + "x": 0.9700288357827326, + "y": 4.267604720435357, + "heading": -1.1289928747720797, + "angularVelocity": -0.964077313990357, + "velocityX": -1.3909812306859015, + "velocityY": 0.15865401379937677, + "timestamp": 1.1234875976963827 + }, + { + "x": 0.849817284913202, + "y": 4.281315937114572, + "heading": -1.1752680781537357, + "angularVelocity": -0.5354555272305733, + "velocityX": -1.3909812306857543, + "velocityY": 0.15865401380066316, + "timestamp": 1.2099097205961045 + }, + { + "x": 0.7300683055480387, + "y": 4.294974393322129, + "heading": -1.1752681292079605, + "angularVelocity": -5.907541149820297e-7, + "velocityX": -1.385628764339792, + "velocityY": 0.15804351651261175, + "timestamp": 1.2963318434958262 + }, + { + "x": 0.650235646923769, + "y": 4.304080031445585, + "heading": -1.1752681557613822, + "angularVelocity": -3.072525969073027e-7, + "velocityX": -0.9237525756790519, + "velocityY": 0.10536235188322049, + "timestamp": 1.382753966395548 + }, + { + "x": 0.6103193163871765, + "y": 4.308632850646973, + "heading": -1.1752681674346586, + "angularVelocity": -1.350727780899982e-7, + "velocityX": -0.46187630200786184, + "velocityY": 0.05268117755763445, + "timestamp": 1.4691760892952697 + }, + { + "x": 0.6103193163871765, + "y": 4.308632850646973, + "heading": -1.1752681674346586, + "angularVelocity": -1.0539576719763061e-24, + "velocityX": -1.6445678469636073e-24, + "velocityY": 1.6880484103936767e-24, + "timestamp": 1.5555982121949914 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/2 piece right.4.traj b/src/main/deploy/choreo/2 piece right.4.traj new file mode 100644 index 0000000..e019fa5 --- /dev/null +++ b/src/main/deploy/choreo/2 piece right.4.traj @@ -0,0 +1,383 @@ +{ + "samples": [ + { + "x": 0.6103193163871765, + "y": 4.308632850646973, + "heading": -1.1752681674346586, + "angularVelocity": -1.0539576719763061e-24, + "velocityX": -1.6445678469636073e-24, + "velocityY": 1.6880484103936767e-24, + "timestamp": 0 + }, + { + "x": 0.6301200506356015, + "y": 4.288467589301025, + "heading": -1.1752681571276171, + "angularVelocity": 1.421971563292891e-7, + "velocityX": 0.27317326110924534, + "velocityY": -0.2782023198675527, + "timestamp": 0.07248415956972609 + }, + { + "x": 0.6697215183493939, + "y": 4.248137067408285, + "heading": -1.1752681353347625, + "angularVelocity": 3.006567885879182e-7, + "velocityX": 0.546346511415333, + "velocityY": -0.5564046287098577, + "timestamp": 0.14496831913945263 + }, + { + "x": 0.7291237182234573, + "y": 4.187641286300679, + "heading": -1.175268100091475, + "angularVelocity": 4.862205422910537e-7, + "velocityX": 0.8195197437161622, + "velocityY": -0.8346069191767541, + "timestamp": 0.21745247870917916 + }, + { + "x": 0.8083266476476012, + "y": 4.106980248642059, + "heading": -1.1752680474685175, + "angularVelocity": 7.259925178088886e-7, + "velocityX": 1.0926929400064915, + "velocityY": -1.1128091728928513, + "timestamp": 0.2899366382789057 + }, + { + "x": 0.9073302987912635, + "y": 4.006153962423965, + "heading": -1.1752679656781906, + "angularVelocity": 0.000001128388979975281, + "velocityX": 1.3658660282654653, + "velocityY": -1.3910113163566968, + "timestamp": 0.36242079784863224 + }, + { + "x": 1.0190560711374494, + "y": 3.8923713433209195, + "heading": -1.1633695164980866, + "angularVelocity": 0.1641524058599062, + "velocityX": 1.5413819103291277, + "velocityY": -1.5697584103681614, + "timestamp": 0.4349049574183588 + }, + { + "x": 1.1307818435783537, + "y": 3.7785887243108656, + "heading": -1.1293524727567825, + "angularVelocity": 0.46930314075837803, + "velocityX": 1.5413819116358685, + "velocityY": -1.5697584090852401, + "timestamp": 0.5073891169880853 + }, + { + "x": 1.2425076160184954, + "y": 3.6648061053000625, + "heading": -1.0777990352762379, + "angularVelocity": 0.7112372935903718, + "velocityX": 1.5413819116253513, + "velocityY": -1.5697584090955665, + "timestamp": 0.5798732765578118 + }, + { + "x": 1.3542333884574531, + "y": 3.5510234862880976, + "heading": -1.0126066866684242, + "angularVelocity": 0.8994013174023452, + "velocityX": 1.5413819116090188, + "velocityY": -1.5697584091116035, + "timestamp": 0.6523574361275384 + }, + { + "x": 1.4659591608957312, + "y": 3.437240867275465, + "heading": -0.9370316872968693, + "angularVelocity": 1.0426415898339216, + "velocityX": 1.5413819115996374, + "velocityY": -1.5697584091208152, + "timestamp": 0.7248415956972649 + }, + { + "x": 1.5776849333349647, + "y": 3.32345824826377, + "heading": -0.8537746458661194, + "angularVelocity": 1.1486239465970567, + "velocityX": 1.5413819116128218, + "velocityY": -1.569758409107869, + "timestamp": 0.7973257552669915 + }, + { + "x": 1.6894107057772814, + "y": 3.209675629255103, + "heading": -0.7650856064117465, + "angularVelocity": 1.2235644309162248, + "velocityX": 1.541381911655357, + "velocityY": -1.569758409066102, + "timestamp": 0.869809914836718 + }, + { + "x": 1.8011364782241641, + "y": 3.095893010250919, + "heading": -0.6728754035736392, + "angularVelocity": 1.2721428155541408, + "velocityX": 1.5413819117183474, + "velocityY": -1.5697584090042507, + "timestamp": 0.9422940744064445 + }, + { + "x": 1.912862250675531, + "y": 2.9821103912511386, + "heading": -0.5788183350553536, + "angularVelocity": 1.2976223919352636, + "velocityX": 1.5413819117802157, + "velocityY": -1.5697584089435004, + "timestamp": 1.014778233976171 + }, + { + "x": 2.0245880231296054, + "y": 2.8683277722540166, + "heading": -0.4844571570147463, + "angularVelocity": 1.3018179227122904, + "velocityX": 1.541381911817565, + "velocityY": -1.5697584089068264, + "timestamp": 1.0872623935458976 + }, + { + "x": 2.136313795583653, + "y": 2.7545451532568683, + "heading": -0.39129970033528816, + "angularVelocity": 1.2852112410828893, + "velocityX": 1.5413819118172043, + "velocityY": -1.5697584089071805, + "timestamp": 1.1597465531156241 + }, + { + "x": 2.2480395680351752, + "y": 2.6407625342572403, + "heading": -0.3009250355302803, + "angularVelocity": 1.2468195167258806, + "velocityX": 1.5413819117823546, + "velocityY": -1.5697584089414005, + "timestamp": 1.2322307126853507 + }, + { + "x": 2.3597653404829035, + "y": 2.5269799152538868, + "heading": -0.21509325739484195, + "angularVelocity": 1.1841453173347793, + "velocityX": 1.5413819117300178, + "velocityY": -1.5697584089927912, + "timestamp": 1.3047148722550772 + }, + { + "x": 2.471491112929133, + "y": 2.413197296249062, + "heading": -0.13585736651971517, + "angularVelocity": 1.0931476800652649, + "velocityX": 1.5413819117093417, + "velocityY": -1.5697584090130938, + "timestamp": 1.3771990318248037 + }, + { + "x": 2.5832171123561207, + "y": 2.299414900122313, + "heading": -0.06567656811151243, + "angularVelocity": 0.9682225582086267, + "velocityX": 1.541385043162603, + "velocityY": -1.5697553341609145, + "timestamp": 1.4496831913945303 + }, + { + "x": 2.707773208618164, + "y": 2.199841260910034, + "heading": 8.190882767180045e-23, + "angularVelocity": 0.9060816666893224, + "velocityX": 1.7183905697661621, + "velocityY": -1.3737296507727865, + "timestamp": 1.5221673509642568 + }, + { + "x": 2.9476739245436137, + "y": 2.102450103083525, + "heading": 0.10663557803360654, + "angularVelocity": 0.9060793794235005, + "velocityX": 2.03842934804067, + "velocityY": -0.8275298120198638, + "timestamp": 1.639856353114879 + }, + { + "x": 3.19386895017516, + "y": 2.0222915070712038, + "heading": 0.17351190219088491, + "angularVelocity": 0.5682461651912727, + "velocityX": 2.091911913030402, + "velocityY": -0.6811052396359981, + "timestamp": 1.7575453552655014 + }, + { + "x": 3.4400639885616346, + "y": 1.9421329502337281, + "heading": 0.209115544808741, + "angularVelocity": 0.30252310723383896, + "velocityX": 2.0919120214086484, + "velocityY": -0.6811049067684878, + "timestamp": 1.8752343574161237 + }, + { + "x": 3.6862590269481257, + "y": 1.861974393396304, + "heading": 0.2231570731239334, + "angularVelocity": 0.11931045432114051, + "velocityX": 2.091912021408791, + "velocityY": -0.6811049067680496, + "timestamp": 1.992923359566746 + }, + { + "x": 3.9324540653346163, + "y": 1.781815836558879, + "heading": 0.2223654503026519, + "angularVelocity": -0.006726395897794665, + "velocityX": 2.0919120214087887, + "velocityY": -0.6811049067680567, + "timestamp": 2.110612361717368 + }, + { + "x": 4.178649103721107, + "y": 1.7016572797214544, + "heading": 0.21145396988639198, + "angularVelocity": -0.09271452911373225, + "velocityX": 2.0919120214087896, + "velocityY": -0.6811049067680542, + "timestamp": 2.2283013638679905 + }, + { + "x": 4.424844142107599, + "y": 1.6214987228840316, + "heading": 0.1937295052437245, + "angularVelocity": -0.15060425629221624, + "velocityX": 2.091912021408795, + "velocityY": -0.6811049067680375, + "timestamp": 2.3459903660186128 + }, + { + "x": 4.671039180494092, + "y": 1.5413401660466126, + "heading": 0.1715372470156766, + "angularVelocity": -0.18856696736748246, + "velocityX": 2.0919120214088047, + "velocityY": -0.6811049067680072, + "timestamp": 2.463679368169235 + }, + { + "x": 4.917234218880585, + "y": 1.4611816092091974, + "heading": 0.14658987046095176, + "angularVelocity": -0.21197712699438517, + "velocityX": 2.0919120214088154, + "velocityY": -0.681104906767975, + "timestamp": 2.5813683703198573 + }, + { + "x": 5.16342925726708, + "y": 1.3810230523717848, + "heading": 0.12020702939394358, + "angularVelocity": -0.22417422685972488, + "velocityX": 2.091912021408823, + "velocityY": -0.6811049067679519, + "timestamp": 2.6990573724704796 + }, + { + "x": 5.409624295653576, + "y": 1.3008644955343727, + "heading": 0.0935025745426398, + "angularVelocity": -0.22690696975344068, + "velocityX": 2.091912021408825, + "velocityY": -0.6811049067679456, + "timestamp": 2.816746374621102 + }, + { + "x": 5.65581933404007, + "y": 1.2207059386969594, + "heading": 0.06754361372222109, + "angularVelocity": -0.2205725288349003, + "velocityX": 2.091912021408821, + "velocityY": -0.6811049067679572, + "timestamp": 2.934435376771724 + }, + { + "x": 5.902014372426565, + "y": 1.140547381859543, + "heading": 0.04350373821186817, + "angularVelocity": -0.20426611723316299, + "velocityX": 2.0919120214088127, + "velocityY": -0.6811049067679843, + "timestamp": 3.0521243789223464 + }, + { + "x": 6.148209410813057, + "y": 1.0603888250221227, + "heading": 0.022824509740632836, + "angularVelocity": -0.1757107978940074, + "velocityX": 2.0919120214088016, + "velocityY": -0.6811049067680177, + "timestamp": 3.1698133810729687 + }, + { + "x": 6.3944044491995475, + "y": 0.9802302681846989, + "heading": 0.0074310588512180266, + "angularVelocity": -0.13079770078867528, + "velocityX": 2.0919120214087927, + "velocityY": -0.681104906768045, + "timestamp": 3.287502383223591 + }, + { + "x": 6.640599487586059, + "y": 0.9000717113473415, + "heading": 1.8581771247773188e-8, + "angularVelocity": -0.06314133125146484, + "velocityX": 2.091912021408959, + "velocityY": -0.681104906767483, + "timestamp": 3.4051913853742133 + }, + { + "x": 6.853129975330638, + "y": 0.8308739838333334, + "heading": 8.159371436393743e-9, + "angularVelocity": -8.855882555653375e-8, + "velocityX": 1.8058653218299596, + "velocityY": -0.5879710614373866, + "timestamp": 3.5228803875248356 + }, + { + "x": 6.9948169750991465, + "y": 0.7847421629058822, + "heading": 2.531204760537721e-9, + "angularVelocity": -4.7822367196664074e-8, + "velocityX": 1.2039102820089622, + "velocityY": -0.3919807295877177, + "timestamp": 3.640569389675458 + }, + { + "x": 7.06566047668457, + "y": 0.7616762518882751, + "heading": -1.9444191856833368e-23, + "angularVelocity": -2.1507572621766622e-8, + "velocityX": 0.6019551554592679, + "velocityY": -0.19599036950017273, + "timestamp": 3.75825839182608 + }, + { + "x": 7.06566047668457, + "y": 0.7616762518882751, + "heading": -9.311643766094953e-24, + "angularVelocity": 6.975199973230743e-24, + "velocityX": 3.42127778712228e-24, + "velocityY": -1.1784054582051222e-24, + "timestamp": 3.8759473939767024 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece right.5.traj b/src/main/deploy/choreo/2 piece right.5.traj new file mode 100644 index 0000000..93ac904 --- /dev/null +++ b/src/main/deploy/choreo/2 piece right.5.traj @@ -0,0 +1,184 @@ +{ + "samples": [ + { + "x": 2.7077732086181636, + "y": 2.1998412609100346, + "heading": 2.735444848762468e-18, + "angularVelocity": -2.5364805221997074e-18, + "velocityX": -1.3365971515795454e-16, + "velocityY": 1.2755059167320944e-16, + "timestamp": 0 + }, + { + "x": 2.783994140884582, + "y": 2.1815396244139365, + "heading": -6.913548175875285e-18, + "angularVelocity": -7.993053002780074e-17, + "velocityX": 0.6314005512964709, + "velocityY": -0.15160747880743441, + "timestamp": 0.1207172406009338 + }, + { + "x": 2.9364360018279116, + "y": 2.144936352283628, + "heading": -1.1825996831406165e-17, + "angularVelocity": -4.069384481589834e-17, + "velocityX": 1.2628010728581114, + "velocityY": -0.30321495047514957, + "timestamp": 0.2414344812018676 + }, + { + "x": 3.165098780679637, + "y": 2.0900314471047694, + "heading": -1.8877982605670813e-17, + "angularVelocity": -5.841738710433222e-17, + "velocityX": 1.8942015052152972, + "velocityY": -0.45482240072371566, + "timestamp": 0.3621517218028014 + }, + { + "x": 3.4233367594479955, + "y": 2.028025152031049, + "heading": 1.3234683466552848e-14, + "angularVelocity": 1.0979012925727819e-13, + "velocityX": 2.139197164239702, + "velocityY": -0.513649042713796, + "timestamp": 0.4828689624037352 + }, + { + "x": 3.681574738216356, + "y": 1.9660188569573283, + "heading": 8.966413209720006e-15, + "angularVelocity": -3.5357586336345784e-14, + "velocityX": 2.139197164239714, + "velocityY": -0.5136490427137981, + "timestamp": 0.603586203004669 + }, + { + "x": 3.939812716984716, + "y": 1.9040125618836077, + "heading": 5.837294795235967e-14, + "angularVelocity": 4.092748848192075e-13, + "velocityX": 2.1391971642397136, + "velocityY": -0.513649042713798, + "timestamp": 0.7243034436056028 + }, + { + "x": 4.198050695753076, + "y": 1.8420062668098869, + "heading": 1.0674696317407068e-13, + "angularVelocity": 4.007216780378996e-13, + "velocityX": 2.139197164239714, + "velocityY": -0.513649042713798, + "timestamp": 0.8450206842065366 + }, + { + "x": 4.456288674521436, + "y": 1.7799999717361663, + "heading": 1.4572897454366185e-13, + "angularVelocity": 3.229200002877606e-13, + "velocityX": 2.139197164239714, + "velocityY": -0.513649042713798, + "timestamp": 0.9657379248074704 + }, + { + "x": 4.714526653289795, + "y": 1.7179936766624455, + "heading": 1.5144680025336213e-13, + "angularVelocity": 4.736544408434699e-14, + "velocityX": 2.139197164239714, + "velocityY": -0.513649042713798, + "timestamp": 1.0864551654084043 + }, + { + "x": 4.972764632058155, + "y": 1.655987381588725, + "heading": 1.2356361201484233e-13, + "angularVelocity": -2.309793373317404e-13, + "velocityX": 2.139197164239714, + "velocityY": -0.5136490427137981, + "timestamp": 1.207172406009338 + }, + { + "x": 5.231002610826515, + "y": 1.5939810865150041, + "heading": 8.481737461837235e-14, + "angularVelocity": -3.2096689092287344e-13, + "velocityX": 2.139197164239714, + "velocityY": -0.5136490427137981, + "timestamp": 1.3278896466102719 + }, + { + "x": 5.489240589594875, + "y": 1.5319747914412833, + "heading": 3.644351765856618e-14, + "angularVelocity": -4.0072036702462715e-13, + "velocityX": 2.139197164239714, + "velocityY": -0.513649042713798, + "timestamp": 1.4486068872112057 + }, + { + "x": 5.747478568363234, + "y": 1.4699684963675628, + "heading": 8.953330830931353e-15, + "angularVelocity": -2.27723784032737e-13, + "velocityX": 2.139197164239714, + "velocityY": -0.513649042713798, + "timestamp": 1.5693241278121395 + }, + { + "x": 6.005716547131594, + "y": 1.407962201293842, + "heading": -3.230680878301987e-16, + "angularVelocity": -7.684402718769493e-14, + "velocityX": 2.139197164239714, + "velocityY": -0.513649042713798, + "timestamp": 1.6900413684130733 + }, + { + "x": 6.263954525899953, + "y": 1.3459559062201216, + "heading": 2.5738532927961268e-17, + "angularVelocity": 2.88945157312828e-15, + "velocityX": 2.139197164239702, + "velocityY": -0.5136490427137953, + "timestamp": 1.810758609014007 + }, + { + "x": 6.492617304751678, + "y": 1.2910510010412626, + "heading": 1.5719519359178488e-17, + "angularVelocity": -8.299571394273679e-17, + "velocityX": 1.894201505215297, + "velocityY": -0.4548224007237162, + "timestamp": 1.931475849614941 + }, + { + "x": 6.645059165695007, + "y": 1.254447728910954, + "heading": 9.463618751509253e-18, + "angularVelocity": -5.18227601676043e-17, + "velocityX": 1.2628010728581116, + "velocityY": -0.3032149504751499, + "timestamp": 2.0521930902158747 + }, + { + "x": 6.721280097961426, + "y": 1.236146092414856, + "heading": -1.8630663507818504e-28, + "angularVelocity": -7.839492275233649e-17, + "velocityX": 0.6314005512964709, + "velocityY": -0.15160747880743458, + "timestamp": 2.1729103308168085 + }, + { + "x": 6.721280097961426, + "y": 1.236146092414856, + "heading": -8.648149721186173e-29, + "angularVelocity": 1.1053632926040662e-28, + "velocityX": -7.56285135271065e-18, + "velocityY": 1.8159388918179766e-18, + "timestamp": 2.2936275714177423 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece right.traj b/src/main/deploy/choreo/2 piece right.traj index 019d384..8e527a1 100644 --- a/src/main/deploy/choreo/2 piece right.traj +++ b/src/main/deploy/choreo/2 piece right.traj @@ -4,586 +4,757 @@ "x": 0.6313003897666931, "y": 4.255156517028809, "heading": -1.2897, - "angularVelocity": 2.5588458209070936e-23, - "velocityX": 3.6747187166411656e-23, - "velocityY": 2.9631382958424116e-23, + "angularVelocity": -1.3029252310464996e-24, + "velocityX": 9.818866312976863e-25, + "velocityY": -2.13204077766819e-25, "timestamp": 0 }, { - "x": 0.6442648165759174, - "y": 4.253481867543582, - "heading": -1.2896998999232763, - "angularVelocity": 0.000002030081537303308, - "velocityX": 0.26298666147739386, - "velocityY": -0.03397068638245913, - "timestamp": 0.0492968986959011 - }, - { - "x": 0.6701936695279935, - "y": 4.25013256816077, - "heading": -1.289699690051548, - "angularVelocity": 0.000004257300839893244, - "velocityX": 0.5259733094372531, - "velocityY": -0.06794138112974009, - "timestamp": 0.0985937973918022 - }, - { - "x": 0.7090869476233618, - "y": 4.245108618261832, - "heading": -1.2896993558074163, - "angularVelocity": 0.000006780226354565095, - "velocityX": 0.7889599371207936, - "velocityY": -0.10191208842422805, - "timestamp": 0.1478906960877033 - }, - { - "x": 0.7609446491960873, - "y": 4.238410016815877, - "heading": -1.2896988728952734, - "angularVelocity": 0.00000979599439019053, - "velocityX": 1.051946531010426, - "velocityY": -0.13588281663065846, - "timestamp": 0.1971875947836044 - }, - { - "x": 0.8257667709142923, - "y": 4.230036761761128, - "heading": -1.2896981927241198, - "angularVelocity": 0.000013797443073734933, - "velocityX": 1.3149330573120783, - "velocityY": -0.1698535866607121, - "timestamp": 0.2464844934795055 - }, - { - "x": 0.90355330278228, - "y": 4.219988846912366, - "heading": -1.2896971695227037, - "angularVelocity": 0.00002075589830370764, - "velocityX": 1.5779193808485028, - "velocityY": -0.20382448215952323, - "timestamp": 0.2957813921754066 - }, - { - "x": 0.9915565757190965, - "y": 4.208621348540917, - "heading": -1.2730220238110053, - "angularVelocity": 0.3382595285468722, - "velocityX": 1.7851685453822215, - "velocityY": -0.23059256610788906, - "timestamp": 0.3450782908713077 - }, - { - "x": 1.0795598357636118, - "y": 4.1972537503614955, - "heading": -1.2172146776466959, - "angularVelocity": 1.1320660658304207, - "velocityX": 1.7851682838586471, - "velocityY": -0.23059459073774355, - "timestamp": 0.3943751895672088 - }, - { - "x": 1.167563095807722, - "y": 4.1858861521789406, - "heading": -1.1235960458886458, - "angularVelocity": 1.8990775126759454, - "velocityX": 1.7851682838504348, - "velocityY": -0.23059459080130768, - "timestamp": 0.4436720882631099 - }, - { - "x": 1.2555663558538792, - "y": 4.17451855401223, - "heading": -0.9942697695783443, - "angularVelocity": 2.6234160714263077, - "velocityX": 1.7851682838919467, - "velocityY": -0.2305945904799162, - "timestamp": 0.492968986959011 - }, - { - "x": 1.343569615881345, - "y": 4.163150955700822, - "heading": -0.8330305095755579, - "angularVelocity": 3.270778979372043, - "velocityX": 1.7851682835127938, - "velocityY": -0.23059459341511745, - "timestamp": 0.5422658856549121 - }, - { - "x": 1.4315728758027453, - "y": 4.151783356568302, - "heading": -0.6481619169999266, - "angularVelocity": 3.750105939037553, - "velocityX": 1.7851682813612264, - "velocityY": -0.23059461007159965, - "timestamp": 0.5915627843508132 - }, - { - "x": 1.5195761357005462, - "y": 4.140415757253084, - "heading": -0.46092963892673033, - "angularVelocity": 3.798053894387562, - "velocityX": 1.785168280882508, - "velocityY": -0.2305946137776485, - "timestamp": 0.6408596830467144 - }, - { - "x": 1.607579395709073, - "y": 4.129048158795058, - "heading": -0.29529130470998255, - "angularVelocity": 3.3600153072209418, - "velocityX": 1.7851682831286084, - "velocityY": -0.23059459638932683, - "timestamp": 0.6901565817426155 - }, - { - "x": 1.6955826557496305, - "y": 4.117680560584999, - "heading": -0.16079446814860227, - "angularVelocity": 2.7283021877512823, - "velocityX": 1.785168283778364, - "velocityY": -0.23059459135922616, - "timestamp": 0.7394534804385167 - }, - { - "x": 1.7835859158077405, - "y": 4.106312962510822, - "heading": -0.06161125041031065, - "angularVelocity": 2.011956540108646, - "velocityX": 1.7851682841344167, - "velocityY": -0.23059458860283424, - "timestamp": 0.7887503791344178 + "x": 0.6559720855575579, + "y": 4.251969633511474, + "heading": -1.2896999477311053, + "angularVelocity": 7.68600953739464e-7, + "velocityX": 0.3627910812108772, + "velocityY": -0.04686232056148057, + "timestamp": 0.06800524342692997 + }, + { + "x": 0.7053154748885234, + "y": 4.245595866526154, + "heading": -1.2896998274887912, + "angularVelocity": 0.0000017681329843892469, + "velocityX": 0.7255821293248338, + "velocityY": -0.09372464039730581, + "timestamp": 0.13601048685385994 + }, + { + "x": 0.7793305510073084, + "y": 4.236035216220894, + "heading": -1.289699592159626, + "angularVelocity": 0.0000034604561825362674, + "velocityX": 1.0883730781481924, + "velocityY": -0.14058695805615118, + "timestamp": 0.2040157302807899 + }, + { + "x": 0.8737534106566409, + "y": 4.2238384675544545, + "heading": -1.2686933528400068, + "angularVelocity": 0.3088914657321989, + "velocityX": 1.3884644020249364, + "velocityY": -0.17935012142916007, + "timestamp": 0.2720209737077199 + }, + { + "x": 0.9681762696017908, + "y": 4.211641713436369, + "heading": -1.1852876077469632, + "angularVelocity": 1.2264605034854565, + "velocityX": 1.388464391670108, + "velocityY": -0.17935020159425882, + "timestamp": 0.34002621713464987 + }, + { + "x": 1.06259912854715, + "y": 4.199444959319902, + "heading": -1.0458164737621631, + "angularVelocity": 2.0508879456428724, + "velocityX": 1.3884643916731867, + "velocityY": -0.17935020157042256, + "timestamp": 0.40803146056157985 + }, + { + "x": 1.1570219874918148, + "y": 4.1872482051980615, + "heading": -0.8604729695452614, + "angularVelocity": 2.7254296121452617, + "velocityX": 1.388464391662975, + "velocityY": -0.17935020164947402, + "timestamp": 0.47603670398850984 + }, + { + "x": 1.2514448464329961, + "y": 4.175051451049253, + "heading": -0.6468064986930537, + "angularVelocity": 3.1419117127606087, + "velocityX": 1.3884643916117518, + "velocityY": -0.17935020204602378, + "timestamp": 0.5440419474154398 + }, + { + "x": 1.3458677053738775, + "y": 4.162854696898122, + "heading": -0.43285566722442825, + "angularVelocity": 3.1460931641030103, + "velocityX": 1.3884643916073398, + "velocityY": -0.17935020208018032, + "timestamp": 0.6120471908423697 + }, + { + "x": 1.4402905643180246, + "y": 4.1506579427722725, + "heading": -0.2468424865709028, + "angularVelocity": 2.735277035709639, + "velocityX": 1.3884643916553625, + "velocityY": -0.17935020170840796, + "timestamp": 0.6800524342692996 + }, + { + "x": 1.5347134232631718, + "y": 4.138461188654166, + "heading": -0.1064956083992093, + "angularVelocity": 2.0637655436450704, + "velocityX": 1.3884643916700699, + "velocityY": -0.17935020159455195, + "timestamp": 0.7480576776962296 + }, + { + "x": 1.6291362822082969, + "y": 4.126264434535885, + "heading": -0.022089749460474963, + "angularVelocity": 1.241166926038967, + "velocityX": 1.3884643916697392, + "velocityY": -0.17935020159711362, + "timestamp": 0.8160629211231595 + }, + { + "x": 1.7235591414801577, + "y": 4.114067682947199, + "heading": -4.084921018506283e-7, + "angularVelocity": 0.3248182030567626, + "velocityX": 1.3884643964743102, + "velocityY": -0.17935016440007143, + "timestamp": 0.8840681645500894 + }, + { + "x": 1.7975742177797642, + "y": 4.104507034041778, + "heading": -1.729250271854775e-7, + "angularVelocity": 0.000003463954583417705, + "velocityX": 1.0883730808071284, + "velocityY": -0.14058693747185988, + "timestamp": 0.9520734079770193 + }, + { + "x": 1.8469176071929212, + "y": 4.0981332676927495, + "heading": -5.242149315395944e-8, + "angularVelocity": 0.0000017719741590366667, + "velocityX": 0.7255821305334361, + "velocityY": -0.09372463104079504, + "timestamp": 1.0200786514039493 }, { "x": 1.8715893030166624, "y": 4.094946384429932, - "heading": 3.044635127032748e-22, - "angularVelocity": 1.2497997245298, - "velocityX": 1.7851708634206476, - "velocityY": -0.23057389778225362, - "timestamp": 0.838047277830319 - }, - { - "x": 2.001501484117773, - "y": 4.096925277087298, - "heading": 0.03742913022292678, - "angularVelocity": 0.5185396539200237, - "velocityX": 1.7997911526916002, - "velocityY": 0.027415392972907092, - "timestamp": 0.9102290867623309 - }, - { - "x": 2.1314136513515103, - "y": 4.098905079913163, - "heading": 0.0000021975515072904746, - "angularVelocity": -0.5185092092478852, - "velocityX": 1.7997909605743085, - "velocityY": 0.02742800236179506, - "timestamp": 0.9824108956943429 - }, - { - "x": 2.243505108146192, - "y": 4.10061330393091, - "heading": 0.0000012819688946605248, - "angularVelocity": -0.000012684395503198552, - "velocityX": 1.5529045122748328, - "velocityY": 0.023665575066908254, - "timestamp": 1.054592704626355 - }, - { - "x": 2.3275737085405206, - "y": 4.101894472003175, - "heading": 6.315287448797483e-7, - "angularVelocity": -0.00000901113673104848, - "velocityX": 1.1646784922432887, - "velocityY": 0.017749182116954924, - "timestamp": 1.126774513558367 - }, - { - "x": 2.3836194443971235, - "y": 4.102748584068442, - "heading": 2.0840831354227822e-7, - "angularVelocity": -0.000005861870706731764, - "velocityX": 0.7764523594773347, - "velocityY": 0.011832788314715953, - "timestamp": 1.1989563224903792 + "heading": -9.905348338146396e-24, + "angularVelocity": 7.708448718411703e-7, + "velocityX": 0.36279108169431745, + "velocityY": -0.046862316818874365, + "timestamp": 1.0880838948308793 }, { - "x": 2.41164231300354, - "y": 4.103175640106201, - "heading": -4.343161804381014e-22, - "angularVelocity": -0.000002887269197403718, - "velocityX": 0.3882261891332122, - "velocityY": 0.0059163942283817755, - "timestamp": 1.2711381314223913 + "x": 1.8715893030166624, + "y": 4.094946384429932, + "heading": -4.879834777552459e-24, + "angularVelocity": 2.1422164275294007e-24, + "velocityX": -3.536647328700017e-24, + "velocityY": 5.299012492424587e-25, + "timestamp": 1.1560891382578093 + }, + { + "x": 1.9311588685312648, + "y": 4.095854097517496, + "heading": 5.98134982599928e-18, + "angularVelocity": 5.683500198215718e-17, + "velocityX": 0.5660317002762608, + "velocityY": 0.008625115491089496, + "timestamp": 1.261329826165445 + }, + { + "x": 2.0363873382706825, + "y": 4.097457554892782, + "heading": 2.3583226342325868e-17, + "angularVelocity": 1.6725352965931608e-16, + "velocityX": 0.9998839026192134, + "velocityY": 0.015236097436880057, + "timestamp": 1.3665705140730808 + }, + { + "x": 2.1416158080101013, + "y": 4.099061012268066, + "heading": 3.485956605861461e-16, + "angularVelocity": 3.0882773640653748e-15, + "velocityX": 0.9998839026192254, + "velocityY": 0.01523609743688024, + "timestamp": 1.4718112019807166 + }, + { + "x": 2.24684427774952, + "y": 4.100664469643351, + "heading": 2.1220134489662188e-16, + "angularVelocity": -1.29602265436758e-15, + "velocityX": 0.9998839026192254, + "velocityY": 0.01523609743688024, + "timestamp": 1.5770518898883523 + }, + { + "x": 2.3520727474889376, + "y": 4.102267927018636, + "heading": 1.5391468007019663e-22, + "angularVelocity": -2.016341732469286e-15, + "velocityX": 0.9998839026192134, + "velocityY": 0.015236097436880057, + "timestamp": 1.682292577795988 }, { "x": 2.41164231300354, "y": 4.103175640106201, - "heading": -2.1484022653531463e-22, - "angularVelocity": 8.099300815455187e-23, - "velocityX": 1.2310180125110273e-23, - "velocityY": -5.1753556121623715e-23, - "timestamp": 1.3433199403544034 - }, - { - "x": 2.3823203832476643, - "y": 4.105678739540131, - "heading": -4.713122979131873e-8, - "angularVelocity": -6.372031214090865e-7, - "velocityX": -0.39642558127400507, - "velocityY": 0.03384131455001896, - "timestamp": 1.4172857264715435 - }, - { - "x": 2.3236765252241245, - "y": 4.110684938319997, - "heading": -1.489263961217733e-7, - "angularVelocity": -0.0000013762466631428888, - "velocityX": -0.7928511424277298, - "velocityY": 0.06768262791039739, - "timestamp": 1.4912515125886836 - }, - { - "x": 2.2357107419093456, - "y": 4.118194236269814, - "heading": -3.204509063199309e-7, - "angularVelocity": -0.000002318970962148814, - "velocityX": -1.1892766633409002, - "velocityY": 0.10152393889149473, - "timestamp": 1.5652172987058237 - }, - { - "x": 2.118423042232596, - "y": 4.128206632861628, - "heading": -6.069009309173134e-7, - "angularVelocity": -0.000003872736837322732, - "velocityX": -1.585702063532462, - "velocityY": 0.1353652427347487, - "timestamp": 1.6391830848229638 - }, - { - "x": 1.9857671108062405, - "y": 4.1395309526428035, - "heading": -0.05006393031110381, - "angularVelocity": -0.676844336256865, - "velocityX": -1.7934769356235545, - "velocityY": 0.15310213513098953, - "timestamp": 1.713148870940104 - }, - { - "x": 1.8531111795597293, - "y": 4.150855274530819, - "heading": -0.16469064728369365, - "angularVelocity": -1.5497262043704214, - "velocityX": -1.7934769331921003, - "velocityY": 0.1531021636149704, - "timestamp": 1.787114657057244 - }, - { - "x": 1.7204552483160596, - "y": 4.162179596452121, - "heading": -0.3328870431123415, - "angularVelocity": -2.2739756400651823, - "velocityX": -1.7934769331536837, - "velocityY": 0.15310216406498003, - "timestamp": 1.8610804431743841 - }, - { - "x": 1.5877993170796554, - "y": 4.173503918458529, - "heading": -0.5380010346158053, - "angularVelocity": -2.7730928348226245, - "velocityX": -1.793476933055458, - "velocityY": 0.15310216521560938, - "timestamp": 1.9350462292915243 - }, - { - "x": 1.455143385839801, - "y": 4.1848282404245225, - "heading": -0.7563600030365335, - "angularVelocity": -2.952161801875693, - "velocityX": -1.793476933102103, - "velocityY": 0.15310216466919027, - "timestamp": 2.0090120154086644 - }, - { - "x": 1.322487454589416, - "y": 4.196152562267158, - "heading": -0.9608101638901362, - "angularVelocity": -2.7641180008526347, - "velocityX": -1.7934769332444735, - "velocityY": 0.15310216300143226, - "timestamp": 2.0829778015258045 - }, - { - "x": 1.189831523339715, - "y": 4.207476884117805, - "heading": -1.1279167268349728, - "angularVelocity": -2.2592413562696274, - "velocityX": -1.7934769332352274, - "velocityY": 0.15310216310975738, - "timestamp": 2.1569435876429446 - }, - { - "x": 1.0571755920924437, - "y": 4.218801205996916, - "heading": -1.241183305817285, - "angularVelocity": -1.5313374592265028, - "velocityX": -1.7934769332023779, - "velocityY": 0.15310216349457723, - "timestamp": 2.2309093737600847 - }, - { - "x": 0.9245196595573416, - "y": 4.230125512789948, - "heading": -1.2896993930671263, - "angularVelocity": -0.6559260679391166, - "velocityX": -1.793476950613546, - "velocityY": 0.15310195953433603, - "timestamp": 2.304875159877225 - }, - { - "x": 0.8072319603911625, - "y": 4.240137915362714, - "heading": -1.289699679914329, - "angularVelocity": -0.000003878106589948581, - "velocityX": -1.5857020566296687, - "velocityY": 0.13536532359582226, - "timestamp": 2.378840945994365 - }, - { - "x": 0.7192661773418804, - "y": 4.24764721642263, - "heading": -1.289699851355636, - "angularVelocity": -0.000002317846073346953, - "velocityX": -1.1892766597514466, - "velocityY": 0.10152398093927968, - "timestamp": 2.452806732111505 - }, - { - "x": 0.6606223194613006, - "y": 4.252653416877164, - "heading": -1.289699952984873, - "angularVelocity": -0.000001374003340703804, - "velocityX": -0.7928511404949471, - "velocityY": 0.06768265055151669, - "timestamp": 2.526772518228645 - }, - { - "x": 0.6313003897666931, - "y": 4.255156517028809, - "heading": -1.2897, - "angularVelocity": -6.356334408172667e-7, - "velocityX": -0.3964255804456696, - "velocityY": 0.033841324253356724, - "timestamp": 2.6007383043457852 + "heading": 2.0916641305797265e-25, + "angularVelocity": -1.4605141481442386e-21, + "velocityX": 0.5660317002762608, + "velocityY": 0.008625115491089496, + "timestamp": 1.7875332657036238 }, { - "x": 0.6313003897666931, - "y": 4.255156517028809, - "heading": -1.2897, - "angularVelocity": 4.084009254517039e-24, - "velocityX": 9.66972044230157e-25, - "velocityY": -2.029977656808017e-25, - "timestamp": 2.6747040904629253 - }, - { - "x": 0.6646314231944946, - "y": 4.224442212318432, - "heading": -1.2896999897619994, - "angularVelocity": 1.1153252970301958e-7, - "velocityX": 0.3631074603784824, - "velocityY": -0.3346008819328531, - "timestamp": 2.7664979450784686 - }, - { - "x": 0.7312934878819572, - "y": 4.163013604898331, - "heading": -1.2896999663947442, - "angularVelocity": 2.545623060054011e-7, - "velocityX": 0.7262148971372956, - "velocityY": -0.6692017420706505, - "timestamp": 2.858291799694012 - }, - { - "x": 0.8312865773246638, - "y": 4.070870700770457, - "heading": -1.289699921224478, - "angularVelocity": 4.920837706738393e-7, - "velocityX": 1.0893222630371484, - "velocityY": -1.0038025368233243, - "timestamp": 2.950085654309555 - }, - { - "x": 0.9527932606026395, - "y": 3.958903176412677, - "heading": -1.2720972534731236, - "angularVelocity": 0.19176303059805994, - "velocityX": 1.3236908264379732, - "velocityY": -1.2197714632066494, - "timestamp": 3.0418795089250983 - }, - { - "x": 1.0742999439102807, - "y": 3.846935652087084, - "heading": -1.2219246048700179, - "angularVelocity": 0.5465796028856406, - "velocityX": 1.323690826761149, - "velocityY": -1.2197714628560048, - "timestamp": 3.1336733635406415 - }, - { - "x": 1.195806627217665, - "y": 3.7349681277612126, - "heading": -1.1466520807441116, - "angularVelocity": 0.8200170310002478, - "velocityX": 1.323690826758351, - "velocityY": -1.2197714628590413, - "timestamp": 3.2254672181561848 - }, - { - "x": 1.3173133105246166, - "y": 3.6230006034348716, - "heading": -1.0525810772100828, - "angularVelocity": 1.024807204447648, - "velocityX": 1.3236908267536351, - "velocityY": -1.2197714628641585, - "timestamp": 3.317261072771728 - }, - { - "x": 1.4388199938313129, - "y": 3.5110330791082536, - "heading": -0.944987004768565, - "angularVelocity": 1.1721271853344641, - "velocityX": 1.3236908267508531, - "velocityY": -1.2197714628671776, - "timestamp": 3.4090549273872712 - }, - { - "x": 1.560326677138363, - "y": 3.3990655547820197, - "heading": -0.8283257581326825, - "angularVelocity": 1.2709047585428277, - "velocityX": 1.3236908267547107, - "velocityY": -1.2197714628629912, - "timestamp": 3.5008487820028145 - }, - { - "x": 1.6818333604464215, - "y": 3.28709803045688, - "heading": -0.706465158639104, - "angularVelocity": 1.3275463810073416, - "velocityX": 1.3236908267656953, - "velocityY": -1.219771462851071, - "timestamp": 3.5926426366183577 - }, - { - "x": 1.8033400437556388, - "y": 3.1751305061329975, - "heading": -0.5829200485324901, - "angularVelocity": 1.3458973982959206, - "velocityX": 1.3236908267783167, - "velocityY": -1.219771462837374, - "timestamp": 3.684436491233901 - }, - { - "x": 1.924846727065488, - "y": 3.063162981809801, - "heading": -0.4610849728764882, - "angularVelocity": 1.3272683249470145, - "velocityX": 1.3236908267852028, - "velocityY": -1.2197714628299015, - "timestamp": 3.776230345849444 - }, - { - "x": 2.0463534103751515, - "y": 2.951195457486403, - "heading": -0.3444658570370702, - "angularVelocity": 1.2704457866798307, - "velocityX": 1.3236908267831804, - "velocityY": -1.2197714628320961, - "timestamp": 3.8680242004649874 - }, - { - "x": 2.1678600936840966, - "y": 2.8392279331622254, - "heading": -0.23692016972533708, - "angularVelocity": 1.1716000789177297, - "velocityX": 1.3236908267753509, - "velocityY": -1.2197714628405927, - "timestamp": 3.9598180550805306 - }, - { - "x": 2.289366776992324, - "y": 2.7272604088372687, - "heading": -0.14289670217852118, - "angularVelocity": 1.0242893485692561, - "velocityX": 1.3236908267675338, - "velocityY": -1.219771462849076, - "timestamp": 4.051611909696074 - }, - { - "x": 2.4108734603001536, - "y": 2.6152928845118804, - "heading": -0.06767016726185003, - "angularVelocity": 0.819516025683196, - "velocityX": 1.3236908267631986, - "velocityY": -1.2197714628537804, - "timestamp": 4.143405764311618 - }, - { - "x": 2.5323801436078655, - "y": 2.5033253601863636, - "heading": -0.017545761093029383, - "angularVelocity": 0.5460540509901755, - "velocityX": 1.323690826761916, - "velocityY": -1.2197714628551724, - "timestamp": 4.235199618927162 - }, - { - "x": 2.6538868269212, - "y": 2.391357835866955, - "heading": -7.879123548258675e-8, - "angularVelocity": 0.19114223250869905, - "velocityX": 1.3236908268231702, - "velocityY": -1.2197714627886354, - "timestamp": 4.326993473542705 - }, - { - "x": 2.7538799163461247, - "y": 2.2992149317197845, - "heading": -3.3611593837330055e-8, - "angularVelocity": 4.921859075913182e-7, - "velocityX": 1.0893222628434338, - "velocityY": -1.0038025370335428, - "timestamp": 4.418787328158249 - }, - { - "x": 2.820541981025505, - "y": 2.2377863242909117, - "heading": -1.0239860234289194e-8, - "angularVelocity": 2.546110924410768e-7, - "velocityX": 0.7262148970492437, - "velocityY": -0.6692017421662043, - "timestamp": 4.510581182773793 - }, - { - "x": 2.8538730144500732, - "y": 2.2070720195770264, - "heading": 1.5542259532589824e-23, - "angularVelocity": 1.1155278615519993e-7, - "velocityX": 0.36310746034326163, - "velocityY": -0.33460088197107457, - "timestamp": 4.602375037389336 - }, - { - "x": 2.8538730144500732, - "y": 2.2070720195770264, - "heading": 7.662197382375648e-24, - "angularVelocity": -2.3734134355543795e-24, - "velocityX": -9.29253035229337e-26, - "velocityY": 8.677134783852105e-26, - "timestamp": 4.69416889200488 + "x": 2.41164231300354, + "y": 4.103175640106201, + "heading": 2.1285305918257914e-25, + "angularVelocity": 3.5030688234893543e-26, + "velocityX": 9.550053625303018e-28, + "velocityY": 2.3969568427667223e-26, + "timestamp": 1.8927739536112596 + }, + { + "x": 2.3717259824669474, + "y": 4.107728459307588, + "heading": -1.1672418235553326e-8, + "angularVelocity": -1.35062850158138e-7, + "velocityX": -0.46187630200786284, + "velocityY": 0.052681177557626406, + "timestamp": 1.9791960765109815 + }, + { + "x": 2.2918933238426775, + "y": 4.116834097431042, + "heading": -3.822420718940779e-8, + "angularVelocity": -3.0723370432202035e-7, + "velocityX": -0.9237525756790543, + "velocityY": 0.10536235188320037, + "timestamp": 2.0656181994107032 + }, + { + "x": 2.172144344477514, + "y": 4.130492553638595, + "heading": -8.92762742154659e-8, + "angularVelocity": -5.907291479670698e-7, + "velocityX": -1.3856287643397973, + "velocityY": 0.1580435165125675, + "timestamp": 2.152040322310425 + }, + { + "x": 2.051932793607985, + "y": 4.144203770317826, + "heading": -0.046276400034424556, + "angularVelocity": -0.5354683408072037, + "velocityX": -1.3909812306857334, + "velocityY": 0.15865401380084698, + "timestamp": 2.2384624452101467 + }, + { + "x": 1.9317212427384578, + "y": 4.157914986997074, + "heading": -0.1295981296882628, + "angularVelocity": -0.9641250047805334, + "velocityX": -1.3909812306857108, + "velocityY": 0.1586540138010495, + "timestamp": 2.3248845681098684 + }, + { + "x": 1.8115096918689613, + "y": 4.171626203676588, + "heading": -0.24101062814613916, + "angularVelocity": -1.289166416186646, + "velocityX": -1.3909812306853604, + "velocityY": 0.15865401380412056, + "timestamp": 2.41130669100959 + }, + { + "x": 1.691298140999544, + "y": 4.185337420356798, + "heading": -0.371998449853314, + "angularVelocity": -1.5156746595910844, + "velocityX": -1.3909812306844416, + "velocityY": 0.15865401381217495, + "timestamp": 2.497728813909312 + }, + { + "x": 1.5710865901302022, + "y": 4.199048637037669, + "heading": -0.5145015619975004, + "angularVelocity": -1.6489193665091622, + "velocityX": -1.3909812306835692, + "velocityY": 0.15865401381982447, + "timestamp": 2.5841509368090336 + }, + { + "x": 1.4508750392608447, + "y": 4.212759853718402, + "heading": -0.6607983544001016, + "angularVelocity": -1.6928164628905682, + "velocityX": -1.3909812306837512, + "velocityY": 0.15865401381822844, + "timestamp": 2.6705730597087554 + }, + { + "x": 1.3306634883913862, + "y": 4.22647107039825, + "heading": -0.8032937885994436, + "angularVelocity": -1.648830524155066, + "velocityX": -1.3909812306849194, + "velocityY": 0.15865401380798513, + "timestamp": 2.756995182608477 + }, + { + "x": 1.2104519375218399, + "y": 4.240182287077329, + "heading": -0.9342710439721007, + "angularVelocity": -1.5155523953586953, + "velocityX": -1.3909812306859348, + "velocityY": 0.15865401379908228, + "timestamp": 2.843417305508199 + }, + { + "x": 1.090240386652276, + "y": 4.253893503756253, + "heading": -1.0456752666575715, + "angularVelocity": -1.2890706563033194, + "velocityX": -1.3909812306861384, + "velocityY": 0.15865401379729932, + "timestamp": 2.9298394284079206 + }, + { + "x": 0.9700288357827326, + "y": 4.267604720435357, + "heading": -1.1289928747720797, + "angularVelocity": -0.964077313990357, + "velocityX": -1.3909812306859015, + "velocityY": 0.15865401379937677, + "timestamp": 3.0162615513076423 + }, + { + "x": 0.849817284913202, + "y": 4.281315937114572, + "heading": -1.1752680781537357, + "angularVelocity": -0.5354555272305733, + "velocityX": -1.3909812306857543, + "velocityY": 0.15865401380066316, + "timestamp": 3.102683674207364 + }, + { + "x": 0.7300683055480387, + "y": 4.294974393322129, + "heading": -1.1752681292079605, + "angularVelocity": -5.907541149820297e-7, + "velocityX": -1.385628764339792, + "velocityY": 0.15804351651261175, + "timestamp": 3.1891057971070857 + }, + { + "x": 0.650235646923769, + "y": 4.304080031445585, + "heading": -1.1752681557613822, + "angularVelocity": -3.072525969073027e-7, + "velocityX": -0.9237525756790519, + "velocityY": 0.10536235188322049, + "timestamp": 3.2755279200068075 + }, + { + "x": 0.6103193163871765, + "y": 4.308632850646973, + "heading": -1.1752681674346586, + "angularVelocity": -1.350727780899982e-7, + "velocityX": -0.46187630200786184, + "velocityY": 0.05268117755763445, + "timestamp": 3.361950042906529 + }, + { + "x": 0.6103193163871765, + "y": 4.308632850646973, + "heading": -1.1752681674346586, + "angularVelocity": -1.0539576719763061e-24, + "velocityX": -1.6445678469636073e-24, + "velocityY": 1.6880484103936767e-24, + "timestamp": 3.448372165806251 + }, + { + "x": 0.6301200506356015, + "y": 4.288467589301025, + "heading": -1.1752681571276171, + "angularVelocity": 1.421971563292891e-7, + "velocityX": 0.27317326110924534, + "velocityY": -0.2782023198675527, + "timestamp": 3.520856325375977 + }, + { + "x": 0.6697215183493939, + "y": 4.248137067408285, + "heading": -1.1752681353347625, + "angularVelocity": 3.006567885879182e-7, + "velocityX": 0.546346511415333, + "velocityY": -0.5564046287098577, + "timestamp": 3.5933404849457036 + }, + { + "x": 0.7291237182234573, + "y": 4.187641286300679, + "heading": -1.175268100091475, + "angularVelocity": 4.862205422910537e-7, + "velocityX": 0.8195197437161622, + "velocityY": -0.8346069191767541, + "timestamp": 3.66582464451543 + }, + { + "x": 0.8083266476476012, + "y": 4.106980248642059, + "heading": -1.1752680474685175, + "angularVelocity": 7.259925178088886e-7, + "velocityX": 1.0926929400064915, + "velocityY": -1.1128091728928513, + "timestamp": 3.7383088040851566 + }, + { + "x": 0.9073302987912635, + "y": 4.006153962423965, + "heading": -1.1752679656781906, + "angularVelocity": 0.000001128388979975281, + "velocityX": 1.3658660282654653, + "velocityY": -1.3910113163566968, + "timestamp": 3.810792963654883 + }, + { + "x": 1.0190560711374494, + "y": 3.8923713433209195, + "heading": -1.1633695164980866, + "angularVelocity": 0.1641524058599062, + "velocityX": 1.5413819103291277, + "velocityY": -1.5697584103681614, + "timestamp": 3.8832771232246097 + }, + { + "x": 1.1307818435783537, + "y": 3.7785887243108656, + "heading": -1.1293524727567825, + "angularVelocity": 0.46930314075837803, + "velocityX": 1.5413819116358685, + "velocityY": -1.5697584090852401, + "timestamp": 3.9557612827943363 + }, + { + "x": 1.2425076160184954, + "y": 3.6648061053000625, + "heading": -1.0777990352762379, + "angularVelocity": 0.7112372935903718, + "velocityX": 1.5413819116253513, + "velocityY": -1.5697584090955665, + "timestamp": 4.028245442364063 + }, + { + "x": 1.3542333884574531, + "y": 3.5510234862880976, + "heading": -1.0126066866684242, + "angularVelocity": 0.8994013174023452, + "velocityX": 1.5413819116090188, + "velocityY": -1.5697584091116035, + "timestamp": 4.100729601933789 + }, + { + "x": 1.4659591608957312, + "y": 3.437240867275465, + "heading": -0.9370316872968693, + "angularVelocity": 1.0426415898339216, + "velocityX": 1.5413819115996374, + "velocityY": -1.5697584091208152, + "timestamp": 4.173213761503516 + }, + { + "x": 1.5776849333349647, + "y": 3.32345824826377, + "heading": -0.8537746458661194, + "angularVelocity": 1.1486239465970567, + "velocityX": 1.5413819116128218, + "velocityY": -1.569758409107869, + "timestamp": 4.245697921073242 + }, + { + "x": 1.6894107057772814, + "y": 3.209675629255103, + "heading": -0.7650856064117465, + "angularVelocity": 1.2235644309162248, + "velocityX": 1.541381911655357, + "velocityY": -1.569758409066102, + "timestamp": 4.318182080642969 + }, + { + "x": 1.8011364782241641, + "y": 3.095893010250919, + "heading": -0.6728754035736392, + "angularVelocity": 1.2721428155541408, + "velocityX": 1.5413819117183474, + "velocityY": -1.5697584090042507, + "timestamp": 4.3906662402126955 + }, + { + "x": 1.912862250675531, + "y": 2.9821103912511386, + "heading": -0.5788183350553536, + "angularVelocity": 1.2976223919352636, + "velocityX": 1.5413819117802157, + "velocityY": -1.5697584089435004, + "timestamp": 4.463150399782422 + }, + { + "x": 2.0245880231296054, + "y": 2.8683277722540166, + "heading": -0.4844571570147463, + "angularVelocity": 1.3018179227122904, + "velocityX": 1.541381911817565, + "velocityY": -1.5697584089068264, + "timestamp": 4.5356345593521485 + }, + { + "x": 2.136313795583653, + "y": 2.7545451532568683, + "heading": -0.39129970033528816, + "angularVelocity": 1.2852112410828893, + "velocityX": 1.5413819118172043, + "velocityY": -1.5697584089071805, + "timestamp": 4.608118718921875 + }, + { + "x": 2.2480395680351752, + "y": 2.6407625342572403, + "heading": -0.3009250355302803, + "angularVelocity": 1.2468195167258806, + "velocityX": 1.5413819117823546, + "velocityY": -1.5697584089414005, + "timestamp": 4.680602878491602 + }, + { + "x": 2.3597653404829035, + "y": 2.5269799152538868, + "heading": -0.21509325739484195, + "angularVelocity": 1.1841453173347793, + "velocityX": 1.5413819117300178, + "velocityY": -1.5697584089927912, + "timestamp": 4.753087038061328 + }, + { + "x": 2.471491112929133, + "y": 2.413197296249062, + "heading": -0.13585736651971517, + "angularVelocity": 1.0931476800652649, + "velocityX": 1.5413819117093417, + "velocityY": -1.5697584090130938, + "timestamp": 4.825571197631055 + }, + { + "x": 2.5832171123561207, + "y": 2.299414900122313, + "heading": -0.06567656811151243, + "angularVelocity": 0.9682225582086267, + "velocityX": 1.541385043162603, + "velocityY": -1.5697553341609145, + "timestamp": 4.898055357200781 + }, + { + "x": 2.707773208618164, + "y": 2.199841260910034, + "heading": 8.190882767180045e-23, + "angularVelocity": 0.9060816666893224, + "velocityX": 1.7183905697661621, + "velocityY": -1.3737296507727865, + "timestamp": 4.970539516770508 + }, + { + "x": 2.9476739245436137, + "y": 2.102450103083525, + "heading": 0.10663557803360654, + "angularVelocity": 0.9060793794235005, + "velocityX": 2.03842934804067, + "velocityY": -0.8275298120198638, + "timestamp": 5.08822851892113 + }, + { + "x": 3.19386895017516, + "y": 2.0222915070712038, + "heading": 0.17351190219088491, + "angularVelocity": 0.5682461651912727, + "velocityX": 2.091911913030402, + "velocityY": -0.6811052396359981, + "timestamp": 5.205917521071752 + }, + { + "x": 3.4400639885616346, + "y": 1.9421329502337281, + "heading": 0.209115544808741, + "angularVelocity": 0.30252310723383896, + "velocityX": 2.0919120214086484, + "velocityY": -0.6811049067684878, + "timestamp": 5.323606523222375 + }, + { + "x": 3.6862590269481257, + "y": 1.861974393396304, + "heading": 0.2231570731239334, + "angularVelocity": 0.11931045432114051, + "velocityX": 2.091912021408791, + "velocityY": -0.6811049067680496, + "timestamp": 5.441295525372997 + }, + { + "x": 3.9324540653346163, + "y": 1.781815836558879, + "heading": 0.2223654503026519, + "angularVelocity": -0.006726395897794665, + "velocityX": 2.0919120214087887, + "velocityY": -0.6811049067680567, + "timestamp": 5.558984527523619 + }, + { + "x": 4.178649103721107, + "y": 1.7016572797214544, + "heading": 0.21145396988639198, + "angularVelocity": -0.09271452911373225, + "velocityX": 2.0919120214087896, + "velocityY": -0.6811049067680542, + "timestamp": 5.676673529674241 + }, + { + "x": 4.424844142107599, + "y": 1.6214987228840316, + "heading": 0.1937295052437245, + "angularVelocity": -0.15060425629221624, + "velocityX": 2.091912021408795, + "velocityY": -0.6811049067680375, + "timestamp": 5.794362531824864 + }, + { + "x": 4.671039180494092, + "y": 1.5413401660466126, + "heading": 0.1715372470156766, + "angularVelocity": -0.18856696736748246, + "velocityX": 2.0919120214088047, + "velocityY": -0.6811049067680072, + "timestamp": 5.912051533975486 + }, + { + "x": 4.917234218880585, + "y": 1.4611816092091974, + "heading": 0.14658987046095176, + "angularVelocity": -0.21197712699438517, + "velocityX": 2.0919120214088154, + "velocityY": -0.681104906767975, + "timestamp": 6.029740536126108 + }, + { + "x": 5.16342925726708, + "y": 1.3810230523717848, + "heading": 0.12020702939394358, + "angularVelocity": -0.22417422685972488, + "velocityX": 2.091912021408823, + "velocityY": -0.6811049067679519, + "timestamp": 6.1474295382767306 + }, + { + "x": 5.409624295653576, + "y": 1.3008644955343727, + "heading": 0.0935025745426398, + "angularVelocity": -0.22690696975344068, + "velocityX": 2.091912021408825, + "velocityY": -0.6811049067679456, + "timestamp": 6.265118540427353 + }, + { + "x": 5.65581933404007, + "y": 1.2207059386969594, + "heading": 0.06754361372222109, + "angularVelocity": -0.2205725288349003, + "velocityX": 2.091912021408821, + "velocityY": -0.6811049067679572, + "timestamp": 6.382807542577975 + }, + { + "x": 5.902014372426565, + "y": 1.140547381859543, + "heading": 0.04350373821186817, + "angularVelocity": -0.20426611723316299, + "velocityX": 2.0919120214088127, + "velocityY": -0.6811049067679843, + "timestamp": 6.500496544728597 + }, + { + "x": 6.148209410813057, + "y": 1.0603888250221227, + "heading": 0.022824509740632836, + "angularVelocity": -0.1757107978940074, + "velocityX": 2.0919120214088016, + "velocityY": -0.6811049067680177, + "timestamp": 6.61818554687922 + }, + { + "x": 6.3944044491995475, + "y": 0.9802302681846989, + "heading": 0.0074310588512180266, + "angularVelocity": -0.13079770078867528, + "velocityX": 2.0919120214087927, + "velocityY": -0.681104906768045, + "timestamp": 6.735874549029842 + }, + { + "x": 6.640599487586059, + "y": 0.9000717113473415, + "heading": 1.8581771247773188e-8, + "angularVelocity": -0.06314133125146484, + "velocityX": 2.091912021408959, + "velocityY": -0.681104906767483, + "timestamp": 6.853563551180464 + }, + { + "x": 6.853129975330638, + "y": 0.8308739838333334, + "heading": 8.159371436393743e-9, + "angularVelocity": -8.855882555653375e-8, + "velocityX": 1.8058653218299596, + "velocityY": -0.5879710614373866, + "timestamp": 6.9712525533310865 + }, + { + "x": 6.9948169750991465, + "y": 0.7847421629058822, + "heading": 2.531204760537721e-9, + "angularVelocity": -4.7822367196664074e-8, + "velocityX": 1.2039102820089622, + "velocityY": -0.3919807295877177, + "timestamp": 7.088941555481709 + }, + { + "x": 7.06566047668457, + "y": 0.7616762518882751, + "heading": -1.9444191856833368e-23, + "angularVelocity": -2.1507572621766622e-8, + "velocityX": 0.6019551554592679, + "velocityY": -0.19599036950017273, + "timestamp": 7.206630557632331 + }, + { + "x": 7.06566047668457, + "y": 0.7616762518882751, + "heading": -9.311643766094953e-24, + "angularVelocity": 6.975199973230743e-24, + "velocityX": 3.42127778712228e-24, + "velocityY": -1.1784054582051222e-24, + "timestamp": 7.324319559782953 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/exit center.1.traj b/src/main/deploy/choreo/exit center.1.traj index 53d22c6..9ae7f75 100644 --- a/src/main/deploy/choreo/exit center.1.traj +++ b/src/main/deploy/choreo/exit center.1.traj @@ -3,146 +3,290 @@ { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": -8.351489873798432e-33, - "angularVelocity": -9.52467798814217e-33, - "velocityX": 5.856461918837007e-19, - "velocityY": 3.2988456083555626e-20, + "heading": 2.4064931720572804e-32, + "angularVelocity": 5.341037702942834e-32, + "velocityX": -1.8920825597869374e-21, + "velocityY": 5.007478148860853e-21, "timestamp": 0 }, { - "x": 1.3974808893772463, - "y": 5.571866984328364, - "heading": -4.619528479929238e-22, - "angularVelocity": -3.675562971006093e-21, - "velocityX": 0.6749835681204585, - "velocityY": 0.03802713189750964, - "timestamp": 0.12568221137930363 - }, - { - "x": 1.5671477356384225, - "y": 5.581425651896191, - "heading": -1.730475779700296e-21, - "angularVelocity": -1.009309860545018e-20, - "velocityX": 1.3499670669315913, - "velocityY": 0.07605425989027997, - "timestamp": 0.25136442275860726 - }, - { - "x": 1.7930175454481574, - "y": 5.594150674333725, - "heading": -1.411814827850765e-19, - "angularVelocity": -1.109552445453708e-18, - "velocityX": 1.797150188009124, - "velocityY": 0.10124760137399545, - "timestamp": 0.3770466341379109 - }, - { - "x": 2.018887355257855, - "y": 5.606875696771257, - "heading": 6.884754478081912e-20, - "angularVelocity": 1.671111824496415e-18, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399623, - "timestamp": 0.5027288455172145 - }, - { - "x": 2.2447571650676026, - "y": 5.619600719208795, - "heading": 2.509060822170839e-19, - "angularVelocity": 1.4485625091677524e-18, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399623, - "timestamp": 0.6284110568965182 - }, - { - "x": 2.470626974877369, - "y": 5.632325741646326, - "heading": 2.991709519339792e-19, - "angularVelocity": 3.84023092969776e-19, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399623, - "timestamp": 0.7540932682758219 - }, - { - "x": 2.6964967846870462, - "y": 5.645050764083861, - "heading": 3.625534697671079e-19, - "angularVelocity": 5.043077983998515e-19, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399622, - "timestamp": 0.8797754796551256 - }, - { - "x": 2.9223665944967436, - "y": 5.657775786521395, - "heading": 7.427083243993144e-19, - "angularVelocity": 3.0247307984331e-18, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399622, - "timestamp": 1.0054576910344293 - }, - { - "x": 3.1482364043064752, - "y": 5.6705008089589315, - "heading": 7.718223822590269e-19, - "angularVelocity": 2.3164819703884333e-19, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399624, - "timestamp": 1.131139902413733 - }, - { - "x": 3.3741062141162126, - "y": 5.683225831396466, - "heading": 6.75954597848925e-19, - "angularVelocity": -7.627792740852862e-19, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399623, - "timestamp": 1.2568221137930367 - }, - { - "x": 3.5999760239259384, - "y": 5.695950853833998, - "heading": 5.558602175529875e-19, - "angularVelocity": -9.555400262857742e-19, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399623, - "timestamp": 1.3825043251723403 - }, - { - "x": 3.8258458337356256, - "y": 5.708675876271534, - "heading": 1.467111215234519e-21, - "angularVelocity": -4.4110706322056545e-18, - "velocityX": 1.797150188009124, - "velocityY": 0.10124760137399544, - "timestamp": 1.508186536551644 - }, - { - "x": 3.99551267999683, - "y": 5.718234543839361, - "heading": 3.9980033222945746e-22, - "angularVelocity": -8.492139436872755e-21, - "velocityX": 1.3499670669315909, - "velocityY": 0.07605425989027999, - "timestamp": 1.6338687479309477 - }, - { - "x": 4.08034610748291, - "y": 5.723013877868652, - "heading": 2.0668023676312887e-32, - "angularVelocity": -3.1810414607342227e-21, - "velocityX": 0.6749835681204581, - "velocityY": 0.03802713189750965, - "timestamp": 1.7595509593102514 - }, - { - "x": 4.08034610748291, - "y": 5.723013877868652, - "heading": 9.734730637251588e-33, - "angularVelocity": -9.536452132221093e-33, - "velocityX": 4.231540995901582e-19, - "velocityY": 2.3828735253686786e-20, - "timestamp": 1.885233170689555 + "x": 1.3458512299446481, + "y": 5.579396056710049, + "heading": 3.413187474556536e-18, + "angularVelocity": 4.206691136177212e-17, + "velocityX": 0.409230368386038, + "velocityY": 0.15169885784339893, + "timestamp": 0.08113710667276726 + }, + { + "x": 1.412258764942075, + "y": 5.6040128691207105, + "heading": 6.827483140754657e-18, + "angularVelocity": 4.208056962106495e-17, + "velocityX": 0.8184607230974358, + "velocityY": 0.3033977106177016, + "timestamp": 0.16227421334553452 + }, + { + "x": 1.511870065034253, + "y": 5.64093808684557, + "heading": 7.957629289102601e-18, + "angularVelocity": 1.3928844577901407e-17, + "velocityX": 1.2276910550177553, + "velocityY": 0.45509655494350654, + "timestamp": 0.2434113200183018 + }, + { + "x": 1.644685126522774, + "y": 5.690171708513654, + "heading": 1.0892134038406229e-17, + "angularVelocity": 3.6167234322702574e-17, + "velocityX": 1.6369213413558763, + "velocityY": 0.6067953823723014, + "timestamp": 0.32454842669106904 + }, + { + "x": 1.810703938312389, + "y": 5.75171373001203, + "heading": 1.3242101695542206e-17, + "angularVelocity": 2.896292157187237e-17, + "velocityX": 2.0461514909470804, + "velocityY": 0.7584941591099562, + "timestamp": 0.4056855333638363 + }, + { + "x": 1.9780760222264302, + "y": 5.813757400096333, + "heading": 1.125270337500381e-16, + "angularVelocity": 1.22366862864508e-15, + "velocityX": 2.062830322371079, + "velocityY": 0.7646768861814419, + "timestamp": 0.4868226400366035 + }, + { + "x": 2.145448106140464, + "y": 5.875801070180656, + "heading": 2.6117964956039346e-16, + "angularVelocity": 1.832116301729673e-15, + "velocityX": 2.0628303223709894, + "velocityY": 0.7646768861816947, + "timestamp": 0.5679597467093708 + }, + { + "x": 2.312820190054498, + "y": 5.937844740264981, + "heading": 3.764540638551346e-16, + "angularVelocity": 1.4207360728261472e-15, + "velocityX": 2.0628303223709845, + "velocityY": 0.7646768861817085, + "timestamp": 0.6490968533821381 + }, + { + "x": 2.4801922739685316, + "y": 5.999888410349303, + "heading": 4.151881500968984e-16, + "angularVelocity": 4.773905285775699e-16, + "velocityX": 2.0628303223709925, + "velocityY": 0.7646768861816863, + "timestamp": 0.7302339600549054 + }, + { + "x": 2.6475643578824943, + "y": 6.061932080433819, + "heading": 3.881070204693485e-16, + "angularVelocity": -3.337699695993216e-16, + "velocityX": 2.06283032237011, + "velocityY": 0.7646768861840669, + "timestamp": 0.8113710667276727 + }, + { + "x": 2.8149364417978595, + "y": 6.123975750514552, + "heading": 2.850633506870914e-16, + "angularVelocity": -1.2699943836774666e-15, + "velocityX": 2.0628303223873994, + "velocityY": 0.7646768861374271, + "timestamp": 0.89250817340044 + }, + { + "x": 2.982308527462817, + "y": 6.186019415875497, + "heading": 1.5426652083783285e-16, + "angularVelocity": -1.6120470055306626e-15, + "velocityX": 2.062830343950803, + "velocityY": 0.7646768279669163, + "timestamp": 0.9736452800732073 + }, + { + "x": 3.1499392986297607, + "y": 6.247360706329346, + "heading": -2.6221041253671193e-33, + "angularVelocity": -1.901306654426338e-15, + "velocityX": 2.0660185954499513, + "velocityY": 0.7560201856992955, + "timestamp": 1.0547823867459745 + }, + { + "x": 3.395592203319553, + "y": 6.266105388578719, + "heading": -2.25016120284868e-16, + "angularVelocity": -2.0093413357633876e-15, + "velocityX": 2.193622994737605, + "velocityY": 0.16738562917950292, + "timestamp": 1.1667674020740877 + }, + { + "x": 3.6412951619585874, + "y": 6.284182136410596, + "heading": -5.420070367092222e-16, + "angularVelocity": -2.830654757652916e-15, + "velocityX": 2.1940699647995836, + "velocityY": 0.16142113102287564, + "timestamp": 1.278752417402201 + }, + { + "x": 3.8869981207102944, + "y": 6.302258882711007, + "heading": -1.2457261466691004e-15, + "angularVelocity": -6.28404709235429e-15, + "velocityX": 2.1940699658057228, + "velocityY": 0.1614211173472237, + "timestamp": 1.3907374327303141 + }, + { + "x": 4.132701079462025, + "y": 6.320335629011082, + "heading": -2.1563980313879527e-15, + "angularVelocity": -8.132086976553131e-15, + "velocityX": 2.194069965805942, + "velocityY": 0.16142111734423847, + "timestamp": 1.5027224480584274 + }, + { + "x": 4.378404038213756, + "y": 6.338412375311165, + "heading": -3.277495117328277e-15, + "angularVelocity": -1.0011134817060459e-14, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734430015, + "timestamp": 1.6147074633865406 + }, + { + "x": 4.624106996965487, + "y": 6.356489121611248, + "heading": -4.357861685209099e-15, + "angularVelocity": -9.647420815324042e-15, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429992, + "timestamp": 1.7266924787146538 + }, + { + "x": 4.869809955717218, + "y": 6.374565867911331, + "heading": -4.984271791839074e-15, + "angularVelocity": -5.593695770765471e-15, + "velocityX": 2.1940699658059377, + "velocityY": 0.1614211173442999, + "timestamp": 1.838677494042767 + }, + { + "x": 5.115512914468949, + "y": 6.392642614211413, + "heading": -5.0340664077739056e-15, + "angularVelocity": -4.446542761898409e-16, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429992, + "timestamp": 1.9506625093708803 + }, + { + "x": 5.36121587322068, + "y": 6.410719360511496, + "heading": -4.508040648188578e-15, + "angularVelocity": 4.6972870258050625e-15, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429992, + "timestamp": 2.0626475246989937 + }, + { + "x": 5.606918831972411, + "y": 6.428796106811578, + "heading": -3.510023344735256e-15, + "angularVelocity": 8.91206114076206e-15, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429992, + "timestamp": 2.174632540027107 + }, + { + "x": 5.852621790724142, + "y": 6.446872853111661, + "heading": -2.3215831189941424e-15, + "angularVelocity": 1.0612493307778848e-14, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429995, + "timestamp": 2.28661755535522 + }, + { + "x": 6.098324749475873, + "y": 6.464949599411743, + "heading": -1.250017912777245e-15, + "angularVelocity": 9.568826713799511e-15, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429923, + "timestamp": 2.3986025706833334 + }, + { + "x": 6.344027708227604, + "y": 6.48302634571183, + "heading": -4.511696357394538e-16, + "angularVelocity": 7.133528309097315e-15, + "velocityX": 2.194069965805935, + "velocityY": 0.16142111734433204, + "timestamp": 2.5105875860114466 + }, + { + "x": 6.589730666979331, + "y": 6.501103092011914, + "heading": -4.711917488169644e-19, + "angularVelocity": 4.024631712288509e-15, + "velocityX": 2.1940699658059026, + "velocityY": 0.16142111734431822, + "timestamp": 2.62257260133956 + }, + { + "x": 6.791556314990098, + "y": 6.515951716767905, + "heading": 1.0824486990110142e-21, + "angularVelocity": 4.2172981459668045e-18, + "velocityX": 1.8022558412786114, + "velocityY": 0.13259474682827085, + "timestamp": 2.734557616667673 + }, + { + "x": 6.926106755210862, + "y": 6.525850800542852, + "heading": 2.72309248565345e-22, + "angularVelocity": -7.234355816903654e-21, + "velocityX": 1.2015039675311439, + "velocityY": 0.08839650328164239, + "timestamp": 2.8465426319957863 + }, + { + "x": 6.993381977081299, + "y": 6.5308003425598145, + "heading": -3.4836420737679767e-34, + "angularVelocity": -2.4316579121587528e-21, + "velocityX": 0.6007519994824513, + "velocityY": 0.04419825279713492, + "timestamp": 2.9585276473238995 + }, + { + "x": 6.993381977081299, + "y": 6.5308003425598145, + "heading": -6.944682501042333e-34, + "angularVelocity": -9.29206724482002e-33, + "velocityX": -5.0941360422492845e-22, + "velocityY": -3.0837835472219657e-21, + "timestamp": 3.0705126626520127 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/exit center.traj b/src/main/deploy/choreo/exit center.traj index 53d22c6..9ae7f75 100644 --- a/src/main/deploy/choreo/exit center.traj +++ b/src/main/deploy/choreo/exit center.traj @@ -3,146 +3,290 @@ { "x": 1.3126474618911743, "y": 5.567087650299072, - "heading": -8.351489873798432e-33, - "angularVelocity": -9.52467798814217e-33, - "velocityX": 5.856461918837007e-19, - "velocityY": 3.2988456083555626e-20, + "heading": 2.4064931720572804e-32, + "angularVelocity": 5.341037702942834e-32, + "velocityX": -1.8920825597869374e-21, + "velocityY": 5.007478148860853e-21, "timestamp": 0 }, { - "x": 1.3974808893772463, - "y": 5.571866984328364, - "heading": -4.619528479929238e-22, - "angularVelocity": -3.675562971006093e-21, - "velocityX": 0.6749835681204585, - "velocityY": 0.03802713189750964, - "timestamp": 0.12568221137930363 - }, - { - "x": 1.5671477356384225, - "y": 5.581425651896191, - "heading": -1.730475779700296e-21, - "angularVelocity": -1.009309860545018e-20, - "velocityX": 1.3499670669315913, - "velocityY": 0.07605425989027997, - "timestamp": 0.25136442275860726 - }, - { - "x": 1.7930175454481574, - "y": 5.594150674333725, - "heading": -1.411814827850765e-19, - "angularVelocity": -1.109552445453708e-18, - "velocityX": 1.797150188009124, - "velocityY": 0.10124760137399545, - "timestamp": 0.3770466341379109 - }, - { - "x": 2.018887355257855, - "y": 5.606875696771257, - "heading": 6.884754478081912e-20, - "angularVelocity": 1.671111824496415e-18, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399623, - "timestamp": 0.5027288455172145 - }, - { - "x": 2.2447571650676026, - "y": 5.619600719208795, - "heading": 2.509060822170839e-19, - "angularVelocity": 1.4485625091677524e-18, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399623, - "timestamp": 0.6284110568965182 - }, - { - "x": 2.470626974877369, - "y": 5.632325741646326, - "heading": 2.991709519339792e-19, - "angularVelocity": 3.84023092969776e-19, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399623, - "timestamp": 0.7540932682758219 - }, - { - "x": 2.6964967846870462, - "y": 5.645050764083861, - "heading": 3.625534697671079e-19, - "angularVelocity": 5.043077983998515e-19, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399622, - "timestamp": 0.8797754796551256 - }, - { - "x": 2.9223665944967436, - "y": 5.657775786521395, - "heading": 7.427083243993144e-19, - "angularVelocity": 3.0247307984331e-18, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399622, - "timestamp": 1.0054576910344293 - }, - { - "x": 3.1482364043064752, - "y": 5.6705008089589315, - "heading": 7.718223822590269e-19, - "angularVelocity": 2.3164819703884333e-19, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399624, - "timestamp": 1.131139902413733 - }, - { - "x": 3.3741062141162126, - "y": 5.683225831396466, - "heading": 6.75954597848925e-19, - "angularVelocity": -7.627792740852862e-19, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399623, - "timestamp": 1.2568221137930367 - }, - { - "x": 3.5999760239259384, - "y": 5.695950853833998, - "heading": 5.558602175529875e-19, - "angularVelocity": -9.555400262857742e-19, - "velocityX": 1.7971501880091392, - "velocityY": 0.10124760137399623, - "timestamp": 1.3825043251723403 - }, - { - "x": 3.8258458337356256, - "y": 5.708675876271534, - "heading": 1.467111215234519e-21, - "angularVelocity": -4.4110706322056545e-18, - "velocityX": 1.797150188009124, - "velocityY": 0.10124760137399544, - "timestamp": 1.508186536551644 - }, - { - "x": 3.99551267999683, - "y": 5.718234543839361, - "heading": 3.9980033222945746e-22, - "angularVelocity": -8.492139436872755e-21, - "velocityX": 1.3499670669315909, - "velocityY": 0.07605425989027999, - "timestamp": 1.6338687479309477 - }, - { - "x": 4.08034610748291, - "y": 5.723013877868652, - "heading": 2.0668023676312887e-32, - "angularVelocity": -3.1810414607342227e-21, - "velocityX": 0.6749835681204581, - "velocityY": 0.03802713189750965, - "timestamp": 1.7595509593102514 - }, - { - "x": 4.08034610748291, - "y": 5.723013877868652, - "heading": 9.734730637251588e-33, - "angularVelocity": -9.536452132221093e-33, - "velocityX": 4.231540995901582e-19, - "velocityY": 2.3828735253686786e-20, - "timestamp": 1.885233170689555 + "x": 1.3458512299446481, + "y": 5.579396056710049, + "heading": 3.413187474556536e-18, + "angularVelocity": 4.206691136177212e-17, + "velocityX": 0.409230368386038, + "velocityY": 0.15169885784339893, + "timestamp": 0.08113710667276726 + }, + { + "x": 1.412258764942075, + "y": 5.6040128691207105, + "heading": 6.827483140754657e-18, + "angularVelocity": 4.208056962106495e-17, + "velocityX": 0.8184607230974358, + "velocityY": 0.3033977106177016, + "timestamp": 0.16227421334553452 + }, + { + "x": 1.511870065034253, + "y": 5.64093808684557, + "heading": 7.957629289102601e-18, + "angularVelocity": 1.3928844577901407e-17, + "velocityX": 1.2276910550177553, + "velocityY": 0.45509655494350654, + "timestamp": 0.2434113200183018 + }, + { + "x": 1.644685126522774, + "y": 5.690171708513654, + "heading": 1.0892134038406229e-17, + "angularVelocity": 3.6167234322702574e-17, + "velocityX": 1.6369213413558763, + "velocityY": 0.6067953823723014, + "timestamp": 0.32454842669106904 + }, + { + "x": 1.810703938312389, + "y": 5.75171373001203, + "heading": 1.3242101695542206e-17, + "angularVelocity": 2.896292157187237e-17, + "velocityX": 2.0461514909470804, + "velocityY": 0.7584941591099562, + "timestamp": 0.4056855333638363 + }, + { + "x": 1.9780760222264302, + "y": 5.813757400096333, + "heading": 1.125270337500381e-16, + "angularVelocity": 1.22366862864508e-15, + "velocityX": 2.062830322371079, + "velocityY": 0.7646768861814419, + "timestamp": 0.4868226400366035 + }, + { + "x": 2.145448106140464, + "y": 5.875801070180656, + "heading": 2.6117964956039346e-16, + "angularVelocity": 1.832116301729673e-15, + "velocityX": 2.0628303223709894, + "velocityY": 0.7646768861816947, + "timestamp": 0.5679597467093708 + }, + { + "x": 2.312820190054498, + "y": 5.937844740264981, + "heading": 3.764540638551346e-16, + "angularVelocity": 1.4207360728261472e-15, + "velocityX": 2.0628303223709845, + "velocityY": 0.7646768861817085, + "timestamp": 0.6490968533821381 + }, + { + "x": 2.4801922739685316, + "y": 5.999888410349303, + "heading": 4.151881500968984e-16, + "angularVelocity": 4.773905285775699e-16, + "velocityX": 2.0628303223709925, + "velocityY": 0.7646768861816863, + "timestamp": 0.7302339600549054 + }, + { + "x": 2.6475643578824943, + "y": 6.061932080433819, + "heading": 3.881070204693485e-16, + "angularVelocity": -3.337699695993216e-16, + "velocityX": 2.06283032237011, + "velocityY": 0.7646768861840669, + "timestamp": 0.8113710667276727 + }, + { + "x": 2.8149364417978595, + "y": 6.123975750514552, + "heading": 2.850633506870914e-16, + "angularVelocity": -1.2699943836774666e-15, + "velocityX": 2.0628303223873994, + "velocityY": 0.7646768861374271, + "timestamp": 0.89250817340044 + }, + { + "x": 2.982308527462817, + "y": 6.186019415875497, + "heading": 1.5426652083783285e-16, + "angularVelocity": -1.6120470055306626e-15, + "velocityX": 2.062830343950803, + "velocityY": 0.7646768279669163, + "timestamp": 0.9736452800732073 + }, + { + "x": 3.1499392986297607, + "y": 6.247360706329346, + "heading": -2.6221041253671193e-33, + "angularVelocity": -1.901306654426338e-15, + "velocityX": 2.0660185954499513, + "velocityY": 0.7560201856992955, + "timestamp": 1.0547823867459745 + }, + { + "x": 3.395592203319553, + "y": 6.266105388578719, + "heading": -2.25016120284868e-16, + "angularVelocity": -2.0093413357633876e-15, + "velocityX": 2.193622994737605, + "velocityY": 0.16738562917950292, + "timestamp": 1.1667674020740877 + }, + { + "x": 3.6412951619585874, + "y": 6.284182136410596, + "heading": -5.420070367092222e-16, + "angularVelocity": -2.830654757652916e-15, + "velocityX": 2.1940699647995836, + "velocityY": 0.16142113102287564, + "timestamp": 1.278752417402201 + }, + { + "x": 3.8869981207102944, + "y": 6.302258882711007, + "heading": -1.2457261466691004e-15, + "angularVelocity": -6.28404709235429e-15, + "velocityX": 2.1940699658057228, + "velocityY": 0.1614211173472237, + "timestamp": 1.3907374327303141 + }, + { + "x": 4.132701079462025, + "y": 6.320335629011082, + "heading": -2.1563980313879527e-15, + "angularVelocity": -8.132086976553131e-15, + "velocityX": 2.194069965805942, + "velocityY": 0.16142111734423847, + "timestamp": 1.5027224480584274 + }, + { + "x": 4.378404038213756, + "y": 6.338412375311165, + "heading": -3.277495117328277e-15, + "angularVelocity": -1.0011134817060459e-14, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734430015, + "timestamp": 1.6147074633865406 + }, + { + "x": 4.624106996965487, + "y": 6.356489121611248, + "heading": -4.357861685209099e-15, + "angularVelocity": -9.647420815324042e-15, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429992, + "timestamp": 1.7266924787146538 + }, + { + "x": 4.869809955717218, + "y": 6.374565867911331, + "heading": -4.984271791839074e-15, + "angularVelocity": -5.593695770765471e-15, + "velocityX": 2.1940699658059377, + "velocityY": 0.1614211173442999, + "timestamp": 1.838677494042767 + }, + { + "x": 5.115512914468949, + "y": 6.392642614211413, + "heading": -5.0340664077739056e-15, + "angularVelocity": -4.446542761898409e-16, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429992, + "timestamp": 1.9506625093708803 + }, + { + "x": 5.36121587322068, + "y": 6.410719360511496, + "heading": -4.508040648188578e-15, + "angularVelocity": 4.6972870258050625e-15, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429992, + "timestamp": 2.0626475246989937 + }, + { + "x": 5.606918831972411, + "y": 6.428796106811578, + "heading": -3.510023344735256e-15, + "angularVelocity": 8.91206114076206e-15, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429992, + "timestamp": 2.174632540027107 + }, + { + "x": 5.852621790724142, + "y": 6.446872853111661, + "heading": -2.3215831189941424e-15, + "angularVelocity": 1.0612493307778848e-14, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429995, + "timestamp": 2.28661755535522 + }, + { + "x": 6.098324749475873, + "y": 6.464949599411743, + "heading": -1.250017912777245e-15, + "angularVelocity": 9.568826713799511e-15, + "velocityX": 2.1940699658059377, + "velocityY": 0.16142111734429923, + "timestamp": 2.3986025706833334 + }, + { + "x": 6.344027708227604, + "y": 6.48302634571183, + "heading": -4.511696357394538e-16, + "angularVelocity": 7.133528309097315e-15, + "velocityX": 2.194069965805935, + "velocityY": 0.16142111734433204, + "timestamp": 2.5105875860114466 + }, + { + "x": 6.589730666979331, + "y": 6.501103092011914, + "heading": -4.711917488169644e-19, + "angularVelocity": 4.024631712288509e-15, + "velocityX": 2.1940699658059026, + "velocityY": 0.16142111734431822, + "timestamp": 2.62257260133956 + }, + { + "x": 6.791556314990098, + "y": 6.515951716767905, + "heading": 1.0824486990110142e-21, + "angularVelocity": 4.2172981459668045e-18, + "velocityX": 1.8022558412786114, + "velocityY": 0.13259474682827085, + "timestamp": 2.734557616667673 + }, + { + "x": 6.926106755210862, + "y": 6.525850800542852, + "heading": 2.72309248565345e-22, + "angularVelocity": -7.234355816903654e-21, + "velocityX": 1.2015039675311439, + "velocityY": 0.08839650328164239, + "timestamp": 2.8465426319957863 + }, + { + "x": 6.993381977081299, + "y": 6.5308003425598145, + "heading": -3.4836420737679767e-34, + "angularVelocity": -2.4316579121587528e-21, + "velocityX": 0.6007519994824513, + "velocityY": 0.04419825279713492, + "timestamp": 2.9585276473238995 + }, + { + "x": 6.993381977081299, + "y": 6.5308003425598145, + "heading": -6.944682501042333e-34, + "angularVelocity": -9.29206724482002e-33, + "velocityX": -5.0941360422492845e-22, + "velocityY": -3.0837835472219657e-21, + "timestamp": 3.0705126626520127 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/source exit.1.traj b/src/main/deploy/choreo/source exit.1.traj new file mode 100644 index 0000000..4729d36 --- /dev/null +++ b/src/main/deploy/choreo/source exit.1.traj @@ -0,0 +1,382 @@ +{ + "samples": [ + { + "x": 0.6313003897666931, + "y": 4.255156517028809, + "heading": -1.2897, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 4.938118129828658e-32, + "timestamp": 0 + }, + { + "x": 0.6547225060554301, + "y": 4.233901437352739, + "heading": -1.2896998806389426, + "angularVelocity": 0.0000015565956410580199, + "velocityX": 0.3054494072906231, + "velocityY": -0.2771889358303716, + "timestamp": 0.0766808372505756 + }, + { + "x": 0.7015667377724408, + "y": 4.191391278797934, + "heading": -1.289699622292801, + "angularVelocity": 0.000003369109545120088, + "velocityX": 0.6108988033598843, + "velocityY": -0.5543778612626656, + "timestamp": 0.1533616745011512 + }, + { + "x": 0.7718330834830761, + "y": 4.127626042692661, + "heading": -1.289699192032996, + "angularVelocity": 0.000005611047281688477, + "velocityX": 0.9163481807197896, + "velocityY": -0.8315667693729352, + "timestamp": 0.2300425117517268 + }, + { + "x": 0.8655215403161615, + "y": 4.042605731691311, + "heading": -1.2896985232175402, + "angularVelocity": 0.000008722067696213578, + "velocityX": 1.2217975206365133, + "velocityY": -1.1087556428671073, + "timestamp": 0.3067233490023024 + }, + { + "x": 0.9826320996478911, + "y": 3.936330353745298, + "heading": -1.2896974106735322, + "angularVelocity": 0.000014508761822433673, + "velocityX": 1.5272467480896021, + "velocityY": -1.3859444126663505, + "timestamp": 0.38340418625287803 + }, + { + "x": 1.113237043415476, + "y": 3.8178091039826962, + "heading": -1.2790397450792215, + "angularVelocity": 0.1389873399462232, + "velocityX": 1.7032279308687956, + "velocityY": -1.5456436576885717, + "timestamp": 0.46008502350345365 + }, + { + "x": 1.2438419873099618, + "y": 3.6992878543599033, + "heading": -1.2518722162171183, + "angularVelocity": 0.3542935867187001, + "velocityX": 1.7032279325237185, + "velocityY": -1.5456436558653144, + "timestamp": 0.5367658607540292 + }, + { + "x": 1.3744469312210372, + "y": 3.5807666047553917, + "heading": -1.212078387332647, + "angularVelocity": 0.5189540217777049, + "velocityX": 1.7032279327400672, + "velocityY": -1.5456436556269055, + "timestamp": 0.6134466980046048 + }, + { + "x": 1.5050518752146576, + "y": 3.4622453552418415, + "heading": -1.162751180302459, + "angularVelocity": 0.6432794528441913, + "velocityX": 1.70322793381654, + "velocityY": -1.5456436544406769, + "timestamp": 0.6901275352551804 + }, + { + "x": 1.6356568192666512, + "y": 3.343724105792617, + "heading": -1.1063318116829663, + "angularVelocity": 0.7357688132058888, + "velocityX": 1.7032279345777896, + "velocityY": -1.5456436536018043, + "timestamp": 0.7668083725057561 + }, + { + "x": 1.7662617633452586, + "y": 3.22520285637272, + "heading": -1.044747551464474, + "angularVelocity": 0.8031245148934248, + "velocityX": 1.703227934924863, + "velocityY": -1.54564365321933, + "timestamp": 0.8434892097563317 + }, + { + "x": 1.8968667073747523, + "y": 3.1066816068987038, + "heading": -0.9795374269556493, + "angularVelocity": 0.8504096570533181, + "velocityX": 1.7032279342843641, + "velocityY": -1.5456436539251106, + "timestamp": 0.9201700470069073 + }, + { + "x": 2.0274716511557966, + "y": 2.988160357150909, + "heading": -0.911962353611442, + "angularVelocity": 0.8812511152347953, + "velocityX": 1.7032279310443175, + "velocityY": -1.5456436574954706, + "timestamp": 0.9968508842574829 + }, + { + "x": 2.158076594153494, + "y": 2.869639106539904, + "heading": -0.8431013334736036, + "angularVelocity": 0.8980212345989256, + "velocityX": 1.7032279208286418, + "velocityY": -1.5456436687526536, + "timestamp": 1.0735317215080584 + }, + { + "x": 2.288681534488559, + "y": 2.751117852994802, + "heading": -0.7739408419480853, + "angularVelocity": 0.9019266612794504, + "velocityX": 1.7032278861050756, + "velocityY": -1.5456437070164089, + "timestamp": 1.150212558758634 + }, + { + "x": 2.419286472128144, + "y": 2.632596596479403, + "heading": -0.705434667355344, + "angularVelocity": 0.8933936697754613, + "velocityX": 1.703227850953138, + "velocityY": -1.5456437457522652, + "timestamp": 1.2268933960092097 + }, + { + "x": 2.5498914265636636, + "y": 2.5140753584723274, + "heading": -0.6385807654336066, + "angularVelocity": 0.8718462697959073, + "velocityX": 1.7032280699900582, + "velocityY": -1.5456435043839587, + "timestamp": 1.3035742332597853 + }, + { + "x": 2.680496509530869, + "y": 2.3955542621014185, + "heading": -0.5745105008427962, + "angularVelocity": 0.8355446665435626, + "velocityX": 1.7032297461804866, + "velocityY": -1.5456416572971043, + "timestamp": 1.380255070510361 + }, + { + "x": 2.8111011984005865, + "y": 2.277032731452427, + "heading": -0.5145723016278465, + "angularVelocity": 0.7816581216906595, + "velocityX": 1.7032246067284333, + "velocityY": -1.5456473207470263, + "timestamp": 1.4569359077609365 + }, + { + "x": 2.9417192752167938, + "y": 2.158525955314311, + "heading": -0.46033957015895777, + "angularVelocity": 0.7072527298009322, + "velocityX": 1.7033991998467222, + "velocityY": -1.5454549061698755, + "timestamp": 1.5336167450115121 + }, + { + "x": 3.0872981548309326, + "y": 2.058968305587769, + "heading": -0.4081492403189991, + "angularVelocity": 0.6806176316178785, + "velocityX": 1.8985040439558556, + "velocityY": -1.2983380632792187, + "timestamp": 1.6102975822620877 + }, + { + "x": 3.288618968752435, + "y": 1.9787858722638514, + "heading": -0.3440273039506474, + "angularVelocity": 0.6805713802306704, + "velocityX": 2.1367599289661108, + "velocityY": -0.8510327729965853, + "timestamp": 1.70451537958802 + }, + { + "x": 3.4973190456299355, + "y": 1.9204457429666257, + "heading": -0.2885241078330584, + "angularVelocity": 0.5890946051900753, + "velocityX": 2.215081256416269, + "velocityY": -0.6192049798766697, + "timestamp": 1.798733176913952 + }, + { + "x": 3.7060194347295887, + "y": 1.8621067305954644, + "heading": -0.24314566154797057, + "angularVelocity": 0.48163348722870364, + "velocityX": 2.215084570250411, + "velocityY": -0.6191931251517853, + "timestamp": 1.8929509742398842 + }, + { + "x": 3.914719841235457, + "y": 1.8037677804928323, + "heading": -0.20514297433349235, + "angularVelocity": 0.40334934898785263, + "velocityX": 2.2150847549948707, + "velocityY": -0.6191924642519187, + "timestamp": 1.9871687715658164 + }, + { + "x": 4.123420253984318, + "y": 1.7454288527237616, + "heading": -0.17255263081013517, + "angularVelocity": 0.34590432432439966, + "velocityX": 2.215084821256147, + "velocityY": -0.6191922272100663, + "timestamp": 2.0813865688917486 + }, + { + "x": 4.332120665934124, + "y": 1.68708992209617, + "heading": -0.14399111470382664, + "angularVelocity": 0.3031435346286237, + "velocityX": 2.215084812775212, + "velocityY": -0.6191922575495665, + "timestamp": 2.175604366217681 + }, + { + "x": 4.540821077336117, + "y": 1.6287509895088441, + "heading": -0.11850572029946865, + "angularVelocity": 0.27049448328946984, + "velocityX": 2.2150848069609004, + "velocityY": -0.6191922783496103, + "timestamp": 2.2698221635436133 + }, + { + "x": 4.749521488592162, + "y": 1.5704120563993975, + "heading": -0.09547134573973612, + "angularVelocity": 0.2444800792788083, + "velocityX": 2.2150848054118457, + "velocityY": -0.6191922838912438, + "timestamp": 2.3640399608695457 + }, + { + "x": 4.958221899850797, + "y": 1.5120731232992075, + "heading": -0.07452259619207273, + "angularVelocity": 0.22234386859179237, + "velocityX": 2.21508480543933, + "velocityY": -0.6191922837929981, + "timestamp": 2.458257758195478 + }, + { + "x": 5.166922311144001, + "y": 1.4537341903226784, + "heading": -0.055514581735497524, + "angularVelocity": 0.2017454769272586, + "velocityX": 2.2150848058062342, + "velocityY": -0.619192282480497, + "timestamp": 2.5524755555214105 + }, + { + "x": 5.3756227224674955, + "y": 1.3953952574545083, + "heading": -0.038508342854423575, + "angularVelocity": 0.18049921950761413, + "velocityX": 2.215084806127732, + "velocityY": -0.619192281330405, + "timestamp": 2.646693352847343 + }, + { + "x": 5.5843231338114325, + "y": 1.3370563246594693, + "heading": -0.023778548081694613, + "angularVelocity": 0.15633771103522998, + "velocityX": 2.2150848063447075, + "velocityY": -0.6191922805542137, + "timestamp": 2.7409111501732752 + }, + { + "x": 5.793023545170744, + "y": 1.278717391919428, + "heading": -0.01184315101724952, + "angularVelocity": 0.12667879533577564, + "velocityX": 2.2150848065078788, + "velocityY": -0.619192279970488, + "timestamp": 2.8351289474992076 + }, + { + "x": 6.001723956538716, + "y": 1.2203784592103721, + "heading": -0.0035175510847978773, + "angularVelocity": 0.08836546988728466, + "velocityX": 2.2150848065998088, + "velocityY": -0.6191922796416123, + "timestamp": 2.92934674482514 + }, + { + "x": 6.210424367778128, + "y": 1.1620395260406786, + "heading": 9.95199444319593e-7, + "angularVelocity": 0.03734481577905508, + "velocityX": 2.2150848052352985, + "velocityY": -0.619192284530687, + "timestamp": 3.0235645421510724 + }, + { + "x": 6.394372705631795, + "y": 1.1106196478377304, + "heading": 5.066604047984432e-7, + "angularVelocity": -0.00000518520965030981, + "velocityX": 1.952373575634825, + "velocityY": -0.545755469373464, + "timestamp": 3.117782339477005 + }, + { + "x": 6.532333969364687, + "y": 1.0720547363320128, + "heading": 2.3088395723940584e-7, + "angularVelocity": -0.000002927010090872124, + "velocityX": 1.4642802914998674, + "velocityY": -0.4093166323163768, + "timestamp": 3.212000136802937 + }, + { + "x": 6.624308148185072, + "y": 1.0463447945031064, + "heading": 7.204386582174162e-8, + "angularVelocity": -0.00000168588200609885, + "velocityX": 0.9761868928246619, + "velocityY": -0.27287776363489813, + "timestamp": 3.3062179341288696 + }, + { + "x": 6.670295238494873, + "y": 1.0334898233413696, + "heading": 0, + "angularVelocity": -7.646523997427724e-7, + "velocityX": 0.4880934559605067, + "velocityY": -0.1364388844420419, + "timestamp": 3.400435731454802 + }, + { + "x": 6.670295238494873, + "y": 1.0334898233413696, + "heading": 0, + "angularVelocity": 0, + "velocityX": -8.306900544678907e-28, + "velocityY": -1.1351138938300595e-27, + "timestamp": 3.4946535287807343 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/source exit.traj b/src/main/deploy/choreo/source exit.traj new file mode 100644 index 0000000..4729d36 --- /dev/null +++ b/src/main/deploy/choreo/source exit.traj @@ -0,0 +1,382 @@ +{ + "samples": [ + { + "x": 0.6313003897666931, + "y": 4.255156517028809, + "heading": -1.2897, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 4.938118129828658e-32, + "timestamp": 0 + }, + { + "x": 0.6547225060554301, + "y": 4.233901437352739, + "heading": -1.2896998806389426, + "angularVelocity": 0.0000015565956410580199, + "velocityX": 0.3054494072906231, + "velocityY": -0.2771889358303716, + "timestamp": 0.0766808372505756 + }, + { + "x": 0.7015667377724408, + "y": 4.191391278797934, + "heading": -1.289699622292801, + "angularVelocity": 0.000003369109545120088, + "velocityX": 0.6108988033598843, + "velocityY": -0.5543778612626656, + "timestamp": 0.1533616745011512 + }, + { + "x": 0.7718330834830761, + "y": 4.127626042692661, + "heading": -1.289699192032996, + "angularVelocity": 0.000005611047281688477, + "velocityX": 0.9163481807197896, + "velocityY": -0.8315667693729352, + "timestamp": 0.2300425117517268 + }, + { + "x": 0.8655215403161615, + "y": 4.042605731691311, + "heading": -1.2896985232175402, + "angularVelocity": 0.000008722067696213578, + "velocityX": 1.2217975206365133, + "velocityY": -1.1087556428671073, + "timestamp": 0.3067233490023024 + }, + { + "x": 0.9826320996478911, + "y": 3.936330353745298, + "heading": -1.2896974106735322, + "angularVelocity": 0.000014508761822433673, + "velocityX": 1.5272467480896021, + "velocityY": -1.3859444126663505, + "timestamp": 0.38340418625287803 + }, + { + "x": 1.113237043415476, + "y": 3.8178091039826962, + "heading": -1.2790397450792215, + "angularVelocity": 0.1389873399462232, + "velocityX": 1.7032279308687956, + "velocityY": -1.5456436576885717, + "timestamp": 0.46008502350345365 + }, + { + "x": 1.2438419873099618, + "y": 3.6992878543599033, + "heading": -1.2518722162171183, + "angularVelocity": 0.3542935867187001, + "velocityX": 1.7032279325237185, + "velocityY": -1.5456436558653144, + "timestamp": 0.5367658607540292 + }, + { + "x": 1.3744469312210372, + "y": 3.5807666047553917, + "heading": -1.212078387332647, + "angularVelocity": 0.5189540217777049, + "velocityX": 1.7032279327400672, + "velocityY": -1.5456436556269055, + "timestamp": 0.6134466980046048 + }, + { + "x": 1.5050518752146576, + "y": 3.4622453552418415, + "heading": -1.162751180302459, + "angularVelocity": 0.6432794528441913, + "velocityX": 1.70322793381654, + "velocityY": -1.5456436544406769, + "timestamp": 0.6901275352551804 + }, + { + "x": 1.6356568192666512, + "y": 3.343724105792617, + "heading": -1.1063318116829663, + "angularVelocity": 0.7357688132058888, + "velocityX": 1.7032279345777896, + "velocityY": -1.5456436536018043, + "timestamp": 0.7668083725057561 + }, + { + "x": 1.7662617633452586, + "y": 3.22520285637272, + "heading": -1.044747551464474, + "angularVelocity": 0.8031245148934248, + "velocityX": 1.703227934924863, + "velocityY": -1.54564365321933, + "timestamp": 0.8434892097563317 + }, + { + "x": 1.8968667073747523, + "y": 3.1066816068987038, + "heading": -0.9795374269556493, + "angularVelocity": 0.8504096570533181, + "velocityX": 1.7032279342843641, + "velocityY": -1.5456436539251106, + "timestamp": 0.9201700470069073 + }, + { + "x": 2.0274716511557966, + "y": 2.988160357150909, + "heading": -0.911962353611442, + "angularVelocity": 0.8812511152347953, + "velocityX": 1.7032279310443175, + "velocityY": -1.5456436574954706, + "timestamp": 0.9968508842574829 + }, + { + "x": 2.158076594153494, + "y": 2.869639106539904, + "heading": -0.8431013334736036, + "angularVelocity": 0.8980212345989256, + "velocityX": 1.7032279208286418, + "velocityY": -1.5456436687526536, + "timestamp": 1.0735317215080584 + }, + { + "x": 2.288681534488559, + "y": 2.751117852994802, + "heading": -0.7739408419480853, + "angularVelocity": 0.9019266612794504, + "velocityX": 1.7032278861050756, + "velocityY": -1.5456437070164089, + "timestamp": 1.150212558758634 + }, + { + "x": 2.419286472128144, + "y": 2.632596596479403, + "heading": -0.705434667355344, + "angularVelocity": 0.8933936697754613, + "velocityX": 1.703227850953138, + "velocityY": -1.5456437457522652, + "timestamp": 1.2268933960092097 + }, + { + "x": 2.5498914265636636, + "y": 2.5140753584723274, + "heading": -0.6385807654336066, + "angularVelocity": 0.8718462697959073, + "velocityX": 1.7032280699900582, + "velocityY": -1.5456435043839587, + "timestamp": 1.3035742332597853 + }, + { + "x": 2.680496509530869, + "y": 2.3955542621014185, + "heading": -0.5745105008427962, + "angularVelocity": 0.8355446665435626, + "velocityX": 1.7032297461804866, + "velocityY": -1.5456416572971043, + "timestamp": 1.380255070510361 + }, + { + "x": 2.8111011984005865, + "y": 2.277032731452427, + "heading": -0.5145723016278465, + "angularVelocity": 0.7816581216906595, + "velocityX": 1.7032246067284333, + "velocityY": -1.5456473207470263, + "timestamp": 1.4569359077609365 + }, + { + "x": 2.9417192752167938, + "y": 2.158525955314311, + "heading": -0.46033957015895777, + "angularVelocity": 0.7072527298009322, + "velocityX": 1.7033991998467222, + "velocityY": -1.5454549061698755, + "timestamp": 1.5336167450115121 + }, + { + "x": 3.0872981548309326, + "y": 2.058968305587769, + "heading": -0.4081492403189991, + "angularVelocity": 0.6806176316178785, + "velocityX": 1.8985040439558556, + "velocityY": -1.2983380632792187, + "timestamp": 1.6102975822620877 + }, + { + "x": 3.288618968752435, + "y": 1.9787858722638514, + "heading": -0.3440273039506474, + "angularVelocity": 0.6805713802306704, + "velocityX": 2.1367599289661108, + "velocityY": -0.8510327729965853, + "timestamp": 1.70451537958802 + }, + { + "x": 3.4973190456299355, + "y": 1.9204457429666257, + "heading": -0.2885241078330584, + "angularVelocity": 0.5890946051900753, + "velocityX": 2.215081256416269, + "velocityY": -0.6192049798766697, + "timestamp": 1.798733176913952 + }, + { + "x": 3.7060194347295887, + "y": 1.8621067305954644, + "heading": -0.24314566154797057, + "angularVelocity": 0.48163348722870364, + "velocityX": 2.215084570250411, + "velocityY": -0.6191931251517853, + "timestamp": 1.8929509742398842 + }, + { + "x": 3.914719841235457, + "y": 1.8037677804928323, + "heading": -0.20514297433349235, + "angularVelocity": 0.40334934898785263, + "velocityX": 2.2150847549948707, + "velocityY": -0.6191924642519187, + "timestamp": 1.9871687715658164 + }, + { + "x": 4.123420253984318, + "y": 1.7454288527237616, + "heading": -0.17255263081013517, + "angularVelocity": 0.34590432432439966, + "velocityX": 2.215084821256147, + "velocityY": -0.6191922272100663, + "timestamp": 2.0813865688917486 + }, + { + "x": 4.332120665934124, + "y": 1.68708992209617, + "heading": -0.14399111470382664, + "angularVelocity": 0.3031435346286237, + "velocityX": 2.215084812775212, + "velocityY": -0.6191922575495665, + "timestamp": 2.175604366217681 + }, + { + "x": 4.540821077336117, + "y": 1.6287509895088441, + "heading": -0.11850572029946865, + "angularVelocity": 0.27049448328946984, + "velocityX": 2.2150848069609004, + "velocityY": -0.6191922783496103, + "timestamp": 2.2698221635436133 + }, + { + "x": 4.749521488592162, + "y": 1.5704120563993975, + "heading": -0.09547134573973612, + "angularVelocity": 0.2444800792788083, + "velocityX": 2.2150848054118457, + "velocityY": -0.6191922838912438, + "timestamp": 2.3640399608695457 + }, + { + "x": 4.958221899850797, + "y": 1.5120731232992075, + "heading": -0.07452259619207273, + "angularVelocity": 0.22234386859179237, + "velocityX": 2.21508480543933, + "velocityY": -0.6191922837929981, + "timestamp": 2.458257758195478 + }, + { + "x": 5.166922311144001, + "y": 1.4537341903226784, + "heading": -0.055514581735497524, + "angularVelocity": 0.2017454769272586, + "velocityX": 2.2150848058062342, + "velocityY": -0.619192282480497, + "timestamp": 2.5524755555214105 + }, + { + "x": 5.3756227224674955, + "y": 1.3953952574545083, + "heading": -0.038508342854423575, + "angularVelocity": 0.18049921950761413, + "velocityX": 2.215084806127732, + "velocityY": -0.619192281330405, + "timestamp": 2.646693352847343 + }, + { + "x": 5.5843231338114325, + "y": 1.3370563246594693, + "heading": -0.023778548081694613, + "angularVelocity": 0.15633771103522998, + "velocityX": 2.2150848063447075, + "velocityY": -0.6191922805542137, + "timestamp": 2.7409111501732752 + }, + { + "x": 5.793023545170744, + "y": 1.278717391919428, + "heading": -0.01184315101724952, + "angularVelocity": 0.12667879533577564, + "velocityX": 2.2150848065078788, + "velocityY": -0.619192279970488, + "timestamp": 2.8351289474992076 + }, + { + "x": 6.001723956538716, + "y": 1.2203784592103721, + "heading": -0.0035175510847978773, + "angularVelocity": 0.08836546988728466, + "velocityX": 2.2150848065998088, + "velocityY": -0.6191922796416123, + "timestamp": 2.92934674482514 + }, + { + "x": 6.210424367778128, + "y": 1.1620395260406786, + "heading": 9.95199444319593e-7, + "angularVelocity": 0.03734481577905508, + "velocityX": 2.2150848052352985, + "velocityY": -0.619192284530687, + "timestamp": 3.0235645421510724 + }, + { + "x": 6.394372705631795, + "y": 1.1106196478377304, + "heading": 5.066604047984432e-7, + "angularVelocity": -0.00000518520965030981, + "velocityX": 1.952373575634825, + "velocityY": -0.545755469373464, + "timestamp": 3.117782339477005 + }, + { + "x": 6.532333969364687, + "y": 1.0720547363320128, + "heading": 2.3088395723940584e-7, + "angularVelocity": -0.000002927010090872124, + "velocityX": 1.4642802914998674, + "velocityY": -0.4093166323163768, + "timestamp": 3.212000136802937 + }, + { + "x": 6.624308148185072, + "y": 1.0463447945031064, + "heading": 7.204386582174162e-8, + "angularVelocity": -0.00000168588200609885, + "velocityX": 0.9761868928246619, + "velocityY": -0.27287776363489813, + "timestamp": 3.3062179341288696 + }, + { + "x": 6.670295238494873, + "y": 1.0334898233413696, + "heading": 0, + "angularVelocity": -7.646523997427724e-7, + "velocityX": 0.4880934559605067, + "velocityY": -0.1364388844420419, + "timestamp": 3.400435731454802 + }, + { + "x": 6.670295238494873, + "y": 1.0334898233413696, + "heading": 0, + "angularVelocity": 0, + "velocityX": -8.306900544678907e-28, + "velocityY": -1.1351138938300595e-27, + "timestamp": 3.4946535287807343 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2 piece amp side.auto b/src/main/deploy/pathplanner/autos/2 piece amp side.auto index 034397b..3617285 100644 --- a/src/main/deploy/pathplanner/autos/2 piece amp side.auto +++ b/src/main/deploy/pathplanner/autos/2 piece amp side.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.6235248446464541, + "x": 0.6235248446464539, "y": 6.866034507751465 }, - "rotation": 74.69650775120544 + "rotation": 74.69650775120543 }, "command": { "type": "sequential", @@ -72,6 +72,18 @@ "data": { "pathName": "2 piece left.1" } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "2 piece left.2" + } } ] } @@ -80,15 +92,15 @@ } }, { - "type": "wait", + "type": "path", "data": { - "waitTime": 0.5 + "pathName": "2 piece left.3" } }, { - "type": "path", + "type": "wait", "data": { - "pathName": "2 piece left.2" + "waitTime": 0.2 } }, { @@ -98,21 +110,21 @@ } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 1.0 + "name": "Index" } }, { "type": "named", "data": { - "name": "Index" + "name": "ResetScoring" } }, { - "type": "named", + "type": "path", "data": { - "name": "ResetScoring" + "pathName": "2 piece left.4" } } ] diff --git a/src/main/deploy/pathplanner/autos/2 piece center.auto b/src/main/deploy/pathplanner/autos/2 piece center.auto index 6369bcf..e92c78e 100644 --- a/src/main/deploy/pathplanner/autos/2 piece center.auto +++ b/src/main/deploy/pathplanner/autos/2 piece center.auto @@ -51,6 +51,12 @@ "name": "RaceTimeout" } }, + { + "type": "named", + "data": { + "name": "WaitIntake" + } + }, { "type": "sequential", "data": { @@ -69,12 +75,6 @@ } ] } - }, - { - "type": "named", - "data": { - "name": "WaitIntake" - } } ] } @@ -94,7 +94,7 @@ { "type": "wait", "data": { - "waitTime": 1.0 + "waitTime": 0.2 } }, { @@ -108,6 +108,12 @@ "data": { "name": "ResetScoring" } + }, + { + "type": "path", + "data": { + "pathName": "exit center.1" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/2 piece source side.auto b/src/main/deploy/pathplanner/autos/2 piece source side.auto index f2dd852..4703595 100644 --- a/src/main/deploy/pathplanner/autos/2 piece source side.auto +++ b/src/main/deploy/pathplanner/autos/2 piece source side.auto @@ -72,6 +72,18 @@ "data": { "pathName": "2 piece right.1" } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "path", + "data": { + "pathName": "2 piece right.2" + } } ] } @@ -79,16 +91,10 @@ ] } }, - { - "type": "wait", - "data": { - "waitTime": 0.5 - } - }, { "type": "path", "data": { - "pathName": "2 piece right.2" + "pathName": "2 piece right.3" } }, { @@ -118,7 +124,7 @@ { "type": "path", "data": { - "pathName": "2 piece right.3" + "pathName": "2 piece right.4" } } ] diff --git a/src/main/deploy/pathplanner/autos/3 piece (towards amp).auto b/src/main/deploy/pathplanner/autos/3 piece (towards amp).auto deleted file mode 100644 index 280c43e..0000000 --- a/src/main/deploy/pathplanner/autos/3 piece (towards amp).auto +++ /dev/null @@ -1,179 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.3126474618911605, - "y": 5.567087650299072 - }, - "rotation": 5.013350972124652e-28 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Options" - } - }, - { - "type": "named", - "data": { - "name": "SpeakerSetting" - } - }, - { - "type": "named", - "data": { - "name": "Index" - } - }, - { - "type": "named", - "data": { - "name": "StopShooter" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "WaitIntake" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "4 piece.1" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 3.0 - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "4 piece.2" - } - }, - { - "type": "named", - "data": { - "name": "SpeakerSetting" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "named", - "data": { - "name": "Index" - } - }, - { - "type": "named", - "data": { - "name": "StopShooter" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "WaitIntake" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "path", - "data": { - "pathName": "4 piece.3" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 4.0 - } - } - ] - } - }, - { - "type": "path", - "data": { - "pathName": "4 piece.4" - } - }, - { - "type": "named", - "data": { - "name": "SpeakerSetting" - } - }, - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "named", - "data": { - "name": "Index" - } - }, - { - "type": "named", - "data": { - "name": "ResetScoring" - } - } - ] - } - }, - "folder": null, - "choreoAuto": true -} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ab114f5..8963b71 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,7 +7,6 @@ import frc.robot.constants.Constants.ShuffleboardConstants; import frc.robot.constants.IntakeConstants; import frc.robot.constants.ScoringConstants; -import frc.robot.constants.ScoringConstants.ScoringPosition; import frc.robot.io.GyroIO; import frc.robot.io.GyroIOPigeon2; import frc.robot.io.GyroIOSim; @@ -34,12 +33,13 @@ import frc.robot.commands.vision.AprilTagVision; import frc.robot.commands.vision.GamePieceVision; +import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.util.PixelFormat; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.Relay; import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; -import edu.wpi.first.wpilibj.shuffleboard.SendableCameraWrapper; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; @@ -115,15 +115,22 @@ public RobotContainer() { .withSize(4, 1).withPosition(0, 5); driveTab.add("alerts", Alert.getAlertsSendable()) .withSize(5, 4).withPosition(4, 5).withWidget(Alert.widgetName); - driveTab.add("camera", SendableCameraWrapper.wrap("limelight-stream", "http://10.1.2.12:5800/stream.mjpg")) - .withProperties(Map.of("show crosshair", false, "show controls", false)) - .withWidget(BuiltInWidgets.kCameraStream) - .withSize(11, 5) - .withPosition(0, 0); + // driveTab.add("camera", SendableCameraWrapper.wrap("limelight-stream", "http://10.1.2.12:5800/stream.mjpg")) + // .withProperties(Map.of("show crosshair", false, "show controls", false)) + // .withWidget(BuiltInWidgets.kCameraStream) + // .withSize(11, 5) + // .withPosition(0, 0); + var camera = CameraServer.startAutomaticCapture(); + camera.setResolution(160, 120); + camera.setFPS(15); + camera.setPixelFormat(PixelFormat.kMJPEG); + driveTab.add("camera", camera).withWidget(BuiltInWidgets.kCameraStream).withProperties(Map.of("comp", 50)) + .withPosition(0, 0).withSize(11, 5); Command shuffleboardAutoOptions = Commands.parallel( Commands.waitSeconds(delayEntry.getBoolean(false) ? 2 : 0), - Commands.runOnce(() -> intake.resetNoteDetection(noteStartEntry.getBoolean(false)))); + Commands.runOnce(() -> intake.resetNoteDetection(noteStartEntry.getBoolean(false))), + Commands.runOnce(() -> swerve.gyroIO.setYaw(swerve.getPose().getRotation().getDegrees()))); // named commands must be registered before any paths are created NamedCommands.registerCommand("Options", shuffleboardAutoOptions); @@ -131,19 +138,13 @@ public RobotContainer() { NamedCommands.registerCommand("SpeakerAlign", new AprilTagVision(vision, swerve).withTimeout(1)); NamedCommands.registerCommand("NoteAlign", new GamePieceVision(vision, swerve).withTimeout(1)); NamedCommands.registerCommand("SpeakerSetting", - new SetScoringPosition(arm, shooter, ScoringConstants.subwooferPosition).withTimeout(.5)); + new SetScoringPosition(arm, shooter, ScoringConstants.subwooferPosition)); NamedCommands.registerCommand("LimelightSetting", new SetScoringPosition(arm, shooter, vision::estimateScoringPosition_math)); NamedCommands.registerCommand("WaitUntilEnd", Commands.idle().until(() -> DriverStation.getMatchTime() <= 5)); NamedCommands.registerCommand("WaitUntilVeryEnd", Commands.idle().until(() -> DriverStation.getMatchTime() <= 2)); - NamedCommands.registerCommand("WaitIntake", Commands.startEnd(() -> { - arm.setPosition(-2); - intake.setMotorSpeed(IntakeConstants.intakeSpeed); - intake.resetNoteDetection(); - }, () -> { - arm.setPosition(4); - intake.stopMotor(); - }, intake, arm).until(intake::isHoldingNote)); + NamedCommands.registerCommand("WaitIntake", + IntakeWithArm.intakeWithDelay(intake, arm).beforeStarting(() -> intake.resetNoteDetection())); NamedCommands.registerCommand("Index", new SetIntakeSpeed(intake, true).withTimeout(1)); NamedCommands.registerCommand("ArmDown", new SetArmPosition(arm, 4)); NamedCommands.registerCommand("ArmCarry", new SetArmPosition(arm, 40)); @@ -154,11 +155,13 @@ public RobotContainer() { NamedCommands.registerCommand("ResetScoring", new SetScoringPosition(arm, shooter, ScoringConstants.lowCarryPosition)); NamedCommands.registerCommand("StopShooter", new StopShooter(shooter)); - NamedCommands.registerCommand("RaceTimeout", Commands.idle().withTimeout(4)); - NamedCommands.registerCommand("Print", Commands.print("hello!").beforeStarting(Commands.print("another hi"))); + NamedCommands.registerCommand("RaceTimeout", Commands.idle().withTimeout(5)); + NamedCommands.registerCommand("Print", Commands.print("hello!")); // create paths final List autoNames = AutoBuilder.getAllAutoNames(); for (final String autoName : autoNames) { + if (autoName == "pits test" && !Constants.tuningMode) + continue; final Command auto = new PathPlannerAuto(autoName); autoChooser.addOption(autoName, auto); } @@ -219,7 +222,7 @@ private void configureBindings() { // *OPERATOR CONTROLS* // operatorController.a() - .onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(84, 1750))); + .onTrue(new SetScoringPosition(arm, shooter, ScoringConstants.ampPosition)); operatorController.b() .onTrue(new SetScoringPosition(arm, shooter, ScoringConstants.subwooferPosition)); operatorController.x().onTrue( @@ -229,7 +232,7 @@ private void configureBindings() { operatorController.leftBumper().onTrue(new SetArmPosition(arm, 4)); operatorController.rightBumper().onTrue(new SetArmPosition(arm, 40)); operatorController.leftTrigger(boolTriggerThreshold) - .whileTrue(new IntakeWithArm(intake, arm)); + .whileTrue(IntakeWithArm.intakeWithDelay(intake, arm)); operatorController.rightTrigger(boolTriggerThreshold).whileTrue(new SetIntakeSpeed(intake, true)); operatorController.povDown().onTrue(Commands.runOnce(() -> arm.setClimberRelay(Relay.Value.kForward), arm) .unless(() -> DriverStation.getMatchTime() > 32)); @@ -342,4 +345,4 @@ public void checkRumble() { } cachedHoldingNote = currHoldingNote; } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/arm/SetArmPosition.java b/src/main/java/frc/robot/commands/arm/SetArmPosition.java index 5ef2811..3ae3bbd 100644 --- a/src/main/java/frc/robot/commands/arm/SetArmPosition.java +++ b/src/main/java/frc/robot/commands/arm/SetArmPosition.java @@ -11,7 +11,7 @@ public class SetArmPosition extends Command { private Arm arm; - protected double targetPosition_rad; // did I do this right? + protected double targetPosition_deg; // did I do this right? /** * Command to set the arm position, cancels the command and moves on @@ -21,22 +21,24 @@ public class SetArmPosition extends Command { */ public SetArmPosition(Arm arm, double targetPosition_deg) { this.arm = arm; - this.targetPosition_rad = targetPosition_deg; + this.targetPosition_deg = targetPosition_deg; addRequirements(arm); } @Override public void initialize() { - arm.setPosition(targetPosition_rad); + } @Override - public void execute() {} + public void execute() { + arm.setPosition(targetPosition_deg, arm.getBestPIDSlot(targetPosition_deg)); + } @Override public void end(boolean interrupted) { // only stop if interrupted. Otherwise, continue adjusting arm position just move on to another command - if (interrupted) { + if (interrupted || targetPosition_deg >= 65) { arm.stop(); } } diff --git a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java index e6e981a..e14a541 100644 --- a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java +++ b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java @@ -14,7 +14,7 @@ public class IntakeWithArm extends Command { private Intake intake; private Arm arm; - public IntakeWithArm(Intake intake, Arm arm) { + private IntakeWithArm(Intake intake, Arm arm) { this.intake = intake; this.arm = arm; addRequirements(intake, arm); @@ -41,7 +41,10 @@ public void end(boolean interrupted) { arm.setPosition(4); else arm.setPosition(40); - Commands.waitSeconds(.015).andThen(intake::stopMotor, intake).andThen(Commands.print("intook")).schedule(); + // Commands.waitSeconds(.015).andThen(intake::stopMotor, intake).schedule(); + if (interrupted) { + intake.stopMotor(); + } } // Returns true when the command should end. @@ -49,4 +52,8 @@ public void end(boolean interrupted) { public boolean isFinished() { return intake.isHoldingNote(); } + + public static Command intakeWithDelay(Intake intake, Arm arm) { + return new IntakeWithArm(intake, arm).andThen(Commands.waitSeconds(.015)).andThen(intake::stopMotor, intake); + } } diff --git a/src/main/java/frc/robot/commands/scoring/SetScoringPosition.java b/src/main/java/frc/robot/commands/scoring/SetScoringPosition.java index a052794..b58372d 100644 --- a/src/main/java/frc/robot/commands/scoring/SetScoringPosition.java +++ b/src/main/java/frc/robot/commands/scoring/SetScoringPosition.java @@ -13,6 +13,7 @@ public class SetScoringPosition extends Command { private Shooter shooter; private Supplier posSupplier; + private ScoringPosition targetPosition; /** * Create a set scoring position command that adjusts dynamically according to a method that provides a {@link ScoringPosition} object @@ -40,22 +41,23 @@ public SetScoringPosition(Arm arm, Shooter shooter, ScoringPosition position) { // Called when the command is initially scheduled. @Override public void initialize() { - var targetPosition = posSupplier.get(); - arm.setPosition(targetPosition.armAngle_deg()); + targetPosition = posSupplier.get(); shooter.setVelocity(targetPosition.shooterSpeed_rpm()); } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + arm.setPosition(targetPosition.armAngle_deg(), arm.getBestPIDSlot(targetPosition.armAngle_deg())); + } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - if (interrupted) { + if (interrupted) shooter.stop(); + if (interrupted || targetPosition.armAngle_deg() >= 65) arm.stop(); - } } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java index 2c4bb64..7ce6f05 100644 --- a/src/main/java/frc/robot/constants/ArmConstants.java +++ b/src/main/java/frc/robot/constants/ArmConstants.java @@ -4,13 +4,16 @@ public final class ArmConstants { public static final int leadMotorId = 29; public static final int followerMotorId = 30; - public static final double shaftEncoderOffset_deg = 125.28; // needs to be tuned + public static final double shaftEncoderOffset_deg = 124.52; // needs to be tuned // pid public static final double kP = .000095; public static final double kI = 0; public static final double kD = .00000165; + public static final double kPHigh = 0.000047; + public static final double kDHigh = 0.00000725; + public static final double maxOutput = .5; public static final double minOutput = -maxOutput; @@ -28,9 +31,12 @@ public final class ArmConstants { public static double maxAccel_rpmps = 7200; public static double maxVelocity_rpm = 3800; + public static double maxVelocityHigh_rpm = 1900; + // Conversion (not needed) // public static final int gearRatio = 1; // 1 IS PLACEHOLDER // closeEnough - public static final double accuracyTolerance_deg = .6; + public static final double accuracyTolerance_deg = .65; + public static final double accuracyToleranceHigh_deg = 1; } \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java index abd39c3..ebe8b94 100644 --- a/src/main/java/frc/robot/constants/ScoringConstants.java +++ b/src/main/java/frc/robot/constants/ScoringConstants.java @@ -16,7 +16,7 @@ public record ScoringPosition(double armAngle_deg, double shooterSpeed_rpm) {}; scoringMap.put(3.227, new ScoringPosition(19, 3450)); } - public static final ScoringPosition ampPosition = new ScoringPosition(84, 1750); + public static final ScoringPosition ampPosition = new ScoringPosition(90, 1750); public static final ScoringPosition subwooferPosition = new ScoringPosition(-1.5, 3250); public static final ScoringPosition carryPosition = new ScoringPosition(40, 0); public static final ScoringPosition lowCarryPosition = new ScoringPosition(4, 0); diff --git a/src/main/java/frc/robot/constants/ShooterConstants.java b/src/main/java/frc/robot/constants/ShooterConstants.java index 488ff0d..e11c408 100644 --- a/src/main/java/frc/robot/constants/ShooterConstants.java +++ b/src/main/java/frc/robot/constants/ShooterConstants.java @@ -8,7 +8,7 @@ public final class ShooterConstants { public static final double kD = .0000025; public static final double kS = 0.026257; - public static final double kV = .002092; + public static final double kV = .002097; public static final double kA = .008631; public static final double maxVelocity_rpm = 5000; diff --git a/src/main/java/frc/robot/constants/SwerveConstants.java b/src/main/java/frc/robot/constants/SwerveConstants.java index a6cbfaa..9a5fdeb 100644 --- a/src/main/java/frc/robot/constants/SwerveConstants.java +++ b/src/main/java/frc/robot/constants/SwerveConstants.java @@ -7,10 +7,10 @@ public final class SwerveConstants { // FL, FR, BL, BR (matches AdvantageScope convention) public static final SwerveModuleConstants moduleConstants[] = { - new SwerveModuleConstants(21, 22, .692), - new SwerveModuleConstants(23, 24, 1.962), - new SwerveModuleConstants(25, 26, 2.645), - new SwerveModuleConstants(27, 28, 2.966) + new SwerveModuleConstants(21, 22, .719), + new SwerveModuleConstants(23, 24, 1.921), + new SwerveModuleConstants(25, 26, 2.619), + new SwerveModuleConstants(27, 28, 2.877) }; // the left-to-right distance between the drivetrain wheels, should be measured from center to center diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 26df863..d7f1da8 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -80,13 +80,22 @@ public Arm() { pidController.setSmartMotionAllowedClosedLoopError(accuracyTolerance_deg, 0); pidController.setOutputRange(minOutput, maxOutput); // since we are using smartmotion, the PID numbers are for velocity control, not position. - pidController.setP(kP); - pidController.setD(kD); + pidController.setP(kP, 0); + pidController.setD(kD, 0); // Treats 0 and 360 degrees as the same number, so going from one side of 0 to the other doesnt make it do a 360 pidController.setPositionPIDWrappingEnabled(true); pidController.setPositionPIDWrappingMinInput(0); pidController.setPositionPIDWrappingMaxInput(360); + // high angles PID loop + pidController.setSmartMotionAccelStrategy(AccelStrategy.kTrapezoidal, 1); + pidController.setSmartMotionMaxAccel(maxAccel_rpmps, 1); + pidController.setSmartMotionMaxVelocity(maxVelocityHigh_rpm, 1); + pidController.setSmartMotionAllowedClosedLoopError(accuracyToleranceHigh_deg, 1); + + pidController.setP(kPHigh, 1); + pidController.setD(kDHigh, 1); + // if (tuningMode) { // new AutoSetterTunableNumber("Arm/kP", kP, (value) -> pidController.setP(value)); // new AutoSetterTunableNumber("Arm/kD", kD, (value) -> pidController.setD(value)); @@ -141,12 +150,17 @@ private void updateInputs(ArmIOInputs inputs) { inputs.motorVelocity_rpm = Math102.truncate(motorEncoder.getVelocity(), 2); } - public void setPosition(double position_deg) { + public void setPosition(double position_deg, int pidSlot) { + Logger.recordOutput("Arm/pidSlot", pidSlot); targetPosition_deg = position_deg; - pidController.setReference(targetPosition_deg + shaftEncoderOffset_deg, ControlType.kSmartMotion, 0, + pidController.setReference(targetPosition_deg + shaftEncoderOffset_deg, ControlType.kSmartMotion, pidSlot, feedforwardController.calculate(Units.degreesToRadians(targetPosition_deg), 0)); } + public void setPosition(double position_deg) { + setPosition(position_deg, 0); + } + public void setMotorVoltage(double voltage_V) { Logger.recordOutput("Arm/targetVoltage_V", voltage_V); pidController.setReference(voltage_V, ControlType.kVoltage, 0, @@ -167,4 +181,9 @@ public void stop() { public boolean closeEnough() { return MathUtil.isNear(targetPosition_deg, inputs.shaftPosition_deg, 5); } + + public int getBestPIDSlot(double targetPosition_deg) { + boolean chooseTop = (inputs.shaftPosition_deg > 65) && (targetPosition_deg > 65); + return chooseTop ? 1 : 0; + } }